Untitled

//#define PRINT_DEBUG_BUILD  //This is to print the mpu data on serial monitor to debug
//PID library
// #include <PID_v1.h>
#include "PID_v1.h"

//These are needed for MPU
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// 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
MPU6050 mpu;
 
#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
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
VectorInt16 gy;         // [x, y, z]            gyro sensor measurements


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}

#define PID_MIN_LIMIT -255  //Min limit for PID 
#define PID_MAX_LIMIT 255  //Max limit for PID 
#define PID_SAMPLE_TIME_IN_MILLI 10  //This is PID sample time in milliseconds

//The pitch angle given by MPU6050 when robot is vertical and MPU6050 is horizontal is 0 in ideal case.
//However in real case its slightly off and we need add some correction to keep robot vertical.
//This is the angle correction to keep our robot stand vertically. Sometimes robot moves in one direction so we need to adjust this.
#define SETPOINT_PITCH_ANGLE_OFFSET -2.2 

#define MIN_ABSOLUTE_SPEED 0  //Min motor speed 

double setpointPitchAngle = SETPOINT_PITCH_ANGLE_OFFSET;
double pitchGyroAngle = 0;
double pitchPIDOutput = 0;

double setpointYawRate = 0;
double yawGyroRate = 0;
double yawPIDOutput = 0;

#define PID_PITCH_KP 20
#define PID_PITCH_KI 70 
#define PID_PITCH_KD 0.5

#define PID_YAW_KP 0.1
#define PID_YAW_KI 0.1
#define PID_YAW_KD 0

PID pitchPID(&pitchGyroAngle, &pitchPIDOutput, &setpointPitchAngle, PID_PITCH_KP, PID_PITCH_KI, PID_PITCH_KD, DIRECT);
PID yawPID(&yawGyroRate, &yawPIDOutput, &setpointYawRate, PID_YAW_KP, PID_YAW_KI, PID_YAW_KD, DIRECT);

int enableMotor1=9;
int motor1Pin1=5;
int motor1Pin2=6;

int motor2Pin1=7;
int motor2Pin2=8;
int enableMotor2=10;

void setupPID()
{
  pitchPID.SetOutputLimits(PID_MIN_LIMIT, PID_MAX_LIMIT);
  pitchPID.SetMode(AUTOMATIC);
  pitchPID.SetSampleTime(PID_SAMPLE_TIME_IN_MILLI);

  yawPID.SetOutputLimits(PID_MIN_LIMIT, PID_MAX_LIMIT);
  yawPID.SetMode(AUTOMATIC);
  yawPID.SetSampleTime(PID_SAMPLE_TIME_IN_MILLI);
    
}

void setupMotors()
{
  pinMode(enableMotor1,OUTPUT);
  pinMode(motor1Pin1,OUTPUT);
  pinMode(motor1Pin2,OUTPUT);
  
  rotateMotor(0);
}

void setupMPU()
{

  // 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
  
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
//           X Accel  Y Accel  Z Accel   X Gyro   Y Gyro   Z Gyro
//OFFSETS    -2334,     -571,    1192,    68,      -70,      -23
  mpu.setXAccelOffset(-2334); 
  mpu.setYAccelOffset(-571); 
  mpu.setZAccelOffset(1192);   
  mpu.setXGyroOffset(68);
  mpu.setYGyroOffset(-70);
  mpu.setZGyroOffset(-23);  
  // 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);
      // turn on the DMP, now that it's ready
      mpu.setDMPEnabled(true);
      mpuIntStatus = mpu.getIntStatus();
      dmpReady = true;
      // get expected DMP packet size for later comparison
      packetSize = mpu.dmpGetFIFOPacketSize();
  } 
  else 
  {
      // ERROR!
  }
}

void setup()
{
  //This is to set up motors
  setupMotors();   
  //This is to set up MPU6050 sensor
  setupMPU();
  //This is to set up PID 
  setupPID();
}

void loop()
{
  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  // read a packet from FIFO. Get the Latest packet
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) 
  {  
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    mpu.dmpGetGyro(&gy, fifoBuffer);

    yawGyroRate = gy.z;                   //rotation rate in degrees per second
    pitchGyroAngle = ypr[1] * 180/M_PI;   //angle in degree

    pitchPID.Compute(true);
    yawPID.Compute(true);

    rotateMotor(pitchPIDOutput+yawPIDOutput);

    #ifdef PRINT_DEBUG_BUILD
      Serial.println("The gyro  before ");
      Serial.println(pitchGyroAngle);
      Serial.println("The setpoints ");
      Serial.println(setpointPitchAngle);
      Serial.println("The pid output ");
      Serial.println(pitchPIDOutput);
      delay(500);    
    #endif
  }
}

void rotateMotor(int speed1)
{
  if (speed1 < 0)
  {
    digitalWrite(motor1Pin1,HIGH);
    digitalWrite(motor1Pin2,LOW);    
  }
  else if (speed1 >= 0)
  {
    digitalWrite(motor1Pin1,LOW);
    digitalWrite(motor1Pin2,HIGH);      
  }

 
  speed1 = abs(speed1) + MIN_ABSOLUTE_SPEED;

  speed1 = constrain(speed1, MIN_ABSOLUTE_SPEED, 255); // speed1 always in [0,255]
    
  analogWrite(enableMotor1,speed1);
}