#include "Settings.h"
#define MATH_TYPE 1 // this will define float to float and _IQ(x) to x, so use true float-point registers
#include <IQmathLib.h>
#include <park.h> // Include header for the PARK object
#include <ipark.h> // Include header for the IPARK object
#include <clarke.h> // Include header for the CLARKE object
#include <rampgen.h> // Include header for the RAMPGEN object
#include <rmp_cntl.h> // Include header for the RMPCNTL object
#include <svgen.h> // Include header for the SVGENDQ object
#include <pi.h> // Include header for the PIDREG3 object
#include <speed_fr.h> // Include header for the SPEED_MEAS_QEP object
#include <pid_grando.h>
#undef _iq
// die xxx_DEFAULTS und xxx_MACRO brauchen das Makro noch
#include "DLOG_4CH.h"
inline float frac(float x) {return x-int(x);}
#ifdef CPU1
# define EPwmURegs EPwm1Regs
# define EPwmVRegs EPwm2Regs
# define EPwmWRegs EPwm3Regs
#else
# define EPwmURegs EPwm4Regs
# define EPwmVRegs EPwm5Regs
# define EPwmWRegs EPwm6Regs
#endif
namespace nsMOTOR{
// Define the electrical motor parametes (Estun Servomotor)
const float RS=2.35; // Stator resistance (ohm)
//#define RR // Rotor resistance (ohm)
const float LS=0.0065; // Stator inductance (H)
//#define LR // Rotor inductance (H)
//#define LM // Magnatizing inductance (H)
const float FREQ = 200; // Base electrical frequency (Hz)
const int POLES = 8; // Number of poles
}
bool RunMotor;
// Instance a ramp controller to smoothly ramp the frequency
MONITOR RMPCNTL rc1 = RMPCNTL_DEFAULTS; // RampDelayMax u.a. !=0
static IPARK ipark1; // alles 0
static PARK park1; // alles 0
// Instance a Space Vector PWM modulator. This modulator generates a, b and c
// phases based on the d and q stationery reference frame inputs
MONITOR SVGEN svgen1; // alles 0
// Instance a few transform objects
static CLARKE clarke1; // alles 0
MONITOR float VdTesting = 0.25; // Vd reference (pu)
MONITOR float VqTesting = 0.125;// Vq reference (pu)
static float IdRef; // Id reference (pu)
static float IqRef = BUILDLEVEL==3 ? 0 : 0.05; // Iq reference (pu)
MONITOR float SpeedRef = 0.25; // For Closed Loop tests
static float cntr,alignCnt = 20000;
static float IdRef_start = 0.1, IdRef_run;
static Currentsense current_sensor[3];
static PID_CONTROLLER pid_spd = {PID_TERM_DEFAULTS, PID_PARAM_DEFAULTS, PID_DATA_DEFAULTS};
static PI_CONTROLLER pi_id = PI_CONTROLLER_DEFAULTS;
static PI_CONTROLLER pi_iq = PI_CONTROLLER_DEFAULTS;
// Instance a ramp generator to simulate an Anglele
MONITOR RAMPGEN rg1 = RAMPGEN_DEFAULTS; // Gain und Offset = 1
// Instance a speed calculator based on Encoder position
static SPEED_MEAS_QEP speed1 = SPEED_MEAS_QEP_DEFAULTS;
// ****************************************************************************
// Variables for Datalog module
// ****************************************************************************
MONITOR float DBUFF_4CH1[200], DBUFF_4CH2[200], DBUFF_4CH3[200], DBUFF_4CH4[200];
static DLOG_4CH<float> dlog; // Create an instance of data logger
// ******************************************************************************
// CURRENT SENSOR SUITE
// - Read motor current from inverter bottom leg Shunt
// - Read motor current from LEM current sensors
// - Read motor current from shunts with AMC1204 chips
// ******************************************************************************
RAMFUNC static void currentSensorSuite() {
if (SENSES & bit(SHUNT_CURRENT_SENSE))
current_sensor[SHUNT_CURRENT_SENSE].readShunt();
if (SENSES & bit(LEM_CURRENT_SENSE))
current_sensor[LEM_CURRENT_SENSE].readLEM();
if (SENSES & bit(SD_CURRENT_SENSE))
current_sensor[SD_CURRENT_SENSE].readSDFM();
}
/***********************************
* POSITION LOOP UTILITY FUNCTIONS *
***********************************/
// slew programmable ramper
RAMFUNC static void ramp(float&cur, float dst, float rampDelta) {
float err = dst - cur;
if (err > rampDelta) {cur+=rampDelta; return;}
if (err < -rampDelta) {cur-=rampDelta; return;}
cur=dst;
}
// ----------------------------------------------------------------------------
// Alignment Routines: these routines aligns the motor to zero electrical angle
// and in case of QEP also find the index location and initializes the angle
// w.r.t. the index location
// ----------------------------------------------------------------------------
RAMFUNC static void Alignment2() {
if (!RunMotor) {lsw = 0; return;}
if (lsw) return;
// for restarting from (RunMotor = 0)
rc1.TargetValue = rc1.SetpointValue = 0;
lsw = POSITION_ENCODER==QEP_ENCODER ? 1 : 2;
// for QEP, spin the motor to find the index pulse
// for absolute encoders no need for lsw=1
}
RAMFUNC static void Alignment3() {
if(!RunMotor) {
lsw = 0;
pi_id.ui = 0;
pi_iq.ui = 0;
return;
}
if (lsw) return;
// alignment current
IdRef = IdRef_start; //0.1;
// for restarting from (RunMotor = 0)
rc1.TargetValue = rc1.SetpointValue = 0;
// set up an alignment and hold time for shaft to settle down
if (pi_id.Ref >= IdRef) {
if (cntr < alignCnt) cntr++;
else{
if (POSITION_ENCODER==QEP_ENCODER) {
lsw = 1; // for QEP, spin the motor to find the index pulse
IqRef = 0.05;
}else lsw = 2; // for absolute encoders no need for lsw=1
cntr = 0;
IdRef = IdRef_run;
}
}
}
RAMFUNC static void Alignment4() {
if (!RunMotor) {lsw = 0; return;}
if (lsw) return;
// alignment current
IdRef = IdRef_start; //0.1;
// for restarting from (RunMotor = 0)
rc1.TargetValue = rc1.SetpointValue = 0;
// set up an alignment and hold time for shaft to settle down
if (pi_id.Ref >= IdRef) {
if (cntr < alignCnt) cntr++;
else{
cntr = 0;
IdRef = IdRef_run;
if (POSITION_ENCODER==QEP_ENCODER) {
lsw = 1; // for QEP, spin the motor to find the index pulse
IqRef = 0.05;
}else lsw = 2; // for absolute encoders no need for lsw=1
}
}
}
RAMFUNC static float RampGen(float speed) {
// ----------------------------------------------------------------------------
// Connect inputs of the RMP module and call the ramp control macro
// ----------------------------------------------------------------------------
rc1.TargetValue = speed;
RC_MACRO(rc1)
// ----------------------------------------------------------------------------
// Connect inputs of the RAMP GEN module and call the ramp generator macro
// ----------------------------------------------------------------------------
rg1.Freq = rc1.SetpointValue;
RG_MACRO(rg1)
return rg1.Out;
}
// ------------------------------------------------------------------------------
// Measure phase currents, subtract the offset and normalize from (-0.5,+0.5) to (-1,+1).
// Connect inputs of the CLARKE module and call the clarke transformation macro
// ------------------------------------------------------------------------------
RAMFUNC static void SenseAndClarke() {
currentSensorSuite();
clarke1.As = current_sensor[CURRENT_TAKE].As; // Phase A curr.
clarke1.Bs = current_sensor[CURRENT_TAKE].Bs; // Phase B curr.
CLARKE_MACRO(clarke1)
}
// ----------------------------------------------------------------------------
// Connect inputs of the PARK module and call the park trans. macro
// There are two option for trigonometric functions:
// IQ sin/cos look-up table provides 512 discrete sin and cos points in Q30 format
// IQsin/cos PU functions interpolate the data in the lookup table yielding higher resolution.
// ----------------------------------------------------------------------------
RAMFUNC static void Park1_SetSinCos(float angle) {
park1.Angle = angle;
park1.Sine = __sinpuf32(angle);
park1.Cosine = __cospuf32(angle);
}
RAMFUNC static void Park1(float angle) {
park1.Alpha = clarke1.Alpha;
park1.Beta = clarke1.Beta;
Park1_SetSinCos(angle);
PARK_MACRO(park1)
}
// ----------------------------------------------------------------------------
// Connect inputs of the INV_PARK module and call the inverse park trans. macro
// ----------------------------------------------------------------------------
RAMFUNC static void Ipark1(float d, float q) {
ipark1.Ds = d;
ipark1.Qs = q;
ipark1.Sine=park1.Sine;
ipark1.Cosine=park1.Cosine;
IPARK_MACRO(ipark1)
}
// ----------------------------------------------------------------------------
// Connect inputs of the SVGEN_DQ module and call the space-vector gen. macro
// ----------------------------------------------------------------------------
RAMFUNC static void Svgen1AndPwm1() {
svgen1.Ualpha = ipark1.Alpha;
svgen1.Ubeta = ipark1.Beta;
SVGENDQ_MACRO(svgen1)
// Write computed duties to CMPA registers; Ta,Tb,Tc im Bereich -1..1
const int MIDDLE = F_CPU/2/2/F_PWM/2;
// Halbierungen: 1. ePWM-Eingangsteiler auf 100 MHz
// 2. wegen phasenrichtiger PWM (Auf- und Abwärtszähler)
// 3. wegen Mittelwert
EPwmURegs.CMPA.bit.CMPA = int(MIDDLE*svgen1.Ta)+MIDDLE;
EPwmVRegs.CMPA.bit.CMPA = int(MIDDLE*svgen1.Tb)+MIDDLE;
EPwmWRegs.CMPA.bit.CMPA = int(MIDDLE*svgen1.Tc)+MIDDLE;
}
//-----------------------------------------------------------------------------
// Variable display on PWM-DAC
//-----------------------------------------------------------------------------
RAMFUNC static void logDAC(float dacA, float dacB) {
// DacaRegs.DACVALS.bit.DACVALS = dacB*4096;
// DacbRegs.DACVALS.bit.DACVALS = dacC*4096;
}
MONITOR Uint32 IsrTicker;
MONITOR Uint16 Amc1210Data[8];
RAMFUNC static interrupt void MotorControlISR() {
EINT;
GpioDataRegs.GPACLEAR.all = bit(31); // GPIO31 auf Low — LED ein
IsrTicker++; // Verifying the ISR
static const Uint16 SpeedLoopPrescaler = 10; // Speed loop pre scalar
static Uint16 SpeedLoopCount = 1; // Speed loop counter
switch (BUILDLEVEL) {
// =============================== LEVEL 1 ====================================
// Checks target independent modules, duty cycle waveforms and PWM update
// Keep the motors disconnected at this level
// ============================================================================
case 1: {
// control flow: ramp 0..1, Ipark1, Svgen1AndPwm1
amc1210.recv(0x1D,Amc1210Data,elemof(Amc1210Data)); // SPI beobachten; 80×8 CPU-Takte
float out=RampGen(SpeedRef);
Park1_SetSinCos(out);
Ipark1(VdTesting,VqTesting);
Svgen1AndPwm1();
// logging helpers
dlog(out,svgen1.Ta,svgen1.Tb,svgen1.Tc);
logDAC(svgen1.Tb*0.5+0.5,svgen1.Tc*0.5+0.5);
amc1210.wait(); // warte bis fertig + FIFO auslesen
}break;
// =============================== LEVEL 2 ====================================
// verifies
// - all current sense schems
// - analog-to-digital conversion (shunt and LEM)
// - SDFM function
// - Current Limit Settings for over current protection
// - clarke/park transformations (CLARKE/PARK)
// - Position sensor interface
// - speed estimation
// ============================================================================
case 2: {
Alignment2();
float out=RampGen(lsw ? SpeedRef : 0);
SenseAndClarke();
Park1(out);
Ipark1(VdTesting,VqTesting);
posEncoderSuite(); // if needed reverse the sense of position in this module
// ------------------------------------------------------------------------------
// Connect inputs of the SPEED_FR module and call the speed calculation macro
// ------------------------------------------------------------------------------
speed1.ElecTheta = posEncElecTheta;
SPEED_FR_MACRO(speed1)
Svgen1AndPwm1();
dlog(out,svgen1.Ta,clarke1.As,clarke1.Bs);
logDAC(out,posEncElecTheta);
}break;
// =============================== LEVEL 3 ====================================
// verifies the dq-axis current regulation performed by PID and speed
// measurement modules
// lsw=0: lock the rotor of the motor
// lsw=1: close the current loop
// NOTE: Iq loop is closed using internal ramp angle as position feedback.
// Therefore, motor speed does not race high with lighter load. User's
// wanting to use actual rotor angle should ensure that the test value
// for Iq reference will not race the motor to high speeds.
// In other words, to use the actual angle, a loaded motor is needed.
// ============================================================================
case 3: {
Alignment3();
float out=RampGen(lsw ? SpeedRef : 0);
SenseAndClarke();
posEncoderSuite(); // if needed reverse the sense of position in this module
speed1.ElecTheta = posEncElecTheta;
SPEED_FR_MACRO(speed1)
Park1(lsw ? out : 0); // lsw==1 exists only for QEP
// ------------------------------------------------------------------------------
// Connect inputs of the PID_REG3 module and call the PID IQ controller macro
// ------------------------------------------------------------------------------
pi_iq.Ref = lsw ? IqRef : 0;
pi_iq.Fbk = park1.Qs;
PI_MACRO(pi_iq)
// ------------------------------------------------------------------------------
// Connect inputs of the PI module and call the PID ID controller macro
// ------------------------------------------------------------------------------
ramp(pi_id.Ref,IdRef,0.00001);
pi_id.Fbk = park1.Ds;
PI_MACRO(pi_id)
Ipark1(pi_id.Out,pi_iq.Out);
Svgen1AndPwm1();
dlog(posEncElecTheta,
out,
clarke1.As,
clarke1.Bs);
logDAC(out,posEncElecTheta);
}break;
// =============================== LEVEL 4 ======================================
// Level 4 verifies the speed regulator performed by PID module.
// The system speed loop is closed by using the measured speed as feedback
// lsw=0: lock the rotor of the motor
// lsw=1: - needed only with QEP encoders until first index pulse
// - Loops shown for lsw=2 are closed in this stage
// lsw=2: close speed loop and current loops Id, Iq
// ==============================================================================
case 4: {
Alignment4();
float out=RampGen(lsw ? SpeedRef : 0);
SenseAndClarke();
posEncoderSuite(); // if needed reverse the sense of position in this module
// ------------------------------------------------------------------------------
// Connect inputs of the SPEED_FR module and call the speed calculation macro
// ------------------------------------------------------------------------------
speed1.ElecTheta = posEncElecTheta;
SPEED_FR_MACRO(speed1)
Park1(lsw ? posEncElecTheta : 0);
// ------------------------------------------------------------------------------
// Connect inputs of the PI module and call the PID speed controller macro
// ------------------------------------------------------------------------------
if (SpeedLoopCount==SpeedLoopPrescaler) {
pid_spd.term.Ref = rc1.SetpointValue; //SpeedRef;
pid_spd.term.Fbk = speed1.Speed;
PID_MACRO(pid_spd);
SpeedLoopCount = 1;
}else SpeedLoopCount++;
if (!lsw || lsw==1) {
pid_spd.data.d1 = 0; pid_spd.data.d2 = 0; pid_spd.data.i1 = 0;
pid_spd.data.ud = 0; pid_spd.data.ui = 0; pid_spd.data.up = 0;
}
// ------------------------------------------------------------------------------
// Connect inputs of the PI module and call the PID IQ controller macro
// ------------------------------------------------------------------------------
if (!lsw) {
pi_iq.Ref = 0;
pi_iq.ui = 0;
}else pi_iq.Ref = lsw==1 ? IqRef : pid_spd.term.Out;
pi_iq.Fbk = park1.Qs;
PI_MACRO(pi_iq)
// ------------------------------------------------------------------------------
// Connect inputs of the PI module and call the PID ID controller macro
// ------------------------------------------------------------------------------
ramp(pi_id.Ref,IdRef,0.00001);
pi_id.Fbk = park1.Ds;
PI_MACRO(pi_id)
Ipark1(pi_id.Out,pi_iq.Out);
Svgen1AndPwm1();
dlog(posEncElecTheta,svgen1.Ta,clarke1.As,clarke1.Bs);
logDAC(out,posEncElecTheta);
}break;
}/*switch(BUILDLEVEL)*/
dlog(); // DATALOG update function.
EPwm11Regs.ETCLR.bit.INT = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP3;
GpioDataRegs.GPASET.all = bit(31); // GPIO31 auf High — LED aus
}
// PWM Configuration
inline void configPWM(volatile EPWM_REGS®s, Uint16 period, Uint16 db, Uint16 slave_phase) {
// Time Base SubModule Registers
TBCTL_REG tbctl = {0};
tbctl.bit.CTRMODE = TB_COUNT_UPDOWN; // 10: Auf- und abwärts (für phasenrichtige PWM)
if (slave_phase) {
tbctl.bit.PHSEN = TB_ENABLE;
tbctl.bit.PHSDIR = slave_phase >= period>>1 ? TB_DOWN : TB_UP;
}else tbctl.bit.SYNCOSEL = TB_CTR_ZERO; // 01: sync "down-stream" when counter == 0
tbctl.bit.FREE_SOFT = 0b10; // 1x: Continue running at emulation events
regs.TBCTL.all = tbctl.all;
regs.TBPRD = period>>1; // PWM frequency = 1 / period
// Action Qualifier SubModule Registers
AQCTLA_REG aqctla = {0};
aqctla.bit.CAD = AQ_SET;
aqctla.bit.CAU = AQ_CLEAR;
regs.AQCTLA.all = aqctla.all;
// Active low complementary (ALC) PWMs - Set up deadband
DBCTL_REG dbctl = {0};
dbctl.bit.OUT_MODE = DB_FULL_ENABLE;
dbctl.bit.POLSEL = DB_ACTV_LOC;
regs.DBCTL.all = dbctl.all;
regs.DBRED = db;
regs.DBFED = db;
regs.TBPHS.bit.TBPHS = slave_phase > period>>1 ? period-slave_phase : slave_phase;
}
void initMotorControl() {
// Initialize PWM generators and outputs used
// By default the PWM clock is divided by 2, as maximum ePWM speed is 100 MHz
// ClkCfgRegs.PERCLKDIVSEL.bit.EPWMCLKDIV=1
// Deadband needs to be 2 µs => 10 ns * 200 = 2 µs
const int INV_PWM_TICKS=F_CPU/2/F_PWM;
// switch GPIO1..12 to PWM outputs (GPADIR not necessary)
configPWM(EPwmURegs,INV_PWM_TICKS,200,0);
configPWM(EPwmVRegs,INV_PWM_TICKS,200,2+INV_PWM_TICKS*1/3);
configPWM(EPwmWRegs,INV_PWM_TICKS,200,2+INV_PWM_TICKS*2/3);
#ifdef CPU1
GpioCtrlRegs.GPAMUX1.all |= // switch GPIO1..12 to be PWM outputs (GPADIR not necessary)
1UL<< 0 | 1UL<< 2 | 1UL<< 4 | 1UL<< 6 | 1UL<< 8 | 1UL<<10
| 1UL<<12 | 1UL<<14 | 1UL<<16 | 1UL<<18 | 1UL<<20 | 1UL<<22;
#endif
// Initialize DATALOG
dlog.init();
dlog.output[0] = DBUFF_4CH1;
dlog.output[1] = DBUFF_4CH2;
dlog.output[2] = DBUFF_4CH3;
dlog.output[3] = DBUFF_4CH4;
dlog.size = 200;
dlog.triggered = true;
dlog.pre_scalar = 5;
dlog.trig_value = 0.01;
const float T=1/F_ISR; // Abtastrate in Sa/s
static const float PI=3.14159265358979;
// Initialize the RAMPGEN module
rg1.StepAngleMax = nsMOTOR::FREQ*T;
// Initialize the Speed module for speed calculation from QEP/RESOLVER
speed1.K1 = 1/(nsMOTOR::FREQ*T);
speed1.K2 = 1/(1+T*2*PI*5); // Low-pass cut-off frequency
speed1.K3 = 1-speed1.K2;
speed1.BaseRpm = 120*(nsMOTOR::FREQ/nsMOTOR::POLES);
if (BUILDLEVEL>=4) {
// Initialize the PID module for speed
pid_spd.param.Kp = 1.0;
pid_spd.param.Ki = 0.001;
pid_spd.param.Kd = 0;
pid_spd.param.Kr = 1.0;
pid_spd.param.Umax = 0.95;
pid_spd.param.Umin = -0.95;
}
if (BUILDLEVEL>=3) {
// Init PI module for ID loop
pi_id.Kp = 1.0; //3.0;
pi_id.Ki = T/0.04; //0.0075);
pi_id.Umax = 0.5;
pi_id.Umin = -0.5;
// Init PI module for IQ loop
pi_iq.Kp = 1.0;//4.0;
pi_iq.Ki = T/0.04;//0.015;
pi_iq.Umax = 0.8;
pi_iq.Umin = -0.8;
}
// Interrupt-Initialisierung
//PWM11 INT is used to trigger Motor Control ISR
configPwm(EPwm11Regs,F_CPU/2/F_ISR,2);
EPwm11Regs.ETSEL.bit.INTSEL = ET_CTRU_CMPA; // INT on PRD event
EPwm11Regs.ETSEL.bit.INTEN = 1; // Enable INT
EPwm11Regs.ETPS.bit.INTPRD = ET_1ST; // Generate INT on every event
PieVectTable.EPWM11_INT = MotorControlISR;
PieCtrlRegs.PIEIER3.bit.INTx11 = 1; // Enable PWM11INT in PIE group 3
IER |= M_INT3; // Enable group 3 interrupts
}
Detected encoding: UTF-8 | 0
|