Diff: states.cpp

Revision:

8:8ff6db217a17

Parent:

6:3cb78320d4db

--- a/states.cpp	Fri Sep 13 03:01:12 2019 +0000
+++ b/states.cpp	Fri Oct 04 08:15:43 2019 +0000
@@ -35,12 +35,8 @@
     for(int i=0; i<4; i++) stick[i] = pad.getStick(i);
     stick[0] -= 128;
     stick[1] = 128-stick[1];
-    stick[2] -= 128;
-    stick[3] = 128-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;
+    if(norm[0] < 50) norm[0] = 0;
     for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i);
     mechanism.setBrake(trigger[0]);
 
@@ -75,8 +71,8 @@
             emStop = false;
         }else{
             turnPower = anglePID.compute();
-            if(norm[0] && norm[0] < 100) mechanism.move(0,0,-turnPower);
-            else mechanism.move(stick[0], stick[1], -turnPower);
+            //if(norm[0] < 70) mechanism.move(0,0,-turnPower,nowAngle, 0);
+            /*else*/ mechanism.move(stick[0], stick[1], -turnPower, nowAngle, norm[0]);
            /*昇降機構*/    
             if(b[0]) {mechanism.up();}
             else if(b[1]) {mechanism.down();}