技术说明

VAR_INPUT
  dOffsetA1               : LREAL;
  dOffsetA2               : LREAL;
  dOffsetA3               : LREAL;
  dOffsetA4               : LREAL;
  dLinkLength1            : LREAL;
  dLinkLength2            : LREAL;
  dPitchZC                : LREAL;
END_VAR

FUNCTION_BLOCK test_scara_with_axis
VAR_INPUT
        robPower : BOOL;
        robEnable : BOOL;
        robReset : BOOL;
        robToolSet : BOOL;
        robWObjSet : BOOL;
END_VAR
VAR_OUTPUT
        robState : DINT;
        bPowerDone : BOOL;
END_VAR
VAR_IN_OUT
        ToolOffset : MC_COORD_REF;
        WobjOffset : MC_COORD_REF;
END_VAR
VAR

        bRegulatorOn :BOOL :=FALSE;
        bDriveStart  :BOOL :=FALSE;
        jogAxis : SMC_GroupJog2;
        jogCartesian : SMC_GroupJog2;
        moveAbsJ : MC_MoveDirectAbsolute;
        moveJ : MC_MoveDirectAbsolute; 
        moveL : MC_MoveLinearAbsolute;
        gStop : MC_GroupStop;
        read_Axis_position : MC_GroupReadActualPosition;
        read_Pose_position : MC_GroupReadActualPosition;
        set_Tool : SMC_GroupSetTool;
        gmcs : MC_SetCoordinateTransform ;
        set_PathTolerance : SMC_GroupSetPathTolerance;
        

        
        gPower : SMC_GroupPower ;
        gEnable : MC_GroupEnable ;        
        gReadStatus : MC_GroupReadStatus ;
        gReadError : MC_GroupReadError ;
        gReset : MC_GroupReset ;
        gSetTool : SMC_GroupSetTool ;
        gSetWobj : MC_SetCoordinateTransform;
        state : DINT;
        
        statusText : STRING;
        jogJointEnable: BOOL := FALSE;
        jogCartEnable: BOOL := FALSE;
        jogSpeedFactor: LREAL := 0.5;
        
        jogForward : ARRAY[0..5] OF BOOL ;
        jogBackward : ARRAY[0..5] OF BOOL ;
        bJogAxisActive : BOOL;
        bJogAxisBusy : BOOL;
        bJogAxisError : BOOL;
        bJogAxisErrorID : SMC_ERROR;
        jogAxisPosition: SMC_POS_REF;
        fbGroupSetPathTolerance : SMC_GroupSetPathTolerance;

        iCoordSystem : UDINT := 0;
        select_coord_system : SMC_COORD_SYSTEM;
        jogCartesianForward : ARRAY[0..5] OF BOOL ;
        jogCartesianBackward : ARRAY[0..5] OF BOOL ;
        bJogCartesianActive : BOOL;
        bJogCartesianBusy : BOOL;
        bJogCartesianError : BOOL;
        bJogCartesianErrorID : SMC_ERROR;
        jogCartesianPosition: SMC_POS_REF;
        
        moveAbsJ_Enable : BOOL := FALSE;
        moveAbsJ_Target : SMC_POS_REF;
        moveAbsJ_BufferMode: SM3_Robotics.MC_BUFFER_MODE;
        moveAbsJ_TransitionMode: SM3_Robotics.MC_TRANSITION_MODE;
        moveAbsJ_TransitioParameter: ARRAY [0..(SMC_RCNST.MAX_TRANS_PARAMS - 1)] OF LREAL;
        moveAbsJ_Done: BOOL;
        moveAbsJ_Busy: BOOL;
        moveAbsJ_Active: BOOL;        
        moveAbsJ_CommandAborted: BOOL;
        moveAbsJ_CommandAccepted: BOOL;
        moveAbsJ_Error: BOOL;
        moveAbsJ_ErrorID: SM3_Robotics.SMC_ERROR;
        moveAbsJ_MovementId: SM3_Robotics.SMC_Movement_Id;
        
        
        moveJ_Enable: BOOL := FALSE;
        moveJ_Target: SM3_Robotics.SMC_POS_REF;
        moveJ_Done: BOOL;
        moveJ_Busy: BOOL;
        moveJ_Active: BOOL;
        moveJ_CommandAborted: BOOL;
        moveJ_CommandAccepted: BOOL;
        moveJ_Error: BOOL;
        moveJ_ErrorID: SM3_Robotics.SMC_ERROR;
        moveJ_MovementId: SM3_Robotics.SMC_Movement_Id;
        

        moveL_Enable: BOOL := FALSE;
        moveL_Target: SM3_Robotics.SMC_POS_REF;
        moveL_Done: BOOL;
        moveL_Busy: BOOL;
        moveL_Active: BOOL;
        moveL_CommandAborted: BOOL;
        moveL_CommandAccepted: BOOL;
        moveL_Error: BOOL;
        moveL_ErrorID: SM3_Robotics.SMC_ERROR;
        moveL_MovementId: SM3_Robotics.SMC_Movement_Id;
        

        bStop : BOOL;        
        stop_Done: BOOL;
        stop_Busy: BOOL;
        stop_Active: BOOL;
        stop_CommandAborted: BOOL;
        stop_CommandAccepted: BOOL;
        stop_Error: BOOL;
        stop_ErrorID: SM3_Robotics.SMC_ERROR;
        stop_MovementId: SM3_Robotics.SMC_Movement_Id;
        
        read_Axis_position_enable: BOOL := TRUE;
        read_Axis_position_Valid: BOOL;
        read_Axis_position_Busy: BOOL;
        read_Axis_position_Error: BOOL;
        read_Axis_position_ErrorID: SM3_Robotics.SMC_ERROR;
        read_Axis_position_Position: SM3_Robotics.SMC_POS_REF;
        read_Actual_Axis_position_Position: SM3_Robotics.SMC_POS_REF;
        read_Axis_position_KinematicConfig: SM3_Robotics.TRAFO.CONFIGDATA;
        
        read_Pose_position_enable: BOOL := TRUE;
        read_Pose_position_Valid: BOOL;
        read_Pose_position_Busy: BOOL;
        read_Pose_position_Error: BOOL;
        read_Pose_position_ErrorID: SM3_Robotics.SMC_ERROR;
        read_Pose_position_Position: SM3_Robotics.SMC_POS_REF;
        read_Pose_position_KinematicConfig: SM3_Robotics.TRAFO.CONFIGDATA;
                
        set_Tool_Excute: BOOL;
        set_Tool_ToolOffset: SM3_Robotics.MC_COORD_REF;
        set_Tool_Done: BOOL;
        set_Tool_Error: BOOL;
        set_Tool_ErrorID: SM3_Robotics.SMC_ERROR;
        
        bGMCS : BOOL := FALSE;
        gmcs_coord: SM3_Robotics.MC_COORD_REF;

        set_PathTolerance_Execute: BOOL := FALSE;
        PathTolerance_AxisLag: SM3_Robotics.TRAFO.AXISPOS_REF;
        PathTolerance_MaxPositionLag: LREAL := 10000;
        PathTolerance_MaxOrientationLag: LREAL := 10000;
        PathTolerance_Done: BOOL;
        PathTolerance_Error: BOOL;
        PathTolerance_ErrorID: SM3_Robotics.SMC_ERROR;
        
        axispos : TRAFO.AXISPOS_REF;
        
        robStatus: DINT;
        bPower : BOOL := FALSE;
        bEnable : BOOL := FALSE;
        bReset : BOOL := FALSE;
        bToolSet : BOOL := FALSE;
        bWobjSet : BOOL := FALSE;
        
        ToolOffset1 : MC_COORD_REF;
        WobjOffset1 : MC_COORD_REF;
   
        config1_enable :BOOL :=FALSE;
        config2_enable :BOOL :=FALSE;
        xElbowRight :BOOL :=FALSE;
        config_to_set : TRAFO.Kin_Scara3_Z_Config;
        config_read: TRAFO.Kin_Scara3_Z_ReadConfig;
        skc: SMC_SetKinConfiguration;
        skc_execure :BOOL :=FALSE;
        nP1 :DINT:=1;  nP2 :DINT:=1;  nP3 :DINT:=1;  nP4 :DINT:=1; 
        setConfig : SMC_SetKinConfiguration;
        setConfig_Execute :BOOL :=FALSE;
        Scara3ZConfig : TRAFO.Kin_Scara3_Z_Config;
        periodConfig :SM3_Robotics.TRAFO.CONFIGDATA ;

