Diff: calccomp.h

Revision:

8:981f7e2365b6

Parent:

6:033ad7377fee

--- a/calccomp.h	Tue Jul 17 14:56:05 2018 +0000
+++ b/calccomp.h	Tue Jul 31 20:36:57 2018 +0000
@@ -1,11 +1,17 @@
 // Coded by: Erik van de Coevering
 
-// Axis are mixed up, will fix soon.
 #include "mbed.h"
+#include "MAfilter.h"
 
 DigitalOut led1(LED1); // for stick arming (leds are active low)
 
 extern float KP_x, KI_x, KD_x, KP_y, KI_y, KD_y, KP_z, KI_z, KD_z;
+float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz, error_x1, error_x, error_y1, error_y;
+MAfilter10 ma_errorx, ma_errory;
+static signed int m1 = 0;
+static signed int m2 = 0;
+static signed int m3 = 0;
+static signed int m4 = 0;
 
 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
 {
@@ -21,14 +27,9 @@
     int rud = 0;
     int zcomp = 0;
     int throttle = 0;
-    static signed int m1 = 0;
-    static signed int m2 = 0;
-    static signed int m3 = 0;
-    static signed int m4 = 0;
     static bool armed = false;
     float xcntrl = 0;
     float ycntrl = 0;
-    float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz;
     float error_z = 0;
     float error_z1 = 0;
 
@@ -47,14 +48,16 @@
     m3 = throttle;
     m4 = throttle;
 
-    // Current values used on a 250 size mini racer (still needs tuning): P: 1.9, I: 2.4, D: 0.45
+    // Current values used on a 250 size mini racer (still needs tuning): P: 2.7, I: 2.0, D: 0.4
     // Calc PID values and prevent integral windup on KIx
-    KPx = (angles[0] - xcntrl) * KP_y;
-    KIx = KIx + ((angles[0] - xcntrl) * KI_y * 0.001);
+    error_x = angles[0] - xcntrl;
+    KPx = error_x * KP_y;
+    KIx = KIx + (error_x * 0.001f * KI_y); // PID gains mixed up, will fix soon.
 
-    if(KIx > 50) KIx = 50;
-    if(KIx < -50) KIx = -50;
-    KDx = (angles[3] + (angles[0] - xcntrl)) * KD_y;
+    if(KIx > 10) KIx = 10;
+    if(KIx < -10) KIx = -10;
+
+    KDx = (angles[3] + (error_x)) * KD_y; // Should be a derative of the error, but this way feels much more responsive
 
     xcomp = KPx + KIx + KDx;
     xcomp2 = xcomp*-1;
@@ -66,12 +69,13 @@
     m4 = m4 - xcomp2;
 
     // Calc PID values and prevent integral windup on KIy
-    KPy = (angles[1] + ycntrl) * KP_x;
-    KIy = KIy + ((angles[1] + ycntrl) * KI_x * 0.001);
+    error_y = angles[1] + ycntrl;
+    KPy = error_y * KP_x;
+    KIy = KIy + (error_y * 0.001f * KI_x);
 
-    if(KIy > 50) KIy = 50;
-    if(KIy < -50) KIy = -50;
-    KDy = (angles[4] + (angles[1] + ycntrl)) * KD_x;
+    if(KIy > 10) KIy = 10;
+    if(KIy < -10) KIy = -10;
+    KDy = (angles[4] + (error_y)) * KD_x;
 
     ycomp = KPy + KIy + KDy;
     ycomp = ycomp*-1;
@@ -89,13 +93,14 @@
     KPz = error_z * KP_z;
     KIz = KIz + (error_z * 0.001f * KI_z);
 
-    if(KIz > 80) KIz = 80;
-    if(KIz < -80) KIz = -80;
+    if(KIz > 20) KIz = 20;
+    if(KIz < -20) KIz = -20;
 
     KDz = (error_z - error_z1) * KD_z;
+
     error_z1 = error_z;
 
-    zcomp = (KPz + KIz + KDz) * -1.0f;
+    zcomp = (KPz + KDz) * -1.0f;
 
     //Mix yaw
     m1 = m1 - zcomp;
@@ -128,4 +133,4 @@
     motor[1] = m2;
     motor[2] = m3;
     motor[3] = m4;
-}
\ No newline at end of file
+}