Diff: bot/bot.cpp

Revision:

25:d199d621ecca

Parent:

24:9a4ca5442717

Child:

26:7258d5ad0bff

diff -r 9a4ca5442717 -r d199d621ecca bot/bot.cpp
--- a/bot/bot.cpp	Wed Sep 13 23:27:17 2017 +0900
+++ b/bot/bot.cpp	Sat Sep 30 17:59:10 2017 +0900
@@ -3,7 +3,7 @@
 Bot::Bot() :
     PIDC(),
     pad1(XBee1TX, XBee1RX, ADDR1),
-    //pad2(XBee2TX, XBee2RX, ADDR2),
+    pad2(XBee2TX, XBee2RX, ADDR2),
     RS485(MDTX, MDRX, 38400),
     RS485Controller(PWM2),
     powerSwitch(MDstop),
@@ -12,11 +12,11 @@
     nishijo(&RS485Controller, &RS485),
     nishijoSword(&RS485Controller, &RS485),
     receiveSuccessed1(0),
-    //receiveSuccessed2(0),
+    receiveSuccessed2(0),
     led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
         DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
     test(PWM1),
-    debugSerial(USBTX, USBRX, 115200)
+    debugSerial(USBTX, USBRX, 230400)
 {
 
     test = 0;
@@ -39,7 +39,7 @@
 void Bot::confirmAll()
 {
     receiveSuccessed1 = pad1.receiveState();
-    //receiveSuccessed2 = pad2.receiveState();
+    receiveSuccessed2 = pad2.receiveState();
     if(!receiveSuccessed1) {
         quadOmni.moveXY(0, 0, 0);
         tentacle.stop();
@@ -48,18 +48,19 @@
 
 void Bot::controllDrive()
 {
+    test = !test;
     if(receiveSuccessed1) {
-        if(pad1.getNorm(1) > 0.5) {
-            PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
+        if(pad1.getNorm(0) > 0.5) {
+            PIDC::PID::setSetPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
             PIDC::confirm();
         }
         //PIDC::confirm();
         PIDC::renewAngle();
-        test = !test;
+        //test = !test;
         quadOmni.moveXY(
             pad1.getStick(0) * 0.8,
             -pad1.getStick(1) * 0.8,
-            -pad1.getStick(2) * 0.4
+            pad1.getStick(2) * 0.4
              //PIDC::calculationResult
         );
         /*quadOmni.moveCircular(
@@ -109,19 +110,42 @@
 
 void Bot::controllMech()
 {
-    if(receiveSuccessed1) {
-        if(!pad1.getButton2(2)) tentacle.rightMove(TENTACLE_SPEED);
+    //nishijoSword.move(1.0);
+    if(receiveSuccessed2) {
+        /*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.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.getButton2(0) && pad1.getButton2(1))  tentacle.leftMove(0);*/
+
+        if(pad2.getStick(1) > 0){
+            tentacle.leftMove(pad2.getStick(1));
+        } else {
+            tentacle.leftMove(-TENTACLE_SPEED);
+        }
 
-        if(!pad1.getButton1(2) && !pad1.getButton1(3) && !pad1.getButton1(4) && !pad1.getButton1(5)) powerSwitch = 0;
+        if(pad2.getStick(3) > 0){
+            tentacle.leftMove(pad2.getStick(3));
+        } else {
+            tentacle.leftMove(-TENTACLE_SPEED);
+        }
+
+        if (!pad1.getButton2(3)) nishijoSword.move(WIND_UP_SPEED);
+        if (!pad1.getButton2(5)) nishijoSword.move(-WIND_UP_SPEED);
+        if(pad2.getButton2(3) && pad2.getButton2(5))  nishijoSword.move(0);
+
+        //test = !pad1.getButton1(5);
+
+        //if(!pad1.getButton1(6)) powerSwitch = 0;
+        //if(!pad1.getButton1(5)&&!pad1.getButton1(4)) powerSwitch = 1;
+
         //else powerSwitch = 1;
     } else {
       tentacle.stop();
+      nishijoSword.stop();
+      //test =0;
       //powerSwitch = 0;
     }
     /*if(receiveSuccessed2){
@@ -138,8 +162,13 @@
 void Bot::debugPrint()
 {
   if(receiveSuccessed1) {
-    //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\r\n", PIDC::beforeDegree, PIDC::rawDegree, PIDC::calculationResult);
+    debugSerial.printf("%f %f %f %f \r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3));
+  } else {
+    debugSerial.printf("\r\n");
+  }
+  if(receiveSuccessed2) {
+    debugSerial.printf("%f %f %f %f \r\n",pad2.getStick(0),pad2.getStick(1),pad2.getStick(2),pad2.getStick(3));
+
   } else {
     debugSerial.printf("\r\n");
   }