Diff: bot/bot.cpp

Revision:

54:857390145ac4

Parent:

52:320f910ca6ca

Child:

55:ccf2ac8f6f32

--- a/bot/bot.cpp	Thu Nov 23 04:15:02 2017 +0000
+++ b/bot/bot.cpp	Thu Nov 23 14:59:59 2017 +0900
@@ -1,220 +1,52 @@
 #include "bot.h"
 
 Bot::Bot() :
-    pad1(XBee1TX, XBee1RX, ADDR1),
-    pad2(XBee2TX, XBee2RX, ADDR2),
+    pad(FEPTX, FEPRX, ADDR2),
     RS485(MDTX, MDRX, 115200),
-    RS485Controller(PWM1),
+    RS485Controller(NC),
     powerSwitch(MDstop),
-    quadOmni(&RS485Controller, &RS485),
     tentacle(&RS485Controller, &RS485),
     nishijoSword(&RS485Controller, &RS485),
     nishijo(&RS485Controller, &RS485),
-    plane(),
-    axis(),
-    receiveSuccessed1(0),
-    receiveSuccessed2(0),
-    frontDegree(0),
+    receiveSuccessed(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(3) && !pad.getButton2(0)) {
         powerSwitch = 0;
     }
-    if(!pad1.getButton2(2) && !pad1.getButton2(3)) {
-        powerSwitch = 1;
-    }
-    if(!receiveSuccessed1) {
-        quadOmni.moveXY(0, 0, 0);
-    }
-}
-
-void Bot::confirmPad2()
-{
-    receiveSuccessed2 = pad2.receiveState();
-    if(!pad2.getButton2(3) && !pad2.getButton2(0)) {
-        powerSwitch = 0;
-    }
-    if(!receiveSuccessed2) {
+    if(!receiveSuccessed) {
         tentacle.stop();
         nishijoSword.stop();
         nishijo.stop();
     }
 }
 
-void Bot::controllDrive()
-{
-    if(receiveSuccessed1) {
-        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));
-            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(
-                pad1.getStick(2),
-                -pad1.getStick(3),
-                0,
-                0,
-                moment
-                );
-    } else {
-        quadOmni.moveXY(0, 0, 0);
-    }
-}
-
-void Bot::controllDrive2()
-{
-  plane.confirm();
-  axis.confirm();
-  float moment = 0;
-  float norm = 0;
-  static float beforestick = pad1.getStick(0);
-
-  if(!pad1.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)) {
-      plane.setPoint(0.0);
-      plane.resetOffset();
-  }
-  /*if(!pad1.getButton2(1)&&pad1.getButton2(3)) {
-        plane.setPoint((M_PI / 4)*(180.0 / M_PI));
-      } else if(pad1.getButton2(1)&&!pad1.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;
-  } else  {
-      moment = plane.getCalculationResult();
-
-  }
-  if(pad1.getButton1(0)) {
-      norm = pad1.getNorm(1);
-  } else {
-      norm = pad1.getNorm(1) / 2.0;
-  }
-  if(receiveSuccessed1) {
-      quadOmni.moveCircular(
-              norm,
-              pad1.getRadian(1) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
-              0.0,
-              0.0,
-              -moment
-              );
-  } else {
-      quadOmni.moveXY(0, 0, 0);
-  }
-  beforestick = pad1.getStick(0);
-}
-
-void Bot::controllDrive3()
-{
-    static int rollR = 0;
-    static int rollL = 0;
-    static int mode = 1;
-    if(receiveSuccessed1) {
-        if(rollR && !pad1.getButton2(2)) {
-            frontDegree += ADJUST_DEGREE;
-        }
-        rollR = pad1.getButton2(2);
-
-        if(rollL && !pad1.getButton2(0)) {
-            frontDegree -= ADJUST_DEGREE;
-        }
-        rollL = pad1.getButton2(0);
-
-        if(!pad1.getButton2(4)) {
-            mode = 1;
-        }
-
-        if(!pad1.getButton2(5)) {
-            mode = 2;
-        }
-
-        if(mode == 1) {
-            plane.setPoint(frontDegree);
-            plane.confirm();
-
-            quadOmni.moveXY(
-                    pad1.getStick(0),
-                    -pad1.getStick(1),
-                    0.5,
-                    0.5,
-                    -plane.getCalculationResult()
-                    );
-        }
-        if(mode == 2) {
-            plane.setPoint(90.0 + frontDegree);
-            plane.confirm();
-
-            quadOmni.moveXY(
-                    -pad1.getStick(1),
-                    -pad1.getStick(0),
-                    0.5,
-                    0.5,
-                    -plane.getCalculationResult()
-                    );
-        }
-    } else {
-        quadOmni.moveXY(0, 0, 0);
-    }
-}
-
-void Bot::controllDrive4()
-{
-    if(receiveSuccessed1) {
-        quadOmni.moveXY(
-                pad1.getStick(2),
-                pad1.getStick(3),
-                0.0,
-                0.0,
-                -pad1.getStick(0)/2.0
-                );
-    } else {
-        quadOmni.moveXY(0, 0, 0);
-    }
-}
 void Bot::controllMech()
 {
-  if(receiveSuccessed2) {
+  if(receiveSuccessed) {
 
-      //if(!pad2.getButton1(2)&&!pad2.getButton1(4)) powerSwitch=0;
-    tentacle.leftMove(pad2.getStick(3));
-    tentacle.rightMove(pad2.getStick(1));
+      //if(!pad.getButton1(2)&&!pad.getButton1(4)) powerSwitch=0;
+    tentacle.leftMove(pad.getStick(3));
+    tentacle.rightMove(pad.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(0)) nishijoSword.move(-WIND_UP_SPEED);
+    if (!pad.getButton2(1)) nishijoSword.move(WIND_UP_SPEED);
+    if(pad.getButton2(0) && pad.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);
+    if (!pad.getButton2(3)) nishijo.move(-SWORD_SPEED);
+    if (!pad.getButton2(2)) nishijo.move(SWORD_SPEED);
+    if(pad.getButton2(2) && pad.getButton2(3))  nishijo.move(0);
   } else {
     tentacle.stop();
     nishijoSword.stop();
@@ -222,35 +54,13 @@
   }
 }
 
-void Bot::calibrate()
-{
-}
-
 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);
-}
-
-void Bot::checkDegree()
-{
-    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);
-    }
+  if(receiveSuccessed == 1) debugSerial.printf("ON\r\n");
+  else  debugSerial.printf("OFF");
 }
 
 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));
 }