;************************************************ HEADER **********************************************
;Title:		Pick and Place with Fault Mon
;Author:		940 Product Managment
;Description:	This is a sample program program that picks up a part moves to a set position and drops
;			the part. If a fault occurs the part is droped and the pick/place arm is retracted.
;			The program then waits for a reset input before restarting the program. 

;*********************************************  I/O List  *********************************************
;	Input A1	-	not used
;	Input A2	-	not used
;	Input A3	-	Enabled
;	Input A4	-	not used
;	Input B1	-	not used
;	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
ACCEL = 75
DECEL =75
MAXV = 10

;********************************************* 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
MOVEP  0	 				;Move to Pick position
OUT1 = 1					;Turn on output 1 on to extend Pick arm
WAIT TIME 1000			;Delay 1 sec to extend arm
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
MOVEP 100				;Move to Place position
OUT1 = 1					;Turn on output 1 on to extend Pick arm
WAIT TIME 1000			;Delay 1 sec to extend arm
OUT2 = 0					;Turn off output 1 to Disengage gripper
WAIT TIME 1000			;Delay 1 sec to Place part
OUT1 = 0					;Retract Pick arm
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

