Diff: bot/bot.cpp

Revision:

28:676330f1d186

Parent:

27:47c6eee26e76

Child:

29:41f6fc4c8962

--- a/bot/bot.cpp	Sun Oct 01 16:52:33 2017 +0900
+++ b/bot/bot.cpp	Tue Oct 03 19:15:00 2017 +0900
@@ -14,6 +14,7 @@
         ikarashiMDC({&RS485Controller, 1, 3, SM, &RS485})
     }),
     receiveSuccessed(0),
+    frontDegree(0),
     led({DebugLED3, DebugLED4, DebugLED5}),
     debugSerial(USBTX, USBRX, 115200)
 {
@@ -106,9 +107,19 @@
 
 void Bot::controllDrive3()
 {
+    static int rollR = 0;
+    static int rollL = 0;
     static int mode = 1;
     if(receiveSuccessed) {
-        debugSerial.printf("%d\n\r", PIDC::getCurrentDegree());
+        if(rollR && !pad.getButton2(2)) {
+            frontDegree += ADJUST_DEGREE;
+        }
+        rollR = pad.getButton2(2);
+
+        if(rollL && !pad.getButton2(0)) {
+            frontDegree -= ADJUST_DEGREE;
+        }
+        rollL = pad.getButton2(0);
 
         if(!pad.getButton2(4)) {
             mode = 1;
@@ -119,7 +130,7 @@
         }
 
         if(mode == 1) {
-            PIDC::PID::setSetPoint(0.0);
+            PIDC::PID::setSetPoint(frontDegree);
             PIDC::confirm();
 
             quadOmni.moveXY(
@@ -131,7 +142,7 @@
             );
         }
         if(mode == 2) {
-            PIDC::PID::setSetPoint(90.0);
+            PIDC::PID::setSetPoint(90.0 + frontDegree);
             PIDC::confirm();
 
             quadOmni.moveXY(