Diff: bot/bot.cpp

Revision:

3:369d9ee17e84

Parent:

1:845af5425eec

Child:

4:1073deb368df

--- a/bot/bot.cpp	Tue Sep 05 16:32:31 2017 +0900
+++ b/bot/bot.cpp	Tue Sep 05 20:51:23 2017 +0900
@@ -14,7 +14,6 @@
 {
     suc = pad.receiveState();
     PIDC::confirm();
-    if(pad.getNorm(1) > 0.5) PIDC::setSetPoint(pad.getRadian(1) * (180.0 / M_PI) - M_PI / 2.0);
     if(!suc) {
         motor.goXY(0, 0, 0);
         motor.moveSlider(0);
@@ -26,7 +25,40 @@
 
 void Bot::controllDrive()
 {
-    if(suc) motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) / 2.0, PIDC::co);
+    if(suc) {
+        if(pad.getNorm(1) > 0.5) {
+            PIDC::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
+            PIDC::confirm();
+        }
+        motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) /2.0, PIDC::co);
+    } else {
+        motor.goXY(0, 0, 0);
+    }
+}
+
+void Bot::controllDrive2()
+{
+    static float moment = 0;
+    static float beforestick = pad.getStick(2);
+
+    if((beforestick >= 0.1 && pad.getStick(2) < 0.1) || (beforestick <= -0.1 && pad.getStick(2) > -0.1)) {
+        PIDC::setSetPoint(PIDC::getDegree());
+    }
+
+    if(pad.getStick(2) > 0.1 && pad.getStick(2) < -0.1) {
+        moment = pad.getStick(2) / 2.0;
+        PIDC::confirm();
+    } else {
+        PIDC::confirm();
+        moment = PIDC::co;
+    }
+
+    if(suc) {
+        motor.goXY(pad.getStick(0) / 2.0, pad.getStick(1) / 2.0, moment);
+    } else {
+        motor.goXY(0, 0, 0);
+    }
+    beforestick = pad.getStick(2);
 }
 
 void Bot::controllMech()
@@ -51,18 +83,23 @@
 //            motor.destroy(0);
 //        }
 //
-//        if(!pad.getButton2(0)) motor.release();
-    }
+       if(!pad.getButton2(1)) motor.release();
+   } else {
+       motor.moveSlider(0);
+       motor.destroy(0);
+       motor.swing(0);
+       motor.shakeHead(0);
+   }
 }
 
 
 void Bot::calibrate()
 {
-    if(suc && !pad.getButton2(0) && !pad.getButton2(1) && !pad.getButton2(2) && !pad.getButton2(3)) {
+    if(suc && !pad.getButton2(0) && !pad.getButton2(1)) {
         PIDC::calibration(HMC6352_ENTER_CALIB);
-        motor.goXY(0, 0, 0.3);
-        wait(2.0);
+        motor.goXY(0, 0, 0.4);
+        wait(5.0);
         motor.goXY(0, 0, 0);
         PIDC::calibration(HMC6352_EXIT_CALIB);
     }
-}
\ No newline at end of file
+}