byte[] getMasterCommand(Servo.ServoPortEnum servo, functionInstructionEnum functionInstruction, byte memoryRegister, params byte[] payload)
{
List <byte> data = new List <byte>();
byte id = (byte)(servo - Servo.ServoPortEnum.V0);
byte msgLen = (byte)(3 + payload.Length); // instruction, register, checksum + payload
data.Add(0xff);
data.Add(0xff);
data.Add(id);
data.Add(msgLen);
data.Add((byte)functionInstruction);
data.Add(memoryRegister);
data.AddRange(payload);
byte checkSum = (byte)(id + msgLen + (byte)functionInstruction + memoryRegister);
foreach (byte p in payload)
{
checkSum += p;
}
data.Add((byte)~checkSum);
return(data.ToArray());
}
public void ChangeID(Servo.ServoPortEnum source, Servo.ServoPortEnum destination)
{
byte souceID = (byte)(source - Servo.ServoPortEnum.AXV0);
byte destinationID = (byte)(destination - Servo.ServoPortEnum.AXV0);
SendCommandToEZB(CreateDynamixelCommand(souceID, 3, 3, destinationID));
}
private void Servo_OnServoGetPosition(Servo.ServoPortEnum servoPort, EZ_B.Classes.GetServoValueResponse getServoResponse)
{
if (getServoResponse.Success)
{
return;
}
if (!_customConfig.VirtualPorts.Contains(servoPort))
{
getServoResponse.ErrorStr = "No matching lewansoul servo specified";
getServoResponse.Success = false;
return;
}
Invokers.SetAppendText(tbLog, true, "Reading position from {0}", servoPort);
if (Convert.ToBoolean(_cf.STORAGE[ConfigTitles.USE_COM_PORT]))
{
getServoPositionComSerial(servoPort, getServoResponse);
}
if (!getServoResponse.Success && Convert.ToBoolean(_cf.STORAGE[ConfigTitles.USE_HARDWARE_UART]))
{
getServoPositionEZBUART(servoPort, getServoResponse);
}
}
private void getServoPositionComSerial(Servo.ServoPortEnum servoPort, EZ_B.Classes.GetServoValueResponse getServoResponse)
{
if (!_serialPort.IsOpen)
{
getServoResponse.Success = false;
getServoResponse.ErrorStr = "COM Port not open";
return;
}
_serialPort.DiscardInBuffer();
sendServoCommand(_controller.GetServoPosition(servoPort));
System.Threading.Thread.Sleep(100);
var ret = new byte[_serialPort.BytesToRead];
_serialPort.Read(ret, 0, ret.Length);
if (ret.Length != 14)
{
getServoResponse.ErrorStr = "Servo did not respond";
getServoResponse.Success = false;
return;
}
getServoResponse.Position = (int)EZ_B.Functions.RemapScalar(BitConverter.ToInt16(ret, 11), 1, 1000, Servo.SERVO_MIN, Servo.SERVO_MAX);
getServoResponse.Success = true;
}
byte[] getMasterCommand(Servo.ServoPortEnum servo, byte cmd, params byte[] payload)
{
List <byte> data = new List <byte>();
data.Add(0x55);
data.Add(0x55);
data.Add((byte)(servo - Servo.ServoPortEnum.V0));
data.Add((byte)(payload.Length + 3)); // length = this + cmd + payload + checksum
data.Add(cmd);
foreach (var b in payload)
{
data.Add(b);
}
int checksum = 0;
for (int x = 2; x < data.Count; x++)
{
checksum += data[x];
}
checksum = ~checksum;
data.Add((byte)(checksum & 0xff));
return(data.ToArray());
}
public byte[] SetServoId(Servo.ServoPortEnum servo, Servo.ServoPortEnum newId)
{
return(getMasterCommand(
servo,
SERVO_ID_WRITE,
(byte)(newId - Servo.ServoPortEnum.V0)));
}
public byte[] SetMotorOn(Servo.ServoPortEnum servo, bool status)
{
return(getMasterCommand(
servo,
SERVO_LOAD_OR_UNLOAD_WRITE,
status ? (byte)1 : (byte)0));
}
public byte[] SetMotorOn(Servo.ServoPortEnum servo, bool status)
{
return(getMasterCommand(
servo,
functionInstructionEnum.INST_WRITE,
SCSCL_TORQUE_ENABLE,
status ? (byte)1 : (byte)0));
}
public void SetServoInContinuousMode(Servo.ServoPortEnum servo)
{
if (_MOTOR_MODE_SERVOS.Contains(servo))
{
return;
}
_MOTOR_MODE_SERVOS.Add(servo);
}
public byte[] SetModeServo(Servo.ServoPortEnum servo)
{
return(getMasterCommand(
servo,
SERVO_OR_MOTOR_MODE_WRITE,
0,
0,
0,
0));
}
public ServoDefinition(string jointName, AxisType axis, Servo.ServoPortEnum port, bool inverted = false, int minValue = 1, int maxValue = 180, int middleValue = 90)
{
this.Port = port;
this.JointName = jointName;
this.Axis = axis;
this.inverted = inverted;
this.minValue = minValue;
this.maxValue = maxValue;
this.middleValue = middleValue;
}
/// <summary>
/// Change the LED status of the dynamixel servo
/// </summary>
public void LED(Servo.ServoPortEnum servo, bool status)
{
if (servo < Servo.ServoPortEnum.AX0 || servo > Servo.ServoPortEnum.AX50)
{
throw new Exception("Servo out of range for Dynamixel");
}
byte id = (byte)(servo - Servo.ServoPortEnum.AX0);
LED(id, status);
}
public byte[] SetContinuousMode(Servo.ServoPortEnum servo, int speed)
{
byte[] speedArray = BitConverter.GetBytes((Int16)speed);
return(getMasterCommand(
servo,
SERVO_OR_MOTOR_MODE_WRITE,
1,
0,
speedArray[0],
speedArray[1]));
}
public void DisableAllAlarms(Servo.ServoPortEnum servo)
{
byte id = (byte)(servo - Servo.ServoPortEnum.AX0);
List <byte> buffer = new List <byte>();
buffer.Add(3); // Write data
buffer.Add(0x12);
buffer.Add(254);
SendCommandToEZB(CreateDynamixelCommand(id, buffer.ToArray()));
}
public void ChangeID(Servo.ServoPortEnum source, Servo.ServoPortEnum destination)
{
byte souceID = (byte)(source - Servo.ServoPortEnum.AX0);
byte destinationID = (byte)(destination - Servo.ServoPortEnum.AX0);
List <byte> buffer = new List <byte>();
buffer.Add(3); // Write data
buffer.Add(3); // Change ID
buffer.Add(destinationID);
SendCommandToEZB(CreateDynamixelCommand(souceID, buffer.ToArray()));
}
public byte[] SetServoMove(Servo.ServoPortEnum servo, int position)
{
byte[] positionArray = BitConverter.GetBytes((Int16)position);
return(getMasterCommand(
servo,
SERVO_MOVE_TIME_WRITE,
positionArray[0], // 1 - lower 8 angle
positionArray[1], // 2 - higher 8 angle
0x00, // 3 - lower 8 of time
0x00)); // 4 - higher 8 of time
}
public byte[] SetPositionLimits(Servo.ServoPortEnum servo, int minPosition, int maxPosition)
{
minPosition = Functions.Clamp(minPosition, 0, 1000);
maxPosition = Functions.Clamp(maxPosition, 0, 1000);
byte[] minArray = BitConverter.GetBytes((Int16)minPosition);
byte[] maxArray = BitConverter.GetBytes((Int16)maxPosition);
return(getMasterCommand(
servo,
SERVO_ANGLE_LIMIT_WRITE,
minArray[0],
minArray[1],
maxArray[0],
maxArray[1]));
}
public void SetServoPosition(Servo.ServoPortEnum servoPort, int position)
{
if (!this.ezb.IsConnected)
{
Debug.LogWarning("EZB is not connected, SetServoPosition will be ignored");
return;
}
try
{
this.ezb.Servo.SetServoPosition(servoPort, position);
}
catch (Exception ex)
{
Debug.LogError(ex);
}
}
public byte[] SetPositionLimits(Servo.ServoPortEnum servo, int minPosition, int maxPosition)
{
minPosition = Functions.Clamp(minPosition, 0, 1023);
maxPosition = Functions.Clamp(maxPosition, 0, 1023);
byte[] minArray = BitConverter.GetBytes((Int16)minPosition);
byte[] maxArray = BitConverter.GetBytes((Int16)maxPosition);
return(getMasterCommand(
servo,
functionInstructionEnum.INST_WRITE,
SCSCL_GOAL_POSITION_L,
minArray[0],
minArray[1],
maxArray[0],
maxArray[1]));
}
public byte[] SetServoMove(Servo.ServoPortEnum servo, int position)
{
position = Functions.Clamp(position, 0, 1023);
byte[] positionArray = BitConverter.GetBytes((Int16)position);
return(getMasterCommand(
servo,
functionInstructionEnum.INST_WRITE,
SCSCL_GOAL_POSITION_L,
positionArray[1], // 1 - lower 8 angle
positionArray[0], // 2 - higher 8 angle
0, // time
0, // time
3, // velocity 1000
232 // velocity 1000
));
}
byte[] getMasterCommand(Servo.ServoPortEnum servo, functionInstructionEnum functionInstruction)
{
List <byte> data = new List <byte>();
byte id = (byte)(servo - Servo.ServoPortEnum.V0);
byte msgLen = 2; // instruction + checksum
data.Add(0xff);
data.Add(0xff);
data.Add(id);
data.Add(msgLen);
data.Add((byte)functionInstruction);
byte checkSum = (byte)(id + msgLen + (byte)functionInstruction);
data.Add((byte)~checkSum);
return(data.ToArray());
}
private void getServoPositionEZBUART(Servo.ServoPortEnum servoPort, EZ_B.Classes.GetServoValueResponse getServoResponse)
{
if (!EZBManager.EZBs[0].IsConnected)
{
getServoResponse.Success = false;
getServoResponse.ErrorStr = "Not connected to EZ-B 0";
return;
}
if (!Convert.ToBoolean(_cf.STORAGE[ConfigTitles.USE_HARDWARE_UART]))
{
getServoResponse.ErrorStr = "This feature is only available when using the hardware uart";
getServoResponse.Success = false;
return;
}
initUART();
sendServoCommand(_controller.GetServoPosition(servoPort));
System.Threading.Thread.Sleep(100);
var ret = EZBManager.EZBs[0].Uart.UARTExpansionReadAvailable(Convert.ToInt16(_cf.STORAGE[ConfigTitles.HARDWARE_PORT]));
if (ret.Length != 14)
{
getServoResponse.ErrorStr = "Servo did not respond";
getServoResponse.Success = false;
return;
}
getServoResponse.Position = (int)EZ_B.Functions.RemapScalar(BitConverter.ToInt16(ret.Reverse().ToArray(), 0), 1, 1000, Servo.SERVO_MIN, Servo.SERVO_MAX);
getServoResponse.Success = true;
}
public byte[] GetServoPosition(Servo.ServoPortEnum servo)
{
return(new byte[0]);
}
public void SetServoInServoMode(Servo.ServoPortEnum servo)
{
_MOTOR_MODE_SERVOS.Remove(servo);
}
public bool IsServoInServoMode(Servo.ServoPortEnum servo)
{
return(!_MOTOR_MODE_SERVOS.Contains(servo));
}
public byte[] Ping(Servo.ServoPortEnum servo)
{
return(getMasterCommand(
servo,
functionInstructionEnum.INST_PING));
}
public ServoItem(Servo.ServoPortEnum port, int position)
{
Port = port;
Position = position;
}
public void LED(Servo.ServoPortEnum servo, bool status)
{
byte id = (byte)(servo - Servo.ServoPortEnum.AXV0);
LED(id, status);
}
public void DisableStatusPacket(Servo.ServoPortEnum servo)
{
byte id = (byte)(servo - Servo.ServoPortEnum.AX0);
SendCommandToEZB(GetDisableStatusPacketCmd(id));
}
public void DisableAllAlarms(Servo.ServoPortEnum servo)
{
byte id = (byte)(servo - Servo.ServoPortEnum.AXV0);
SendCommandToEZB(CreateDynamixelCommand(id, 3, 0x12, 254));
}
