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
diff -r d0d65376140f -r 8d8a98ec2904 main.cpp --- 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 );