#include "Settings.h"
#include "MotorControl.h"
#include "DLOG_4CH.h"
#include "regdef2.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 (Ω)
//#define RR // Rotor resistance (Ω)
const float LS=0.0065; // Stator inductance (H)
//#define LR // Rotor inductance (H)
//#define LM // Magnetizing 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 mc::Rmpcntl rc1; // RampDelayMax u.a. !=0
static mc::Ipark ipark1; // alles 0
static mc::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 mc::Svgen svgen1; // alles 0
// Instance a few transform objects
static mc::Clarke clarke1; // alles 0
MONITOR float VdTesting = 0.25; // Vd reference
MONITOR float VqTesting = 0.125;// Vq reference
static float IdRef; // Id reference
static float IqRef = BUILDLEVEL==3 ? 0 : 0.05; // Iq reference
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 mc::PidController pid_spd;
static mc::PiController pi_id;
static mc::PiController pi_iq;
// Instance a ramp generator to simulate an Anglele
MONITOR mc::Rampgen rg1; // Gain und Offset = 1
// Instance a speed calculator based on Encoder position
static mc::SpeedMeasQep speed1;
// ****************************************************************************
// 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) {PosEnc::lsw = 0; return;}
if (PosEnc::lsw) return;
// for restarting from (RunMotor = 0)
rc1.TargetValue = rc1.SetpointValue = 0;
PosEnc::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) {
PosEnc::lsw = 0;
//pi_id.ui = 0;
//pi_iq.ui = 0;
return;
}
if (PosEnc::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) {
PosEnc::lsw = 1; // for QEP, spin the motor to find the index pulse
IqRef = 0.05;
}else PosEnc::lsw = 2; // for absolute encoders no need for lsw=1
cntr = 0;
IdRef = IdRef_run;
}
}
}
RAMFUNC static void Alignment4() {
if (!RunMotor) {PosEnc::lsw = 0; return;}
if (PosEnc::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) {
PosEnc::lsw = 1; // for QEP, spin the motor to find the index pulse
IqRef = 0.05;
}else PosEnc::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 functor
// ----------------------------------------------------------------------------
rc1(speed);
// ----------------------------------------------------------------------------
// Connect inputs of the RAMP GEN module and call the ramp generator functor
// ----------------------------------------------------------------------------
return rg1(rc1.SetpointValue);
}
// ------------------------------------------------------------------------------
// 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.
clarke1.run2();
}
// ----------------------------------------------------------------------------
// 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(float angle) {
park1.Alpha = clarke1.Alpha;
park1.Beta = clarke1.Beta;
park1.setSinCos(angle);
park1();
}
// ----------------------------------------------------------------------------
// Connect inputs of the INV_PARK module and call the inverse park transform functor
// ----------------------------------------------------------------------------
RAMFUNC static void Ipark1(float d, float q) {
ipark1(d,q,park1);
}
// ----------------------------------------------------------------------------
// Connect inputs of the SVGEN_DQ module and call the space-vector generator functor
// ----------------------------------------------------------------------------
RAMFUNC static void Svgen1AndPwm1() {
svgen1(ipark1);
// 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;
unsigned Amc1210Okay[4];
unsigned Amc1210IR,Amc1210SR; // Interrupt-Register, Status-Register (immer OR-verknüpft)
int Amc1210Data[4]; // 4 Ergebnis-Register (ohne Zeitstempel)
static bool check(Amc1210::data16_t&recvdat) {
for (int i=0; i<4; i++) if (recvdat[i].timestamp>=256) return false; // Huch, jetzt kommen plötzlich kleine Zeitstempel!
return true;
}
MONITOR DataLog<int,4,160,1> logger; // triggert auf Kanal 1 (von 0 ab gezählt)
MONITOR DataLog<unsigned,1,200> logadc[3]; // ADC (12 Bit = 0..4095)
// Grafisch im CCS-Debugger zu beobachten mit logadc[0].buf, logadc[1].buf und logadc[2].buf
// (geht leider nicht in einem Graf zusammen; blöd!!)
RAMFUNC static interrupt void MotorControlISR() {
EINT;
GpioDataRegsA.doCLEAR(31);
// 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: {
Amc1210IR|=Amc1210::recv(0); // synchrone Version
// control flow: ramp 0..1, Ipark1, Svgen1AndPwm1
struct{
uint16_t status; // Statusregister
Amc1210::data16_t data; // Daten und Zeitstempel
}recvdat;
Amc1210::recv(0x1C,&recvdat,sizeof recvdat); // SPI beobachten; 80×9 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);
for(int versuch=0;;) {
Amc1210::wait(); // warte bis fertig + FIFO auslesen
if (check(recvdat.data)) {
Amc1210Okay[versuch]++;
break;
}
if (++versuch==4) break;
Amc1210::recv(0x1C,&recvdat,sizeof recvdat); // nochmal versuchen
}
Amc1210SR|=recvdat.status;
for (int i=0; i<4; i++) Amc1210Data[i]=recvdat.data[i].value;
logger.put(Amc1210Data);
for (int i=0; i<3; i++) logadc[i].putaddr()[0]=AdcResultRegsA[i][5];
}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(PosEnc::lsw ? SpeedRef : 0);
SenseAndClarke();
Park1(out);
Ipark1(VdTesting,VqTesting);
PosEnc::Suite(); // if needed reverse the sense of position in this module
// ------------------------------------------------------------------------------
// Connect inputs of the SPEED_FR module and call the speed calculation macro
// ------------------------------------------------------------------------------
speed1(PosEnc::ElecTheta);
Svgen1AndPwm1();
dlog(out,svgen1.Ta,clarke1.As,clarke1.Bs);
logDAC(out,PosEnc::ElecTheta);
}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(PosEnc::lsw ? SpeedRef : 0);
SenseAndClarke();
PosEnc::Suite(); // if needed reverse the sense of position in this module
speed1(PosEnc::ElecTheta);
Park1(PosEnc::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 = PosEnc::lsw ? IqRef : 0;
pi_iq(park1.Qs);
// ------------------------------------------------------------------------------
// Connect inputs of the PI module and call the PID ID controller macro
// ------------------------------------------------------------------------------
ramp(pi_id.Ref,IdRef,0.00001);
pi_id(park1.Ds);
Ipark1(pi_id.Out,pi_iq.Out);
Svgen1AndPwm1();
dlog(PosEnc::ElecTheta,
out,
clarke1.As,
clarke1.Bs);
logDAC(out,PosEnc::ElecTheta);
}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(PosEnc::lsw ? SpeedRef : 0);
SenseAndClarke();
PosEnc::Suite(); // if needed reverse the sense of position in this module
// ------------------------------------------------------------------------------
// Connect inputs of the SPEED_FR module and call the speed calculation macro
// ------------------------------------------------------------------------------
speed1(PosEnc::ElecTheta);
Park1(PosEnc::lsw ? PosEnc::ElecTheta : 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(speed1.Speed);
SpeedLoopCount = 1;
}else SpeedLoopCount++;
if (!PosEnc::lsw || PosEnc::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
// ------------------------------------------------------------------------------
float ref=0;
if (PosEnc::lsw) ref = PosEnc::lsw==1 ? IqRef : pid_spd.term.Out;
pi_iq(park1.Qs,ref);
// ------------------------------------------------------------------------------
// Connect inputs of the PI module and call the PID ID controller macro
// ------------------------------------------------------------------------------
ramp(pi_id.Ref,IdRef,0.00001);
pi_id(park1.Ds);
Ipark1(pi_id.Out,pi_iq.Out);
Svgen1AndPwm1();
dlog(PosEnc::ElecTheta,svgen1.Ta,clarke1.As,clarke1.Bs);
logDAC(out,PosEnc::ElecTheta);
}break;
}/*switch(BUILDLEVEL)*/
dlog(); // DATALOG update function.
EPwm11Regs.ETCLR.bit.INT = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP3;
GpioDataRegsA.doSET(31);
// 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.U.max = 0.95;
pid_spd.param.U.min = -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.U.max = 0.5;
pi_id.U.min = -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.U.max = 0.8;
pi_iq.U.min = -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
|