//This file was generated by the LASAL2 CodeGenerator  -- 
//Please, do not edit this file (it might be overwritten by the next generator run)
//{{LSL_DECLARATION

(*!
<Class
	Name               = "MotionControl"
	Revision           = "0.0"
	GUID               = "{E2AEF431-8434-41FE-8517-4703A47D4FE7}"
	RealtimeTask       = "true"
	DefRealtime        = "1 ms"
	CyclicTask         = "false"
	BackgroundTask     = "false"
	Sigmatek           = "false"
	OSInterface        = "false"
	HighPriority       = "false"
	Automatic          = "false"
	UpdateMode         = "Prescan"
	SharedCommandTable = "true"
	Objectsize         = "(690,420)">
	<Channels>
		<Server Name="Axis1CmdError" GUID="{B0409E07-7B46-46B2-85D8-4BF94C6B353F}" Visualized="false" Initialize="false" WriteProtected="true" Retentive="false"/>
		<Server Name="Axis2CmdError" GUID="{68541D9C-ECC6-4035-BCD8-854E8C81A3CB}" Visualized="false" Initialize="false" WriteProtected="true" Retentive="false"/>
		<Server Name="IntError" GUID="{C86BF1BB-FAA5-41C6-BABC-C2EA6F5024FF}" Visualized="false" Initialize="false" WriteProtected="true" Retentive="false"/>
		<Server Name="QuitError" GUID="{F34948B8-6019-43D0-BAEC-4913EEBF58C3}" Visualized="false" Initialize="false" WriteProtected="false" Retentive="false"/>
		<Server Name="Start" GUID="{43694706-603B-47CF-A129-148FE897BA77}" Visualized="false" Initialize="true" WriteProtected="false" Retentive="File"/>
		<Server Name="Steps" GUID="{F53D9352-1C89-4B93-9A6B-F002F5BBEA66}" Visualized="false" Initialize="false" WriteProtected="true" Retentive="false"/>
		<Client Name="Axis1" Required="true" Internal="false"/>
		<Client Name="Axis2" Required="true" Internal="false"/>
		<Client Name="RefDir1" Required="true" Internal="false"/>
		<Client Name="RefDir2" Required="true" Internal="false"/>
	</Channels>
	<Separators>
		<Servers>
			<SepChn Position="4"/>
		</Servers>
	</Separators>
</Class>
*)
MotionControl : CLASS
	TYPE
	  t_e_MotionControlSteps :  //! <Type Public="true" Name="t_e_MotionControlSteps"/>
	  (
	    MCS_WaitForHWReady,
	    MCS_WaitForStart,
	    MCS_PowerOff:=2,
	    MCS_Error:=3,
	    MCS_PowerOn:=4,
	    MCS_WaitForPowerOn:=5,
	    MCS_DoReference:=6,
	    MCS_IsReferenced:=7,
	    MCS_MoveAxis1_360deg:=8,
	    MCS_WaitAxis1_360deg:=9,
	    MCS_MoveAxis2_360deg:=10,
	    MCS_WaitAxis2_360deg:=11,
	    MCS_StartAxis1_2_Tune:=12,
	    MCS_WaitAxis1_2_Tune:=13,
	    MCS_IncreaseAxis1_2_Tune:=14,
	    MCS_MaxAxis1_2_Tune:=15
	  )$UDINT;
	END_TYPE
  //Servers:
	Steps 	: SvrChCmd_t_e_MotionControlSteps_PTofCls_MotionControl;
	IntError 	: SvrCh_DINT;
	Axis1CmdError 	: SvrCh__LMCAXIS_CMDERROR;
	Axis2CmdError 	: SvrCh__LMCAXIS_CMDERROR;
	Start 	: SvrCh_DINT;
	QuitError 	: SvrCh_DINT;
  //Clients:
	Axis1 	: CltChCmd__LMCAxis;
	Axis2 	: CltChCmd__LMCAxis;
	RefDir1 	: CltCh_DINT;
	RefDir2 	: CltCh_DINT;
  //Variables:
		TimeMark 	: UDINT;
  //Functions:
	
	FUNCTION VIRTUAL GLOBAL RtWork
		VAR_INPUT
			EAX 	: UDINT;
		END_VAR
		VAR_OUTPUT
			state (EAX) 	: UDINT;
		END_VAR;
	
	FUNCTION MotionIsActive
		VAR_OUTPUT
			IsActive 	: BOOL;
		END_VAR;
	
	FUNCTION RefMode
		VAR_INPUT
			RefDir 	: DINT;
		END_VAR
		VAR_OUTPUT
			Mode 	: _LMCAXIS_REFMODE;
		END_VAR;
  //Tables:
	FUNCTION @STD
		VAR_OUTPUT
			ret_code	: CONFSTATES;
		END_VAR;
	FUNCTION GLOBAL TAB @CT_;
