ROB 1

MODULE modRobotStudio
	CONST robtarget pPickPosC1:=[[60,60.0000000000002,2.5],[6.12303176911189E-17,1,4.48916032525077E-33,-3.69778549322349E-32],[1,0,-2,5],[9E9,9E9,9E9,9E9,9E9,9E9]];
	CONST robtarget pPickPosC2:=[[60,60.0000000000001,2.5],[-6.12303176911189E-17,1,4.62223186652937E-33,-6.16297582203915E-33],[1,0,-2,5],[9E9,9E9,9E9,9E9,9E9,9E9]];
	CONST robtarget pPickPosC3:=[[60,60.0000000000001,2.5],[-6.12303176911188E-17,1,1.07852076885685E-32,1.23259516440783E-32],[0,0,-1,5],[9E9,9E9,9E9,9E9,9E9,9E9]];
	CONST robtarget pBufferPosROB1:=[[224.500035230802,100.000011574079,166.454416191376],[-8.61528821582825E-16,0.707106781186548,-0.707106781186547,3.72302807738425E-17],[0,0,-1,4],[9E9,9E9,9E9,9E9,9E9,9E9]];
	CONST robtarget pTurnTablePosROB1:=[[0,0,4],[-6.12303176911189E-17,1,-5.55111512312578E-17,-1.97057596545677E-46],[1,0,-3,4],[9E9,9E9,9E9,9E9,9E9,9E9]];
	PERS bool bPartPlacedOnTurnTable:=TRUE;
	PERS num nZoffset:=0;
	CONST robtarget pPickPos:=[[0,0,0],[-6.12303176911189E-17,1,4.75531733503912E-33,-3.08148791101958E-32],[1,0,-2,5],[9E9,9E9,9E9,9E9,9E9,9E9]];
	PERS bool bWobjCalculated:=TRUE;

PROC main()
	MoveJ RelTool(pPickPosC1,0,0,-100),v200,fine,tGripper\WObj:=wobjPickPosStatic;
	bPartPlacedOnTurnTable:=FALSE;
	nZoffset:=0;
	WHILE TRUE DO
		PickPanel;
!		IF diPlaceCellInBuffer=1 THEN
!			PlacePanelInControlBuffer;
!		ELSE
			PlacePanelOnTurnTable;
!		ENDIF
	ENDWHILE
ENDPROC

PROC PickPanel()
	WaitUntil diSensorC1=1 AND bWobjCalculated=TRUE;
	MoveJ RelTool(pPickPos,0,0,-100),v2000,z5,tGripper\WObj:=wobjPickPos;
	MoveL RelTool(pPickPos,0,0,-2),v100,fine,tGripper\WObj:=wobjPickPos;
	SetDO doVacuumOn,1;
	WaitDI diVacuum,1;
	MoveL RelTool(pPickPos,0,0,-100),v500,z50,tGripper\WObj:=wobjPickPos;

ENDPROC

PROC PlacePanelInControlBuffer()
	MoveJ RelTool(pBufferPosROB1,50,0,-100),v2000,z10,tGripper\WObj:=wobjBuffer;
	MoveL RelTool(pBufferPosROB1,0,0,-100),v2000,z1,tGripper\WObj:=wobjBuffer;
	MoveL RelTool(pBufferPosROB1,0,0,nZoffset),v100,fine,tGripper\WObj:=wobjBuffer;
	IF diBufferFull=1 THEN
		nZoffset:=0;
	ELSE
		nZoffset:=nZoffset-4;
	ENDIF
	SetDO doVacuumOn,0;
	WaitDI diVacuum,0;
	MoveL RelTool(pBufferPosROB1,0,0,-100),v200,z1,tGripper\WObj:=wobjBuffer;
	MoveL RelTool(pBufferPosROB1,100,0,-100),v2000,z10,tGripper\WObj:=wobjBuffer;
ENDPROC

PROC PlacePanelOnTurnTable()
	MoveJ  RelTool(pTurnTablePosROB1,0,0,-100),v2000,fine,tGripper\WObj:=wobjTurnTableROB1;
	WaitUntil diTurnTableInPos=1 AND bPartPlacedOnTurnTable=FALSE;
	MoveL  pTurnTablePosROB1,v100,fine,tGripper\WObj:=wobjTurnTableROB1;
	SetDO doVacuumOn,0;
	WaitDI diVacuum,0;
	MoveL  RelTool(pTurnTablePosROB1,0,0,-100),v200,z10,tGripper\WObj:=wobjTurnTableROB1;
	MoveL  RelTool(pTurnTablePosROB1,-100,0,-100),v200,fine,tGripper\WObj:=wobjTurnTableROB1;
	bPartPlacedOnTurnTable:=TRUE;
ENDPROC
ENDMODULE