/**************************************************************************************************/ /** * @file CddStp.c * @brief CddStp example file. * @version V1.0.0 * @date 2024 * @author Jiangweih * * @note * Copyright (C) 2024. * **************************************************************************************************/ #include "CddStp.h" #include "Rte_ctadCddStp.h" //============================================================================= // defines //============================================================================= // usage of angle #define STP_GEAR_LV0_ANGLE (97+0x8000) #define STP_GEAR_LV1_ANGLE (603) #define STP_GEAR_LV2_ANGLE (756) #define STP_GEAR_LV3_ANGLE (909) #define STP_GEAR_LV4_ANGLE (1062) #define STP_GEAR_LV5_ANGLE (1215) // 1/8 step , steper speed: 60HS/mm step ( 60 half step -> 1mm ) #define STP_HEAD_POS_REFERENCE 7200u #define STP_HEAD_POS_INIT_POINT 8933u #define STP_HEAD_POS_LV1_POINT 8640u #define STP_HEAD_POS_LV2_POINT 8576u #define STP_HEAD_POS_LV3_POINT 8512u #define STP_HEAD_POS_LV4_POINT 8448u #define STP_HEAD_POS_LV5_POINT 8383u #define STP_INIT_RUN_POSITION STP_HEAD_POS_LV1_POINT #define STP_STEPER_SHORTEN_MAX_TIME_CUNT 330 // steper maximum stroke: 12mm, for steper 1/8 step configuration and 60HS/mm step, // running 12mm needs 240*12 step, and for 1KHz PWM, 1 pluse -> 1 step // so 240*12 steps need 240*12 ms, for 10ms task , it needs 288 count // so use 330 > 288 to make steper to shorten to the bottom. #define STP_RUN_DELAY 2u // steper change direction needs delay 20ms #define STP_STALL_DEBOUNCE_CUNT 10u // for check steper stall stauts //============================================================================= // Golbal variables //============================================================================= uint8 ucSigLevel = 0; boolean boStallFlag = FALSE; boolean boStpRefFlag = FALSE; uint8 boSteperRealDir = FALSE; boolean boReqStopFlag = FALSE; tieDirStatus tePreDir = STP_STOP; tieCddStpState teStpState = STP_STATE_CFG_INT; boolean boStartRefFlag = FALSE; uint16 u16RefPointTimeCunt = 0; uint8 ucStpRunDelay = STP_RUN_DELAY; tisCddSTPPositionInfo tsCddStpInfo = {0,0,STP_STOP,0,{STP_STOP,0}}; tisAllStpActual CddStp_ActAll = {{0, 0u, {SUPPORT_NO, FALSE, FALSE, FALSE, FALSE, 0, FALSE, STP_PROGRESS_INIT, 0}}}; tisAllStpTarget CddStp_TgtAll = { { 0, TRUE, 0u } }; //============================================================================= // External functions //============================================================================= //============================================================================= // Public functions implementation //============================================================================= //----------------------------------------------------------------------------- /// \brief ctadCddStp_riCddStpInit /// /// \descr Usually called in file 'Rte.c' /// /// \param - /// /// \return void //----------------------------------------------------------------------------- void ctadCddStp_riCddStpInit(void) { CddStpInit(); } //----------------------------------------------------------------------------- /// \brief ctadCddStp_rdCddStpDeInit /// /// \descr Usually called in file 'Rte.c' /// /// \param - /// /// \return void //----------------------------------------------------------------------------- void ctadCddStp_rdCddStpDeInit(void) { } //----------------------------------------------------------------------------- /// \brief ctadCddStp_roCddStpOff /// /// \descr Switch stepper OFF /// /// \param - /// /// \return Std_ReturnType //----------------------------------------------------------------------------- Std_ReturnType ctadCddStp_roCddStpOff(void) { boReqStopFlag = TRUE; } //----------------------------------------------------------------------------- /// \brief ctadCddStp_roCddStpOn /// /// \descr Switch stepper ON /// /// \param - /// /// \return Std_ReturnType //----------------------------------------------------------------------------- Std_ReturnType ctadCddStp_roCddStpOn(void) { boReqStopFlag = FALSE; } //----------------------------------------------------------------------------- /// \brief ctadCddStp_roCddStpStartRefRun /// /// \descr Started reference runs. /// /// \param stepperIndex /// \param position /// /// \return void //----------------------------------------------------------------------------- Std_ReturnType ctadCddStp_roCddStpStartRefRun(void) { boStartRefFlag = TRUE; } //----------------------------------------------------------------------------- /// \brief ctadCddStp_roCddStpStartRefRun /// /// \descr Started reference runs. /// /// \param stepperIndex /// \param position /// /// \return void //----------------------------------------------------------------------------- Std_ReturnType ctadCddStp_roCddStpErrProcess_ErrProcess(void) { boStartRefFlag = TRUE; teStpState = STP_STATE_CFG_INT; CddStpInit(); CddStp8381Com_ResetScheduleIndex(); } //----------------------------------------------------------------------------- /// \brief Cyclic runnable '10ms' /// /// \descr Usually called via Rte.c: TASK(OsTask_Timing_0_10ms) /// /// \param - /// /// \return void //----------------------------------------------------------------------------- void ctadCddStp_rpCddStp10ms(void) { CddStp_10ms_Task_Handle(); } //----------------------------------------------------------------------------- /// \brief Cyclic runnable '1ms' /// /// \descr Usually called via Rte.c: TASK(OsTask_Timing_0_1ms) /// /// \param - /// /// \return void //----------------------------------------------------------------------------- void ctadCddStp_rpCddStp1ms(void) { } //----------------------------------------------------------------------------- /// \brief Stepper initialization /// /// \descr Usually called in func ctadCddStp_riCddStpInit /// /// \param - /// /// \return - //----------------------------------------------------------------------------- void CddStpInit(void) { CddStp8381_DataInit(); CDDSTP8381DRV_Init(); CDDSTP8381DRV_CmdLoad_SetCTR_Default(); CDDSTP8381DRV_CmdLoad_GetSTA1(); CDDSTP8381DRV_CmdLoad_GetSTA2(); CDDSTP8381DRV_CmdLoad_Motor_En(STP_ENABLE); // set Motor Enable=1 CDDSTP8381DRV_CmdLoad_CTRL1_Fun(SET); // used CTRL1 PWM output not PH register SPI output CDDSTP8381DRV_CmdLoad_CTRL2_Fun(RESET); // used DIR register not CTRL2 pin CDDSTP8381DRV_CmdLoad_CTRL3_Fun(CTRL3_OFF); // CTRL3 no function,CTRL3_OFF=0 CDDSTP8381DRV_CmdLoad_UNLOCK_SET(STP_ENABLE); CDDSTP8381DRV_CmdLoad_SM_SET(SM8_microstep); // 8MICROSTEP, 32steps CDDSTP8381DRV_CmdLoad_ASM_SET(SM8_microstep); CDDSTP8381DRV_CmdLoad_RunDecay_SET(Autodecay2); // decay mode CDDSTP8381DRV_CmdLoad_RunCurrent_SET(RS812mA); // MCRREF current in RUN mode CDDSTP8381DRV_CmdLoad_HoldCurrent_SET(HD95mA); // hold current mode CDDSTP8381DRV_CmdLoad_PWM_SS_Fun(SET); // PWM_SS=1 CDDSTP8381DRV_CmdLoad_CP_SS_Fun(SET); // CP_SS=1 CDDSTP8381DRV_CmdLoad_SetCVLLB(0x0000000A); // 0.1V CDDSTP8381DRV_CmdLoad_SetCVLLA(0x0000000A); CDDSTP8381DRV_CmdLoad_SetCVUL(0x00000100); // 7V CDDSTP8381DRV_CmdLoad_SlewRate_SET(SR_70V_MS); // 70v/ms CDDSTP8381DRV_CmdLoad_SetSTALLNUM(STALLNUM3); // STALLNUMx times out of range,STALL FLAG set CDDSTP8381DRV_CmdLoad_AHMSD_SET(SET); CDDSTP8381DRV_CmdLoad_HOLDM_SET(STP_ENABLE); //ignore phase counter during HOLD mode CDDSTP8381DRV_CmdLoad_CoilVolt_EN(STP_ENABLE); // Set CVE=1 CDDSTP8381DRV_CmdLoad_DIR_SET(STP_SHORTEN); //need to shorten motor shorten CDDSTP8381DRV_CmdLoad_ClearAllSTA(); } //----------------------------------------------------------------------------- /// \brief Stepper 10ms handle /// /// \descr Usually called in func ctadCddStp_rpCddStp10ms /// /// \param - /// /// \return - //----------------------------------------------------------------------------- void CddStp_10ms_Task_Handle(void) { CddStp_ActAll.sLwrActual.sStatusError.eStpProgress = teStpState; switch (teStpState) { case STP_STATE_CFG_INT: CDDSTP8381_SetSTPChannelPwm(CDDSTP8381_CTRL1_PWM_CHANNEL,CDDSTP8381_PWM_DUTY_0); /* code */ break; case STP_STATE_CHECK_REF_POINT: if( TRUE == boStartRefFlag ) { // Disable enter hold when stall CDDSTP8381DRV_CmdLoad_AHMSD_SET(RESET); // finding reference point CddStp8381_RefPos_Finding(); } break; case STP_STATE_INIT_RUN: // get motor target level signal tsCddStpInfo.u16StpTrgPos = STP_HEAD_POS_LV1_POINT; ucSigLevel = 1; // update the motor's trip and direction CddStp8381_StpInfoUpdate(); // delay 20ms if steper direction change. CddStp8381_StpDirectionCheck(); // control steper move CddStp8381_SteperControl(); if ( STP_HEAD_POS_LV1_POINT == tsCddStpInfo.u16StpCurPos ) { teStpState = STP_STATE_NORMAL_RUN; CddStp_ActAll.sLwrActual.nPosition = ucSigLevel; } break; case STP_STATE_NORMAL_RUN: // motor enter hold mode when stall CDDSTP8381DRV_CmdLoad_AHMSD_SET(SET); CDDSTP8381DRV_CmdLoad_RunCurrent_SET(RS812mA); if( FALSE == boReqStopFlag) { // get motor target level signal CddStp8381_GetTgtLevel(); } else { // request off, use current position to update target position to stop steper tsCddStpInfo.u16StpTrgPos == tsCddStpInfo.u16StpCurPos; } // update the motor's trip and direction CddStp8381_StpInfoUpdate(); // delay 20ms if steper direction change. CddStp8381_StpDirectionCheck(); if ( tsCddStpInfo.u16StpCurPos != tsCddStpInfo.u16StpTrgPos) { CDDSTP8381DRV_CmdLoad_HoldM_En(STP_DISABLE); } else { CDDSTP8381DRV_CmdLoad_HoldM_En(STP_ENABLE); } // control steper move CddStp8381_SteperControl(); if ( TRUE == boStallFlag ) { teStpState = STP_STATE_ERROR_PROCESS; } break; case STP_STATE_ERROR_PROCESS: //teStpState = STP_STATE_CFG_INT; break; default: break; } CddStp_WriteSignals(); } //----------------------------------------------------------------------------- /// \brief CddStp8381_RefPos_Finding /// /// \descr Usually called in start or enter error /// /// \param void /// /// \return void //----------------------------------------------------------------------------- void CddStp8381_DataInit(void) { ucSigLevel = 0; tePreDir = STP_STOP; boStallFlag = FALSE; boStpRefFlag = FALSE; boSteperRealDir = 0; teStpState = STP_STATE_CFG_INT; u16RefPointTimeCunt = 0; ucStpRunDelay = STP_RUN_DELAY; tsCddStpInfo.teStpRunDirection = STP_STOP; tsCddStpInfo.u16StpCurPos = 0; tsCddStpInfo.u16StpTrgPos = 0; tsCddStpInfo.u16StpRemainCunt = 0; tsCddStpInfo.tsStpOffset.teStpOffsetDir = STP_STOP; tsCddStpInfo.tsStpOffset.ucStpOffsetCunt = 0; } //----------------------------------------------------------------------------- /// \brief CddStp8381_RefPos_Finding /// /// \descr Usually called in start or enter error /// /// \param void /// /// \return void //----------------------------------------------------------------------------- void CddStp8381_RefPos_Finding(void) { if( 1 == boSteperRealDir ) // shorten, get from steper, make sure that direction is shorten. { if ( u16RefPointTimeCunt < STP_STEPER_SHORTEN_MAX_TIME_CUNT ) { CDDSTP8381_SetSTPChannelPwm(CDDSTP8381_CTRL1_PWM_CHANNEL,CDDSTP8381_PWM_DUTY_50); if( TRUE == boStallFlag ) { teStpState = STP_STATE_INIT_RUN; boStpRefFlag = TRUE; boStartRefFlag = FALSE; tsCddStpInfo.u16StpCurPos = STP_HEAD_POS_REFERENCE; CddStp_ActAll.sLwrActual.sStatusError.boIsRef = 1u; //Get zero point complete CDDSTP8381DRV_CmdLoad_RunCurrent_SET(RS920mA); // MCRREF current in RUN mode } u16RefPointTimeCunt ++; } else { // steper has run over the threshold, steper is considered to be at the reference point teStpState = STP_STATE_INIT_RUN; boStpRefFlag = TRUE; boStartRefFlag = FALSE; tsCddStpInfo.u16StpCurPos = STP_HEAD_POS_REFERENCE; CddStp_ActAll.sLwrActual.sStatusError.boIsRef = 1u; //Get zero point complete CDDSTP8381DRV_CmdLoad_RunCurrent_SET(RS920mA); // MCRREF current in RUN mode } } } //----------------------------------------------------------------------------- /// \brief CddStp8381_CheckSteperStatus /// /// \descr Usually called in Cycle /// /// \param - /// /// \return - //----------------------------------------------------------------------------- void CddStp8381_CheckSteperStatus(void) { uint16 u16StpStatus = 0u; // overcurrent if( 1u == CddStp8381Com_DataMirror[0].StaRx.aSTAReg1.Bit.unOC ) { u16StpStatus = u16StpStatus | ((uint16)(1u << CDDSTP_STATUS_SHORT_BIT_POS)); } else { /* dothing */ } // openload if(( 1u == CddStp8381Com_DataMirror[0].StaRx.aSTAReg1.Bit.unOPL ) || ( 1u == CddStp8381Com_DataMirror[0].StaRx.aSTAReg2.Bit.unOPLB ) || ( 1u == CddStp8381Com_DataMirror[0].StaRx.aSTAReg2.Bit.unOPLA )) { u16StpStatus = u16StpStatus | ((uint16)(1u << CDDSTP_STATUS_OPEN_BIT_POS)); } else { /* dothing */ } // over temperature shutdown if( 1u == CddStp8381Com_DataMirror[0].StaRx.aSTAReg1.Bit.unOTSD ) { u16StpStatus = u16StpStatus | ((uint16)(1u << CDDSTP_STATUS_OT_SHUTDOWN_BIT_POS)); } else { /* dothing */ } CddStp_ActAll.sLwrActual.sStatusError.bfStpFaultStatus = u16StpStatus; } //----------------------------------------------------------------------------- /// \brief CddStp8381_CheckSteperStall /// /// \descr Usually called in Cycle /// /// \param - /// /// \return - //----------------------------------------------------------------------------- void CddStp8381_CheckSteperStall(void) { if( 1u == CddStp8381Com_DataMirror[0].StaRx.aSTAReg1.Bit.unSTALL ) { boStallFlag = TRUE; //motor stall } else { boStallFlag = FALSE; } } //----------------------------------------------------------------------------- /// \brief CddStp8381_CheckSteperDir /// /// \descr Usually called in Cycle /// /// \param - /// /// \return - //----------------------------------------------------------------------------- void CddStp8381_CheckSteperDir(void) { if( 1u == CddStp8381Com_DataMirror[0].CtrlRx.aCReg3.Bit.unDIR ) { boSteperRealDir = 1; // STP_SHORTEN } else { boSteperRealDir = 0; // STP_LONGATION } } //----------------------------------------------------------------------------- /// \brief CddStp8381_SteperControl /// /// \descr used for Motor control /// /// \param void /// /// \return - //----------------------------------------------------------------------------- void CddStp8381_SteperControl(void) { if( 0 != tsCddStpInfo.u16StpRemainCunt ) { if ( 0 == ucStpRunDelay) { CDDSTP8381_SetSTPChannelPwm(CDDSTP8381_CTRL1_PWM_CHANNEL,CDDSTP8381_PWM_DUTY_50); if ( tsCddStpInfo.u16StpRemainCunt >= CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT ) { tsCddStpInfo.u16StpRemainCunt = tsCddStpInfo.u16StpRemainCunt - CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT; if ( 0 == tsCddStpInfo.u16StpRemainCunt ) { tsCddStpInfo.tsStpOffset.ucStpOffsetCunt = 0; tsCddStpInfo.tsStpOffset.teStpOffsetDir = tsCddStpInfo.teStpRunDirection; } } else { tsCddStpInfo.tsStpOffset.ucStpOffsetCunt = CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT - tsCddStpInfo.u16StpRemainCunt; tsCddStpInfo.tsStpOffset.teStpOffsetDir = tsCddStpInfo.teStpRunDirection; tsCddStpInfo.u16StpRemainCunt = 0; } if ( STP_LONGATION == tsCddStpInfo.teStpRunDirection ) { tsCddStpInfo.u16StpCurPos = tsCddStpInfo.u16StpCurPos + CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT; } else { tsCddStpInfo.u16StpCurPos = tsCddStpInfo.u16StpCurPos - CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT; } if ( 0 == tsCddStpInfo.u16StpRemainCunt ) { tsCddStpInfo.u16StpCurPos = tsCddStpInfo.u16StpTrgPos; } } else { ucStpRunDelay --; } } else { CDDSTP8381_SetSTPChannelPwm(CDDSTP8381_CTRL1_PWM_CHANNEL,CDDSTP8381_PWM_DUTY_0); //Stop the motor from rotating by changing the PWM duty cycle CddStp_ActAll.sLwrActual.nPosition = ucSigLevel; //Feedback the location information when the target location is reached tsCddStpInfo.u16StpCurPos = tsCddStpInfo.u16StpTrgPos; } } //----------------------------------------------------------------------------- /// \brief CddStp8381_StpInfoUpdate /// /// \descr update steper infomation /// /// \param - /// /// \return - //----------------------------------------------------------------------------- void CddStp8381_StpInfoUpdate(void) { /* update steper information when steper stops, update once */ if ( STP_STOP == tsCddStpInfo.teStpRunDirection ) { if ( tsCddStpInfo.u16StpCurPos < tsCddStpInfo.u16StpTrgPos ) { tsCddStpInfo.teStpRunDirection = STP_LONGATION; if ( tsCddStpInfo.tsStpOffset.teStpOffsetDir != STP_LONGATION ) { tsCddStpInfo.u16StpRemainCunt = tsCddStpInfo.u16StpTrgPos - tsCddStpInfo.u16StpCurPos + tsCddStpInfo.tsStpOffset.ucStpOffsetCunt; } else { tsCddStpInfo.u16StpRemainCunt = tsCddStpInfo.u16StpTrgPos - tsCddStpInfo.u16StpCurPos - tsCddStpInfo.tsStpOffset.ucStpOffsetCunt; } /* when steper start running in CddStp8381_SteperControl, the u16StpRemainCunt has been decreased CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT so, add CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT to make sure that steper runs to tsCddStpInfo.u16StpTrgPos noramlly*/ tsCddStpInfo.u16StpRemainCunt = tsCddStpInfo.u16StpRemainCunt + CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT; } else if( tsCddStpInfo.u16StpCurPos > tsCddStpInfo.u16StpTrgPos ) { tsCddStpInfo.teStpRunDirection = STP_SHORTEN; if ( tsCddStpInfo.tsStpOffset.teStpOffsetDir != STP_SHORTEN ) { tsCddStpInfo.u16StpRemainCunt = tsCddStpInfo.u16StpCurPos - tsCddStpInfo.u16StpTrgPos + tsCddStpInfo.tsStpOffset.ucStpOffsetCunt; } else { tsCddStpInfo.u16StpRemainCunt = tsCddStpInfo.u16StpCurPos - tsCddStpInfo.u16StpTrgPos - tsCddStpInfo.tsStpOffset.ucStpOffsetCunt; } /* when steper start running in CddStp8381_SteperControl, the u16StpRemainCunt has been decreased CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT so, add CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT to make sure that steper runs to tsCddStpInfo.u16StpTrgPos noramlly*/ tsCddStpInfo.u16StpRemainCunt = tsCddStpInfo.u16StpRemainCunt + CDDSTP8381_2KHZ_PLUSE_DECREASE_CUNT; } else { /* do nothing */ } } else { if ( tsCddStpInfo.u16StpCurPos == tsCddStpInfo.u16StpTrgPos ) { tsCddStpInfo.u16StpRemainCunt = 0; tsCddStpInfo.teStpRunDirection = STP_STOP; } } } //----------------------------------------------------------------------------- /// \brief CddStp8381_StpDirectionCheck /// /// \descr check steper direction /// /// \param - /// /// \return - //----------------------------------------------------------------------------- void CddStp8381_StpDirectionCheck(void) { if ( tePreDir != tsCddStpInfo.teStpRunDirection ) { tePreDir = tsCddStpInfo.teStpRunDirection; if ( STP_STOP != tsCddStpInfo.teStpRunDirection ) { CDDSTP8381_SetSTPChannelPwm(CDDSTP8381_CTRL1_PWM_CHANNEL,CDDSTP8381_PWM_DUTY_0); CDDSTP8381DRV_CmdLoad_DIR_SET(tsCddStpInfo.teStpRunDirection); ucStpRunDelay = STP_RUN_DELAY; } } } //----------------------------------------------------------------------------- /// \brief CddStp8381_GetTgtLevel /// /// \descr Usually called get or update target signal /// /// \param - /// /// \return - //----------------------------------------------------------------------------- void CddStp8381_GetTgtLevel(void) { Rte_Read_ppareCtrlStpTarget0_StpTargetValues(&CddStp_TgtAll); /* add STP_STOP limitation, only update u16StpTrgPos when steper stops. */ if ( STP_STOP == tsCddStpInfo.teStpRunDirection) { switch (CddStp_TgtAll.sLwrTarget.nPosition) { case STP_GEAR_LV0_ANGLE: tsCddStpInfo.u16StpTrgPos = STP_HEAD_POS_INIT_POINT; ucSigLevel = 0; break; case STP_GEAR_LV1_ANGLE: tsCddStpInfo.u16StpTrgPos = STP_HEAD_POS_LV1_POINT; ucSigLevel = 1; break; case STP_GEAR_LV2_ANGLE: tsCddStpInfo.u16StpTrgPos = STP_HEAD_POS_LV2_POINT; ucSigLevel = 2; break; case STP_GEAR_LV3_ANGLE: tsCddStpInfo.u16StpTrgPos = STP_HEAD_POS_LV3_POINT; ucSigLevel = 3; break; case STP_GEAR_LV4_ANGLE: tsCddStpInfo.u16StpTrgPos = STP_HEAD_POS_LV4_POINT; ucSigLevel = 4; break; case STP_GEAR_LV5_ANGLE: tsCddStpInfo.u16StpTrgPos = STP_HEAD_POS_LV5_POINT; ucSigLevel = 5; break; default: tsCddStpInfo.u16StpTrgPos = STP_HEAD_POS_INIT_POINT; ucSigLevel = 0; break; } } } //----------------------------------------------------------------------------- /// \brief CddStp8381_GetTgtLevel /// /// \descr Usually called get or update target signal /// /// \param - /// /// \return retvalue : target signal value //----------------------------------------------------------------------------- void CddStp_WriteSignals(void) { (void)Rte_Write_ppaseCddStpActual0_sStpActualValues(&CddStp_ActAll); }