Ejemplo n.º 1
0
        public void GroupSyncSetGoalPosition(byte[] servoIds, ushort[] goalPositions, DynamixelProtocol protocol = DynamixelProtocol.Version1)
        {
            // assert
            if (servoIds.Length != goalPositions.Length)
            {
                throw new ArgumentException($"{nameof(servoIds)} and {nameof(goalPositions)} have to be the same size");
            }
            ushort address = protocol == DynamixelProtocol.Version1 ? ADDR_MX_GOAL_POSITION : ADDR_XL_GOAL_POSITION;
            // initi group write
            int syncGroupNum = dynamixel.groupSyncWrite(_portNumber, (int)protocol, address, 2);

            for (int index = 0; index < servoIds.Length; index++)
            {
                byte   servoId      = servoIds[index];
                ushort goalPosition = goalPositions[index];
                // add parameters to group write
                bool success = dynamixel.groupSyncWriteAddParam(syncGroupNum, servoId, goalPosition, 2);
                if (!success)
                {
                    throw new IOException("Group write adda param failed");
                }
            }
            // Send commands
            dynamixel.groupSyncWriteTxPacket(syncGroupNum);
            int commResult = dynamixel.getLastTxRxResult(_portNumber, (int)protocol);

            if (commResult != CommSuccess)
            {
                string servoIdDescriptor = servoIds.OrderBy(id => id).Aggregate(string.Empty, (current, id) => current + $" {id}");
                throw new IOException(DynamixelErrorHelper.GetTxRxResultDescription(commResult) + $" group write on:{servoIdDescriptor}");
            }
            // clear group write cache
            dynamixel.groupSyncWriteClearParam(syncGroupNum);
        }
Ejemplo n.º 2
0
        private void VerifyLastMessage(byte servoId, DynamixelProtocol protocol)
        {
            int commResult = dynamixel.getLastTxRxResult(_portNumber, (int)protocol);

            if (commResult != CommSuccess)
            {
                throw new IOException(DynamixelErrorHelper.GetTxRxResultDescription(commResult) + $" on servo: {servoId}");
            }
            byte dxlError = dynamixel.getLastRxPacketError(_portNumber, (int)protocol);

            if (dxlError != 0)
            {
                throw new IOException(DynamixelErrorHelper.GetRxPackErrorDescription(dxlError) + $" on servo: {servoId}");
            }
        }
Ejemplo n.º 3
0
        public bool Ping(byte servoId, DynamixelProtocol protocol = DynamixelProtocol.Version1)
        {
            dynamixel.ping(_portNumber, (int)protocol, servoId);
            if (dynamixel.getLastTxRxResult(_portNumber, (int)protocol) != CommSuccess)
            {
                return(false);
            }
            byte dxlError = dynamixel.getLastRxPacketError(_portNumber, (int)protocol);

            if (dxlError != 0)
            {
                throw new IOException(DynamixelErrorHelper.GetRxPackErrorDescription(dxlError));
            }
            return(true);
        }