Diff: calccomp.h

Revision:

0:0929d3d566cf

Child:

2:61ad38ea550d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/calccomp.h	Mon Jul 09 16:31:40 2018 +0000
@@ -0,0 +1,139 @@
+#include "mbed.h"
+
+DigitalOut led1(LED1); // for stick arming (leds are active low)
+
+extern float Kp, Ki, Kd;
+
+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
+{
+		//Rx variables
+		int ruddercenter = 1562;
+		int elevcenter = 1554;
+		int aileroncenter = 1550;
+	
+		//Variables for calccomp
+		float xcomp = 0;
+		float ycomp = 0;
+		int xcomp2 = 0;
+//		int ycomp2 = 0;
+		float ruddercomp = 0;
+//		float ratio = 0.3;
+		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, KDx, KPy, KDy;
+//		static float KIx = 0;
+//		static float KIy = 0;
+	
+    //Scale rx channels
+    rud = (((float)(ctrl[0] - ruddercenter))/2.5);
+    ycntrl = ((float)(ctrl[3] - elevcenter))/8;
+    throttle = ctrl[2] - 20;
+    xcntrl = ((float)(ctrl[1] - aileroncenter))/8;
+
+    //Limit throttle
+    if(throttle > 1950) throttle = 1950;
+
+    //Start by mixing throttle
+    m1 = throttle;
+    m2 = throttle;
+    m3 = throttle;
+    m4 = throttle;
+	/*
+		// Calc PID values and prevent integral windup on KIx
+		KPx = (angles[0] + xcntrl) * 1.05f;
+		KIx = KIx + ((angles[0] + xcntrl) * 0.001f);
+		
+		if(KIx > 30) KIx = 30;
+		if(KIx < -30) KIx = -30;
+		KDx = (angles[3] + (angles[0] + xcntrl)*0.8f) * 0.45f;
+		
+		xcomp = KPx + KIx + KDx;
+		xcomp2 = xcomp*-1;
+	*/
+    //Calc aileron compensation and mix
+    xcomp = ((((angles[0])-xcntrl)*Kp) + ((angles[3]+((angles[0]-xcntrl)*1.0f))*Kd));
+    xcomp2 = (xcomp*-1);
+ //   xcomp2 = 0;
+
+    //Mix aileron
+    m1 = m1 - xcomp2;
+    m2 = m2 + xcomp2;
+    m3 = m3 + xcomp2;
+    m4 = m4 - xcomp2;
+	/*
+		// Calc PID values and prevent integral windup on KIx
+		KPy = (angles[1] + ycntrl) * 1.05f;
+		KIy = KIx + ((angles[1] + ycntrl) * 0.001f);
+		
+		if(KIy > 30) KIy = 30;
+		if(KIy < -30) KIy = -30;
+		KDx = (angles[4] + (angles[1] + ycntrl)*0.8f) * 0.45f;
+		
+		ycomp = KPy + KIy + KDy;
+	*/
+    //ycntrl = ycntrl+5;
+    //Calc elevator compensation and mix
+    ycomp = ((((angles[1])+(ycntrl))*Kp) + ((angles[4]+((angles[1]+ycntrl)*1.0f))*Kd));
+    ycomp = ycomp*-1;
+   // ycomp2 = 0;
+
+    //Mix elevator
+    m1 = m1 + ycomp;
+    m2 = m2 + ycomp;
+    m3 = m3 - ycomp;
+    m4 = m4 - ycomp;
+
+
+    //Calc rudder compensation and mix
+    ruddercomp = (rud + (angles[5]*1.0f));  //has drift
+    zcomp = ruddercomp*-1;
+		//zcomp = 0;
+
+    //Mix rudder
+    m1 = m1 - zcomp;
+    m2 = m2 + zcomp;
+    m3 = m3 - zcomp;
+    m4 = m4 + zcomp;
+
+    //Prevent motors from stalling
+//           if(m1 < 1075 && throttle > 1000) m1 = 1280;
+//           if(m2 < 1075 && throttle > 1000) m2 = 1280;
+//           if(m3 < 1075 && throttle > 1000) m3 = 1280;
+//           if(m4 < 1075 && throttle > 1000) m4 = 1280;
+
+		//When throttle down or if not armed, stop motors
+    if(throttle < 1100 || armed == false) {
+        m1=1000;
+        m2=1000;
+        m3=1000;
+        m4=1000;
+    }
+
+    //Stick arming
+    if(throttle < 1100 && rud > 100) {
+        armed = true;
+        led1 = 0;
+    }
+    if(throttle < 1100 && rud < -100) {
+        armed = false;
+        led1 = 1;
+    }
+
+
+		
+    //Output to motors
+		motor[0] = m1;
+		motor[1] = m2;
+		motor[2] = m3;
+		motor[3] = m4;
+}
+