Po.ino

#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.

double currentAngle = 0;
#define m1_left   12
#define m1_right   10
#define m2_left  3
#define m2_right  4
float set = 0;
int f = 0;
int b = 0;
bool flag = false;
int count = 0;
int pwm_on = 0;
int pwm_off = 0;
void setup() {
  // Set up MPU 6050:
  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;

  //start a timer
  timer = micros();
}

void loop() {
//Now begins the main loop. 
  //Collect raw data from the sensor.
  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_Y+8;
pwm_adjust(currentAngle);
if(count==1200){
  if(f>b){
    set-=0.03;
  }
  else if(f<b){
    set+=0.03;
  }
  count = 0;
}
if(currentAngle<=set+0.4 && currentAngle>=set-0.4){
  flag = true;
  stop();}
else
  flag = false;
if(!flag){
  if(currentAngle>set && currentAngle<20){
    backward();    
  }else if(currentAngle>-20 && currentAngle<set){
    forward();          
  }else{
    stop();
  }
}
  count+=1;
}

void forward(){
  digitalWrite(m1_left, HIGH);
  digitalWrite(m1_right, LOW);
  digitalWrite(m2_left, HIGH);
  digitalWrite(m2_right, LOW);
  delay(pwm_on);

  digitalWrite(m1_left, LOW);
  digitalWrite(m1_right, LOW);
  digitalWrite(m2_left, LOW);
  digitalWrite(m2_right, LOW);
  delay(pwm_off);
  f+=1;
}

void backward(){
  digitalWrite(m1_left, LOW);
  digitalWrite(m1_right, HIGH);
  digitalWrite(m2_left, LOW);
  digitalWrite(m2_right, HIGH);
  delay(pwm_on);

  digitalWrite(m1_left, LOW);
  digitalWrite(m1_right, LOW);
  digitalWrite(m2_left, LOW);
  digitalWrite(m2_right, LOW);
  delay(pwm_off);
  b+=1;
}

void stop(){
  digitalWrite(m1_left, LOW);
  digitalWrite(m1_right, LOW);
  digitalWrite(m2_left, LOW);
  digitalWrite(m2_right, LOW);
  delay(pwm_on);
}

void pwm_adjust(int value_y){
  if(value_y > set && value_y < 1){
    //28% Duty Cycle
    pwm_on = 5; 
    pwm_off = 13;  
  }else if(value_y >= 1  && value_y < 3) {
    // 38 % Duty Cycle
    pwm_on = 7; // ms ON
    pwm_off = 11; // ms OFF
  }else if(value_y >= 3  && value_y < 10) {
    // 83 % Duty Cycle
    pwm_on = 15; // ms ON
    pwm_off = 3; // ms OFF
  }else if(value_y >= 10  && value_y < 25){
    // 100 % Duty Cycle
    pwm_on = 21; // ms ON
    pwm_off = 0; // ms OFF
  }else if(value_y <set  && value_y >-1) {
    pwm_on = 5; // ms ON
    pwm_off = 13; // ms OFF
  }else if(value_y <=-1 && value_y > -3){
    pwm_on = 7; // ms ON
    pwm_off = 11; // ms OFF  
  }else if(value_y <= -3  && value_y > -10) {
    pwm_on = 15; // ms ON
    pwm_off = 3; // ms OFF
  }else if(value_y <= -10  && value_y > -25){
    pwm_on = 21; // ms ON
    pwm_off = 0; // ms OFF
  }    
}