Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 30:c25c5bf0d951
- Parent:
- 29:50e6e4e46580
- Child:
- 31:ebe42589ab9d
diff -r 50e6e4e46580 -r c25c5bf0d951 main.cpp --- a/main.cpp Mon Nov 07 11:22:25 2016 +0000 +++ b/main.cpp Mon Nov 07 13:25:35 2016 +0000 @@ -6,6 +6,8 @@ #include "PwmIn.h" #include "MathHelpers.h" +#include "driving.h" + #include "config_motor.h" #include "config_loop.h" #include "config_pins.h" @@ -23,7 +25,7 @@ int adval1, adval2; float ia, ib, ic, alpha, beta, d, q, vd, vq, p; -float p_mech, last_p_mech, w; +float p_mech, last_p_mech, w = 0.0f; float d_filtered = 0.0f, q_filtered = 0.0f; float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV) @@ -80,7 +82,11 @@ } float get_torque_cmd(float throttle, float w) { - return throttle * FORWARD_TORQUE_MAX; + if (TORQUE_MODE) { + return throttle * FORWARD_TORQUE_MAX; + } else { + return get_driving_torque_cmd(throttle, w); + } } //fill in d, q ref based on torque cmd and current velocity @@ -127,17 +133,17 @@ q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX); d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX); - if(control_enabled) { - vd = KP * d_err + d_integral; - vq = KP * q_err + q_integral; - } else { - vd = 0; - vq = 0; - } + vd = KP * d_err + d_integral - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE * (float) (1e-6); + vq = KP * q_err + q_integral + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE * (float) (1e-6); vd = constrain(vd, -1.0f, 1.0f); vq = constrain(vq, -1.0f, 1.0f); + if (!control_enabled) { + vd = 0.0f; + vq = 0.0f; + } + float valpha = vd * cos_p - vq * sin_p; float vbeta = vd * sin_p + vq * cos_p;