Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 36:cac9785c91cb
- Parent:
- 35:89385f64c867
- Child:
- 37:ba7ebf4f8a78
diff -r 89385f64c867 -r cac9785c91cb main.cpp --- a/main.cpp Wed Nov 23 02:40:31 2016 +0000 +++ b/main.cpp Sun Nov 27 09:10:09 2016 +0000 @@ -92,10 +92,13 @@ //fill in d, q ref based on torque cmd and current velocity void get_dq(float torque, float w, float *d, float *q) { + /* *d = 0.0f; *q = torque / KT; if (*q > IPHASE_MOTOR_LIMIT) *q = IPHASE_MOTOR_LIMIT; if (*q > IPHASE_INVERTER_LIMIT) *q = IPHASE_INVERTER_LIMIT; + */ + /* torque = fabsf(torque); get_mtpa_dq(torque, d, q); @@ -103,7 +106,11 @@ *d = -*d; *q = -*q; } - */ + */ + + float is = torque / KT; + *d = (-FLUX_LINKAGE + sqrtf(FLUX_LINKAGE * FLUX_LINKAGE + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq)); + *q = sqrtf(is * is - *d * *d); } void commutate() { @@ -145,16 +152,16 @@ vd = KP * d_err + d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE; vq = KP * q_err + q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE; - //vd = constrain(vd, -1.0f, 1.0f); - //vq = constrain(vq, -1.0f, 1.0f); + 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) { - //vd /= v; - //vq /= v; vq = sqrtf(1.0f - vd * vd); } + */ if (!control_enabled) { vd = 0.0f;