Diff: main.cpp

Revision:

65:a01cf17a2bce

Parent:

64:a0ca27f75801

Parent:

63:d17f45d88c7a

Child:

66:17be4d8e2c26

--- a/main.cpp	Fri Nov 01 09:10:52 2019 +0000
+++ b/main.cpp	Fri Nov 01 09:12:00 2019 +0000
@@ -686,7 +686,7 @@
     u_p1 = Kp * motor1_error;
 
     //Integral part
-    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY ) {
+    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) {
         error_integral1 = error_integral1 + ei1 * Ts;
         u_i1 = Ki * error_integral1;
     }
@@ -719,7 +719,7 @@
     u_p2 = Kp * motor2_error;
 
     //Integral part
-    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY ) {
+    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) {
         error_integral2 = error_integral2 + ei2 * Ts;
         u_i2 = Ki * error_integral2;
     }
@@ -1016,8 +1016,8 @@
         timerStateChange.start();
     }
 
-    Vx = 8.0;
-    Vy = 8.0;
+    Vx = 5.0;
+    Vy = 5.0;
 
     PID_controller();
     motorControl();
@@ -1177,6 +1177,11 @@
         global_curr_state = global_wait;
         global_state_changed = true;
         set_led_color('o'); // Disable demo mode LED
+        
+        motor1_ref = motor1_init;
+        motor1_angle = motor1_init;
+        motor2_ref = motor2_init;
+        motor2_angle = motor2_init;
     }
 }