quadcopter

#include <PID_v1.h>12
#include <Servo.h>
#include <FreeSixIMU.h>
#include <FIMU_ADXL345.h>
#include <FIMU_ITG3200.h>

#include <Wire.h>

#define FRONT_LEFT 0      //FRONT LEFT MOTOR
#define BACK_LEFT 1    //FRONT RIGHT MOTOR
#define BACK_RIGHT 2      //BACK LEFT MOTOR
#define FRONT_RIGHT 3     //BACK RIGHT MOTOR

float angles[3]; // yaw pitch roll

//basespeed to turn the motors on
int BaseSpeed = 40;

//conservative yaw PID
#define consyawKP .25
#define consyawKI 0
#define consyawKD .1
//conservative pitch PID
#define conspitchKP .15
#define conspitchKI 0
#define conspitchKD .05
//Conservative roll PID
#define consrollKP .15
#define consrollKI 0
#define consrollKD .05

//setpoints, inputs and output variables for each PID object
double yawSetpoint,yaw_DIRECT_Output,yaw_REVERSE_Output,yawInput;
double pitchSetpoint,pitch_DIRECT_Output,pitch_REVERSE_Output,pitchInput;
double rollSetpoint,roll_DIRECT_Output,roll_REVERSE_Output,rollInput;

// Set the FreeSixIMU object
FreeSixIMU sixDOF = FreeSixIMU();


//Setup PID object
PID yaw_DIRECT_PID(&yawInput,&yaw_DIRECT_Output,&yawSetpoint,consyawKP,consyawKI,consyawKD,DIRECT); //PID for yaw
PID yaw_REVERSE_PID(&yawInput,&yaw_REVERSE_Output,&yawSetpoint,consyawKP,consyawKI,consyawKD,REVERSE); //PID for yaw
PID pitch_DIRECT_PID(&pitchInput,&pitch_DIRECT_Output,&pitchSetpoint,conspitchKP,conspitchKI,conspitchKD,DIRECT); //PID for pitch
PID pitch_REVERSE_PID(&pitchInput,&pitch_REVERSE_Output,&pitchSetpoint,conspitchKP,conspitchKI,conspitchKD,REVERSE); //PID for pitch
PID roll_DIRECT_PID(&rollInput,&roll_DIRECT_Output,&rollSetpoint,consrollKP,consrollKI,consrollKD,DIRECT); //PID for roll
PID roll_REVERSE_PID(&rollInput,&roll_REVERSE_Output,&rollSetpoint,consrollKP,consrollKI,consrollKD,REVERSE); //PID for roll

//Declare Servo array
Servo motors[4];

//attach motors to appropriate pins
void init_motors()
{
  motors[0].attach(8);
  motors[1].attach(9);
  motors[2].attach(10);
  motors[3].attach(11);
  Serial.println("Motors initialized."); // let us know when you're done initializing motors
}

//arm the motors (turn them on)
void armAll(){
  // arm the speed controller, modify as necessary for your ESC
  setSpeed(0,0);
  setSpeed(0,1);
  setSpeed(0,2);
  setSpeed(0,3);
  delay(1000); //delay 1 second,  some speed controllers may need longer

}
//set the desired speed with params of speed and id of motor to set
void setSpeed(int speed,int id){
  // speed is from 0 to 100 where 0 is off and 100 is maximum speed
  //the following maps speed values of 0-100 to angles from 0-180,
  // some speed controllers may need different values, see the ESC instructions
  Serial.print("Motor: ");
  Serial.print(id,DEC);
  Serial.print(" Speed changed to: ");
  Serial.print(speed,DEC);
  Serial.println();
  int angle = map(speed, 0, 100, 0, 180);
  motors[id].write(angle);
//  Serial.print("Motor: ");
//  Serial.print(id,DEC);
//  Serial.print(" Speed: ");
//  Serial.print(speed,DEC);
//  Serial.println();

}
void setup() {
  Serial.begin(115200);  //initialize serial port
  Wire.begin();          //start I2C bus

  init_motors(); //attach pins to motors
  armAll();      //initialize all motor speeds to 0

  sixDOF.init();  //initialize 6dof
  sixDOF.getEuler(angles);  //initial reading of IMU

  yawInput = angles[0];
  pitchInput = angles[1];  //assign input value to pitch
  rollInput  = angles[2];  //assign input value to roll

  yawSetpoint = 0;
  pitchSetpoint = 0;    //we dont want any pitch ideally
  rollSetpoint = 0;    //same for roll

  yaw_DIRECT_PID.SetMode(AUTOMATIC);
  yaw_REVERSE_PID.SetMode(AUTOMATIC);
  pitch_DIRECT_PID.SetMode(AUTOMATIC);      //set PID modes to AUTOMATIC
  pitch_REVERSE_PID.SetMode(AUTOMATIC);
  roll_DIRECT_PID.SetMode(AUTOMATIC);
  roll_REVERSE_PID.SetMode(AUTOMATIC);

  Serial.print("pitch P: ");
  Serial.print(pitch_DIRECT_PID.GetKp());
  Serial.print(" I: ");
  Serial.print(pitch_DIRECT_PID.GetKi());
  Serial.print (" D: ");
  Serial.print(pitch_DIRECT_PID.GetKd());
  Serial.println();
}

