Diff: PidCombine.cpp

Revision:

0:458d6472bd2d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PidCombine.cpp	Thu Apr 11 22:17:16 2019 +0000
@@ -0,0 +1,39 @@
+#include "mbed.h"
+#include "PidCombine.h"
+
+void pidcombine(float rolls, float pitchs)
+{
+    float roll = (rolls/2);
+    float pitch = (pitchs-15);
+    float top = roll+15;
+    float bot = 15-roll;
+    float centre = 15;
+    
+    while (pitch != 0)
+    {
+        if (pitch > 0)
+        {
+            if (top<48)
+            {
+                centre = centre+1;
+                pitch = pitch-1;
+                top=top+1;
+            }else{
+                pitch=0;
+            }
+        }else{
+            if (bot>-18)
+            {
+                centre=centre-1;
+                pitch=pitch+1;
+                bot=bot-1;
+            }else{
+                pitch=0;
+            }
+        }
+    }
+        
+    float diff=roll*2;
+    centre=centre;
+
+}
\ No newline at end of file