/* --COPYRIGHT--,BSD * Copyright (c) 2017, Viettel R&D Institute at Viettel Corp. * * Processor target: TMS320F28069M * Programmer: duongtb61 (Eng. Tran Binh Duong) * Email: tran.binhduong@gmail.com or duongtb61@viettel.com.vn * Created: 13-Feb-2017 * Updated: 12-May-2017 * * All rights reserved. * * --/COPYRIGHT--*/ //! \file /servo_main.c //! \brief Using MotorWare platforms to build a Servo controller for PMSM motor //! //! ----------------------------------------------------------------------------------------------- //! HISTORY OF FIRMWARE DEVELOPMENT //! ----------------------------------------------------------------------------------------------- //! Date | Description //! ----------------------------------------------------------------------------------------------- //! 13-Feb-2017 Created the project //! 1. Added platform modules: HAL, USER, CTRL and MotorWare's driver/modules //! 2. Defined module BSP (Board Support Packet) //! //! 11-Apr-2017 Added modules: CPU_TIMER, ANGLE_GEN and VS_FREQ for open-loop testing //! //! 12-Apr-2017 Added modules: //! 1. TRIP-ZONE Management to protect the system //! 2. gTimerDelayMs variable to Timer0ISR to control a delay or time-out //! //! 13-Apr-2017 Added and tested modules: //! 1. ADCINT1 interrupt //! 2. SCIBRXINT interrupt //! //! 14-Apr-2017 Added and tested modules: //! 1. DACPWM for real-time debug by Oscilloscope //! 2. DATALOG for visualizing debug on Debugging Window //! 3. CPU_TIMER for measuring task time //! //! 18-Apr-2017 Added and tested modules: //! 1. Replaced State Machine using "switch-case" structure by //! the method of using function/state pointers. //! 2. Added "utiLib" modules into the project. This modules is ported from //! open-source library for 8-bit MCU written by Pascal Stang. //! 3. Generated the 2nd version for servo_main.c (called servo_main_v2.c) //! //! 19-Apr-2017 Added and tested modules: //! 1. Integrated Error Management block for ERR_ST //! 2. The SERVO_checkForNewEvents() function. //! 3. Trip-zone TZ2 (cycle-by-cycle) interrupt //! //! 20-Apr-2017 Added and tested modules: //! 1. Trip-zone TZ3 (One-shot) interrupt //! //! 21-Apr-2017 Added and tested modules: //! 1. SERVO_checkForErrors() function //! 2. SERVO_resolveSysErrors() function //! 3. SERVO_stateError() function //! //! 22-Apr-2017 Added and tested modules: //! 1. The section to manage IdcBus, T_hsink and AI#1 in HAL_setupAdcs() function //! 2. The functions in hal.h: HAL_getDcbusVoltage_V(), HAL_getDcBusCurrent_A() and //! HAL_getTempAtHeatsink_C //! //! 24-Apr-2017 Added and tested modules: //! 1. HAL_sw1IsHolding() and HAL_sw2IsHolding() to go to Jogging state //! 2. SERVO_stateJog() function //! //! 25-Apr-2017 Added and tested modules: //! 1. V/Hz profile (VS_FREQ) and ANGLE_GEN for open-loop controlling (BUILDLEVEL == LEVEL2) //! 2. SERVO_runVpHzCtrl(): Completely integrated V/Hz control for testing hardware //! 3. Removed gServoVars and redefined _SERVO_Vars_ structure //! //! 26-Apr-2017 Added and tested modules: //! 1. V/Hz control with Current closed loop (BUILDLEVEL == LEVEL3) //! //! 27-Apr-2017 Added and tested modules: //! 1. Sensorless FOC control with FAST Estimator (BUILDLEVEL == LEVEL4) //! //! 03-May-2017 Added and tested modules on OneSERVO-DRV-rev1.0 //! 1. Status LEDs and PWM-DAC //! 2. PWM and DRV8305 //! 3. ADC //! 4. SCIA module //! 5. TripZone and TripZone Interrupt (on TZ3) //! //! 04-May-2017 Added and tested the following modules: //! 1. rprintf() and cmdLine() functions //! 2. CAN module (tested by hieutd1) //! //! 05-May-2017 Added and tested the following modules: //! 1. SpinTAC-Velocity functions (haven't tested) //! 2. Fixed error in HAL_getTempAtHeatsink_C() -> Test is OK! //! //! 06-May-2017 Added and tested the following modules: //! 1. Fixed error due to EMI at RL_PWR turn on //! 2. SpinTAC-Position functions (haven't tested) //! //! 08-May-2017 Added and tested the following modules: //! 1. Changed the name of SERVO_stateTable[]() to SERVO_runStateMachine[]() function //! 2. Encoder hardware interface was tested OK!!! //! 3. SpinTAC-Velocity and SpinTAC-Move was tested at the 1st running time. //! 4. Found an Short-circuit error when transising from READY_ST to RUN_ST at the 2nd running time //! //! 12-May-2017 Added and test the following modules: //! 1. SpinTAC-Velocity Controller for PTUv1.0 using Ketterer Motor (Model: 3206I) //! 2. SpinTAC-Position with Velocity mode for PTUv1.0 using Ketterer Motor (Model: 3206I) //! // ************************************************************************************************ // system includes #include #include "servo_main.h" #ifdef USE_PIL #include "pil.h" #pragma RETAIN(PIL_D_FirmwareDescription) const char PIL_D_FirmwareDescription[] = "Servo Motor Driver"; typedef struct SCOPE_VARS { int16_t Ia; int16_t Ib; int16_t Id; int16_t Iq; int16_t Va_in; int16_t Vb_in; int16_t Va_out; int16_t Vb_out; int16_t Vd; int16_t Vq; uint16_t Angle; uint16_t ctrlState; uint16_t estState; float Rhf; float Lhf; } SCOPE_VARS; SCOPE_VARS ScopeVars; typedef struct G_MOTOR_VARS { PIL_OVERRIDE_PROBE(uint16_t, Flag_enableSys); PIL_OVERRIDE_PROBE(uint16_t, Flag_enableForceAngle); PIL_OVERRIDE_PROBE(int32_t, IqRef_A); } G_MOTOR_VARS; G_MOTOR_VARS gServoVarsOverride; #endif //end of #ifdef USE_PIL // code sections #ifdef FLASH #pragma CODE_SECTION(mainISR,"ramfuncs"); #pragma CODE_SECTION(SERVO_mainISR,"ramfuncs"); //#pragma CODE_SECTION(HAL_pwm1TzISR,"ramfuncs"); //#pragma CODE_SECTION(HAL_sciARxISR,"ramfuncs"); //#pragma CODE_SECTION(HAL_sciBRxISR,"ramfuncs"); //#pragma CODE_SECTION(HAL_timer0ISR,"ramfuncs"); //#pragma CODE_SECTION(HAL_timer1ISR,"ramfuncs"); //#pragma CODE_SECTION(HAL_timer2ISR,"ramfuncs"); //#pragma CODE_SECTION(HAL_ext1ISR, "ramfuncs"); #endif //FLASH // ************************************************************************** // the defines //! \brief Defines the rate of blinking of status LEDs #define LED_RUN_BLINK_FREQ_Hz 10 //Normal Run: 10Hz #define LED_JOG_BLINK_FREQ_Hz 5 //Jogging Run: 5Hz #define LED_CFG_BLINK_FREQ_Hz 2 //Config Mode: 2Hz #define LED_READY_BLINK_FREQ_Hz 1 //Ready Mode: 1Hz #define LED_ERR_SHOTCIRCUIT_BLINK_FREQ_Hz 16 //Blinking rate for short-circuit error #define LED_ERR_OVERVOLTAGE_BLINK_FREQ_Hz 14 //Blinking rate for over-voltage error #define LED_ERR_UNDERVOLTAGE_BLINK_FREQ_Hz 12 //Blinking rate for under-voltage error #define LED_ERR_ENCODER_BLINK_FREQ_Hz 10 //Blinking rate for Encoder error #define LED_ERR_OVERLOAD_BLINK_FREQ_Hz 8 //Blinking rate for over-load error #define LED_ERR_OVERHEAT_BLINK_FREQ_Hz 6 //Blinking rate for over-heat error #define LED_ERR_USERPARAM_BLINK_FREQ_Hz 4 //Blinking rate for userParameters error #define LED_ERR_EST_BLINK_FREQ_Hz 2 //Blinking rate for FAST Estimator error #define LED_ERR_CTRL_BLINK_FREQ_Hz 1 //Blinking rate for Controller error #define LED_ERR_UNKNOWN_BLINK_FREQ_Hz 0.5 //Blinking rate for Unknown error // ************************************************************************** // the globals volatile uint16_t gLEDcnt = 0; //!< General LED counter volatile uint16_t gRunLEDcnt = 0; //!< Manage the blinking rate of the LED-RUN volatile uint16_t gErrLEDcnt = 0; //!< Manage the blinking rate of the LED-ERR volatile uint16_t gErrLEDBlinkRate = 0; #if (SERVO_USING_TZINT == true) uint16_t gTzCnt = 0; //!< For testing Tz_INT #endif //buttons bool gSw1; //JOG button bool gSw2; //START button //! \brief Array of state pointers which point to the function to call in each state //! \notes Added by Duongtb61 (18-Apr-2017) //! void (*SERVO_runStateMachine[])(void) = { SERVO_stateStartUp, SERVO_stateReady, SERVO_stateConfig, SERVO_stateJog, SERVO_stateRun, SERVO_stateError}; //SERVO_State_e gCurrentState; //UART data for testing uint16_t dataRx; uint16_t success; #ifdef CODE_HIEU //! \brief Declare UART/SCI variables //! \notes: This section is declared by hieutd1 (22-Feb-2017) char readByte = 0; BYTE array[4] ={0x31, 0x32, 0x33, 0x34}; SCIbuffer scib_buffer; SCIbuffer commandData; //! \end of SCI variable section that is written by hieutd1 #endif //CODE_HIEU bool Flag_Latch_softwareUpdate = true; uint_least16_t gCounter_updateGlobals = 0; #ifdef CSM_ENABLE #pragma DATA_SECTION(halHandle,"rom_accessed_data"); #endif HAL_Handle halHandle; #ifdef CSM_ENABLE #pragma DATA_SECTION(gUserParams,"rom_accessed_data"); #endif USER_Params gUserParams; HAL_PwmData_t gPwmData = {_IQ(0.0), _IQ(0.0), _IQ(0.0)}; HAL_AdcData_t gAdcData; _iq gMaxCurrentSlope = _IQ(0.0); CTRL_Handle ctrlHandle; //the CTRL handle #ifdef FAST_ROM_V1p6 CTRL_Obj *controller_obj; //the CTRL instant #else #ifdef CSM_ENABLE #pragma DATA_SECTION(ctrl,"rom_accessed_data"); #endif CTRL_Obj *controller_obj; // CTRL_Obj ctrl; //v1p7 format #endif // declare Angle Generate ANGLE_GEN_Handle angle_genHandle; ANGLE_GEN_Obj angle_gen; // declare Vs per Freq VS_FREQ_Handle vs_freqHandle; VS_FREQ_Obj vs_freq; #ifdef QEP // declare ENC components ENC_Handle encHandle; ENC_Obj enc; #endif SLIP_Handle slipHandle; SLIP_Obj slip; ST_Obj st_obj; //the SpinTAC handle ST_Handle stHandle; // Declare servo system variables SERVO_Vars_t gServoVars = SERVO_Vars_INIT; #ifdef FLASH // Used for running BackGround in flash, and ISR in RAM extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart; #ifdef CSM_ENABLE extern uint16_t *econst_start, *econst_end, *econst_ram_load; extern uint16_t *switch_start, *switch_end, *switch_ram_load; #endif #endif //end of #ifdef FLASH #ifdef DRV8301_SPI // Watch window interface to the 8301 SPI DRV_SPI_8301_Vars_t gDrvSpi8301Vars; #endif #ifdef DRV8305_SPI // Watch window interface to the 8305 SPI DRV_SPI_8305_Vars_t gDrvSpi8305Vars; #endif // define CPU time CPU_TIME_Handle cpu_timeHandle; CPU_TIME_Obj cpu_time; #ifndef F2802xF HAL_DacData_t gDacData; #endif _iq gFlux_pu_to_Wb_sf; //The scalling value to convert Flux_pu to Flux_Wb _iq gFlux_pu_to_VpHz_sf; //The scalling value to convert Flux_pu to Flux_VpHz _iq gTorque_Ls_Id_Iq_pu_to_Nm_sf; //The scalling value to convert Torqe_pu to Torqe_Nm _iq gTorque_Flux_Iq_pu_to_Nm_sf; _iq gSpeed_krpm_to_pu_sf; //Added by Duongtb61 (12-May-2017) _iq gSpeed_pu_to_krpm_sf; //Added by Duongtb61 (12-May-2017) //! \brief The global temporary variable which is used in DACPWM calculation //! \notes Added by Duongtb61 (3-May-2017) _iq gTemp[4]={_IQ(0.0),_IQ(0.0),_IQ(0.0),_IQ(0.0)}; //************************************************************************************************* // the function prototypes interrupt void SERVO_mainISR(void); // \brief the TripZone PWM interrupts interrupt void HAL_pwm1TzISR(void); //Added by duongtb61 (17-Feb-2017) interrupt void HAL_pwm4TzISR(void); //Added by duongtb61 (19-Apr-2017) // \brief the Timer interrupts interrupt void HAL_timer0ISR(void); //Added by duongtb61 (17-Feb-2017) interrupt void HAL_timer1ISR(void); //Added by duongtb61 (17-Feb-2017) interrupt void HAL_timer2ISR(void); //Added by duongtb61 (17-Feb-2017) // \brief the SCI interrupts interrupt void HAL_sciARxISR(void); //Added by duongtb61 (11-Apr-2017) interrupt void HAL_sciBRxISR(void); //Added by duongtb61 (17-Feb-2017) //! \brief the External interrupts interrupt void HAL_ext1ISR(void); //Added by duongtb61 (22-Feb-2017) interrupt void HAL_ext2ISR(void); //Added by duongtb61 (22-Feb-2017) interrupt void HAL_ext3ISR(void); //Added by duongtb61 (22-Feb-2017) // \brief Define the state machine function prototypes void SERVO_initStateMachine(void); void SERVO_stateStartUp(void); void SERVO_stateReady(void); void SERVO_stateJog(void); void SERVO_stateRun(void); void SERVO_stateConfig(void); void SERVO_stateError(void); //************************************************************************************************* // the functions #ifdef USE_PIL void ServicePilBackground(){ // allow PLECS to override some of the global control variables SET_OPROBE(gServoVarsOverride.IqRef_A, gServoVars.IqRef_A); SET_OPROBE(gServoVarsOverride.Flag_enableForceAngle, gServoVars.Flag_enableForceAngle); SET_OPROBE(gServoVarsOverride.Flag_enableSys, gServoVars.Flag_enableSys); PIL_backgroundCall(); HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED3); } #endif //end of #ifdef USE_PIL // ************************************************************************************************ // the main function //! \brief main() function //! void main(void) { uint_least8_t estNumber = 0; #ifdef FAST_ROM_V1p6 uint_least8_t ctrlNumber = 0; #endif // ---------------------------------------------------------------------------------------------- // SECTION 1: Load the critical functions and variables from FLASH to RAM // ---------------------------------------------------------------------------------------------- //\notes: Only used if running from FLASH // Note that the variable FLASH is defined by the project management tool #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 CSM_ENABLE //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 //end of #ifdef CSM_ENABLE #endif //end of #ifdef FLASH // ---------------------------------------------------------------------------------------------- // SECTION 2: initialize the hardware abstraction layer // ---------------------------------------------------------------------------------------------- halHandle = HAL_init(&hal,sizeof(hal)); // Turn off RELAY_PWR firstly when power-on reset HAL_RL_PWR_OFF(); // Stop timer1 to disable the Error management HAL_stopTimer(halHandle,(uint_least8_t)Timer_Number_1); // ---------------------------------------------------------------------------------------------- // SECTION 3: check for errors in user parameters // ---------------------------------------------------------------------------------------------- USER_checkForErrors(&gUserParams); // store user parameter error in global variable gServoVars.UserErrorCode = USER_getErrorCode(&gUserParams); // do not allow code execution if there is a user parameter error if(gServoVars.UserErrorCode != USER_ErrorCode_NoError) { //for(;;) { // Disable the system gServoVars.Flag_enableSys = false; // Notify with the Servo Controller that has an error due to wrong user's parameters gServoVars.ServoState = SERVO_ERROR_ST; gServoVars.ServoErrCode = SERVO_ErrorCode_UserParameterError; // Go to the Background loop and then branch to ERROR state goto MainLoop; } } // ---------------------------------------------------------------------------------------------- // SECTION 4: Initialize all necessary PLATFORMS (hardware driver (HAL) and controllers) // that will be used in the project. // ---------------------------------------------------------------------------------------------- // Load the user parameters which is defined in user.h 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 // set the default controller parameters CTRL_setParams(ctrlHandle,&gUserParams); // Get the version of controller { CTRL_Version version; // get the version number CTRL_getVersion(ctrlHandle,&version); gServoVars.CtrlVersion = version; } #ifdef DEBUG #if (BUILDLEVEL == LEVEL1) // set DAC parameters gDacData.ptrData[0] = &gPwmData.Tabc.value[0]; gDacData.ptrData[1] = &gPwmData.Tabc.value[1]; gDacData.ptrData[2] = &gPwmData.Tabc.value[2]; gDacData.ptrData[3] = &gAdcData.V.value[0]; //#if (BUILDLEVEL == LEVEL2) #elif (BUILDLEVEL == LEVEL2) gDacData.ptrData[0] = &angle_gen.Angle_pu; gDacData.ptrData[1] = &gAdcData.I.value[0]; gDacData.ptrData[2] = &gPwmData.Tabc.value[0]; gDacData.ptrData[3] = &gAdcData.V.value[0]; //#if (BUILDLEVEL == LEVEL3) #elif (BUILDLEVEL == LEVEL3) gDacData.ptrData[0] = &angle_gen.Angle_pu; gDacData.ptrData[1] = &controller_obj->pid_Iq.refValue; gDacData.ptrData[2] = &controller_obj->pid_Iq.fbackValue; gDacData.ptrData[3] = &gPwmData.Tabc.value[0]; //#if (BUILDLEVEL == LEVEL3) #elif (BUILDLEVEL == LEVEL4) gDacData.ptrData[0] = &angle_gen.Angle_pu; gDacData.ptrData[1] = &gAdcData.I.value[0]; gTemp[0] = gAdcData.V.value[0]-gAdcData.V.value[1]; gDacData.ptrData[2] = &gTemp[0]; gDacData.ptrData[3] = &gPwmData.Tabc.value[0]; //#if (BUILDLEVEL == LEVEL4) #elif (BUILDLEVEL == LEVEL5) gDacData.ptrData[0] = &gServoVars.angle_est_pu; gDacData.ptrData[1] = &gServoVars.SpeedRef_pu; gDacData.ptrData[2] = &gServoVars.Speed_pu; gDacData.ptrData[3] = &gPwmData.Tabc.value[0]; //#if (BUILDLEVEL == LEVEL5) #elif (BUILDLEVEL == LEVEL6) gDacData.ptrData[0] = &gServoVars.angle_sen_pu; // gDacData.ptrData[1] = &gPwmData.Tabc.value[0]; // gDacData.ptrData[2] = &&gAdcData.V.value[0]; // gDacData.ptrData[3] = &&gAdcData.V.value[1]; gDacData.ptrData[1] = &gServoVars.SpeedRef_pu; gDacData.ptrData[2] = &gServoVars.Speed_pu; gDacData.ptrData[3] = &gAdcData.V.value[0]; #endif HAL_setDacParameters(halHandle, &gDacData); // Initialize Datalog datalogHandle = DATALOG_init(&datalog,sizeof(datalog)); #if (BUILDLEVEL == LEVEL1) // Connect inputs of the datalog module datalog.iptr[0] = &gPwmData.Tabc.value[0]; // datalogBuff[0] datalog.iptr[1] = &gPwmData.Tabc.value[1]; // datalogBuff[1] datalog.iptr[2] = &gPwmData.Tabc.value[2]; // datalogBuff[2] #endif //#if (BUILDLEVEL == LEVEL2) #if (BUILDLEVEL == LEVEL2) datalog.iptr[0] = &angle_gen.Angle_pu; // datalogBuff[0] datalog.iptr[1] = &gAdcData.I.value[0]; // datalogBuff[1] datalog.iptr[2] = &gAdcData.V.value[0]; // datalogBuff[2] #endif //#if (BUILDLEVEL == LEVEL2) #if (BUILDLEVEL == LEVEL3) datalog.iptr[0] = &angle_gen.Angle_pu; // datalogBuff[0] datalog.iptr[1] = &controller_obj->pid_Iq.refValue; // datalogBuff[1] datalog.iptr[2] = &controller_obj->pid_Iq.fbackValue; // datalogBuff[2] #endif //#if (BUILDLEVEL == LEVEL3) #if (BUILDLEVEL == LEVEL4) datalog.iptr[0] = &angle_gen.Angle_pu; // datalogBuff[0] datalog.iptr[1] = &gAdcData.I.value[0]; // datalogBuff[1] datalog.iptr[2] = &gAdcData.V.value[0]; // datalogBuff[2] #endif //#if (BUILDLEVEL == LEVEL4) #if (BUILDLEVEL == LEVEL5) datalog.iptr[0] = &gServoVars.angle_est_pu; datalog.iptr[1] = &gServoVars.SpeedRef_pu; datalog.iptr[2] = &gServoVars.Speed_pu; #endif //#if (BUILDLEVEL == LEVEL5) #if (BUILDLEVEL == LEVEL6) datalog.iptr[0] = &gServoVars.angle_sen_pu; datalog.iptr[1] = &gServoVars.SpeedRef_pu; datalog.iptr[2] = &gServoVars.Speed_pu; #endif datalog.Flag_EnableLogData = true; datalog.Flag_EnableLogOneShot = false; #endif //DEBUG // ---------------------------------------------------------------------------------------------- // SECTION 5: Initialize INTERRUPTs and Protection // ---------------------------------------------------------------------------------------------- // setup faults (or Active Trip-zone to protect the system) HAL_setupFaults(halHandle); // initialize the interrupt vector table HAL_initIntVectorTable(halHandle); // disable the ADC interrupts during initialization HAL_disableAdcInts(halHandle); #if (SERVO_USING_TZINT == 1) // disable the Trip-zone (ePWM1TZ) interrupt. Added by Duongtb61 (03-May-2017) HAL_disableTzInt(halHandle); #endif #if (SERVO_USING_TIMER0INT == 1) // enable the Timer0 interrupt. Added by Duongtb61 (12-Jan-2017) HAL_enableTimer0Int(halHandle); #endif #if (SERVO_USING_TIMER1INT == 1) // enable the Timer1 interrupt. Added by Duongtb61 (12-Jan-2017) HAL_enableTimer1Int(halHandle); #endif #if (SERVO_USING_TIMER2INT == 1) // enable the Timer2 interrupt. Added by Duongtb61 (12-Jan-2017) HAL_enableTimer2Int(halHandle); #endif #if (SERVO_USING_XINT1 == true) // enable the external 1 interrupt. Added by Duongtb61 (6-May-2017) HAL_enableXint1Int(halHandle); #endif #if (SERVO_USING_SCIARXINT == true) // enable the SciA-Rx interrupt. Added by Duongtb61 (11-Apr-2017) HAL_enableSciAInts(halHandle); #endif #if (SERVO_USING_SCIBRXINT == true) // enable the SciB-Rx interrupt. Added by Duongtb61 (11-Apr-2017) HAL_enableSciBInts(halHandle); #endif // enable global interrupts HAL_enableGlobalInts(halHandle); // enable debug interrupts HAL_enableDebugInt(halHandle); // disable the PWM HAL_disablePwm(halHandle); // ---------------------------------------------------------------------------------------------- // SECTION 6: Setup the parameters for firmware modules and hardware driver // ---------------------------------------------------------------------------------------------- // initialize the ENC module encHandle = ENC_init(&enc, sizeof(enc)); // setup the ENC module ENC_setup(encHandle, 1, USER_MOTOR_NUM_POLE_PAIRS, USER_MOTOR_ENCODER_LINES, 0, USER_IQ_FULL_SCALE_FREQ_Hz, USER_ISR_FREQ_Hz, 8000.0); // initialize the SLIP module slipHandle = SLIP_init(&slip, sizeof(slip)); // setup the SLIP module SLIP_setup(slipHandle, _IQ(gUserParams.ctrlPeriod_sec)); // initialize the SpinTAC Components stHandle = ST_init(&st_obj, sizeof(st_obj)); // setup the SpinTAC Components ST_setupPosConv(stHandle); ST_setupPosCtl(stHandle); ST_setupPosMove(stHandle); gServoVars.SpeedRef_krpm = 0; // initialize the CPU usage module cpu_timeHandle = CPU_TIME_init(&cpu_time,sizeof(cpu_time)); CPU_TIME_setParams(cpu_timeHandle, PWM_getPeriod(halHandle->pwmHandle[0])); // ---------------------------------------------------------------------------------------------- // SECTION 7: Initialize global variables, global functions and peripheral driver // ---------------------------------------------------------------------------------------------- // 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(); #ifdef CODE_HIEU //! \notes Added by Hieutd1 (22-Feb-2017) SERVO_initScibBuffer(); // Send a message to notify the initialization was done SCI_sendMsg(halHandle->sciBHandle, "Initialization has done!!!\r\n"); #endif //CODE_HIEU #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 #if (SERVO_USING_SCIA == true) #if (SERVO_USING_RPRINT == true) // make all rprintf statements use uart for output rprintfInit(SERVO_sciaSendByte); // Prints a testing message rprintfProgStrM("This message is sent from SciaA through RPRINT module\r\n"); #endif //\MODULE_RPRINT #if (SERVO_USING_CMD_LINE == true) // initialize cmdline system cmdlineInit(); // direct cmdline output to SCI-A module (serial port) cmdlineSetOutputFunc(SERVO_sciaSendByte); #endif //\MODULE_CMD_LINE #endif // end of #if (SERVO_USING_SCIA == true) // ---------------------------------------------------------------------------------------------- // SECTION 8: Initialize the Finite State Machine (FSM) // ---------------------------------------------------------------------------------------------- // initialize the start-up state for the FSM in case of no-error SERVO_initStateMachine(); // Blinking Status LEDs (RUN and ERR) to notify that the system has been initialized successfully uint16_t cnt; HAL_LED_RUN_OFF(); HAL_LED_ERR_OFF(); for(cnt=0; cnt<10; cnt++) { HAL_toggleGpio(halHandle,HAL_LED_RUN_PIN); HAL_toggleGpio(halHandle,HAL_LED_ERR_PIN); // Delay 500mS or Blink at 2Hz HAL_DelayMs(500); } // Goto the background loop to implement the FSM MainLoop: for(;;) //the main loop (loop forever)!!! { #ifdef ONESERVO // Dis-able the Library internal PI. Iq has no reference now CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false); gServoVars.Flag_enableSys = true; //!< Checks for new event to switch to the appropriate state SERVO_checkForNewEvents(&gServoVars); //!< Runs the state machine for Servo Driving system SERVO_runStateMachine[gServoVars.ServoState](); //!< Active the SERVO Background Tasks if allowed if(gServoVars.Flag_enableSys == true) { // Runs the controller's background tasks SERVO_runBackgroundTasks(ctrlHandle,&gServoVars); } #else // Dis-able the Library internal PI. Iq has no reference now CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false); gServoVars.Flag_enableSys = true; // loop while the enable system flag is true while(gServoVars.Flag_enableSys == true) { CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle; ST_Obj *stObj = (ST_Obj *)stHandle; // increment counters gCounter_updateGlobals++; #ifndef DEBUG_FSM //!< Checks for new event to switch to the appropriate state SERVO_checkForNewEvents(&gServoVars); //!< Runs the state machine for Servo Driving system SERVO_runStateMachine[gServoVars.ServoState](); #else // enable the ADC interrupts HAL_enableAdcInts(halHandle); #endif //DEBUG_FSM // enable/disable the use of motor parameters being loaded from user.h CTRL_setFlag_enableUserMotorParams(ctrlHandle,gServoVars.Flag_enableUserParams); // enable/disable Rs recalibration during motor startup EST_setFlag_enableRsRecalc(obj->estHandle,gServoVars.Flag_enableRsRecalc); // enable/disable automatic calculation of bias values CTRL_setFlag_enableOffset(ctrlHandle,gServoVars.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 gServoVars.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, gServoVars.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(gServoVars.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 gServoVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0); gServoVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1); gServoVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2); // Return the bias value for voltages gServoVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0); gServoVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1); gServoVars.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); gServoVars.Flag_Run_Identify = false; } if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) && (ctrlState > CTRL_State_Idle) && (gServoVars.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); gServoVars.Flag_MotorIdentified = true; #if (SERVO_USING_SPINTAC_POSMOVE == true) // set the speed reference CTRL_setSpd_ref_krpm(ctrlHandle, STPOSMOVE_getVelocityReference(stObj->posMoveHandle)); #else // set the speed reference CTRL_setSpd_ref_krpm(ctrlHandle,gServoVars.SpeedRef_krpm); #endif // set the speed acceleration CTRL_setMaxAccel_pu(ctrlHandle, _IQmpy(MAX_ACCEL_KRPMPS_SF,gServoVars.MaxAccel_krpmps)); // enable the SpinTAC Position Controller STPOSCTL_setEnable(stObj->posCtlHandle, true); if(EST_getState(obj->estHandle) != EST_State_OnLine) { // if the system is not running, disable SpinTAC Position Controller STPOSCTL_setEnable(stObj->posCtlHandle, false); #if (SERVO_USING_SPINTAC_POSMOVE == true) // If motor is not running, feed the position feedback into SpinTAC Position Move STPOSMOVE_setPositionStart_mrev(stObj->posMoveHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle)); #endif } if(Flag_Latch_softwareUpdate) { Flag_Latch_softwareUpdate = false; USER_calcPIgains(ctrlHandle); // initialize the watch window kp and ki current values with pre-calculated values gServoVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id); gServoVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id); // initialize the watch window Bw value with the default value gServoVars.SpinTAC.PosCtlBw_radps = STPOSCTL_getBandwidth_radps(stObj->posCtlHandle); // initialize the watch window with maximum and minimum Iq reference gServoVars.SpinTAC.PosCtlOutputMax_A = _IQmpy(STPOSCTL_getOutputMaximum(stObj->posCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A)); gServoVars.SpinTAC.PosCtlOutputMin_A = _IQmpy(STPOSCTL_getOutputMinimum(stObj->posCtlHandle), _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; SERVO_updateGlobalVariables(ctrlHandle, stHandle); } // update Kp and Ki gains SERVO_updateKpKiGains(ctrlHandle); // set the SpinTAC (ST) bandwidth scale STPOSCTL_setBandwidth_radps(stObj->posCtlHandle, gServoVars.SpinTAC.PosCtlBw_radps); // set the maximum and minimum values for Iq reference STPOSCTL_setOutputMaximums(stObj->posCtlHandle, _IQmpy(gServoVars.SpinTAC.PosCtlOutputMax_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)), _IQmpy(gServoVars.SpinTAC.PosCtlOutputMin_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A))); // enable/disable the forced angle EST_setFlag_enableForceAngle(obj->estHandle,gServoVars.Flag_enableForceAngle); // enable or disable power warp CTRL_setFlag_enablePowerWarp(ctrlHandle,gServoVars.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); gServoVars.Flag_Run_Identify = false; // setup the SpinTAC Components ST_setupPosConv(stHandle); ST_setupPosCtl(stHandle); #endif //ONESERVO } // end of the for(;;) loop } //end of main() function // ************************************************************************** // the other functions //! \brief The mainISR() function //! \notes This function is attached to ADCINT1 interrupt which is set as //! the highest priority interrupt after RESET vector. interrupt void SERVO_mainISR(void) { static uint16_t stCnt = 0; CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle; SERVO_State_e currState = SERVO_getState(&gServoVars); // acknowledge the ADC interrupt. // Note that: ADCINT1 is configured as the highest priority interrupt HAL_acqAdcInt(halHandle,ADC_IntNumber_1); // convert the ADC data HAL_readAdcData(halHandle,&gAdcData); // Run suitable controllers according to user's commands //if (currState == SERVO_RUN_ST) { // toggle status LED (LED_BLUE) at 10Hz if(gRunLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_RUN_BLINK_FREQ_Hz)) { HAL_toggleGpio(halHandle,HAL_LED_RUN_PIN); gRunLEDcnt = 0; } #ifdef QEP // compute the electrical angle ENC_calcElecAngle(encHandle, HAL_getQepPosnCounts(halHandle)); #endif // Run the SpinTAC Components to implement the Velocity Controller instead of traditional PI-Controller. if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) { #ifdef QEP SERVO_runSpinTACPosConv(stHandle, encHandle, ctrlHandle); #endif //QEP #if (SERVO_USING_SPINTAC_VEL == true) SERVO_runSpinTACVelMove(stHandle, ctrlHandle); SERVO_runSpinTACVelCtrl(stHandle, ctrlHandle); #else #if (SERVO_USING_SPINTAC_POSMOVE == true) // run the SpinTAC-Position Profiler #if (SERVO_USING_SPINTAC_POS == true) { SERVO_runSpinTACPosMoveForPositionMode(stHandle); } #else { SERVO_runSpinTACPosMoveForVelocityMode(stHandle); } #endif #endif // run the SpinTAC-Postion Controller SERVO_runSpinTACPosCtl(stHandle, ctrlHandle); #endif // end of #if (SERVO_USING_SPINTAC_VEL == true) // reset counter for the next calculating period stCnt = 1; } // run the suitable controllers if(USER_MOTOR_TYPE == MOTOR_Type_Induction) { #ifndef QEP SLIP_setElectricalAngle(slipHandle, EST_getAngle_pu(obj->estHandle)); #else // update the electrical angle for the SLIP module SLIP_setElectricalAngle(slipHandle, ENC_getElecAngle(encHandle)); #endif //QEP // compute the amount of slip SLIP_run(slipHandle); // run the controller #ifndef QEP // run the sensorless FOC controller using the FAST estimator CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData); #else CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData,SLIP_getMagneticAngle(slipHandle)); #endif //end of #ifndef QEP } else { #ifndef QEP // run the sensorless FOC controller using the FAST estimator CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData); #else // run the sensored FOC controller CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData,ENC_getElecAngle(encHandle)); #endif //end of #ifndef QEP } } // end of if(currState == SERVO_RUN_ST) // write the PWM compare values HAL_writePwmData(halHandle,&gPwmData); // setup the controller CTRL_setup(ctrlHandle); #ifdef QEP // if we are forcing alignment, using the Rs Recalculation, align the eQEP angle with the rotor angle if((EST_getState(obj->estHandle) == EST_State_Rs) && (USER_MOTOR_TYPE == MOTOR_Type_Pm)) { ENC_setZeroOffset(encHandle, (uint32_t)(HAL_getQepPosnMaximum(halHandle) - HAL_getQepPosnCounts(halHandle))); } #endif // end of #ifdef QEP gSpeed_krpm_to_pu_sf = EST_get_krpm_to_pu_sf(controller_obj->estHandle); gSpeed_pu_to_krpm_sf = EST_get_pu_to_krpm_sf(controller_obj->estHandle); #if (BUILDLEVEL == LEVEL5) // Calculate the system variables gServoVars.angle_est_pu = EST_getAngle_pu(obj->estHandle); gServoVars.SpeedRef_pu = _IQmpy(gServoVars.SpeedRef_krpm, gSpeed_krpm_to_pu_sf); gServoVars.Speed_pu = _IQmpy(gServoVars.Speed_krpm, gSpeed_krpm_to_pu_sf); #elif (BUILDLEVEL == LEVEL6) // Calculate the system variables gServoVars.angle_sen_pu = ENC_getElecAngle(encHandle); gServoVars.SpeedRef_pu = _IQmpy(gServoVars.SpeedRef_krpm, gSpeed_krpm_to_pu_sf); gServoVars.Speed_pu = _IQmpy(gServoVars.SpeedQEP_krpm, gSpeed_krpm_to_pu_sf); //gServoVars.Speed_pu = _IQmpy(gServoVars.Speed_krpm, gSpeed_krpm_to_pu_sf); #endif //end of #if ((BUILDLEVEL == LEVEL5)||(BUILDLEVEL == LEVEL6)) #ifdef DEBUG // update data to DATALOG module DATALOG_update(datalogHandle); // connect inputs of the PWMDAC module. gDacData.value[0] = (*gDacData.ptrData[0]); // gDacData.value[1] = (*gDacData.ptrData[1]); // gDacData.value[2] = (*gDacData.ptrData[2]); // gDacData.value[3] = (*gDacData.ptrData[3]); // // update data to PWMDAC module HAL_writeDacData(halHandle,&gDacData); #endif //DEBUG return; } // end of interrupt void mainISR() function interrupt void mainISR(void) { static uint16_t stCnt = 0; CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle; // toggle status LED (LED_BLUE) at 10Hz if(gRunLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_RUN_BLINK_FREQ_Hz)) { HAL_toggleGpio(halHandle,HAL_LED_RUN_PIN); gRunLEDcnt = 0; } // compute the electrical angle ENC_calcElecAngle(encHandle, HAL_getQepPosnCounts(halHandle)); // acknowledge the ADC interrupt HAL_acqAdcInt(halHandle,ADC_IntNumber_1); // convert the ADC data HAL_readAdcData(halHandle,&gAdcData); // Run the SpinTAC Components if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) { ST_runPosConv(stHandle, encHandle, ctrlHandle); ST_runPosMove(stHandle); ST_runPosCtl(stHandle, ctrlHandle); stCnt = 1; } if(USER_MOTOR_TYPE == MOTOR_Type_Induction) { // update the electrical angle for the SLIP module SLIP_setElectricalAngle(slipHandle, ENC_getElecAngle(encHandle)); // compute the amount of slip SLIP_run(slipHandle); // run the controller CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData,SLIP_getMagneticAngle(slipHandle)); } else { // run the controller CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData,ENC_getElecAngle(encHandle)); } // write the PWM compare values HAL_writePwmData(halHandle,&gPwmData); // setup the controller CTRL_setup(ctrlHandle); // if we are forcing alignment, using the Rs Recalculation, align the eQEP angle with the rotor angle if((EST_getState(obj->estHandle) == EST_State_Rs) && (USER_MOTOR_TYPE == MOTOR_Type_Pm)) { ENC_setZeroOffset(encHandle, (uint32_t)(HAL_getQepPosnMaximum(halHandle) - HAL_getQepPosnCounts(halHandle))); } return; } // end of interrupt void mainISR() function //-------------------------------------------------------------------------------------- // A P P L I C A T I O N S T A T E - M A C H I N E F U N C T I O N S //-------------------------------------------------------------------------------------- void SERVO_initStateMachine(void) { SERVO_setState(&gServoVars, SERVO_STARTUP_ST); SERVO_setErrorCode(&gServoVars, SERVO_ErrorCode_NoError); SERVO_setFlag_sysStateChanged(&gServoVars, true); gTimerDelayMs = 0; return; } //end of the SERVO_initStateMachine() function //!< -------------------- Startup State ----------------------------------- //! \brief Initializes the system and load user's parameters to prepare //! for all system operations. //! \notes Added by Duongtb61 (18-Apr-2017) //! void SERVO_stateStartUp(void) { HAL_Obj *obj = (HAL_Obj *)halHandle; if(SERVO_stateIsChanged(&gServoVars) == true) { #ifdef ONESERVO rprintfProgStrM("Startup State\r\n"); #else SCI_sendMsg(halHandle->sciBHandle, "Startup State\r\n\r\n"); #endif //ONESERVO // Clear the Flag_sysStateChanged to remain this state until having a state transition event. SERVO_setFlag_sysStateChanged(&gServoVars,false); // Turn on RELAY_PWR to supply power into the Inverter HAL_RL_PWR_ON(); // \brief Make a pulse RESET at EN_GATE pin to clear the "dummy error" at nFAULT pin of DRV8305 // due to EMI generated when Relay turn on // \note Fixed by Duongtb61 (06-May-2017) // HAL_resetDrv(halHandle); #if (SERVO_USING_TZINT == 1) uint_least8_t cnt; for(cnt=0;cnt<3;cnt++) { // Clear any spurious fault PWM_clearOneShotTrip(obj->pwmHandle[cnt]); PWM_clearTripZone(obj->pwmHandle[cnt],PWM_TripZoneFlag_CBC); PWM_clearTripZone(obj->pwmHandle[cnt],PWM_TripZoneFlag_Global); } // enable the Trip-zone (ePWM1TZ) interrupt. Added by Duongtb61 (03-May-2017) HAL_enableTzInt(halHandle); #endif // end of #if (SERVO_USING_TZINT == 1) // Start timer1 to run the Error management HAL_startTimer(halHandle,(uint_least8_t)Timer_Number_1); } // Load user's parameters from FLASH memory SERVO_loadUserParameters(&gUserParams); // Delay 05 seconds before going to the next state automatically gFlag_TimerDelayMsIsOver = false; //Clear the flag gTimerDelayMs = 5000; //Set a delay of 5000mS = 5sec while(gTimerDelayMs != 0) //Wait for TimeOut is over!!! { //!< Check for error event in all states of Servo Controller if (SERVO_getFlag_sysIsError(&gServoVars) == true) { // signals the Error event SERVO_setEvent(&gServoVars, SERVO_Event_Error); // Go to the ERROR_ST SERVO_setState(&gServoVars, SERVO_ERROR_ST); // Signals a change of state to the State Machine SERVO_setFlag_sysStateChanged(&gServoVars,true); } } return; } //end of the SERVO_stateStartUp() function //!< -------------------- Ready State ----------------------------------- //! \brief Waits for an available command to run the motor or to configure //! the system parameters from PC Master Utility //! \notes Added by Duongtb61 (18-Apr-2017) //! void SERVO_stateReady(void) { CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle; if(SERVO_stateIsChanged(&gServoVars) == true) { #ifdef ONESERVO rprintfProgStrM("Ready State\r\n"); #else SCI_sendMsg(halHandle->sciBHandle, "Ready State\r\n\r\n"); #endif //ONESERVO // Clear the Flag_sysStateChanged to remain this state until having a state transition event. SERVO_setFlag_sysStateChanged(&gServoVars,false); // enable the ADC interrupts HAL_enableAdcInts(halHandle); // Enable the PWM to trigger the ADCINT1 interrupt //HAL_enablePwm(halHandle); // Disable all controllers gServoVars.Flag_Run_Identify = false; // Signal to the status LEDs HAL_LED_ERR_OFF(); //turn red LED off HAL_LED_RUN_ON(); //turn Blue/Green LED on // Turn on RELAY_PWR to supply power into the Inverter HAL_RL_PWR_ON(); #ifndef READY_DEBUG //!< Set a delay of 1000mS = 1sec or Blinking rate = 1Hz gTimerDelayMs = 1000; #endif // READY_DEBUG // Enable the system gServoVars.Flag_enableSys = true; } #ifndef READY_DEBUG //!< Blinking LED_RUN at the Rate = 1Hz if(gFlag_TimerDelayMsIsOver == true) { HAL_LED_RUN_BLINK(); //Clear the flag time-out gFlag_TimerDelayMsIsOver = false; //Reload the time-out for the next cycle //gTimerDelayMs = 1000; SERVO_setState(&gServoVars, SERVO_RUN_ST); // Signals a change of state to the State Machine SERVO_setFlag_sysStateChanged(&gServoVars,true); } #endif // READY_DEBUG if(EST_getState(obj->estHandle) == EST_State_OnLine) { //Always set to Zero-Speed in this READY_ST gServoVars.SpeedRef_krpm = _IQ(0.0); } //!< Check for error event in all states of Servo Controller if (SERVO_getFlag_sysIsError(&gServoVars) == true) { // signals the Error event SERVO_setEvent(&gServoVars, SERVO_Event_Error); // Go to the ERROR_ST SERVO_setState(&gServoVars, SERVO_ERROR_ST); // Signals a change of state to the State Machine SERVO_setFlag_sysStateChanged(&gServoVars,true); } #ifdef CODE_HIEU //Insert the CMD come from PC Master HERE #endif //CODE_HIEU return; } //end of the SERVO_stateWaitForCmd() function //!< -------------------- Jogging State ----------------------------------- //! \brief Spins the motor at low-speed with V/Hz open-loop control to //! test the direction of motor and mechanical system. //! \notes Added by Duongtb61 (18-Apr-2017) //! void SERVO_stateJog(void) { CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle; //!< Prepares for running in Jog Mode if(SERVO_stateIsChanged(&gServoVars) == true) { #ifdef ONESERVO rprintfProgStrM("Jog State\r\n"); #else SCI_sendMsg(halHandle->sciBHandle, "Jog State\r\n\r\n"); #endif //ONESERVO // Clear the Flag_sysStateChanged to remain this state until having a state transition event. SERVO_setFlag_sysStateChanged(&gServoVars,false); // enable the ADC interrupts HAL_enableAdcInts(halHandle); // Enable the PWM to trigger the ADCINT1 interrupt HAL_enablePwm(halHandle); // Disable the closed-loop control gServoVars.Flag_enableClosedloopCtrl = false; // Enable the Library internal PI. Iq is referenced by the speed PI now gServoVars.Flag_enableSpeedCtrl = true; CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true); // Enable the Flag_Run_Identify to allow to run controllers gServoVars.Flag_Run_Identify = true; #ifdef JOG_DEBUG //!< Set a delay of 200mS = 0.2sec or Blinking rate = 5Hz gTimerDelayMs = 200; #endif //JOG_DEBUG } #ifdef JOG_DEBUG //!< Blinking LED_RUN at the Rate = 5Hz if(gFlag_TimerDelayMsIsOver == true) { HAL_LED_RUN_BLINK(); //Clear the flag time-out gFlag_TimerDelayMsIsOver = false; //Reload the time-out for the next cycle gTimerDelayMs = 200; } #endif //JOG_DEBUG if(EST_getState(obj->estHandle) == EST_State_OnLine) { //Set to Jogging Speed at 0.3krpm or 300 rpm (10% of the rated speed) gServoVars.SpeedRef_krpm = _IQ(0.3); } else { //Reset the reference when the motor control system haven't been ready gServoVars.SpeedRef_krpm = _IQ(0.0); } //!< Check for error event in all states of Servo Controller if (SERVO_getFlag_sysIsError(&gServoVars) == true) { // signals the Error event SERVO_setEvent(&gServoVars, SERVO_Event_Error); // Go to the ERROR_ST SERVO_setState(&gServoVars, SERVO_ERROR_ST); // Signals a change of state to the State Machine SERVO_setFlag_sysStateChanged(&gServoVars,true); } #ifdef CODE_HIEU //Insert the CMD come from PC Master HERE #endif //CODE_HIEU return; } //end of the SERVO_stateJog() function //!< -------------------- Running State ----------------------------------- //! \brief Spins the motor with full closed-loop control //! (torque, velocity and speed) //! \notes Added by Duongtb61 (18-Apr-2017) //! void SERVO_stateRun(void) { if(SERVO_stateIsChanged(&gServoVars) == true) { #ifdef ONESERVO rprintfProgStrM("Running State\r\n"); #else SCI_sendMsg(halHandle->sciBHandle, "Running State\r\n\r\n"); #endif //ONESERVO // Clear the Flag_sysStateChanged to remain this state until having a state transition event. SERVO_setFlag_sysStateChanged(&gServoVars,false); //Set the initial Speed at 0.0krpm or Zero Speed gServoVars.SpeedRef_krpm = _IQ(0.0); // enable the ADC interrupts HAL_enableAdcInts(halHandle); // Enable the PWM to trigger the ADCINT1 interrupt HAL_enablePwm(halHandle); // Enable the Flag_Run_Identify to allow to run controllers gServoVars.Flag_Run_Identify = true; } //!< Check for error event in all states of Servo Controller if (SERVO_getFlag_sysIsError(&gServoVars) == true) { // signals the Error event SERVO_setEvent(&gServoVars, SERVO_Event_Error); // Go to the ERROR_ST SERVO_setState(&gServoVars, SERVO_ERROR_ST); // Signals a change of state to the State Machine SERVO_setFlag_sysStateChanged(&gServoVars,true); } #ifdef CODE_HIEU //Insert the CMD come from PC Master HERE #endif //CODE_HIEU return; } //end of the SERVO_stateRun() function //!< -------------------- Configuration State --------------------------- //! \brief Configure the parameters, such as: motor's parameters, controller's coefficients //! \notes Added by Duongtb61 (18-Apr-2017) //! void SERVO_stateConfig(void) { if(SERVO_stateIsChanged(&gServoVars) == true) { #ifdef ONESERVO rprintfProgStrM("Configuration State\r\n"); #else SCI_sendMsg(halHandle->sciBHandle, "Configuration State\r\n\r\n"); #endif //ONESERVO // Clear the Flag_sysStateChanged to remain this state until having a state transition event. SERVO_setFlag_sysStateChanged(&gServoVars,false); //Disable PWM before jumping to configuration mode HAL_disablePwm(halHandle); // Disable the system gServoVars.Flag_enableSys = false; // Signal to the status LEDs HAL_LED_ERR_OFF(); //turn red LED off HAL_LED_RUN_ON(); //turn Blue/Green LED on // Turn off RELAY_PWR to supply power into the Inverter HAL_RL_PWR_OFF(); } //!< Check for error event in all states of Servo Controller if (SERVO_getFlag_sysIsError(&gServoVars) == true) { // signals the Error event SERVO_setEvent(&gServoVars, SERVO_Event_Error); // Go to the ERROR_ST SERVO_setState(&gServoVars, SERVO_ERROR_ST); // Signals a change of state to the State Machine SERVO_setFlag_sysStateChanged(&gServoVars,true); } #ifdef CODE_HIEU //Insert the CMD come from PC Master HERE #endif //CODE_HIEU return; } //end of the SERVO_stateConfig() function //!< -------------------- Error State ----------------------------------- //! \brief Manage all fault events which can damage the system //! \notes Added by Duongtb61 (18-Apr-2017) //! void SERVO_stateError() { static uint16_t errLedBlinkRate = 0; if(SERVO_stateIsChanged(&gServoVars) == true) { #ifdef ONESERVO rprintfProgStrM("Error State\r\n"); #else SCI_sendMsg(halHandle->sciBHandle, "Error State\r\n"); #endif //ONESERVO // Clear the Flag_sysStateChanged to remain this state until having a state transition event. SERVO_setFlag_sysStateChanged(&gServoVars,false); // Disable PWMs HAL_disablePwm(halHandle); // Disable ADCINT1 HAL_disableAdcInts(halHandle); // Disable TripZone Epwm1TzINT HAL_disableTzInt(halHandle); // Disable TIMER1INT that manages the SERVO_checkForErrors() function //HAL_disableTimer1Int(halHandle); // or Stop the TIMER1 that manages the SERVO_checkForErrors() function HAL_stopTimer(halHandle,(uint_least8_t)Timer_Number_1); // Turn off RELAY_PWR to reject the power into the Inverter HAL_RL_PWR_OFF(); // Turn off LED_RUN HAL_LED_RUN_OFF(); // Disable the system gServoVars.Flag_enableSys = false; // Don't allow to run any controllers gServoVars.Flag_Run_Identify = false; // set the default controller parameters (Reset the control to re-identify the motor) CTRL_setParams(ctrlHandle,&gUserParams); // call the SERVO_resolveSysErrors() to determine type of the occurred errors errLedBlinkRate = SERVO_resolveErrorCode(&gServoVars); // Set the rate of blinking for LED_ERR according error code gTimerDelayMs = errLedBlinkRate; } //!< Blinking LED_ERR at the defined rate according to Error Code if(gFlag_TimerDelayMsIsOver == true) { HAL_LED_ERR_BLINK(); //Clear the flag time-out gFlag_TimerDelayMsIsOver = false; //Reload the time-out for the next cycle gTimerDelayMs = errLedBlinkRate; } #ifdef CODE_HIEU //Insert the CMD come from PC Master HERE #endif //CODE_HIEU return; } //end of the SERVO_stateError() function //! \brief Checks for new events that causes to transform the current state //! \detail This function is called in background and MUST be called before //! the calling of SERVO_runStateMachine[idx]() function. //! \notes Added by Duongtb61 (19-Apr-2017) //! void SERVO_checkForNewEvents(SERVO_Vars_t *pServoVars) { SERVO_State_e servoCurrentState = SERVO_getState(pServoVars); //!< Set no event to remain the current state SERVO_setEvent(pServoVars, SERVO_Event_NoEvent); //!< Check for new events in STARTUP state if (servoCurrentState == SERVO_STARTUP_ST) { if(gFlag_TimerDelayMsIsOver == true) { // signals the TimeOut event SERVO_setEvent(pServoVars, SERVO_Event_Timeout); // Go to the next state automatically after the timeout without any errors SERVO_setState(pServoVars, SERVO_READY_ST); // Notify to the application that having a new state transition. SERVO_setFlag_sysStateChanged(pServoVars,true); } } //!< Check for new events in READY state else if(servoCurrentState == SERVO_READY_ST) { //!< Check for Jogging Command gSw1 = HAL_readGpio(halHandle, HAL_GPIO_SW1); gSw2 = HAL_readGpio(halHandle, HAL_GPIO_SW2); //!< Check for Running Command if(HAL_Sw1IsClicked(halHandle) == true) { SCI_sendMsg(halHandle->sciBHandle, "SW1 was clicked\r\n"); // signals the RUN event SERVO_setEvent(pServoVars, SERVO_Event_Run); // Set the current state to the next state SERVO_setState(pServoVars, SERVO_RUN_ST); // Notify to the application that having a new state transition. SERVO_setFlag_sysStateChanged(pServoVars,true); } //!< Check for Configuration Command if(HAL_Sw2IsClicked(halHandle) == true) { SCI_sendMsg(halHandle->sciBHandle, "SW2 was clicked\r\n"); // signals the CONFIG event SERVO_setEvent(pServoVars, SERVO_Event_Configuration); // Set the current state to the next state SERVO_setState(pServoVars, SERVO_CONFIG_ST); // Notify to the application that having a new state transition. SERVO_setFlag_sysStateChanged(pServoVars,true); } //!< Check for Jog Command if(HAL_sw1IsHolding(halHandle) == true) { SCI_sendMsg(halHandle->sciBHandle, "SW1 was pressed\r\n"); // signals the CONFIG event SERVO_setEvent(pServoVars, SERVO_Event_Jog); // Set the current state to the next state SERVO_setState(pServoVars, SERVO_JOG_ST); // Notify to the application that having a new state transition. SERVO_setFlag_sysStateChanged(pServoVars,true); } } //!< Check for new events in CONFIGURATION state else if(servoCurrentState == SERVO_CONFIG_ST) { if(HAL_Sw2IsClicked(halHandle) == true) { SCI_sendMsg(halHandle->sciBHandle, "SW2 was clicked\r\n"); // signals the EXIT event SERVO_setEvent(pServoVars, SERVO_Event_Exit); // change the current state to come back to READY_ST state SERVO_setState(pServoVars, SERVO_READY_ST); // Notify to the application that having a new state transition. SERVO_setFlag_sysStateChanged(pServoVars,true); } //Insert the CMD come from PC Master HERE to exit the CONFIG state #ifdef CODE_HIEU #endif //CODE_HIEU } //!< Check for new events in JOG state else if(servoCurrentState == SERVO_JOG_ST) { // Check for Sw1 was released? if((HAL_Sw1IsClicked(halHandle) == true)) { SCI_sendMsg(halHandle->sciBHandle, "SW1 was released\r\n"); // signals the EXIT event SERVO_setEvent(pServoVars, SERVO_Event_Stop); // change the current state to come back to WAITING_ST state SERVO_setState(pServoVars, SERVO_READY_ST); // Notify to the application that having a new state transition. SERVO_setFlag_sysStateChanged(pServoVars,true); } #ifdef CODE_HIEU //Insert the CMD come from PC Master HERE to exit the JOG state #endif //CODE_HIEU } //!< Check for new events in RUN state else if(servoCurrentState == SERVO_RUN_ST) { if(HAL_Sw1IsClicked(halHandle) == true) { SCI_sendMsg(halHandle->sciBHandle, "SW1 was clicked\r\n"); // signals the STOP event SERVO_setEvent(pServoVars, SERVO_Event_Stop); // change the current state to come back to WAITING_ST state SERVO_setState(pServoVars, SERVO_READY_ST); // Notify to the application that having a new state transition. SERVO_setFlag_sysStateChanged(pServoVars,true); } #ifdef CODE_HIEU //Insert the CMD come from PC Master HERE to exit the RUN state #endif //CODE_HIEU } //!< Check for new events in ERROR state else if(servoCurrentState == SERVO_ERROR_ST) { //Remain this state until POWER is TURN OFF SERVO_setState(pServoVars, SERVO_ERROR_ST); } return; } // end of SERVO_checkForEvents() function //! \brief Check for any error that causes to damage the system //! \detail This function MUST be called in a 10mS timer interrupt //! \notes Added by Duongtb61 (21-Apr-2017) //! void SERVO_checkForErrors(SERVO_Vars_t *pServoVars) { //!> Clear all errors to prepare for detecting new errors SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_NoError); //!> Check for UserParrameter error { USER_checkForErrors(&gUserParams); // store user parameter error in global variable gServoVars.UserErrorCode = USER_getErrorCode(&gUserParams); // do not allow code execution if there is a user parameter error if(gServoVars.UserErrorCode != USER_ErrorCode_NoError) { SERVO_setFlag_sysIsError(pServoVars); SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_UserParameterError); } } //!< Check for driver fault error due to a short-circuit event? if (pServoVars->Flag_drvIsFault == true) { SERVO_setFlag_sysIsError(pServoVars); SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_ShortCircuit); } //!< Check for DcBus Under-voltage error? if (pServoVars->VdcBus_V < _IQ(BSP_DC_MIN_VOLTAGE_V)) { //Check for the under-volatage condition is remaining in 100*10mS = 1000mS = 1 second if(pServoVars->DcBusUnderVoltageCount++ >= 100) { SERVO_setFlag_sysIsError(pServoVars); SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_UnderVoltage); } } //!< Check for DcBus over-voltage error? else if(pServoVars->VdcBus_V > _IQ(BSP_DC_MAX_VOLTAGE_V)) { //Check for the over-volatage condition is remaining in 500*10mS = 5000mS or 5 seconds if(pServoVars->DcBusOverVoltageCount++ >= 500) { SERVO_setFlag_sysIsError(pServoVars); SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_OverVoltage); } } else { // reset the counters pServoVars->DcBusOverVoltageCount = 0; pServoVars->DcBusUnderVoltageCount = 0; } //!< Check for overload error? if ((pServoVars->IdcBus_A) > _IQ(BSP_TRIP_DRIVER_CURRENT_A)) { //Check for the overload condition is remaining in 1000*10mS = 10,000mS or 10 seconds if(pServoVars->MotorOverCurrentCount++ >= 1000) { SERVO_setFlag_sysIsError(pServoVars); SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_OverLoad); } } else { // reset the counter pServoVars->MotorOverCurrentCount = 0; } //!< Check for over-temperature error at heatsink? if (pServoVars->Theatsink_C > _IQ(BSP_MAX_HEATSINK_TEMP_C)) { //Check for the over-temperature condition is remaining in 3000*10mS = 30,000mS or 30 seconds if(pServoVars->HeatsinkTemperatureCount++ >= 3000) { SERVO_setFlag_sysIsError(pServoVars); SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_OverHeat); } } else { // reset the counter pServoVars->HeatsinkTemperatureCount = 0; } //!< Check for Controller error? if (CTRL_isError(ctrlHandle) == true) { SERVO_setFlag_sysIsError(pServoVars); SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_CtrlError); } //!< Check for FAST Estimator error? #if (SERVO_USING_FAST_EST == true) if (EST_isError(estHandle) == true) { SERVO_setFlag_sysIsError(pServoVars); SERVO_setErrorCode(pServoVars, SERVO_ErrorCode_EstError); } #endif return; } // end of the SERVO_checkForErrors() function uint16_t SERVO_resolveErrorCode(SERVO_Vars_t *pServoVars) { static uint16_t blinkRate = 0; //!< Get the error code SERVO_ErrorCode_e errCode = SERVO_getErrorCode(pServoVars); //!< Set the rate of blinking for ERR_LED (red) according to Error Code if (errCode == SERVO_ErrorCode_ShortCircuit) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_SHOTCIRCUIT_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Short Circuit Error\r\n"); } else if(errCode == SERVO_ErrorCode_OverVoltage) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_OVERVOLTAGE_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Over-voltage Error\r\n"); } else if(errCode == SERVO_ErrorCode_UnderVoltage) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_UNDERVOLTAGE_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Under-voltage Error\r\n"); } else if(errCode == SERVO_ErrorCode_EncoderLost) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_ENCODER_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Encoder Lost Error\r\n"); } else if(errCode == SERVO_ErrorCode_OverLoad) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_OVERLOAD_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Over-load Error\r\n"); } else if(errCode == SERVO_ErrorCode_OverHeat) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_OVERHEAT_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Over-temperature Error\r\n"); } else if(errCode == SERVO_ErrorCode_UserParameterError) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_USERPARAM_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("User Parameter Error\r\n"); } else if(errCode == SERVO_ErrorCode_EstError) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_EST_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Estimator Error\r\n"); } else if(errCode == SERVO_ErrorCode_CtrlError) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_CTRL_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Controller Error\r\n"); } else if(errCode == SERVO_ErrorCode_Unknown) { blinkRate = (uint16_t)(USER_TIMER0_ISR_FREQ_Hz/LED_ERR_UNKNOWN_BLINK_FREQ_Hz); //print error code to host rprintfProgStrM("Unknown Error\r\n"); } else //No-error { blinkRate = 0; //No blink //print error code to host rprintfProgStrM("No-Error\r\n"); } return (blinkRate); } // end of the SERVO_resolveError() function //-------------------------------------------------------------------------------------- // A P P L I C A T I O N F U N C T I O N S //-------------------------------------------------------------------------------------- //! \brief Loads user's parameters from the FLASH memory //! \notes Added by Duongtb61 (19-Apr-2017) //! void SERVO_loadUserParameters(USER_Params *pUserParams) { return; } void SERVO_runBackgroundTasks(CTRL_Handle handle, SERVO_Vars_t *pServoVars) { CTRL_Obj *obj = (CTRL_Obj *)handle; 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(handle,gServoVars.Flag_enableUserParams); // enable/disable Rs recalibration during motor startup EST_setFlag_enableRsRecalc(obj->estHandle,gServoVars.Flag_enableRsRecalc); // enable/disable automatic calculation of bias values CTRL_setFlag_enableOffset(handle,gServoVars.Flag_enableOffsetcalc); if(CTRL_isError(handle)) { // set the enable controller flag to false CTRL_setFlag_enableCtrl(handle,false); // set the enable system flag to false pServoVars->Flag_enableSys = false; // disable the PWM HAL_disablePwm(halHandle); } else { // update the controller state bool flag_ctrlStateChanged = CTRL_updateState(handle); // enable or disable the control CTRL_setFlag_enableCtrl(handle, gServoVars.Flag_Run_Identify); if(flag_ctrlStateChanged) { CTRL_State_e ctrlState = CTRL_getState(handle); if(ctrlState == CTRL_State_OffLine) { // enable the PWM HAL_enablePwm(halHandle); } else if(ctrlState == CTRL_State_OnLine) { if(gServoVars.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 gServoVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0); gServoVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1); gServoVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2); // Return the bias value for voltages gServoVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0); gServoVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1); gServoVars.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); gServoVars.Flag_Run_Identify = false; } if((CTRL_getFlag_enableUserMotorParams(handle) == true) && (ctrlState > CTRL_State_Idle) && (gServoVars.CtrlVersion.minor == 6)) { // call this function to fix 1p6 USER_softwareUpdate1p6(handle); } } } //!< Check if the motor was identified? if(EST_isMotorIdentified(obj->estHandle)) { // set the current ramp EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope); gServoVars.Flag_MotorIdentified = true; #if (SERVO_USING_SPINTAC_VEL == 1) // set the speed reference CTRL_setSpd_ref_krpm(handle,gServoVars.SpeedRef_krpm); // set the speed acceleration CTRL_setMaxAccel_pu(handle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gServoVars.MaxAccel_krpmps)); //!< Run the background tasks of SpinTAC Suite //! \notes Added by Duongtb61 (06-May-2017) //! // enable the SpinTAC Speed Controller. This action also disable the PI 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)); } // select the SpinTAC Speed Controller (true) or the PI Speed Controller (false) //CTRL_setFlag_enableSpeedCtrl(ctrlHandle, !gServoVars.SpinTAC.VelCtlEnb); #else // set the speed reference CTRL_setSpd_ref_krpm(handle, STPOSMOVE_getVelocityReference(stObj->posMoveHandle)); // set the speed acceleration CTRL_setMaxAccel_pu(handle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gServoVars.MaxAccel_krpmps)); // enable the SpinTAC Position Controller STPOSCTL_setEnable(stObj->posCtlHandle, true); if(EST_getState(obj->estHandle) != EST_State_OnLine) { // if the system is not running, disable SpinTAC Position Controller STPOSCTL_setEnable(stObj->posCtlHandle, false); #if (SERVO_USING_SPINTAC_POSMOVE == true) // If motor is not running, feed the position feedback into SpinTAC Position Move STPOSMOVE_setPositionStart_mrev(stObj->posMoveHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle)); #endif } #endif //end of #if (SERVO_USING_SPINTAC_VEL == 1) //!< Check for the Flag_Latch_softwareUpdate is set? if(Flag_Latch_softwareUpdate) { Flag_Latch_softwareUpdate = false; USER_calcPIgains(ctrlHandle); // initialize the watch window kp and ki current values with pre-calculated values gServoVars.Kp_Idq = CTRL_getKp(handle,CTRL_Type_PID_Id); gServoVars.Ki_Idq = CTRL_getKi(handle,CTRL_Type_PID_Id); #if (SERVO_USING_SPINTAC_VEL == 1) // initialize the watch window Bw value with the default value gServoVars.SpinTAC.VelCtlBw_radps = STVELCTL_getBandwidth_radps(stObj->velCtlHandle); // initialize the watch window with maximum and minimum Iq reference gServoVars.SpinTAC.VelCtlOutputMax_A = _IQmpy(STVELCTL_getOutputMaximum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A)); gServoVars.SpinTAC.VelCtlOutputMin_A = _IQmpy(STVELCTL_getOutputMinimum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A)); #else // initialize the watch window Bw value with the default value gServoVars.SpinTAC.PosCtlBw_radps = STPOSCTL_getBandwidth_radps(stObj->posCtlHandle); // initialize the watch window with maximum and minimum Iq reference gServoVars.SpinTAC.PosCtlOutputMax_A = _IQmpy(STPOSCTL_getOutputMaximum(stObj->posCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A)); gServoVars.SpinTAC.PosCtlOutputMin_A = _IQmpy(STPOSCTL_getOutputMinimum(stObj->posCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A)); #endif //end of #if (SERVO_USING_SPINTAC_VEL == 1) } } 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; SERVO_updateGlobalVariables(handle,stHandle); } // update Kp and Ki gains SERVO_updateKpKiGains(handle); #if (SERVO_USING_SPINTAC_VEL == 1) // set the SpinTAC (ST) bandwidth scale STVELCTL_setBandwidth_radps(stObj->velCtlHandle, gServoVars.SpinTAC.VelCtlBw_radps); // set the maximum and minimum values for Iq reference STVELCTL_setOutputMaximums(stObj->velCtlHandle, _IQmpy(gServoVars.SpinTAC.VelCtlOutputMax_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)), _IQmpy(gServoVars.SpinTAC.VelCtlOutputMin_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A))); #else // set the SpinTAC (ST) bandwidth scale STPOSCTL_setBandwidth_radps(stObj->posCtlHandle, gServoVars.SpinTAC.PosCtlBw_radps); // set the maximum and minimum values for Iq reference STPOSCTL_setOutputMaximums(stObj->posCtlHandle, _IQmpy(gServoVars.SpinTAC.PosCtlOutputMax_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)), _IQmpy(gServoVars.SpinTAC.PosCtlOutputMin_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A))); #endif //end of #if (SERVO_USING_SPINTAC_VEL == 1) // enable/disable the forced angle EST_setFlag_enableForceAngle(obj->estHandle,gServoVars.Flag_enableForceAngle); // enable or disable power warp CTRL_setFlag_enablePowerWarp(handle,gServoVars.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 return; } // end of the SERVO_runBackgroundTasks() function void SERVO_updateGlobalVariables(CTRL_Handle handle, ST_Handle sthandle) { CTRL_Obj *obj = (CTRL_Obj *)handle; ST_Obj *stObj = (ST_Obj *)sthandle; // get the speed estimate gServoVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle); #ifdef QEP // get the speed from eQEP gServoVars.SpeedQEP_krpm = _IQmpy(STPOSCONV_getVelocityFiltered(stObj->posConvHandle), _IQ(ST_SPEED_KRPM_PER_PU)); #endif //QEP // get the real time speed reference coming out of the speed trajectory generator gServoVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle),EST_get_pu_to_krpm_sf(obj->estHandle)); // get the torque estimate gServoVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf); // get the magnetizing current gServoVars.MagnCurr_A = EST_getIdRated(obj->estHandle); // get the rotor resistance gServoVars.Rr_Ohm = EST_getRr_Ohm(obj->estHandle); // get the stator resistance gServoVars.Rs_Ohm = EST_getRs_Ohm(obj->estHandle); // get the stator inductance in the direct coordinate direction gServoVars.Lsd_H = EST_getLs_d_H(obj->estHandle); // get the stator inductance in the quadrature coordinate direction gServoVars.Lsq_H = EST_getLs_q_H(obj->estHandle); // get the flux in V/Hz in floating point gServoVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle); // get the flux in Wb in fixed point gServoVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf); // get the controller state gServoVars.CtrlState = CTRL_getState(handle); // get the estimator state gServoVars.EstState = EST_getState(obj->estHandle); // Get the DC buss voltage gServoVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,_IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0)); #if (SERVO_USING_SPINTAC_VEL == true) // get the Iq Reference if(gServoVars.SpinTAC.VelIdRun) { gServoVars.IqRef_A = _IQmpy(STVELID_getTorqueReference(stObj->velIdHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A)); } else { gServoVars.IqRef_A = _IQmpy(CTRL_getSpd_out_pu(handle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A)); } // gets the Velocity Controller status gServoVars.SpinTAC.VelCtlStatus = STVELCTL_getStatus(stObj->velCtlHandle); // get the inertia setting gServoVars.SpinTAC.InertiaEstimate_Aperkrpm = _IQmpy(STVELCTL_getInertia(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A)); // get the friction setting gServoVars.SpinTAC.FrictionEstimate_Aperkrpm = _IQmpy(STVELCTL_getFriction(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A)); // get the Velocity Controller error gServoVars.SpinTAC.VelCtlErrorID = STVELCTL_getErrorID(stObj->velCtlHandle); #else // get the Iq reference from the position controller gServoVars.IqRef_A = _IQmpy(STPOSCTL_getTorqueReference(stObj->posCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A)); // gets the Position Controller status gServoVars.SpinTAC.PosCtlStatus = STPOSCTL_getStatus(stObj->posCtlHandle); // get the inertia setting gServoVars.SpinTAC.InertiaEstimate_Aperkrpm = _IQmpy(STPOSCTL_getInertia(stObj->posCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A)); // get the friction setting gServoVars.SpinTAC.FrictionEstimate_Aperkrpm = _IQmpy(STPOSCTL_getFriction(stObj->posCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A)); // get the Position Controller error gServoVars.SpinTAC.PosCtlErrorID = STPOSCTL_getErrorID(stObj->posCtlHandle); // get the Position Move status gServoVars.SpinTAC.PosMoveStatus = STPOSMOVE_getStatus(stObj->posMoveHandle); // get the Position Move profile time uint32_t ProTime_tick, ProTime_mtick; STPOSMOVE_getProfileTime_tick(stObj->posMoveHandle, &ProTime_tick, &ProTime_mtick); gServoVars.SpinTAC.PosMoveTime_ticks = ProTime_tick; gServoVars.SpinTAC.PosMoveTime_mticks = ProTime_mtick; // get the Position Move error gServoVars.SpinTAC.PosMoveErrorID = STPOSMOVE_getErrorID(stObj->posMoveHandle); // get the Position Converter error gServoVars.SpinTAC.PosConvErrorID = STPOSCONV_getErrorID(stObj->posConvHandle); #endif //end of #if (SERVO_USING_SPINTAC_VEL == true) #ifdef QEP // get the Position Converter error gServoVars.SpinTAC.PosConvErrorID = STPOSCONV_getErrorID(stObj->posConvHandle); #endif //QEP return; } // end of SERVO_updateGlobalVariables() function void SERVO_updateKpKiGains(CTRL_Handle handle) { if((gServoVars.CtrlState == CTRL_State_OnLine) && (gServoVars.Flag_MotorIdentified == true) && (Flag_Latch_softwareUpdate == false)) { // set the kp and ki speed values from the watch window CTRL_setKp(handle,CTRL_Type_PID_spd,gServoVars.Kp_spd); CTRL_setKi(handle,CTRL_Type_PID_spd,gServoVars.Ki_spd); // set the kp and ki current values for Id and Iq from the watch window CTRL_setKp(handle,CTRL_Type_PID_Id,gServoVars.Kp_Idq); CTRL_setKi(handle,CTRL_Type_PID_Id,gServoVars.Ki_Idq); CTRL_setKp(handle,CTRL_Type_PID_Iq,gServoVars.Kp_Idq); CTRL_setKi(handle,CTRL_Type_PID_Iq,gServoVars.Ki_Idq); } return; } // end of SERVO_updateKpKiGains() function void SERVO_setupCtrlParams(CTRL_Handle handle,USER_Params *pUserParams) { CTRL_Obj *obj = (CTRL_Obj *)handle; _iq maxCurrent_pu = _IQ(USER_MOTOR_MAX_CURRENT / USER_IQ_FULL_SCALE_CURRENT_A); _iq maxVoltage_pu = _IQ(USER_MAX_VS_MAG_PU * USER_VD_SF); float_t fullScaleCurrent = USER_IQ_FULL_SCALE_CURRENT_A; float_t fullScaleVoltage = USER_IQ_FULL_SCALE_VOLTAGE_V; float_t IsrPeriod_sec = 1.0 / USER_ISR_FREQ_Hz; float_t Ls_d = USER_MOTOR_Ls_d; float_t Ls_q = USER_MOTOR_Ls_q; float_t Rs = USER_MOTOR_Rs; float_t RoverLs_d = Rs/Ls_d; //The bandwidth of current loop for d-channel float_t RoverLs_q = Rs/Ls_q; //The bandwidth of current loop for q-channel #if (SERVO_USING_TI_PARAMETERS == 1) _iq Kp_Id = _IQ((0.25*Ls_d*fullScaleCurrent)/(IsrPeriod_sec*fullScaleVoltage)); _iq Ki_Id = _IQ(RoverLs_d*IsrPeriod_sec); _iq Kp_Iq = _IQ((0.25*Ls_q*fullScaleCurrent)/(IsrPeriod_sec*fullScaleVoltage)); _iq Ki_Iq = _IQ(RoverLs_q*IsrPeriod_sec); #else _iq Kp_Id = _IQ((Ls_d*RoverLs_d*fullScaleCurrent)/fullScaleVoltage); _iq Ki_Id = _IQ(RoverLs_d*IsrPeriod_sec); _iq Kp_Iq = _IQ((Ls_q*RoverLs_q*fullScaleCurrent)/fullScaleVoltage); _iq Ki_Iq = _IQ(RoverLs_q*IsrPeriod_sec); #endif // set the optimal Id PID controller parameters PID_setGains(obj->pidHandle_Id,Kp_Id,Ki_Id,_IQ(0.0)); PID_setUi(obj->pidHandle_Id,_IQ(0.0)); PID_setMinMax(obj->pidHandle_Id,-maxVoltage_pu,maxVoltage_pu); CTRL_setGains(handle,CTRL_Type_PID_Id,Kp_Id,Ki_Id,_IQ(0.0)); // set the optimal the Iq PID controller parameters PID_setGains(obj->pidHandle_Iq,Kp_Iq,Ki_Iq,_IQ(0.0)); PID_setUi(obj->pidHandle_Iq,_IQ(0.0)); PID_setMinMax(obj->pidHandle_Iq,-maxVoltage_pu,maxVoltage_pu); CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp_Iq,Ki_Iq,_IQ(0.0)); #if (SERVO_USING_TI_PARAMETERS == 1) // set the optimal speed PID controller parameters _iq Kp_spd = _IQ(0.05*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz/pUserParams->iqFullScaleCurrent_A); _iq Ki_spd = _IQ(0.78*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz*pUserParams->ctrlPeriod_sec/pUserParams->iqFullScaleCurrent_A); _iq Kd_spd = _IQ(0.0); #else // set the optimal speed PID controller parameters _iq Kp_spd = _IQ(0.08*2*MATH_PI*pUserParams->iqFullScaleFreq_Hz \ /(pUserParams->motor_numPolePairs*pUserParams->iqFullScaleCurrent_A)); _iq Ki_spd = _IQ(3.55*2*MATH_PI*pUserParams->iqFullScaleFreq_Hz*pUserParams->iqFullScaleFreq_Hz*pUserParams->ctrlPeriod_sec \ /(pUserParams->motor_numPolePairs*pUserParams->iqFullScaleCurrent_A)); _iq Kd_spd = _IQ(0.0); #endif PID_setGains(obj->pidHandle_spd,Kp_spd,Ki_spd,Kd_spd); PID_setUi(obj->pidHandle_spd,_IQ(0.0)); PID_setMinMax(obj->pidHandle_spd,-maxCurrent_pu,maxCurrent_pu); CTRL_setGains(handle,CTRL_Type_PID_spd,Kp_spd,Ki_spd,Kd_spd); return; } // end of the SERVO_setCtrlParams() function #if ((BUILDLEVEL == LEVEL1)||(BUILDLEVEL == LEVEL2)) void SERVO_runVpHzOpenloopCtrl(CTRL_Handle handle,HAL_Handle halHandle, const HAL_AdcData_t *pAdcData,HAL_PwmData_t *pPwmData) { CTRL_Obj *obj = (CTRL_Obj *)handle; // run the V/Hz controller in the Jogging Mode uint_least16_t count_isr = CTRL_getCount_isr(handle); uint_least16_t numIsrTicksPerCtrlTick = CTRL_getNumIsrTicksPerCtrlTick(handle); // if needed, run the controller if(count_isr >= numIsrTicksPerCtrlTick) { CTRL_State_e ctrlState = CTRL_getState(handle); // reset the isr count CTRL_resetCounter_isr(handle); // increment the state counter CTRL_incrCounter_state(handle); // increment the trajectory count CTRL_incrCounter_traj(handle); // run the appropriate controller if(ctrlState == CTRL_State_OnLine) { // increment the current count CTRL_incrCounter_current(handle); // increment the speed count CTRL_incrCounter_speed(handle); MATH_vec2 phasor; // run Clarke transform on current CLARKE_run(obj->clarkeHandle_I,&(pAdcData->I),CTRL_getIab_in_addr(handle)); // run Clarke transform on voltage CLARKE_run(obj->clarkeHandle_V,&(pAdcData->V),CTRL_getVab_in_addr(handle)); // get the reference speed/frequency obj->speed_ref_pu = TRAJ_getIntValue(obj->trajHandle_spd); // run V/Hz profiler ANGLE_GEN_run(angle_genHandle, obj->speed_ref_pu); VS_FREQ_run(vs_freqHandle, obj->speed_ref_pu); // generate the motor electrical angle obj->angle_pu = ANGLE_GEN_getAngle_pu(angle_genHandle); obj->Vdq_out.value[0] = vs_freq.Vdq_out.value[0]; obj->Vdq_out.value[1] = vs_freq.Vdq_out.value[1]; // compute the sin/cos phasor CTRL_computePhasor(obj->angle_pu,&phasor); // set the phasor in the Park transform PARK_setPhasor(obj->parkHandle,&phasor); // run the Park transform PARK_run(obj->parkHandle,CTRL_getIab_in_addr(handle),CTRL_getIdq_in_addr(handle)); // set the phasor in the inverse Park transform IPARK_setPhasor(obj->iparkHandle,&phasor); // run the inverse Park module IPARK_run(obj->iparkHandle,CTRL_getVdq_out_addr(handle),CTRL_getVab_out_addr(handle)); // run the space Vector Generator (SVGEN) module SVGEN_run(obj->svgenHandle,CTRL_getVab_out_addr(handle),&(pPwmData->Tabc)); } else if(ctrlState == CTRL_State_OffLine) { // run the offline controller CTRL_runOffLine(handle,halHandle,pAdcData,pPwmData); } else if(ctrlState == CTRL_State_Idle) { // set all pwm outputs to zero pPwmData->Tabc.value[0] = _IQ(0.0); pPwmData->Tabc.value[1] = _IQ(0.0); pPwmData->Tabc.value[2] = _IQ(0.0); } } else { // increment the isr count CTRL_incrCounter_isr(handle); } return; } // end of the SERVO_runVpHzOpenloopCtrl() function. #endif // end of #if (BUILDLEVEL == LEVEL2) #if (BUILDLEVEL == LEVEL3) void SERVO_runVpHzWithCurrentCtrl(CTRL_Handle handle,HAL_Handle halHandle, SERVO_Vars_t *pServoVars, const HAL_AdcData_t *pAdcData,HAL_PwmData_t *pPwmData) { CTRL_Obj *obj = (CTRL_Obj *)handle; // run the V/Hz controller with current closed loop uint_least16_t count_isr = CTRL_getCount_isr(handle); uint_least16_t numIsrTicksPerCtrlTick = CTRL_getNumIsrTicksPerCtrlTick(handle); // if needed, run the controller if(count_isr >= numIsrTicksPerCtrlTick) { CTRL_State_e ctrlState = CTRL_getState(handle); // reset the isr count CTRL_resetCounter_isr(handle); // increment the state counter CTRL_incrCounter_state(handle); // increment the trajectory count CTRL_incrCounter_traj(handle); // run the appropriate controllers if(ctrlState == CTRL_State_OnLine) { // increment the current count CTRL_incrCounter_current(handle); // increment the speed count CTRL_incrCounter_speed(handle); MATH_vec2 phasor; // run Clarke transform on current CLARKE_run(obj->clarkeHandle_I,&(pAdcData->I),CTRL_getIab_in_addr(handle)); // run Clarke transform on voltage CLARKE_run(obj->clarkeHandle_V,&(pAdcData->V),CTRL_getVab_in_addr(handle)); // get the reference speed/frequency obj->speed_ref_pu = TRAJ_getIntValue(obj->trajHandle_spd); ANGLE_GEN_run(angle_genHandle, obj->speed_ref_pu); // generate the motor electrical angle obj->angle_pu = ANGLE_GEN_getAngle_pu(angle_genHandle); // set the reference values of current loop CTRL_setId_ref_pu(handle, pServoVars->IdRef_pu); CTRL_setIq_ref_pu(handle, pServoVars->IqRef_pu); // compute the sin/cos phasor CTRL_computePhasor(obj->angle_pu,&phasor); // set the phasor in the Park transform PARK_setPhasor(obj->parkHandle,&phasor); // run the Park transform for the current control loop PARK_run(obj->parkHandle,CTRL_getIab_in_addr(handle),CTRL_getIdq_in_addr(handle)); // when appropriate, run the PID Id and Iq controllers if(CTRL_doCurrentCtrl(handle)) { _iq refValue; _iq fbackValue; _iq outMin,outMax; // read max voltage vector to set proper limits to current controllers _iq maxVsMag = CTRL_getMaxVsMag_pu(handle); // reset the current count CTRL_resetCounter_current(handle); // *********************************** // configure and run the Id controller // compute the reference value refValue = TRAJ_getIntValue(obj->trajHandle_Id) + CTRL_getId_ref_pu(handle); // update the Id reference value EST_updateId_ref_pu(obj->estHandle,&refValue); // get the feedback value fbackValue = CTRL_getId_in_pu(handle); // set minimum and maximum for Id controller output outMax = maxVsMag; outMin = -outMax; // set the minimum and maximum values PID_setMinMax(obj->pidHandle_Id,outMin,outMax); // run the Id PID controller PID_run(obj->pidHandle_Id,refValue,fbackValue,CTRL_getVd_out_addr(handle)); // *********************************** // configure and run the Iq controller // get the reference value if(CTRL_getFlag_enableSpeedCtrl(handle)) { refValue = CTRL_getSpd_out_pu(handle); } else { // get the Iq reference value refValue = CTRL_getIq_ref_pu(handle); } // get the feedback value fbackValue = CTRL_getIq_in_pu(handle); // set minimum and maximum for Id controller output outMax = _IQsqrt(_IQmpy(maxVsMag,maxVsMag) - _IQmpy(CTRL_getVd_out_pu(handle),CTRL_getVd_out_pu(handle))); outMin = -outMax; // set the minimum and maximum values PID_setMinMax(obj->pidHandle_Iq,outMin,outMax); // run the Iq PID controller PID_run(obj->pidHandle_Iq,refValue,fbackValue,CTRL_getVq_out_addr(handle)); } // set the phasor in the inverse Park transform IPARK_setPhasor(obj->iparkHandle,&phasor); // run the inverse Park module IPARK_run(obj->iparkHandle,CTRL_getVdq_out_addr(handle),CTRL_getVab_out_addr(handle)); // run the space Vector Generator (SVGEN) module SVGEN_run(obj->svgenHandle,CTRL_getVab_out_addr(handle),&(pPwmData->Tabc)); } else if(ctrlState == CTRL_State_OffLine) { // run the offline controller CTRL_runOffLine(handle,halHandle,pAdcData,pPwmData); } else if(ctrlState == CTRL_State_Idle) { // set all pwm outputs to zero pPwmData->Tabc.value[0] = _IQ(0.0); pPwmData->Tabc.value[1] = _IQ(0.0); pPwmData->Tabc.value[2] = _IQ(0.0); } } else { // increment the isr count CTRL_incrCounter_isr(handle); } return; } // end of the SERVO_runVpHzlWithCurrentCtrl() function #endif //end of #if (BUILDLEVEL == LEVEL3) #if ((BUILDLEVEL == LEVEL4)) void SERVO_runSensorlessCtrl(CTRL_Handle handle,HAL_Handle halHandle, const HAL_AdcData_t *pAdcData, HAL_PwmData_t *pPwmData) { uint_least16_t count_isr = CTRL_getCount_isr(handle); uint_least16_t numIsrTicksPerCtrlTick = CTRL_getNumIsrTicksPerCtrlTick(handle); // if needed, run the controller if(count_isr >= numIsrTicksPerCtrlTick) { CTRL_State_e ctrlState = CTRL_getState(handle); // reset the isr count CTRL_resetCounter_isr(handle); // increment the state counter CTRL_incrCounter_state(handle); // increment the trajectory count CTRL_incrCounter_traj(handle); // run the appropriate controller if(ctrlState == CTRL_State_OnLine) { CTRL_Obj *obj = (CTRL_Obj *)handle; // increment the current count CTRL_incrCounter_current(handle); // increment the speed count CTRL_incrCounter_speed(handle); if(EST_getState(obj->estHandle) >= EST_State_MotorIdentified) { // run the online controller CTRL_runOnLine_User(handle,pAdcData,pPwmData); } else { // run the online controller CTRL_runOnLine(handle,pAdcData,pPwmData); } } else if(ctrlState == CTRL_State_OffLine) { // run the offline controller CTRL_runOffLine(handle,halHandle,pAdcData,pPwmData); } else if(ctrlState == CTRL_State_Idle) { // set all pwm outputs to zero pPwmData->Tabc.value[0] = _IQ(0.0); pPwmData->Tabc.value[1] = _IQ(0.0); pPwmData->Tabc.value[2] = _IQ(0.0); } } else { // increment the isr count CTRL_incrCounter_isr(handle); } return; } // end of SERVO_runSensorlessCtrl() function #endif //end of #if ((BUILDLEVEL == LEVEL4)||(BUILDLEVEL == LEVEL5)||(BUILDLEVEL == LEVEL6)||(BUILDLEVEL == LEVEL7)) #ifdef QEP void SERVO_runSpinTACPosConv(ST_Handle handle, ENC_Handle encHandle, CTRL_Handle ctrlHandle) { ST_Obj *stObj = (ST_Obj *)handle; // get the electrical angle from the ENC module STPOSCONV_setElecAngle_erev(stObj->posConvHandle, ENC_getElecAngle(encHandle)); if(USER_MOTOR_TYPE == MOTOR_Type_Induction) { // The CurrentVector feedback is only needed for ACIM // get the vector of the direct/quadrature current input vector values from CTRL STPOSCONV_setCurrentVector(stObj->posConvHandle, CTRL_getIdq_in_addr(ctrlHandle)); } // run the SpinTAC Position Converter STPOSCONV_run(stObj->posConvHandle); if(USER_MOTOR_TYPE == MOTOR_Type_Induction) { // The Slip Velocity is only needed for ACIM // update the slip velocity in electrical angle per second, Q24 SLIP_setSlipVelocity(slipHandle, STPOSCONV_getSlipVelocity(stObj->posConvHandle)); } } #endif //QEP #if ((BUILDLEVEL == LEVEL5)||(BUILDLEVEL == LEVEL6)||(BUILDLEVEL == LEVEL7)) #if (SERVO_USING_SPINTAC_VEL == true) void SERVO_runSpinTACVelMove(ST_Handle handle, CTRL_Handle ctrlHandle) { ST_Obj *stObj = (ST_Obj *)handle; CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle; // Run SpinTAC Move // If we are not in reset, and the SpeedRef_krpm has been modified if((EST_getState(ctrlObj->estHandle) == EST_State_OnLine) && (_IQmpy(gServoVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)) != STVELMOVE_getVelocityEnd(stObj->velMoveHandle))) { // Get the configuration for SpinTAC Move STVELMOVE_setCurveType(stObj->velMoveHandle, gServoVars.SpinTAC.VelMoveCurveType); STVELMOVE_setVelocityEnd(stObj->velMoveHandle, _IQmpy(gServoVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM))); STVELMOVE_setAccelerationLimit(stObj->velMoveHandle, _IQmpy(gServoVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); STVELMOVE_setJerkLimit(stObj->velMoveHandle, _IQ20mpy(gServoVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM))); // Enable SpinTAC Move STVELMOVE_setEnable(stObj->velMoveHandle, true); // If starting from zero speed, enable ForceAngle, otherwise disable ForceAngle if(_IQabs(STVELMOVE_getVelocityStart(stObj->velMoveHandle)) < _IQ(ST_MIN_ID_SPEED_PU)) { EST_setFlag_enableForceAngle(ctrlObj->estHandle, true); gServoVars.Flag_enableForceAngle = true; } else { EST_setFlag_enableForceAngle(ctrlObj->estHandle, false); gServoVars.Flag_enableForceAngle = false; } } // Call STVELMOVE_run() which is placed in ROM. STVELMOVE_run(stObj->velMoveHandle); return; } //end of the SERVO_runSpinTACVelMove() function void SERVO_runSpinTACVelCtrl(ST_Handle handle, CTRL_Handle ctrlHandle) { _iq speedFeedback, iqReference; ST_Obj *stObj = (ST_Obj *)handle; CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle; #ifdef QEP // Get the mechanical speed in pu from SpinTAC-Position Converter // Revised by Duongtb61 (08-May-2017) speedFeedback = STPOSCONV_getVelocityFiltered(stObj->posConvHandle); #else // Get the mechanical speed in pu from FAST estimator speedFeedback = EST_getFm_pu(ctrlObj->estHandle); #endif //QEP // Run the SpinTAC Controller if(gServoVars.RunVelocityProfile == true) { // Use the Velocity Reference from SpinTAC-Move STVELCTL_setVelocityReference(stObj->velCtlHandle, STVELMOVE_getVelocityReference(stObj->velMoveHandle)); // Use the Acceleration Reference which is provided by SpinTAC-Move STVELCTL_setAccelerationReference(stObj->velCtlHandle, STVELMOVE_getAccelerationReference(stObj->velMoveHandle)); } else { // Note that the library internal ramp generator is used to set the speed reference STVELCTL_setVelocityReference(stObj->velCtlHandle, TRAJ_getIntValue(ctrlObj->trajHandle_spd)); // Internal ramp generator does not provide Acceleration Reference STVELCTL_setAccelerationReference(stObj->velCtlHandle, _IQ(0.0)); } // Assign the velocity feedback source STVELCTL_setVelocityFeedback(stObj->velCtlHandle, speedFeedback); // Call STVELCTRL_run() which is placed in ROM. STVELCTL_run(stObj->velCtlHandle); // select SpinTAC Velocity Controller iqReference = STVELCTL_getTorqueReference(stObj->velCtlHandle); // Set the Iq reference that came out of SpinTAC Velocity Control CTRL_setIq_ref_pu(ctrlHandle, iqReference); return; } // end of the SERVO_runSpinTACVelCtrl() function #else void SERVO_runSpinTACPosCtl(ST_Handle handle, CTRL_Handle ctrlHandle) { ST_Obj *stObj = (ST_Obj *)handle; _iq iqReference; // provide the updated references to the SpinTAC Position Control STPOSCTL_setPositionReference_mrev(stObj->posCtlHandle, STPOSMOVE_getPositionReference_mrev(stObj->posMoveHandle)); STPOSCTL_setVelocityReference(stObj->posCtlHandle, STPOSMOVE_getVelocityReference(stObj->posMoveHandle)); STPOSCTL_setAccelerationReference(stObj->posCtlHandle, STPOSMOVE_getAccelerationReference(stObj->posMoveHandle)); #ifdef QEP // provide the feedback to the SpinTAC Position Control STPOSCTL_setPositionFeedback_mrev(stObj->posCtlHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle)); #else CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle; // provide the position information from FAST Estimator STPOSCTL_setPositionFeedback_mrev(stObj->posCtlHandle, EST_getAngle_pu(obj->estHandle)); #endif // Run SpinTAC Position Control STPOSCTL_run(stObj->posCtlHandle); // select SpinTAC Velocity Controller iqReference = STPOSCTL_getTorqueReference(stObj->posCtlHandle); // Provide SpinTAC Position Control Torque Output to the FOC CTRL_setIq_ref_pu(ctrlHandle, iqReference); return; } // end of the SERVO_runSpinTACPosCtl() function void SERVO_runSpinTACPosMoveForVelocityMode(ST_Handle handle) { ST_Obj *stObj = (ST_Obj *)handle; // Run SpinTAC Position Profile Generator // If we are not running a profile, and the PosStep_MRev has been modified if(_IQmpy(gServoVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)) != STPOSMOVE_getVelocityEnd(stObj->posMoveHandle)) { // Get the configuration for SpinTAC Velocity Profile Generator STPOSMOVE_setCurveType(stObj->posMoveHandle, gServoVars.SpinTAC.PosMoveCurveType); // Choose the Velocity mode for Profile Generator STPOSMOVE_setProfileType(stObj->posMoveHandle, ST_POS_MOVE_VEL_TYPE); STPOSMOVE_setVelocityEnd(stObj->posMoveHandle, _IQmpy(gServoVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM))); // Setup the motion limits STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gServoVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM))); // Enable the SpinTAC Position Profile Generator STPOSMOVE_setEnable(stObj->posMoveHandle, true); } STPOSMOVE_run(stObj->posMoveHandle); return; } // end of the SERVO_runSpinTACPosMoveForVelocityMode() function void SERVO_runSpinTACPosMoveForPositionMode(ST_Handle handle) { ST_Obj *stObj = (ST_Obj *)handle; // Run SpinTAC Position Profile Generator // If we are not running a profile, and command indicates we should has been modified if((STPOSMOVE_getStatus(stObj->posMoveHandle) == ST_MOVE_IDLE) && (gServoVars.RunPositionProfile == true)) { // Get the configuration for SpinTAC Position Move STPOSMOVE_setCurveType(stObj->posMoveHandle, gServoVars.SpinTAC.PosMoveCurveType); STPOSMOVE_setPositionStep_mrev(stObj->posMoveHandle, gServoVars.PosStepInt_MRev, gServoVars.PosStepFrac_MRev); //the position step [Mrev] STPOSMOVE_setVelocityLimit(stObj->posMoveHandle, _IQmpy(gServoVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM))); //the velocity limit [pu/s] STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); //the acceleration limit [pu/s^2] STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); //the deceleration limit [pu/s^2] STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gServoVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM))); //the Jerk limit [pu/s^3] // Enable the SpinTAC Position Profile Generator STPOSMOVE_setEnable(stObj->posMoveHandle, true); // clear the position step command gServoVars.PosStepInt_MRev = 0; gServoVars.PosStepFrac_MRev = 0; gServoVars.RunPositionProfile = false; } STPOSMOVE_run(stObj->posMoveHandle); return; } // end of the SERVO_runSpinTACPosMoveForPositionMode() function #endif #endif //end of #if (BUILDLEVEL == LEVEL5) void ST_runPosConv(ST_Handle handle, ENC_Handle encHandle, CTRL_Handle ctrlHandle) { ST_Obj *stObj = (ST_Obj *)handle; // get the electrical angle from the ENC module STPOSCONV_setElecAngle_erev(stObj->posConvHandle, ENC_getElecAngle(encHandle)); if(USER_MOTOR_TYPE == MOTOR_Type_Induction) { // The CurrentVector feedback is only needed for ACIM // get the vector of the direct/quadrature current input vector values from CTRL STPOSCONV_setCurrentVector(stObj->posConvHandle, CTRL_getIdq_in_addr(ctrlHandle)); } // run the SpinTAC Position Converter STPOSCONV_run(stObj->posConvHandle); if(USER_MOTOR_TYPE == MOTOR_Type_Induction) { // The Slip Velocity is only needed for ACIM // update the slip velocity in electrical angle per second, Q24 SLIP_setSlipVelocity(slipHandle, STPOSCONV_getSlipVelocity(stObj->posConvHandle)); } } void ST_runPosCtl(ST_Handle handle, CTRL_Handle ctrlHandle) { ST_Obj *stObj = (ST_Obj *)handle; // provide the updated references to the SpinTAC Position Control STPOSCTL_setPositionReference_mrev(stObj->posCtlHandle, STPOSMOVE_getPositionReference_mrev(stObj->posMoveHandle)); STPOSCTL_setVelocityReference(stObj->posCtlHandle, STPOSMOVE_getVelocityReference(stObj->posMoveHandle)); STPOSCTL_setAccelerationReference(stObj->posCtlHandle, STPOSMOVE_getAccelerationReference(stObj->posMoveHandle)); // provide the feedback to the SpinTAC Position Control STPOSCTL_setPositionFeedback_mrev(stObj->posCtlHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle)); // Run SpinTAC Position Control STPOSCTL_run(stObj->posCtlHandle); // Provide SpinTAC Position Control Torque Output to the FOC CTRL_setIq_ref_pu(ctrlHandle, STPOSCTL_getTorqueReference(stObj->posCtlHandle)); } #if (SERVO_USING_SPINTAC_POS == 1) void ST_runPosMove(ST_Handle handle) { ST_Obj *stObj = (ST_Obj *)handle; // Run SpinTAC Position Profile Generator // If we are not running a profile, and command indicates we should has been modified if((STPOSMOVE_getStatus(stObj->posMoveHandle) == ST_MOVE_IDLE) && (gServoVars.RunPositionProfile == true)) { // Get the configuration for SpinTAC Position Move STPOSMOVE_setCurveType(stObj->posMoveHandle, gServoVars.SpinTAC.PosMoveCurveType); STPOSMOVE_setPositionStep_mrev(stObj->posMoveHandle, gServoVars.PosStepInt_MRev, gServoVars.PosStepFrac_MRev); STPOSMOVE_setVelocityLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM))); STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gServoVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM))); // Enable the SpinTAC Position Profile Generator STPOSMOVE_setEnable(stObj->posMoveHandle, true); // clear the position step command gServoVars.PosStepInt_MRev = 0; gServoVars.PosStepFrac_MRev = 0; gServoVars.RunPositionProfile = false; } STPOSMOVE_run(stObj->posMoveHandle); } #else void ST_runPosMove(ST_Handle handle) { ST_Obj *stObj = (ST_Obj *)handle; // Run SpinTAC Position Profile Generator // If we are not running a profile, and the PosStep_MRev has been modified if(_IQmpy(gServoVars.MaxVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM)) != STPOSMOVE_getVelocityEnd(stObj->posMoveHandle)) { // Get the configuration for SpinTAC Velocity Profile Generator STPOSMOVE_setCurveType(stObj->posMoveHandle, gServoVars.SpinTAC.PosMoveCurveType); STPOSMOVE_setProfileType(stObj->posMoveHandle, ST_POS_MOVE_VEL_TYPE); STPOSMOVE_setVelocityEnd(stObj->posMoveHandle, _IQmpy(gServoVars.MaxVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM))); STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gServoVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM))); STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gServoVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM))); // Enable the SpinTAC Position Profile Generator STPOSMOVE_setEnable(stObj->posMoveHandle, true); } STPOSMOVE_run(stObj->posMoveHandle); } #endif //! \brief Write a data to the SCI-A interface //! \param[in] handle The hardware abstraction layer (HAL) handle //! \param[in] data The data which need to send //! \notes This function is served for rprintf() and cmdLine() functions //! \notes Added by Duongtb61 (5-May-2017) //! void SERVO_sciaSendByte(unsigned char data) { HAL_Obj *obj = (HAL_Obj *)halHandle; SCI_putDataBlocking(obj->sciAHandle, (uint16_t)data); return; } // end of the SERVO_sciaSendByte() function #ifdef CODE_HIEU void SERVO_initScibBuffer(void) { WORD i = 0; for (i = 0; i < SCI_BUFFER_SIZE; i++) { scib_buffer.data[i] = '\0'; } scib_buffer.value = '\0'; scib_buffer.flag = false; scib_buffer.index = 0; } void SERVO_initCommandData(void) { WORD i = 0; for (i = 0; i < SCI_BUFFER_SIZE; i++) { commandData.data[i] = '\0'; } commandData.value = '\0'; commandData.flag = false; commandData.index = 0; } #endif //CODE_HIEU //-------------------------------------------------------------------------------------- // I N T E R R U P T S E R V I C E R O U T I N E S / F U N C T I O N S (ISR) //-------------------------------------------------------------------------------------- #if (SERVO_USING_SCIARXINT == true) //! \brief The interrupt handle for SCIB-RX //! \notes Written by Duongtb61 //! interrupt void HAL_sciARxISR(void) { HAL_Obj *obj = (HAL_Obj *)halHandle; dataRx = SCI_getDataNonBlocking(halHandle->sciAHandle, &success); //echo back for testing success = SCI_putDataNonBlocking(halHandle->sciAHandle, dataRx); //acknowledge interrupt from SCI group so that SCI interrupt is not received twice SCI_clearRxFifoOvf(halHandle->sciAHandle); SCI_clearRxFifoInt(halHandle->sciAHandle); PIE_clearInt(obj->pieHandle, PIE_GroupNumber_9); return; } // end of HAL_sciARxISR() function #endif // end of #if (SERVO_USING_SCIARXINT == true) //! \brief The interrupt handle for SCIB-RX //! \notes Written by hieutd1 //! interrupt void HAL_sciBRxISR(void) { #if (SERVO_TEST_SCIBRX == true) HAL_toggleLed(halHandle,HAL_LED_ERR_PIN); #endif //DEBUG #ifdef CODE_HIEU if(SCI_rxDataReady(halHandle->sciBHandle) == true) { scib_buffer.value = SCI_read(halHandle->sciBHandle); } if(scib_buffer.flag == false) { switch(scib_buffer.index) { case 0: if((scib_buffer.value == '^')||(scib_buffer.value == '!')||(scib_buffer.value == '~')||(scib_buffer.value == '?')) { scib_buffer.data[scib_buffer.index++] = scib_buffer.value; } break; default: scib_buffer.data[scib_buffer.index++] = scib_buffer.value; if((scib_buffer.value == 0x0D)&&(scib_buffer.index < SCI_BUFFER_SIZE)) { scib_buffer.flag = true; } else if(scib_buffer.index >= SCI_BUFFER_SIZE) { SERVO_initScibBuffer(); } break; } } #endif //CODE_HIEU //!> acknowledge the SCIB-RX interrupt SCI_clearRxFifoOvf(halHandle->sciBHandle); SCI_clearRxFifoInt(halHandle->sciBHandle); PIE_clearInt(halHandle->pieHandle,PIE_GroupNumber_9); return; } // end of HAL_sciBRxISR() function. #if (SERVO_USING_TZINT == true) //! \brief This function is Trip-zone interrupt linked to ePWM1 //! \detail This function will manage Short-circuit error which is sent from DRV8305 //! \notes Added by Duongtb (11-Jan-2016) //! interrupt void HAL_pwm1TzISR(void) { // acknowledge the epwm1_TZ interrupt HAL_acqPwmTzInt(halHandle,PWM_Number_1); #if (SERVO_TEST_TZINT == true) // Counts the number of TZ interrupt for testing gTzCnt++; // Flips the LED_RED LED_RED_TOGGLE(); //HAL_DelayMs(100); #endif // Check for an assurance of driver fault if(gServoVars.DriverFaultCount++ >= 5) { // set the Error flag to signal to the background loop gServoVars.Flag_drvIsFault = true; // Reset the counter gServoVars.DriverFaultCount = 0; } return; } //end of HAL_pwm1TZISR() function #endif // end of #if (SERVO_USING_TZINT == true) #if (SERVO_USING_TIMER0INT == true) //! HAL_Timer0ISR() function //! \notes The timer0 is setup to tick at 1mS //! interrupt void HAL_timer0ISR(void) { // Acknowledge this interrupt to receive more interrupts from group 1 HAL_acqTimer0Int(halHandle); //! \brief Do a time-out if be actived //! \notes The gTimerDelayMs is in mSec. Added by Duongtb61 (12-Apr-2017) //! if (gTimerDelayMs > 0) { gTimerDelayMs--; if(gTimerDelayMs == 0) { gFlag_TimerDelayMsIsOver = true; } } //! \brief Do a 2nd soft timer if (gTimerButton > 0) { gTimerButton--; if(gTimerButton == 0) { gFlag_TimeButtonIsOver = true; } } #if (SERVO_TEST_TIMER0_ISR == true) gLEDcnt++; if(gLEDcnt++ >= 1000) { LED_RED_TOGGLE(); gLEDcnt = 0; } #endif return; } //end of HAL_timer0ISR() function #endif // end of #if (SERVO_USING_TIMER0INT == true) #if (SERVO_USING_TIMER1INT == true) //! \brief The interrupt handle for Timer1_ISR //! \notes The timer1 is setup to tick at 10mS //! interrupt void HAL_timer1ISR(void) { //Do something in the interrupt handler #if (SERVO_USING_ERR_CHECK == true) //static uint_least8_t errTick = 0; //if(errTick++ >=1) { #ifdef ONESERVO // read the monitoring parameters to protect the system gServoVars.VdcBus_V = HAL_getDcbusVoltage_V(halHandle); gServoVars.IdcBus_A = HAL_getDcBusCurrent_A(halHandle); gServoVars.Theatsink_C = HAL_getTempAtHeatsink_C(halHandle); #else gServoVars.VdcBus_V = HAL_getDcbusVoltage_V(halHandle); gServoVars.IdcBus_A = _IQ(0.5); gServoVars.Theatsink_C = _IQ(25.0); #endif //ONESERVO // call the SERVO_checkForErrors() to check for any system error that maybe occurred SERVO_checkForErrors(&gServoVars); // reload the counter //errTick = 0; } #endif // end of #if (SERVO_USING_ERR_CHECK == true) #if (SERVO_TEST_TIMER1_ISR == true) gLEDcnt++; if(gLEDcnt++ >= 200) { LED_RED_TOGGLE(); gLEDcnt = 0; } #endif return; } //end of HAL_timer1ISR() function #endif // end of #if (SERVO_USING_TIMER1INT == true) #if (SERVO_USING_TIMER2INT == true) //! \brief The interrupt handle for Timer2_ISR //! \notes The timer2 is setup to tick at 50mS //! interrupt void HAL_timer2ISR(void) { //Do something in the interrupt handler #if (SERVO_TEST_TIMER2_ISR == true) gLEDcnt2++; if(gLEDcnt2++ >= 30) { LED_RED_TOGGLE(); gLEDcnt2 = 0; } #endif return; } //end of HAL_timer2ISR() function #endif //end of #if (SERVO_USING_TIMER2INT == true) #if (SERVO_USING_XINT1 == true) interrupt void HAL_ext1ISR(void) { // Acknowledge this interrupt to receive more interrupts from group 1 HAL_acqExt1Int(halHandle); //Do something in the interrupt handler #if (SERVO_TEST_XINT1 == true) LED_RED_TOGGLE(); #endif return; } // end of HAL_ext1ISR() function #endif // end of #if (SERVO_USING_XINT1 == true) // end of file. No more and have fun :) !!!