Diff: bot/bot.cpp

Revision:

23:37bb9afe9fdc

Parent:

22:682cc376da6f

Child:

24:9a4ca5442717

--- a/bot/bot.cpp	Thu Sep 07 06:26:53 2017 +0000
+++ b/bot/bot.cpp	Wed Sep 13 14:26:47 2017 +0900
@@ -3,44 +3,63 @@
 Bot::Bot() :
     PIDC(),
     pad1(XBee1TX, XBee1RX, ADDR1),
-    /* pad2(XBee2TX, XBee2RX, ADDR2),*/
-    motor(MDSDA, MDSCL)
+    //pad2(XBee2TX, XBee2RX, ADDR2),
+    RS485(MDTX, MDRX, 38400),
+    RS485Controller(PWM1),
+    powerSwitch(MDstop),
+    quadOmni(&RS485Controller, &RS485),
+    tentacle(&RS485Controller, &RS485),
+    nishijo(&RS485Controller, &RS485),
+    nishijoSword(&RS485Controller, &RS485),
+    receiveSuccessed1(0),
+    //receiveSuccessed2(0),
+    led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
+        DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
+    debugSerial(USBTX, USBRX, 115200)
 {
-  motor.goXY(0, 0, 0);
-  motor.moveRightTentacle(0);
-  motor.moveLeftTentacle(0);
-  motor.windUp(0);
+
+
+    quadOmni.moveXY(0, 0, 0);
+
+    powerSwitch = 1;
+
+    for(int i = 0; i < 8; i++) {
+        led[i] = 1;
+        wait(0.1);
+        led[i] = 0;
+    }
+    for(int i = 0; i < 8; i++) {
+        led[i] = 1;
+        wait(0.1);
+        led[i] = 0;
+    }
 }
 
 void Bot::confirmAll()
 {
-    receiveSuccessed = pad1.receiveState();
-    if(!receiveSuccessed) {
-        motor.goXY(0, 0, 0);
-        motor.moveLeftTentacle(0);
-        motor.moveRightTentacle(0);
-        motor.windUp(0);
+    receiveSuccessed1 = pad1.receiveState();
+    //receiveSuccessed2 = pad2.receiveState();
+    if(!receiveSuccessed1) {
+        quadOmni.moveXY(0, 0, 0);
+        tentacle.stop();
     }
-    //pad2.receiveState();
-    //PIDC::confirm();
-    //if(pad1.getNorm(1) > 0.5) PIDC::setSetPoint(pad1.getRadian(1) * (180.0 / M_PI));
 }
 
 void Bot::controllDrive()
 {
-    //motor.goXY(pad1.getStick(0),pad1.getStick(1), PIDC::co);
-    if(receiveSuccessed) {
+    if(receiveSuccessed1) {
         if(pad1.getNorm(1) > 0.5) {
             PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
             PIDC::confirm();
         }
-        motor.goXY(
+        PIDC::renewAngle();
+        quadOmni.moveXY(
             pad1.getStick(0) / 2.0,
             -pad1.getStick(1) /2.0,
             PIDC::calculationResult
         );
     } else {
-        motor.goXY(0, 0, 0);
+        quadOmni.moveXY(0, 0, 0);
     }
 }
 
@@ -67,41 +86,51 @@
         moment = PIDC::calculationResult;
     }
 
-    if(receiveSuccessed) {
-        motor.goCircular(
+    if(receiveSuccessed1) {
+        quadOmni.moveCircular(
             pad1.getNorm(0) / 2.0,
             pad1.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 = pad1.getStick(2);
 }
 
 void Bot::controllMech()
 {
-    if(receiveSuccessed){
-      if(!pad1.getButton1(0)) motor.moveRightTentacle(TENTACLE_MAX_SPEED);
-      if(!pad1.getButton1(1)) motor.moveRightTentacle(-TENTACLE_MAX_SPEED);
-      if(pad1.getButton1(0) && pad1.getButton1(1)) motor.moveRightTentacle(0);
+    if(receiveSuccessed1) {
+        if(!pad1.getButton2(2)) tentacle.rightMove(TENTACLE_SPEED);
+        if(!pad1.getButton2(3)) tentacle.rightMove(-TENTACLE_SPEED);
+        if(pad1.getButton2(2) && pad1.getButton2(3))  tentacle.rightMove(0);
 
-      if(!pad1.getButton1(2)) motor.moveLeftTentacle(TENTACLE_MAX_SPEED);
-      if(!pad1.getButton1(3)) motor.moveLeftTentacle(-TENTACLE_MAX_SPEED);
-      if(pad1.getButton1(2) && pad1.getButton1(3)) motor.moveLeftTentacle(0);
+        if(!pad1.getButton2(0)) tentacle.leftMove(TENTACLE_SPEED);
+        if(!pad1.getButton2(1)) tentacle.leftMove(-TENTACLE_SPEED);
+        if(pad1.getButton2(0) && pad1.getButton2(1))  tentacle.leftMove(0);
 
-      if(!pad1.getButton1(4)) motor.windUp(WIND_UP_SPEED);
-      if(!pad1.getButton1(5)) motor.windUp(-WIND_UP_SPEED);
-      if(pad1.getButton1(4) && pad1.getButton1(5)) motor.windUp(0);
-
+        //if(!pad1.getButton1(0)); powerSwitch = 0;
     } else {
-      motor.moveLeftTentacle(0);
-      motor.moveRightTentacle(0);
-      motor.windUp(0);
+      tentacle.stop();
+      //powerSwitch = 0;
     }
+    /*if(receiveSuccessed2){
+
+    }*/
+}
+
+
+void Bot::calibrate()
+{
 
 }
 
-void Bot::calibrate(){
-
+void Bot::debugPrint()
+{
+  if(receiveSuccessed1) {
+    //debugSerial.printf("%f %f %f %f ,%f\r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3));
+    debugSerial.printf("%f\r\n",PIDC::angle);
+  } else {
+    debugSerial.printf("\r\n");
+  }
 }