Untitled

//---- ChangeCoordinate System ---------------------------------------------------------------------------------------------------------------------------------
IF THIS^.xSuccess THEN 
	IF i_xChangeCOS THEN
		
		THIS^.ifMotion.SetCoordinateSystem(	i_etName		:= i_etCoordinateSystem,  
											q_etDiag		=> etDiag, 
											q_etDiagExt		=> etDiagExtROB, 
											q_sMsg			=> sMsg);

		IF etDiag <> GD.ET_Diag.Ok THEN
			THIS^.xSuccess := FALSE;
			
			ReportException(i_etSource		:= ET_ExceptionSource.ROB,
							i_etDiag		:= etDiag,
							i_udiDiagExt	:= etDiagExtROB,
							i_sMsg			:= sMsg);
			RETURN;
		END_IF
	END_IF

	IF THIS^.iPlacePos_tmp > 0 THEN
// 		IF	THIS^.iq_stRobotModule_Ad.stTracking.xOutfeedBelt_XDirection THEN
// 			THIS^.stTempPosition.lrX :=131.65; //THIS^.iq_astPlacePositions[iPlacePos_tmp].lrActPos;
// 			THIS^.stTempPosition.lrY := 168.25;//THIS^.iq_astPlacePositions[iPlacePos_tmp].lrPosY;
// 		ELSE
// 			THIS^.stTempPosition.lrX :=131.65; //THIS^.iq_astPlacePositions[iPlacePos_tmp].lrPosX;
// 			THIS^.stTempPosition.lrY :=168.25; //THIS^.iq_astPlacePositions[iPlacePos_tmp].lrActPos;
// 		END_IF
IF	THIS^.iq_stRobotModule_Ad.stTracking.xOutfeedBelt_XDirection THEN
			THIS^.stTempPosition.lrX :=THIS^.iq_astPlacePositions[iPlacePos_tmp].lrActPos;
			THIS^.stTempPosition.lrY :=THIS^.iq_astPlacePositions[iPlacePos_tmp].lrPosY;
		ELSE
			THIS^.stTempPosition.lrX :=THIS^.iq_astPlacePositions[iPlacePos_tmp].lrPosX;
			THIS^.stTempPosition.lrY :=THIS^.iq_astPlacePositions[iPlacePos_tmp].lrActPos;
		END_IF
		THIS^.stTempPosition.lrZ := THIS^.iq_stRobotModule_Ad.stMechanicPos.lrMechanicHeightPlace 
									+ (THIS^.iq_stRobotModule_Ad.stMechanicPos.lrProductHeight*(THIS^.iq_astPlacePositions[iPlacePos_tmp].iStackedProducts + 1));
	ELSE	
// // 		THIS^.stTempPosition.lrX := 131.65;//THIS^.iq_stRobotModule_Ad.stMechanicPos.stPlacePos.stPosition.lrX;
// // 		THIS^.stTempPosition.lrY :=168.25; //THIS^.iq_stRobotModule_Ad.stMechanicPos.stPlacePos.stPosition.lrY ;
// // 		THIS^.stTempPosition.lrZ :=112.21// THIS^.iq_stRobotModule_Ad.stMechanicPos.lrMechanicHeightPlace
// 									+ (THIS^.iq_stRobotModule_Ad.stMechanicPos.lrProductHeight*(THIS^.iStackedProducts + 1));	
		THIS^.stTempPosition.lrX := THIS^.iq_stRobotModule_Ad.stMechanicPos.stPlacePos.stPosition.lrX;
		THIS^.stTempPosition.lrY :=THIS^.iq_stRobotModule_Ad.stMechanicPos.stPlacePos.stPosition.lrY ;
		THIS^.stTempPosition.lrZ := THIS^.iq_stRobotModule_Ad.stMechanicPos.lrMechanicHeightPlace
									+ (THIS^.iq_stRobotModule_Ad.stMechanicPos.lrProductHeight*(THIS^.iStackedProducts + 1));
	END_IF

	// Transform target from TCP in flange coordinates
	THIS^.ifRobotTool.CalcFlangePosition(	i_udiToolId				:= i_udiToolId, 
											i_stPositionTCP			:= THIS^.stTempPosition,
											i_lrOrientationTCP		:= THIS^.lrTempAuxPos,
											q_stPositionFlange		=> THIS^.stTempPositionFlange,
											q_lrOrientationFlange	=> THIS^.lrTempAuxPosFlange,
											q_etDiag				=> etDiag,
											q_etDiagExt				=> etDiagExtRobotTool,
											q_sMsg					=> sMsg);
	
	IF	etDiag <> GD.ET_Diag.Ok THEN
		THIS^.xSuccess := FALSE;
		
		ReportException(i_etSource		:= ET_ExceptionSource.RTT,
						i_etDiag		:= etDiag,
						i_udiDiagExt	:= etDiagExtRobotTool,
						i_sMsg			:= sMsg);
						
		RETURN;
	END_IF
