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?
User | Revision | Line number | New 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 | } |