
;********************************************** HEADER ************************************************
;Title:		Pick and Place example program with I/O
;Author:		940 Product Management
;Description:	This is a sample program that picks up a part, moves to a set position and drops the
;			part. Inputs are used to detect when the Pick/Place arm is extended and retracted 

;*********************************************  I/O List  *********************************************
;	Input A1	-	Reset switch
;	Input A2	-	not used
;	Input A3	-	Enable
;	Input A4	-	Pick/Place Arm Retracted
;	Input B1	-	Pick/Place Arm Extended
;	Input B2	-	not used
;	Input B3	-	not used
;	Input B4	-	not used
;	Input C1	-	not used
;	Input C2	-	not used
;	Input C3	-	not used
;	Input C4	-	not used
;
;	Output 1	-	Pick Arm
;	Output 2	-	Gripper
;	Output 3	-	not used
;	Output 4	-	not used


;******************************** Initialize and Set Variables ****************************************
UNITS = 1				; Set unit of measure. "1" means move 1 is one rev
ACCEL = 75			; Set accel rate
DECEL =	75			; Set Decel rate
MAXV = 10			; Set max velocity
;********************************************* Events *************************************************
;Set Events handling here

;******************************************* Main Program ********************************************
RESET_DRIVE:		;This is a place holder for the Fault Handler program.
WAIT UNTIL IN_A3		;Make sure that the Enable / Inhibit switch is made before continuing
ENABLE				;Enable the Drive
PROGRAM_START:		;This is a place holder used to loop the program
WAIT UNTIL IN_A4	 	;Make sure Arm retracted
MOVEP  0	 			;Move to Pick position
OUT1 = 1				;Turn on output 1 on to extend Pick arm
WAIT UNTIL IN_B1		;Wait for Arm to extend
OUT2 = 1				;Turn on output 2 to Engage gripper
WAIT TIME 1000		;Delay 1 sec to Pick part
OUT1 = 0				;Turn off output 1 to Retract Pick arm
WAIT UNTIL IN_A4		;Wait for Arm to retract
MOVEP 100			;Move to Place position
OUT1 = 1				;Turn on output 1 on to extend Pick arm
WAIT UNTIL IN_B1		;Wait for Arm to extend
OUT2 = 0				;Turn off output 1 to Disengage gripper
WAIT TIME 1000		;Delay 1 sec to Place part
OUT1 = 0				;Retract Pick arm
WAIT UNTIL IN_A4		;Wait for Arm to retract
GOTO PROGRAM_START

FPROCESS:			;This is a place holder for the FAULT HANDLER
WAIT UNTIL IN_A1		;Wait for reset switch to be made
WAIT UNTIL !IN_A1		;Wait for reset switch to be released
GOTO RESET_DRIVE	;Go back to the main program
END

;******************************************* Sub-Routines ********************************************
;	Enter Sub-Routine code here


;************************************** Fault Handler Routine ****************************************
ON FAULT			;Once the drive detects a fault the program comes here.
					;Motion is stopped, drive is disabled, and events are no longer scanned.
OUT2 = 0				;Output 2 is turned off to disengage the gripper releasing the part.
OUT1 = 0				;Output 1 is turned off to retract Pick & Place arm
RESUME FPROCESS	;Sends code execution to the FPROCESS routine.
ENDFAULT
