Diff: bot/bot.cpp

Revision:

47:43f55ff8916b

Parent:

46:9b714d4acdac

Child:

49:69a7235d837a

--- a/bot/bot.cpp	Sun Oct 29 15:40:26 2017 +0900
+++ b/bot/bot.cpp	Mon Nov 06 18:27:03 2017 +0900
@@ -12,11 +12,14 @@
             ikarashiMDC({&RS485Controller, 1, 2, SM, &RS485}),
             ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485})
             }),
+    plane(),
+    axis(),
     receiveSuccessed(0),
     frontDegree(0),
     led({DebugLED3, DebugLED4, DebugLED5}),
     debugSerial(USBTX, USBRX, 115200)
 {
+    debugSerial.printf("OK\r\n");
     for(int i = 0; i < 3; i++) {
         armMotor[i].braking = true;
     }
@@ -53,7 +56,7 @@
         debugSerial.printf("%d\n\r", plane.getRawDegree());
         led[1] = !led[1];
         if(pad.getNorm(1) > 0.5) {
-            plane.setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
+            plane.setPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
             plane.confirm();
         }
         quadOmni.moveXY(
@@ -61,7 +64,7 @@
                 -pad.getStick(1),
                 0,
                 0,
-                - pad.getStick(2) / 3.0 // PIDC::calculationResult
+                -pad.getStick(2) / 3.0 // PIDC::calculationResult
                 );
     } else {
         quadOmni.moveXY(0, 0, 0);
@@ -82,12 +85,12 @@
     }
 
     if((beforestick >= 0.2 && pad.getStick(2) < 0.2) || (beforestick <= -0.2 && pad.getStick(2) > -0.2)) {
-        plane.setSetPoint(0.0);
+        plane.setPoint(0.0);
         plane.resetOffset();
     }
 
     if(pad.getStick(2) > 0.2 || pad.getStick(2) < -0.2) {
-        moment = pad.getStick(2) / 4.0;
+        moment = pad.getStick(2) / 2.0;
     } else  {
         moment = plane.getCalculationResult();
     }
@@ -100,7 +103,7 @@
         led[1] = !led[1];
         quadOmni.moveCircular(
                 norm,
-                pad.getRadian(0) + M_PI,
+                pad.getRadian(0) + axis.getCurrentDegree() /1.0 * (M_PI / 180.0) + M_PI,
                 0.0,
                 0.0,
                 -moment
@@ -202,7 +205,7 @@
         }
         if(pad.getButton2(3) && pad.getButton2(2)) armMotor[SWORD].setSpeed(0.0);
 
-        slider.slide(-pad.getStick(3));
+        slider.slide(pad.getStick(3));
 
         if(!pad.getButton2(1)) {
             debugSerial.printf("DESTROYYY\n\r");
@@ -221,22 +224,4 @@
 
 void Bot::calibrate()
 {
-    if(receiveSuccessed &&
-            !pad.getButton2(4) &&
-            !pad.getButton2(5)
-      ){
-        t.start();
-        t.reset();
-        plane.calibration(HMC6352_ENTER_CALIB);
-        while(t.read() < 5.0) {
-            quadOmni.moveXY(0, 0, 0.4);
-            slider.slide(0);
-            for(int i = 0; i < 3; i++) {
-                armMotor[i].setSpeed(0);
-            }
-        }
-        t.stop();
-        quadOmni.moveXY(0, 0, 0);
-        plane.calibration(HMC6352_EXIT_CALIB);
-    }
 }