Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 31:ebe42589ab9d
- Parent:
- 30:c25c5bf0d951
- Child:
- 32:b31423041c4e
--- a/main.cpp Mon Nov 07 13:25:35 2016 +0000 +++ b/main.cpp Thu Nov 10 14:43:25 2016 +0000 @@ -5,8 +5,8 @@ #include "FastPWM.h" #include "PwmIn.h" #include "MathHelpers.h" - -#include "driving.h" +#include "Driving.h" +#include "Optimize.h" #include "config_motor.h" #include "config_loop.h" @@ -95,6 +95,14 @@ *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_ipm_dq(torque, d, q); + if (torque < 0.0f) { + *d = -*d; + *q = -*q; + } + */ } void commutate() { @@ -133,11 +141,18 @@ q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX); d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX); - 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 = 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); - 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; + } if (!control_enabled) { vd = 0.0f;