Diff: main.cpp

Revision:

1:ba8cdae2652a

Parent:

0:6c83a0871cc3

Child:

2:b9297c2b2632

--- a/main.cpp	Mon Jul 03 14:24:30 2017 +0000
+++ b/main.cpp	Thu Jul 13 11:33:01 2017 +0000
@@ -2,21 +2,90 @@
 #include "omni.h"
 #include "MotorDriverController.h"
 #include "pin_config.h"
+#include "FEP.h"
 
 MDC motor(i2c_sda, i2c_scl);
 Omni omni(4, 45);
+FEP fep(PB_10,PB_11);
+DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
+PwmOut pwm(PA_6);
 
 int main()
 {
+    char data[10] = { 0 };
+    int  i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 };
+    uint8_t fep_temp;
     double polarVector[2];
+    double Xstick,Ystick;
+    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.write(7, j, 0);
+    }
     while(1) {
-        for(int i = 0; i < 360; i++) {
-            polarVector[0] = i;
-            polarVector[1] = 1.0;
-            omni.computePolar(polarVector, 0);
+        fep_temp=fep.read_txt(data,6);
+        if(fep_temp==FEP_RX_SUCCESS) {
+//            pc.printf("Recieve succeeded,%s\r\n",data);
+            error_val = 0;
+            leds[0] = 0;
+            tem[0] = data[4];
+            tem[1] = data[5];
+            for(i = 0; i < 7; i++) {
+                Button1[i] = tem[0] % 2;
+                tem[0] /= 2;
+//                pc.printf("%d,",Button1[i]);
+            }
+            for(i = 0; i < 6; i++) {
+                Button2[i] = tem[1] % 2;
+                tem[1] /= 2;
+//                pc.printf("%d,",Button2[i]);
+            }
+//            pc.printf("\r\n");
+        } else if(fep_temp==FEP_NO_RESPONSE) {
+//            pc.printf("No response\r\n");
+            leds[0] = 1;
+            continue;
+        } else {
+//            pc.printf("Recieve failed\r\n");
+            leds[0] = 1;
+            error_val++;
+        }
+        if(error_val < 4) {
+            if (data[0]>100 && data[0]<150 && data[1]>100 && data[1]<150) {
+                omni.stop();
+                leds[2] = 1;
+            } else {
+                leds[2] = 0;
+                Xstick = (double)(data[0]/255.0)*2.0-1.0;
+                Ystick = (double)(data[1]/255.0)*2.0-1.0;
+                radian = atan2(-(Ystick),-(Xstick))+PI/2.0;
+                deg = (radian*180/PI-20);
+                polarVector[0] = deg;
+                polarVector[1] = 0.5;
+                
+                omni.computePolar(polarVector, 0);
+            }
+            
             for(int j = 0; j < 4; j++) {
                 motor.write(7, j, omni.getOutput(j));
             }
+            if(Button2[0] == 1 && Button2[1] == 0) {
+                pwm = 0.2;
+            } else if(Button2[0] == 0 && Button2[1] == 1) {
+                pwm = 0.8;
+            } else {
+                pwm = 0.5;
+            }
+        } else {
+            omni.stop();
+            for(int j = 0; j < 4; j++) {
+                motor.write(7, j, omni.getOutput(j));
+            }
+            pwm = 0.5;
         }
     }
 }