Jared DiCarlo
/
george
motor controller
vehicle_controller.cpp@7:ed19a937daa0, 2016-11-06 (annotated)
- Committer:
- dicarloj
- Date:
- Sun Nov 06 03:09:28 2016 +0000
- Revision:
- 7:ed19a937daa0
- Parent:
- 4:4e00b310811d
works at 200V
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dicarloj | 2:7312ac02785d | 1 | #include "vehicle_controller.h" |
dicarloj | 2:7312ac02785d | 2 | #include "config.h" |
dicarloj | 2:7312ac02785d | 3 | #include "io.h" |
dicarloj | 4:4e00b310811d | 4 | #include "plotter.h" |
dicarloj | 2:7312ac02785d | 5 | #include <math.h> |
dicarloj | 2:7312ac02785d | 6 | |
dicarloj | 3:08746709f023 | 7 | float filtered_velocity = 0; |
dicarloj | 3:08746709f023 | 8 | float alpha = .3; |
dicarloj | 3:08746709f023 | 9 | float vel_int = 0; |
dicarloj | 7:ed19a937daa0 | 10 | float filtered_throttle = 0; |
dicarloj | 7:ed19a937daa0 | 11 | float throt_alpha = .002; |
dicarloj | 3:08746709f023 | 12 | |
dicarloj | 3:08746709f023 | 13 | //simple current controller |
dicarloj | 2:7312ac02785d | 14 | float current_control(float throttle) |
dicarloj | 2:7312ac02785d | 15 | { |
dicarloj | 7:ed19a937daa0 | 16 | return (throttle * alpha + filtered_throttle * (1 - alpha)) * K_Q_CURRENT_MAX; |
dicarloj | 2:7312ac02785d | 17 | } |
dicarloj | 2:7312ac02785d | 18 | |
dicarloj | 3:08746709f023 | 19 | //servo controller |
dicarloj | 3:08746709f023 | 20 | float servo_control(float throttle) |
dicarloj | 2:7312ac02785d | 21 | { |
dicarloj | 2:7312ac02785d | 22 | float velocity = get_velocity(); |
dicarloj | 2:7312ac02785d | 23 | velocity = isnan(velocity) ? 0 : velocity; |
dicarloj | 2:7312ac02785d | 24 | filtered_velocity = alpha * velocity + (1 - alpha) * filtered_velocity; |
dicarloj | 3:08746709f023 | 25 | float v_err = -filtered_velocity + throttle * 500; |
dicarloj | 2:7312ac02785d | 26 | vel_int += v_err; |
dicarloj | 3:08746709f023 | 27 | return v_err * K_SERVO_KP + vel_int * K_SERVO_KI; |
dicarloj | 3:08746709f023 | 28 | } |
dicarloj | 3:08746709f023 | 29 | |
dicarloj | 3:08746709f023 | 30 | |
dicarloj | 3:08746709f023 | 31 | float voltage_control(float throttle) |
dicarloj | 3:08746709f023 | 32 | { |
dicarloj | 3:08746709f023 | 33 | float velocity = get_velocity(); |
dicarloj | 3:08746709f023 | 34 | velocity = isnan(velocity) ? 0 : velocity; |
dicarloj | 4:4e00b310811d | 35 | //plot(0, velocity); |
dicarloj | 3:08746709f023 | 36 | filtered_velocity = alpha * velocity + (1 - alpha) * filtered_velocity; |
dicarloj | 3:08746709f023 | 37 | float v_err = -filtered_velocity + throttle * 500; |
dicarloj | 3:08746709f023 | 38 | vel_int += v_err; |
dicarloj | 4:4e00b310811d | 39 | if(v_err < 0) v_err = 0; |
dicarloj | 4:4e00b310811d | 40 | //plot(1, v_err); |
dicarloj | 3:08746709f023 | 41 | return v_err * K_VOLTAGE_KP; |
dicarloj | 2:7312ac02785d | 42 | } |