Diff: bot/bot.cpp

Revision:

51:64534e908d5c

Parent:

50:3a7c858aa0f9

--- a/bot/bot.cpp	Mon Nov 13 17:33:30 2017 +0900
+++ b/bot/bot.cpp	Mon Nov 13 17:34:01 2017 +0900
@@ -3,7 +3,7 @@
 Bot::Bot() :
     pad1(XBee1TX, XBee1RX, ADDR1),
     pad2(XBee2TX, XBee2RX, ADDR2),
-    RS485(MDTX, MDRX, 115200),
+    RS485(MDTX, MDRX, 38400),
     RS485Controller(PWM1),
     powerSwitch(MDstop),
     quadOmni(&RS485Controller, &RS485),
@@ -15,6 +15,7 @@
     receiveSuccessed1(0),
     receiveSuccessed2(0),
     frontDegree(0),
+    led({DebugLED3, DebugLED4, DebugLED5}),
     debugSerial(USBTX, USBRX, 115200)
 {
     debugSerial.printf("OK\r\n");
@@ -26,17 +27,19 @@
 
 
     powerSwitch = true;
+    for(int i = 0; i < 3; i++) {
+        led[i] = true;
+        wait(0.1);
+        led[i] = false;
+    }
 }
 
 void Bot::confirmPad1()
 {
     receiveSuccessed1 = pad1.receiveState();
-    if(!pad1.getButton2(0) && !pad1.getButton2(1)) {
+    if(!pad1.getButton2(3) && !pad1.getButton2(0)) {
         powerSwitch = 0;
     }
-    if(!pad1.getButton2(2) && !pad1.getButton2(3)) {
-        powerSwitch = 1;
-    }
     if(!receiveSuccessed1) {
         quadOmni.moveXY(0, 0, 0);
     }
@@ -60,6 +63,7 @@
     if(receiveSuccessed1) {
         float moment = 0;
         //debugSerial.printf("%d\n\r", plane.getRawDegree());
+        led[1] = !led[1];
         /*if(pad1.getNorm(0) > 0.5) {
             plane.setPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
             plane.confirm();
@@ -86,51 +90,45 @@
 
 void Bot::controllDrive2()
 {
-  plane.confirm();
-  axis.confirm();
-  float moment = 0;
-  float norm = 0;
-  static float beforestick = pad1.getStick(0);
+    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(!pad1.getButton2(4)) {
+        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 {
+    if((beforestick >= 0.2 && pad1.getStick(0) < 0.2) || (beforestick <= -0.2 && pad1.getStick(0) > -0.2)) {
         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();
+        plane.resetOffset();
+    }
 
-  }
-  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);
+    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) / 2.0;
+    } else {
+        norm = pad1.getNorm(1);
+    }
+    if(receiveSuccessed1) {
+        led[1] = !led[1];
+        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()