FCXI.ino

/*
Go ahead and use this for whatever you want. Credit would be nice, but I can't stop you and if this helps you in any way, I'm cool with it. Besides, I didn't even write a few chunks of this.

Author: Zweiter
Version: 1.1


*/


//Declaring Variables
#include <Wire.h>
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23

int Addr = 105;                 // I2C address of L3G4200D gyro
int x, y, z;
int millislast = 0;
int avg = 0;
int count = 2000;

double pitchOffset, rollOffset, yawOffset; //y = pitch, x = roll, z = yaw
double gyro_pitch, gyro_roll, gyro_yaw, gyro_pitch_est, gyro_roll_est, gyro_yaw_est;
int cal;
byte highByte, lowByte;

byte last_channel_1, last_channel_2, last_channel_3, last_channel_4;
int receiver_input_channel_1, receiver_input_channel_2, receiver_input_channel_3, receiver_input_channel_4;
int counter_channel_1, counter_channel_2, counter_channel_3, counter_channel_4, start;
unsigned long timer_channel_1, timer_channel_2, timer_channel_3, timer_channel_4, esc_timer, esc_loop_timer;
unsigned long zero_timer, timer_1, timer_2, timer_3, timer_4, current_time;
int esc1, esc2, esc3, esc4;
  
float KPp, KIp, KDp, KPr, KIr, KDr, KPy, KIy;
float errorTemp, prevErrorP, prevErrorR, prevErrorY, integralP, integralR, integralY, derivativeP, derivativeR, derivativeY;
float setPointP, setPointR, setPointY, throttle;
float outputPitch, outputRoll, outputYaw;


void setup(){
  Serial.begin(9600);
  Wire.begin();
  DDRD |= B11110000;                                 //Set ports 4, 5, 6 and 7 to output
  DDRB |= B00010000;                                 //Set port 12 to output
  
  PCICR |= (1 << PCIE0);                             // set PCIE0 to enable PCMSK0 scan
  PCMSK0 |= (1 << PCINT0);                           // set PCINT0 (digital input 8) to trigger an interrupt on state change
  PCMSK0 |= (1 << PCINT1);                           // set PCINT1 (digital input 9) to trigger an interrupt on state change
  PCMSK0 |= (1 << PCINT2);                           // set PCINT2 (digital input 10) to trigger an interrupt on state change
  PCMSK0 |= (1 << PCINT3);                           // set PCINT3 (digital input 11) to trigger an interrupt on state change
  
  KPp = .18; //previously used: .23
  KIp = .0000;
  KDp = 5.6;//previously used: 4.5
  
  KPr = .18;
  KIr = .0000;
  KDr = 5.6;
  
  KPy = .4;
  KIy = .0002;
  
  setupGyro();
  
  ///Wait until connection with controller is established and the throttle is set to zero.
  while(receiver_input_channel_3 < 990 || receiver_input_channel_3 > 1020 || receiver_input_channel_4 < 1400){
    start ++;                                        
    PORTD |= B11110000;                              //Set ports 4, 5, 6 and 7 to HIGH.
    delayMicroseconds(1000);                         //Wait 1000us. (sends a PWM pulse of 1000)
    PORTD &= B00001111;                              //Set ports 4, 5, 6 and 7 low.
    delay(3);                                        
    if(start == 125){                                //Basically, all this part does is flash the LED every so often.
      digitalWrite(12, !digitalRead(12));            
      start = 0;                                     
    }
  }
  start = 0;
  digitalWrite(12, HIGH);                             //Turn on the led.
  zero_timer = micros();                              //Set the zero_timer for the first loop.
}

