Quadcopter code

#include "I2Cdev.h"
#include <PID_v1.h>

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

long previousMillis = 0;


MPU6050 mpu;
#include <string.h> // we'll need this for subString

char serialbuf[32]; //This gives the incoming serial some room. Change it if you want a longer incoming.
#define MAX_STRING_LEN 15 // like 3 lines above, change as needed.
int xoff, yoff=0;
const char EOPmarker = '.'; //This is the end of packet marker

int throttleFL;;
int throttleFR;
int throttleBL;
int throttleBR;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
double Setpoint1, Input1, Output1;
double Setpoint2, Input2, Output2;

int frontright = 9;
int frontleft = 6;
int rearright = 5;
int rearleft = 10;

double accYangle, accXangle, YAngle, XAngle, YawAngle;


 static int bufpos = 0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* BACKUP
double Kp = 66.3;
double Ki = 1.235;
double Kd = 2.88;
*/
//double Kp = 69.3;
//double Ki = 0;
//double Kd = 3.08;

//double Kp = 55.3;
//double Ki = 0;
//double Kd = 0;
//
//
//
//
//double Kp1 = 56.8;
//double Ki1 = 0;
//double Kd1 = 0;


double Kp = 68.3;
double Ki = 1.235;
double Kd = 3.08;

double Kp2 = 65.9;
double Ki2 = 0.0;
double Kd2 = 0.01;

int   STARTSPEED = 0;
int val=0;
int xang=0;
int yang=0;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,Kp,Ki,Kd, REVERSE); //Y axis
PID myPID1(&Input1, &Output1, &Setpoint1,Kp,Ki,Kd, REVERSE); //X axis

PID myPID2(&Input2, &Output2, &Setpoint2,Kp2,Ki2,Kd2, REVERSE); //Yaw

char inchar;


// MPU control/status vars
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

VectorInt16 gyro;       // [x, y, z]            gyroscopic output


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
boolean interruptLock = false;




// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    Input = (YAngle);
    Input1 = (XAngle);
    Input2 = YawAngle;

    Setpoint = 0.0;
    Setpoint1 = 0.0;
    Setpoint2 = 0.0;


    pinMode(13, OUTPUT);
    pinMode(frontright, OUTPUT);
    pinMode(frontleft, OUTPUT);
    pinMode(rearright, OUTPUT);
    pinMode(rearleft, OUTPUT);
    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
     Serial.begin(115200);


     initMPU();


    myPID.SetOutputLimits(-90, 90);
    myPID1.SetOutputLimits(-90, 90);
    myPID2.SetOutputLimits(-90, 90);
    myPID.SetMode(AUTOMATIC);
    myPID1.SetMode(AUTOMATIC);
    myPID2.SetMode(AUTOMATIC);
    myPID.SetSampleTime(5);
    myPID1.SetSampleTime(5);
    myPID2.SetSampleTime(5);
    // configure LED for output
    //pinMode(LED_PIN, OUTPUT);
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ==============================//checkcommands==================================
void loop() {
    // if programming failed, don't try to do anything
    getYPR();

  //Serial.print(ypr[2]);
  //Serial.print('\t');
  //Serial.print(ypr[1]);
  //Serial.print('\t');
  //Serial.print(ypr[0]);
  //Serial.print('\t');
  //Serial.print(gyro.x);
  //Serial.println();
    YAngle = (ypr[1]);
    XAngle = (ypr[2]);
    YawAngle = ypr[0];

    checkcommands();

    Input = YAngle;
    Input1 = XAngle;
    Input2 = YawAngle;


    myPID.Compute();
    myPID1.Compute();
    myPID2.Compute();


    throttleFR = constrain((val + Output + Output1 - Output2 - xang + yang), 0, 255);
    throttleBR = constrain((val + Output - Output1 + Output2 - xang - yang), 0, 255);
    throttleFL = constrain((val - Output + Output1 + Output2 + xang + yang), 0, 255);
    throttleBL = constrain((val - Output - Output1 - Output2 + xang - yang), 0, 255);

    analogWrite(frontright, throttleFR);
    analogWrite(frontleft, throttleFL);
    analogWrite(rearright, throttleBR);
    analogWrite(rearleft, throttleBL);

//
// DEBUG sensors and PID vals
//    Serial.print("Y: "); //side to side
//    Serial.print(YAngle);
//    Serial.print("\t");
//    Serial.print("X: "); //forward to backward
//    Serial.print(XAngle);
//    Serial.print("\t");
//    Serial.print("Z: ");
//    Serial.println(YawAngle);
//    Serial.print("\t");
//    //Serial.print("FR: ");
//    Serial.print(throttleFR);
//    Serial.print("\t");
//    //Serial.print("BR: ");
//    Serial.print(throttleBR);
//    Serial.print("\t");
//    //Serial.print("FL: ");
//    Serial.print(throttleFL);
//    Serial.print("\t");
//    //Serial.print("BL: ");
//    Serial.println(throttleBL);
////////////////////////////////////////////////////////////////////////////////////////
// Makes sure Quadcopter stops propellors after a certain angle
    if((YAngle) > 1.0 || (YAngle) < -1.0)
    {
        analogWrite(frontright, 0);
        analogWrite(frontleft, 0);
        analogWrite(rearright, 0);
        analogWrite(rearleft, 0);
        delay(80000);
    }
    else if((XAngle) > 1.0 || (XAngle) < -1.0)
    {
        analogWrite(frontright, 0);
        analogWrite(frontleft, 0);
        analogWrite(rearright, 0);
        analogWrite(rearleft, 0);
        delay(80000);

    }





}




