Untitled

avatar
unknown
plain_text
a year ago
11 kB
8
Indexable
//------------------------------------- 200 Place -------------------------------------------------------
//--- ET_MotionState._0210_MoveToPlacePositionSpl: ------------------------------------------------------
	ET_MotionState._0210_MoveToPlacePositionSpl:
		
		IF fbTimerPick.Q
		OR (iq_stRobotModule_Ad.stApplication.xVacuumActive AND iq_stRobotModule_Ad.stApplication.xVacuumSensor) THEN
			AddMotionStateToAPL(i_sMessage:= '_0210_MoveToPlacePositionSpl');
			xStartTimerPick := FALSE;
			SetRobotExecute(i_xExecute := FALSE);
					iq_stRobotModule_Ad.stApplication.xBlow := FALSE;//Thang
				iq_stRobotModule_Ad.stApplication.xVacuum := TRUE;// Lat
			IF iq_stRobotModule_Ad.stApplication.xRobotStart THEN
				// Intelligent Tracking
				IF ((iPlacePos > 0) OR (iNextPossiblePlacePos > 0 AND iq_stRobotModule_Ad.stApplication.xIntelligentTracking))
				OR iq_stRobotModule_Ad.stApplication.xPlace THEN
					// OutfeedBelt active -------------------------------------------------------------------------------------------
					IF iq_stRobotModule_Ad.stTracking.xOutFeedBeltActive THEN
						// Intelligent Tracking
						IF iq_stRobotModule_Ad.stApplication.xIntelligentTracking THEN
							IF iPlacePos > 0 THEN 
								iPlacePos_tmp := iPlacePos;
								xFlagIntTrack := FALSE;
							ELSE
								iPlacePos_tmp := iNextPossiblePlacePos;
								xFlagIntTrack := TRUE;
							END_IF
						ELSE
							iPlacePos_tmp := iPlacePos;
						END_IF
							
						//Spline XYDistance aendern
						//iq_stRobotModule_Ad.stApplication.lrXYDistanceStartToObstacle;
						//iq_stRobotModule_Ad.stApplication.lrXYDistanceEndToObstacle;
						SetMotionParameterPlace();
						// Path Number = 210
						_0210_MoveToPlacePositionSpl( 	i_xChangeCOS 			:= TRUE,
							//i_udiToolId:=
							i_udiToolId	:= iq_stRobotModule_Ad.stTool.udiCurrentToolId,
														i_etCoordinateSystem	:= iq_etOutfeedBelt,
														i_lrStartOffsetBT		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
														i_lrEndOffsetBT			:= -iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace);
						
						IF iq_stRobotModule_Ad.stApplication.xAuxAx1 THEN	// Rotate only if AuxAx1 configured										
							_2000_RotateAuxAx1(	i_udiMovementId		:= ET_MotionState._0210_MoveToPlacePositionSpl+c_udiRotationOffset, 
												i_udiStartSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_udiEndSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_lrTarget			:= -93.37, // iq_astPlacePositions[iPlacePos_tmp].lrRotation, 
												i_lrStartOffset		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
												i_lrEndOffset		:= -iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace);
						END_IF
						IF iq_stRobotModule_Ad.stApplication.xAuxAx2 THEN	// Rotate only if AuxAx2 configured									
							_3000_RotateAuxAx2(	i_udiMovementId		:= ET_MotionState._0210_MoveToPlacePositionSpl+c_udiTiltingOffset, 
												i_udiStartSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_udiEndSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_lrTarget			:= iq_astPlacePositions[iPlacePos_tmp].lrTilting, 
												i_lrStartOffset		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
												i_lrEndOffset		:= -iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace);
						END_IF	
						
						IF iq_stRobotModule_Ad.stTracking.xOutfeedBelt_XDirection THEN							
							stPlaceForMaxSyncPos.lrX := iq_astPlacePositions[iPlacePos_tmp].lrActPos;
						ELSE
							stPlaceForMaxSyncPos.lrY := iq_astPlacePositions[iPlacePos_tmp].lrActPos;
						END_IF
						// Set Stop Pos
						SetPathStop(i_udiStopSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl,
									i_lrStopOffset		:= -iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace);
									
						IF xSuccess THEN
							SetRobotExecute(i_xExecute := TRUE);
							// Intelligent Tracking
							IF iq_stRobotModule_Ad.stApplication.xIntelligentTracking THEN
								etMotionState := ET_MotionState._0212_IntelligentTracking;
							ELSE
								etMotionState := ET_MotionState._0215_ResetStopOnPath;
								
							END_IF
							AddLastTargetToAPL();
						END_IF
					ELSE
					// OutfeedBelt NOT active ---------------------------------------------------------------------------------------
						iPlacePos_tmp := 0;
						
						//Spline XYDistance aendern
						//iq_stRobotModule_Ad.stApplication.lrXYDistanceStartToObstacle;
						//iq_stRobotModule_Ad.stApplication.lrXYDistanceEndToObstacle;
						SetMotionParameterPlace();
						// Path Number = 210
						_0210_MoveToPlacePositionSpl(	i_xChangeCOS			:= TRUE,
							i_udiToolId	:= iq_stRobotModule_Ad.stTool.udiCurrentToolId,
														i_etCoordinateSystem	:= ROB.ET_CoordinateSystem.CSR,
														i_lrStartOffsetBT		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
														i_lrEndOffsetBT			:= -iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace);
															
						IF iq_stRobotModule_Ad.stApplication.xAuxAx1 THEN	// Rotate only if AuxAx1 configured					
							_2000_RotateAuxAx1(	i_udiMovementId		:= ET_MotionState._0210_MoveToPlacePositionSpl+c_udiRotationOffset, 
												i_udiStartSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_udiEndSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_lrTarget			:= iq_stRobotModule_Ad.stMechanicPos.stPlacePos.stOrientation.lrZ, 
												i_lrStartOffset		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
												i_lrEndOffset		:= -iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace);
						END_IF
						IF iq_stRobotModule_Ad.stApplication.xAuxAx2 THEN	// Rotate only if AuxAx2 configured									
							_3000_RotateAuxAx2(	i_udiMovementId		:= ET_MotionState._0210_MoveToPlacePositionSpl+c_udiTiltingOffset, 
												i_udiStartSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_udiEndSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_lrTarget			:= iq_stRobotModule_Ad.stMechanicPos.stPlacePos.stOrientation.lrX, 
												i_lrStartOffset		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
												i_lrEndOffset		:= -iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace);
						END_IF
						// Set Stop Pos
						SetPathStop(i_udiStopSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl,
									i_lrStopOffset		:= -iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace);
						
						IF xSuccess THEN
							SetRobotExecute(i_xExecute := TRUE);
							etMotionState := ET_MotionState._0215_ResetStopOnPath;
							AddLastTargetToAPL();
						END_IF
						
					END_IF
		
				ELSE
					//Place position available, move to wait
					//Spline XYDistance aendern
					//iq_stRobotModule_Ad.stApplication.lrXYDistanceStartToObstacle;
					//iq_stRobotModule_Ad.stApplication.lrXYDistanceEndToObstacle;
					SetMotionParameterPlace();
					// Path Number = 211
					_0211_MoveToWaitPosHalfSpl(	i_xChangeCOS			:= TRUE,
												i_etCoordinateSystem	:= ROB.ET_CoordinateSystem.CSR,
												i_lrStartOffsetBT		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
												i_lrEndOffsetBT			:= 0.0);
														
					IF iq_stRobotModule_Ad.stApplication.xAuxAx1 THEN	// Rotate only if AuxAx1 configured										
						_2000_RotateAuxAx1(	i_udiMovementId		:= ET_MotionState._0211_MoveToWaitPosHalfSpl+c_udiRotationOffset, 
											i_udiStartSegmentId	:= ET_MotionState._0211_MoveToWaitPosHalfSpl, 
											i_udiEndSegmentId	:= ET_MotionState._0211_MoveToWaitPosHalfSpl, 
											i_lrTarget			:= iq_stRobotModule_Ad.stMechanicPos.stPlacePos.stOrientation.lrZ,
											i_lrStartOffset		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
											i_lrEndOffset		:= 0.0);
					END_IF
					IF iq_stRobotModule_Ad.stApplication.xAuxAx2 THEN	// Rotate only if AuxAx2 configured									
						_3000_RotateAuxAx2(	i_udiMovementId		:= ET_MotionState._0210_MoveToPlacePositionSpl+c_udiTiltingOffset, 
											i_udiStartSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
											i_udiEndSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
											i_lrTarget			:= iq_stRobotModule_Ad.stMechanicPos.stPlacePos.stOrientation.lrX, 
											i_lrStartOffset		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick,
											i_lrEndOffset		:= 0.0);
					END_IF
					IF xSuccess THEN
						SetRobotExecute(i_xExecute := TRUE);
						etMotionState := ET_MotionState._0221_WaitMoveToPlacePosHalfSpl;
						AddLastTargetToAPL();
					END_IF									 
				END_IF
				
			ELSE
			// Roboter Stop "End of Cycle"
				//Spline XYDistance aendern
				//iq_stRobotModule_Ad.stApplication.lrXYDistanceStartToObstacle;
				//iq_stRobotModule_Ad.stApplication.lrXYDistanceEndToObstacle;
				SetMotionParameterPlace();
				_1005_MoveToWaitPosSpl( i_xChangeCOS			:= TRUE,
										i_etCoordinateSystem	:= ROB.ET_CoordinateSystem.CSR,
										i_lrStartOffsetBT		:= 0.0,
										i_lrEndOffsetBT			:= 0.0);
										
				IF iq_stRobotModule_Ad.stApplication.xAuxAx1 THEN	// Rotate only if AuxAx1 configured							
					_2000_RotateAuxAx1(	i_udiMovementId		:= ET_MotionState._1005_MoveToWaitPosSpl+c_udiRotationOffset, 
										i_udiStartSegmentId	:= ET_MotionState._1005_MoveToWaitPosSpl, 
										i_udiEndSegmentId	:= ET_MotionState._1005_MoveToWaitPosSpl, 
										i_lrTarget			:= iq_stRobotModule_Ad.stMechanicPos.stPickPos.stOrientation.lrZ, 
										i_lrStartOffset		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace,
										i_lrEndOffset		:= 0.0);
				END_IF
				IF iq_stRobotModule_Ad.stApplication.xAuxAx2 THEN	// Rotate only if AuxAx2 configured									
					_3000_RotateAuxAx2(	i_udiMovementId		:= ET_MotionState._1005_MoveToWaitPosSpl+c_udiTiltingOffset, 
										i_udiStartSegmentId	:= ET_MotionState._1005_MoveToWaitPosSpl, 
										i_udiEndSegmentId	:= ET_MotionState._1005_MoveToWaitPosSpl, 
										i_lrTarget			:= iq_stRobotModule_Ad.stMechanicPos.stPickPos.stOrientation.lrX, 
										i_lrStartOffset		:= iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace,
										i_lrEndOffset		:= 0.0);
				END_IF						
				IF xSuccess THEN
					SetRobotExecute(i_xExecute := TRUE);
					etMotionState := ET_MotionState._0000_Wait;
					AddMotionStateToAPL(i_sMessage:='Stop_End_Of_Cycle');
					AddLastTargetToAPL();
				END_IF		
			END_IF
		END_IF
Editor is loading...
Leave a Comment