//helper function to print out double data types

//we want hover mode enabled for right now
boolean hover = true;
//correct the pitch and roll
void correctYawPitchRoll(double adjusted_DIRECT_Yaw,double adjusted_REVERSE_Yaw,double adjusted_DIRECT_Pitch,double adjusted_REVERSE_Pitch,double adjusted_DIRECT_Roll,double adjusted_REVERSE_Roll,double Basespeed)
{
 double CommandYaw;
 double CommandPitch;
 double CommandRoll;

 CommandYaw = adjusted_REVERSE_Yaw - adjusted_DIRECT_Yaw;
 CommandPitch = adjusted_REVERSE_Pitch - adjusted_DIRECT_Pitch;
 CommandRoll = adjusted_REVERSE_Roll - (1.1*adjusted_DIRECT_Roll);

 setSpeed(constrain(Basespeed - CommandPitch + CommandRoll + CommandYaw,.8*Basespeed,1.2*Basespeed), FRONT_LEFT);
 setSpeed(constrain(Basespeed - CommandPitch - CommandRoll - CommandYaw,.8*Basespeed,1.2*Basespeed), FRONT_RIGHT);
 setSpeed(constrain(Basespeed + CommandPitch + CommandRoll - CommandYaw,.8*Basespeed,1.2*Basespeed), BACK_LEFT);
 setSpeed(constrain(Basespeed + CommandPitch - CommandRoll + CommandYaw,.8*Basespeed,1.2*Basespeed), BACK_RIGHT);

}

//main loop
void loop() {

  //if hover mode disengaged, set speed to 30 (shuts off) and hold
  if(hover == false)
  {
    setSpeed(30,0);
    setSpeed(30,1);
    setSpeed(30,2);
    setSpeed(30,3);
  }
  else
  {
    //otherwise initialize to basespeed on all
    setSpeed(BaseSpeed,FRONT_LEFT);
    setSpeed(BaseSpeed,FRONT_RIGHT);
    setSpeed(BaseSpeed,BACK_LEFT);
    setSpeed(BaseSpeed,BACK_RIGHT);
    //enter loop after speed set
    while(hover)
    {
      //if we get a message, parse the number
    if(Serial.available() == 3)
      {
          int hundreds = Serial.read() - '0'; // read hundreds place
          int tens = Serial.read() - '0';      //read tens place
          int ones = Serial.read() - '0';      //read ones
          int input = ((ones) + (tens*10) + hundreds*100);
         //if input matches kill signal, kill
           BaseSpeed = input;
          if(input == 123)
          {
            Serial.println("Kill Sequence Initiated...Powering down...");
            hover = false; //break loop and hold
          }
         // else
         // {
         //   BaseSpeed = input;
         // }

      }
      else
      {
        //otherwise, read sensors and compensate

        sixDOF.getEuler(angles);    //read 6dof

        yawInput = angles[0];
        pitchInput = angles[1];     //assign new readings appropriately
        rollInput  = angles[2];

        yaw_DIRECT_PID.Compute();
        yaw_REVERSE_PID.Compute();
        pitch_DIRECT_PID.Compute();
        pitch_REVERSE_PID.Compute();
        roll_DIRECT_PID.Compute();
        roll_REVERSE_PID.Compute();

        correctYawPitchRoll(yaw_DIRECT_Output, yaw_REVERSE_Output,pitch_DIRECT_Output,pitch_REVERSE_Output,roll_DIRECT_Output,roll_REVERSE_Output,BaseSpeed);
      }
    }
  }


}