Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 39:80b38a8e1787
- Parent:
- 38:07cb4ae6c1bd
- Child:
- 40:22aede3d096f
--- a/main.cpp Tue Nov 29 00:31:18 2016 +0000 +++ b/main.cpp Sat Dec 03 17:49:02 2016 +0000 @@ -83,17 +83,29 @@ } float get_torque_cmd(float throttle, float w) { + float tq; if (TORQUE_MODE) { - return throttle * FORWARD_TORQUE_MAX; + tq = throttle * FORWARD_TORQUE_MAX; } else { - return get_driving_torque_cmd(throttle, w); + tq = get_driving_torque_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) { - torque = fabsf(torque); - get_mtpa_dq(torque, d, 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; @@ -108,12 +120,10 @@ update_velocity(); p = pos.GetElecPosition() - POS_OFFSET; + float sin_p = sinf(p); + float cos_p = cosf(p); float torque = get_torque_cmd(throttle_in.get_throttle(), w); - get_dq(torque, w, &d_ref, &q_ref); - - float sin_p = sinf(p); - float cos_p = cosf(p); 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; @@ -127,6 +137,8 @@ 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); + float d_err = d_ref - d_filtered; float q_err = q_ref - q_filtered; @@ -142,14 +154,6 @@ vd = constrain(vd, -1.0f, 1.0f); vq = constrain(vq, -1.0f, 1.0f); - /* - float v = sqrtf(vd * vd + vq * vq); - - if (v > 1.0f) { - vq = sqrtf(1.0f - vd * vd); - } - */ - if (!control_enabled) { vd = 0.0f; vq = 0.0f;