Diff: states.cpp

Revision:

5:3273db2411d8

Parent:

4:eafd5a0c3351

Child:

7:9d5746931f80

--- a/states.cpp	Mon Sep 09 04:48:16 2019 +0000
+++ b/states.cpp	Tue Sep 10 01:49:31 2019 +0000
@@ -38,16 +38,16 @@
     for(int i=0; i<4; i++) stick[i] = pad.getStick(i);
     stick[1] = 255-stick[1];
     stick[3] = 255-stick[3];
-    norm[0] = hypot(stick[0],stick[1]);
-    norm[1] = hypot(stick[2],stick[3]);
+    norm[0] = hypotf(stick[0],stick[1]);
+    norm[1] = hypotf(stick[2],stick[3]);
     if(norm[0] > 20) norm[0] = 0;
     if(norm[1] > 20) norm[1] = 0;
     for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i);
     omni.setBrake(trigger[0]);
     nano.sendTrigger(trigger[0]);
     mechanism.getTrigger(trigger[1]);
-    rad[0] = atan2(stick[1],stick[0]) + (nowAngle * (PI / 180));
-    rad[1] = atan2(stick[2],stick[3]);
+    rad[0] = atan2f(stick[1],stick[0]) + (nowAngle * (PI / 180));
+    rad[1] = atan2f(stick[2],stick[3]);
     if(b3[4] == 1) state++;
     if(b3[5] == 1) state--;
     /*
@@ -63,21 +63,6 @@
         case 3: idealAngle = left; break;
         default: idealAngle = front; break;
     }
-   /*昇降機構*/    
-    if(b[0] && check == 1) mechanism.up();
-    else if(b[1] && check == 1) mechanism.down();
-    /*回収機構*/
-    else if(b[6] && check == 1) mechanism.collect();
-    /*つり上げ機構*/
-    else if(b[7] && check == 1) mechanism.lift();
-    /*装填機構*/
-    else if(b[2] && check == 1) mechanism.loadRight();
-    else if(b[3]) mechanism.loadLeft();
-    /*かけ機構*/
-    else if(b[9] && check == 1) mechanism.hookRight();
-    else if(b[8] && check == 1) mechanism.hookLeft();
-    
-    else nano.sendMove(0x0f);
 }
 
 void States::move()
@@ -96,5 +81,20 @@
         turnPower = anglePID.compute();
         if(norm[0] && norm[0] < 200) omni.stop();
         else omni.move(stick[0], stick[1], -turnPower);
+       /*昇降機構*/    
+        if(b[0] && check == 1) mechanism.up();
+        else if(b[1] && check == 1) mechanism.down();
+        /*回収機構*/
+        else if(b[6] && check == 1) mechanism.collect();
+        /*つり上げ機構*/
+        else if(b[7] && check == 1) mechanism.lift();
+        /*装填機構*/
+        else if(b[2] && check == 1) mechanism.loadRight();
+        else if(b[3]) mechanism.loadLeft();
+        /*かけ機構*/
+        else if(b[9] && check == 1) mechanism.hookRight();
+        else if(b[8] && check == 1) mechanism.hookLeft();
+        
+        else nano.sendMove(0x0f);
     }
 }
\ No newline at end of file