PID_CHICKEN.ino

#include <Wire.h>

//initialize variables
int in1 = 9;
int in2 = 8;
int ena = 10;
int enb = 5;
int in3 = 7;
int in4 = 6;
static float stp = 0.0;
static float a[5] = {};
float  Kp = 0.5, Ki = 1.0, Kd = 1.5;
static int por, flag = 0, flag1 = 0, ratelimit = 10;
String inText;
static float value0, value1, value2, err, avg, interv = 0.0, c = 0.0, avgvalue;
int inCommand = 0;
int sensorType = 0;
unsigned long logCount = 0L;
char getChar = ' ';
static long latertime = 0;
double dt = 0.01;
float preverr = 0.0, deri = 0.0;
int dir = 1;

static float pout = 0.0;
float inputrate = 0.0, out = 0.0, finout = 0.0;

int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;

long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;

float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;

long loop_timer, count = 0;
int temp;


//in1 L in2 H in3 H in4 L BACK
//in1 H in2 L in3 L in4 H FORWARD
// if err > 0 move backward

void setup_mpu_6050_registers() {
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x6B);                                                    //Send the requested starting register
  Wire.write(0x00);                                                    //Set the requested starting register
  Wire.endTransmission();
  //Configure the accelerometer (+/-8g)
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1C);                                                    //Send the requested starting register
  Wire.write(0x10);                                                    //Set the requested starting register
  Wire.endTransmission();
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1B);                                                    //Send the requested starting register
  Wire.write(0x08);                                                    //Set the requested starting register
  Wire.endTransmission();
}


void read_mpu_6050_data() {                                            //Subroutine for reading the raw gyro and accelerometer data
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x3B);                                                    //Send the requested starting register
  Wire.endTransmission();                                              //End the transmission
  Wire.requestFrom(0x68, 14);                                          //Request 14 bytes from the MPU-6050
  while (Wire.available() < 14);                                       //Wait until all the bytes are received
  acc_x = Wire.read() << 8 | Wire.read();
  acc_y = Wire.read() << 8 | Wire.read();
  acc_z = Wire.read() << 8 | Wire.read();
  temp = Wire.read() << 8 | Wire.read();
  gyro_x = Wire.read() << 8 | Wire.read();
  gyro_y = Wire.read() << 8 | Wire.read();
  gyro_z = Wire.read() << 8 | Wire.read();
}



void setup() {
  Serial.begin(115200);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(ena, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enb, OUTPUT);
  Wire.begin();                                                        //Start I2C as master
  setup_mpu_6050_registers();                                          //Setup the registers of the MPU-6050
  for (int cal_int = 0; cal_int < 1000 ; cal_int ++) {                 //Read the raw acc and gyro data from the MPU-6050 for 1000 times
    read_mpu_6050_data();
    gyro_x_cal += gyro_x;                                              //Add the gyro x offset to the gyro_x_cal variable
    gyro_y_cal += gyro_y;                                              //Add the gyro y offset to the gyro_y_cal variable
    gyro_z_cal += gyro_z;                                              //Add the gyro z offset to the gyro_z_cal variable
    delay(3);
  }
  // divide by 1000 to get avarage offset
  gyro_x_cal /= 1000;
  gyro_y_cal /= 1000;
  gyro_z_cal /= 1000;
  loop_timer = micros();
  
}


void loop() {
  read_mpu_6050_data();
  //Subtract the offset values from the raw gyro values
  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;

  //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
  angle_pitch += gyro_x * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable
  angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable
  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel

  //Accelerometer angle calculations
  acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z)); //Calculate the total accelerometer vector
  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
  angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;    //Calculate the pitch angle
  angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;    //Calculate the roll angle

  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch
  angle_roll_acc -= 0.0;                                               //Accelerometer calibration value for roll

  if (set_gyro_angles) {                                               //If the IMU is already started
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle
  }
  else {                                                               //At first start
    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle
    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle
   
    set_gyro_angles = true;                                            //Set the IMU started flag
  }

  //To dampen the pitch and roll angles a complementary filter is used
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value


  count += 1;
  if (count > 1200)
    por = 0.0;
  else if (count > 1050)
    por = -20.0;
  else if (count > 900)
    por = 20.0;
  else if (count > 600)
    por = -20.0;
  else if (count > 150)
    por = 20.0;
  else
    por = 0;

  //averaging values
  if (flag == 0) {
    a[0] = a[1] = a[2] = a[3] = a[4] = angle_pitch_output;
    flag = 1;
  }
  else if (flag == 1) {
    for (int i = 0; i <= 3; i++) {
      a[i] = a[i + 1];
    }
    a[4] = angle_pitch_output;
  }

  avgvalue = (a[0] + a[1] + a[2] + a[3] + a[4]) / 5.0;

//  long startime = millis();
//  dt = (startime - latertime) / 1000.0;
//  
//  //rate limiter
//  inputrate = (stp - pout) / dt;
//  if (inputrate > ratelimit)
//    out = ratelimit * dt + pout;
//  else if (inputrate < ratelimit)
//    out = pout - (ratelimit * dt);
//  else
//    out = stp;
//  pout = out;
//
//  //pid
//  stp = 0.0;
//  err = stp - avgvalue;
//  //deadband on error
//  if (abs(err) < 0.7){
//  err=0;
//  interv=0.0;
//  }  
//  interv = interv + err * dt;
//  deri = (err - preverr) / dt;
//  
//  if (interv > 20) {
//    interv = 20;
//  }
//  else if (interv < -20) {
//    interv = -20;
//  }
//
//  //damping
//  por = err * Kp + interv * Ki + Kd * deri;
//  preverr = err;
//
//  //kick
//  if(err > 3){
//    por = por + 30;
//  }
//  else if(err < -3){
//    por = por - 35;
//  }

  //deadband
  if(por > 0){
    finout = 50.0 + (por/255.0)*(255.0-50.0);
    digitalWrite(in1 , HIGH);
    digitalWrite(in2 , LOW);
    analogWrite(ena, min(abs(finout), 250));
  }
  else if(por < 0){
    finout = -57.0 + (por/-255.0)*(-255.0+57.0);
    digitalWrite(in2 , HIGH);
    digitalWrite(in1 , LOW);
    analogWrite(ena, min(abs(finout), 250));
  }
  else{
    finout = 0; 
    analogWrite(ena, 0);
  }
  
  //testing
  Serial.print(por); Serial.print(" , ");
  Serial.println(avgvalue, 4);
  Serial.print(out, 4); Serial.print(" , ");
  Serial.print(err, 4); Serial.print(" , ");
  Serial.print(por); Serial.print(" , ");
  Serial.print(dt, 5); Serial.print(" , ");
  Serial.print(finout, 4); Serial.print(" , ");
  Serial.print(dir); Serial.print(" , ");
  Serial.println(interv, 4);

  latertime = startime;

}