Diff: main.cpp

Revision:

4:f1d2000776c6

Parent:

3:4cd170cdf049

Child:

5:dee9310ec990

--- a/main.cpp	Thu Jul 13 12:46:41 2017 +0000
+++ b/main.cpp	Fri Jul 14 11:17:24 2017 +0000
@@ -6,9 +6,10 @@
 
 //#define DEBUG 
 
-MDC motor[4];
-Omni omni(4, 45);
-FEP fep(PA_0,PA_1);
+//I2C i2c(i2c_sda,i2c_scl);
+MDC motor(i2c_sda,i2c_scl);
+Omni omni(4, 135);
+FEP fep(PB_10,PB_11);
 DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
 PwmOut pwm(PA_6);
 
@@ -17,13 +18,14 @@
     for(int i = 0;i<4;i++) {
         leds[i] = 0;
     }
-    for(int j = 0; j < 4; j++) {
-        motor[j].setAddr(7, j);
-    }
+//    for(int j = 0; j < 4; j++) {
+//        motor[j].setAddr(7, j);
+//    }
 }
 
 int main()
 {
+    pwm.period(0.00005);
     char data[10] = { 0 };
     int  i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 };
     uint8_t fep_temp;
@@ -32,6 +34,9 @@
     double radian;
     double deg = 360;
     pwm = 0.5;
+    for(int j = 0; j < 4; j++) {
+        motor.write(7,j,0);
+    }
     
     while(1) {
         fep_temp=fep.read_txt(data,6);
@@ -82,35 +87,67 @@
             error_val++;
         }
         if(error_val < 4) {
+            /*
             if (data[0]>100 && data[0]<150 && data[1]>100 && data[1]<150) {
                 omni.stop();
-                leds[2] = 1;
+                polarVector[0] = 360;
+                polarVector[1] = 0;
+                leds[1] = 1;
             } else {
-                leds[2] = 0;
+                leds[1] = 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;
-                
+            }
+            */
+            polarVector[1] = 0.3;
+            if(Button1[2] == 0 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 1) {
+                deg = 270;
+                leds[1] = 0;
+            }else if(Button1[2] == 1 && Button1[3] == 0 && Button1[4] == 1 && Button1[5] == 1) {
+                deg = 180;
+                leds[1] = 0;
+            }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 0 && Button1[5] == 1) {
+                deg = 90;
+                leds[1] = 0;
+            }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 0) {
+                deg = 0;
+                leds[1] = 0;
+            }else {
+                deg = 360;
+                polarVector[1] = 0;
+                leds[1] = 1;
+            }
+            polarVector[0] = deg;
+            if(data[2] < 50) {
+                omni.computePolar(polarVector, 0.15);
+                leds[2] = 1;
+                leds[3] = 0;
+            } else if(data[2] > 200) {
+                omni.computePolar(polarVector, -0.15);
+                leds[2] = 0;
+                leds[3] = 1;
+            } else {
                 omni.computePolar(polarVector, 0);
+                leds[2] = 0;
+                leds[3] = 0;
             }
             
             for(int j = 0; j < 4; j++) {
-                motor[j].write(omni.getOutput(j));
+                motor.write(7,j,omni.getOutput(j));
             }
             if(Button2[0] == 1 && Button2[1] == 0) {
-                pwm = 0.2;
+                pwm = 0.3;
             } else if(Button2[0] == 0 && Button2[1] == 1) {
-                pwm = 0.8;
+                pwm = 0.7;
             } else {
                 pwm = 0.5;
             }
         } else {
             omni.stop();
             for(int j = 0; j < 4; j++) {
-                motor[j].write(omni.getOutput(j));
+                motor.write(7,j,omni.getOutput(j));
             }
             pwm = 0.5;
         }