Diff: main.cpp

Revision:

1:72b8490a4ece

Parent:

0:bba4760cb391

--- a/main.cpp	Tue Mar 05 09:18:07 2019 +0000
+++ b/main.cpp	Wed Aug 28 01:27:36 2019 +0000
@@ -4,52 +4,48 @@
 Serial pc(USBTX,USBRX);
 DigitalOut POWER_LED(D5);
 
-int direct,speed;
-int Raxis[2],Laxis[2];
-char Rfmot,Lfmot;
-char Rbmot,Lbmot;
-char mot1,air1;
+static int direct,speed;
+static char wheel_RF,wheel_LF;
+static char wheel_RB,wheel_LB;
+static char arm_moter;
 
 int main(void)
 {
+    POWER_LED = 1;
     speed = fast;
-    direct = sb;
-    format_data(&Rfmot);
-    format_data(&Lfmot);
-    format_data(&Rbmot);
-    format_data(&Lbmot);
-    format_data(&mot1);
-    format_data(&air1);
-    POWER_LED = 1;
+    direct = SBREAK;
+    format_data(&wheel_RF);
+    format_data(&wheel_LF);
+    format_data(&wheel_RB);
+    format_data(&wheel_LB);
+    format_data(&arm_moter);
     while(1) {
-        get_stick(Raxis,'R');
-        get_stick(Laxis,'L');
-        speed = set_speed(speed);
-        direct = set_direct(Raxis, Laxis);
-
         set_emg();
+        set_direct(&direct);
+        set_speed(&speed);
+        set_mot(&arm_moter, cir, cro);
         if(direct != 0) {
-            set_tire(&Rfmot, &Rbmot, direct, 'R', speed);
-            set_tire(&Lfmot, &Lbmot, direct, 'L', speed);
+            set_tire(&wheel_RF, &wheel_RB, direct, 'R', speed);
+            set_tire(&wheel_LF, &wheel_LB, direct, 'L', speed);
         } else {
-            set_slide(&Rfmot,&Lfmot,&Rbmot,&Lbmot,r1,l1, speed);
+            set_slide(&wheel_RF,&wheel_LF,&wheel_RB,&wheel_LB,r1,l1, speed);
         }
-        write_data(Rfmotadd,Rfmot);
-        write_data(Lfmotadd,Lfmot);
-        write_data(Rbmotadd,Rbmot);
-        write_data(Lbmotadd,Lbmot);
-        write_data(Lbmotadd,Lbmot);
-
 
-        pri("\033[2J\033[1;1H RX:%d",Raxis[0]);
-        pri("\033[2;1H RY:%d",Raxis[1]);
-        pri("\033[3;1H LX:%d",Laxis[0]);
-        pri("\033[4;1H LY:%d",Laxis[1]);
-        pri("\033[5;1H Rfmot:0x%x",Rfmot);
-        pri("\033[6;1H Lfmot:0x%x",Lfmot);
-        pri("\033[7;1H Rbmot:0x%x",Rbmot);
-        pri("\033[8;1H Lbmot:0x%x",Lbmot);
-        pri("\033[9;1H direct:%d",direct);
-        pri("\033[10;1H speed:%d",speed);
+        write_data(Rf_add,wheel_RF);
+        write_data(Lf_add,wheel_LF);
+        write_data(Rb_add,wheel_RB);
+        write_data(Lb_add,wheel_LB);
+        write_data(Mot1add,arm_moter);
+/*
+        pri("\033[2J");
+        pri("\033[1;1H wheel_RF:0x%x",wheel_RF);
+        pri("\033[2;1H wheel_LF:0x%x",wheel_LF);
+        pri("\033[3;1H wheel_RB:0x%x",wheel_RB);
+        pri("\033[4;1H wheel_LB:0x%x",wheel_LB);
+        pri("\033[5;1H arm_moter:0x%x",arm_moter);
+        pri("\033[6;1H direct:%d",direct);
+        pri("\033[7;1H speed:%d",speed);
+        */
+
     }
 }
\ No newline at end of file