Diff: bot/bot.cpp

Branch:

develop1

Revision:

15:9aa11febe517

Parent:

14:1fadf7d2f583

Child:

16:50651ff960b9

--- a/bot/bot.cpp	Tue Sep 12 10:21:00 2017 +0900
+++ b/bot/bot.cpp	Sun Sep 17 00:53:01 2017 +0900
@@ -2,10 +2,10 @@
 
 Bot::Bot() :
     PIDC(),
-    pad(XBee1TX, XBee1RX, ADDR),
+    pad(XBee2TX, XBee2RX, ADDR),
     RS485(MDTX, MDRX, 38400),
     RS485Controller(PWM1),
-    powerSwitch(PWM2),
+    powerSwitch(MDstop),
     quadOmni(&RS485Controller, &RS485),
     slider(&RS485Controller, &RS485),
     armMotor({
@@ -23,7 +23,7 @@
 
     quadOmni.moveXY(0, 0, 0);
 
-    powerSwitch = 1;
+    powerSwitch = true;
     for(int i = 0; i < 3; i++) {
         led[i] = true;
         wait(0.1);
@@ -34,6 +34,7 @@
 void Bot::confirmAll()
 {
     receiveSuccessed = pad.receiveState();
+if(!pad.getButton2(3)) powerSwitch = 0;
     led[0] = receiveSuccessed;
     if(!receiveSuccessed) {
         quadOmni.moveXY(0, 0, 0);
@@ -49,13 +50,13 @@
     if(receiveSuccessed) {
         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();
+            // PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
+            // PIDC::confirm();
         }
         quadOmni.moveXY(
-            pad.getStick(0) / 2.0,
-            -pad.getStick(1) /2.0,
-            PIDC::calculationResult
+            pad.getStick(0),
+            -pad.getStick(1),
+            - pad.getStick(2) / 3.0 // PIDC::calculationResult
         );
     } else {
         quadOmni.moveXY(0, 0, 0);
@@ -113,6 +114,15 @@
         if(!pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.6);
         if(pad.getButton1(5) && pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.0);
 
+        if(!pad.getButton1(3)) slider.slide(1.0);
+        if(!pad.getButton1(4)) slider.slide(-1.0);
+        if(pad.getButton1(3) && pad.getButton1(4)) slider.slide(0.0);
+
+        if(!pad.getButton2(4)) {
+            armMotor[DESTROY].setSpeed(-1.0);
+        } else {
+            armMotor[DESTROY].setSpeed(0.0);
+        }
 //
 //        if(!pad.getButton1(2)) {
 //            motor.destroy(DESTROY_MAX_SPEED);
@@ -120,8 +130,6 @@
 //            motor.destroy(0);
 //        }
 //
-       if(!pad.getButton2(1)) slider.release();
-       if(!pad.getButton2(3)) powerSwitch = 0;
    } else {
        slider.slide(0);
        for(int i = 0; i < 3; i++) {