void CTRL_setParams(CTRL_Handle handle,USER_Params *pUserParams)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
_iq Kp,Ki,Kd;
_iq outMin,outMax;
_iq maxModulation;
MATH_vec2 Iab_out_pu = {_IQ(0.0),_IQ(0.0)};
MATH_vec2 Idq_out_pu = {_IQ(0.0),_IQ(0.0)};
MATH_vec2 Idq_ref_pu = {_IQ(0.0),_IQ(0.0)};
MATH_vec2 Vab_in_pu = {_IQ(0.0),_IQ(0.0)};
MATH_vec2 Vab_out_pu = {_IQ(0.0),_IQ(0.0)};
MATH_vec2 Vdq_out_pu = {_IQ(0.0),_IQ(0.0)};
// assign the motor type
CTRL_setMotorParams(handle,pUserParams->motor_type,
pUserParams->motor_numPolePairs,
pUserParams->motor_ratedFlux,
pUserParams->motor_Ls_d,
pUserParams->motor_Ls_q,
pUserParams->motor_Rr,
pUserParams->motor_Rs);
// assign other controller parameters
CTRL_setNumIsrTicksPerCtrlTick(handle,pUserParams->numIsrTicksPerCtrlTick);
CTRL_setNumCtrlTicksPerCurrentTick(handle,pUserParams->numCtrlTicksPerCurrentTick);
CTRL_setNumCtrlTicksPerSpeedTick(handle,pUserParams->numCtrlTicksPerSpeedTick);
CTRL_setNumCtrlTicksPerTrajTick(handle,pUserParams->numCtrlTicksPerTrajTick);
CTRL_setCtrlFreq_Hz(handle,pUserParams->ctrlFreq_Hz);
CTRL_setTrajFreq_Hz(handle,pUserParams->trajFreq_Hz);
CTRL_setTrajPeriod_sec(handle,_IQ(1.0/pUserParams->trajFreq_Hz));
CTRL_setCtrlPeriod_sec(handle,pUserParams->ctrlPeriod_sec);
CTRL_setMaxVsMag_pu(handle,_IQ(pUserParams->maxVsMag_pu));
CTRL_setIab_in_pu(handle,&Iab_out_pu);
CTRL_setIdq_in_pu(handle,&Idq_out_pu);
CTRL_setIdq_ref_pu(handle,&Idq_ref_pu);
CTRL_setIdRated_pu(handle,_IQ(pUserParams->IdRated/pUserParams->iqFullScaleCurrent_A));
CTRL_setVab_in_pu(handle,&Vab_in_pu);
CTRL_setVab_out_pu(handle,&Vab_out_pu);
CTRL_setVdq_out_pu(handle,&Vdq_out_pu);
CTRL_setSpd_out_pu(handle,_IQ(0.0));
CTRL_setRhf(handle,0.0);
CTRL_setLhf(handle,0.0);
CTRL_setRoverL(handle,0.0);
// reset the counters
CTRL_resetCounter_current(handle);
CTRL_resetCounter_isr(handle);
CTRL_resetCounter_speed(handle);
CTRL_resetCounter_state(handle);
CTRL_resetCounter_traj(handle);
// set the wait times for each state
CTRL_setWaitTimes(handle,&pUserParams->ctrlWaitTime[0]);
// set flags
CTRL_setFlag_enablePowerWarp(handle,false);
CTRL_setFlag_enableCtrl(handle,false);
CTRL_setFlag_enableOffset(handle,true);
CTRL_setFlag_enableSpeedCtrl(handle,true);
CTRL_setFlag_enableUserMotorParams(handle,false);
CTRL_setFlag_enableDcBusComp(handle,true);
// initialize the controller error code
CTRL_setErrorCode(handle,CTRL_ErrorCode_NoError);
// set the default controller state
CTRL_setState(handle,CTRL_State_Idle);
// set the number of current sensors
CTRL_setupClarke_I(handle,pUserParams->numCurrentSensors);
// set the number of voltage sensors
CTRL_setupClarke_V(handle,pUserParams->numVoltageSensors);
// set the default Id PID controller parameters
Kp = _IQ(0.1);
Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
Kd = _IQ(0.0);
outMin = _IQ(-0.95);
outMax = _IQ(0.95);
PID_setGains(obj->pidHandle_Id,Kp,Ki,Kd);
PID_setUi(obj->pidHandle_Id,_IQ(0.0));
PID_setMinMax(obj->pidHandle_Id,outMin,outMax);
CTRL_setGains(handle,CTRL_Type_PID_Id,Kp,Ki,Kd);
// set the default the Iq PID controller parameters
Kp = _IQ(0.1);
Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
Kd = _IQ(0.0);
outMin = _IQ(-0.95);
outMax = _IQ(0.95);
PID_setGains(obj->pidHandle_Iq,Kp,Ki,Kd);
PID_setUi(obj->pidHandle_Iq,_IQ(0.0));
PID_setMinMax(obj->pidHandle_Iq,outMin,outMax);
CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp,Ki,Kd);
// set the default speed PID controller parameters
Kp = _IQ(0.02*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz/pUserParams->iqFullScaleCurrent_A);
Ki = _IQ(2.0*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz*pUserParams->ctrlPeriod_sec/pUserParams->iqFullScaleCurrent_A);
Kd = _IQ(0.0);
outMin = _IQ(-1.0);
outMax = _IQ(1.0);
PID_setGains(obj->pidHandle_spd,Kp,Ki,Kd);
PID_setUi(obj->pidHandle_spd,_IQ(0.0));
PID_setMinMax(obj->pidHandle_spd,outMin,outMax);
CTRL_setGains(handle,CTRL_Type_PID_spd,Kp,Ki,Kd);
// set the speed reference
CTRL_setSpd_ref_pu(handle,_IQ(0.0));
// set the default Id current trajectory module parameters
TRAJ_setIntValue(obj->trajHandle_Id,_IQ(0.0));
TRAJ_setTargetValue(obj->trajHandle_Id,_IQ(0.0));
TRAJ_setMinValue(obj->trajHandle_Id,_IQ(0.0));
TRAJ_setMaxValue(obj->trajHandle_Id,_IQ(0.0));
TRAJ_setMaxDelta(obj->trajHandle_Id,_IQ(0.0));
// set the default the speed trajectory module parameters
TRAJ_setIntValue(obj->trajHandle_spd,_IQ(0.0));
TRAJ_setTargetValue(obj->trajHandle_spd,_IQ(0.0));
TRAJ_setMinValue(obj->trajHandle_spd,_IQ(0.0));
TRAJ_setMaxValue(obj->trajHandle_spd,_IQ(0.0));
TRAJ_setMaxDelta(obj->trajHandle_spd,_IQ(0.0));
// set the default maximum speed trajectory module parameters
TRAJ_setIntValue(obj->trajHandle_spdMax,_IQ(0.0));
TRAJ_setTargetValue(obj->trajHandle_spdMax,_IQ(0.0));
TRAJ_setMinValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
TRAJ_setMaxValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
TRAJ_setMaxDelta(obj->trajHandle_spdMax,_IQ(0.0)); // not used
// set the default estimator parameters
CTRL_setEstParams(obj->estHandle,pUserParams);
// set the maximum modulation for the SVGEN module
maxModulation = _IQ(MATH_TWO_OVER_THREE);
SVGEN_setMaxModulation(obj->svgenHandle,maxModulation);
return;
} // end of CTRL_setParams() function
void main(void)
{
uint_least8_t estNumber = 0;
#ifdef FAST_ROM_V1p6
uint_least8_t ctrlNumber = 0;
#endif
// Only used if running from FLASH
// Note that the variable FLASH is defined by the project
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
#endif
// initialize the hardware abstraction layer
halHandle = HAL_init(&hal,sizeof(hal));
// check for errors in user parameters
USER_checkForErrors(&gUserParams);
// store user parameter error in global variable
gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
// do not allow code execution if there is a user parameter error
if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
{
for(;;)
{
gMotorVars.Flag_enableSys = false;
}
}
// initialize the user parameters
USER_setParams(&gUserParams);
// set the hardware abstraction layer parameters
HAL_setParams(halHandle,&gUserParams);
// initialize the controller
#ifdef FAST_ROM_V1p6
ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber); //v1p6 format (06xF and 06xM devices)
controller_obj = (CTRL_Obj *)ctrlHandle;
#else
ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl)); //v1p7 format default
#endif
{
CTRL_Version version;
// get the version number
CTRL_getVersion(ctrlHandle,&version);
gMotorVars.CtrlVersion = version;
}
// set the default controller parameters
CTRL_setParams(ctrlHandle,&gUserParams);
// Initialize and setup the 100% SVM generator
svgencurrentHandle = SVGENCURRENT_init(&svgencurrent,sizeof(svgencurrent));
{
float_t minWidth_microseconds = 2.0;
uint16_t minWidth_counts = (uint16_t)(minWidth_microseconds * USER_SYSTEM_FREQ_MHz / 2.0) + HAL_PWM_DBFED_CNT;
SVGENCURRENT_setMinWidth(svgencurrentHandle, minWidth_counts);
}
// Initialize field weakening
fwHandle = FW_init(&fw,sizeof(fw));
// Disable field weakening
FW_setFlag_enableFw(fwHandle, false);
// Clear field weakening counter
FW_clearCounter(fwHandle);
// Set the number of ISR per field weakening ticks
FW_setNumIsrTicksPerFwTick(fwHandle, FW_NUM_ISR_TICKS_PER_CTRL_TICK);
// Set the deltas of field weakening
FW_setDeltas(fwHandle, FW_INC_DELTA, FW_DEC_DELTA);
// Set initial output of field weakening to zero
FW_setOutput(fwHandle, _IQ(0.0));
// Set the field weakening controller limits
FW_setMinMax(fwHandle,_IQ(USER_MAX_NEGATIVE_ID_REF_CURRENT_A/USER_IQ_FULL_SCALE_CURRENT_A),_IQ(0.0));
// setup faults
HAL_setupFaults(halHandle);
// initialize the interrupt vector table
HAL_initIntVectorTable(halHandle);
// enable the ADC interrupts
HAL_enableAdcInts(halHandle);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// disable the PWM
HAL_disablePwm(halHandle);
// initialize the SpinTAC Components
stHandle = ST_init(&st_obj, sizeof(st_obj));
// setup the SpinTAC Components
ST_setupVelCtl(stHandle);
ST_setupVelMove(stHandle);
#ifdef DRV8301_SPI
// turn on the DRV8301 if present
HAL_enableDrv(halHandle);
// initialize the DRV8301 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
#endif
// enable DC bus compensation
CTRL_setFlag_enableDcBusComp(ctrlHandle, true);
// compute scaling factors for flux and torque calculations
gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();
for(;;)
{
// Waiting for enable system flag to be set
while(!(gMotorVars.Flag_enableSys));
// Dis-able the Library internal PI. Iq has no reference now
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false);
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
ST_Obj *stObj = (ST_Obj *)stHandle;
// increment counters
gCounter_updateGlobals++;
// enable/disable the use of motor parameters being loaded from user.h
CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);
// enable/disable Rs recalibration during motor startup
EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);
// enable/disable automatic calculation of bias values
CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);
if(CTRL_isError(ctrlHandle))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(ctrlHandle,false);
// set the enable system flag to false
gMotorVars.Flag_enableSys = false;
// disable the PWM
HAL_disablePwm(halHandle);
}
else
{
// update the controller state
bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);
// enable or disable the control
CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);
if(flag_ctrlStateChanged)
{
CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);
if(ctrlState == CTRL_State_OffLine)
{
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_OnLine)
{
if(gMotorVars.Flag_enableOffsetcalc == true)
{
// update the ADC bias values
HAL_updateAdcBias(halHandle);
}
else
{
// set the current bias
HAL_setBias(halHandle,HAL_SensorType_Current,0,_IQ(I_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,1,_IQ(I_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,2,_IQ(I_C_offset));
// set the voltage bias
HAL_setBias(halHandle,HAL_SensorType_Voltage,0,_IQ(V_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,1,_IQ(V_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,2,_IQ(V_C_offset));
}
// Return the bias value for currents
gMotorVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0);
gMotorVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1);
gMotorVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2);
// Return the bias value for voltages
gMotorVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0);
gMotorVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1);
gMotorVars.V_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Voltage,2);
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_Idle)
{
// disable the PWM
HAL_disablePwm(halHandle);
gMotorVars.Flag_Run_Identify = false;
}
if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) &&
(ctrlState > CTRL_State_Idle) &&
(gMotorVars.CtrlVersion.minor == 6))
{
// call this function to fix 1p6
USER_softwareUpdate1p6(ctrlHandle);
}
}
}
if(EST_isMotorIdentified(obj->estHandle))
{
_iq Is_Max_squared_pu = _IQ((USER_MOTOR_MAX_CURRENT*USER_MOTOR_MAX_CURRENT)/ \
(USER_IQ_FULL_SCALE_CURRENT_A*USER_IQ_FULL_SCALE_CURRENT_A));
_iq Id_squared_pu = _IQmpy(CTRL_getId_ref_pu(ctrlHandle),CTRL_getId_ref_pu(ctrlHandle));
// Take into consideration that Iq^2+Id^2 = Is^2
Iq_Max_pu = _IQsqrt(Is_Max_squared_pu-Id_squared_pu);
//Set new max trajectory
CTRL_setSpdMax(ctrlHandle, Iq_Max_pu);
//Set the maximum current controller output for the Iq and Id current controllers to enable
//over-modulation.
//An input into the SVM above 2/SQRT(3) = 1.1547 is in the over-modulation region. An input of 1.1547 is where
//the crest of the sinewave touches the 100% duty cycle. At an input of 4/3, the SVM generator
//produces a trapezoidal waveform touching every corner of the hexagon
CTRL_setMaxVsMag_pu(ctrlHandle,gMotorVars.OverModulation);
// set the current ramp
EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
gMotorVars.Flag_MotorIdentified = true;
// set the speed reference
CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);
// set the speed acceleration
CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));
// enable the SpinTAC Speed Controller
STVELCTL_setEnable(stObj->velCtlHandle, true);
if(EST_getState(obj->estHandle) != EST_State_OnLine)
{
// if the estimator is not running, place SpinTAC into reset
STVELCTL_setEnable(stObj->velCtlHandle, false);
// if the estimator is not running, set SpinTAC Move start & end velocity to 0
STVELMOVE_setVelocityEnd(stObj->velMoveHandle, _IQ(0.0));
STVELMOVE_setVelocityStart(stObj->velMoveHandle, _IQ(0.0));
}
if(Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
// initialize the watch window kp and ki current values with pre-calculated values
gMotorVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id);
gMotorVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id);
// initialize the watch window Bw value with the default value
gMotorVars.SpinTAC.VelCtlBw_radps = STVELCTL_getBandwidth_radps(stObj->velCtlHandle);
// initialize the watch window with maximum and minimum Iq reference
gMotorVars.SpinTAC.VelCtlOutputMax_A = _IQmpy(STVELCTL_getOutputMaximum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
gMotorVars.SpinTAC.VelCtlOutputMin_A = _IQmpy(STVELCTL_getOutputMinimum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
}
}
else
{
Flag_Latch_softwareUpdate = true;
// the estimator sets the maximum current slope during identification
gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
}
// when appropriate, update the global variables
if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
{
// reset the counter
gCounter_updateGlobals = 0;
updateGlobalVariables_motor(ctrlHandle, stHandle);
}
// set field weakening enable flag depending on user's input
FW_setFlag_enableFw(fwHandle,gMotorVars.Flag_enableFieldWeakening);
// update Kp and Ki gains
updateKpKiGains(ctrlHandle);
// set the SpinTAC (ST) bandwidth scale
STVELCTL_setBandwidth_radps(stObj->velCtlHandle, gMotorVars.SpinTAC.VelCtlBw_radps);
// set the maximum and minimum values for Iq reference
STVELCTL_setOutputMaximums(stObj->velCtlHandle, _IQmpy(gMotorVars.SpinTAC.VelCtlOutputMax_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)), _IQmpy(gMotorVars.SpinTAC.VelCtlOutputMin_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
// enable/disable the forced angle
EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);
// enable or disable power warp
CTRL_setFlag_enablePowerWarp(ctrlHandle,gMotorVars.Flag_enablePowerWarp);
#ifdef DRV8301_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8301Vars);
HAL_readDrvData(halHandle,&gDrvSpi8301Vars);
#endif
} // end of while(gFlag_enableSys) loop
// disable the PWM
HAL_disablePwm(halHandle);
// set the default controller parameters (Reset the control to re-identify the motor)
CTRL_setParams(ctrlHandle,&gUserParams);
gMotorVars.Flag_Run_Identify = false;
// setup the SpinTAC Components
ST_setupVelCtl(stHandle);
ST_setupVelMove(stHandle);
} // end of for(;;) loop
} // end of main() function
