![](/media/cache/profiles/916954106a13204fa4909ae2d48e0602.jpg.50x50_q85.jpg)
motor controller
vehicle_controller.cpp
- Committer:
- dicarloj
- Date:
- 2016-11-06
- Revision:
- 7:ed19a937daa0
- Parent:
- 4:4e00b310811d
File content as of revision 7:ed19a937daa0:
#include "vehicle_controller.h" #include "config.h" #include "io.h" #include "plotter.h" #include <math.h> float filtered_velocity = 0; float alpha = .3; float vel_int = 0; float filtered_throttle = 0; float throt_alpha = .002; //simple current controller float current_control(float throttle) { return (throttle * alpha + filtered_throttle * (1 - alpha)) * K_Q_CURRENT_MAX; } //servo controller float servo_control(float throttle) { float velocity = get_velocity(); velocity = isnan(velocity) ? 0 : velocity; filtered_velocity = alpha * velocity + (1 - alpha) * filtered_velocity; float v_err = -filtered_velocity + throttle * 500; vel_int += v_err; return v_err * K_SERVO_KP + vel_int * K_SERVO_KI; } float voltage_control(float throttle) { float velocity = get_velocity(); velocity = isnan(velocity) ? 0 : velocity; //plot(0, velocity); filtered_velocity = alpha * velocity + (1 - alpha) * filtered_velocity; float v_err = -filtered_velocity + throttle * 500; vel_int += v_err; if(v_err < 0) v_err = 0; //plot(1, v_err); return v_err * K_VOLTAGE_KP; }