Active Suspension

Active Suspension Code can be found underneath which should be copied in Arduino 1.8.13. Click here to download the software.

//Author: Giovanni Cornaglia

//Resource: Jeff Rowberg

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files

// for both classes must be in the include path of your project

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#include "MPU6050.h" 

//  SDA must be connected to analog pin A4

//  SCL must be connected to analog pin A5

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation

// is used in I2Cdev.h

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

    #include "Wire.h"

#endif

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here

// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)

// AD0 high = 0x69

MPU6050 mpu;

//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL

#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards

// MPU control/status vars

bool dmpReady = false;  // set true if DMP init was successful

uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU

uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount;     // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q;           // [w, x, y, z]         quaternion container

VectorInt16 aa;         // [x, y, z]            accel sensor measurements

VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements

VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements

VectorFloat gravity;    // [x, y, z]            gravity vector

float euler[3];         // [psi, theta, phi]    Euler angle container

float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// ================================================================

// ===               INTERRUPT DETECTION ROUTINE                ===

// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady() {

    mpuInterrupt = true;

}

// ================================================================

// ===                      INITIAL SETUP                       ===

// ================================================================

void setup() {

    // join I2C bus (I2Cdev library doesn't do this automatically)

    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

        Wire.begin();

        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

        Fastwire::setup(400, true);

    #endif

    // initialize serial communication

    Serial.begin(115200);  // can be changed depending on testing

    while (!Serial);  // wait for Leonardo enumeration, others continue immediately

    // initialize device

    Serial.println(F("Initializing I2C devices..."));

    mpu.initialize();

    pinMode(INTERRUPT_PIN, INPUT);

    // verify connection

    Serial.println(F("Testing device connections..."));

    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready

    Serial.println(F("\nSend any character to begin DMP programming and demo: "));

    while (Serial.available() && Serial.read()); // empty buffer

    while (!Serial.available());                 // wait for data

    while (Serial.available() && Serial.read()); // empty buffer again

    // load and configure the DMP

    Serial.println(F("Initializing DMP..."));

    devStatus = mpu.dmpInitialize();

// If data readings are not starting from 0

// Change offsets values according to errors

    mpu.setXGyroOffset(220);

    mpu.setYGyroOffset(76);

    mpu.setZGyroOffset(-85);

    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)

    if (devStatus == 0) {

        // Calibration Time: generate offsets and calibrate our MPU6050

        mpu.CalibrateAccel(6);

        mpu.CalibrateGyro(6);

        mpu.PrintActiveOffsets();

        // turn on the DMP, now that it's ready

        Serial.println(F("Enabling DMP..."));

        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection

        Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));

        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));

        Serial.println(F(")..."));

        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);

        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it

        Serial.println(F("DMP ready! Waiting for first interrupt..."));

        dmpReady = true;

        // get expected DMP packet size for later comparison

        packetSize = mpu.dmpGetFIFOPacketSize();

    } else {

        // ERROR!

        // 1 = initial memory load failed

        // 2 = DMP configuration updates failed

        // (if it's going to break, usually the code will be 1)

        Serial.print(F("DMP Initialization failed (code "));

        Serial.print(devStatus);

        Serial.println(F(")"));

    }

  pinMode(4, OUTPUT); // Configure pin 4 as an Output (attach relay 1 to pin 4)

  pinMode(5, OUTPUT); // Configure pin 5 as an Output (attach relay 2 to pin 5)

  pinMode(6, OUTPUT); // Configure pin 6 as an Output (attach relay 3 to pin 6)

  pinMode(7, OUTPUT); // Configure pin 7 as an Output (attach relay 4 to pin 7)

  digitalWrite(4, HIGH); // Initialize pin 4 as Low

  digitalWrite(5, HIGH); // Initialize pin 5 as Low

  digitalWrite(6, HIGH); // Initialize pin 6 as Low

  digitalWrite(7, HIGH); // Initialize pin 7 as Low 

}

// ================================================================

// ===                    MAIN PROGRAM LOOP                     ===

// ================================================================

void loop() {

    // if programming failed, don't try to do anything

    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available

    while (!mpuInterrupt && fifoCount < packetSize) {

        if (mpuInterrupt && fifoCount < packetSize) {

          // try to get out of the infinite loop 

          fifoCount = mpu.getFIFOCount();

        }  

    }

    // reset interrupt flag and get INT_STATUS byte

    mpuInterrupt = false;

    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count

    fifoCount = mpu.getFIFOCount();

  if(fifoCount < packetSize){

          //Lets go back and wait for another interrupt. We shouldn't be here, we got an interrupt from another event

      // This is blocking so don't do it   while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

  }

    // check for overflow (this should never happen unless our code is too inefficient)

    else if ((mpuIntStatus & (0x01 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {

        // reset so we can continue cleanly

        mpu.resetFIFO();

      //  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to ask

        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)

    } else if (mpuIntStatus & (0x01 << MPU6050_INTERRUPT_DMP_INT_BIT)) {

        // read a packet from FIFO

  while(fifoCount >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!

    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // track FIFO count here in case there is > 1 packet available

    // (this lets us immediately read more without waiting for an interrupt)

    fifoCount -= packetSize;

  }

#ifdef OUTPUT_READABLE_YAWPITCHROLL

 // display Euler angles in degrees

    mpu.dmpGetQuaternion(&q, fifoBuffer);

    mpu.dmpGetGravity(&gravity, &q);

    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    Serial.print("ypr\t");

    Serial.print(ypr[0] * 180/M_PI);

    Serial.print("\t");

    Serial.print(ypr[1] * 180/M_PI);

    Serial.print("\t");

    Serial.println(ypr[2] * 180/M_PI);

#endif

        {

  if ( ypr[2] != 0 and ypr[2]>0)  // Body roll to RIGHT

    { 

      // Extend right servo

      digitalWrite(4, LOW); 

      digitalWrite(5, HIGH); 

      // Retract left servo

      digitalWrite(6, HIGH); 

      digitalWrite(7, LOW); 

    } 

  else if (ypr[2] != 0 and ypr[2]<0) // Body roll to left

  {

      // Retract right servo

      digitalWrite(4, HIGH); 

      digitalWrite(5, LOW); 

      // Extend left servo

      digitalWrite(6, LOW); 

      digitalWrite(7, HIGH);

  } 

    else

  {

       // Keep all constant

       digitalWrite(4, HIGH); 

       digitalWrite(5, HIGH); 

       digitalWrite(6, HIGH); 

       digitalWrite(7, HIGH); 

    }

   }

  }

 }