![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
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 );