Diff: bot/bot.cpp

Revision:

4:1073deb368df

Parent:

3:369d9ee17e84

Child:

5:16ea97725085

--- a/bot/bot.cpp	Tue Sep 05 20:51:23 2017 +0900
+++ b/bot/bot.cpp	Wed Sep 06 00:01:47 2017 +0900
@@ -1,7 +1,8 @@
 #include "bot.h"
 
 Bot::Bot() :
-    PIDC(), pad(XBee1TX, XBee1RX, ADDR), motor(MDSDA, MDSCL, solenoidPin)
+    PIDC(), pad(XBee1TX, XBee1RX, ADDR),
+    motor(MDSDA, MDSCL, solenoidPin)
 {
     motor.goXY(0, 0, 0);
     motor.moveSlider(0);
@@ -38,23 +39,24 @@
 
 void Bot::controllDrive2()
 {
-    static float moment = 0;
+    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((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
+        PIDC::setSetPoint(PIDC::getDegree() / 10.0);
     }
 
-    if(pad.getStick(2) > 0.1 && pad.getStick(2) < -0.1) {
+    if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
         moment = pad.getStick(2) / 2.0;
         PIDC::confirm();
-    } else {
+    }
+    if(fabs(pad.getStick(2)) < 0.5) {
         PIDC::confirm();
         moment = PIDC::co;
     }
 
     if(suc) {
-        motor.goXY(pad.getStick(0) / 2.0, pad.getStick(1) / 2.0, moment);
+        motor.goCircular(pad.getNorm(0),pad.getRadian(0) - PIDC::getDegree() / 10.0 * (M_PI / 180.0) + M_PI, moment);
     } else {
         motor.goXY(0, 0, 0);
     }