Diff: main.cpp

Revision:

1:2e084048b5f9

Parent:

0:a6f29afbd285

--- a/main.cpp	Wed Oct 07 07:49:21 2020 +0000
+++ b/main.cpp	Sun Oct 11 08:41:49 2020 +0000
@@ -2,7 +2,7 @@
 #include "string"
 #include "Sabertooth_Serial.h"
 #define INT_TIME 0.02
-#define ADRS 130
+#define ADRS 133
 #define UP    0b00000001
 #define DOWN  0b00000010
 #define HANDR 0b00000100
@@ -12,12 +12,15 @@
 
 Ticker timer;
 
+RawSerial pc(USBTX,USBRX,115200);
 RawSerial Master(p28, p27, 115200);
 
 SaberSerial MD(115200, p13, p14);
 
-DigitalOut hund_fetR(p23);
-DigitalOut hund_fetL(p24);
+DigitalOut hund_fetR(p15);
+DigitalOut hund_fetL(p16);
+
+DigitalIn limit(p25);
 
 string Master_recv = "";
 char buttons = 0b00000000, pre_buttons = 0b00000000;
@@ -38,31 +41,42 @@
 void timer_interrupt(){
     
     int saber_speed = 0, saber_def = 0;
-    int hundR_counter = 0, hundL_counter = 0;
+    bool hand_R = buttons & HANDR;
+    bool hand_L = buttons & HANDL;
     
-    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);
+    hund_fetR.write(hand_R);
+    hund_fetL.write(hand_L);
     
     if(buttons & UP){
+        saber_speed = 10;
+        saber_def = 4;
+    }
+    else if(buttons & OTHER){
         saber_speed = 50;
-        saber_def = 0;
+        saber_def = 4;
     }
     else if(buttons & DOWN){
-        saber_speed = 50;
-        saber_def = 1;
+        saber_speed = 10;
+        saber_def = 5;
     }
-    else saber_speed = 0;
+    else {
+        saber_speed = 0;
+        saber_def = 4;
+    }
+        
+    if(limit.read() && saber_def == 4) saber_speed = 0;
     
     MD.Serial(ADRS,saber_def,saber_speed);
+    pc.printf("%d,%d\n",hand_R,hand_L);
     
     pre_buttons = buttons;
 }
 
 int main() {
     
+    hund_fetR = 0;
+    hund_fetL = 0;
+    
     timer.attach(&timer_interrupt,INT_TIME);
     
     Master.attach(&recv_interrupt, RawSerial::RxIrq);