Luccas Eagles
/
CONTROL_MOTOR_MELODY
added melody to old motorcontol
Diff: main.cpp
- Revision:
- 21:302d9043cb4b
- Parent:
- 20:4371ed979fbf
- Child:
- 22:af49b9e79d9c
diff -r 4371ed979fbf -r 302d9043cb4b main.cpp --- a/main.cpp Fri Mar 13 15:00:35 2020 +0000 +++ b/main.cpp Fri Mar 13 17:35:19 2020 +0000 @@ -304,23 +304,25 @@ - position_error = target_position - current_position; - previous_position_error = position_error; - derivative_position = position_error - previous_position_error; + position_error = target_position - (current_position/6); float velocity_out = get_velocity_out(position_error); + float position_out = (6*position_error) + (40*derivative_position); + derivative_position = position_error - previous_position_error; + previous_position_error = position_error; + float motor_out = get_motor_out(velocity_out); - update_lead(velocity_out); + update_lead(position_out); MotorPWM.period(pwm_period); MotorPWM.write(motor_out); if(i > 10) { - pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", velocity, current_position, motor_out, velocity_out, lead); + pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", velocity, (current_position/6), motor_out, derivative_position, lead); i = 0; } i++;