//---- MoveS --------------------------------------------------------------------------------------------------------------------------------------	
	stExtendedConfig.xUserApex				:= FALSE;
	stExtendedConfig.lrApexScalingFactor	:= 0.0; 

	stExtendedConfig.lrStartObstacleDistance		:= THIS^.iq_stRobotModule_Ad.stApplication.lrDistanceToObstaclePick; 
	stExtendedConfig.lrStartObstacleHeight			:= THIS^.iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPick;
	stExtendedConfig.diStartNumberOfVerticalPoints	:= THIS^.iq_stRobotModule_Ad.stApplication.diNrOfVerticalPointsPick; 
	
	stExtendedConfig.lrTargetObstacleDistance		:= THIS^.iq_stRobotModule_Ad.stApplication.lrDistanceToObstaclePlace;
	stExtendedConfig.lrTargetObstacleHeight			:= THIS^.iq_stRobotModule_Ad.stMechanicPos.lrLinearMovementPlace;
	stExtendedConfig.diTargetNumberOfVerticalPoints	:= THIS^.iq_stRobotModule_Ad.stApplication.diNrOfVerticalPointsPlace;
	
	stTargetILT		:= THIS^.stTempPosition;
	
	IF THIS^.iq_stRobotModule_Ad.stApplication.xIntelligentTracking
	THEN
		stTargetILT.lrX	:= THIS^.iq_stRobotModule_Ad.stMechanicPos.stWaitPickPosInCSR.stPosition.lrX + THIS^.iq_stRobotModule_Ad.stTracking.LE_OutFeed_Handling.Velocity*lrOffsetTimeRobot_Place;
	END_IF
	
	THIS^.lrTempSplineHeight	:=  FC_CalcSplineHeight(i_stStart				:= THIS^.ifFeedback.rstLastTarget, 
														i_stTarget				:= stTargetILT, 
														i_xUserApex				:= stExtendedConfig.xUserApex,
														i_lrApexScalingFactor	:= stExtendedConfig.lrApexScalingFactor,
														i_lrLinStart			:= stExtendedConfig.lrStartObstacleHeight, 
														i_lrLinTarget			:= stExtendedConfig.lrTargetObstacleHeight, 
														i_lrMinHeight			:= 0.0, 
														i_lrMaxHeight			:= THIS^.iq_stRobotModule_Ad.stMechanicPos.lrMaxSplineHeight, 
														i_lrHeightFactor		:= THIS^.iq_stRobotModule_Ad.stMechanicPos.lrSplineHeightFactor);
												
	THIS^.fbSplineCalc.CalcSplineExtended(	i_stStart			:= THIS^.ifFeedback.rstLastTarget, 
											i_stTarget			:= THIS^.stTempPosition, 
											i_lrAbsHeight		:= THIS^.lrTempSplineHeight, 
											iq_stConfiguration	:= stExtendedConfig, 
											q_etDiag			=> etDiag, 
											q_etDiagExt			=> etDiagExtROB, 
											q_sMsg				=> sMsg, 
											q_stForward			=> THIS^.stSplineTable, 
											q_stReverse			=> , 
											q_stFeedback		=> );

							
	IF etDiag <> GD.ET_Diag.Ok THEN
		THIS^.xSuccess := FALSE;
		
		ReportException(i_etSource		:= ET_ExceptionSource.ROB,
						i_etDiag		:= etDiag,
						i_udiDiagExt	:= etDiagExtROB,
						i_sMsg			:= sMsg);
						
		RETURN;
	END_IF

	THIS^.ifMotion.MoveS(	i_stTarget		:= THIS^.stTempPosition,
							i_stSplineTable := THIS^.stSplineTable, 
							i_lrMaxZone		:= 0.0, 
							i_udiSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
							q_etDiag		=> etDiag, 
							q_etDiagExt		=> etDiagExtROB, 
							q_sMsg			=> sMsg);

	// Reports target position					
	IF	etDiagExtROB = ROB.ET_DiagExt.IdenticalTarget
	OR	etDiagExtROB = ROB.ET_DiagExt.TargetInvalid
	THEN
		ReportTarget(	i_stTarget		:= THIS^.stTempPosition,
						i_udiSegmentID	:= ET_MotionState._0210_MoveToPlacePositionSpl,
						i_etDiagExtROB	:= etDiagExtROB);
	END_IF
				
	IF etDiag <> GD.ET_Diag.Ok THEN
		THIS^.xSuccess := FALSE;
		
		ReportException(i_etSource		:= ET_ExceptionSource.ROB,
						i_etDiag		:= etDiag,
						i_udiDiagExt	:= etDiagExtROB,
						i_sMsg			:= sMsg);
						
		RETURN;
	END_IF

//---- Tracking -------------------------------------------------------------------------
	IF i_xChangeCOS THEN
		THIS^.ifMotion.ChangeCoordinateSystem2(	i_udiStartSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_lrStartOffset		:= i_lrStartOffsetBT, 
												i_udiEndSegmentId	:= ET_MotionState._0210_MoveToPlacePositionSpl, 
												i_lrEndOffset		:= i_lrEndOffsetBT, 
												q_etDiag			=> etDiag, 
												q_etDiagExt			=> etDiagExtROB, 
												q_sMsg				=> sMsg);
											
		IF etDiag <> GD.ET_Diag.Ok THEN
			THIS^.xSuccess := FALSE;
			
			ReportException(i_etSource		:= ET_ExceptionSource.ROB,
							i_etDiag		:= etDiag,
							i_udiDiagExt	:= etDiagExtROB,
							i_sMsg			:= sMsg);
			RETURN;
		END_IF
	END_IF
END_IF