├── pidsetup.ino ├── README.md ├── .gitattributes ├── .gitignore ├── pidconfig.h ├── pid.ino ├── piManual.ino ├── landing.ino ├── anglesetup.ino ├── finalcombo3rx.ino ├── Kalman.h ├── angle.ino └── comboconfig.h /pidsetup.ino: -------------------------------------------------------------------------------- 1 | void pidsetup() 2 | { 3 | 4 | //initialize the variables we're linked to 5 | 6 | 7 | //turn the PID on 8 | myPID1.SetMode(AUTOMATIC); 9 | myPID2.SetMode(AUTOMATIC); 10 | myPID3.SetMode(AUTOMATIC); 11 | myPID4.SetMode(AUTOMATIC); 12 | // motor1.writeMicroseconds(800); 13 | //motor2.writeMicroseconds(800); 14 | } 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | quadcopterarduino 2 | ================= 3 | 4 | Arduino code for a quadcopter which uses an mpu 6050 imu to read accelerometer , gyro and magnetometer readings and run it through a PID control system in order to make changes to the quadcopters rotor speeds.A kalman.h file is also included so that the user can use his own accelerometer plus gyro instead of using the mpu6050 5 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # ========================= 18 | # Operating System Files 19 | # ========================= 20 | 21 | # OSX 22 | # ========================= 23 | 24 | .DS_Store 25 | .AppleDouble 26 | .LSOverride 27 | 28 | # Icon must end with two \r 29 | Icon 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear on external disk 35 | .Spotlight-V100 36 | .Trashes 37 | 38 | # Directories potentially created on remote AFP share 39 | .AppleDB 40 | .AppleDesktop 41 | Network Trash Folder 42 | Temporary Items 43 | .apdisk 44 | -------------------------------------------------------------------------------- /pidconfig.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | // #define MAX_SIGNAL 2000 6 | // #define MIN_SIGNAL 7007 7 | #define MOTOR_PIN1 11//w 8 | #define MOTOR_PIN2 10//a 9 | #define MOTOR_PIN3 9//S 10 | #define MOTOR_PIN4 6//D 11 | 12 | ServoTimer2 motor1; 13 | ServoTimer2 motor2; 14 | ServoTimer2 motor3; 15 | ServoTimer2 motor4; 16 | // double aggKp=4, aggKi=0.2, aggKd=1; 17 | // consKp12=0,consKi12=0, consKd12=0,consKp34=0,consKi34=0,consKd34=0; 18 | double consKp12=0.00,consKi12=0.00, consKd12=0.00,consKp34=0.00,consKi34=0.00,consKd34=0.00; 19 | //double consKp12=0,consKi12=0, consKd12=0,consKp34=0,consKi34=0,consKd34=0; 20 | //Define Variables we'll be connecting to 21 | double Setpoint1,Setpoint2,Setpoint3,Setpoint4; 22 | double Input1,Input2,Input3,Input4; 23 | double Output1,Output2,Output3,Output4; 24 | 25 | //Specify the links and initial tuning parameters 26 | PID myPID1(&Input1, &Output1, &Setpoint1,consKp12,consKi12,consKd12,DIRECT); 27 | PID myPID2(&Input2, &Output2, &Setpoint2,consKp12,consKi12,consKd12,DIRECT); 28 | PID myPID3(&Input3, &Output3, &Setpoint3,consKp34,consKi34,consKd34,DIRECT); 29 | PID myPID4(&Input4, &Output4, &Setpoint4,consKp34,consKi34,consKd34,DIRECT); 30 | 31 | -------------------------------------------------------------------------------- /pid.ino: -------------------------------------------------------------------------------- 1 | void pid() 2 | { 3 | 4 | Setpoint1 =0; 5 | Setpoint2=-Setpoint1; 6 | Setpoint4=0; 7 | Setpoint3 =-Setpoint4; 8 | 9 | 10 | Input1 = -pitch; 11 | Input2 = pitch; 12 | Input3=roll; 13 | Input4=-roll; 14 | 15 | myPID1.Compute(); 16 | myPID2.Compute(); 17 | myPID3.Compute(); 18 | myPID4.Compute(); 19 | 20 | 21 | m1=aa1+Output1-Output2; 22 | m3=aa3+Output2-Output1; 23 | m2=aa2+Output3-Output4; 24 | m4=aa4+Output4-Output3; 25 | 26 | motor1.write(m1); 27 | motor2.write(m2); 28 | motor3.write(m3); 29 | motor4.write(m4); 30 | 31 | 32 | #ifdef Debugpid 33 | Serial.println(Sensor1Data); 34 | Serial.print("\t"); 35 | Serial.print(m1); 36 | Serial.print("\t"); 37 | Serial.print(m2); 38 | Serial.print("\t"); 39 | Serial.print(m3); 40 | Serial.print("\t"); 41 | Serial.print(m4); 42 | 43 | Serial.print("\t"); 44 | Serial.print(pitch); 45 | Serial.print("\t"); 46 | Serial.print(roll); 47 | Serial.print("\t"); 48 | 49 | 50 | Serial.print("\t"); 51 | Serial.print(consKp12); 52 | Serial.print("\t"); 53 | Serial.print(consKi12); 54 | Serial.print("\t"); 55 | Serial.print(consKd12); 56 | Serial.print("\t"); 57 | 58 | 59 | 60 | Serial.print(consKp34); 61 | Serial.print("\t"); 62 | Serial.print(consKi34); 63 | Serial.print("\t"); 64 | Serial.print(consKd34); 65 | Serial.print("\t"); 66 | 67 | Serial.print(Setpoint1); 68 | Serial.print("\t"); 69 | Serial.print(Setpoint2); 70 | Serial.print("\t"); 71 | Serial.print(Setpoint3); 72 | Serial.print("\t"); 73 | Serial.println(Setpoint4); 74 | 75 | #endif 76 | 77 | 78 | 79 | 80 | } 81 | 82 | -------------------------------------------------------------------------------- /piManual.ino: -------------------------------------------------------------------------------- 1 | void piManual() 2 | { 3 | 4 | char number; 5 | if(Serial.available()) 6 | { 7 | 8 | number=(char)Serial.read(); 9 | //time=millis(); 10 | } 11 | /* if(millis()-time>5000) 12 | { 13 | safeLanding(); 14 | }*/ 15 | 16 | if(number=='a') 17 | { 18 | 19 | aa1=aa1+10; 20 | 21 | 22 | } 23 | 24 | 25 | else if(number=='8') 26 | { 27 | safeLanding(); 28 | } 29 | 30 | 31 | else if(number=='z') 32 | { 33 | 34 | } 35 | 36 | 37 | else if(number=='5') 38 | { 39 | 40 | aa1=aa1+50; 41 | aa2=aa2+50; 42 | aa3=aa3+50; 43 | aa4=aa4+50; 44 | 45 | 46 | 47 | } 48 | 49 | else if(number=='f') 50 | { 51 | 52 | aa1=aa1-50; 53 | aa2=aa2-50; 54 | aa3=aa3-50; 55 | aa4=aa4-50; 56 | 57 | 58 | 59 | } 60 | //TTTTTTT 61 | else if(number=='t') 62 | { 63 | 64 | aa1=aa1-10; 65 | aa2=aa2-10; 66 | aa3=aa3-10; 67 | aa4=aa4-10; 68 | 69 | 70 | 71 | } 72 | //ttttttt 73 | else if(number=='c') 74 | { 75 | 76 | aa1=aa1+10; 77 | aa2=aa2+10; 78 | aa3=aa3+10; 79 | aa4=aa4+10; 80 | 81 | 82 | 83 | } 84 | 85 | else if(number=='r') 86 | { 87 | 88 | aa1=750; 89 | aa2=750; 90 | aa3=750; 91 | aa4=750; 92 | } 93 | 94 | 95 | 96 | //xxxxx 97 | 98 | 99 | 100 | 101 | //yyyyy 102 | else if(number=='1') 103 | { 104 | consKi12=consKi12+0.01; 105 | } 106 | 107 | //XXXXXXXX 108 | else if(number=='2') 109 | { 110 | consKp12=consKp12-0.01; 111 | } 112 | //YYYYY 113 | else if(number=='4') 114 | { 115 | consKi12=consKi12-0.01; 116 | } 117 | //ZZZZZZZZ 118 | if(number=='6') 119 | { 120 | consKd12=consKd12-0.01; 121 | } 122 | //zzzzz 123 | else if(number=='7') 124 | { 125 | consKp12=consKp12+0.01; 126 | } 127 | 128 | else if(number=='9') 129 | { 130 | consKp12=consKp12+0.01; 131 | } 132 | 133 | 134 | //iiiiiiiii 135 | else if(number=='p') 136 | { 137 | consKp34=consKp34+0.01; 138 | } 139 | //jjjjjj 140 | else if(number=='i') 141 | { 142 | consKi34=consKi34+0.01; 143 | } 144 | //kkkkkkkkk 145 | else if(number=='d') 146 | { 147 | consKd34=consKd34+0.01; 148 | } 149 | 150 | //IIIIIIII 151 | else if(number=='P') 152 | { 153 | consKp34=consKp34-0.01; 154 | } 155 | //JJJJJJJJ 156 | else if(number=='I') 157 | { 158 | consKi34=consKi34-0.01; 159 | } 160 | //KKKKKKKKKK 161 | else if(number=='D') 162 | { 163 | consKd34=consKd34-0.01; 164 | } 165 | 166 | 167 | // safety cutoff 168 | // space bar 169 | else if(number=='s') 170 | { 171 | /* // motor1.writeMicroseconds(aa1); 172 | Serial.println("aa11 + reset"); 173 | //Serial.println(aa1); 174 | aa1=700; 175 | aa2=700; 176 | aa3=700; 177 | aa4=700; 178 | */ 179 | Setpoint1 =0; 180 | Setpoint2 =0; 181 | Setpoint3 =0; 182 | Setpoint4 =0; 183 | safeLanding(); 184 | } 185 | //} 186 | //prevNumber = number; 187 | 188 | } 189 | -------------------------------------------------------------------------------- /landing.ino: -------------------------------------------------------------------------------- 1 | void safeLanding() 2 | { 3 | led1_status(1); 4 | uint8_t buf[VW_MAX_MESSAGE_LEN]; 5 | uint8_t buflen = VW_MAX_MESSAGE_LEN; 6 | 7 | { 8 | int addr10 =10,addr1=1,addr2=2,addr3=3,addr4=4,addr5=5,addr6=6,addr7=7,addr8=8,addr9=9; 9 | EEPROM.write(addr1,aa1/10); 10 | EEPROM.write(addr2,aa2/10); 11 | EEPROM.write(addr3,aa3/10); 12 | EEPROM.write(addr4,aa4/10); 13 | 14 | EEPROM.write(addr5,consKp12*100); 15 | EEPROM.write(addr6,consKi12*100); 16 | EEPROM.write(addr7,consKd12*100); 17 | 18 | EEPROM.write(addr8,consKp34*100); 19 | EEPROM.write(addr9,consKi34*100); 20 | EEPROM.write(addr10,consKd34*100); 21 | } 22 | 23 | // int St=millis(); 24 | //int st=safetime;//10secs 25 | // slow landing, reduces throttle by 200 in a second; 26 | // if(500 > millis()-St ) 27 | { 28 | // St= millis(); 29 | while(1) 30 | {Serial.println("stuck"); 31 | // if(aa1>700 || aa2>700 || aa3>700 || aa4>700) 32 | { 33 | if(aa1>700) 34 | aa1=aa1-10; 35 | if(aa2>700 ) 36 | aa2=aa2-10; 37 | if(aa3>700) 38 | aa3=aa3-10; 39 | if(aa4>700) 40 | aa4=aa4-10; 41 | } 42 | 43 | if(aa1<701 && aa2<701 & aa3<701 & aa4<701) 44 | break; 45 | // if(vw_get_message(buf, &buflen)) 46 | { 47 | // break; 48 | } 49 | 50 | motor1.write(aa1); 51 | motor2.write(aa2); 52 | motor3.write(aa3); 53 | motor4.write(aa4); 54 | 55 | 56 | Serial.print(aa1); 57 | Serial.print("\t"); 58 | Serial.print(aa2); 59 | Serial.print("\t"); 60 | Serial.print(aa3); 61 | Serial.print("\t"); 62 | Serial.println(aa4); 63 | delay(100); 64 | 65 | } 66 | /* 67 | motor1.writeMicroseconds(aa1); 68 | motor2.writeMicroseconds(aa2); 69 | motor3.writeMicroseconds(aa3); 70 | motor4.writeMicroseconds(aa4); 71 | Serial.println("safe"); */ 72 | } 73 | 74 | 75 | } 76 | 77 | void safeShutDown() 78 | { 79 | // danger shutdown; 80 | // if(pitchmaxSafeangle || rollmaxSafeangle ) 81 | { 82 | //t= millis(); 83 | Serial.println("dont shut"); 84 | while(1) 85 | { 86 | int aa=700; 87 | Serial.println("shut"); 88 | motor1.attach(MOTOR_PIN1); 89 | motor2.attach(MOTOR_PIN2); 90 | motor3.attach(MOTOR_PIN3); 91 | motor4.attach(MOTOR_PIN4); 92 | 93 | motor1.write(aa); 94 | motor2.write(aa); 95 | motor3.write(aa); 96 | motor4.write(aa); 97 | } 98 | } 99 | } 100 | 101 | 102 | void led1_init() 103 | { 104 | pinMode(13,OUTPUT); 105 | //f 106 | pinMode(4,OUTPUT); 107 | digitalWrite(13,LOW); 108 | } 109 | 110 | void led1_status(byte stat) 111 | { 112 | digitalWrite(13,stat); 113 | } 114 | void led2_init() 115 | { 116 | pinMode(5,OUTPUT); 117 | digitalWrite(13,LOW); 118 | } 119 | 120 | void led2_status(byte stat) 121 | { 122 | analogWrite(5,stat); 123 | } 124 | -------------------------------------------------------------------------------- /anglesetup.ino: -------------------------------------------------------------------------------- 1 | void mpusetup() { 2 | // join I2C bus (I2Cdev library doesn't do this automatically) 3 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 4 | Wire.begin(); 5 | TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz) 6 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 7 | Fastwire::setup(400, true); 8 | #endif 9 | 10 | // initialize serial communication 11 | // (115200 chosen because it is required for Teapot Demo output, but it's 12 | // really up to you depending on your project) 13 | // Serial.begin(115200); 14 | while (!Serial); // wait for Leonardo enumeration, others continue immediately 15 | 16 | // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio 17 | // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to 18 | // the baud timing being too misaligned with processor ticks. You must use 19 | // 38400 or slower in these cases, or use some kind of external separate 20 | // crystal solution for the UART timer. 21 | 22 | // initialize device 23 | mpu.initialize(); 24 | 25 | // verify connection 26 | Serial.println(F("Testing device connections...")); 27 | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 28 | 29 | // wait for ready 30 | Serial.println(F("\n dont Send any character to begin DMP programming and demo: ")); 31 | /* while (Serial.available() && Serial.read()); // empty buffer 32 | while (!Serial.available()); // wait for data 33 | while (Serial.available() && Serial.read()); // empty buffer again 34 | */ 35 | // load and configure the DMP 36 | Serial.println(F("Initializing DMP...")); 37 | devStatus = mpu.dmpInitialize(); 38 | 39 | // supply your own gyro offsets here, scaled for min sensitivity 40 | mpu.setXGyroOffset(220); 41 | mpu.setYGyroOffset(76); 42 | mpu.setZGyroOffset(-85); 43 | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 44 | 45 | // make sure it worked (returns 0 if so) 46 | if (devStatus == 0) { 47 | // turn on the DMP, now that it's ready 48 | Serial.println(F("Enabling DMP...")); 49 | mpu.setDMPEnabled(true); 50 | 51 | // enable Arduino interrupt detection 52 | Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 53 | attachInterrupt(0, dmpDataReady, RISING); 54 | mpuIntStatus = mpu.getIntStatus(); 55 | 56 | // set our DMP Ready flag so the main loop() function knows it's okay to use it 57 | Serial.println(F("DMP ready! Waiting for first interrupt...")); 58 | dmpReady = true; 59 | 60 | // get expected DMP packet size for later comparison 61 | packetSize = mpu.dmpGetFIFOPacketSize(); 62 | } else { 63 | // ERROR! 64 | // 1 = initial memory load failed 65 | // 2 = DMP configuration updates failed 66 | // (if it's going to break, usually the code will be 1) 67 | Serial.print(F("DMP Initialization failed (code ")); 68 | Serial.print(devStatus); 69 | Serial.println(F(")")); 70 | } 71 | 72 | // configure LED for output 73 | pinMode(LED_PIN, OUTPUT); 74 | } 75 | 76 | 77 | -------------------------------------------------------------------------------- /finalcombo3rx.ino: -------------------------------------------------------------------------------- 1 | #include "comboconfig.h" 2 | #include "I2Cdev.h" 3 | #include "pidconfig.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | //#include 9 | #include 10 | 11 | #include "MPU6050.h" 12 | 13 | 14 | 15 | //#define DebugAngle 16 | #define Debugpid 17 | #define OUTPUT_READABLE_YAWPITCHROLL 18 | 19 | 20 | int16_t aX11, aY11, aZ11; 21 | int16_t gX11, gY11, gZ11; 22 | int Sensor1Data; 23 | int NNin,NNnot,mytime; 24 | char *controller; 25 | int ijk=0.01; 26 | int debugmpu; 27 | int prevNumber; 28 | 29 | //motor init 30 | int aa1=700; 31 | int aa2=700; 32 | int aa3=700; 33 | int aa4=700; 34 | int m1,m2,m3,m4; 35 | //int maxSafeangle=60;// these are used in shutDown function; 36 | //int minSafeangle=-60;// - - 37 | boolean calibrate; 38 | int charflag,mpuflag; 39 | //int safetime=10000;// safe lands after this time limit. 40 | int led = 13; 41 | int addr10 =10,addr1=1,addr2=2,addr3=3,addr4=4,addr5=5,addr6=6,addr7=7,addr8=8,addr9=9; 42 | boolean firstrun=0; 43 | double pitch;double roll;double yaw; 44 | 45 | 46 | void setup() 47 | { 48 | 49 | pinMode(4,OUTPUT); 50 | digitalWrite(4,LOW); 51 | 52 | 53 | Serial.begin(115200); 54 | mpusetup(); 55 | 56 | 57 | led1_init(); 58 | led2_init(); 59 | pidsetup(); 60 | 61 | } 62 | 63 | 64 | //The loop section of the sketch will read the X,Y and Z output rates from the gyroscope and output them in the Serial Terminal 65 | void loop() 66 | { 67 | if (firstrun==0) 68 | { 69 | int aa=700; 70 | motor1.attach(MOTOR_PIN1); 71 | motor2.attach(MOTOR_PIN2); 72 | motor3.attach(MOTOR_PIN3); 73 | motor4.attach(MOTOR_PIN4); 74 | 75 | motor1.write(aa); 76 | motor2.write(aa); 77 | motor3.write(aa); 78 | motor4.write(aa); 79 | firstrun=1; 80 | 81 | } 82 | mpuloop(); 83 | piManual(); 84 | pid(); 85 | 86 | 87 | /*double gap1 = abs(Setpoint1-Input1); 88 | double gap2 = abs(Setpoint2-Input2); 89 | double gap3 = abs(Setpoint3-Input3); 90 | double gap4 = abs(Setpoint4-Input4);*/ 91 | // if(gap1<10) 92 | //we're close to setpoint, use conservative tuning parameters 93 | // myPID1.SetTunings(consKp, consKi, consKd); 94 | 95 | /* else if(gap1>=10) 96 | { 97 | //we're far from setpoint, use aggressive tuning parameters 98 | myPID1.SetTunings(aggKp, aggKi, aggKd); 99 | } 100 | // if(gap2<10) 101 | { //we're close to setpoint, use conservative tuning parameters 102 | myPID2.SetTunings(consKp, consKi, consKd); 103 | } 104 | /*else if(gap2>=10) 105 | { 106 | //we're far from setpoint, use aggressive tuning parameters 107 | myPID2.SetTunings(aggKp, aggKi, aggKd); 108 | } 109 | if(gap3<10) 110 | { //we're close to setpoint, use conservative tuning parameters 111 | myPID3.SetTunings(consKp, consKi, consKd); 112 | } 113 | else if(gap3>=10) 114 | { 115 | //we're far from setpoint, use aggressive tuning parameters 116 | myPID3.SetTunings(aggKp, aggKi, aggKd); 117 | } 118 | if(gap4<10) 119 | { //we're close to setpoint, use conservative tuning parameters 120 | myPID4.SetTunings(consKp, consKi, consKd); 121 | } 122 | else if(gap4>=10) 123 | { 124 | //we're far from setpoint, use aggressive tuning parameters 125 | myPID4.SetTunings(aggKp, aggKi, aggKd); 126 | 127 | */ 128 | 129 | 130 | 131 | myPID1.SetTunings(consKp12, consKi12, consKd12); 132 | myPID2.SetTunings(consKp12, consKi12, consKd12); 133 | myPID3.SetTunings(consKp34, consKi34, consKd34); 134 | myPID4.SetTunings(consKp34, consKi34, consKd34); 135 | 136 | //delay(100); 137 | 138 | } 139 | -------------------------------------------------------------------------------- /Kalman.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. 2 | 3 | This software may be distributed and modified under the terms of the GNU 4 | General Public License version 2 (GPL2) as published by the Free Software 5 | Foundation and appearing in the file GPL2.TXT included in the packaging of 6 | this file. Please note that GPL2 Section 2[b] requires that all works based 7 | on this software must also be made publicly available under the terms of 8 | the GPL2 ("Copyleft"). 9 | 10 | Contact information 11 | ------------------- 12 | 13 | Kristian Lauszus, TKJ Electronics 14 | Web : http://www.tkjelectronics.com 15 | e-mail : kristianl@tkjelectronics.com 16 | */ 17 | 18 | #ifndef _Kalman_h 19 | #define _Kalman_h 20 | 21 | class Kalman { 22 | public: 23 | Kalman() { 24 | /* We will set the variables like so, these can also be tuned by the user */ 25 | Q_angle = 0.001; 26 | Q_bias = 0.003; 27 | R_measure = 0.03; 28 | 29 | angle = 0; // Reset the angle 30 | bias = 0; // Reset bias 31 | 32 | P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical 33 | P[0][1] = 0; 34 | P[1][0] = 0; 35 | P[1][1] = 0; 36 | }; 37 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds 38 | double getAngle(double newAngle, double newRate, double dt) { 39 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145 40 | // Modified by Kristian Lauszus 41 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it 42 | 43 | // Discrete Kalman filter time update equations - Time Update ("Predict") 44 | // Update xhat - Project the state ahead 45 | /* Step 1 */ 46 | rate = newRate - bias; 47 | angle += dt * rate; 48 | 49 | // Update estimation error covariance - Project the error covariance ahead 50 | /* Step 2 */ 51 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); 52 | P[0][1] -= dt * P[1][1]; 53 | P[1][0] -= dt * P[1][1]; 54 | P[1][1] += Q_bias * dt; 55 | 56 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") 57 | // Calculate Kalman gain - Compute the Kalman gain 58 | /* Step 4 */ 59 | S = P[0][0] + R_measure; 60 | /* Step 5 */ 61 | K[0] = P[0][0] / S; 62 | K[1] = P[1][0] / S; 63 | 64 | // Calculate angle and bias - Update estimate with measurement zk (newAngle) 65 | /* Step 3 */ 66 | y = newAngle - angle; 67 | /* Step 6 */ 68 | angle += K[0] * y; 69 | bias += K[1] * y; 70 | 71 | // Calculate estimation error covariance - Update the error covariance 72 | /* Step 7 */ 73 | P[0][0] -= K[0] * P[0][0]; 74 | P[0][1] -= K[0] * P[0][1]; 75 | P[1][0] -= K[1] * P[0][0]; 76 | P[1][1] -= K[1] * P[0][1]; 77 | 78 | return angle; 79 | }; 80 | void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle 81 | double getRate() { return rate; }; // Return the unbiased rate 82 | 83 | /* These are used to tune the Kalman filter */ 84 | void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; 85 | void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; 86 | void setRmeasure(double newR_measure) { R_measure = newR_measure; }; 87 | 88 | double getQangle() { return Q_angle; }; 89 | double getQbias() { return Q_bias; }; 90 | double getRmeasure() { return R_measure; }; 91 | 92 | private: 93 | /* Kalman filter variables */ 94 | double Q_angle; // Process noise variance for the accelerometer 95 | double Q_bias; // Process noise variance for the gyro bias 96 | double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise 97 | 98 | double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector 99 | double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector 100 | double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate 101 | 102 | double P[2][2]; // Error covariance matrix - This is a 2x2 matrix 103 | double K[2]; // Kalman gain - This is a 2x1 vector 104 | double y; // Angle difference 105 | double S; // Estimate error 106 | }; 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /angle.ino: -------------------------------------------------------------------------------- 1 | void mpuloop() 2 | { 3 | mytime=millis(); 4 | if(pitch>5 || roll >5 || pitch<-5 || roll<-5) 5 | led2_status(120); 6 | else 7 | led2_status(0); 8 | 9 | 10 | // if programming failed, don't try to do anything 11 | if (!dmpReady) return; 12 | 13 | // wait for MPU interrupt or extra packet(s) available 14 | while (!mpuInterrupt && fifoCount < packetSize) { 15 | // other program behavior stuff here 16 | // . 17 | // . 18 | delay(20); 19 | // . 20 | // if you are really paranoid you can frequently test in between other 21 | // stuff to see if mpuInterrupt is true, and if so, "break;" from the 22 | // while() loop to immediately process the MPU data 23 | // . 24 | // . 25 | // . 26 | } 27 | 28 | // reset interrupt flag and get INT_STATUS byte 29 | mpuInterrupt = false; 30 | mpuIntStatus = mpu.getIntStatus(); 31 | 32 | // get current FIFO count 33 | fifoCount = mpu.getFIFOCount(); 34 | 35 | // check for overflow (this should never happen unless our code is too inefficient) 36 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 37 | // reset so we can continue cleanly 38 | mpu.resetFIFO(); 39 | Serial.println(F("FIFO overflow!")); 40 | 41 | // otherwise, check for DMP data ready interrupt (this should happen frequently) 42 | } 43 | else if (mpuIntStatus & 0x02) { 44 | // wait for correct available data length, should be a VERY short wait 45 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 46 | 47 | // read a packet from FIFO 48 | mpu.getFIFOBytes(fifoBuffer, packetSize); 49 | 50 | // track FIFO count here in case there is > 1 packet available 51 | // (this lets us immediately read more without waiting for an interrupt) 52 | fifoCount -= packetSize; 53 | 54 | #ifdef OUTPUT_READABLE_QUATERNION 55 | // display quaternion values in easy matrix form: w x y z 56 | mpu.dmpGetQuaternion(&q, fifoBuffer); 57 | Serial.print("quat\t"); 58 | Serial.print(q.w); 59 | Serial.print("\t"); 60 | Serial.print(q.x); 61 | Serial.print("\t"); 62 | Serial.print(q.y); 63 | Serial.print("\t"); 64 | Serial.println(q.z); 65 | #endif 66 | 67 | #ifdef OUTPUT_READABLE_EULER 68 | // display Euler angles in degrees 69 | mpu.dmpGetQuaternion(&q, fifoBuffer); 70 | mpu.dmpGetEuler(euler, &q); 71 | Serial.print("euler\t"); 72 | Serial.print(euler[0] * 180/M_PI); 73 | Serial.print("\t"); 74 | Serial.print(euler[1] * 180/M_PI); 75 | Serial.print("\t"); 76 | Serial.println(euler[2] * 180/M_PI); 77 | #endif 78 | 79 | #ifdef OUTPUT_READABLE_YAWPITCHROLL 80 | // display Euler angles in degrees 81 | mpu.dmpGetQuaternion(&q, fifoBuffer); 82 | mpu.dmpGetGravity(&gravity, &q); 83 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 84 | /* Serial.print("ypr\t"); 85 | Serial.print(ypr[0] * 180/M_PI); 86 | Serial.print("\t"); 87 | Serial.print(ypr[1] * 180/M_PI); 88 | Serial.print("\t"); 89 | Serial.println(ypr[2] * 180/M_PI);*/ 90 | yaw=(ypr[0] * 180/M_PI) ; 91 | roll=(ypr[1] * 180/M_PI) ; 92 | pitch=(ypr[2] * 180/M_PI); 93 | #endif 94 | #ifdef DebugAngle 95 | Serial.print("ypr\t"); 96 | Serial.print(ypr[0] * 180/M_PI); 97 | Serial.print("\t"); 98 | Serial.print(ypr[1] * 180/M_PI); 99 | Serial.print("\t"); 100 | Serial.println(ypr[2] * 180/M_PI); 101 | #endif 102 | 103 | #ifdef OUTPUT_READABLE_REALACCEL 104 | // display real acceleration, adjusted to remove gravity 105 | mpu.dmpGetQuaternion(&q, fifoBuffer); 106 | mpu.dmpGetAccel(&aa, fifoBuffer); 107 | mpu.dmpGetGravity(&gravity, &q); 108 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 109 | Serial.print("areal\t"); 110 | Serial.print(aaReal.x); 111 | Serial.print("\t"); 112 | Serial.print(aaReal.y); 113 | Serial.print("\t"); 114 | Serial.println(aaReal.z); 115 | #endif 116 | 117 | #ifdef OUTPUT_READABLE_WORLDACCEL 118 | // display initial world-frame acceleration, adjusted to remove gravity 119 | // and rotated based on known orientation from quaternion 120 | mpu.dmpGetQuaternion(&q, fifoBuffer); 121 | mpu.dmpGetAccel(&aa, fifoBuffer); 122 | mpu.dmpGetGravity(&gravity, &q); 123 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 124 | mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); 125 | Serial.print("aworld\t"); 126 | Serial.print(aaWorld.x); 127 | Serial.print("\t"); 128 | Serial.print(aaWorld.y); 129 | Serial.print("\t"); 130 | Serial.println(aaWorld.z); 131 | #endif 132 | 133 | #ifdef OUTPUT_TEAPOT 134 | // display quaternion values in InvenSense Teapot demo format: 135 | teapotPacket[2] = fifoBuffer[0]; 136 | teapotPacket[3] = fifoBuffer[1]; 137 | teapotPacket[4] = fifoBuffer[4]; 138 | teapotPacket[5] = fifoBuffer[5]; 139 | teapotPacket[6] = fifoBuffer[8]; 140 | teapotPacket[7] = fifoBuffer[9]; 141 | teapotPacket[8] = fifoBuffer[12]; 142 | teapotPacket[9] = fifoBuffer[13]; 143 | Serial.write(teapotPacket, 14); 144 | teapotPacket[11]++; // packetCount, loops at 0xFF on purpose 145 | #endif 146 | 147 | } 148 | } 149 | 150 | -------------------------------------------------------------------------------- /comboconfig.h: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) 2 | // 6/21/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2013-05-08 - added seamless Fastwire support 7 | // - added note about gyro calibration 8 | // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error 9 | // 2012-06-20 - improved FIFO overflow handling and simplified read process 10 | // 2012-06-19 - completely rearranged DMP initialization code and simplification 11 | // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly 12 | // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING 13 | // 2012-06-05 - add gravity-compensated initial reference frame acceleration output 14 | // - add 3D math helper file to DMP6 example sketch 15 | // - add Euler output and Yaw/Pitch/Roll output formats 16 | // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee) 17 | // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250 18 | // 2012-05-30 - basic DMP initialization working 19 | 20 | /* ============================================ 21 | I2Cdev device library code is placed under the MIT license 22 | Copyright (c) 2012 Jeff Rowberg 23 | 24 | Permission is hereby granted, free of charge, to any person obtaining a copy 25 | of this software and associated documentation files (the "Software"), to deal 26 | in the Software without restriction, including without limitation the rights 27 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 28 | copies of the Software, and to permit persons to whom the Software is 29 | furnished to do so, subject to the following conditions: 30 | 31 | The above copyright notice and this permission notice shall be included in 32 | all copies or substantial portions of the Software. 33 | 34 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 35 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 36 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 37 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 38 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 39 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 40 | THE SOFTWARE. 41 | =============================================== 42 | */ 43 | 44 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 45 | // for both classes must be in the include path of your project 46 | #include "I2Cdev.h" 47 | 48 | #include "MPU6050_6Axis_MotionApps20.h" 49 | //#include "MPU6050.h" // not necessary if using MotionApps include file 50 | 51 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 52 | // is used in I2Cdev.h 53 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 54 | #include "Wire.h" 55 | #endif 56 | 57 | // class default I2C address is 0x68 58 | // specific I2C addresses may be passed as a parameter here 59 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) 60 | // AD0 high = 0x69 61 | MPU6050 mpu; 62 | //MPU6050 mpu(0x69); // <-- use for AD0 high 63 | 64 | /* ========================================================================= 65 | NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch 66 | depends on the MPU-6050's INT pin being connected to the Arduino's 67 | external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is 68 | digital I/O pin 2. 69 | * ========================================================================= */ 70 | 71 | /* ========================================================================= 72 | NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error 73 | when using Serial.write(buf, len). The Teapot output uses this method. 74 | The solution requires a modification to the Arduino USBAPI.h file, which 75 | is fortunately simple, but annoying. This will be fixed in the next IDE 76 | release. For more info, see these links: 77 | 78 | http://arduino.cc/forum/index.php/topic,109987.0.html 79 | http://code.google.com/p/arduino/issues/detail?id=958 80 | * ========================================================================= */ 81 | 82 | 83 | 84 | // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual 85 | // quaternion components in a [w, x, y, z] format (not best for parsing 86 | // on a remote host such as Processing or something though) 87 | //#define OUTPUT_READABLE_QUATERNION 88 | 89 | // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles 90 | // (in degrees) calculated from the quaternions coming from the FIFO. 91 | // Note that Euler angles suffer from gimbal lock (for more info, see 92 | // http://en.wikipedia.org/wiki/Gimbal_lock) 93 | //#define OUTPUT_READABLE_EULER 94 | 95 | // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ 96 | // pitch/roll angles (in degrees) calculated from the quaternions coming 97 | // from the FIFO. Note this also requires gravity vector calculations. 98 | // Also note that yaw/pitch/roll angles suffer from gimbal lock (for 99 | // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) 100 | 101 | // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration 102 | // components with gravity removed. This acceleration reference frame is 103 | // not compensated for orientation, so +X is always +X according to the 104 | // sensor, just without the effects of gravity. If you want acceleration 105 | // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. 106 | //#define OUTPUT_READABLE_REALACCEL 107 | 108 | // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration 109 | // components with gravity removed and adjusted for the world frame of 110 | // reference (yaw is relative to initial orientation, since no magnetometer 111 | // is present in this case). Could be quite handy in some cases. 112 | //#define OUTPUT_READABLE_WORLDACCEL 113 | 114 | // uncomment "OUTPUT_TEAPOT" if you want output that matches the 115 | // format used for the InvenSense teapot demo 116 | //#define OUTPUT_TEAPOT 117 | 118 | 119 | 120 | #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) 121 | bool blinkState = false; 122 | 123 | // MPU control/status vars 124 | bool dmpReady = false; // set true if DMP init was successful 125 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 126 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 127 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 128 | uint16_t fifoCount; // count of all bytes currently in FIFO 129 | uint8_t fifoBuffer[64]; // FIFO storage buffer 130 | 131 | // orientation/motion vars 132 | Quaternion q; // [w, x, y, z] quaternion container 133 | VectorInt16 aa; // [x, y, z] accel sensor measurements 134 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 135 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 136 | VectorFloat gravity; // [x, y, z] gravity vector 137 | float euler[3]; // [psi, theta, phi] Euler angle container 138 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 139 | 140 | // packet structure for InvenSense teapot demo 141 | uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; 142 | 143 | 144 | 145 | // ================================================================ 146 | // === INTERRUPT DETECTION ROUTINE === 147 | // ================================================================ 148 | 149 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 150 | void dmpDataReady() { 151 | mpuInterrupt = true; 152 | } 153 | 154 | 155 | //Pi Control 156 | 157 | #define SLAVE_ADDRESS 0x04 158 | int number = 0; 159 | int state = 0; 160 | 161 | 162 | 163 | --------------------------------------------------------------------------------