void loop(){ //Main loop

  //Roll
  if(receiver_input_channel_1 > 1508)setPointR = (receiver_input_channel_1 - 1508)/-2.0;
  else if(receiver_input_channel_1 < 1492)setPointR = (receiver_input_channel_1 - 1492)/-2.0;
  
  //Pitch
  if(receiver_input_channel_2 > 1508)setPointP = (receiver_input_channel_2 - 1508)/2.0;
  else if(receiver_input_channel_2 < 1492)setPointP = (receiver_input_channel_2 - 1492)/2.0;
  
  //Throttle
  throttle = (receiver_input_channel_3);
  
  //Yaw
  if(receiver_input_channel_4 > 1508)setPointY = (receiver_input_channel_4 - 1508)/2.0;
  else if(receiver_input_channel_4 < 1492)setPointY = (receiver_input_channel_4 - 1492)/2.0;
  
  
  
  gyro_read();
  calculate_pid();
  
  esc1 = throttle - outputPitch + outputRoll - outputYaw;
  esc2 = throttle + outputPitch - outputRoll - outputYaw;
  esc3 = throttle + outputPitch + outputRoll + outputYaw;
  esc4 = throttle - outputPitch - outputRoll + outputYaw;
  /*
  Serial.println(esc1);
  Serial.println(esc2);
  Serial.println(esc3);
  Serial.println(esc4);
  Serial.println();*/
  
  if(receiver_input_channel_4 < 1040 && receiver_input_channel_3 < 1040){  //If throttle is pushed to lower left corner, reset orientation, recalibrate gyro, and reset integral value in PID controller.
    gyro_pitch_est = 0;
    gyro_roll_est = 0;
    gyro_yaw_est = 0;
    
    outputYaw = 0;
    outputPitch = 0;
    outputRoll = 0;
    
    integralP = 0;
    integralR = 0;
    integralY = 0;
      
    prevErrorP = 0;
    prevErrorY = 0;
    prevErrorR = 0;
    calibrateGyro(); 
  }
  while(zero_timer + 4000 > micros());                       //Make sure each loop() is at least 4ms long.
  zero_timer = micros();                                     //Reset zero timer.
  PORTD |= B11110000;
  timer_channel_1 = esc1 + zero_timer;                       //Determine how long each port should be set to HIGH for. (Add the correction (in units of PWM) to the zero timer)
  timer_channel_2 = esc2 + zero_timer;   
  timer_channel_3 = esc3 + zero_timer;   
  timer_channel_4 = esc4 + zero_timer;   
  
  while(PORTD >= 16){                                        //Execute the loop until digital port 4 til 7 is low.
    esc_loop_timer = micros();                               //Check the current time.
    if(timer_channel_1 <= esc_loop_timer)PORTD &= B11101111; //digital port 4 is set low.
    if(timer_channel_2 <= esc_loop_timer)PORTD &= B11011111; //digital port 5 is set low.
    if(timer_channel_3 <= esc_loop_timer)PORTD &= B10111111; //digital port 6 is set low.
    if(timer_channel_4 <= esc_loop_timer)PORTD &= B01111111; //digital port 7 is set low.
  }
}

