Diff: main.cpp

Revision:

3:79e62f9b13c8

Parent:

0:6b18a09a6628

Child:

4:7d2eae0115a2

--- a/main.cpp	Thu Jan 21 07:32:48 2021 +0000
+++ b/main.cpp	Thu Jan 21 08:59:55 2021 +0000
@@ -3,15 +3,72 @@
 SBUS sbus(PD_5, PD_6);
 Serial pc(USBTX, USBRX);
 
+DigitalOut led1(LED1);
+DigitalOut led3(LED3);
+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, ch1_b, ch2_b;
+float rc1, rc2;
+int out1, out2;
+
+void interrupt_SBUS()
+{
+    led1 = !led1;
+    if(sbus.failSafe == false)
+    {
+        ch1_b = ch1;
+        ch2_b = ch2;
+        ch1 = int(sbus.getData(1));
+        ch2 = int(sbus.getData(2));
+        pc.printf("ch1 :%d\r\n", ch1);
+        pc.printf("ch2 :%d\r\n", ch1);
+        if(abs(ch1-ch1_b) < 100){
+          rc1 = (float)(map(ch1,368,1680,-1000,1000))/1000.0;
+        }
+        if(abs(ch2-ch2_b) < 100){
+          rc2 = (float)(map(ch2,368,1680,-1000,1000))/1000.0;
+        }
+        pc.printf("rc1 :%f\r\n", rc1);
+        pc.printf("rc2 :%f\r\n", rc1);
+        int pwmMax = 1800;
+        int pwmMin = 1200;
+        out1 = map((int)(rc1*1000.0),-1000,1000,pwmMin,pwmMax);
+        if(out1<pwmMin){out1 = pwmMin;};
+        if(out1>pwmMax){out1 = pwmMax;};
+        out2 = map((int)(rc2*1000.0),-1000,1000,pwmMin,pwmMax);
+        if(out2<pwmMin){out2 = pwmMin;};
+        if(out2>pwmMax){out2 = pwmMax;};
+        
+        pc.printf("out1:%d\r\n", out1);
+        pc.printf("out2:%d\r\n", out2);
+        servo1.pulsewidth_us(out1);
+        servo2.pulsewidth_us(out2);
+    }
+    
+    
+}
+
+void interrupt_Gyro()
+{
+    led3 = !led3;
+}
+
 int main()
 {
     pc.baud(9600);
+    servo1.period(0.020);
+    servo2.period(0.020);
+    Ticker timer_SBUS;
+    Ticker timer_Gyro;
+    timer_SBUS.attach(interrupt_SBUS, 1.0);
+    timer_Gyro.attach(interrupt_Gyro, 0.1);
     while(1) {
-        if(sbus.failSafe == false){
-            for (int i = 0; i < 4; i++){
-                pc.printf("%d\t", sbus.getData(i));
-                }
-            pc.printf("\n");
-        } 
+        
     }
 }
\ No newline at end of file