Diff: bot/bot.cpp

Revision:

54:7176e6a08600

Parent:

52:320f910ca6ca

Child:

55:52b2cb45f21f

diff -r 10e586d71c42 -r 7176e6a08600 bot/bot.cpp
--- a/bot/bot.cpp	Thu Nov 23 04:12:15 2017 +0000
+++ b/bot/bot.cpp	Thu Nov 23 14:59:02 2017 +0900
@@ -1,80 +1,57 @@
 #include "bot.h"
 
 Bot::Bot() :
-    pad1(XBee1TX, XBee1RX, ADDR1),
-    pad2(XBee2TX, XBee2RX, ADDR2),
+    pad(XBee1TX, XBee1RX, ADDR1),
     RS485(MDTX, MDRX, 115200),
     RS485Controller(PWM1),
     powerSwitch(MDstop),
     quadOmni(&RS485Controller, &RS485),
-    tentacle(&RS485Controller, &RS485),
-    nishijoSword(&RS485Controller, &RS485),
-    nishijo(&RS485Controller, &RS485),
     plane(),
     axis(),
-    receiveSuccessed1(0),
-    receiveSuccessed2(0),
+    receiveSuccessed(0),
     frontDegree(0),
     debugSerial(USBTX, USBRX, 115200)
 {
     debugSerial.printf("OK\r\n");
 
     quadOmni.moveXY(0, 0, 0);
-    tentacle.stop();
-    nishijoSword.stop();
-    nishijo.stop();
-
-
     powerSwitch = true;
 }
 
