// ============================================================================= // foc3ph.cpp - Field-Oriented Control + SVPWM for a QSPICE C-Block // ============================================================================= // // PURPOSE // Closed-loop FOC controller for the Alonso/Budiono BLDC macromodel driving // a 3-phase 2-level inverter. Replaces the open-loop spwm3ph/svpwm3ph // blocks. Regulates d/q currents with PI + decoupling + EMF feedforward, // optional outer speed loop, min/max zero-sequence injection (SVPWM), // carrier compare and dead-time insertion — i.e., everything an MCU's // FOC ISR + ePWM/TIM1 peripheral would do. // // The control law executes DISCRETELY once per carrier period (at the // carrier valley, where phase current equals its ripple average for // center-aligned PWM) — just like a real 20kHz ISR. Between samples the // stored duty commands are compared to the carrier continuously. // // BUILD (Digital Mars C++, QSPICE default toolchain): // dmc -mn -WD foc3ph.cpp kernel32.lib // // SYMBOL PORT ORDER (pins MUST be in this order; it fixes data[] indices): // in : ref (analog) SPEED_MODE=1: speed ref [rpm as volts] // SPEED_MODE=0: iq ref [A as volts] // in : ia (analog) phase A current [A as volts] (see wiring notes) // in : ib (analog) phase B current [A as volts] // in : theta_m (analog) mechanical rotor angle [rad], from BLDC theta_m pin // in : w_rpm (analog) mechanical speed [rpm], from BLDC w_rpm pin // out : AH AL BH BL CH CL (digital) gate commands, legs A/B/C // // WIRING THE CURRENT SENSE (C-Block pins read node VOLTAGES only): // Insert 0V sources in phases A and B between inverter and motor: // Vsa phaseA_inv phaseA_mot 0 // Vsb phaseB_inv phaseB_mot 0 // Then convert current -> voltage with behavioral sources: // Bia ia_node 0 V=I(Vsa) // Bib ib_node 0 V=I(Vsb) // and wire ia_node/ib_node to this block's ia/ib pins. // (ic is reconstructed internally as -(ia+ib).) // // ANGLE CONVENTION / ALIGNMENT: // The Alonso model generates ea from the ELECTRICAL angle // theta_e = pp * theta_m, with ea's fundamental ~ sin(theta_e). // This block uses the matching sine-aligned Park transform, so with // THETA_OFFSET = 0, positive iq produces positive (motoring) torque. // PP below MUST equal the model's pp parameter. If you ever see torque // of the wrong sign or poor Nm/A, sweep THETA_OFFSET (+/- pi/2 first). // ============================================================================= #include #include // ----------------------------- CONFIGURATION --------------------------------- // --- PWM / gate stage (same semantics as your svpwm3ph.cpp) --- #define FSW 20000.0 // switching / ISR frequency [Hz] #define DT_RISE 100.0e-9 // dead time before HIGH side on [s] #define DT_FALL 100.0e-9 // dead time before LOW side on [s] #define GATE_ACTIVE_HIGH 1 #define CARRIER_OVERSAMPLE 100 // sim steps per carrier period // --- Plant parameters (MUST match schematic: motor + any series RL) --- #define VDC 60.0 // DC bus [V] #define RS 0.008 // per-phase resistance [ohm] (motor only!) #define LS 25.0e-6 // per-phase inductance [H] (motor only!) #define PP 4.0 // pole pairs — must equal BLDC pp #define KV 0.118 // BLDC model Kv: EMF flat-top [V/(rad/s mech)] #define B1_TRAP 1.2158 // fundamental coefficient of the 120deg trapezoid // lambda_m referred to ELECTRICAL rad/s, using EMF fundamental: #define LAMBDA_M (KV * B1_TRAP / PP) // = 0.0359 V/(rad/s elec) // Torque constant with sinusoidal currents: Kt = 1.5*PP*LAMBDA_M = 1.5*B1*KV #define KT_FOC (1.5 * PP * LAMBDA_M) // = 0.215 Nm/A(peak) // --- Current loops (BW ~ 1.5 kHz = fsw/13; Kp = L*wc, Ki = R*wc) --- #define WC_I (6.2832 * 1500.0) #define KP_I (LS * WC_I) // ~0.236 V/A #define KI_I (RS * WC_I) // ~75 V/(A s) #define IMAX 300.0 // phase current limit [A peak] -> ~64.5 Nm max // --- Outer speed loop (SPEED_MODE=1) --- #define SPEED_MODE 1 // 1: 'ref' pin is rpm. 0: 'ref' pin is iq [A] #define KP_W 1.46 // A per (rad/s mech) error (~50 Hz BW, J=1e-3) #define KI_W 92.0 // A per (rad mech) error #define EMF_FF 1 // 1: add w_e*LAMBDA_M feedforward to vq #define THETA_OFFSET 0.0 // [rad elec] alignment trim if needed #define TWO_PI 6.283185307179586 #define SQRT3_INV 0.5773502691896258 union uData { bool b; char c; unsigned char uc; short s; unsigned short us; int i; unsigned int ui; float f; double d; long long int i64; unsigned long long int ui64; char *str; unsigned char *bytes; }; int __stdcall DllMain(void *module, unsigned int reason, void *reserved) { return 1; } #undef ref #undef ia #undef ib #undef theta_m #undef w_rpm #undef AH #undef AL #undef BH #undef BL #undef CH #undef CL // ----------------------------------------------------------------------------- // Per-instance state: controller integrators, latched duties, dead-time machine // ----------------------------------------------------------------------------- struct InstanceData { // discrete controller long long nPer; // index of the carrier period last serviced by the "ISR" double intD, intQ; // current-loop integrators [V] double intW; // speed-loop integrator [A] double duty[3]; // latched duty commands 0..1, held for one full period // gate stage int cmd[3]; double tEdge[3]; }; static double carrier(double t) // triangle 0->1->0, valley at period start { double ph = t * FSW; ph -= floor(ph); return (ph < 0.5) ? (2.0 * ph) : (2.0 - 2.0 * ph); } static double clampd(double x, double lo, double hi) { return (x < lo) ? lo : ((x > hi) ? hi : x); } // ============================================================================= extern "C" __declspec(dllexport) void foc3ph(void **opaque, double t, union uData *data) { double ref = data[0].d; double ia = data[1].d; double ib = data[2].d; double theta_m = data[3].d; double w_rpm = data[4].d; bool &AH = data[5].b; bool &AL = data[6].b; bool &BH = data[7].b; bool &BL = data[8].b; bool &CH = data[9].b; bool &CL = data[10].b; struct InstanceData *st = (struct InstanceData *)(*opaque); if (!st) { st = (struct InstanceData *)malloc(sizeof(struct InstanceData)); st->nPer = -1; st->intD = st->intQ = st->intW = 0.0; st->duty[0] = st->duty[1] = st->duty[2] = 0.5; // both legs at 50% = 0V avg st->cmd[0] = st->cmd[1] = st->cmd[2] = 0; st->tEdge[0] = st->tEdge[1] = st->tEdge[2] = t; *opaque = st; } // ================= DISCRETE CONTROL: run once per carrier period ========== long long n = (long long)floor(t * FSW); if (n != st->nPer) { st->nPer = n; const double Ts = 1.0 / FSW; // --- angles & speed ---------------------------------------------------- double the = PP * theta_m + THETA_OFFSET; // electrical angle [rad] the -= TWO_PI * floor(the / TWO_PI); double sn = sin(the), cs = cos(the); double s2 = sin(the - TWO_PI/3.0), c2 = cos(the - TWO_PI/3.0); double s3 = sin(the + TWO_PI/3.0), c3 = cos(the + TWO_PI/3.0); double wm = w_rpm * TWO_PI / 60.0; // mech rad/s double we = wm * PP; // elec rad/s // --- measured currents -> dq (sine-aligned, amplitude-invariant) ------- double ic = -(ia + ib); double id = (2.0/3.0) * (ia*cs + ib*c2 + ic*c3); double iq = (2.0/3.0) * (ia*sn + ib*s2 + ic*s3); // --- references --------------------------------------------------------- double iqRef; #if SPEED_MODE double wRef = ref * TWO_PI / 60.0; // rpm -> rad/s mech double ew = wRef - wm; iqRef = KP_W * ew + st->intW; // anti-windup: integrate only when the speed loop's output isn't clipped if (fabs(iqRef) < IMAX) st->intW += KI_W * ew * Ts; iqRef = clampd(iqRef, -IMAX, IMAX); #else iqRef = clampd(ref, -IMAX, IMAX); #endif double idRef = 0.0; // no field weakening (yet) // --- PI current loops with decoupling + EMF feedforward ----------------- double ed = idRef - id; double eq = iqRef - iq; double vd = KP_I * ed + st->intD - we * LS * iq; double vq = KP_I * eq + st->intQ + we * LS * id; #if EMF_FF vq += we * LAMBDA_M; #endif // --- voltage vector limit (linear SVPWM region) + anti-windup ----------- double vmax = 0.95 * VDC * SQRT3_INV; // ~32.9 V double vmag = sqrt(vd*vd + vq*vq); if (vmag > vmax) // scale back, freeze integ { double k = vmax / vmag; vd *= k; vq *= k; } else // only integrate unsaturated { st->intD += KI_I * ed * Ts; st->intQ += KI_I * eq * Ts; } // --- inverse Park -> phase references, normalized to +/-1 --------------- double kn = 2.0 / VDC; // r = 1 <-> VDC/2 phase peak double r0 = kn * (vd*cs + vq*sn); double r1 = kn * (vd*c2 + vq*s2); double r2 = kn * (vd*c3 + vq*s3); // --- SVPWM min/max zero-sequence injection ------------------------------ double mx = r0; if (r1 > mx) mx = r1; if (r2 > mx) mx = r2; double mn = r0; if (r1 < mn) mn = r1; if (r2 < mn) mn = r2; double vcm = -0.5 * (mx + mn); r0 += vcm; r1 += vcm; r2 += vcm; // DPWM1: clamp the phase of largest magnitude to its nearest rail (+/-1) // double a0 = fabs(r0), a1 = fabs(r1), a2 = fabs(r2); // double vcm; // if (a0 >= a1 && a0 >= a2) vcm = (r0 > 0.0) ? (1.0 - r0) : (-1.0 - r0); // else if (a1 >= a2) vcm = (r1 > 0.0) ? (1.0 - r1) : (-1.0 - r1); // else vcm = (r2 > 0.0) ? (1.0 - r2) : (-1.0 - r2); // r0 += vcm; r1 += vcm; r2 += vcm; st->duty[0] = clampd(0.5 * (1.0 + r0), 0.0, 1.0); st->duty[1] = clampd(0.5 * (1.0 + r1), 0.0, 1.0); st->duty[2] = clampd(0.5 * (1.0 + r2), 0.0, 1.0); } // =========================================================================== // --- carrier compare + dead-time machine (continuous, like the hardware) --- double carr = carrier(t); bool gH[3], gL[3]; for (int k = 0; k < 3; k++) { int cmd = (st->duty[k] > carr) ? 1 : 0; if (cmd != st->cmd[k]) { st->cmd[k] = cmd; st->tEdge[k] = t; } double held = t - st->tEdge[k]; gH[k] = (cmd == 1) && (held >= DT_RISE); gL[k] = (cmd == 0) && (held >= DT_FALL); } #if GATE_ACTIVE_HIGH AH = gH[0]; AL = gL[0]; BH = gH[1]; BL = gL[1]; CH = gH[2]; CL = gL[2]; #else AH = !gH[0]; AL = !gL[0]; BH = !gH[1]; BL = !gL[1]; CH = !gH[2]; CL = !gL[2]; #endif } // ============================================================================= extern "C" __declspec(dllexport) double MaxExtStepSize(void **opaque, double t) { double step = 1.0 / (FSW * CARRIER_OVERSAMPLE); double dtmin = 1e30; if (DT_RISE > 0 && DT_RISE < dtmin) dtmin = DT_RISE; if (DT_FALL > 0 && DT_FALL < dtmin) dtmin = DT_FALL; if (dtmin < 1e30) { double s = dtmin / 4.0; if (s < step) step = s; } return step; } extern "C" __declspec(dllexport) void Destroy(void **opaque) { if (*opaque) { free(*opaque); *opaque = 0; } }