Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 131:031df63c7dbc
- Parent:
- 130:639cd8586f86
- Child:
- 132:101b74e4763a
--- a/main.cpp Sun Apr 30 08:53:22 2017 +0000 +++ b/main.cpp Mon May 01 04:04:52 2017 +0000 @@ -85,10 +85,15 @@ control.d_integral += d_err * KI_D; control.q_integral += q_err * KI_Q; - constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, 1.0f); + constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, INTEGRAL_MAX); + + foc.vd_decouple = -Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f; + foc.vq_decouple = Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f; - foc.vd = KP_D * d_err + control.d_integral;// + Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f; - foc.vq = KP_Q * q_err + control.q_integral;// - Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f; + constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f); + + foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple; + foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple; constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f); @@ -155,9 +160,9 @@ } void log() { - io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), - (int) (255 * foc.vd), (int) (255 * foc.vq)); - //io.pc->printf("%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_filtered, (int) control.q_filtered, (int) (255 * foc.vd), (int) (255 * foc.vq)); + //io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), + // (int) (255 * foc.vd), (int) (255 * foc.vq)); + io.pc->printf("%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_filtered, (int) control.q_filtered, (int) (255 * foc.vd_decouple), (int) (255 * foc.vq_decouple)); wait(1.0f / LOG_FREQUENCY); } @@ -180,7 +185,7 @@ } int main() { - dq = new LutMapper(); + dq = new DirectMapper(-82.0f, 22.0f);//LutMapper(); th = new NullThrottleMapper(); BREMSInit(&io, &read, &foc, &control, false);