CtrlGroup.c

Go to the documentation of this file.

// CtrlGroup.c
//
/*
* Software License Agreement (BSD License) 
*
* Copyright (c) 2013, Yaskawa America, Inc.
* All rights reserved.
*
* Redistribution and use in binary form, with or without modification,
* is permitted provided that the following conditions are met:
*
*       * Redistributions in binary form must reproduce the above copyright
*       notice, this list of conditions and the following disclaimer in the
*       documentation and/or other materials provided with the distribution.
*       * Neither the name of the Yaskawa America, Inc., nor the names 
*       of its contributors may be used to endorse or promote products derived
*       from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/ 

#include "MotoROS.h"

//-----------------------
// Function implementation
//-----------------------

//-------------------------------------------------------------------
// Search through the control group to find the GroupId that matches 
// the group number
//-------------------------------------------------------------------
MP_GRP_ID_TYPE Ros_CtrlGroup_FindGrpId(int groupNo)
{
        MP_GRP_ID_TYPE grp_id;
        
        for(grp_id = MP_R1_GID; grp_id < MP_S3_GID; grp_id++)
        {
                if(groupNo == mpCtrlGrpId2GrpNo(grp_id))
                        return grp_id;
        }
        
        return -1;
}

//-------------------------------------------------------------------
// Create a CtrlGroup data structure for existing group otherwise 
// return NULL
//-------------------------------------------------------------------
CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod)
{
        CtrlGroup* ctrlGroup;
        int numAxes;
        int i;
#ifdef DX100            
        float speedCap;
#endif
        long maxSpeedPulse[MP_GRP_AXES_NUM];
        STATUS status;
        BOOL bInitOk;
        BOOL slaveAxis;
        
        // Check if group is defined
        numAxes = GP_getNumberOfAxes(groupNo);
#ifdef DEBUG
        printf("Group %d: Num Axes %d\n", groupNo, numAxes);
#endif
        if(numAxes > 0)
        {
                bInitOk = TRUE;
                // Allocate and initialize memory
                ctrlGroup = mpMalloc(sizeof(CtrlGroup));
                memset(ctrlGroup, 0x00, sizeof(CtrlGroup));
                
                // Populate values
                ctrlGroup->groupNo = groupNo;
                ctrlGroup->numAxes = numAxes;
                ctrlGroup->groupId = Ros_CtrlGroup_FindGrpId(groupNo);
                ctrlGroup->tool = 0;

                status = GP_getAxisMotionType(groupNo, &ctrlGroup->axisType);
                if (status != OK)
                        bInitOk = FALSE;

                status = GP_getPulseToRad(groupNo, &ctrlGroup->pulseToRad);
                if (status != OK)
                        bInitOk = FALSE;

                status = GP_getPulseToMeter(groupNo, &ctrlGroup->pulseToMeter);
                if (status != OK)
                        bInitOk = FALSE;

                status = GP_getFBPulseCorrection(groupNo, &ctrlGroup->correctionData);
                if(status!=OK)
                        bInitOk = FALSE;

                status = GP_getMaxIncPerIpCycle(groupNo, interpolPeriod, &ctrlGroup->maxInc);
                if (status != OK)
                        bInitOk = FALSE;

                status = GP_isBaxisSlave(groupNo, &slaveAxis);
                if (status != OK)
                        bInitOk = FALSE;

                status = GP_getFeedbackSpeedMRegisterAddresses(groupNo, //zero based index of the control group
                                                                                                                TRUE, //If the register-speed-feedback is not enabled, automatically modify the SC.PRM file to enable this feature.
                                                                                                                bIsLastGrpToInit, //If activating the reg-speed-feedback feature, delay the alarm until all the groups have been processed.
                                                                                                                &ctrlGroup->speedFeedbackRegisterAddress); //[OUT] Index of the M registers containing the feedback speed values.
                if (status != OK)
                {
                        ctrlGroup->speedFeedbackRegisterAddress.bFeedbackSpeedEnabled = FALSE;
                }

                ctrlGroup->bIsBaxisSlave = (numAxes == 5) && slaveAxis;

                //adjust the axisType field to account for robots with non-contiguous axes (such as delta or palletizing which use SLU--T axes)
                for (i = 0; i < MP_GRP_AXES_NUM; i += 1)
                {
                        if (ctrlGroup->maxInc.maxIncrement[i] == 0) //if the axis can't move, then I assume it's invalid
                                ctrlGroup->axisType.type[i] = AXIS_INVALID;
                }

                memset(&ctrlGroup->inc_q, 0x00, sizeof(Incremental_q));
                ctrlGroup->inc_q.q_lock = mpSemBCreate(SEM_Q_FIFO, SEM_FULL);

#ifdef DX100            
                speedCap = GP_getGovForIncMotion(groupNo);
                if(speedCap != -1)
                {
                        for(i=0; i<MP_GRP_AXES_NUM; i++)
                                ctrlGroup->maxInc.maxIncrement[i] *= speedCap;
                }
                else
                        bInitOk = FALSE;
#endif

                // Calculate maximum speed in radian per second
                memset(maxSpeedPulse, 0x00, sizeof(maxSpeedPulse));
                for(i=0; i<MP_GRP_AXES_NUM; i++)
                        maxSpeedPulse[i] = ctrlGroup->maxInc.maxIncrement[i] * 1000.0 / interpolPeriod; 
                Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, maxSpeedPulse, ctrlGroup->maxSpeed); 

                printf("axisType[%d]: ", groupNo);
                for (i = 0; i < MP_GRP_AXES_NUM; i++)
                {
                        if (ctrlGroup->axisType.type[i] == AXIS_LINEAR)
                                printf("Lin\t");
                        else if (ctrlGroup->axisType.type[i] == AXIS_ROTATION)
                                printf("Rot\t");
                        else
                        {
                                printf("---\t");
                                //motoRosAssert(FALSE, SUBCODE_INVALID_AXIS_TYPE, "Grp%d Axs%d type invalid", groupNo, i);
                        }
                }
                printf(";\r\n");

                printf("pulse->unit[%d]: ", groupNo);
                for (i = 0; i < MP_GRP_AXES_NUM; i++)
                {
                        if (ctrlGroup->axisType.type[i] == AXIS_LINEAR)
                                printf("%.4f\t", ctrlGroup->pulseToMeter.PtoM[i]);
                        else if (ctrlGroup->axisType.type[i] == AXIS_ROTATION)
                                printf("%.4f\t", ctrlGroup->pulseToRad.PtoR[i]);
                        else
                                printf("--\t");
                }
                printf(";\r\n");

                printf("maxInc[%d] (in motoman joint order): %d, %d, %d, %d, %d, %d, %d\r\n", 
                        groupNo,
                        ctrlGroup->maxInc.maxIncrement[0],ctrlGroup->maxInc.maxIncrement[1],ctrlGroup->maxInc.maxIncrement[2],
                        ctrlGroup->maxInc.maxIncrement[3],ctrlGroup->maxInc.maxIncrement[4],ctrlGroup->maxInc.maxIncrement[5],
                        ctrlGroup->maxInc.maxIncrement[6]);

                printf("maxSpeed[%d] (in ros joint order): %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f\r\n",
                        groupNo,
                        ctrlGroup->maxSpeed[0],ctrlGroup->maxSpeed[1],ctrlGroup->maxSpeed[2],
                        ctrlGroup->maxSpeed[3],ctrlGroup->maxSpeed[4],ctrlGroup->maxSpeed[5],
                        ctrlGroup->maxSpeed[6]);

                ctrlGroup->tidAddToIncQueue = INVALID_TASK;
                
                if(bInitOk == FALSE)
                {
                        mpFree(ctrlGroup);
                        ctrlGroup = NULL;
                }
        }
        else
        {
                ctrlGroup = NULL;
        }
        
        return ctrlGroup;
}


//-------------------------------------------------------------------
// Get the commanded pulse position in pulse (in motoman joint order)
// Used for MOTION SERVER connection for positional planning calculations.
//-------------------------------------------------------------------
BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES])
{
        LONG status = 0;
        MP_CTRL_GRP_SEND_DATA sData;
        MP_PULSE_POS_RSP_DATA pulse_data;
        float rosAnglePos[MP_GRP_AXES_NUM]; //temporary storage for B axis compensation
        int i;

        memset(pulsePos, 0, MAX_PULSE_AXES*sizeof(long));  // clear result, in case of error

        // Set the control group
        switch(ctrlGroup->groupId)
        {
                case MP_R1_GID: sData.sCtrlGrp = 0; break;
                case MP_R2_GID: sData.sCtrlGrp = 1; break;
                case MP_R3_GID: sData.sCtrlGrp = 2; break;
                case MP_R4_GID: sData.sCtrlGrp = 3; break;
                case MP_B1_GID: sData.sCtrlGrp = 8; break;
                case MP_B2_GID: sData.sCtrlGrp = 9; break;
                case MP_B3_GID: sData.sCtrlGrp = 10; break;
                case MP_B4_GID: sData.sCtrlGrp = 11; break;
                case MP_S1_GID: sData.sCtrlGrp = 16; break;
                case MP_S2_GID: sData.sCtrlGrp = 17; break;
                case MP_S3_GID: sData.sCtrlGrp = 18; break;
                default:
                        printf("Failed to get pulse feedback position\nInvalid groupId: %d\n", ctrlGroup->groupId);
                        return FALSE;
        }
        
        // get the command joint positions
        status = mpGetPulsePos (&sData,&pulse_data);
        if (0 != status)
        {
                printf("Failed to get pulse position (command): %u\n", status);
                return FALSE;
        }
        
        // assign return value
        for (i=0; i<MAX_PULSE_AXES; ++i)
                pulsePos[i] = pulse_data.lPos[i];

        // For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
        // to maintain orientation as other axes are moved.
        if (ctrlGroup->bIsBaxisSlave)
        {
                //B axis compensation works on the ROS ANGLE positions, not on MOTO PULSE positions
                Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, pulsePos, rosAnglePos);
                rosAnglePos[3] += -rosAnglePos[1] + rosAnglePos[2];
                Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, rosAnglePos, pulsePos);
        }

        return TRUE;
}


//-------------------------------------------------------------------
// Get the corrected feedback pulse position in pulse.
// Used exclusively for STATE SERVER connection to report position.
//-------------------------------------------------------------------
BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES])
{
        LONG status = 0;
        MP_CTRL_GRP_SEND_DATA sData;
#ifndef DUMMY_SERVO_MODE
        MP_FB_PULSE_POS_RSP_DATA pulse_data;
#else
        MP_PULSE_POS_RSP_DATA pulse_data;
#endif
        int i;

        memset(pulsePos, 0, MAX_PULSE_AXES*sizeof(long));  // clear result, in case of error

        // Set the control group
        switch(ctrlGroup->groupId)
        {
                case MP_R1_GID: sData.sCtrlGrp = 0; break;
                case MP_R2_GID: sData.sCtrlGrp = 1; break;
                case MP_R3_GID: sData.sCtrlGrp = 2; break;
                case MP_R4_GID: sData.sCtrlGrp = 3; break;
                case MP_B1_GID: sData.sCtrlGrp = 8; break;
                case MP_B2_GID: sData.sCtrlGrp = 9; break;
                case MP_B3_GID: sData.sCtrlGrp = 10; break;
                case MP_B4_GID: sData.sCtrlGrp = 11; break;
                case MP_S1_GID: sData.sCtrlGrp = 16; break;
                case MP_S2_GID: sData.sCtrlGrp = 17; break;
                case MP_S3_GID: sData.sCtrlGrp = 18; break;
                default: 
                        printf("Failed to get pulse feedback position\nInvalid groupId: %d\n", ctrlGroup->groupId);
                        return FALSE;
        }
        
#ifndef DUMMY_SERVO_MODE
        // get raw (uncorrected/unscaled) joint positions
        status = mpGetFBPulsePos (&sData,&pulse_data);
        if (0 != status)
        {
                printf("Failed to get pulse feedback position: %u\n", status);
                return FALSE;
        }
        
        // apply correction to account for cross-axis coupling
        // Note: this is only required for feedback position
        // controller handles this correction internally when 
        // dealing with command positon.
        for (i=0; i<MAX_PULSE_AXES; ++i)
        {
                FB_AXIS_CORRECTION *corr = &ctrlGroup->correctionData.correction[i];
                if (corr->bValid)
                {
                        int src_axis = corr->ulSourceAxis;
                        int dest_axis = corr->ulCorrectionAxis;
                        pulse_data.lPos[dest_axis] -= (int)(pulse_data.lPos[src_axis] * corr->fCorrectionRatio);
                }
        }
#else
        mpGetPulsePos(&sData, &pulse_data);
#endif
        
        // assign return value
        for (i=0; i<MAX_PULSE_AXES; ++i)
                pulsePos[i] = pulse_data.lPos[i];

        //--------------------------------------------------------------------
        //NOTE: Do NOT apply any B axis compensation here. 
        //              This is actual feedback which is reported to the state server.
        //--------------------------------------------------------------------
        
        return TRUE;
}

//-------------------------------------------------------------------
// Get the corrected feedback pulse speed in pulse for each axis.
//-------------------------------------------------------------------
BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PULSE_AXES])
{
        int i;

#ifndef DUMMY_SERVO_MODE
#if (DX100 || FS100) //Use mpSvsGetVelTrqFb for older controller models
        MP_GRP_AXES_T dst_vel;
        LONG status;

        if (ctrlGroup->groupNo >= MAX_CONTROLLABLE_GROUPS)
                return FALSE;
        
        memset(&dst_vel, 0x00, sizeof(MP_GRP_AXES_T));

        status = mpSvsGetVelTrqFb(dst_vel, NULL); //units are 0.1 pulse/sec
        if (status != OK)
                return FALSE;

        for (i = 0; i < MAX_PULSE_AXES; i += 1)
        {
                pulseSpeed[i] = dst_vel[ctrlGroup->groupNo][i] * 0.1;
        }

        // Apply correction to account for cross-axis coupling.
        // Note: This is only required for feedback.
        // Controller handles this correction internally when 
        // dealing with command positon.
        for (i = 0; i< MAX_PULSE_AXES; ++i)
        {
                FB_AXIS_CORRECTION *corr = &ctrlGroup->correctionData.correction[i];
                if (corr->bValid)
                {
                        int src_axis = corr->ulSourceAxis;
                        int dest_axis = corr->ulCorrectionAxis;
                        pulseSpeed[dest_axis] -= (int)(pulseSpeed[src_axis] * corr->fCorrectionRatio);
                }
        }

#else //DX200 and newer supports the M-register analog feedback (higher precision feedback)
        LONG status;
        MP_IO_INFO registerInfo[MAX_PULSE_AXES * 2]; //values are 4 bytes, which consumes 2 registers
        USHORT registerValues[MAX_PULSE_AXES * 2];
        UINT32 registerValuesLong[MAX_PULSE_AXES * 2];
        double dblRegister;

        if (!ctrlGroup->speedFeedbackRegisterAddress.bFeedbackSpeedEnabled)
                return FALSE;

        for (i = 0; i < MAX_PULSE_AXES; i += 1)
        {
                registerInfo[i * 2].ulAddr = ctrlGroup->speedFeedbackRegisterAddress.cioAddressForAxis[i][0];
                registerInfo[(i * 2) + 1].ulAddr = ctrlGroup->speedFeedbackRegisterAddress.cioAddressForAxis[i][1];
        }

        // get raw (uncorrected/unscaled) joint speeds
        status = mpReadIO(registerInfo, registerValues, MAX_PULSE_AXES * 2);
        if (status != OK)
        {
                printf("Failed to get pulse feedback speed: %u\n", status);
                return FALSE;
        }

        for (i = 0; i < MAX_PULSE_AXES; i += 1)
        {
                //move to 32 bit storage
                registerValuesLong[i * 2] = registerValues[i * 2];
                registerValuesLong[(i * 2) + 1] = registerValues[(i * 2) + 1];

                //combine both registers into single 4 byte value (0.0001 deg/sec or 1 um/sec)
                dblRegister = (registerValuesLong[(i * 2) + 1] << 16) | registerValuesLong[i * 2];

                //convert to pulse/sec
                if (ctrlGroup->axisType.type[i] == AXIS_ROTATION)
                {
                        dblRegister /= 1.0E4; //deg/sec
                        dblRegister *= RAD_PER_DEGREE; //rad/sec
                        dblRegister *= ctrlGroup->pulseToRad.PtoR[i]; //pulse/sec
                }
                else if (ctrlGroup->axisType.type[i] == AXIS_LINEAR)
                {
                        dblRegister /= 1.0E6; //m/sec
                        dblRegister *= ctrlGroup->pulseToMeter.PtoM[i]; //pulse/sec
                }

                pulseSpeed[i] = (long)dblRegister;
        }
#endif

#else //dummy-servo mode for testing
        MP_CTRL_GRP_SEND_DATA sData;
        MP_SERVO_SPEED_RSP_DATA pulse_data;

        mpGetServoSpeed(&sData, &pulse_data);

        // assign return value
        for (i = 0; i<MAX_PULSE_AXES; ++i)
                pulseSpeed[i] = pulse_data.lSpeed[i];
#endif

        return TRUE;
}

//-------------------------------------------------------------------
// Retrieves the absolute value (Nm) of the maximum current servo torque.
//-------------------------------------------------------------------
BOOL Ros_CtrlGroup_GetTorque(CtrlGroup* ctrlGroup, double torqueValues[MAX_PULSE_AXES])
{
    MP_GRP_AXES_T dst_vel;
    MP_TRQ_CTL_VAL dst_trq;
        LONG status = 0;
        int i;

        memset(torqueValues, 0, sizeof(torqueValues)); // clear result, in case of error
        memset(dst_trq.data, 0, sizeof(MP_TRQCTL_DATA));
        dst_trq.unit = TRQ_NEWTON_METER; //request data in Nm

        memset(&dst_vel, 0x00, sizeof(MP_GRP_AXES_T));

        status = mpSvsGetVelTrqFb(dst_vel, &dst_trq);
        if (status != OK)
                return FALSE;

        for (i = 0; i < MAX_PULSE_AXES; i += 1) 
        {
            torqueValues[i] = (double)dst_trq.data[ctrlGroup->groupId][i] * 0.000001; //Use double.  Float only good for 6 sig digits.
        }
    
    return TRUE;
}

// Convert Motoman position in pulse to Ros position in radian/meters
// In the case of a 7, 4, or 5 axis robot, adjust the order to match 
// the physical axis sequence
//-------------------------------------------------------------------
void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long motopulsePos[MAX_PULSE_AXES],
                                                                        float rosPos[MAX_PULSE_AXES])
{
        int i;
        float conversion = 1;
        int rpi = 0; //rosPos index
        int mpi = 0; //motopos index

        if ((ctrlGroup->numAxes == 7) && Ros_CtrlGroup_IsRobot(ctrlGroup)) //is robot, and is 7 axis
        {
                // Adjust joint order for 7 axis robot (SLURBTE > SLEURBT); All rotary axes

                for (i = 0; i < ctrlGroup->numAxes; i++)
                {
                        if (i < 2)
                                rosPos[i] = motopulsePos[i] / ctrlGroup->pulseToRad.PtoR[i];
                        else if (i == 2)
                                rosPos[2] = motopulsePos[6] / ctrlGroup->pulseToRad.PtoR[6];
                        else
                                rosPos[i] = motopulsePos[i - 1] / ctrlGroup->pulseToRad.PtoR[i - 1];
                }
        }
        else if (Ros_CtrlGroup_IsRobot(ctrlGroup) && ctrlGroup->numAxes < 6)
        {
                //Delta: (SLU--T- > SLUT---) All rotary axes
                //Scara: (SLUR--- > SLUR---) U-axis is linear
                //Large Palletizing: (SLU--T- > SLUT---) All rotary axes
                //High Speed Picking: (SLU-BT- > SLUBT--) All rotary axes

                for (i = rpi = mpi = 0; i < ctrlGroup->numAxes; i += 1, rpi += 1, mpi += 1)
                {
                        while (ctrlGroup->axisType.type[mpi] == AXIS_INVALID)
                        {
                                mpi += 1;
                                if (mpi >= MAX_PULSE_AXES)
                                        return;
                        }

                        if (ctrlGroup->axisType.type[mpi] == AXIS_ROTATION)
                                conversion = ctrlGroup->pulseToRad.PtoR[mpi];
                        else if (ctrlGroup->axisType.type[mpi] == AXIS_LINEAR)
                                conversion = ctrlGroup->pulseToMeter.PtoM[mpi];
                        else
                                conversion = 1.0;

                        rosPos[rpi] = motopulsePos[mpi] / conversion;
                }
        }
        else
        {
                for (i = 0; i < MAX_PULSE_AXES; i++)
                {
                        if (ctrlGroup->axisType.type[i] == AXIS_ROTATION)
                                conversion = ctrlGroup->pulseToRad.PtoR[i];
                        else if (ctrlGroup->axisType.type[i] == AXIS_LINEAR)
                                conversion = ctrlGroup->pulseToMeter.PtoM[i];
                        else
                                conversion = 1.0;

                        rosPos[i] = motopulsePos[i] / conversion;
                }
        }
}

//-------------------------------------------------------------------
// Convert Ros position in radian to Motoman position in pulse
// In the case of a 7 or 4 axis robot, adjust the order to match 
// the motoman axis sequence
//-------------------------------------------------------------------
void Ros_CtrlGroup_ConvertToMotoPos(CtrlGroup* ctrlGroup, float radPos[MAX_PULSE_AXES], long motopulsePos[MAX_PULSE_AXES])
{
        int i;
        float conversion = 1;
        int rpi = 0; //radpos index
        int mpi = 0; //motopos index

        // Initialize memory space
        memset(motopulsePos, 0x00, sizeof(long)*MAX_PULSE_AXES);

        if ((ctrlGroup->numAxes == 7) && Ros_CtrlGroup_IsRobot(ctrlGroup))
        {
                // Adjust joint order for 7 axis robot (SLEURBT > SLURBTE); All rotary axes

                for (i = 0; i < ctrlGroup->numAxes; i++)
                {
                        if (i < 2)
                                motopulsePos[i] = (int)(radPos[i] * ctrlGroup->pulseToRad.PtoR[i]);
                        else if (i == 2)
                                motopulsePos[6] = (int)(radPos[2] * ctrlGroup->pulseToRad.PtoR[6]);
                        else
                                motopulsePos[i - 1] = (int)(radPos[i] * ctrlGroup->pulseToRad.PtoR[i - 1]);
                }
        }
        else if (Ros_CtrlGroup_IsRobot(ctrlGroup) && ctrlGroup->numAxes < 6)
        {
                //Delta: (SLUT--- > SLU--T-) All rotary axes
                //Scara: (SLUR--- > SLUR---) U-axis is linear
                //Large Palletizing: (SLUT--- > SLU--T-) All rotary axes
                //High Speed Picking: (SLUBT-- > SLU-BT-) All rotary axes

                for (i = rpi = mpi = 0; i < ctrlGroup->numAxes; i += 1, rpi += 1, mpi += 1)
                {
                        while (ctrlGroup->axisType.type[mpi] == AXIS_INVALID)
                        {
                                mpi += 1;
                                if (mpi >= MAX_PULSE_AXES)
                                        return;
                        }

                        if (ctrlGroup->axisType.type[mpi] == AXIS_ROTATION)
                                conversion = ctrlGroup->pulseToRad.PtoR[mpi];
                        else if (ctrlGroup->axisType.type[mpi] == AXIS_LINEAR)
                                conversion = ctrlGroup->pulseToMeter.PtoM[mpi];
                        else
                                conversion = 1.0;

                        motopulsePos[mpi] = (int)(radPos[rpi] * conversion);
                }
        }
        else
        {
                // Convert to pulse
                for (i = 0; i < MAX_PULSE_AXES; i++)
                {
                        if (ctrlGroup->axisType.type[i] == AXIS_ROTATION)
                                conversion = ctrlGroup->pulseToRad.PtoR[i];
                        else if (ctrlGroup->axisType.type[i] == AXIS_LINEAR)
                                conversion = ctrlGroup->pulseToMeter.PtoM[i];
                        else
                                conversion = 1.0;

                        motopulsePos[i] = (int)(radPos[i] * conversion);
                }
        }
}

//-------------------------------------------------------------------
// Returns a bit wise axis configuration for the increment move API
//-------------------------------------------------------------------
UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup)
{
        int i;
        int axisConfig = 0;
        
        for (i = 0; i < MAX_PULSE_AXES; i++)
        {
                if (ctrlGroup->axisType.type[i] != AXIS_INVALID)
                        axisConfig |= (0x01 << i);
        }
                
        return (UCHAR)axisConfig;
}

//-------------------------------------------------------------------
// Returns TRUE is the specified group is defined as a robot
//-------------------------------------------------------------------
BOOL Ros_CtrlGroup_IsRobot(CtrlGroup* ctrlGroup)
{
        return((ctrlGroup->groupId >= MP_R1_GID) && (ctrlGroup->groupId <= MP_R4_GID));
}