Diff: bot/bot.cpp

Revision:

6:fe9767a50891

Parent:

5:16ea97725085

Child:

9:39be1525dfe0

--- a/bot/bot.cpp	Wed Sep 06 17:47:20 2017 +0900
+++ b/bot/bot.cpp	Wed Sep 06 18:39:19 2017 +0900
@@ -1,27 +1,23 @@
 #include "bot.h"
 
 Bot::Bot() :
-    PIDC(), pad(XBee1TX, XBee1RX, ADDR),
+    PIDC(),
+    pad(XBee1TX, XBee1RX, ADDR),
     motor(MDSDA, MDSCL, solenoidPin),
-    led({DebugLED1, DebugLED2, DebugLED3, DebugLED4})
-    ,debugSerial(USBTX, USBRX, 11520)
+    led({DebugLED1, DebugLED2, DebugLED3, DebugLED4}),
+    debugSerial(USBTX, USBRX, 11520)
 {
     motor.goXY(0, 0, 0);
     motor.moveSlider(0);
     motor.destroy(0);
     motor.swing(0);
     motor.shakeHead(0);
-
-    // led[0] = 1;
-    // led[1] = 1;
-    // led[2] = 1;
 }
 
 void Bot::confirmAll()
 {
-    suc = pad.receiveState();
-    // PIDC::confirm();
-    if(!suc) {
+    receiveSuccessed = pad.receiveState();
+    if(!receiveSuccessed) {
         motor.goXY(0, 0, 0);
         motor.moveSlider(0);
         motor.destroy(0);
@@ -32,12 +28,16 @@
 
 void Bot::controllDrive()
 {
-    if(suc) {
+    if(receiveSuccessed) {
         if(pad.getNorm(1) > 0.5) {
             PIDC::PID::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);
+        motor.goXY(
+            pad.getStick(0) / 2.0,
+            -pad.getStick(1) /2.0,
+            PIDC::calculationResult
+        );
     } else {
         motor.goXY(0, 0, 0);
     }
@@ -47,14 +47,14 @@
 {
     float moment = 0;
     static float beforestick = pad.getStick(2);
+
     if(!pad.getButton2(2)) {
-        PIDC::resetOffset2();
-        led[3] = 1;
+        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::resetOffset();
+        PIDC::resetAxisOffset();
     }
 
     if(pad.getStick(2) > 0.5 || pad.getStick(2) < -0.5) {
@@ -63,21 +63,24 @@
     }
     if(fabs(pad.getStick(2)) < 0.5) {
         PIDC::confirm();
-        moment = PIDC::co;
+        moment = PIDC::calculationResult;
     }
 
-    if(suc) {
-        motor.goCircular(pad.getNorm(0) / 2.0,pad.getRadian(0) - PIDC::initDegree2 / 10.0 * (M_PI / 180.0) + M_PI, moment);
+    if(receiveSuccessed) {
+        motor.goCircular(
+            pad.getNorm(0) / 2.0,
+            pad.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
+            moment
+        );
     } else {
         motor.goXY(0, 0, 0);
     }
     beforestick = pad.getStick(2);
-    debugSerial.printf("%d\n\r", PIDC::initDegree);
 }
 
 void Bot::controllMech()
 {
-    if(suc) {
+    if(receiveSuccessed) {
 //        if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
 //        if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
 //        if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
@@ -109,7 +112,10 @@
 
 void Bot::calibrate()
 {
-    if(suc && !pad.getButton2(0) && !pad.getButton2(1)) {
+    if(receiveSuccessed &&
+         !pad.getButton2(0) &&
+         !pad.getButton2(1)
+     ) {
         PIDC::calibration(HMC6352_ENTER_CALIB);
         motor.goXY(0, 0, 0.4);
         wait(5.0);