END_VAR

       
bRegulatorOn := TRUE;  bDriveStart := TRUE; 
bPower:= robPower;     bEnable:= robEnable;
gPower(AxisGroup:= scara,bRegulatorOn:= bRegulatorOn,bDriveStart:= bDriveStart,Status=>,Busy=>,Error=>,ErrorID=>) ;
gEnable(AxisGroup:= scara,Done=>,Busy=>,Error=>,ErrorID=>) ;
CASE state OF
0:
        gPower.Enable := bPower;
        IF gPower.Status THEN
                state := state + 10;
        ELSE
                state := 0;
                RETURN;
        END_IF
10:
        gEnable.Execute := bEnable;
        IF gEnable.Done THEN
                state := state + 10;
        END_IF
20:
        gReadStatus(AxisGroup:= scara);
        gReadError(AxisGroup:= scara);
        gSetTool(AxisGroup:= scara);
        gSetWobj(AxisGroup:= scara);
        gReset(AxisGroup := scara);
        IF gPower.Status AND gEnable.Done THEN
                gReadStatus.Enable :=TRUE;
                gReadError.Enable := TRUE;
                state := state + 10;
        END_IF
END_CASE

IF   robReset   THEN
        gReset(AxisGroup := scara, Execute:=robReset, Done=>,Busy=>,Error=>,ErrorID=>);
