int
main (void)
{
HAL_init();
/* Set LED port pin to output */
GPIOSetDir (LED_PORT, LED_BIT, 1);
GPIOSetValue (LED_PORT, LED_BIT, 0);
/* Create the queue. */
xQueue = xQueueCreate (mainQUEUE_LENGTH, sizeof (unsigned long));
if (xQueue != NULL)
{
/* Start the two tasks as described in the accompanying application
note. */
xTaskCreate (prvQueueReceiveTask, (signed char *) "Rx",
configMINIMAL_STACK_SIZE, NULL,
mainQUEUE_RECEIVE_TASK_PRIORITY, NULL);
xTaskCreate (prvQueueSendTask, (signed char *) "TX",
configMINIMAL_STACK_SIZE, NULL,
mainQUEUE_SEND_TASK_PRIORITY, NULL);
/* Start the tasks running. */
vTaskStartScheduler ();
}
/* If all is well we will never reach here as the scheduler will now be
running. If we do reach here then it is likely that there was insufficient
heap available for the idle task to be created. */
for (;;);
}
void Alo_Main(multiboot_data *multibootdata) {
HAL_init(multibootdata);
terminal_putstring("Hello to Alo 2!\n");
terminal_putstring(" Just testing newlines....\n");
terminal_printf("-13 when put through itoa is: %d.", -13);
terminal_printf("My name is %s.", "Alex");
terminal_printf("Today is %s %d, %d. It is a %s. It is %d:%d:%d military time.", GetMonth(), GetDayInMonth(), GetYear(), GetWeekDay(), GetHour(), GetMinute(), GetSecond());
/*testtimer.limit = 100;
testtimer.func = OneSecondHandler;
secondtimer.limit = 200;
secondtimer.func = TheResponse;
Timer_Register_Timer(&testtimer);
Timer_Register_Timer(&secondtimer); */
/* int test = 8/(6 - (3 * 2)); */
};
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
/** robot robot initialization function */
void robot_init() {
HAL_init(); //> Initialize hardware abstraction layer
//initialize_sensorlib(); //> Initialize sensor library
}
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) {
// initialize the hardware abstraction layer
halHandle = HAL_init(&hal, sizeof(hal));
// check for errors in user parameters
USER_checkForErrors(&gUserParams);
// initialize the user parameters
USER_setParams(&gUserParams);
// set the hardware abstraction layer parameters
HAL_setParams(halHandle, &gUserParams);
// setup faults
HAL_setupFaults(halHandle);
// yo these are important
// HAL_initIntVectorTable(halHandle);
// HAL_enableAdcInts(halHandle);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// enable SPI A slave and SPI interrupts
// HAL_setupSpiAslave(halHandle);
// HAL_enableSpiInt(halHandle);
//
// // get dis pwm rippin shit up yo
// perspection_pwm_1a_init(halHandle, 40.0);
// perspection_pwm_1b_init(halHandle, 40.0);
// init_duty_cycle_filter();
//
// // ayyyyydc
// perspection_adc_init(halHandle);
// calibrate
// calibrate_zero_torque();
// reset shit
haptics_duty_cycle = 0.0;
for (;;) {
HAL_turnLedOn(halHandle, (GPIO_Number_e)HAL_Gpio_LED2);
// uint32_t rawPosition = HAL_getQepPosnCounts(halHandle);
// double position = (double) (360 * rawPosition) / (double) HAL_getQepPosnMaximum(halHandle);
//
// double desired_torque = torque_from_position(position);
// if(desired_torque < -1.0) {
// desired_torque = -1.0;
// } else if(desired_torque > 1.0) {
// desired_torque = 1.0;
// }
//
// adjust_duty_cycle(desired_torque);
HAL_turnLedOff(halHandle, (GPIO_Number_e)HAL_Gpio_LED2);
}
}
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) {
// 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);
// initialize the user parameters
USER_setParams(&gUserParams);
// set the hardware abstraction layer parameters
HAL_setParams(halHandle, &gUserParams);
// setup faults
HAL_setupFaults(halHandle);
// yo these are important
HAL_initIntVectorTable(halHandle);
HAL_enableAdcInts(halHandle);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// disable the gimbal PWMs to get their interrupts
HAL_disablePwm(halHandle);
// enable SPI A slave and SPI interrupts
HAL_enableSpiInt(halHandle);
HAL_writeSpiSlaveData(halHandle, 30); // Just some junk for the TX buffer, probably don't need
// ayyyydc
perspection_adc_init(halHandle);
// Start up a timer to calculate angular velocity
HAL_startTimer(halHandle, 2);
// reset shit
haptics_duty_cycle = 0.0;
isSystemEnabled = false;
lastPosition = 0.0;
for (;;) {
// HAL_turnLedOn(halHandle, (GPIO_Number_e)HAL_Gpio_LED2);
processSpiMessages();
double position = (360.0 * (double)HAL_getQepPosnCounts(halHandle)) / (double) HAL_getQepPosnMaximum(halHandle);
uint32_t timeDiff = 0xFFFFFFFF - HAL_readTimerCnt(halHandle, 2);
HAL_reloadTimer(halHandle, 2);
double velocity = (position - lastPosition) / ((double)timeDiff / ((double)gUserParams.systemFreq_MHz * 1000000.0));
double desired_torque = torque_from_motion(position, velocity);
if (desired_torque < -1.0) {
desired_torque = -1.0;
} else if (desired_torque > 1.0) {
desired_torque = 1.0;
}
adjust_duty_cycle(desired_torque);
lastPosition = position;
// HAL_turnLedOff(halHandle, (GPIO_Number_e)HAL_Gpio_LED2);
}
}
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
/****************************************************************************
* main
***************************************************************************/
void main(void)
{
uint_least8_t estNumber = 0;
uint_least8_t ctrlNumber = 0;
memcpy(&RamfuncsRunStart, &RamfuncsLoadStart, (unsigned long) &RamfuncsLoadSize); // Copy InitFlash fxn to RAM and run it
// 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);
ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber); //v1p6 format (06xF and 06xM devices)
controller_obj = (CTRL_Obj *) ctrlHandle;
{
CTRL_Version version;
CTRL_getVersion(ctrlHandle, &version); // get the version number
gMotorVars.CtrlVersion = version;
}
// set the default controller parameters
CTRL_setParams(ctrlHandle, &gUserParams);
// setup faults
HAL_setupFaults(halHandle);
// enable the ADC interrupts
HAL_enableAdcInts(halHandle);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// disable the PWM
HAL_disablePwm(halHandle);
// turn on the DRV8301 if present
HAL_enableDrv(halHandle);
// initialize the DRV8301 interface
HAL_setupDrvSpi(halHandle, &gDrvSpi8301Vars);
// 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();
BIOS_start(); // Start BIOS Scheduler
}
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
