Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #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);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement