Diff: bot/bot.cpp

Revision:

55:52b2cb45f21f

Parent:

54:7176e6a08600

--- a/bot/bot.cpp	Thu Nov 23 14:59:02 2017 +0900
+++ b/bot/bot.cpp	Tue Nov 28 17:53:13 2017 +0900
@@ -10,6 +10,7 @@
     axis(),
     receiveSuccessed(0),
     frontDegree(0),
+    led(LED1),
     debugSerial(USBTX, USBRX, 115200)
 {
     debugSerial.printf("OK\r\n");
@@ -86,16 +87,16 @@
         plane.setPoint(0.0);
   }*/
   if(pad.getStick(0) > 0.2 || pad.getStick(0) < -0.2) {
-      moment = pad.getStick(0) / 2.0;
+      moment = pad.getStick(0);
   } else  {
       moment = plane.getCalculationResult();
 
   }
-  if(pad.getButton1(0)) {
+  // if(pad.getButton1(0)) {
       norm = pad.getNorm(1);
-  } else {
-      norm = pad.getNorm(1) / 2.0;
-  }
+  // } else {
+  //     norm = pad.getNorm(1) / 2.0;
+  // }
   if(receiveSuccessed) {
       quadOmni.moveCircular(
               norm,
@@ -168,16 +169,72 @@
     if(receiveSuccessed) {
         quadOmni.moveXY(
                 pad.getStick(2),
-                pad.getStick(3),
+                -pad.getStick(3),
                 0.0,
                 0.0,
-                -pad.getStick(0)/2.0
+                -pad.getStick(0)
                 );
     } else {
         quadOmni.moveXY(0, 0, 0);
     }
 }
 
+void Bot::controllDrive5()
+{
+  plane.confirm();
+  axis.confirm();
+  float moment = 0;
+  float norm = 0;
+  static float beforestick = pad.getStick(0);
+  static int mode = 0;
+
+  led = mode;
+  if(!pad.getButton2(5)) {
+      axis.resetOffset();
+  }
+
+  if((beforestick >= 0.2 && pad.getStick(0) < 0.2) || (beforestick <= -0.2 && pad.getStick(0) > -0.2)) {
+      plane.setPoint(0.0);
+      plane.resetOffset();
+  }
+  if(pad.getStick(0) > 0.2 || pad.getStick(0) < -0.2) {
+      moment = pad.getStick(0);
+  } else  {
+      moment = plane.getCalculationResult();
+  }
+  norm = pad.getNorm(1);
+
+  if(receiveSuccessed) {
+    if(!pad.getButton2(0) && !pad.getButton2(1)) {
+        mode = 0;
+    }
+    if(!pad.getButton2(2) && !pad.getButton2(3)) {
+        mode = 1;
+    }
+    if(mode){
+      quadOmni.moveCircular(
+              norm,
+              pad.getRadian(1) - axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
+              0.0,
+              0.0,
+              -moment
+              );
+    } else {
+      quadOmni.moveXY(
+              pad.getStick(2),
+              -pad.getStick(3),
+              0.0,
+              0.0,
+              -pad.getStick(0)
+              );
+    }
+  } else {
+      quadOmni.moveXY(0, 0, 0);
+  }
+  beforestick = pad.getStick(0);
+}
+
+
 void Bot::calibrate()
 {
 }