Amr El-Gendy / Mbed 2 deprecated QUAD_PROJECT

a quadcopter code

Dependencies:   Pulse RangeFinder mbed

main.cpp@0:4a55d0a21ea9, 2015-11-24 (annotated)

Committer:
Gendy
Date:
Tue Nov 24 19:57:06 2015 +0000
Revision:
0:4a55d0a21ea9
Quad_project PID on angle;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gendy 0:4a55d0a21ea9 1 #include <math.h>
Gendy 0:4a55d0a21ea9 2 #include "I2Cdev.h"
Gendy 0:4a55d0a21ea9 3 #include "mbed.h"
Gendy 0:4a55d0a21ea9 4 #include "MPU6050_6Axis_MotionApps20.h"
Gendy 0:4a55d0a21ea9 5
Gendy 0:4a55d0a21ea9 6 #include "PID.h"
Gendy 0:4a55d0a21ea9 7
Gendy 0:4a55d0a21ea9 8 #include "RangeFinder.h"
Gendy 0:4a55d0a21ea9 9 #ifndef M_PI
Gendy 0:4a55d0a21ea9 10 #define M_PI 3.1415
Gendy 0:4a55d0a21ea9 11 #endif
Gendy 0:4a55d0a21ea9 12 #define OUTPUT_READABLE_YAWPITCHROLL
Gendy 0:4a55d0a21ea9 13 #define OUTPUT_READABLE_QUATERNION
Gendy 0:4a55d0a21ea9 14 #define OUTPUT_READABLE_ACCELGYRO
Gendy 0:4a55d0a21ea9 15
Gendy 0:4a55d0a21ea9 16 ////// VARIABLES //////
Gendy 0:4a55d0a21ea9 17 ///ultrasonic variable///
Gendy 0:4a55d0a21ea9 18 float d=0;
Gendy 0:4a55d0a21ea9 19 //////////////////
Gendy 0:4a55d0a21ea9 20
Gendy 0:4a55d0a21ea9 21
Gendy 0:4a55d0a21ea9 22 /// PID VARIABLES ///
Gendy 0:4a55d0a21ea9 23 float KPRoll=5;
Gendy 0:4a55d0a21ea9 24 float KIRoll=1;
Gendy 0:4a55d0a21ea9 25 float KDRoll=0.5;
Gendy 0:4a55d0a21ea9 26
Gendy 0:4a55d0a21ea9 27 float KPPitch=5;
Gendy 0:4a55d0a21ea9 28 float KIPitch=1.1;
Gendy 0:4a55d0a21ea9 29 float KDPitch=0.5;//
Gendy 0:4a55d0a21ea9 30
Gendy 0:4a55d0a21ea9 31 float KPRPITCH=2;
Gendy 0:4a55d0a21ea9 32 float KIRPITCH=0;
Gendy 0:4a55d0a21ea9 33 float KDRPITCH=0.05;
Gendy 0:4a55d0a21ea9 34
Gendy 0:4a55d0a21ea9 35 float KPRROLL=2;
Gendy 0:4a55d0a21ea9 36 float KIRROLL=0;
Gendy 0:4a55d0a21ea9 37 float KDRROLL=0.05;
Gendy 0:4a55d0a21ea9 38
Gendy 0:4a55d0a21ea9 39 float KPHIGHT=5;
Gendy 0:4a55d0a21ea9 40 float KIHIGHT=1;
Gendy 0:4a55d0a21ea9 41 float KDHIGHT=0.1;
Gendy 0:4a55d0a21ea9 42
Gendy 0:4a55d0a21ea9 43 float SetpointROLL=0;
Gendy 0:4a55d0a21ea9 44 float RateRequired=0;
Gendy 0:4a55d0a21ea9 45 float SetpointPITCH=0;
Gendy 0:4a55d0a21ea9 46
Gendy 0:4a55d0a21ea9 47 float InputPITCH, OutputPITCH=0,RATEXinput=0,RATEXoutput=0,SetpointPitchrate=0,SetpointRollrate=0;
Gendy 0:4a55d0a21ea9 48 float InputROLL, OutputROLL=0,RATEYinput=0,RATEYoutput=0;
Gendy 0:4a55d0a21ea9 49 float Input_altitude,delta_trotle=0,Setpointaltitude=600;
Gendy 0:4a55d0a21ea9 50 /// ESC VARIABLES ///
Gendy 0:4a55d0a21ea9 51 float in_pulse=1200; // ranging from 0 to 1
Gendy 0:4a55d0a21ea9 52 /// MPU VARIABLES ///
Gendy 0:4a55d0a21ea9 53 bool blinkState = false;
Gendy 0:4a55d0a21ea9 54 bool dmpReady = false; // set true if DMP init was successful
Gendy 0:4a55d0a21ea9 55 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
Gendy 0:4a55d0a21ea9 56 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
Gendy 0:4a55d0a21ea9 57 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
Gendy 0:4a55d0a21ea9 58 uint16_t fifoCount; // count of all bytes currently in FIFO
Gendy 0:4a55d0a21ea9 59 uint8_t fifoBuffer[64]; // FIFO storage buffer
Gendy 0:4a55d0a21ea9 60 Quaternion q; // [w, x, y, z] quaternion container
Gendy 0:4a55d0a21ea9 61 VectorInt16 aa; // [x, y, z] accel sensor measurements
Gendy 0:4a55d0a21ea9 62 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
Gendy 0:4a55d0a21ea9 63 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
Gendy 0:4a55d0a21ea9 64 VectorFloat gravity; // [x, y, z] gravity vector
Gendy 0:4a55d0a21ea9 65 float euler[3]; // [psi, theta, phi] Euler angle container
Gendy 0:4a55d0a21ea9 66 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
Gendy 0:4a55d0a21ea9 67 uint8_t teapotPacket[15] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n',0 };
Gendy 0:4a55d0a21ea9 68 float mean_yaw=0 ;
Gendy 0:4a55d0a21ea9 69 //int16_t &ax, &ay, &az;
Gendy 0:4a55d0a21ea9 70 int16_t gx, gy, gz;
Gendy 0:4a55d0a21ea9 71
Gendy 0:4a55d0a21ea9 72 // CALIBRATION VARIABLES //
Gendy 0:4a55d0a21ea9 73 float AngleYaw,pAngleYaw,AnglePitch,pAnglePitch,AngleRoll,pAngleRoll;
Gendy 0:4a55d0a21ea9 74 float AngleOffset[3];
Gendy 0:4a55d0a21ea9 75 int16_t GyroOffset[3];
Gendy 0:4a55d0a21ea9 76 int16_t Rate_x,Rate_y,Rate_z,pgx,pgy,pgz;
Gendy 0:4a55d0a21ea9 77 int start_flag=0;
Gendy 0:4a55d0a21ea9 78 ////// INITIAL SETUP //////
Gendy 0:4a55d0a21ea9 79 ////// mbed setup ///////
Gendy 0:4a55d0a21ea9 80 Serial pc(p13 ,p14); //xbee 13/14
Gendy 0:4a55d0a21ea9 81 /// PID SETUP ///
Gendy 0:4a55d0a21ea9 82
Gendy 0:4a55d0a21ea9 83 PID PITCHPID(&InputPITCH, &OutputPITCH, &SetpointPITCH,KPPitch,KIPitch,KDPitch);
Gendy 0:4a55d0a21ea9 84 PID PITCHPIDrate(&RATEYinput, &RATEYoutput,&SetpointPitchrate,KPRPITCH,KIRPITCH,KDRPITCH);
Gendy 0:4a55d0a21ea9 85 PID ROLLPID(&InputROLL, &OutputROLL, &SetpointROLL,KPRoll,KIRoll,KDRoll);
Gendy 0:4a55d0a21ea9 86 PID ROLLPIDrate(&RATEXinput, &RATEXoutput,&SetpointRollrate,KPRROLL,KIRROLL,KDRROLL);
Gendy 0:4a55d0a21ea9 87 PID altitudecontroller(&Input_altitude, &delta_trotle, &Setpointaltitude,KPHIGHT,KIHIGHT,KDHIGHT);
Gendy 0:4a55d0a21ea9 88 //PID altitudedampingcontroller(&Zvelocity, &trotle_damping,&Setpoint_Zdamping,KPDAMPING,KIDAMPING,KDDAMPING); //dont forget to declare the variables
Gendy 0:4a55d0a21ea9 89 /// MPU SETUP ///
Gendy 0:4a55d0a21ea9 90 MPU6050 mpu;
Gendy 0:4a55d0a21ea9 91 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
Gendy 0:4a55d0a21ea9 92 void dmpDataReady() {
Gendy 0:4a55d0a21ea9 93 mpuInterrupt = true;
Gendy 0:4a55d0a21ea9 94 }
Gendy 0:4a55d0a21ea9 95 /// Motors SETUP ///
Gendy 0:4a55d0a21ea9 96 PwmOut esc1(p21);
Gendy 0:4a55d0a21ea9 97 PwmOut esc2(p22);
Gendy 0:4a55d0a21ea9 98 PwmOut esc3(p23);
Gendy 0:4a55d0a21ea9 99 PwmOut esc4(p24);
Gendy 0:4a55d0a21ea9 100
Gendy 0:4a55d0a21ea9 101 ///ultrasonic setup////
Gendy 0:4a55d0a21ea9 102 RangeFinder rf(p20, 10, 5800.0, 100000);
Gendy 0:4a55d0a21ea9 103 ///////////////////////////
Gendy 0:4a55d0a21ea9 104
Gendy 0:4a55d0a21ea9 105
Gendy 0:4a55d0a21ea9 106
Gendy 0:4a55d0a21ea9 107 ////// FUNCTION DECLARATIONS //////
Gendy 0:4a55d0a21ea9 108
Gendy 0:4a55d0a21ea9 109
Gendy 0:4a55d0a21ea9 110 /*void mpu_code();
Gendy 0:4a55d0a21ea9 111 void send_data();
Gendy 0:4a55d0a21ea9 112 void calibration();*/
Gendy 0:4a55d0a21ea9 113 ////// FUNCTIONS //////
Gendy 0:4a55d0a21ea9 114
Gendy 0:4a55d0a21ea9 115 void mpu_code(){
Gendy 0:4a55d0a21ea9 116
Gendy 0:4a55d0a21ea9 117 mpuInterrupt = false;
Gendy 0:4a55d0a21ea9 118 mpuIntStatus = mpu.getIntStatus();
Gendy 0:4a55d0a21ea9 119 fifoCount = mpu.getFIFOCount();
Gendy 0:4a55d0a21ea9 120 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
Gendy 0:4a55d0a21ea9 121 mpu.resetFIFO();
Gendy 0:4a55d0a21ea9 122 pc.printf("FIFO overflow!");
Gendy 0:4a55d0a21ea9 123 } else if (mpuIntStatus & 0x02) {
Gendy 0:4a55d0a21ea9 124 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
Gendy 0:4a55d0a21ea9 125 mpu.getFIFOBytes(fifoBuffer, packetSize);
Gendy 0:4a55d0a21ea9 126 fifoCount -= packetSize;
Gendy 0:4a55d0a21ea9 127
Gendy 0:4a55d0a21ea9 128 #ifdef OUTPUT_READABLE_QUATERNION
Gendy 0:4a55d0a21ea9 129 mpu.dmpGetQuaternion(&q, fifoBuffer);
Gendy 0:4a55d0a21ea9 130 /*pc.printf("%f",q.w);
Gendy 0:4a55d0a21ea9 131 pc.printf(",");
Gendy 0:4a55d0a21ea9 132 pc.printf("%f",q.x);
Gendy 0:4a55d0a21ea9 133 pc.printf(",");
Gendy 0:4a55d0a21ea9 134 pc.printf("%f",q.y);
Gendy 0:4a55d0a21ea9 135 pc.printf(",");
Gendy 0:4a55d0a21ea9 136 pc.printf("%f\n",q.z);*/
Gendy 0:4a55d0a21ea9 137 #endif
Gendy 0:4a55d0a21ea9 138
Gendy 0:4a55d0a21ea9 139 #ifdef OUTPUT_READABLE_EULER
Gendy 0:4a55d0a21ea9 140 mpu.dmpGetQuaternion(&q, fifoBuffer);
Gendy 0:4a55d0a21ea9 141 mpu.dmpGetEuler(euler, &q);
Gendy 0:4a55d0a21ea9 142 /* pc.printf("euler\t");
Gendy 0:4a55d0a21ea9 143 pc.printf("%f",euler[0] * 180/M_PI);
Gendy 0:4a55d0a21ea9 144 pc.printf("\t");
Gendy 0:4a55d0a21ea9 145 pc.printf("%f",euler[1] * 180/M_PI);
Gendy 0:4a55d0a21ea9 146 pc.printf("\t");
Gendy 0:4a55d0a21ea9 147 pc.printf("%f\n",euler[2] * 180/M_PI);*/
Gendy 0:4a55d0a21ea9 148 #endif
Gendy 0:4a55d0a21ea9 149
Gendy 0:4a55d0a21ea9 150 #ifdef OUTPUT_READABLE_YAWPITCHROLL
Gendy 0:4a55d0a21ea9 151 mpu.dmpGetQuaternion(&q, fifoBuffer);
Gendy 0:4a55d0a21ea9 152 mpu.dmpGetGravity(&gravity, &q);
Gendy 0:4a55d0a21ea9 153 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Gendy 0:4a55d0a21ea9 154 #endif
Gendy 0:4a55d0a21ea9 155
Gendy 0:4a55d0a21ea9 156 #ifdef OUTPUT_READABLE_ACCELGYRO
Gendy 0:4a55d0a21ea9 157 mpu.getRotation(&gx, &gy, &gz);
Gendy 0:4a55d0a21ea9 158 // mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Gendy 0:4a55d0a21ea9 159 /*pc.printf("a/g:\t");
Gendy 0:4a55d0a21ea9 160 pc.printf(gx); pc.printf("\t");
Gendy 0:4a55d0a21ea9 161 pc.printf(gy); pc.printf("\t");
Gendy 0:4a55d0a21ea9 162 pc.printf(gz); pc.printf("\t");
Gendy 0:4a55d0a21ea9 163 pc.printf(gz);*/
Gendy 0:4a55d0a21ea9 164 #endif
Gendy 0:4a55d0a21ea9 165
Gendy 0:4a55d0a21ea9 166 #ifdef OUTPUT_READABLE_REALACCEL
Gendy 0:4a55d0a21ea9 167 mpu.dmpGetQuaternion(&q, fifoBuffer);
Gendy 0:4a55d0a21ea9 168 mpu.dmpGetAccel(&aa, fifoBuffer);
Gendy 0:4a55d0a21ea9 169 mpu.dmpGetGravity(&gravity, &q);
Gendy 0:4a55d0a21ea9 170 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Gendy 0:4a55d0a21ea9 171 #endif
Gendy 0:4a55d0a21ea9 172
Gendy 0:4a55d0a21ea9 173 #ifdef OUTPUT_READABLE_WORLDACCEL
Gendy 0:4a55d0a21ea9 174 mpu.dmpGetQuaternion(&q, fifoBuffer);
Gendy 0:4a55d0a21ea9 175 mpu.dmpGetAccel(&aa, fifoBuffer);
Gendy 0:4a55d0a21ea9 176 mpu.dmpGetGravity(&gravity, &q);
Gendy 0:4a55d0a21ea9 177 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Gendy 0:4a55d0a21ea9 178 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Gendy 0:4a55d0a21ea9 179 #endif
Gendy 0:4a55d0a21ea9 180 #ifdef OUTPUT_TEAPOT
Gendy 0:4a55d0a21ea9 181 teapotPacket[2] = fifoBuffer[0];
Gendy 0:4a55d0a21ea9 182 teapotPacket[3] = fifoBuffer[1];
Gendy 0:4a55d0a21ea9 183 teapotPacket[4] = fifoBuffer[4];
Gendy 0:4a55d0a21ea9 184 teapotPacket[5] = fifoBuffer[5];
Gendy 0:4a55d0a21ea9 185 teapotPacket[6] = fifoBuffer[8];
Gendy 0:4a55d0a21ea9 186 teapotPacket[7] = fifoBuffer[9];
Gendy 0:4a55d0a21ea9 187 teapotPacket[8] = fifoBuffer[12];
Gendy 0:4a55d0a21ea9 188 teapotPacket[9] = fifoBuffer[13];
Gendy 0:4a55d0a21ea9 189
Gendy 0:4a55d0a21ea9 190 for(int i=0;i<14;i++)
Gendy 0:4a55d0a21ea9 191 {
Gendy 0:4a55d0a21ea9 192 pc.putc(teapotPacket[i]);
Gendy 0:4a55d0a21ea9 193 }
Gendy 0:4a55d0a21ea9 194 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
Gendy 0:4a55d0a21ea9 195 #endif
Gendy 0:4a55d0a21ea9 196
Gendy 0:4a55d0a21ea9 197 }
Gendy 0:4a55d0a21ea9 198 }
Gendy 0:4a55d0a21ea9 199
Gendy 0:4a55d0a21ea9 200
Gendy 0:4a55d0a21ea9 201
Gendy 0:4a55d0a21ea9 202 void send_data(){
Gendy 0:4a55d0a21ea9 203 /////////// Pitch Final output calibration ///////////////
Gendy 0:4a55d0a21ea9 204
Gendy 0:4a55d0a21ea9 205 int Final_output_x1 = 1100+OutputPITCH+delta_trotle-RATEYoutput;
Gendy 0:4a55d0a21ea9 206 int Final_output_x2 = 1100-OutputPITCH+delta_trotle+RATEYoutput;
Gendy 0:4a55d0a21ea9 207 float min=1050;
Gendy 0:4a55d0a21ea9 208 if (Final_output_x1 < min){
Gendy 0:4a55d0a21ea9 209 Final_output_x1=min;
Gendy 0:4a55d0a21ea9 210 }
Gendy 0:4a55d0a21ea9 211 else if (Final_output_x2 <min){
Gendy 0:4a55d0a21ea9 212 Final_output_x2=min;
Gendy 0:4a55d0a21ea9 213 }
Gendy 0:4a55d0a21ea9 214 /////////// Roll Final output calibration ///////////////
Gendy 0:4a55d0a21ea9 215
Gendy 0:4a55d0a21ea9 216 int Final_output_y1 = 1100+OutputROLL+delta_trotle+RATEXoutput;
Gendy 0:4a55d0a21ea9 217 int Final_output_y2 = 1100-OutputROLL+delta_trotle-RATEXoutput;
Gendy 0:4a55d0a21ea9 218 if (Final_output_y1 < min){
Gendy 0:4a55d0a21ea9 219 Final_output_y1=min;
Gendy 0:4a55d0a21ea9 220 }
Gendy 0:4a55d0a21ea9 221 else if (Final_output_y2 <min){
Gendy 0:4a55d0a21ea9 222 Final_output_y2=min;
Gendy 0:4a55d0a21ea9 223 }
Gendy 0:4a55d0a21ea9 224
Gendy 0:4a55d0a21ea9 225 //////////////// Altitude final output calibration/////////
Gendy 0:4a55d0a21ea9 226
Gendy 0:4a55d0a21ea9 227
Gendy 0:4a55d0a21ea9 228
Gendy 0:4a55d0a21ea9 229
Gendy 0:4a55d0a21ea9 230 //////////////// FINALIZATION /////////////////
Gendy 0:4a55d0a21ea9 231 esc1.pulsewidth_us(Final_output_y2);
Gendy 0:4a55d0a21ea9 232 esc2.pulsewidth_us(Final_output_x1);
Gendy 0:4a55d0a21ea9 233 esc3.pulsewidth_us(Final_output_y1);
Gendy 0:4a55d0a21ea9 234 esc4.pulsewidth_us(Final_output_x2);
Gendy 0:4a55d0a21ea9 235
Gendy 0:4a55d0a21ea9 236 }
Gendy 0:4a55d0a21ea9 237
Gendy 0:4a55d0a21ea9 238
Gendy 0:4a55d0a21ea9 239 void calibration(){
Gendy 0:4a55d0a21ea9 240 if(pc.readable()) {
Gendy 0:4a55d0a21ea9 241 switch (pc.getc()) {
Gendy 0:4a55d0a21ea9 242 case 'c':
Gendy 0:4a55d0a21ea9 243 AngleOffset[0]=ypr[0];
Gendy 0:4a55d0a21ea9 244 AngleOffset[1]=ypr[1];
Gendy 0:4a55d0a21ea9 245 AngleOffset[2]=ypr[2];
Gendy 0:4a55d0a21ea9 246 GyroOffset[0]=gx;
Gendy 0:4a55d0a21ea9 247 GyroOffset[1]=gy;
Gendy 0:4a55d0a21ea9 248 GyroOffset[2]=gz;
Gendy 0:4a55d0a21ea9 249 break;
Gendy 0:4a55d0a21ea9 250 case 'x':
Gendy 0:4a55d0a21ea9 251 start_flag=0;
Gendy 0:4a55d0a21ea9 252 case 's':
Gendy 0:4a55d0a21ea9 253 start_flag=1;
Gendy 0:4a55d0a21ea9 254 }
Gendy 0:4a55d0a21ea9 255 }
Gendy 0:4a55d0a21ea9 256 }
Gendy 0:4a55d0a21ea9 257 //////////////////////////////////////////////////////////////////////////////////////////////////
Gendy 0:4a55d0a21ea9 258 //////////////////////////////////////////////////////////////////////////////////////////////////
Gendy 0:4a55d0a21ea9 259
Gendy 0:4a55d0a21ea9 260 int main()
Gendy 0:4a55d0a21ea9 261 {
Gendy 0:4a55d0a21ea9 262
Gendy 0:4a55d0a21ea9 263 esc1.period_ms(20);
Gendy 0:4a55d0a21ea9 264 esc2.period_ms(20);
Gendy 0:4a55d0a21ea9 265 esc3.period_ms(20);
Gendy 0:4a55d0a21ea9 266 esc4.period_ms(20);
Gendy 0:4a55d0a21ea9 267 #define D_SDA p9
Gendy 0:4a55d0a21ea9 268 #define D_SCL p10
Gendy 0:4a55d0a21ea9 269 I2C i2c(D_SDA, D_SCL);
Gendy 0:4a55d0a21ea9 270 DigitalOut myled1(LED1);
Gendy 0:4a55d0a21ea9 271 DigitalOut myled2(LED2);
Gendy 0:4a55d0a21ea9 272 DigitalOut myled3(LED3);
Gendy 0:4a55d0a21ea9 273 DigitalOut heartbeatLED(LED4);
Gendy 0:4a55d0a21ea9 274 #define D_BAUDRATE 115200
Gendy 0:4a55d0a21ea9 275 pc.baud(D_BAUDRATE);
Gendy 0:4a55d0a21ea9 276 pc.printf("Initializing I2C devices...\n");
Gendy 0:4a55d0a21ea9 277 mpu.initialize();
Gendy 0:4a55d0a21ea9 278 pc.printf("Testing device connections...\n");
Gendy 0:4a55d0a21ea9 279 bool mpu6050TestResult = mpu.testConnection();
Gendy 0:4a55d0a21ea9 280 if(mpu6050TestResult){
Gendy 0:4a55d0a21ea9 281 pc.printf("MPU6050 test passed \n");
Gendy 0:4a55d0a21ea9 282 } else{
Gendy 0:4a55d0a21ea9 283 pc.printf("MPU6050 test failed \n");
Gendy 0:4a55d0a21ea9 284 }
Gendy 0:4a55d0a21ea9 285 pc.printf("\nSend any character to begin DMP programming and demo: ");
Gendy 0:4a55d0a21ea9 286 while(!pc.readable());
Gendy 0:4a55d0a21ea9 287 pc.getc();
Gendy 0:4a55d0a21ea9 288 pc.printf("\n");
Gendy 0:4a55d0a21ea9 289 pc.printf("Initializing DMP...\n");
Gendy 0:4a55d0a21ea9 290 devStatus = mpu.dmpInitialize();
Gendy 0:4a55d0a21ea9 291 mpu.setXGyroOffset(-61);
Gendy 0:4a55d0a21ea9 292 mpu.setYGyroOffset(-127);
Gendy 0:4a55d0a21ea9 293 mpu.setZGyroOffset(19);
Gendy 0:4a55d0a21ea9 294 mpu.setZAccelOffset(16282);
Gendy 0:4a55d0a21ea9 295 if (devStatus == 0) {
Gendy 0:4a55d0a21ea9 296 pc.printf("Enabling DMP...\n");
Gendy 0:4a55d0a21ea9 297 mpu.setDMPEnabled(true);
Gendy 0:4a55d0a21ea9 298 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n");
Gendy 0:4a55d0a21ea9 299 mpuIntStatus = mpu.getIntStatus();
Gendy 0:4a55d0a21ea9 300 pc.printf("DMP ready! Waiting for first interrupt...\n");
Gendy 0:4a55d0a21ea9 301 dmpReady = true;
Gendy 0:4a55d0a21ea9 302 packetSize = mpu.dmpGetFIFOPacketSize();
Gendy 0:4a55d0a21ea9 303 } else {
Gendy 0:4a55d0a21ea9 304 pc.printf("DMP Initialization failed (code ");
Gendy 0:4a55d0a21ea9 305 pc.printf("%u",devStatus);
Gendy 0:4a55d0a21ea9 306 pc.printf(")\n");
Gendy 0:4a55d0a21ea9 307 }
Gendy 0:4a55d0a21ea9 308
Gendy 0:4a55d0a21ea9 309 esc1.pulsewidth_us(in_pulse);
Gendy 0:4a55d0a21ea9 310 esc2.pulsewidth_us(in_pulse);
Gendy 0:4a55d0a21ea9 311 esc3.pulsewidth_us(in_pulse);
Gendy 0:4a55d0a21ea9 312 esc4.pulsewidth_us(in_pulse);
Gendy 0:4a55d0a21ea9 313
Gendy 0:4a55d0a21ea9 314
Gendy 0:4a55d0a21ea9 315 //pc.printf("i am entering the loop");
Gendy 0:4a55d0a21ea9 316
Gendy 0:4a55d0a21ea9 317 ///////////////////////////////////////////
Gendy 0:4a55d0a21ea9 318 // PROGRAM LOOP //
Gendy 0:4a55d0a21ea9 319 ///////////////////////////////////////////
Gendy 0:4a55d0a21ea9 320
Gendy 0:4a55d0a21ea9 321 while(1)
Gendy 0:4a55d0a21ea9 322 {
Gendy 0:4a55d0a21ea9 323
Gendy 0:4a55d0a21ea9 324
Gendy 0:4a55d0a21ea9 325 // setting an old value for the readings to use it in the filter
Gendy 0:4a55d0a21ea9 326
Gendy 0:4a55d0a21ea9 327 pAngleYaw = AngleYaw;
Gendy 0:4a55d0a21ea9 328 pAnglePitch = AnglePitch;
Gendy 0:4a55d0a21ea9 329 pAngleRoll= AngleRoll;
Gendy 0:4a55d0a21ea9 330 pgx = Rate_x;
Gendy 0:4a55d0a21ea9 331 pgy = Rate_y;
Gendy 0:4a55d0a21ea9 332 pgz = Rate_z;
Gendy 0:4a55d0a21ea9 333 mpu_code();
Gendy 0:4a55d0a21ea9 334
Gendy 0:4a55d0a21ea9 335 calibration();
Gendy 0:4a55d0a21ea9 336 //pc.printf("g");
Gendy 0:4a55d0a21ea9 337 AngleYaw = ypr[0] - AngleOffset[0]; // calibrating the readings
Gendy 0:4a55d0a21ea9 338 AnglePitch=ypr[1] - AngleOffset[1];
Gendy 0:4a55d0a21ea9 339 AngleRoll=ypr[2] - AngleOffset[2];
Gendy 0:4a55d0a21ea9 340 Rate_x =gx - GyroOffset[0];
Gendy 0:4a55d0a21ea9 341 Rate_y =gy - GyroOffset[1];
Gendy 0:4a55d0a21ea9 342 Rate_z =gz - GyroOffset[2];
Gendy 0:4a55d0a21ea9 343 AngleYaw = 0.7 * pAngleYaw + 0.3 * AngleYaw; // low pass filter
Gendy 0:4a55d0a21ea9 344 AnglePitch = 0.7 * pAnglePitch + 0.3 * AnglePitch;
Gendy 0:4a55d0a21ea9 345 AngleRoll = 0.7 * pAngleRoll + 0.3 * AngleRoll;
Gendy 0:4a55d0a21ea9 346 Rate_x = 0.6 * pgx + 0.4 * Rate_x;
Gendy 0:4a55d0a21ea9 347 Rate_y = 0.6 * pgy + 0.4 * Rate_y;
Gendy 0:4a55d0a21ea9 348 Rate_z = 0.6 * pgz + 0.4 * Rate_z;
Gendy 0:4a55d0a21ea9 349
Gendy 0:4a55d0a21ea9 350 InputPITCH=AnglePitch * 180/M_PI;
Gendy 0:4a55d0a21ea9 351 InputROLL=AngleRoll * 180/M_PI;
Gendy 0:4a55d0a21ea9 352
Gendy 0:4a55d0a21ea9 353 RATEXinput=Rate_x;
Gendy 0:4a55d0a21ea9 354 RATEYinput=Rate_y;
Gendy 0:4a55d0a21ea9 355
Gendy 0:4a55d0a21ea9 356
Gendy 0:4a55d0a21ea9 357
Gendy 0:4a55d0a21ea9 358 ///// measure distance ultrasonic////////
Gendy 0:4a55d0a21ea9 359 /* d = rf.read_m();
Gendy 0:4a55d0a21ea9 360 if (d == -1.0) {
Gendy 0:4a55d0a21ea9 361 printf("Timeout Error.\n");
Gendy 0:4a55d0a21ea9 362 } else if (d > 5.0) {
Gendy 0:4a55d0a21ea9 363 // Seeed's sensor has a maximum range of 4m, it returns
Gendy 0:4a55d0a21ea9 364 // something like 7m if the ultrasound pulse isn't reflected.
Gendy 0:4a55d0a21ea9 365 pc.printf("No object within detection range.\n");
Gendy 0:4a55d0a21ea9 366 } else {
Gendy 0:4a55d0a21ea9 367 pc.printf("p:%f r:%f \n\r",AnglePitch*180/M_PI,AngleRoll*180/M_PI);
Gendy 0:4a55d0a21ea9 368 }*/
Gendy 0:4a55d0a21ea9 369 //////////////////////////
Gendy 0:4a55d0a21ea9 370
Gendy 0:4a55d0a21ea9 371
Gendy 0:4a55d0a21ea9 372 if (start_flag==1){
Gendy 0:4a55d0a21ea9 373 //PITCHPID.Compute();
Gendy 0:4a55d0a21ea9 374 //ROLLPID.Compute();
Gendy 0:4a55d0a21ea9 375 PITCHPIDrate.Compute();
Gendy 0:4a55d0a21ea9 376 ROLLPIDrate.Compute();
Gendy 0:4a55d0a21ea9 377 //altitudecontroller.Compute();
Gendy 0:4a55d0a21ea9 378 send_data();
Gendy 0:4a55d0a21ea9 379 } else{
Gendy 0:4a55d0a21ea9 380 esc1.pulsewidth_us(1000);
Gendy 0:4a55d0a21ea9 381 esc2.pulsewidth_us(1000);
Gendy 0:4a55d0a21ea9 382 esc3.pulsewidth_us(1000);
Gendy 0:4a55d0a21ea9 383 esc4.pulsewidth_us(1000);
Gendy 0:4a55d0a21ea9 384 start_flag=0;
Gendy 0:4a55d0a21ea9 385 }
Gendy 0:4a55d0a21ea9 386
Gendy 0:4a55d0a21ea9 387
Gendy 0:4a55d0a21ea9 388 //pc.printf("pitch:%f roll:%f\r\n",AnglePitch*180/M_PI,AngleRoll*180/M_PI);
Gendy 0:4a55d0a21ea9 389 pc.printf("rate y:%d rate x:%d\r\n",Rate_y,Rate_x);
Gendy 0:4a55d0a21ea9 390 blinkState = !blinkState;
Gendy 0:4a55d0a21ea9 391 myled1 = blinkState;
Gendy 0:4a55d0a21ea9 392
Gendy 0:4a55d0a21ea9 393
Gendy 0:4a55d0a21ea9 394 }
Gendy 0:4a55d0a21ea9 395 }