END_IF
IF state = 30 THEN
        bPowerDone := TRUE;
ELSE
        bPowerDone := FALSE;
END_IF        
robStatus := state;
robState := robStatus;
IF robState <> 30 THEN
        RETURN;
END_IF
// >>>> robStatus
IF gReadStatus.GroupErrorStop THEN
        statusText :=CONCAT('Error : ',SMC_ErrorString(gReadError.GroupErrorID, SMC_LANGUAGE_TYPE.english));
ELSE
        statusText := 'No Error';
END_IF

jogAxis(
        AxisGroup:= scara, 
        Enable:= jogJointEnable, Forward:= jogForward, Backward:= jogBackward, 
        MaxLinearDistance:= 100, MaxAngularDistance:= 100, 
        Velocity:= 1, Acceleration:= 10, Deceleration:= 10, Jerk:= 100, 
        VelFactor:= jogSpeedFactor, AccFactor:= 1, JerkFactor:= 1, TorqueFactor:= 1, 
        CoordSystem:= SMC_COORD_SYSTEM.ACS, ABC_as_ACS:= FALSE, 
        Active=> bJogAxisActive, Busy=> bJogAxisBusy, 
    Error=> bJogAxisError, ErrorID=> bJogAxisErrorID, CurrentPosition=> jogAxisPosition);
        
jogCartesian(
        AxisGroup                        := scara, 
        Enable                                := jogCartEnable, Forward := jogCartesianForward, Backward := jogCartesianBackward, 
        MaxLinearDistance        := 20, MaxAngularDistance        := 20, 
        Velocity                        := 1, Acceleration        := 10, Deceleration        := 10, Jerk        := 100, 
        VelFactor                        := jogSpeedFactor, AccFactor:= 1, JerkFactor:= 1, TorqueFactor:= 1, 
        CoordSystem                        := SMC_COORD_SYSTEM.MCS, ABC_as_ACS                        := FALSE, 
        Active                                => bJogCartesianActive,   Busy => bJogCartesianBusy, 
    Error => bJogCartesianError, ErrorID => bJogCartesianErrorID, CurrentPosition=> jogCartesianPosition);

IF TRUE THEN
        read_Axis_position(
                AxisGroup:= scara, Enable:= read_Axis_position_enable, 
                CoordSystem:= SMC_COORD_SYSTEM.ACS, 
                Valid=> read_Axis_position_Valid, Busy=> read_Axis_position_Busy, 
        Error=> read_Axis_position_Error, ErrorID=> read_Axis_position_ErrorID, 
                Position=> read_Axis_position_Position, KinematicConfig=> read_Axis_position_KinematicConfig);
END_IF        


select_coord_system := SMC_COORD_SYSTEM.MCS ;
IF TRUE THEN
        read_Pose_position(
                AxisGroup:= scara, 
                Enable:= read_Pose_position_enable, CoordSystem:= select_coord_system, 
                Valid=> read_Pose_position_Valid, Busy=> read_Pose_position_Busy, 
        Error=> read_Pose_position_Error, ErrorID=> read_Pose_position_ErrorID, 
                Position=> read_Pose_position_Position, KinematicConfig=> read_Pose_position_KinematicConfig);                
        
        config_to_set(xElbowRight := xElbowRight, nPeriodA1:=nP1, nPeriodA2:=nP2, nPeriodA3:=nP3, Config=>periodConfig);
        IF  config1_enable THEN
                skc(AxisGroup := scara, Execute := skc_execure, ConfigData := periodConfig, Done =>, Busy =>, Error =>, ErrorId=>);
        END_IF
        IF config2_enable AND read_Pose_position.Valid THEN
                setConfig(AxisGroup:= scara, Execute:= setConfig_Execute, ConfigData:= read_Pose_position_KinematicConfig, 
               Done=>, Busy=>, Error=>, ErrorID=>);                
                IF setConfig.Done THEN
                        setConfig_Execute := FALSE ;
                ELSIF setConfig.Error THEN
                        setConfig_Execute := FALSE ;
                END_IF
    END_IF
