Diff: bot/bot.cpp

Branch:

develop1

Revision:

17:79fa65706f92

Parent:

16:50651ff960b9

Child:

19:34da005ea4ea

--- a/bot/bot.cpp	Mon Sep 25 18:15:43 2017 +0900
+++ b/bot/bot.cpp	Tue Sep 26 16:55:48 2017 +0900
@@ -50,10 +50,11 @@
 void Bot::controllDrive()
 {
     if(receiveSuccessed) {
+        debugSerial.printf("%d\n\r", PIDC::getRawDegree());
         led[1] = !led[1];
         if(pad.getNorm(1) > 0.5) {
-            // PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
-            // PIDC::confirm();
+            PIDC::PID::setSetPoint((pad.getRadian(1) - M_PI / 2) * (180.0 / M_PI));
+            PIDC::confirm();
         }
         quadOmni.moveXY(
             pad.getStick(0),
@@ -101,17 +102,48 @@
     beforestick = pad.getStick(2);
 }
 
+void Bot::controllDrive3()
+{
+    static int mode = 1;
+    if(receiveSuccessed) {
+        debugSerial.printf("%d\n\r", PIDC::getCurrentDegree());
+
+        if(!pad.getButton2(4)) {
+            mode = 1;
+        }
+
+        if(!pad.getButton2(5)) {
+            mode = 2;
+        }
+
+        if(mode == 1) {
+            PIDC::PID::setSetPoint(0.0);
+            PIDC::confirm();
+
+            quadOmni.moveXY(
+                pad.getStick(0),
+                -pad.getStick(1),
+                -PIDC::calculationResult
+            );
+        }
+        if(mode == 2) {
+            PIDC::PID::setSetPoint(90.0);
+            PIDC::confirm();
+
+            quadOmni.moveXY(
+                pad.getStick(0),
+                -pad.getStick(1),
+                -PIDC::calculationResult
+            );
+        }
+    } else {
+        quadOmni.moveXY(0, 0, 0);
+    }
+}
+
 void Bot::controllMech()
 {
     if(receiveSuccessed) {
-//        if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
-//        if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
-//        if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
-//
-//        if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
-//        if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
-//        if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
-//
         if(!pad.getButton1(6)) {
             armMotor[SWORD].setSpeed(0.6);
         }
@@ -126,13 +158,6 @@
         } else {
             armMotor[DESTROY].setSpeed(0.0);
         }
-//
-//        if(!pad.getButton1(2)) {
-//            motor.destroy(DESTROY_MAX_SPEED);
-//        } else {
-//            motor.destroy(0);
-//        }
-//
    } else {
        slider.slide(0);
        for(int i = 0; i < 3; i++) {