-void Bot::confirmPad1()
+void Bot::confirmPad()
 {
-    receiveSuccessed1 = pad1.receiveState();
-    if(!pad1.getButton2(0) && !pad1.getButton2(1)) {
+    receiveSuccessed = pad.receiveState();
+    if(!pad.getButton2(0) && !pad.getButton2(1)) {
         powerSwitch = 0;
     }
-    if(!pad1.getButton2(2) && !pad1.getButton2(3)) {
+    if(!pad.getButton2(2) && !pad.getButton2(3)) {
         powerSwitch = 1;
     }
-    if(!receiveSuccessed1) {
+    if(!receiveSuccessed) {
         quadOmni.moveXY(0, 0, 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(receiveSuccessed1) {
+    if(receiveSuccessed) {
         float moment = 0;
         //debugSerial.printf("%d\n\r", plane.getRawDegree());
-        /*if(pad1.getNorm(0) > 0.5) {
-            plane.setPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
+        /*if(pad.getNorm(0) > 0.5) {
+            plane.setPoint((pad.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
             plane.confirm();
         }*/
         plane.confirm();
-        if(pad1.getStick(0)) {
-          moment = -pad1.getStick(0) / 2.0;
+        if(pad.getStick(0)) {
+          moment = -pad.getStick(0) / 2.0;
           plane.resetOffset();
         } else {
           moment = -plane.getCalculationResult();
         }
         debugSerial.printf("%f\r\n",moment);
         quadOmni.moveXY(
-                pad1.getStick(2),
-                -pad1.getStick(3),
+                pad.getStick(2),
+                -pad.getStick(3),
                 0,
                 0,
                 moment
@@ -90,39 +67,39 @@
   axis.confirm();
   float moment = 0;
   float norm = 0;
-  static float beforestick = pad1.getStick(0);
+  static float beforestick = pad.getStick(0);
 
-  if(!pad1.getButton2(5)) {
+  if(!pad.getButton2(5)) {
       axis.resetOffset();
       //debugSerial.printf("Force RESeT\n\r");
   }
 
-  if((beforestick >= 0.2 && pad1.getStick(0) < 0.2) || (beforestick <= -0.2 && pad1.getStick(0) > -0.2)) {
+  if((beforestick >= 0.2 && pad.getStick(0) < 0.2) || (beforestick <= -0.2 && pad.getStick(0) > -0.2)) {
       plane.setPoint(0.0);
       plane.resetOffset();
   }
-  /*if(!pad1.getButton2(1)&&pad1.getButton2(3)) {
+  /*if(!pad.getButton2(1)&&pad.getButton2(3)) {
         plane.setPoint((M_PI / 4)*(180.0 / M_PI));
-      } else if(pad1.getButton2(1)&&!pad1.getButton2(3)){
+      } else if(pad.getButton2(1)&&!pad.getButton2(3)){
         plane.setPoint(-(M_PI / 4)*(180.0 / M_PI));
       } else {
         plane.setPoint(0.0);
   }*/
-  if(pad1.getStick(0) > 0.2 || pad1.getStick(0) < -0.2) {
-      moment = pad1.getStick(0) / 2.0;
+  if(pad.getStick(0) > 0.2 || pad.getStick(0) < -0.2) {
+      moment = pad.getStick(0) / 2.0;
   } else  {
       moment = plane.getCalculationResult();
 
   }
-  if(pad1.getButton1(0)) {
-      norm = pad1.getNorm(1);
+  if(pad.getButton1(0)) {
+      norm = pad.getNorm(1);
   } else {
-      norm = pad1.getNorm(1) / 2.0;
+      norm = pad.getNorm(1) / 2.0;
   }
-  if(receiveSuccessed1) {
+  if(receiveSuccessed) {
       quadOmni.moveCircular(
               norm,
-              pad1.getRadian(1) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
+              pad.getRadian(1) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
               0.0,
               0.0,
               -moment
@@ -130,7 +107,7 @@
   } else {
       quadOmni.moveXY(0, 0, 0);
   }
-  beforestick = pad1.getStick(0);
+  beforestick = pad.getStick(0);
 }
 
 void Bot::controllDrive3()
@@ -138,22 +115,22 @@
     static int rollR = 0;
     static int rollL = 0;
     static int mode = 1;
-    if(receiveSuccessed1) {
-        if(rollR && !pad1.getButton2(2)) {
+    if(receiveSuccessed) {
+        if(rollR && !pad.getButton2(2)) {
             frontDegree += ADJUST_DEGREE;
         }
-        rollR = pad1.getButton2(2);
+        rollR = pad.getButton2(2);
 
-        if(rollL && !pad1.getButton2(0)) {
+        if(rollL && !pad.getButton2(0)) {
             frontDegree -= ADJUST_DEGREE;
         }
-        rollL = pad1.getButton2(0);
+        rollL = pad.getButton2(0);
 
-        if(!pad1.getButton2(4)) {
+        if(!pad.getButton2(4)) {
             mode = 1;
         }
 
-        if(!pad1.getButton2(5)) {
+        if(!pad.getButton2(5)) {
             mode = 2;
         }
 
@@ -162,8 +139,8 @@
             plane.confirm();
 
             quadOmni.moveXY(
-                    pad1.getStick(0),
-                    -pad1.getStick(1),
+                    pad.getStick(0),
+                    -pad.getStick(1),
                     0.5,
                     0.5,
                     -plane.getCalculationResult()
@@ -174,8 +151,8 @@
             plane.confirm();
 
             quadOmni.moveXY(
-                    -pad1.getStick(1),
-                    -pad1.getStick(0),
+                    -pad.getStick(1),
+                    -pad.getStick(0),
                     0.5,
                     0.5,
                     -plane.getCalculationResult()
@@ -188,39 +165,18 @@
 
 void Bot::controllDrive4()
 {
-    if(receiveSuccessed1) {
+    if(receiveSuccessed) {
         quadOmni.moveXY(
-                pad1.getStick(2),
-                pad1.getStick(3),
+                pad.getStick(2),
+                pad.getStick(3),
                 0.0,
                 0.0,
-                -pad1.getStick(0)/2.0
+                -pad.getStick(0)/2.0
                 );
     } else {
         quadOmni.moveXY(0, 0, 0);
     }
 }
-void Bot::controllMech()
-{
-  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 (!pad2.getButton2(3)) nishijo.move(-SWORD_SPEED);
-    if (!pad2.getButton2(2)) nishijo.move(SWORD_SPEED);
-    if(pad2.getButton2(2) && pad2.getButton2(3))  nishijo.move(0);
-  } else {
-    tentacle.stop();
-    nishijoSword.stop();
-    nishijo.stop();
-  }
-}
 
 void Bot::calibrate()
 {
@@ -228,11 +184,8 @@
 
 void Bot::checkConnection()
 {
-  if(receiveSuccessed1 == 1) debugSerial.printf("ON ");
-  else  debugSerial.printf("%d ",receiveSuccessed1);
-
-  if(receiveSuccessed2 == 1) debugSerial.printf("ON\r\n");
-  else  debugSerial.printf("%d\r\n",receiveSuccessed2);
+  if(receiveSuccessed == 1) debugSerial.printf("ON \r\n");
+  else  debugSerial.printf("OFF \r\n");
 }
 
 void Bot::checkDegree()
@@ -240,17 +193,8 @@
     debugSerial.printf("%f %f\r\n",plane.getRawDegree(), plane.getCalculationResult());
 }
 
-void Bot::checkPacket()
-{
-    char buf=0;
-    while(1) {
-      buf =  pad1.getc();
-      debugSerial.printf("%d %c\r\n",buf,buf);
-    }
-}
 
 void Bot::checkReceiveData()
 {
-  debugSerial.printf("%f %f %f %f\r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3));
-  debugSerial.printf("%f %f %f %f\r\n",pad2.getStick(0),pad2.getStick(1),pad2.getStick(2),pad2.getStick(3));
+  debugSerial.printf("%f %f %f %f\r\n",pad.getStick(0),pad.getStick(1),pad.getStick(2),pad.getStick(3));
 }