Diff: main.cpp

Revision:

67:edc3c644d316

Parent:

66:17be4d8e2c26

Child:

69:bfefdfb04c29

--- a/main.cpp	Fri Nov 01 09:36:14 2019 +0000
+++ b/main.cpp	Fri Nov 01 09:51:44 2019 +0000
@@ -972,6 +972,11 @@
     }
 
     // Do nothing until end condition is met
+    PID_controller();
+    motorControl();
+    RKI();
+    
+    motorKillPower();
 
     // State transition guard
     if ( button1_pressed == true ) { // demo_XY
@@ -995,6 +1000,10 @@
         demo_state_changed = false;
         set_led_color('c'); // Set LED to motor calibration (CYAN)
     }
+    
+    PID_controller();
+    motorControl();
+    RKI();
 
     // Do stuff until end condition is met
     motor_state_machine();
@@ -1016,7 +1025,7 @@
         timerStateChange.start();
     }
 
-    Vx = 5.0;
+    Vx = 12.0;
     Vy = -5.0;
 
     PID_controller();
@@ -1030,7 +1039,7 @@
     scope.send();
 
     // State transition guard
-    if ( timerStateChange.read() >= 3.0f ) { // demo_wait
+    if ( timerStateChange.read() >= 5.0f ) { // demo_wait
         button1_pressed = false;
         demo_curr_state = demo_XY;
         demo_state_changed = true;
@@ -1048,13 +1057,13 @@
     static float t = 0.0;
 
     if ( t >= 0.0f && t < 3.0f ) {
-        Vx = 5.0;
+        Vx = 7.0;
         Vy = 0.0;
     } else if ( t >= 3.0f && t < 6.0f ) {
         Vx = 0.0;
         Vy = 5.0;
     } else if ( t >= 6.0f && t < 9.0f ) {
-        Vx = -5.0;
+        Vx = -7.0;
         Vy = 0.0;
     }
     if ( t >= 9.0f && t < 12.0f ) {
@@ -1195,6 +1204,8 @@
     }
 
     // Do nothing until end condition is met
+    motorKillPower();
+
 
     // State transition guard
     if ( switch2_pressed == true ) { // DEMO MODE