Diff: bot/bot.cpp

Revision:

24:9a4ca5442717

Parent:

23:37bb9afe9fdc

Child:

25:d199d621ecca

diff -r 37bb9afe9fdc -r 9a4ca5442717 bot/bot.cpp
--- a/bot/bot.cpp	Wed Sep 13 14:26:47 2017 +0900
+++ b/bot/bot.cpp	Wed Sep 13 23:27:17 2017 +0900
@@ -5,7 +5,7 @@
     pad1(XBee1TX, XBee1RX, ADDR1),
     //pad2(XBee2TX, XBee2RX, ADDR2),
     RS485(MDTX, MDRX, 38400),
-    RS485Controller(PWM1),
+    RS485Controller(PWM2),
     powerSwitch(MDstop),
     quadOmni(&RS485Controller, &RS485),
     tentacle(&RS485Controller, &RS485),
@@ -15,10 +15,11 @@
     //receiveSuccessed2(0),
     led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
         DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
+    test(PWM1),
     debugSerial(USBTX, USBRX, 115200)
 {
 
-
+    test = 0;
     quadOmni.moveXY(0, 0, 0);
 
     powerSwitch = 1;
@@ -52,12 +53,20 @@
             PIDC::PID::setSetPoint((pad1.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
             PIDC::confirm();
         }
+        //PIDC::confirm();
         PIDC::renewAngle();
+        test = !test;
         quadOmni.moveXY(
-            pad1.getStick(0) / 2.0,
-            -pad1.getStick(1) /2.0,
-            PIDC::calculationResult
+            pad1.getStick(0) * 0.8,
+            -pad1.getStick(1) * 0.8,
+            -pad1.getStick(2) * 0.4
+             //PIDC::calculationResult
         );
+        /*quadOmni.moveCircular(
+          0.2,
+          0,
+          0
+        );*/
     } else {
         quadOmni.moveXY(0, 0, 0);
     }
@@ -109,7 +118,8 @@
         if(!pad1.getButton2(1)) tentacle.leftMove(-TENTACLE_SPEED);
         if(pad1.getButton2(0) && pad1.getButton2(1))  tentacle.leftMove(0);
 
-        //if(!pad1.getButton1(0)); powerSwitch = 0;
+        if(!pad1.getButton1(2) && !pad1.getButton1(3) && !pad1.getButton1(4) && !pad1.getButton1(5)) powerSwitch = 0;
+        //else powerSwitch = 1;
     } else {
       tentacle.stop();
       //powerSwitch = 0;
@@ -128,8 +138,8 @@
 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);
+    //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);
   } else {
     debugSerial.printf("\r\n");
   }