Diff: bot/bot.cpp

Revision:

49:69a7235d837a

Parent:

47:43f55ff8916b

Child:

50:3a7c858aa0f9

diff -r 13a1386792bd -r 69a7235d837a bot/bot.cpp
--- a/bot/bot.cpp	Mon Nov 06 18:34:02 2017 +0900
+++ b/bot/bot.cpp	Sat Nov 11 17:40:10 2017 +0900
@@ -1,30 +1,30 @@
 #include "bot.h"
 
 Bot::Bot() :
-    pad(XBee2TX, XBee2RX, ADDR),
+    pad1(XBee1TX, XBee1RX, ADDR1),
+    pad2(XBee2TX, XBee2RX, ADDR2),
     RS485(MDTX, MDRX, 38400),
     RS485Controller(PWM1),
     powerSwitch(MDstop),
     quadOmni(&RS485Controller, &RS485),
-    slider(&RS485Controller, &RS485),
-    armMotor({
-            ikarashiMDC({&RS485Controller, 1, 1, SM, &RS485}),
-            ikarashiMDC({&RS485Controller, 1, 2, SM, &RS485}),
-            ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485})
-            }),
+    tentacle(&RS485Controller, &RS485),
+    nishijoSword(&RS485Controller, &RS485),
+    nishijo(&RS485Controller, &RS485),
     plane(),
     axis(),
-    receiveSuccessed(0),
+    receiveSuccessed1(0),
+    receiveSuccessed2(0),
     frontDegree(0),
     led({DebugLED3, DebugLED4, DebugLED5}),
     debugSerial(USBTX, USBRX, 115200)
 {
     debugSerial.printf("OK\r\n");
-    for(int i = 0; i < 3; i++) {
-        armMotor[i].braking = true;
-    }
 
     quadOmni.moveXY(0, 0, 0);
+    tentacle.stop();
+    nishijoSword.stop();
+    nishijo.stop();
+
 
     powerSwitch = true;
     for(int i = 0; i < 3; i++) {
@@ -34,37 +34,54 @@
     }
 }
 
