Jared DiCarlo / Mbed 2 deprecated george

motor controller

Dependencies:   mbed plotter

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?

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