//interrupt routine
ISR(PCINT0_vect){
  current_time = micros();
  //Channel 1=========================================
  if(PINB & B00000001){                                        //Is input 8 high?
    if(last_channel_1 == 0){                                   //Input 8 changed from 0 to 1
      last_channel_1 = 1;                                      //Remember current input state
      timer_1 = current_time;                                  //Set timer_1 to current_time
    }
  }
  else if(last_channel_1 == 1){                                //Input 8 is not high and changed from 1 to 0
    last_channel_1 = 0;                                        //Remember current input state
    receiver_input_channel_1 = current_time - timer_1;         //Channel 1 is current_time - timer_1
  }
  //Channel 2=========================================
  if(PINB & B00000010 ){                                       //Is input 9 high?
    if(last_channel_2 == 0){                                   //Input 9 changed from 0 to 1
      last_channel_2 = 1;                                      //Remember current input state
      timer_2 = current_time;                                  //Set timer_2 to current_time
    }
  }
  else if(last_channel_2 == 1){                                //Input 9 is not high and changed from 1 to 0
    last_channel_2 = 0;                                        //Remember current input state
    receiver_input_channel_2 = current_time - timer_2;         //Channel 2 is current_time - timer_2
  }
  //Channel 3=========================================
  if(PINB & B00000100 ){                                       //Is input 10 high?
    if(last_channel_3 == 0){                                   //Input 10 changed from 0 to 1
      last_channel_3 = 1;                                      //Remember current input state
      timer_3 = current_time;                                  //Set timer_3 to current_time
    }
  }
  else if(last_channel_3 == 1){                                //Input 10 is not high and changed from 1 to 0
    last_channel_3 = 0;                                        //Remember current input state
    receiver_input_channel_3 = current_time - timer_3;         //Channel 3 is current_time - timer_3
  }
  //Channel 4=========================================
  if(PINB & B00001000 ){                                       //Is input 11 high?
    if(last_channel_4 == 0){                                   //Input 11 changed from 0 to 1
      last_channel_4 = 1;                                      //Remember current input state
      timer_4 = current_time;                                  //Set timer_4 to current_time
    }
  }
  else if(last_channel_4 == 1){                                //Input 11 is not high and changed from 1 to 0
    last_channel_4 = 0;                                        //Remember current input state
    receiver_input_channel_4 = current_time - timer_4;         //Channel 4 is current_time - timer_4
  }
}

void setupGyro(){
  writeI2C(CTRL_REG1, 0x1F);                      // Turn on all axes, disable power down
  writeI2C(CTRL_REG3, 0x08);                      // Enable ready signal
  writeI2C(CTRL_REG4, 0x80);                      // Set scale (500 deg/sec)
  delay(150);                                     
  rollOffset = 0;
  pitchOffset = 0;
  yawOffset = 0;
  //Loop to take gyro readings and divide to find the average drift
  for(int cal = 0; cal <= 1500; cal++){             
    gyro_read();
    
    //add up the gyro drift to be divided at end of loop
    rollOffset += gyro_roll;                      
    pitchOffset += gyro_pitch;                    
    yawOffset += gyro_yaw;                       
    
    if(cal%200 == 0){
      digitalWrite(12, !digitalRead(12));
    }
    delay(2);                                     
  }
  Serial.println(" Result: ");
  Serial.print("pitchOffset = "); pitchOffset /= 1500; Serial.println(pitchOffset); 

  Serial.print("rollOffset = "); rollOffset /= 1500; Serial.println(rollOffset); 

  Serial.print("yawOffset = "); yawOffset /= 1500; Serial.println(yawOffset);


  delay(1000);
  gyro_pitch_est = 0;
  gyro_roll_est = 0;
  gyro_yaw_est = 0;
  zero_timer = micros();


}
void calibrateGyro(){
  delay(150);                                     // Wait to synchronize 
  rollOffset = 0;
  pitchOffset = 0;
  yawOffset = 0;
  // For loop to take gyro readings and divide to find the average drift
  for(int cal = 0; cal <= 500; cal++){             
    gyro_read();
    
    //add up the gyro drift to be divided at end of loop
    rollOffset += gyro_roll;                      
    pitchOffset += gyro_pitch;                    
    yawOffset += gyro_yaw;                       
    
    if(cal%200 == 0){
      digitalWrite(12, !digitalRead(12));
    }
    delay(2);                                     
  }
  Serial.println(" Result: ");
  Serial.print("pitchOffset = "); pitchOffset /= 500; Serial.println(pitchOffset); 

  Serial.print("rollOffset = "); rollOffset /= 500; Serial.println(rollOffset); 

  Serial.print("yawOffset = "); yawOffset /= 500; Serial.println(yawOffset);


  delay(500);
  gyro_pitch_est = 0;
  gyro_roll_est = 0;
  gyro_yaw_est = 0;
  zero_timer = micros();  
  
}
void gyro_read(){ //Approximately .97 milliseconds (970 microseconds)

//This stuff is lifted pretty much straight from a tutorial on YouTube, I'm pretty bad at I2C stuff.

  Wire.beginTransmission(105);                       //Start communication with the gyro (adress 105)
  Wire.write(168);                                   //Start reading @ register 28h and auto increment with every read
  Wire.endTransmission();                            //End the transmission
  Wire.requestFrom(105, 6);                          //Request 6 bytes from the gyro
  while(Wire.available() < 6);                       //Wait until the 6 bytes are received
  lowByte = Wire.read();                             //First received byte is the low part of the angular data
  highByte = Wire.read();                            //Second received byte is the high part of the angular data
  gyro_roll = ((highByte<<8)|lowByte);               //Multiply highByte by 256 and add lowByte
  //if(cal == 3000)     //Only compensate after the calibration
  lowByte = Wire.read();                             //First received byte is the low part of the angular data
  highByte = Wire.read();                            //Second received byte is the high part of the angular data
  gyro_pitch = ((highByte<<8)|lowByte);              //Multiply highByte by 256 and ad lowByte
  gyro_pitch *= -1;                                  //Invert axis
  //if(cal == 3000);   //Only compensate after the calibration
  lowByte = Wire.read();                             //First received byte is the low part of the angular data
  highByte = Wire.read();                            //Second received byte is the high part of the angular data
  gyro_yaw = ((highByte<<8)|lowByte);                //Multiply highByte by 256 and ad lowByte
  gyro_yaw *= -1;                                    //Invert axis
  //if(cal == 3000)  gyro_yaw_est += (gyro_yaw - yawOffset)/900;       //Only compensate after the calibration
}

