Jared DiCarlo
/
george
motor controller
vehicle_controller.cpp@2:7312ac02785d, 2016-10-30 (annotated)
- Committer:
- dicarloj
- Date:
- Sun Oct 30 19:37:46 2016 +0000
- Revision:
- 2:7312ac02785d
- Child:
- 3:08746709f023
working, no safety
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 | 2:7312ac02785d | 4 | #include <math.h> |
dicarloj | 2:7312ac02785d | 5 | |
dicarloj | 2:7312ac02785d | 6 | float current_control(float throttle) |
dicarloj | 2:7312ac02785d | 7 | { |
dicarloj | 2:7312ac02785d | 8 | return throttle * K_Q_CURRENT_MAX; |
dicarloj | 2:7312ac02785d | 9 | } |
dicarloj | 2:7312ac02785d | 10 | |
dicarloj | 2:7312ac02785d | 11 | volatile float debug_1 = 0; |
dicarloj | 2:7312ac02785d | 12 | volatile float debug_2 = 0; |
dicarloj | 2:7312ac02785d | 13 | |
dicarloj | 2:7312ac02785d | 14 | float get_debug(int num){return num == 1 ? debug_1 : debug_2;} |
dicarloj | 2:7312ac02785d | 15 | float filtered_velocity = 0; |
dicarloj | 2:7312ac02785d | 16 | float alpha = .3; |
dicarloj | 2:7312ac02785d | 17 | float vK_I = 0.001; |
dicarloj | 2:7312ac02785d | 18 | float vel_int = 0; |
dicarloj | 2:7312ac02785d | 19 | float vK_P = 0.01; |
dicarloj | 2:7312ac02785d | 20 | float velocity_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 | 2:7312ac02785d | 25 | //printf("velocity %f \n\r", filtered_velocity); |
dicarloj | 2:7312ac02785d | 26 | float v_err = -velocity + throttle * 500; |
dicarloj | 2:7312ac02785d | 27 | vel_int += v_err; |
dicarloj | 2:7312ac02785d | 28 | if (v_err < 1) v_err = 0; |
dicarloj | 2:7312ac02785d | 29 | debug_1 = velocity; |
dicarloj | 2:7312ac02785d | 30 | debug_2 = v_err; |
dicarloj | 2:7312ac02785d | 31 | return v_err/10;// + vel_int * vK_I; |
dicarloj | 2:7312ac02785d | 32 | } |