//========================================================================== // traction controller test // A.Brousalis Nov 2013 //========================================================================== //! Based on PROJ_LAB04A // ************************************************************************** // includes // ************************************************************************** #include #include "main.h" // ************************************************************************** // // ************************************************************************** #define TORQUE_MODE //#define FULL_MOTOR_ID #define PART_MOTOR_ID //#define NO_MOTOR_ID #define LED_BLINK_FREQ_Hz 5 // ************************************************************************** #ifdef FLASH #pragma CODE_SECTION(mainISR,"ramfuncs"); #endif // ************************************************************************** // ************************************************************************** static inline bool myDRV_ReadGpio(DRV_Handle handle,const GPIO_Number_e gpioNumber) { DRV_Obj *obj = (DRV_Obj *)handle; // set GPIO high return( GPIO_read(obj->gpioHandle,gpioNumber)); } #define StartPressed() (myDRV_ReadGpio(drvHandle,GPIO_Number_9) == 0) #define StopPressed() (myDRV_ReadGpio(drvHandle,GPIO_Number_7) == 0) #define Led10_ON() DRV_setGpioHigh(drvHandle,GPIO_Number_12) #define Led10_OFF() DRV_setGpioLow(drvHandle,GPIO_Number_12) #define Led10_TOG() DRV_toggleGpio(drvHandle,GPIO_Number_12) #define Led11_ON() DRV_setGpioHigh(drvHandle,GPIO_Number_15) #define Led11_OFF() DRV_setGpioLow(drvHandle,GPIO_Number_15) #define Led11_TOG() DRV_toggleGpio(drvHandle,GPIO_Number_15) // ************************************************************************** // globals // ************************************************************************** _iq PotRef_krpm; _iq TorqueRef; float vare; uint_least16_t gCounter_updateGlobals = 0; bool Flag_Latch_softwareUpdate = true; CTRL_Handle ctrlHandle; DRV_Handle drvHandle; USER_Params gUserParams; DRV_PwmData_t gPwmData; DRV_AdcData_t gAdcData; _iq gMaxCurrentSlope = _IQ(0.0); #ifdef FAST_ROM_V1p6 CTRL_Obj *controller_obj; #else CTRL_Obj ctrl; //v1p7 format #endif ST_Obj st_obj; ST_Handle stHandle; uint16_t gLEDcnt = 0; volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT; #ifdef FLASH // Used for running BackGround in flash, and ISR in RAM extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart; #endif #ifdef DRV8301_SPI // Watch window interface to the 8301 SPI DRV_SPI_8301_Vars_t gDrvSpi8301Vars; #endif // ************************************************************************** /// the functions // ************************************************************************** RatedFlux_OL void main(void) { uint_least8_t estNumber = 0; TorqueRef = 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 driver drvHandle = DRV_init(&drv,sizeof(drv)); // initialize the user parameters USER_setParams(&gUserParams); // set the driver parameters DRV_setParams(drvHandle,&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 Led11_OFF(); // set the default controller parameters CTRL_setParams(ctrlHandle,&gUserParams); // setup faults DRV_setupFaults(drvHandle); // initialize the interrupt vector table DRV_initIntVectorTable(drvHandle); // enable the ADC interrupts DRV_enableAdcInts(drvHandle); // enable global interrupts DRV_enableGlobalInts(drvHandle); // enable debug interrupts DRV_enableDebugInt(drvHandle); // disable the PWM DRV_disablePwm(drvHandle); //=========================================================================================== //=========================================================================================== #ifdef DRV8301_SPI // turn on the DRV8301 if present DRV_enable8301(drvHandle); // initialize the DRV8301 interface DRV_8301_SpiInterface_init(drvHandle,&gDrvSpi8301Vars); #endif // enable DC bus compensation CTRL_setFlag_enableDcBusComp(ctrlHandle, true); #ifndef TORQUE_MODE // initialize the SpinTAC Components stHandle = ST_init(&st_obj, sizeof(st_obj)); // setup the SpinTAC Components ST_setupVelCtl(stHandle); #endif #ifdef NO_MOTOR_ID gMotorVars.Flag_enableOffsetcalc = false; gMotorVars.Flag_enableRsRecalc = false; gMotorVars.Flag_MotorIdentified = true; #endif #ifdef FULL_MOTOR_ID gMotorVars.Flag_MotorIdentified = false; #endif #ifdef PART_MOTOR_ID gMotorVars.Flag_enableOffsetcalc = true; gMotorVars.Flag_enableRsRecalc = true; gMotorVars.Flag_MotorIdentified = false; #endif // gMotorVars.Flag_enableForceAngle = false; for(;;) { CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle; ST_Obj *stObj = (ST_Obj *)stHandle; // Waiting for enable system flag to be set while(!(gMotorVars.Flag_enableSys)) { if(StartPressed()) { Led10_ON(); gMotorVars.Flag_enableSys = true; gMotorVars.Flag_Run_Identify = true; } } #ifndef FULL_MOTOR_ID // Dis-able the Library internal PI. Iq has no reference now //if(gMotorVars.Flag_MotorIdentified == true) // { CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false); //} #endif // loop while the enable system flag is true while(gMotorVars.Flag_enableSys) { // increment counters gCounter_updateGlobals++; #ifdef PART_MOTOR_ID CTRL_setFlag_enableUserMotorParams(ctrlHandle,true ) ; // Use user.h parameters #endif #ifdef NO_MOTOR_ID CTRL_setFlag_enableUserMotorParams(ctrlHandle,true ) ; // Use user.h parameters #endif 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 DRV_disablePwm(drvHandle); } 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 DRV_enablePwm(drvHandle); } else if(ctrlState == CTRL_State_OnLine) { Led11_ON(); if(gMotorVars.Flag_enableOffsetcalc == true) { // update the ADC bias values DRV_updateAdcBias(drvHandle); } else { // set the current bias DRV_setBias(drvHandle,DRV_SensorType_Current,0,_IQ(I_A_offset)); DRV_setBias(drvHandle,DRV_SensorType_Current,1,_IQ(I_B_offset)); DRV_setBias(drvHandle,DRV_SensorType_Current,2,_IQ(I_C_offset)); // set the voltage bias DRV_setBias(drvHandle,DRV_SensorType_Voltage,0,_IQ(V_A_offset)); DRV_setBias(drvHandle,DRV_SensorType_Voltage,1,_IQ(V_B_offset)); DRV_setBias(drvHandle,DRV_SensorType_Voltage,2,_IQ(V_C_offset)); } // enable the PWM DRV_enablePwm(drvHandle); } else if(ctrlState == CTRL_State_Idle) { Led11_OFF(); // disable the PWM DRV_disablePwm(drvHandle); gMotorVars.Flag_Run_Identify = false; } } } if(EST_isMotorIdentified(obj->estHandle)) { // set the current ramp EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope); gMotorVars.Flag_MotorIdentified = true; #ifndef TORQUE_MODE // set the speed reference vare = (float) ( (float)((float)PotRef_krpm / 4096) * USER_MOTOR_MAX_SPEED_KRPM); gMotorVars.SpeedRef_krpm = _IQ(vare); CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm); gMotorVars.MaxAccel_krpmps = _IQ(2.0); // 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); } #endif #ifdef TORQUE_MODE vare = (float) ( (float)((float)PotRef_krpm / 4096)); vare *= USER_MOTOR_MAX_CURRENT; gMotorVars.IqRef_A = _IQ(vare); #endif if(Flag_Latch_softwareUpdate) { Flag_Latch_softwareUpdate = false; #ifdef FAST_ROM_V1p6 if(CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) { // call this function to fix 1p6 softwareUpdate1p6(ctrlHandle); } #endif 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); } } // end of while(gFlag_enableSys) loop // disable the PWM DRV_disablePwm(drvHandle); // set the default controller parameters (Reset the control to re-identify the motor) CTRL_setParams(ctrlHandle,&gUserParams); //gMotorVars.Flag_Run_Identify = false; #ifndef TORQUE_MODE // setup the SpinTAC Components ST_setupVelCtl(stHandle); #endif } // end of for(;;) loop } // end of main() function //=================================================================================== // RT service routine //=================================================================================== interrupt void mainISR(void) { static uint16_t stCnt = 0; // toggle status LED if(gLEDcnt++ > (uint_least32_t)(USER_PWM_FREQ_kHz * 1000 / LED_BLINK_FREQ_Hz)){ DRV_toggleLed(drvHandle,(GPIO_Number_e)DRV_Gpio_LED2); gLEDcnt = 0; } if(StopPressed()) { Led10_OFF(); Led11_OFF(); gMotorVars.Flag_enableSys = false; } // acknowledge the ADC interrupt DRV_acqAdcInt(drvHandle,ADC_IntNumber_1); // convert the ADC data DRV_readAdcData(drvHandle,&gAdcData); #ifndef TORQUE_MODE // Run the SpinTAC Components if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) { ST_runVelCtl(stHandle, ctrlHandle); stCnt = 1; } #endif // run the controller CTRL_run(ctrlHandle,drvHandle,&gAdcData,&gPwmData); // write the PWM compare values DRV_writePwmData(drvHandle,&gPwmData); // setup the controller CTRL_setup(ctrlHandle); return; } // end of mainISR() function void updateGlobalVariables_motor(CTRL_Handle handle) { CTRL_Obj *obj = (CTRL_Obj *)handle; //************************************************** union { int32_t long_value; float_t float_value; } interface; #ifdef TORQUE_MODE _iq iq_ref = _IQmpy(gMotorVars.IqRef_A,_IQ(1.0/USER_MOTOR_MAX_CURRENT)); // get the speed estimate gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle); // set the speed reference so that the forced angle rotates in the correct direction // for startup. if(_IQabs(gMotorVars.Speed_krpm) < _IQ(0.01)) { if(iq_ref < 0) CTRL_setSpd_ref_krpm(handle,_IQ(-0.01)); else if(iq_ref > 0) CTRL_setSpd_ref_krpm(handle,_IQ(0.01)); } // Set the Iq reference that use to come out of the PI speed control TorqueRef = iq_ref; CTRL_setIq_ref_pu(handle, iq_ref); #endif // get the speed estimate gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle); // get the torque estimate gMotorVars.Torque_lbin = EST_getTorque_lbin(obj->estHandle); // get the magnetizing current interface.long_value = EST_getIdRated(obj->estHandle); gMotorVars.MagnCurr_A = interface.float_value; // get the rotor resistance interface.long_value = EST_getRr_Ohm(obj->estHandle); gMotorVars.Rr_Ohm = interface.float_value; // get the stator resistance interface.long_value = EST_getRs_Ohm(obj->estHandle); gMotorVars.Rs_Ohm = interface.float_value; // get the stator inductance in the direct coordinate direction interface.long_value = EST_getLs_d_H(obj->estHandle); gMotorVars.Lsd_H = interface.float_value; // get the stator inductance in the quadrature coordinate direction interface.long_value = EST_getLs_q_H(obj->estHandle); gMotorVars.Lsq_H = interface.float_value; // get the flux interface.long_value = EST_getFlux_VpHz(obj->estHandle); gMotorVars.Flux_VpHz = interface.float_value; // get the controller state gMotorVars.CtrlState = CTRL_getState(handle); // get the estimator state gMotorVars.EstState = EST_getState(obj->estHandle); // enable/disable the forced angle EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle); // enable or disable power warp CTRL_setFlag_enableEpl(handle,gMotorVars.Flag_enableEpl); // Get the DC buss voltage gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,_IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0)); #ifdef DRV8301_SPI DRV_8301_SpiInterface(drvHandle,&gDrvSpi8301Vars); #endif return; } // end of updateGlobalVariables_motor() function //=========================================================================================== // //=========================================================================================== #ifdef FAST_ROM_V1p6 void softwareUpdate1p6(CTRL_Handle handle) { CTRL_Obj *obj = (CTRL_Obj *)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(obj->motorParams.Ls_d/(Ls_coarse_max*fullScaleInductance))/log(2.0)); uint_least8_t Ls_qFmt = 30 - lShift; float_t L_max = fullScaleInductance * pow(2.0,lShift); _iq Ls_d_pu = _IQ30(obj->motorParams.Ls_d / L_max); _iq Ls_q_pu = _IQ30(obj->motorParams.Ls_q / L_max); // store the results EST_setLs_d_pu(obj->estHandle,Ls_d_pu); EST_setLs_q_pu(obj->estHandle,Ls_q_pu); EST_setLs_qFmt(obj->estHandle,Ls_qFmt); return; } // end of softwareUpdate1p6() function #endif //=========================================================================================== // //=========================================================================================== void calcPIgains(CTRL_Handle handle) { CTRL_Obj *obj = (CTRL_Obj *)handle; float_t fullScaleCurrent = USER_IQ_FULL_SCALE_CURRENT_A; float_t fullScaleVoltage = USER_IQ_FULL_SCALE_VOLTAGE_V; float_t ctrlPeriod_sec = CTRL_getCtrlPeriod_sec(handle); float_t Ls_d; float_t Ls_q; float_t Rs; float_t RoverLs_d; float_t RoverLs_q; _iq Kp_Id; _iq Ki_Id; _iq Kp_Iq; _iq Ki_Iq; _iq Kd; union { int32_t long_value; float_t float_value; } interface; interface.long_value = EST_getLs_d_H(obj->estHandle); Ls_d = interface.float_value; interface.long_value = EST_getLs_q_H(obj->estHandle); Ls_q = interface.float_value; interface.long_value = EST_getRs_Ohm(obj->estHandle); Rs = interface.float_value; RoverLs_d = Rs/Ls_d; Kp_Id = _IQ(Kp_Scale * (0.25*Ls_d*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage)); Ki_Id = _IQ(RoverLs_d*ctrlPeriod_sec); RoverLs_q = Rs/Ls_q; Kp_Iq = _IQ(Kp_Scale * (0.25*Ls_q*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage)); Ki_Iq = _IQ(RoverLs_q*ctrlPeriod_sec); Kd = _IQ(0.0); // set the Id controller gains PID_setKi(obj->pidHandle_Id,Ki_Id); CTRL_setGains(handle,CTRL_Type_PID_Id,Kp_Id,Ki_Id,Kd); // set the Iq controller gains PID_setKi(obj->pidHandle_Iq,Ki_Iq); CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp_Iq,Ki_Iq,Kd); return; } // end of calcPIgains() function //@} //defgroup // end of file //=========================================================================================== // //=========================================================================================== void ST_runVelCtl(ST_Handle sthandle, CTRL_Handle handle) { _iq speedFeedback, iqReference; ST_Obj *stObj = (ST_Obj *)sthandle; CTRL_Obj *ctrlObj = (CTRL_Obj *)handle; // Get the mechanical speed in pu speedFeedback = EST_getFm_pu(ctrlObj->estHandle); // Run the SpinTAC Controller // Note that the library internal ramp generator is used to set the speed reference STVELCTL_setVelocityReference(stObj->velCtlHandle, TRAJ_getIntValue(ctrlObj->trajHandle_spd)); STVELCTL_setAccelerationReference(stObj->velCtlHandle, _IQ(0.0)); // Internal ramp generator does not provide Acceleration Reference STVELCTL_setVelocityFeedback(stObj->velCtlHandle, speedFeedback); STVELCTL_run(stObj->velCtlHandle); // select SpinTAC Velocity Controller iqReference = STVELCTL_getTorqueReference(stObj->velCtlHandle); // Set the Iq reference that came out of SpinTAC Velocity Control TorqueRef = iqReference; // just for external monitoring CTRL_setIq_ref_pu(ctrlHandle, iqReference); } //@} //defgroup // end of file