Diff: main.cpp

Revision:

0:a6f29afbd285

diff -r 000000000000 -r a6f29afbd285 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 07 07:49:21 2020 +0000
@@ -0,0 +1,71 @@
+#include "mbed.h"
+#include "string"
+#include "Sabertooth_Serial.h"
+#define INT_TIME 0.02
+#define ADRS 130
+#define UP    0b00000001
+#define DOWN  0b00000010
+#define HANDR 0b00000100
+#define HANDL 0b00001000
+#define OTHER 0b00010000
+
+
+Ticker timer;
+
+RawSerial Master(p28, p27, 115200);
+
+SaberSerial MD(115200, p13, p14);
+
+DigitalOut hund_fetR(p23);
+DigitalOut hund_fetL(p24);
+
+string Master_recv = "";
+char buttons = 0b00000000, pre_buttons = 0b00000000;
+
+void recv_interrupt(){
+    char recv_c = Master.getc();
+    
+    if (recv_c == '\n'){
+        buttons = Master_recv[0];
+        Master_recv = "";
+    }
+    else{
+        Master_recv += recv_c;
+        if(Master_recv.size() == 2) Master_recv = "";
+    }
+}
+
+void timer_interrupt(){
+    
+    int saber_speed = 0, saber_def = 0;
+    int hundR_counter = 0, hundL_counter = 0;
+    
+    if(~buttons & HANDR && pre_buttons & HANDR) hundR_counter++;
+    if(~buttons & HANDL && pre_buttons & HANDL) hundL_counter++;
+    
+    hund_fetR.write(hundR_counter % 2);
+    hund_fetL.write(hundL_counter % 2);
+    
+    if(buttons & UP){
+        saber_speed = 50;
+        saber_def = 0;
+    }
+    else if(buttons & DOWN){
+        saber_speed = 50;
+        saber_def = 1;
+    }
+    else saber_speed = 0;
+    
+    MD.Serial(ADRS,saber_def,saber_speed);
+    
+    pre_buttons = buttons;
+}
+
+int main() {
+    
+    timer.attach(&timer_interrupt,INT_TIME);
+    
+    Master.attach(&recv_interrupt, RawSerial::RxIrq);
+    
+    while(true) {}
+}