Self Balancing Robot Arduino

Self Balancing Robot.ino

#include <PID_v1.h>
#include <Wire.h>

const int MPU_addr=0x68;
double AccelX,AccelY,AccelZ,Tmp,GyroX,GyroY,GyroZ; //These will be the raw data from the MPU6050.
uint32_t timer; //it's a timer, saved as a big-ass unsigned int.  We use it to save times from the "micros()" command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop.
double Angle_X, Angle_Y; //These are the angles in the complementary filter
#define degconvert 57.29577951 //there are like 57 degrees in a radian.

#define m1_left   12
#define m1_right   10
#define m1_en 11
#define m2_left  3
#define m2_right  4
#define m2_en 9

bool f;

double offset = 0.3;
double f_set = 0;
double b_set = 0;

double Kp = 145, Kpx = 225;
double Kd = 120, Kdx = 170; 
double Ki = 5,   Kix = 20;

double targetAngle = 0;
double input, output;
volatile int control = 0;
volatile float currentAngle, prevAngle=0, error, prevError=0, errorSum=0;

PID pid(&input, &output, &targetAngle, Kp, Ki, Kd, DIRECT);
void setup() {

  pinMode(m1_left, OUTPUT);
  pinMode(m1_right, OUTPUT);
  pinMode(m2_left, OUTPUT);
  pinMode(m2_right, OUTPUT);
  Wire.begin();
  #if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
  #else
    TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
  #endif

  
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(57600);
  delay(100);

  //setup starting angle
  //1) collect the data
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AccelX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AccelY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AccelZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyroY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyroZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)


  //2) calculate pitch and roll
  double roll = atan2(AccelY, AccelZ)*degconvert;
  double pitch = atan2(-AccelX, AccelZ)*degconvert;

  //3) set the starting angle to this pitch and roll
  double gyroXangle = roll;
  double gyroYangle = pitch;
  double Angle_X = roll;
  double Angle_Y = pitch;
  input = Angle_X + 3; 
  //start a timer
  timer = micros();
  pid.SetMode(AUTOMATIC);
  pid.SetSampleTime(5);
  pid.SetOutputLimits(-255, 255); 
}


void loop(){
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AccelX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AccelY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AccelZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyroX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyroY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyroZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  double dt = (double)(micros() - timer) / 1000000; //This line does three things: 1) stops the timer, 2)converts the timer's output to seconds from microseconds, 3)casts the value as a double saved to "dt".
  timer = micros(); //start the timer again so that we can calculate the next dt.
  double roll = atan2(AccelY, AccelZ)*degconvert;
  double pitch = atan2(-AccelX, AccelZ)*degconvert;
  double gyroXrate = GyroX/131.0;
  double gyroYrate = GyroY/131.0; 

  Angle_X = 0.99 * (Angle_X + gyroXrate * dt) + 0.01 * roll; // Calculate the angle using a Complimentary filter
 // Angle_Y = 0.99 * (Angle_Y + gyroYrate * dt) + 0.01 * pitch; 
  currentAngle = Angle_X+3;
  input = currentAngle; 
  //Serial.println(currentAngle);
  if (currentAngle > 40.0 || currentAngle < -40.0)
    stop();
  else if(currentAngle > -0.3 && currentAngle < +0.3)
    stop();
  else {
    error = currentAngle - targetAngle;
    if (abs(error) < 3){
      pid.SetTunings(Kp, Ki, Kd);
    }
    else
      pid.SetTunings(Kpx, Kix, Kdx);
    pid.Compute();
    if(currentAngle > targetAngle && currentAngle < 40){
      Serial.println(output);
      command(false, output);
    }
    else if(currentAngle < targetAngle && currentAngle > -40)
      command(true, output);
    else
      stop();
  }
}

void command(bool flag, int pwm) {
  //Serial.println(pwm);
  if(flag == true) {
    analogWrite(m2_en, pwm);
    digitalWrite(m2_left, LOW);
    digitalWrite(m2_right, HIGH);
    analogWrite(m1_en, pwm);
    digitalWrite(m1_right, LOW);
    digitalWrite(m1_left, HIGH);
  }
  else {
    analogWrite(m2_en, abs(pwm));
    digitalWrite(m2_left, HIGH);
    digitalWrite(m2_right, LOW);
    analogWrite(m1_en, abs(pwm));
    digitalWrite(m1_right, HIGH);
    digitalWrite(m1_left, LOW);
  }
}
void stop(){
  digitalWrite(m2_right, LOW);
    digitalWrite(m1_right, LOW);
    digitalWrite(m2_left, LOW);
    digitalWrite(m1_left, LOW);
}