Diff: main.cpp

Revision:

6:2cba569272fe

Parent:

5:9cad4ce807b9

Child:

7:70161eb0f854

--- a/main.cpp	Thu Jan 21 09:09:57 2021 +0000
+++ b/main.cpp	Thu Jan 21 15:25:51 2021 +0000
@@ -1,5 +1,8 @@
 #include "mbed.h"
 #include "SBUS.hpp"
+#include "LoopTicker.hpp"
+
+
 SBUS sbus(PD_5, PD_6);
 Serial pc(USBTX, USBRX);
 
@@ -8,11 +11,6 @@
 PwmOut servo1(PB_4);
 PwmOut servo2(PB_5);
 
-long map(long x, long in_min, long in_max, long out_min, long out_max)
-{
-  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
 int ch1, ch2;
 float rc1, rc2;
 int out1, out2;
@@ -22,33 +20,30 @@
     led1 = !led1;
     if(sbus.failSafe == false)
     {
-
         ch1 = int(sbus.getData(1));
         ch2 = int(sbus.getData(2));
-        pc.printf("ch1 :%d\r\n", ch1);
-        pc.printf("ch2 :%d\r\n", ch1);
-        float LP_rc = 0.65;
-        float LP_rc3 = 0.15;
-        rc1 = LP_rc*float(map(ch1,368,1680,-1000,1000))/1000.0+(1.0-LP_rc)*rc1;
-        rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0+(1.0-LP_rc)*rc2;
-        pc.printf("rc1 :%f\r\n", rc1);
-        pc.printf("rc2 :%f\r\n", rc1);
+        pc.printf("ch1 :%d ", ch1);
+        pc.printf("ch2 :%d ", ch2);
+        float LP_rc = 0.65f;
+        float LP_rc3 = 0.15f;
+        rc1 = LP_rc*float(SBUS::map(ch1,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1;
+        rc2 = LP_rc*float(SBUS::map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2;
+        pc.printf("rc1 :%f ", rc1);
+        pc.printf("rc2 :%f ", rc1);
         int pwmMax = 1800;
         int pwmMin = 1200;
-        out1 = map((int)(rc1*1000.0),-1000,1000,pwmMin,pwmMax);
+        out1 = SBUS::map((int)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax);
         if(out1<pwmMin){out1 = pwmMin;};
         if(out1>pwmMax){out1 = pwmMax;};
-        out2 = map((int)(rc2*1000.0),-1000,1000,pwmMin,pwmMax);
+        out2 = SBUS::map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax);
         if(out2<pwmMin){out2 = pwmMin;};
         if(out2>pwmMax){out2 = pwmMax;};
         
-        pc.printf("out1:%d\r\n", out1);
+        pc.printf("out1:%d ", out1);
         pc.printf("out2:%d\r\n", out2);
         servo1.pulsewidth_us(out1);
         servo2.pulsewidth_us(out2);
     }
-    
-    
 }
 
 void interrupt_Gyro()
@@ -61,11 +56,12 @@
     pc.baud(9600);
     servo1.period(0.020);
     servo2.period(0.020);
-    Ticker timer_SBUS;
-    Ticker timer_Gyro;
+    LoopTicker timer_SBUS;
+    LoopTicker timer_Gyro;
     timer_SBUS.attach(interrupt_SBUS, 0.020);
-    timer_Gyro.attach(interrupt_Gyro, 0.1);
+    timer_Gyro.attach(interrupt_Gyro, 0.5);
     while(1) {
-        
+        timer_SBUS.loop();
+        timer_SBUS.loop();
     }
 }
\ No newline at end of file