Diff: bot/bot.cpp

Revision:

9:39be1525dfe0

Parent:

6:fe9767a50891

Child:

10:99dc4ae08998

--- a/bot/bot.cpp	Thu Sep 07 11:45:33 2017 +0900
+++ b/bot/bot.cpp	Thu Sep 07 17:37:41 2017 +0900
@@ -3,26 +3,45 @@
 Bot::Bot() :
     PIDC(),
     pad(XBee1TX, XBee1RX, ADDR),
-    motor(MDSDA, MDSCL, solenoidPin),
-    led({DebugLED1, DebugLED2, DebugLED3, DebugLED4}),
-    debugSerial(USBTX, USBRX, 11520)
+    RS485(MDTX, MDRX, 38400),
+    RS485Controller(PWM1),
+    powerSwitch(PWM2),
+    quadOmni(&RS485Controller, &RS485),
+    slider(&RS485Controller, &RS485),
+    armMotor({
+        {&RS485Controller, 1, 1, SM, &RS485},
+        {&RS485Controller, 1, 2, SM, &RS485},
+        {&RS485Controller, 1, 3, SM, &RS485}
+    }),
+    receiveSuccessed(0),
+    led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
+        DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
+    debugSerial(USBTX, USBRX, 115200)
 {
-    motor.goXY(0, 0, 0);
-    motor.moveSlider(0);
-    motor.destroy(0);
-    motor.swing(0);
-    motor.shakeHead(0);
+    for(int i = 0; i < 3; i++) {
+        armMotor[i].braking = true;
+    }
+
+    quadOmni.moveXY(0, 0, 0);
+
+    powerSwitch = 1;
+
+    for(int i = 0; i < 8; i++) {
+        led[i] = 1;
+        wait(0.1);
+        led[i] = 0;
+    }
 }
 
 void Bot::confirmAll()
 {
     receiveSuccessed = pad.receiveState();
     if(!receiveSuccessed) {
-        motor.goXY(0, 0, 0);
-        motor.moveSlider(0);
-        motor.destroy(0);
-        motor.swing(0);
-        motor.shakeHead(0);
+        quadOmni.moveXY(0, 0, 0);
+        slider.slide(0);
+        for(int i = 0; i < 3; i++) {
+            armMotor[i].setSpeed(0);
+        }
     }
 }
 
@@ -33,13 +52,13 @@
             PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
             PIDC::confirm();
         }
-        motor.goXY(
+        quadOmni.moveXY(
             pad.getStick(0) / 2.0,
             -pad.getStick(1) /2.0,
             PIDC::calculationResult
         );
     } else {
-        motor.goXY(0, 0, 0);
+        quadOmni.moveXY(0, 0, 0);
     }
 }
 
@@ -67,13 +86,13 @@
     }
 
     if(receiveSuccessed) {
-        motor.goCircular(
+        quadOmni.moveCircular(
             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);
+        quadOmni.moveXY(0, 0, 0);
     }
     beforestick = pad.getStick(2);
 }
@@ -89,9 +108,9 @@
 //        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.6);
-        if(!pad.getButton1(6)) motor.swing(-0.6);
-        if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
+        if(!pad.getButton1(5)) armMotor[SWORD].setSpeed(0.6);
+        if(!pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.6);
+        if(pad.getButton1(5) && pad.getButton1(6)) armMotor[SWORD].setSpeed(-0.0);
 
 //
 //        if(!pad.getButton1(2)) {
@@ -100,12 +119,13 @@
 //            motor.destroy(0);
 //        }
 //
-       if(!pad.getButton2(1)) motor.release();
+       if(!pad.getButton2(1)) slider.release();
+       if(!pad.getButton2(3)) powerSwitch = 0;
    } else {
-       motor.moveSlider(0);
-       motor.destroy(0);
-       motor.swing(0);
-       motor.shakeHead(0);
+       slider.slide(0);
+       for(int i = 0; i < 3; i++) {
+           armMotor[i].setSpeed(0);
+       }
    }
 }
 
@@ -117,9 +137,9 @@
          !pad.getButton2(1)
      ) {
         PIDC::calibration(HMC6352_ENTER_CALIB);
-        motor.goXY(0, 0, 0.4);
+        quadOmni.moveXY(0, 0, 0.4);
         wait(5.0);
-        motor.goXY(0, 0, 0);
+        quadOmni.moveXY(0, 0, 0);
         PIDC::calibration(HMC6352_EXIT_CALIB);
     }
 }