Diff: bot/bot.cpp

Branch:

develop1

Revision:

23:797d25f3df5e

Parent:

19:34da005ea4ea

--- a/bot/bot.cpp	Thu Sep 28 20:37:09 2017 +0900
+++ b/bot/bot.cpp	Sun Oct 01 11:43:53 2017 +0900
@@ -1,7 +1,7 @@
 #include "bot.h"
 
 Bot::Bot() :
-    PIDC(),
+    pidc(),
     pad(XBee2TX, XBee2RX, ADDR),
     RS485(MDTX, MDRX, 38400),
     RS485Controller(PWM1),
@@ -49,64 +49,9 @@
 
 void Bot::controllDrive()
 {
-    if(receiveSuccessed) {
-        debugSerial.printf("%d\n\r", PIDC::getRawDegree());
-        led[1] = !led[1];
-        if(pad.getNorm(1) > 0.5) {
-            PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
-            PIDC::confirm();
-        }
-        quadOmni.moveXY(
-            pad.getStick(0),
-            -pad.getStick(1),
-            - pad.getStick(2) / 3.0 // PIDC::calculationResult
-        );
-    } else {
-        quadOmni.moveXY(0, 0, 0);
-    }
-}
-
-void Bot::controllDrive2()
-{
-    float moment = 0;
-    static float beforestick = pad.getStick(2);
-
-    if(!pad.getButton2(2)) {
-        PIDC::resetPlaneOffset();
-    }
-
-    if((beforestick >= 0.5 && pad.getStick(2) < 0.5) || (beforestick <= -0.5 && pad.getStick(2) > -0.5)) {
-        PIDC::PID::setSetPoint(0.0);
-        PIDC::resetAxisOffset();
-    }
-
-    if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
-        moment = pad.getStick(2) / 4.0;
-        PIDC::confirm();
-    }
-    if(fabs(pad.getStick(2)) < 0.5) {
-        PIDC::confirm();
-        moment = PIDC::calculationResult;
-    }
-
-    if(receiveSuccessed) {
-        led[1] = !led[1];
-        quadOmni.moveCircular(
-            pad.getNorm(0) / 2.0,
-            pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
-            moment
-        );
-    } else {
-        quadOmni.moveXY(0, 0, 0);
-    }
-    beforestick = pad.getStick(2);
-}
-
-void Bot::controllDrive3()
-{
     static int mode = 1;
     if(receiveSuccessed) {
-        debugSerial.printf("%d\n\r", PIDC::getCurrentDegree());
+        debugSerial.printf("%d\n\r", pidc.getCurrentDegree());
 
         if(!pad.getButton2(4)) {
             mode = 1;
@@ -117,23 +62,23 @@
         }
 
         if(mode == 1) {
-            PIDC::PID::setSetPoint(0.0);
-            PIDC::confirm();
+            pidc.setPoint(0.0);
+            pidc.confirm();
 
             quadOmni.moveXY(
                 pad.getStick(0),
                 -pad.getStick(1),
-                -PIDC::calculationResult
+                -pidc.getCalculationResult()
             );
         }
         if(mode == 2) {
-            PIDC::PID::setSetPoint(90.0);
-            PIDC::confirm();
+            pidc.setPoint(90.0);
+            pidc.confirm();
 
             quadOmni.moveXY(
                 -pad.getStick(1),
                 -pad.getStick(0),
-                -PIDC::calculationResult
+                -pidc.getCalculationResult()
             );
         }
     } else {
@@ -173,7 +118,7 @@
      ) {
         t.start();
         t.reset();
-        PIDC::calibration(HMC6352_ENTER_CALIB);
+        pidc.calibration(HMC6352_ENTER_CALIB);
         while(t.read() < 5.0) {
             quadOmni.moveXY(0, 0, 0.4);
             slider.slide(0);
@@ -183,6 +128,6 @@
         }
         t.stop();
         quadOmni.moveXY(0, 0, 0);
-        PIDC::calibration(HMC6352_EXIT_CALIB);
+        pidc.calibration(HMC6352_EXIT_CALIB);
     }
 }