Diff: main.cpp

Revision:

3:4cd170cdf049

Parent:

2:b9297c2b2632

Child:

4:f1d2000776c6

--- a/main.cpp	Thu Jul 13 12:02:06 2017 +0000
+++ b/main.cpp	Thu Jul 13 12:46:41 2017 +0000
@@ -4,12 +4,24 @@
 #include "pin_config.h"
 #include "FEP.h"
 
+//#define DEBUG 
+
 MDC motor[4];
 Omni omni(4, 45);
-FEP fep(PA_0,PA_1,115200);
+FEP fep(PA_0,PA_1);
 DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
 PwmOut pwm(PA_6);
 
+void init()
+{
+    for(int i = 0;i<4;i++) {
+        leds[i] = 0;
+    }
+    for(int j = 0; j < 4; j++) {
+        motor[j].setAddr(7, j);
+    }
+}
+
 int main()
 {
     char data[10] = { 0 };
@@ -20,16 +32,15 @@
     double radian;
     double deg = 360;
     pwm = 0.5;
-    for(i = 0;i<4;i++) {
-        leds[i] = 0;
-    }
-    for(int j = 0; j < 4; j++) {
-        motor[j].setAddr(7, j);
-    }
+    
     while(1) {
         fep_temp=fep.read_txt(data,6);
         if(fep_temp==FEP_RX_SUCCESS) {
-//            pc.printf("Recieve succeeded,%s\r\n",data);
+
+#ifdef DEBUG
+            pc.printf("Recieve succeeded,%s\r\n",data);
+#endif
+
             error_val = 0;
             leds[0] = 0;
             tem[0] = data[4];
@@ -37,20 +48,36 @@
             for(i = 0; i < 7; i++) {
                 Button1[i] = tem[0] % 2;
                 tem[0] /= 2;
-//                pc.printf("%d,",Button1[i]);
+
+#ifdef DEBUG
+                pc.printf("%d,",Button1[i]);
+#endif
+
             }
             for(i = 0; i < 6; i++) {
                 Button2[i] = tem[1] % 2;
                 tem[1] /= 2;
-//                pc.printf("%d,",Button2[i]);
+
+#ifdef DEBUG
+                pc.printf("%d,",Button2[i]);
+#endif
             }
-//            pc.printf("\r\n");
+
+#ifdef DEBUG
+            pc.printf("\r\n");
+#endif 
         } else if(fep_temp==FEP_NO_RESPONSE) {
-//            pc.printf("No response\r\n");
+
+#ifdef DEBUG
+            pc.printf("No response\r\n");
+#endif
             leds[0] = 1;
             continue;
         } else {
-//            pc.printf("Recieve failed\r\n");
+
+#ifdef DEBUG
+            pc.printf("Recieve failed\r\n");
+#endif
             leds[0] = 1;
             error_val++;
         }