Diff: main.cpp

Revision:

12:c7e91c4c2ffa

Parent:

11:a2e3d11f5750

Child:

14:1d9ae3128002

--- a/main.cpp	Mon Sep 04 04:29:54 2017 +0000
+++ b/main.cpp	Mon Sep 04 05:33:42 2017 +0000
@@ -21,6 +21,7 @@
 //DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
 DigitalOut attack[2] = {PA_10,PC_4};
 DigitalOut angle[2] = {PB_3,PB_5};
+DigitalOut powerSw(powerSW);
 R1370 R1370(R1370_TX,R1370_RX);
 PID pid(KP,KI,KD,RATE); 
 ikarashiMDC wheels[] {
@@ -68,30 +69,29 @@
 int main()
 {   
     bool airFlag1=0,airFlag2=0,airStatus1=0,airStatus2=0;
-    int error_val = 0;
-    uint8_t fep_temp;
+    int error_val = 0,i = 0;
     double pwm = 0.0;
     init();
     pidInit();
     while(1) 
     {
-        if(1)//con.receiveState()==0) 
+        if((con.receiveState()==0)&&(R1370.update()==0)) 
         {
+            if(con.getButton1(6)==0) {
+                powerSw = 0;
+            } else {
+                powerSw = 1;
+            }
             error_val = 0;
-            //leds[0] = 0;
-            if(R1370.reciveState() == 0)
-            {
-                //omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0)/-2.0);
-                pid.setProcessValue(R1370.getAngle());
-                pc.printf("%lf    %lf     ",R1370.getAngle(),-1*pid.compute());
-                omni.computeXY(0,0,-1*pid.compute());
-                omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());           
-                for(int i = 0; i < 4; i++) {
-                    pc.printf("%lf,",omni.getOutput(i));
-                    wheels[i].setSpeed(omni.getOutput(i));
-                }
-                pc.printf("\n");
+            pid.setProcessValue(R1370.getAngle());
+            pc.printf("%lf    %lf     ",R1370.getAngle(),-1*pid.compute());
+            omni.computeXY(0,0,-1*pid.compute());
+            omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());           
+            for(int i = 0; i < 4; i++) {
+                pc.printf("%lf,",omni.getOutput(i));
+                wheels[i].setSpeed(omni.getOutput(i));
             }
+            pc.printf("\n");
             //昇降
             if(con.getButton1(0) == 1 && con.getButton1(1) == 0) {
                 pwm = -0.9;
@@ -139,6 +139,9 @@
                 angle[0]=0;
                 angle[1]=0;    
             }
+        }else {
+            error_val++;
+            if(error_val > 10) powerSw = 0;
         }
     }
 }