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

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

;******************************** 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 Values

UNITS = 1 ; Units in RPS

Accel = 300 ; Accelerate 50 RPS/S

Decel = 450 ; Decelerate 50 RPS/s

Define Vel_Calc V0 ; Define Variable for Velocity Calculations

Define But_Run_CW IN_B2 ; positive run button LEFT RUN

Define But_Run_CCW IN_B1 ; negative run button RIGHT RUN

Define Homing_Sensor In_B3 ; In_C1--->>>In_B3 Define Variable for Homing Sensor Input Define Reset_Btn In_B4 ;reset signa

Define System_Enable In_A3 ; define Variable for Enable Input

Define Homing_Request In_A4 ; In_B4--->>>In_A4 Define Variable for Homing Request Input Define AlarmOut Out1

Define Home_Completed Out2 ; Define Variable for Homing complete Indicator

Define Suddun_Stop_Left In_A1 ;Left/bottom/negative Limit

Define Suddun_Stop_Right In_A2 ;Right/top/positive Limit

V AR_REFERENCE = 0 ;set Reference to Internal

V AR_DRIVEMODE = 2 ;Set Operating mode to Position mode

V 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 Code

jump homing

Endevent

;******************************************* Main Program ********************************************

PROGRAM_START:

Wait While System_Enable == 0 ; Wait for drive safety enable to be active - Safe Condition

enable ; enable drive

VELOCITY_LOOP:

if Suddun_Stop_Left==1 || Suddun_Stop_Right ==1

V AR_HLS_MODE = 0

ELSE

V AR_HLS_MODE = 1

ENDIF

; Update the velocity reference

Vel_Calc = 10

if But_Run_CW == 1 && But_Run_CCW == 0&&Suddun_Stop_Right ==0

;Vel_Calc = Vel_Calc ; Move Clockwise command - don't change the calculation

maxv = 10 ; set slow velocity for homing

move back until But_Run_CW==0 ; move back at slow velocity until homing sensor is activate

endif

if 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 calculation

maxv = 10 ; set slow velocity for homing

move until But_Run_CCW==0 ; move back at slow velocity until homing sensor is activat

endif

if But_Run_CW == 0 && But_Run_CCW == 0

; Vel_Calc = 0 ; Invalid Input - don't allow movement

endif

if But_Run_CW == 1 && But_Run_CCW == 1

;Vel_Calc = 0 ; Invalid Input - don't allow movement

endif

If But_Run_CW == 1 && Suddun_Stop_Right==1

Vel_Calc = 0

;maxv = 10

;move back until But_Run_CW==0 ; move back at slow velocity until homing sensor is activate endif

If But_Run_CCW == 1 && Suddun_Stop_Left ==1

Vel_Calc = 0

;maxv = 10

;move until But_Run_CCW==0 ; move back at slow velocity until homing sensor is activate endif

if Reset_Btn == 1

AlarmOut = 0

endif

If 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_LOOP

IF IN_A4

Homing: ; Homing Routine, Performed at Start Up and after Homing Routine Request inout goes high

Accel = 300 ; Accelerate 50 RPS/S

Decel = 450 ; Decelerate 50 RPS/s

Home_Completed = 0 ; Turn off homing completed indicator

event homing_call off ; Switch off event to check for homing request input

maxv = 10 ; set slow velocity for homing

move 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 sensor

Accel = 50 ; Accelerate 50 RPS/S

Decel = 50 ; Decelerate 50 RPS/s

APOS = 0 ; Set actual position register to 0

maxv = 50 ; Set velocity back to normal speed

wait while Homing_Request == 1 ; wait for the homing request signal to be removed

event homing_call on ; Switch on event to check for homing request input

Home_Completed = 1 ; Turn on homing completed indicator

ENDIF

GOTO VELOCITY_LOOP

end

fault_section:

wait while System_Enable == 1 ; wait until enable input is removed

goto program_start ; restart program

;******************************************* Sub-Routines ********************************************

; Enter Sub-Routine code here

;************************************** Fault Handler Routine ****************************************

; Enter Fault Handler code here

ON FAULT

AlarmOut = 1

resume fault_section

ENDFAULT