# 技术说明

### 功能块

#### FB_Scara_ZCcoupled

SCARA机器人运动学变换模块FB_Scara_ZCcoupled有多个输入，具体描述如下。

接口

VAR_INPUT

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

名称 | 类型 | 描述
--- | --- | ---
dOffsetA1 | LREAL | A1轴，运动学零位时关节旋转角度
dOffsetA2 | LREAL | A2轴，运动学零位时关节旋转角度
dOffsetA3 | LREAL | A3轴，运动学零位时关节移动距离
dOffsetA4 | LREAL | A4轴，运动学零位时关节旋转角度
dLinkLength1 | LREAL | 连杆参数1，关节1和关节2轴线的公垂线长度X
dLinkLength2 | LREAL | 连杆参数2，关节2和关节3轴线的公垂线长度X
dPitchZC | LREAL | 关节4正旋转360度，TCP向上移动的高度，若TCP实际向下运动则为负值

### 测试代码

以下示例代码用于 4D-SCARA 机器人的运动学变换测试。运行代码后，用户可以通过网页访问机器人测试的可视化界面，在虚拟环境中对轴运动进行测试和验证。

    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);