伺服电机控制器安全控制程序

伺服电机控制器安全控制程序;******************************** PositionServo User Indexing Program ********************************;********************************************** Header **************************************************;Title : Sample Homing Program From PositionServo Training Exercises;Author : AC Technology International Ltd;Description : Program Performs Simple Homing Routine followed by Preset Sequence of Moves ; : Input A4 used to Re-Initalise Homing Routine during Main Program Execution; : Homing Sensor on Input B1; : Homing Complete Output on Out 1;Version Number : V1.0.1;Date : 22/11/06;;************************************************ I/O List ************************************************; Input A1 - bottom/left/negative limit senso; Input A2 - top/right/positive limit sensor; Input A3 - Enable Input / Safety stop button; Input A4 - homing button; Input B1 - move left/up; Input B2 - move right/down; Input B3 - homing sensor; Input B4 - not used; Input C1 - not used; Input C2 - not used; Input C3 - not used; Input C4 - not used;; Output 1 - alarm; Output 2 - homing complete; Output 3 - not used; Output 4 - not used;; Analog In 1 - not used; Analog In 2 - not used; Analog Out - not used;; Encoder Out - not used;;******************************** Initialize and Set Variables****************************************; Define Constants and Variables. Assign I/O and Initialize Variable ValuesUNITS = 1 ; Units in RPSAccel = 300 ; Accelerate 50 RPS/SDecel = 450 ; Decelerate 50 RPS/sDefine Vel_Calc V0 ; Define Variable for Velocity CalculationsDefine But_Run_CW IN_B2 ; positive run button LEFT RUNDefine But_Run_CCW IN_B1 ; negative run button RIGHT RUNDefine Homing_Sensor In_B3 ; In_C1--->>>In_B3 Define Variable for Homing Sensor Input Define Reset_Btn In_B4 ;reset signaDefine System_Enable In_A3 ; define Variable for Enable InputDefine Homing_Request In_A4 ; In_B4--->>>In_A4 Define Variable for Homing Request Input Define AlarmOut Out1Define Home_Completed Out2 ; Define Variable for Homing complete IndicatorDefine Suddun_Stop_Left In_A1 ;Left/bottom/negative LimitDefine Suddun_Stop_Right In_A2 ;Right/top/positive LimitV AR_REFERENCE = 0 ;set Reference to InternalV AR_DRIVEMODE = 2 ;Set Operating mode to Position modeV AR_ENABLE_SWITCH_TYPE = 0 ;enable switch function set to "Inhibit";************************************************ Events ************************************************Event homing_call input Homing_Request rise ; Events looks for Homing Request Input and Jumps to Homing Codejump homingEndevent;******************************************* Main Program ********************************************PROGRAM_START:Wait While System_Enable == 0 ; Wait for drive safety enable to be active - Safe Conditionenable ; enable driveVELOCITY_LOOP:if Suddun_Stop_Left==1 || Suddun_Stop_Right ==1V AR_HLS_MODE = 0ELSEV AR_HLS_MODE = 1ENDIF; Update the velocity referenceVel_Calc = 10if But_Run_CW == 1 && But_Run_CCW == 0&&Suddun_Stop_Right ==0;Vel_Calc = Vel_Calc ; Move Clockwise command - don't change the calculationmaxv = 10 ; set slow velocity for homingmove back until But_Run_CW==0 ; move back at slow velocity until homing sensor is activateendifif But_Run_CW == 0 && But_Run_CCW == 1&& Suddun_Stop_Left == 0;Vel_Calc = Vel_Calc * -1 ; Move Counter-Clockwise command - change direction of the calculationmaxv = 10 ; set slow velocity for homingmove until But_Run_CCW==0 ; move back at slow velocity until homing sensor is activatendifif But_Run_CW == 0 && But_Run_CCW == 0; Vel_Calc = 0 ; Invalid Input - don't allow movementendifif But_Run_CW == 1 && But_Run_CCW == 1;Vel_Calc = 0 ; Invalid Input - don't allow movementendifIf But_Run_CW == 1 && Suddun_Stop_Right==1Vel_Calc = 0;maxv = 10;move back until But_Run_CW==0 ; move back at slow velocity until homing sensor is activate endifIf But_Run_CCW == 1 && Suddun_Stop_Left ==1Vel_Calc = 0;maxv = 10;move until But_Run_CCW==0 ; move back at slow velocity until homing sensor is activate endifif Reset_Btn == 1AlarmOut = 0endifIf Vel_Calc <= 0.6 && Vel_Calc >= -0.6 ; Add If Statement to Include Deadband;Vel_Calc = 0;Endif;IREF = Vel_Calc;V AR_DRIVEMODE = 2 ;Set Operating mode to V mod;GOTO VELOCITY_LOOPIF IN_A4Homing: ; Homing Routine, Performed at Start Up and after Homing Routine Request inout goes highAccel = 300 ; Accelerate 50 RPS/SDecel = 450 ; Decelerate 50 RPS/sHome_Completed = 0 ; Turn off homing completed indicatorevent homing_call off ; Switch off event to check for homing request inputmaxv = 10 ; set slow velocity for homingmove until Homing_Sensor ; move back at slow velocity until homing sensor is activate;maxv = 2 ; set very slow velocity;move until homing_sensor == 0 ; move forward off homing sensorAccel = 50 ; Accelerate 50 RPS/SDecel = 50 ; Decelerate 50 RPS/sAPOS = 0 ; Set actual position register to 0maxv = 50 ; Set velocity back to normal speedwait while Homing_Request == 1 ; wait for the homing request signal to be removedevent homing_call on ; Switch on event to check for homing request inputHome_Completed = 1 ; Turn on homing completed indicatorENDIFGOTO VELOCITY_LOOPendfault_section:wait while System_Enable == 1 ; wait until enable input is removedgoto program_start ; restart program;******************************************* Sub-Routines ********************************************; Enter Sub-Routine code here;************************************** Fault Handler Routine ****************************************; Enter Fault Handler code hereON FAULTAlarmOut = 1resume fault_sectionENDFAULT。