END_CLASS;

#pragma usingLtd _LMCAxis


//}}LSL_DECLARATION


FUNCTION GLOBAL TAB MotionControl::@CT_
0$UINT,
2#0100000000000010$UINT, //TY_MOTIONCONTROL
0$UINT, 0$UINT, (SIZEOF(::MotionControl))$UINT, 
6$UINT, 4$UINT, 0$UINT, 
TO_UDINT(1864633404), "MotionControl", //Class
TO_UDINT(0), 0, 0$UINT, 0$UINT, //Baseclass
//Servers:
(::MotionControl.Steps.pMeth)$UINT, _CH_CMD$UINT, 2#0000000000000000$UINT, TO_UDINT(3279341762), "Steps", 
(::MotionControl.IntError.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(3161508790), "IntError", 
(::MotionControl.Axis1CmdError.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(3923311871), "Axis1CmdError", 
(::MotionControl.Axis2CmdError.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(3495280698), "Axis2CmdError", 
(::MotionControl.Start.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000011000$UINT, TO_UDINT(1747818303), "Start", 
(::MotionControl.QuitError.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(3032567726), "QuitError", 
//Clients:
(::MotionControl.Axis1.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000010$UINT, TO_UDINT(302717259), "Axis1", TO_UDINT(1422175863), "_LMCAxis", 1$UINT, 119$UINT, 
(::MotionControl.Axis2.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000010$UINT, TO_UDINT(2332182769), "Axis2", TO_UDINT(1422175863), "_LMCAxis", 1$UINT, 119$UINT, 
(::MotionControl.RefDir1.pCh)$UINT, _CH_CLT_DATA$UINT, 2#0000000000000010$UINT, TO_UDINT(325998367), "RefDir1", 
(::MotionControl.RefDir2.pCh)$UINT, _CH_CLT_DATA$UINT, 2#0000000000000010$UINT, TO_UDINT(2322007717), "RefDir2", 
END_FUNCTION


#define USER_CNT_MotionControl 0

TYPE
	_LSL_STD_VMETH	: STRUCT
			CmdTable	: CMDMETH;
			UserFcts	: ARRAY[0..USER_CNT_MotionControl] OF ^Void;
	END_STRUCT;
END_TYPE

FUNCTION MotionControl::@STD
	VAR_OUTPUT
		ret_code	: CONFSTATES;
	END_VAR
	VAR
		vmt	: _LSL_STD_VMETH;
	END_VAR

	//Command Methods
	InitCmdTable (nCmd := nSTDCMD + USER_CNT_MotionControl, pCmd := #vmt.CmdTable);
	vmt.CmdTable.RtWork		:= #RtWork();
	Steps.pMeth		:= StoreCmd (pCmd := #vmt.CmdTable, SHARED);

	IF Steps.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	Start.pMeth			:= StoreMethod( #M_RD_DIRECT(), #M_WR_DIRECT() );
	IF Start.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	QuitError.pMeth			:= StoreMethod( #M_RD_DIRECT(), #M_WR_DIRECT() );
	IF QuitError.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;

END_FUNCTION

//{{LSL_IMPLEMENTATION


FUNCTION VIRTUAL GLOBAL MotionControl::RtWork
	VAR_INPUT
		EAX 	: UDINT;
	END_VAR
	VAR_OUTPUT
		state (EAX) 	: UDINT;
	END_VAR
  
  
  //========================================================================================
  // Power Off Axis
  //========================================================================================    
  if (Start = 0) 
   & MotionIsActive() then
    Steps := MCS_PowerOff;  
  end_if;

  //========================================================================================
  // MOTION CONTROL STEPS FOR MDD2000 TRAINING SETUP
  //========================================================================================
  case Steps of
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_WaitForHWReady:
    
      if (Axis1.AxisStatus.ReadyToPowerOn = true)
       & (Axis2.AxisStatus.ReadyToPowerOn = true) then
        Steps := MCS_WaitForStart;
      end_if;
    
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_WaitForStart:
    
      if (Start = 1) then
        Steps := MCS_PowerOn;
      end_if;
    
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_PowerOn:
    
      Axis1CmdError := Axis1.PowerOn(Mode:= LMCAXIS_MOVE_ANY_WAY);
      Axis2CmdError := Axis2.PowerOn(Mode:= LMCAXIS_MOVE_ANY_WAY);                 

      if (Axis1CmdError = 0)   
       & (Axis2CmdError = 0)  then        
        TimeMark := ops.tAbsolute;  
        Steps := MCS_WaitForPowerOn; 
      else
        intError := -1;
        Steps := MCS_Error;
      end_if;    
    
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_WaitForPowerOn:
    
      if (Axis1.AxisStatus.PowerOn = 1) 
       & (Axis2.AxisStatus.PowerOn = 1) then      
        Steps := MCS_DoReference;  
      else        
        if (ops.tAbsolute - TimeMark) > (5 sec) then
          intError := -2;
          Steps := MCS_Error;
        end_if;
      end_if;
    
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_DoReference:
    
      //Axis1CmdError := Axis1.SetPosition(Mode:= LMCAXIS_SET_ACTPOS_APPUNIT, Position:= 0);
      RefDir1 := RefDir1.Read();
      Axis1CmdError := Axis1.MoveReference( Mode:=RefMode(RefDir1), 
                                            Position:=0, 
                                            VRef1:=(12000 deg), 
                                            VRef2:=(360 deg), 
                                            Accel:=(24000 deg), 
                                            PositionWindow:=(360 deg), 
                                            RefJerk:=0);
      
      //Axis1CmdError := Axis2.SetPosition(Mode:= LMCAXIS_SET_ACTPOS_APPUNIT, Position:= 0);
      RefDir2 := RefDir2.Read();
      Axis2CmdError := Axis2.MoveReference( Mode:=RefMode(RefDir2), 
                                            Position:=0, 
                                            VRef1:=(12000 deg), 
                                            VRef2:=(360 deg), 
                                            Accel:=(24000 deg), 
                                            PositionWindow:=(360 deg), 
                                            RefJerk:=0);         
      if (Axis1CmdError = 0)   
       & (Axis1CmdError = 0) then        
        TimeMark := ops.tAbsolute;  
        Steps := MCS_IsReferenced; 
      else
        intError := -3;
        Steps := MCS_Error;
      end_if; 

    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_IsReferenced:
    
      if (Axis1.AxisStatus.IsReferenced = 1)    
       & (Axis2.AxisStatus.IsReferenced = 1) then          
        Steps := MCS_MoveAxis1_360deg;
      else
        if (ops.tAbsolute - TimeMark) > (60 sec) then
          intError := -4;
          Steps := MCS_Error;
        end_if;      
      end_if;
    
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_MoveAxis1_360deg:
    
      Axis1CmdError := Axis1.MoveAbsolute(  Position:= (360 deg), 
                                            Speed:= (144 deg), 
                                            Accel:= Axis1.AMax, 
                                            Decel:= Axis1.AMax, 
                                            Jerk:= 0);
                               
      if (Axis1CmdError = 0) then
        TimeMark := ops.tAbsolute;
        Steps := MCS_WaitAxis1_360deg; 
      else
        intError := -5;
        Steps := MCS_Error;
      end_if;

    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_WaitAxis1_360deg:

      if Axis1.InPosition(Mode:= LMCAXIS_POSITIONWINDOW_APP_UNITS, PositionWindow:= 0.1 deg) then
         Steps := MCS_MoveAxis2_360deg;
      else
        if (ops.tAbsolute - TimeMark) > (5 sec) then
          intError := -6;
          Steps := MCS_Error;
        end_if;      
      end_if;

    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_MoveAxis2_360deg:
    
      Axis2CmdError := Axis2.MoveAbsolute(  Position:= (360 deg), 
                                            Speed:= (144 deg), 
                                            Accel:= Axis1.AMax, 
                                            Decel:= Axis1.AMax, 
                                            Jerk:= 0);
                               
      if (Axis2CmdError = 0) then
        TimeMark := ops.tAbsolute;
        Steps := MCS_WaitAxis2_360deg; 
      else
        intError := -7;
        Steps := MCS_Error;
      end_if;

    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_WaitAxis2_360deg:

      if Axis2.InPosition(Mode:= LMCAXIS_POSITIONWINDOW_APP_UNITS, PositionWindow:= 0.1 deg) then
         Steps := MCS_StartAxis1_2_Tune;
      else
        if (ops.tAbsolute - TimeMark) > (5 sec) then
          intError := -8;
          Steps := MCS_Error;
        end_if;      
      end_if;
    
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_StartAxis1_2_Tune:
    
      Axis1.Override := 100;
      Axis1CmdError := Axis1.TuneAxis(  Position:= (7200 deg), 
                                        Speed:= Axis1.VMax, 
                                        Accel:= Axis1.AMax, 
                                        Mode:= LMCAXIS_TUNE_ZERO_TO_POSITIVE_POSITION, 
                                        WaitTime:= (500 ms), 
                                        Jerk:= 0);
      Axis2.Override := 100;
      Axis2CmdError := Axis2.TuneAxis(  Position:= (7200 deg), 
                                        Speed:= Axis2.VMax, 
                                        Accel:= Axis2.AMax, 
                                        Mode:= LMCAXIS_TUNE_ZERO_TO_POSITIVE_POSITION, 
                                        WaitTime:= (500 ms), 
                                        Jerk:= 0);
                                        
       if (Axis1CmdError = 0)
        & (Axis2CmdError = 0) then
        TimeMark := ops.tAbsolute;
        Steps := MCS_WaitAxis1_2_Tune;
       else
        IntError := -9;
        Steps := MCS_Error;
       end_if;

    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_WaitAxis1_2_Tune:
    
      if (ops.tAbsolute - TimeMark) > (1 sec) then
        Steps := MCS_IncreaseAxis1_2_Tune;
      end_if;
    
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_IncreaseAxis1_2_Tune:
    
        Axis1.Override += 50;
        Axis1.Override.Write(Axis1.Override);
        
        Axis2.Override += 50;
        Axis2.Override.Write(Axis2.Override);
        
        if (Axis1.Override = 1000)
         & (Axis2.Override = 1000) then
          Steps := MCS_MaxAxis1_2_Tune;
        else
          TimeMark := ops.tAbsolute;
          Steps := MCS_WaitAxis1_2_Tune;
        end_if;

    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_MaxAxis1_2_Tune:
    
      // Wait for Start is 0
    
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_PowerOff:
    
      Axis1CmdError := Axis1.PowerOff(Mode:= LMCAXIS_SMOOTH_STOPP);
      Axis2CmdError := Axis2.PowerOff(Mode:= LMCAXIS_SMOOTH_STOPP);
      
      Steps := MCS_WaitForStart;
      
    //======================================================================================
  	MotionControl::t_e_MotionControlSteps::MCS_Error:
    
      Axis1CmdError := Axis1.PowerOff(Mode:= LMCAXIS_SMOOTH_STOPP);
      Axis2CmdError := Axis2.PowerOff(Mode:= LMCAXIS_SMOOTH_STOPP);
      
      if (QuitError = 1) then
        QuitError := 0;
        intError := 0;
        Axis1.QuitError();
        Axis2.QuitError();
        Steps := MCS_WaitForHWReady;
      end_if;
    
  end_case;

	state := READY;

END_FUNCTION


FUNCTION MotionControl::MotionIsActive
	VAR_OUTPUT
		IsActive 	: BOOL;
	END_VAR

  IsActive := (Steps >=  MCS_PowerOn);       

END_FUNCTION


FUNCTION MotionControl::RefMode
	VAR_INPUT
		RefDir 	: DINT;
	END_VAR
	VAR_OUTPUT
		Mode 	: _LMCAXIS_REFMODE;
	END_VAR

  Mode := 0;
  Mode.NoZImpulse := true;
  Mode.RefDirection := to_bool(RefDir);

END_FUNCTION