//This function calculates the PID values. Right now, only the PD values are used, so the integral is fairly useless until I decide to implement it.
void calculate_pid(){
  gyro_pitch_est += (gyro_pitch - pitchOffset)/900; //This is where the quad estimates its orientation.
  errorTemp = gyro_pitch_est - setPointP; //This is the proprtional term, which is fairly basic.
  integralP += errorTemp; //The integral is just the area under the curve, so all we have to do is keep adding the errors together.
  derivativeP = (errorTemp - prevErrorP); //calculate the rate of change based on this loop's error and last loop's error.
  prevErrorP = errorTemp;
  outputPitch = KPp*errorTemp + KIp*integralP + KDp*derivativeP;
  if(outputPitch > 400) outputPitch = 400; //make sure the output stays within a certain range.
  if(outputPitch < -400) outputPitch = -400;
  
  gyro_roll_est += (gyro_roll - rollOffset)/900;
  errorTemp = gyro_roll_est-setPointR;
  integralR += errorTemp;
  derivativeR = (errorTemp - prevErrorR);
  prevErrorR = errorTemp;
  outputRoll = KPr*errorTemp + KIr*integralR + KDr*derivativeR;
  if(outputRoll > 400) outputRoll = 400;
  if(outputRoll < -400) outputRoll = -400;
  
  gyro_yaw_est += (gyro_yaw - yawOffset)/900;
  
  if(throttle > 1030){
    errorTemp = gyro_yaw_est - setPointY;
  }else{
    errorTemp = 0;
    prevErrorY = 0;
    outputYaw = 0;
    outputPitch = 0;
    outputRoll = 0;
    integralP = 0;
    integralR = 0;
    integralY = 0;
    prevErrorP = 0;
    prevErrorY = 0;
    prevErrorR = 0;  
  }
  integralY += errorTemp;
  prevErrorY = errorTemp;
  outputYaw = KPy*errorTemp + KIy*integralY;
  if(outputYaw > 400) outputYaw = 400;
  if(outputYaw < -400) outputYaw = -400;

  //Serial.print(outputPitch); Serial.print(" "); Serial.print(outputRoll); Serial.print(" "); Serial.println(outputYaw);

}

void writeI2C (byte regAddr, byte val) {
    Wire.beginTransmission(Addr);
    Wire.write(regAddr);
    Wire.write(val);
    Wire.endTransmission();
}