Jared DiCarlo / Mbed 2 deprecated george

motor controller

Dependencies:   mbed plotter

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?

UserRevisionLine numberNew 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 }