calccomp.h

00001 // Coded by: Erik van de Coevering
00002 
00003 #include "mbed.h"
00004 #include "MAfilter.h"
00005 
00006 DigitalOut led1(LED1); // for stick arming (leds are active low)
00007 
00008 extern float KP_x, KI_x, KD_x, KP_y, KI_y, KD_y, KP_z, KI_z, KD_z;
00009 float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz, error_x1, error_x, error_y1, error_y;
00010 MAfilter10 ma_errorx, ma_errory;
00011 static signed int m1 = 0;
00012 static signed int m2 = 0;
00013 static signed int m3 = 0;
00014 static signed int m4 = 0;
00015 
00016 void calccomp(int* ctrl, float* angles, int* motor) //ctrl: 0-rud 1-elev 2-throttle 3-aileron      angles: 0-ax 1-ay 2-az 3-gx 4-gy 5-gz
00017 {
00018     //Rx variables
00019     int ruddercenter = 1562;
00020     int elevcenter = 1554;
00021     int aileroncenter = 1550;
00022 
00023     //Variables for calccomp
00024     float xcomp = 0;
00025     float ycomp = 0;
00026     int xcomp2 = 0;
00027     int rud = 0;
00028     int zcomp = 0;
00029     int throttle = 0;
00030     static bool armed = false;
00031     float xcntrl = 0;
00032     float ycntrl = 0;
00033     float error_z = 0;
00034     float error_z1 = 0;
00035 
00036     //Scale rx channels
00037     rud = (((float)(ctrl[0] - ruddercenter))/2.5);
00038     ycntrl = ((float)(ctrl[3] - elevcenter))/8;
00039     throttle = ctrl[2] - 20;
00040     xcntrl = ((float)(ctrl[1] - aileroncenter))/8;
00041 
00042     //Limit throttle
00043     if(throttle > 1950) throttle = 1950;
00044 
00045     //Start by mixing throttle
00046     m1 = throttle;
00047     m2 = throttle;
00048     m3 = throttle;
00049     m4 = throttle;
00050 
00051     // Current values used on a 250 size mini racer (still needs tuning): P: 2.7, I: 2.0, D: 0.4
00052     // Calc PID values and prevent integral windup on KIx
00053     error_x = angles[0] - xcntrl;
00054     KPx = error_x * KP_y;
00055     KIx = KIx + (error_x * 0.001f * KI_y); // PID gains mixed up, will fix soon.
00056 
00057     if(KIx > 10) KIx = 10;
00058     if(KIx < -10) KIx = -10;
00059 
00060     KDx = (angles[3] + (error_x)) * KD_y; // Should be a derative of the error, but this way feels much more responsive
00061 
00062     xcomp = KPx + KIx + KDx;
00063     xcomp2 = xcomp*-1;
00064 
00065     //Mix pitch
00066     m1 = m1 - xcomp2;
00067     m2 = m2 + xcomp2;
00068     m3 = m3 + xcomp2;
00069     m4 = m4 - xcomp2;
00070 
00071     // Calc PID values and prevent integral windup on KIy
00072     error_y = angles[1] + ycntrl;
00073     KPy = error_y * KP_x;
00074     KIy = KIy + (error_y * 0.001f * KI_x);
00075 
00076     if(KIy > 10) KIy = 10;
00077     if(KIy < -10) KIy = -10;
00078     KDy = (angles[4] + (error_y)) * KD_x;
00079 
00080     ycomp = KPy + KIy + KDy;
00081     ycomp = ycomp*-1;
00082 
00083     //Mix roll
00084     m1 = m1 + ycomp;
00085     m2 = m2 + ycomp;
00086     m3 = m3 - ycomp;
00087     m4 = m4 - ycomp;
00088 
00089 
00090     //Calc yaw compensation and mix
00091     error_z = angles[5] + rud;
00092 
00093     KPz = error_z * KP_z;
00094     KIz = KIz + (error_z * 0.001f * KI_z);
00095 
00096     if(KIz > 20) KIz = 20;
00097     if(KIz < -20) KIz = -20;
00098 
00099     KDz = (error_z - error_z1) * KD_z;
00100 
00101     error_z1 = error_z;
00102 
00103     zcomp = (KPz + KDz) * -1.0f;
00104 
00105     //Mix yaw
00106     m1 = m1 - zcomp;
00107     m2 = m2 + zcomp;
00108     m3 = m3 - zcomp;
00109     m4 = m4 + zcomp;
00110 
00111     //When throttle down or if not armed, stop motors
00112     if(throttle < 1100 || armed == false) {
00113         m1=1000;
00114         m2=1000;
00115         m3=1000;
00116         m4=1000;
00117     }
00118 
00119     //Stick arming
00120     if(throttle < 1100 && rud > 100) {
00121         armed = true;
00122         led1 = 0;
00123     }
00124     if(throttle < 1100 && rud < -100) {
00125         armed = false;
00126         led1 = 1;
00127     }
00128 
00129 
00130 
00131     //Output to motors
00132     motor[0] = m1;
00133     motor[1] = m2;
00134     motor[2] = m3;
00135     motor[3] = m4;
00136 }