void setFeLimitZero(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
EST_State_e EstState = EST_getState(obj->estHandle);
_iq fe_neg_max_pu;
_iq fe_pos_min_pu;
if((EST_isMotorIdentified(obj->estHandle) == false) && (CTRL_getMotorType(handle) == MOTOR_Type_Induction))
{
fe_neg_max_pu = _IQ30(0.0);
fe_pos_min_pu = _IQ30(0.0);
}
else
{
fe_neg_max_pu = _IQ30(-USER_ZEROSPEEDLIMIT);
fe_pos_min_pu = _IQ30(USER_ZEROSPEEDLIMIT);
}
EST_setFe_neg_max_pu(obj->estHandle, fe_neg_max_pu);
EST_setFe_pos_min_pu(obj->estHandle, fe_pos_min_pu);
return;
} // end of setFeLimitZero() function
void recalcKpKi(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
EST_State_e EstState = EST_getState(obj->estHandle);
if((EST_isMotorIdentified(obj->estHandle) == false) && (EstState == EST_State_Rs))
{
float_t Lhf = CTRL_getLhf(handle);
float_t Rhf = CTRL_getRhf(handle);
float_t RhfoverLhf = Rhf/Lhf;
_iq Kp = _IQ(0.25*Lhf*USER_IQ_FULL_SCALE_CURRENT_A/(USER_CTRL_PERIOD_sec*USER_IQ_FULL_SCALE_VOLTAGE_V));
_iq Ki = _IQ(RhfoverLhf*USER_CTRL_PERIOD_sec);
// set Rhf/Lhf
CTRL_setRoverL(handle,RhfoverLhf);
// set the controller proportional gains
CTRL_setKp(handle,CTRL_Type_PID_Id,Kp);
CTRL_setKp(handle,CTRL_Type_PID_Iq,Kp);
// set the Id controller gains
CTRL_setKi(handle,CTRL_Type_PID_Id,Ki);
PID_setKi(obj->pidHandle_Id,Ki);
// set the Iq controller gains
CTRL_setKi(handle,CTRL_Type_PID_Iq,Ki);
PID_setKi(obj->pidHandle_Iq,Ki);
}
return;
} // end of recalcKpKi() function
void recalcKpKiPmsm(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
EST_State_e EstState = EST_getState(obj->estHandle);
if((EST_isMotorIdentified(obj->estHandle) == false) && (EstState == EST_State_Rs))
{
float_t Lhf = CTRL_getLhf(handle);
float_t Rhf = CTRL_getRhf(handle);
float_t RhfoverLhf = Rhf/Lhf;
_iq Kp = _IQ(0.05*Lhf*USER_IQ_FULL_SCALE_CURRENT_A/(USER_CTRL_PERIOD_sec*USER_IQ_FULL_SCALE_VOLTAGE_V));
_iq Ki = _IQ(RhfoverLhf*USER_CTRL_PERIOD_sec);
_iq Kp_spd = _IQ(0.005*USER_IQ_FULL_SCALE_FREQ_Hz*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
// set Rhf/Lhf
CTRL_setRoverL(handle,RhfoverLhf);
// set the controller proportional gains
CTRL_setKp(handle,CTRL_Type_PID_Id,Kp);
CTRL_setKp(handle,CTRL_Type_PID_Iq,Kp);
// set the Id controller gains
CTRL_setKi(handle,CTRL_Type_PID_Id,Ki);
PID_setKi(obj->pidHandle_Id,Ki);
// set the Iq controller gains
CTRL_setKi(handle,CTRL_Type_PID_Iq,Ki);
PID_setKi(obj->pidHandle_Iq,Ki);
// set speed gains
PID_setKp(obj->pidHandle_spd,Kp_spd);
}
else if(EstState == EST_State_RatedFlux)
{
_iq Ki_spd = _IQ(0.5*USER_IQ_FULL_SCALE_FREQ_Hz*USER_CTRL_PERIOD_sec*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
// Set Ki on closing the feedback loop
PID_setKi(obj->pidHandle_spd,Ki_spd);
}
else if(EstState == EST_State_RampDown)
{
_iq Kp_spd = _IQ(0.02*USER_IQ_FULL_SCALE_FREQ_Hz*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
_iq Ki_spd = _IQ(2.0*USER_IQ_FULL_SCALE_FREQ_Hz*USER_CTRL_PERIOD_sec*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
// Set Kp and Ki of the speed controller
PID_setKp(obj->pidHandle_spd,Kp_spd);
PID_setKi(obj->pidHandle_spd,Ki_spd);
TRAJ_setTargetValue(obj->trajHandle_spdMax,_IQ(USER_MOTOR_RES_EST_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A));
}
return;
} // end of recalcKpKiPmsm() function
void CTRL_calcMax_Ls_qFmt(CTRL_Handle handle, uint_least8_t *pLs_qFmt)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
if((EST_isMotorIdentified(obj->estHandle) == false) && (EST_getState(obj->estHandle) == EST_State_Rs))
{
float_t Lhf = CTRL_getLhf(handle);
float_t fullScaleInductance = USER_IQ_FULL_SCALE_VOLTAGE_V/(USER_IQ_FULL_SCALE_CURRENT_A*USER_VOLTAGE_FILTER_POLE_rps);
float_t Ls_coarse_max = _IQ30toF(EST_getLs_coarse_max_pu(obj->estHandle));
int_least8_t lShift = ceil(log((Lhf/20.0)/(Ls_coarse_max*fullScaleInductance))/log(2.0));
*pLs_qFmt = 30 - lShift;
}
return;
} // end of CTRL_calcMax_Ls_qFmt() 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);
#ifdef F2802xF
//copy .econst to unsecure RAM
if(*econst_end - *econst_start)
{
memCopy((uint16_t *)&econst_start,(uint16_t *)&econst_end,(uint16_t *)&econst_ram_load);
}
//copy .switch ot unsecure RAM
if(*switch_end - *switch_start)
{
memCopy((uint16_t *)&switch_start,(uint16_t *)&switch_end,(uint16_t *)&switch_ram_load);
}
#endif
#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 the frequency of execution monitoring module
femHandle = FEM_init(&fem,sizeof(fem));
FEM_setParams(femHandle,
USER_SYSTEM_FREQ_MHz * 1000000.0, // timer frequency, Hz
(uint32_t)USER_SYSTEM_FREQ_MHz * 1000000, // timer period, cnts
USER_CTRL_FREQ_Hz, // set point frequency, Hz
1000.0); // max frequency error, Hz
// initialize the CPU usage module
cpu_usageHandle = CPU_USAGE_init(&cpu_usage,sizeof(cpu_usage));
CPU_USAGE_setParams(cpu_usageHandle,
(uint32_t)USER_SYSTEM_FREQ_MHz * 1000000, // timer period, cnts
(uint32_t)USER_ISR_FREQ_Hz); // average over 1 second of ISRs
// setup faults
HAL_setupFaults(halHandle);
// initialize the interrupt vector table
HAL_initIntVectorTable(halHandle);
// enable the ADC interrupts
HAL_enableAdcInts(halHandle);
// reload timer to start running frequency of execution monitoring
HAL_reloadTimer(halHandle,0);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// disable the PWM
HAL_disablePwm(halHandle);
#ifdef DRV8301_SPI
// turn on the DRV8301 if present
HAL_enableDrv(halHandle);
// initialize the DRV8301 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
#endif
#ifdef DRV8305_SPI
// turn on the DRV8305 if present
HAL_enableDrv(halHandle);
// initialize the DRV8305 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8305Vars);
#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();
// disable offsets recalibration by default
gMotorVars.Flag_enableOffsetcalc = false;
for(;;)
{
// Waiting for enable system flag to be set
while(!(gMotorVars.Flag_enableSys));
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
// 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))
{
// 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));
if(Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
}
}
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);
}
// get the maximum delta count observed
gMaxDeltaCntObserved = FEM_getMaxDeltaCntObserved(femHandle);
// check for errors
if(FEM_isFreqError(femHandle))
{
gNumFreqErrors = FEM_getErrorCnt(femHandle);
}
// update CPU usage
updateCPUusage();
// 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
#ifdef DRV8305_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8305Vars);
HAL_readDrvData(halHandle,&gDrvSpi8305Vars);
#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;
} // end of for(;;) loop
} // end of main() 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 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);
#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));
Flag_Latch_softwareUpdate = true;
// Enable the Library internal PI. Iq is referenced by the speed PI now
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
// 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 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));
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);
}
}
else
{
Flag_Latch_softwareUpdate = true;
// initialize the watch window kp and ki values with pre-calculated values
gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);
// 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);
}
// update Kp and Ki gains
updateKpKiGains(ctrlHandle);
// set field weakening enable flag depending on user's input
FW_setFlag_enableFw(fwHandle,gMotorVars.Flag_enableFieldWeakening);
// 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;
} // end of for(;;) loop
} // end of main() 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);
#ifdef F2802xF
//copy .econst to unsecure RAM
if(*econst_end - *econst_start)
{
memCopy((uint16_t *)&econst_start,(uint16_t *)&econst_end,(uint16_t *)&econst_ram_load);
}
//copy .switch ot unsecure RAM
if(*switch_end - *switch_start)
{
memCopy((uint16_t *)&switch_start,(uint16_t *)&switch_end,(uint16_t *)&switch_ram_load);
}
#endif
#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 the Clarke modules
clarkeHandle_I = CLARKE_init(&clarke_I,sizeof(clarke_I));
clarkeHandle_V = CLARKE_init(&clarke_V,sizeof(clarke_V));
// set the number of current sensors
setupClarke_I(clarkeHandle_I,gUserParams.numCurrentSensors);
// set the number of voltage sensors
setupClarke_V(clarkeHandle_V,gUserParams.numVoltageSensors);
#ifdef FAST_ROM_V1p6
estHandle = controller_obj->estHandle;
#else
estHandle = ctrl.estHandle;
#endif
// initialize the inverse Park module
iparkHandle = IPARK_init(&ipark,sizeof(ipark));
// initialize the Park module
parkHandle = PARK_init(&park,sizeof(park));
// initialize the space vector generator module
svgenHandle = SVGEN_init(&svgen,sizeof(svgen));
// 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);
#ifdef DRV8301_SPI
// turn on the DRV8301 if present
HAL_enableDrv(halHandle);
// initialize the DRV8301 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
#endif
#ifdef DRV8305_SPI
// turn on the DRV8305 if present
HAL_enableDrv(halHandle);
// initialize the DRV8305 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8305Vars);
#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));
// Enable the Library internal PI. Iq is referenced by the speed PI now
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
// 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)
{
uint_least16_t cnt;
// update the ADC bias values
HAL_updateAdcBias(halHandle);
// record the current bias
for(cnt=0;cnt<3;cnt++)
gAdcBiasI.value[cnt] = HAL_getBias(halHandle,HAL_SensorType_Current,cnt);
// record the voltage bias
for(cnt=0;cnt<3;cnt++)
gAdcBiasV.value[cnt] = HAL_getBias(halHandle,HAL_SensorType_Voltage,cnt);
gMotorVars.Flag_enableOffsetcalc = false;
}
else
{
uint_least16_t cnt;
// set the current bias
for(cnt=0;cnt<3;cnt++)
HAL_setBias(halHandle,HAL_SensorType_Current,cnt,gAdcBiasI.value[cnt]);
// set the voltage bias
for(cnt=0;cnt<3;cnt++)
HAL_setBias(halHandle,HAL_SensorType_Voltage,cnt,gAdcBiasV.value[cnt]);
}
// 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))
{
// 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));
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 kp and ki values with pre-calculated values
gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);
}
}
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);
}
// update Kp and Ki gains
updateKpKiGains(ctrlHandle);
// 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
#ifdef DRV8305_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8305Vars);
HAL_readDrvData(halHandle,&gDrvSpi8305Vars);
#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;
} // end of for(;;) loop
} // end of main() 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);
// 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);
ST_setupVelPlan(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))
{
// 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);
}
// 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
/****************************************************************************
* motorware main stuff
***************************************************************************/
void task_motorware()
{
for (;;)
{
// Waiting for enable system flag to be set
while (!(gMotorVars.Flag_enableSys))
{
Task_yield();
}
// loop while the enable system flag is true
while (gMotorVars.Flag_enableSys)
{
Task_yield(); //TODO: change this so that an outside timer activates the motorware task when needed
CTRL_Obj *obj = (CTRL_Obj *) ctrlHandle;
// increment counters
gCounter_updateGlobals++;
// enable/disable the use of motor parameters being loaded from user.h
CTRL_setFlag_enableUserMotorParams(ctrlHandle, gMotorVars.Flag_enableUserParams);
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)
{
// update the ADC bias values
HAL_updateAdcBias(halHandle);
// 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))
{
// 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));
if (Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
}
}
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);
}
// 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;
} // end of for(;;) loop
} // end of task_motorware() function
bool CTRL_updateState(CTRL_Handle handle)
{
CTRL_State_e ctrlState = CTRL_getState(handle);
bool flag_enableCtrl = CTRL_getFlag_enableCtrl(handle);
bool stateChanged = false;
if(flag_enableCtrl)
{
uint_least32_t waitTime = CTRL_getWaitTime(handle,ctrlState);
uint_least32_t counter_ctrlState = CTRL_getCount_state(handle);
// check for errors
CTRL_checkForErrors(handle);
if(counter_ctrlState >= waitTime)
{
// reset the counter
CTRL_resetCounter_state(handle);
if(ctrlState == CTRL_State_OnLine)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
_iq Id_target = TRAJ_getTargetValue(obj->trajHandle_Id);
// update the estimator state
bool flag_estStateChanged = EST_updateState(obj->estHandle,Id_target);
if(flag_estStateChanged)
{
// setup the controller
CTRL_setupCtrl(handle);
// setup the trajectory
CTRL_setupTraj(handle);
}
if(EST_isOnLine(obj->estHandle))
{
// setup the estimator for online state
CTRL_setupEstOnLineState(handle);
}
if(EST_isLockRotor(obj->estHandle) ||
(EST_isIdle(obj->estHandle) && EST_isMotorIdentified(obj->estHandle)))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(handle,false);
// set the next controller state
CTRL_setState(handle,CTRL_State_Idle);
}
}
else if(ctrlState == CTRL_State_OffLine)
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OnLine);
}
else if(ctrlState == CTRL_State_Idle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
bool flag_enableUserMotorParams = CTRL_getFlag_enableUserMotorParams(handle);
if(flag_enableUserMotorParams)
{
// initialize the motor parameters using values from the user.h file
CTRL_setUserMotorParams(handle);
}
if(EST_isIdle(obj->estHandle))
{
// setup the estimator for idle state
CTRL_setupEstIdleState(handle);
if(EST_isMotorIdentified(obj->estHandle))
{
if(CTRL_getFlag_enableOffset(handle))
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OffLine);
}
else
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OnLine);
}
}
else
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OffLine);
}
}
else if(EST_isLockRotor(obj->estHandle))
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OnLine);
}
}
} // if(counter_ctrlState >= waitTime) loop
}
else
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
// set the next controller state
CTRL_setState(handle,CTRL_State_Idle);
// set the estimator to idle
if(!EST_isLockRotor(obj->estHandle))
{
if(EST_isMotorIdentified(obj->estHandle))
{
EST_setIdle(obj->estHandle);
}
else
{
EST_setIdle_all(obj->estHandle);
EST_setRs_pu(obj->estHandle,_IQ30(0.0));
}
}
}
// check to see if the state changed
if(ctrlState != CTRL_getState(handle))
{
stateChanged = true;
}
return(stateChanged);
} // end of CTRL_updateState() 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);
// 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_setupVelId(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));
// Enable the Library internal PI. Iq is referenced by the speed PI now
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
// 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);
EST_State_e estState = EST_getState(obj->estHandle);
if(ctrlState == CTRL_State_OffLine)
{
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_OnLine)
{
if((estState < EST_State_LockRotor) || (estState > EST_State_MotorIdentified))
{
// update the ADC bias values
HAL_updateAdcBias(halHandle);
}
// 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))
{
// 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));
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 kp and ki values with pre-calculated values
gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);
// initializes the value for GoalSpeed and TorqueRampTime
gMotorVars.SpinTAC.VelIdGoalSpeed_krpm = _IQmpy(STVELID_getGoalSpeed(stObj->velIdHandle), _IQ(ST_SPEED_KRPM_PER_PU));
gMotorVars.SpinTAC.VelIdTorqueRampTime_sec = STVELID_getTorqueRampTime_sec(stObj->velIdHandle);
}
}
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);
}
if(gMotorVars.Flag_enableUserParams == true)
{
// update Kp and Ki gains
updateKpKiGains(ctrlHandle);
}
else
{
if(CTRL_getMotorType(ctrlHandle) == MOTOR_Type_Induction)
{
// recalculate Kp and Ki gains to fix the R/L limitation of 2000.0, and Kp limit to 0.11
recalcKpKi(ctrlHandle);
// set electrical frequency limit to zero while identifying an induction motor
setFeLimitZero(ctrlHandle);
// calculate Dir_qFmt for acim motors
acim_Dir_qFmtCalc(ctrlHandle);
}
else
{
// recalculate Kp and Ki gains to fix the R/L limitation of 2000.0, and Kp limit to 0.11
// as well as recalculates gains based on estimator state to allow low inductance pmsm to id
recalcKpKiPmsm(ctrlHandle);
// calculate an Ls qFmt that allows ten times smaller inductance compared to Lhf
CTRL_calcMax_Ls_qFmt(ctrlHandle, &gMax_Ls_qFmt);
gLs_pu = EST_getLs_d_pu(obj->estHandle);
gLs_qFmt = EST_getLs_qFmt(obj->estHandle);
}
}
// 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_setupVelId(stHandle);
} // end of for(;;) loop
} // end of main() function