Diff: main.cpp

Revision:

58:8d8a98ec2904

Parent:

57:d0d65376140f

Child:

59:57b9d3076930

--- a/main.cpp	Fri Nov 01 00:05:58 2019 +0000
+++ b/main.cpp	Fri Nov 01 00:14:59 2019 +0000
@@ -148,7 +148,7 @@
         case 'w':
             curr_led_color = white;
             break;
-        
+
     }
     led_color_changed = true;
 }
@@ -686,8 +686,10 @@
     u_p1 = Kp * motor1_error;
 
     //Integral part
-    error_integral1 = error_integral1 + ei1 * Ts;
-    u_i1 = Ki * error_integral1;
+    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY ) {
+        error_integral1 = error_integral1 + ei1 * Ts;
+        u_i1 = Ki * error_integral1;
+    }
 
     //Derivative part
     float error_derivative1 = (motor1_error - e_prev1) / Ts;
@@ -717,8 +719,10 @@
     u_p2 = Kp * motor2_error;
 
     //Integral part
-    error_integral2 = error_integral2 + ei2 * Ts;
-    u_i2 = Ki * error_integral2;
+    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY ) {
+        error_integral2 = error_integral2 + ei2 * Ts;
+        u_i2 = Ki * error_integral2;
+    }
 
     //Derivative part
     float error_derivative2 = (motor2_error - e_prev2) / Ts;
@@ -1021,7 +1025,7 @@
     PID_controller();
     motorControl();
     RKI();
-    
+
     scope.set(0, motor2_ref * rad2deg );
     scope.set(1, motor2_angle * rad2deg );
     //scope.set(2, motor2_ref );