END_IF        

CASE iCoordSystem OF
0:
    select_coord_system := SMC_COORD_SYSTEM.MCS ;
1:
    select_coord_system := SMC_COORD_SYSTEM.PCS_1;
ELSE
    select_coord_system := SMC_COORD_SYSTEM.MCS ;
END_CASE

axispos.a0 := 100000000; axispos.a1 := 100000000; axispos.a2 := 100000000; 
axispos.a3 := 100000000; axispos.a4 := 100000000; axispos.a5 := 100000000;
fbGroupSetPathTolerance(
        AxisGroup := scara,
        Execute  := set_PathTolerance_Execute,
        MaxPositionLag := 1000000,
        MaxOrientationLag := 360000,
        MaxAxisLag := axispos        
);
        
moveAbsJ(
        AxisGroup:= scara, 
        Execute:= moveAbsJ_Enable, 
        Position:= moveAbsJ_Target, MovementType:= , 
        CoordSystem:= SMC_COORD_SYSTEM.ACS, 
        BufferMode:= SM3_Robotics.MC_BUFFER_MODE.Aborting, 
        TransitionMode:=  SM3_Robotics.MC_TRANSITION_MODE.TMNone, 
        TransitionParameter:= moveAbsJ_TransitioParameter, 
        VelFactor:= 1, AccFactor:= 1, JerkFactor:= 1, TorqueFactor:= 1, 
        Done=> moveAbsJ_Done, Busy=> moveAbsJ_Busy, Active=> moveAbsJ_Active, 
        CommandAborted=> moveAbsJ_CommandAborted, CommandAccepted=> moveAbsJ_CommandAccepted, 
        Error=> moveAbsJ_Error, ErrorID=> moveAbsJ_ErrorID, 
        MovementId=>moveAbsJ_MovementId);
moveJ(
        AxisGroup:= scara, 
        Execute:= moveJ_Enable, Position:= moveJ_Target, MovementType:= , 
        CoordSystem:= SMC_COORD_SYSTEM.PCS_1, 
        BufferMode:= SM3_Robotics.MC_BUFFER_MODE.Aborting, 
        TransitionMode:= SM3_Robotics.MC_TRANSITION_MODE.TMNone, 
        TransitionParameter:= moveAbsJ_TransitioParameter, 
        VelFactor:= 1, AccFactor:= 1, JerkFactor:= 1, TorqueFactor:= 1, 
        Done=> moveJ_Done, Busy=> moveJ_Busy, Active=> moveJ_Active, 
        CommandAborted=> moveJ_CommandAborted, CommandAccepted=> moveJ_CommandAccepted, 
        Error=> moveJ_Error, ErrorID=> moveJ_ErrorID, MovementId=> moveJ_MovementId);
moveL(
        AxisGroup:= scara, Execute:= moveL_Enable, Position:= moveL_Target, 
        Velocity:= 100, Acceleration:= 1000, Deceleration:= 1000, Jerk:= 1000, 
        CoordSystem:= SMC_COORD_SYSTEM.MCS, 
        BufferMode:= SM3_Robotics.MC_BUFFER_MODE.Aborting, 
        TransitionMode:= SM3_Robotics.MC_TRANSITION_MODE.TMNone, 
        TransitionParameter:= moveAbsJ_TransitioParameter, 
        OrientationMode:= SMC_ORIENTATION_MODE.GreatCircle, 
        VelFactor:= 1, AccFactor:= 1, JerkFactor:= 1, TorqueFactor:= 1, 
        Done=> moveL_Done, Busy=> moveL_Busy, Active=> moveL_Active, 
        CommandAborted=> moveL_CommandAborted, CommandAccepted=> moveL_CommandAccepted, 
        Error=> moveL_Error, ErrorID=> moveL_ErrorID, MovementId=> moveL_MovementId);
gStop(
        AxisGroup:= scara, 
        Execute:= bStop, 
        Deceleration:= 1000, Jerk:= 1000, AccFactor:= 1, JerkFactor:= 1, TorqueFactor:= 1,
        ClearMovements:=TRUE, 
        Done=> stop_Done, Busy=> stop_Busy, Active=> stop_Active, 
        CommandAborted=> stop_CommandAborted, CommandAccepted=> stop_CommandAccepted, 
        Error=> stop_Error, ErrorID=> stop_ErrorID, MovementId=> stop_MovementId);