-void Bot::confirmAll()
+void Bot::confirmPad1()
 {
-    receiveSuccessed = pad.receiveState();
-    if(!pad.getButton2(3) && !pad.getButton2(0)) {
+    receiveSuccessed1 = pad1.receiveState();
+    if(!pad1.getButton2(3) && !pad1.getButton2(0)) {
         powerSwitch = 0;
     }
-    led[0] = receiveSuccessed;
-    if(!receiveSuccessed) {
+    if(!receiveSuccessed1) {
         quadOmni.moveXY(0, 0, 0);
-        slider.slide(0);
-        for(int i = 0; i < 3; i++) {
-            armMotor[i].setSpeed(0);
-        }
+    }
+}
+
+void Bot::confirmPad2()
+{
+    receiveSuccessed2 = pad2.receiveState();
+    if(!pad2.getButton2(3) && !pad2.getButton2(0)) {
+        powerSwitch = 0;
+    }
+    if(!receiveSuccessed2) {
+        tentacle.stop();
+        nishijoSword.stop();
+        nishijo.stop();
     }
 }
 
 void Bot::controllDrive()
 {
-    if(receiveSuccessed) {
-        debugSerial.printf("%d\n\r", plane.getRawDegree());
+    if(receiveSuccessed1) {
+        float moment = 0;
+        //debugSerial.printf("%d\n\r", plane.getRawDegree());
         led[1] = !led[1];
-        if(pad.getNorm(1) > 0.5) {
-            plane.setPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
+        /*if(pad1.getNorm(0) > 0.5) {
+            plane.setPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
             plane.confirm();
+        }*/
+        plane.confirm();
+        if(pad1.getStick(0)) {
+          moment = -pad1.getStick(0) / 2.0;
+          plane.resetOffset();
+        } else {
+          moment = -plane.getCalculationResult();
         }
+        debugSerial.printf("%f\r\n",moment);
         quadOmni.moveXY(
-                pad.getStick(0),
-                -pad.getStick(1),
+                pad1.getStick(2),
+                -pad1.getStick(3),
                 0,
                 0,
-                -pad.getStick(2) / 3.0 // PIDC::calculationResult
+                moment
                 );
     } else {
         quadOmni.moveXY(0, 0, 0);
@@ -77,33 +94,33 @@
     axis.confirm();
     float moment = 0;
     float norm = 0;
-    static float beforestick = pad.getStick(2);
+    static float beforestick = pad1.getStick(0);
 
-    if(!pad.getButton2(4)) {
+    if(!pad1.getButton2(4)) {
         axis.resetOffset();
         debugSerial.printf("Force RESeT\n\r");
     }
 
-    if((beforestick >= 0.2 && pad.getStick(2) < 0.2) || (beforestick <= -0.2 && pad.getStick(2) > -0.2)) {
+    if((beforestick >= 0.2 && pad1.getStick(0) < 0.2) || (beforestick <= -0.2 && pad1.getStick(0) > -0.2)) {
         plane.setPoint(0.0);
         plane.resetOffset();
     }
 
-    if(pad.getStick(2) > 0.2 || pad.getStick(2) < -0.2) {
-        moment = pad.getStick(2) / 2.0;
+    if(pad1.getStick(0) > 0.2 || pad1.getStick(0) < -0.2) {
+        moment = pad1.getStick(0) / 2.0;
     } else  {
         moment = plane.getCalculationResult();
     }
-    if(pad.getButton1(0)) {
-        norm = pad.getNorm(0) / 2.0;
+    if(pad1.getButton1(0)) {
+        norm = pad1.getNorm(1) / 2.0;
     } else {
-        norm = pad.getNorm(0);
+        norm = pad1.getNorm(1);
     }
-    if(receiveSuccessed) {
+    if(receiveSuccessed1) {
         led[1] = !led[1];
         quadOmni.moveCircular(
                 norm,
-                pad.getRadian(0) + axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
+                pad1.getRadian(1) + axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
                 0.0,
                 0.0,
                 -moment
@@ -111,7 +128,7 @@
     } else {
         quadOmni.moveXY(0, 0, 0);
     }
-    beforestick = pad.getStick(2);
+    beforestick = pad1.getStick(0);
 }
 
 void Bot::controllDrive3()
@@ -119,22 +136,22 @@
     static int rollR = 0;
     static int rollL = 0;
     static int mode = 1;
-    if(receiveSuccessed) {
-        if(rollR && !pad.getButton2(2)) {
+    if(receiveSuccessed1) {
+        if(rollR && !pad1.getButton2(2)) {
             frontDegree += ADJUST_DEGREE;
         }
-        rollR = pad.getButton2(2);
+        rollR = pad1.getButton2(2);
 
-        if(rollL && !pad.getButton2(0)) {
+        if(rollL && !pad1.getButton2(0)) {
             frontDegree -= ADJUST_DEGREE;
         }
-        rollL = pad.getButton2(0);
+        rollL = pad1.getButton2(0);
 
-        if(!pad.getButton2(4)) {
+        if(!pad1.getButton2(4)) {
             mode = 1;
         }
 
-        if(!pad.getButton2(5)) {
+        if(!pad1.getButton2(5)) {
             mode = 2;
         }
 
@@ -143,8 +160,8 @@
             plane.confirm();
 
             quadOmni.moveXY(
-                    pad.getStick(0),
-                    -pad.getStick(1),
+                    pad1.getStick(0),
+                    -pad1.getStick(1),
                     0.5,
                     0.5,
                     -plane.getCalculationResult()
@@ -155,8 +172,8 @@
             plane.confirm();
 
             quadOmni.moveXY(
-                    -pad.getStick(1),
-                    -pad.getStick(0),
+                    -pad1.getStick(1),
+                    -pad1.getStick(0),
                     0.5,
                     0.5,
                     -plane.getCalculationResult()
@@ -169,13 +186,13 @@
 
 void Bot::controllDrive4()
 {
-    if(receiveSuccessed) {
+    if(receiveSuccessed1) {
         quadOmni.moveXY(
-                pad.getStick(0),
-                pad.getStick(1),
+                pad1.getStick(2),
+                pad1.getStick(3),
                 0.0,
                 0.0,
-                -pad.getStick(2)/2.0
+                -pad1.getStick(0)/2.0
                 );
     } else {
         quadOmni.moveXY(0, 0, 0);
@@ -183,45 +200,40 @@
 }
 void Bot::controllMech()
 {
-    if(receiveSuccessed) {
-        if(!pad.getButton1(2)) {
-            debugSerial.printf("ROLL+\n\r");
-            armMotor[ROLL].setSpeed(1.0);
-        } else if(!pad.getButton1(4)) {
-            debugSerial.printf("ROLL-\n\r");
-            armMotor[ROLL].setSpeed(-1.0);
-        }
-        if(pad.getButton1(2) && pad.getButton1(4)) {
-            armMotor[ROLL].setSpeed(0.0);
-        }
+  if(receiveSuccessed2) {
+
+      //if(!pad2.getButton1(2)&&!pad2.getButton1(4)) powerSwitch=0;
+    tentacle.leftMove(pad2.getStick(3));
+    tentacle.rightMove(pad2.getStick(1));
+
+    if (!pad2.getButton2(0)) nishijoSword.move(-WIND_UP_SPEED);
+    if (!pad2.getButton2(1)) nishijoSword.move(WIND_UP_SPEED);
+    if(pad2.getButton2(0) && pad2.getButton2(1))  nishijoSword.move(0);
 
-        if(!pad.getButton2(3)) {
-            debugSerial.printf("FUKUDA\n\r");
-            armMotor[SWORD].setSpeed(1.0);
-        }
-        if(!pad.getButton2(2)) {
-            debugSerial.printf("FUKUDA\n\r");
-            armMotor[SWORD].setSpeed(-1.0);
-        }
-        if(pad.getButton2(3) && pad.getButton2(2)) armMotor[SWORD].setSpeed(0.0);
-
-        slider.slide(pad.getStick(3));
-
-        if(!pad.getButton2(1)) {
-            debugSerial.printf("DESTROYYY\n\r");
-            armMotor[DESTROY].setSpeed(-1.0);
-        } else {
-            armMotor[DESTROY].setSpeed(0.0);
-        }
-    } else {
-        slider.slide(0);
-        for(int i = 0; i < 3; i++) {
-            armMotor[i].setSpeed(0);
-        }
-    }
+    if (!pad2.getButton2(2)) nishijo.move(-SWORD_SPEED);
+    if (!pad2.getButton2(3)) nishijo.move(SWORD_SPEED);
+    if(pad2.getButton2(2) && pad2.getButton2(3))  nishijo.move(0);
+  } else {
+    tentacle.stop();
+    nishijoSword.stop();
+    nishijo.stop();
+  }
 }
 
-
 void Bot::calibrate()
 {
 }
+
+void Bot::checkConnection()
+{
+  if(receiveSuccessed1) debugSerial.printf("ON ");
+  else  debugSerial.printf("OFF ");
+
+  if(receiveSuccessed2) debugSerial.printf("ON\r\n");
+  else  debugSerial.printf("OFF\r\n");
+}
+
+void Bot::checkDegree()
+{
+    debugSerial.printf("%f %f\r\n",plane.getRawDegree(), plane.getCalculationResult());
+}