Diff: bot/bot.cpp

Revision:

50:3a7c858aa0f9

Parent:

49:69a7235d837a

Child:

51:64534e908d5c

Child:

52:320f910ca6ca

--- a/bot/bot.cpp	Sat Nov 11 17:40:10 2017 +0900
+++ b/bot/bot.cpp	Mon Nov 13 17:33:30 2017 +0900
@@ -3,7 +3,7 @@
 Bot::Bot() :
     pad1(XBee1TX, XBee1RX, ADDR1),
     pad2(XBee2TX, XBee2RX, ADDR2),
-    RS485(MDTX, MDRX, 38400),
+    RS485(MDTX, MDRX, 115200),
     RS485Controller(PWM1),
     powerSwitch(MDstop),
     quadOmni(&RS485Controller, &RS485),
@@ -15,7 +15,6 @@
     receiveSuccessed1(0),
     receiveSuccessed2(0),
     frontDegree(0),
-    led({DebugLED3, DebugLED4, DebugLED5}),
     debugSerial(USBTX, USBRX, 115200)
 {
     debugSerial.printf("OK\r\n");
@@ -27,19 +26,17 @@
 
 
     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(3) && !pad1.getButton2(0)) {
+    if(!pad1.getButton2(0) && !pad1.getButton2(1)) {
         powerSwitch = 0;
     }
+    if(!pad1.getButton2(2) && !pad1.getButton2(3)) {
+        powerSwitch = 1;
+    }
     if(!receiveSuccessed1) {
         quadOmni.moveXY(0, 0, 0);
     }
@@ -63,7 +60,6 @@
     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();
@@ -90,45 +86,51 @@
 
 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(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(5)) {
+      axis.resetOffset();
+      debugSerial.printf("Force RESeT\n\r");
+  }
 
-    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);
+  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()