Diff: main.cpp

Revision:

0:2c7c889dbf1a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 07 07:50:02 2020 +0000
@@ -0,0 +1,68 @@
+#include "mbed.h"
+#include "RoboClaw.h"
+#include "string"
+
+#define SQRT_3 1.73205080757
+#define MAX_QPPS1 40960
+#define MAX_QPPS2 40960
+#define INT_TIME 0.02
+#define ADRS1 129
+#define ADRS2 130
+
+Ticker timer;
+
+RawSerial Slave (p9,p10,115200);
+RawSerial controller (p13,p14,115200);
+
+RoboClaw MD(115200,p28,p27);
+
+string recv_str = "";
+char c_x = 127, c_y = 127, c_rot  = 127, buttons = 0b00000000;
+
+
+
+void controller_recv(){
+    char recv_c = controller.getc();
+    
+    if(recv_str.size() == 4 && recv_c == '\n'){
+        c_x = recv_str[0];
+        c_y = recv_str[1];
+        c_rot = recv_str[2];
+        buttons = recv_str[3];
+        recv_str = "";
+    }
+    
+    else{
+        recv_str += recv_c;
+        if(recv_str.size() >= 5) recv_str = "";
+    }
+}
+
+void Roboclaw_send(double V1, double V2, double V3){
+    MD.SpeedM1(ADRS1,int(V1*MAX_QPPS1));
+    MD.SpeedM2(ADRS1,int(V2*MAX_QPPS1));
+    MD.SpeedM1(ADRS2,int(V3*MAX_QPPS2));
+}
+
+void timer_interrupt(){
+    int i_x = (int)c_x - 127;
+    int i_y = (int)c_y - 127;
+    int i_rot = (int)c_rot - 127;
+    
+    double V1 = (double)i_x + (double)i_rot;
+    double V2 = -0.5*(double)i_x + SQRT_3*0.5*(double)i_y + (double)i_rot;
+    double V3 = -0.5*(double)i_x - SQRT_3*0.5*(double)i_y + (double)i_rot;
+    
+    Roboclaw_send(V1,V2,V3);
+    Slave.putc(buttons);
+    Slave.putc('\n');
+}
+
+int main() {
+    
+    timer.attach(&timer_interrupt,INT_TIME);
+    
+    controller.attach(&controller_recv,RawSerial::RxIrq);
+    
+    while(true) {}
+}