Diff: aldebaran_rc.cpp

Revision:

0:2228de67a3ef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/aldebaran_rc.cpp	Fri Mar 19 16:43:46 2021 +0000
@@ -0,0 +1,100 @@
+#include "aldebaran_rc.h"
+#include "mbed.h"
+#define OFFSET 2
+
+Serial bt(p9, p10);
+
+Timer timer;
+
+DigitalOut st_led(LED1);
+DigitalOut th_led(LED2);
+DigitalOut mid_led(LED3);
+
+InterruptIn ST(p15);
+InterruptIn TH(p14);
+
+PwmOut Right(p21);
+PwmOut Left(p22);
+
+int max_st = 1500, max_th = 1500, min_st = 1500, min_th = 1500, mid_st, mid_th;
+
+int RC_value[2];
+int RC_start[2];
+
+void ST_High() { RC_start[0] = timer.read_us(); } //start the time }
+void ST_Low() { RC_value[0] = timer.read_us() - RC_start[0]; }
+
+void TH_High() { RC_start[1] = timer.read_us(); } //start the time }
+void TH_Low() { RC_value[1] = timer.read_us() - RC_start[1]; }
+
+void RC_mode()
+{
+    int st_start, th_start;
+
+    timer.start();
+
+    ST.fall(&ST_Low);
+    ST.rise(&ST_High);
+
+    TH.fall(&TH_Low);
+    TH.rise(&TH_High);
+
+    Right.period(0.02);
+    Left.period(0.02);
+
+    bt.baud(9600);
+
+    st_led = 1;
+    wait(0.5);
+    st_led = 0;
+
+    st_start = timer.read_us();
+    while (timer.read_us() - st_start < 5000000)
+    {
+        if (RC_value[0] > max_st)
+        {
+            max_st = RC_value[0];
+        }
+        if (RC_value[0] < min_st)
+        {
+            min_st = RC_value[0];
+        }
+    }
+    st_led = 1;
+    wait(1);
+    th_led = 1;
+    wait(0.5);
+    th_led = 0;
+    th_start = timer.read_us();
+    while (timer.read_us() - th_start < 5000000)
+    {
+        if (RC_value[1] > max_th)
+        {
+            max_th = RC_value[1];
+        }
+        if (RC_value[1] < min_th)
+        {
+            min_th = RC_value[1];
+        }
+    }
+    th_led = 1;
+
+    mid_led = 1;
+    wait(5);
+    mid_led = 0;
+    mid_st = RC_value[0];
+    mid_th = RC_value[1];
+    wait(1);
+    mid_led = 1;
+
+    bt.printf("Max-ST: %i Min-ST: %i Max-TH: %i Min-TH: %i\n\r", max_st, min_st, max_th, min_th);
+
+    wait(5);
+    while (1)
+    {
+        bt.printf("Left: %i  Right: %i\n\r", RC_value[1], RC_value[0]);
+
+        Right.pulsewidth_us(mid_st + (mid_st - RC_value[1]) + (mid_th - RC_value[0]));
+        Left.pulsewidth_us(mid_st + (mid_st - RC_value[1]) - (mid_th - RC_value[0]) - OFFSET);
+    }
+}