Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 40:22aede3d096f
- Parent:
- 39:80b38a8e1787
- Child:
- 42:030e0ec4eac5
--- a/main.cpp Sat Dec 03 17:49:02 2016 +0000 +++ b/main.cpp Mon Dec 12 14:18:02 2016 +0000 @@ -6,7 +6,7 @@ #include "PwmIn.h" #include "MathHelpers.h" #include "Driving.h" -#include "Optimize.h" +#include "LutOptimize.h" #include "config_motor.h" #include "config_loop.h" @@ -77,39 +77,25 @@ if (dp_mech < -PI) dp_mech += 2 * PI; if (dp_mech > PI) dp_mech -= 2 * PI; float w_raw = dp_mech * F_SW; //rad/s - if (w_raw > W_LIMIT) w_raw = w; //with this limiting scheme noise < 0 - if (w_raw < -W_LIMIT) w_raw = w; //so we need to throw out the large deltas first + if (w_raw > W_CRAZY) w_raw = w; //with this limiting scheme noise < 0 + if (w_raw < -W_CRAZY) w_raw = w; //so we need to throw out the large deltas first w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw; } -float get_torque_cmd(float throttle, float w) { +/*map throttle to percent max torque*/ +float get_tqpct_cmd(float throttle, float w) { float tq; if (TORQUE_MODE) { - tq = throttle * FORWARD_TORQUE_MAX; + tq = throttle; } else { - tq = get_driving_torque_cmd(throttle, w); + tq = get_driving_tqpct_cmd(throttle, w); } return tq; } -//fill in d, q ref based on torque cmd and current velocity -void get_dq(float torque, float w, float *d, float *q) { - float tqabs = fabsf(torque); - - /*get voltage limits and estimate current voltage*/ - float vs_max2 = (BUS_VOLTAGE * MODULATION_INDEX / 2.0f) * (BUS_VOLTAGE * MODULATION_INDEX / 2.0f); - float vs2 = get_vs2(w, d_filtered, q_filtered); - - if (vs2 < vs_max2) { - get_mtpa_dq(tqabs, d, q); - } else { - get_mtpf_dq(tqabs, w, vs_max2, d, q); - } - - if (torque < 0.0f) { - *d = -*d; - *q = -*q; - } +/*get d, q based on % max torque command and velocity*/ +void get_dq(float torque_percent, float w, float *d, float *q) { + get_dq_lut(torque_percent, w, d, q); } void commutate() { @@ -123,7 +109,7 @@ float sin_p = sinf(p); float cos_p = cosf(p); - float torque = get_torque_cmd(throttle_in.get_throttle(), w); + float torque_percent = get_tqpct_cmd(throttle_in.get_throttle(), w); ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE; ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE; @@ -137,7 +123,7 @@ d_filtered = DQ_FILTER_STRENGTH * d_filtered + (1.0f - DQ_FILTER_STRENGTH) * d; q_filtered = DQ_FILTER_STRENGTH * q_filtered + (1.0f - DQ_FILTER_STRENGTH) * q; - get_dq(torque, w, &d_ref, &q_ref); + get_dq(torque_percent, w, &d_ref, &q_ref); float d_err = d_ref - d_filtered; float q_err = q_ref - q_filtered;