static void DynamixelOpenPort()
{
if (!DynamixelSDK.openPort(DynamixelPortNum))
{
throw new Exception("Failed to open the port!");
}
else
{
Console.WriteLine("Succeeded to open the port!");
}
}
static void DynamixelSetBaudRate()
{
if (!DynamixelSDK.setBaudRate(DynamixelPortNum, DYNAMIXEL_BAUDRATE))
{
throw new Exception("Failed to change the baudrate!");
}
else
{
Console.WriteLine("Succeeded to change the baudrate!");
}
}
static void DynamixelEnableTorque(byte pID)
{
int Result;
byte Error;
DynamixelSDK.write1ByteTxRx(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION, pID, Dynamixel.Constants.ADDR_TORQUE_ENABLE, Dynamixel.Constants.TORQUE_ENABLE);
if ((Result = DynamixelSDK.getLastTxRxResult(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION)) != Dynamixel.Constants.COMM_SUCCESS)
{
throw new Exception(Marshal.PtrToStringAnsi(DynamixelSDK.getTxRxResult(DYNAMIXEL_PROTOCOL_VERSION, Result)));
}
else if ((Error = DynamixelSDK.getLastRxPacketError(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION)) != 0)
{
throw new Exception(Marshal.PtrToStringAnsi(DynamixelSDK.getRxPacketError(DYNAMIXEL_PROTOCOL_VERSION, Error)));
}
else
{
Console.WriteLine("Dynamixel " + pID + " has been successfully connected");
}
}
static void DynamixelEnableWheelMode(byte pID)
{
int Result;
byte Error;
DynamixelSDK.write2ByteTxRx(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION, pID, Dynamixel.Constants.ADDR_CCW_ANGLE_LIMIT_L, DynamixelWheelModeData);
if ((Result = DynamixelSDK.getLastTxRxResult(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION)) != Dynamixel.Constants.COMM_SUCCESS)
{
throw new Exception(Marshal.PtrToStringAnsi(DynamixelSDK.getTxRxResult(DYNAMIXEL_PROTOCOL_VERSION, Result)));
}
else if ((Error = DynamixelSDK.getLastRxPacketError(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION)) != 0)
{
throw new Exception(Marshal.PtrToStringAnsi(DynamixelSDK.getRxPacketError(DYNAMIXEL_PROTOCOL_VERSION, Error)));
}
else
{
Console.WriteLine("Dynamixel " + pID + " is in a wheel mode");
}
}
private static void DynamixelSetGoalPosition(byte pID, ushort pGoalPosition)
{
int Result;
byte Error;
try
{
DynamixelSDK.write2ByteTxRx(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION, pID, Dynamixel.Constants.ADDR_GOAL_POSITION_L, pGoalPosition);
}
catch (Exception)
{
throw;
}
//if ((Result = DynamixelSDK.getLastTxRxResult(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION)) != Dynamixel.Constants.COMM_SUCCESS)
//{
// throw new Exception(Marshal.PtrToStringAnsi(DynamixelSDK.getTxRxResult(DYNAMIXEL_PROTOCOL_VERSION, Result)));
//}
//else if ((Error = DynamixelSDK.getLastRxPacketError(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION)) != 0)
//{
// throw new Exception(Marshal.PtrToStringAnsi(DynamixelSDK.getRxPacketError(DYNAMIXEL_PROTOCOL_VERSION, Error)));
//}
}
static void DynamixelInitialization()
{
DynamixelPortNum = DynamixelSDK.portHandler(DYNAMIXEL_DEVICENAME);
DynamixelSDK.packetHandler();
}
private static ushort DynamixelReadPresentPosition(byte pID)
{
ushort pos = DynamixelSDK.read2ByteTxRx(DynamixelPortNum, DYNAMIXEL_PROTOCOL_VERSION, pID, Dynamixel.Constants.ADDR_PRESENT_POSITION);
return(pos);
}
static void DynamixelClosePort()
{
DynamixelSDK.closePort(DynamixelPortNum);
}
