Diff: bot/bot.cpp

Revision:

19:3a62cbc6fee9

Parent:

17:6f52f4b1086a

--- a/bot/bot.cpp	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/bot.cpp	Mon Sep 04 01:01:13 2017 +0000
@@ -1,41 +1,68 @@
 #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);
+    motor.destroy(0);
+    motor.swing(0);
+    motor.shakeHead(0);
 }
 
 void Bot::confirmAll()
 {
-    pad.receiveState();
+    suc = pad.receiveState();
     PIDC::confirm();
-    if(pad.getNorm(1) > 0.5) PIDC::setSetPoint(pad.getRadian(1) * (180.0 / M_PI));
+    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);
+        motor.destroy(0);
+        motor.swing(0);
+        motor.shakeHead(0);
+    }
 }
 
 void Bot::controllDrive()
 {
-    motor.goXY(pad.getStick(0),pad.getStick(1), PIDC::co);
+    if(suc) motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) / 2.0, PIDC::co);
 }
 
 void Bot::controllMech()
 {
-    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);
-
-    if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
-    if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
-    if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
+    if(suc) {
+//        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);
+//
+//        if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
+//        if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
+//        if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
+//
+        if(!pad.getButton1(5)) motor.swing(0.2);
+        if(!pad.getButton1(6)) motor.swing(-0.2);
+        if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
 
-    if(!pad.getButton1(5)) motor.swing(ARM_MAX_SPEED);
-    if(!pad.getButton1(6)) motor.swing(-ARM_MAX_SPEED);
-    if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
+//
+//        if(!pad.getButton1(2)) {
+//            motor.destroy(DESTROY_MAX_SPEED);
+//        } else {
+//            motor.destroy(0);
+//        }
+//
+//        if(!pad.getButton2(0)) motor.release();
+    }
+}
 
-    if(!pad.getButton1(2)) {
-        motor.destroy(DESTROY_MAX_SPEED);
-    } else {
-        motor.destroy(0);
+
+void Bot::calibrate()
+{
+    if(suc && !pad.getButton2(0) && !pad.getButton2(1) && !pad.getButton2(2) && !pad.getButton2(3)) {
+        PIDC::calibration(HMC6352_ENTER_CALIB);
+        motor.goXY(0, 0, 0.3);
+        wait(2.0);
+        motor.goXY(0, 0, 0);
+        PIDC::calibration(HMC6352_EXIT_CALIB);
     }
-
-    if(!pad.getButton2(0)) motor.release();
-}
+}
\ No newline at end of file