//SUBROUTINES




void checkcommands()
{
         while(Serial.available()>0){


            inchar = Serial.read();

            if(inchar == '\n') break;

            if (inchar == EOPmarker) { //if the incoming character is not the byte that is the incoming package ender
                          serialbuf[bufpos] = 0; //restart the buff
                          bufpos = 0; //restart the position of the buff
                          val  = atoi(subStr(serialbuf, ",", 1));
                          xang = atoi(subStr(serialbuf, ",", 2));
                          yang = atoi(subStr(serialbuf, ",", 3));
            }
            else { //once the end of package marker has been read
                    serialbuf[bufpos] = inchar; //the buffer position in the array get assigned to the current read
                    bufpos++; //once that has happend the buffer advances, doing this over and over again until the end of package marker is read.






              }
            }
}
char* subStr (char* input_string, char *separator, int segment_number) {
  char *act, *sub, *ptr;
  static char copy[MAX_STRING_LEN];
  int i;
  strcpy(copy, input_string);
  for (i = 1, act = copy; i <= segment_number; i++, act = NULL) {
    sub = strtok_r(act, separator, &ptr);
    if (sub == NULL) break;
  }
 return sub;
}


void initMPU(){

  Wire.begin();

  mpu.initialize();
  devStatus = mpu.dmpInitialize();
          digitalWrite(13, HIGH);
    delay(8000);

    digitalWrite(13, LOW);
      mpu.setXGyroOffset(38);
    mpu.setYGyroOffset(-28);
    mpu.setZGyroOffset(-33);
    mpu.setXAccelOffset(-2351);
    mpu.setYAccelOffset(502);
    mpu.setZAccelOffset(1617);
  if(devStatus == 0){

    mpu.setDMPEnabled(true);
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    packetSize = mpu.dmpGetFIFOPacketSize();

  }
}

inline void dmpDataReady() {
    mpuInterrupt = true;
}
void getYPR(){

    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();

    if((mpuIntStatus & 0x10) || fifoCount >= 1024){

      mpu.resetFIFO();

    }else if(mpuIntStatus & 0x02){

      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      mpu.getFIFOBytes(fifoBuffer, packetSize);

      fifoCount -= packetSize;

      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGyro(&gyro, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    }



}