Diff: bot/bot.cpp

Revision:

1:845af5425eec

Child:

3:369d9ee17e84

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bot/bot.cpp	Tue Sep 05 16:11:20 2017 +0900
@@ -0,0 +1,68 @@
+#include "bot.h"
+
+Bot::Bot() :
+    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()
+{
+    suc = pad.receiveState();
+    PIDC::confirm();
+    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()
+{
+    if(suc) motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) / 2.0, PIDC::co);
+}
+
+void Bot::controllMech()
+{
+    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(2)) {
+//            motor.destroy(DESTROY_MAX_SPEED);
+//        } else {
+//            motor.destroy(0);
+//        }
+//
+//        if(!pad.getButton2(0)) motor.release();
+    }
+}
+
+
+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);
+    }
+}
\ No newline at end of file