Diff: bot/bot.cpp

Revision:

22:682cc376da6f

Parent:

21:9c1061982b16

Child:

23:37bb9afe9fdc

--- a/bot/bot.cpp	Tue Sep 05 14:07:18 2017 +0900
+++ b/bot/bot.cpp	Thu Sep 07 06:26:53 2017 +0000
@@ -1,34 +1,107 @@
 #include "bot.h"
 
 Bot::Bot() :
-PIDC(), pad1(XBee1TX, XBee1RX, ADDR1), pad2(XBee2TX, XBee2RX, ADDR2), motor(MDSDA, MDSCL)
+    PIDC(),
+    pad1(XBee1TX, XBee1RX, ADDR1),
+    /* pad2(XBee2TX, XBee2RX, ADDR2),*/
+    motor(MDSDA, MDSCL)
 {
+  motor.goXY(0, 0, 0);
+  motor.moveRightTentacle(0);
+  motor.moveLeftTentacle(0);
+  motor.windUp(0);
 }
 
 void Bot::confirmAll()
 {
-    pad1.receiveState();
-    pad2.receiveState();
-    PIDC::confirm();
-    if(pad1.getNorm(1) > 0.5) PIDC::setSetPoint(pad1.getRadian(1) * (180.0 / M_PI));
+    receiveSuccessed = pad1.receiveState();
+    if(!receiveSuccessed) {
+        motor.goXY(0, 0, 0);
+        motor.moveLeftTentacle(0);
+        motor.moveRightTentacle(0);
+        motor.windUp(0);
+    }
+    //pad2.receiveState();
+    //PIDC::confirm();
+    //if(pad1.getNorm(1) > 0.5) PIDC::setSetPoint(pad1.getRadian(1) * (180.0 / M_PI));
 }
 
 void Bot::controllDrive()
 {
-    motor.goXY(pad1.getStick(0),pad1.getStick(1), PIDC::co);
+    //motor.goXY(pad1.getStick(0),pad1.getStick(1), PIDC::co);
+    if(receiveSuccessed) {
+        if(pad1.getNorm(1) > 0.5) {
+            PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
+            PIDC::confirm();
+        }
+        motor.goXY(
+            pad1.getStick(0) / 2.0,
+            -pad1.getStick(1) /2.0,
+            PIDC::calculationResult
+        );
+    } else {
+        motor.goXY(0, 0, 0);
+    }
+}
+
+void Bot::controllDrive2()
+{
+    float moment = 0;
+    static float beforestick = pad1.getStick(2);
+
+    if(!pad1.getButton2(2)) {
+        PIDC::resetPlaneOffset();
+    }
+
+    if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) {
+        PIDC::PID::setSetPoint(0.0);
+        PIDC::resetAxisOffset();
+    }
+
+    if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) {
+        moment = pad1.getStick(2) / 4.0;
+        PIDC::confirm();
+    }
+    if(fabs(pad1.getStick(2)) < 0.5) {
+        PIDC::confirm();
+        moment = PIDC::calculationResult;
+    }
+
+    if(receiveSuccessed) {
+        motor.goCircular(
+            pad1.getNorm(0) / 2.0,
+            pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
+            moment
+        );
+    } else {
+        motor.goXY(0, 0, 0);
+    }
+    beforestick = pad1.getStick(2);
 }
 
 void Bot::controllMech()
 {
-    if(!pad1.getButton1(0)) motor.moveRightTentacle(TENTACLE_MAX_SPEED);
-    if(!pad1.getButton1(1)) motor.moveRightTentacle(-TENTACLE_MAX_SPEED);
-    if(pad1.getButton1(0) && pad1.getButton1(1)) motor.moveRightTentacle(0);
+    if(receiveSuccessed){
+      if(!pad1.getButton1(0)) motor.moveRightTentacle(TENTACLE_MAX_SPEED);
+      if(!pad1.getButton1(1)) motor.moveRightTentacle(-TENTACLE_MAX_SPEED);
+      if(pad1.getButton1(0) && pad1.getButton1(1)) motor.moveRightTentacle(0);
+
+      if(!pad1.getButton1(2)) motor.moveLeftTentacle(TENTACLE_MAX_SPEED);
+      if(!pad1.getButton1(3)) motor.moveLeftTentacle(-TENTACLE_MAX_SPEED);
+      if(pad1.getButton1(2) && pad1.getButton1(3)) motor.moveLeftTentacle(0);
 
-    if(!pad1.getButton1(2)) motor.moveLeftTentacle(TENTACLE_MAX_SPEED);
-    if(!pad1.getButton1(3)) motor.moveLeftTentacle(-TENTACLE_MAX_SPEED);
-    if(pad1.getButton1(2) && pad1.getButton1(3)) motor.moveLeftTentacle(0);
+      if(!pad1.getButton1(4)) motor.windUp(WIND_UP_SPEED);
+      if(!pad1.getButton1(5)) motor.windUp(-WIND_UP_SPEED);
+      if(pad1.getButton1(4) && pad1.getButton1(5)) motor.windUp(0);
 
-
-
+    } else {
+      motor.moveLeftTentacle(0);
+      motor.moveRightTentacle(0);
+      motor.windUp(0);
+    }
 
 }
+
+void Bot::calibrate(){
+
+}