/// <summary>
/// Release servo. Release a servo from holding its position.
/// If modified, stops the servo.
/// </summary>
public async Task ReleaseServo(ServoPortEnum servoPort)
{
_servoLastMoveTime[(int)servoPort] = DateTime.Now;
_servoReleased[(int)servoPort] = true;
if (servoPort >= ServoPortEnum.D0 && servoPort <= ServoPortEnum.D23)
{
await _ezb.sendCommand(EZB.CommandEnum.CmdSetServoPosition + (byte)servoPort, new byte[] { (byte)0 });
}
else if (servoPort >= ServoPortEnum.AX0 && servoPort <= ServoPortEnum.AX50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4)
{
byte id = (byte)(servoPort - ServoPortEnum.AX0);
List <byte> cmdData = new List <byte>();
cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4);
cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo);
byte[] cmd = Dynamixel.ReleaseServo(id);
cmdData.Add((byte)cmd.Length);
cmdData.AddRange(cmd);
await _ezb.sendCommandData(0, cmdData.ToArray());
}
else if (servoPort >= ServoPortEnum.AXV0 && servoPort <= ServoPortEnum.AXV50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4)
{
byte id = (byte)(servoPort - ServoPortEnum.AXV0);
List <byte> cmdData = new List <byte>();
cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4);
cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo);
byte[] cmd = DynamixelV2.ReleaseServo(id);
cmdData.Add((byte)cmd.Length);
cmdData.AddRange(cmd);
await _ezb.sendCommandData(0, cmdData.ToArray());
}
}
/// <summary>
/// Set the speed of multiple servos
/// </summary>
/// <param name="servoPort"></param>
/// <param name="speed"></param>
public async Task SetServoSpeed(ServoPortEnum[] servoPorts, int speed)
{
if (!_ezb.IsConnected)
{
return;
}
if (speed > SERVO_SPEED_SLOWEST)
{
speed = SERVO_SPEED_SLOWEST;
}
else if (speed < SERVO_SPEED_FASTEST)
{
speed = SERVO_SPEED_FASTEST;
}
List <byte> cmdData = new List <byte>();
foreach (ServoPortEnum servoPort in servoPorts)
{
if (_servoReleased[(int)servoPort])
{
return;
}
if (_servoSpeeds[(int)servoPort] == speed)
{
return;
}
_servoSpeeds[(int)servoPort] = speed;
if (servoPort >= ServoPortEnum.D0 && servoPort <= ServoPortEnum.D23)
{
cmdData.Add((byte)((byte)EZB.CommandEnum.CmdSetServoSpeed + servoPort));
cmdData.Add((byte)speed);
}
else if (servoPort >= ServoPortEnum.AX0 && servoPort <= ServoPortEnum.AX50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4)
{
byte id = (byte)(servoPort - ServoPortEnum.AX0);
int position = speed * 51;
cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4);
cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo);
byte[] cmd = Dynamixel.ServoSpeed(id, position);
cmdData.Add((byte)cmd.Length);
cmdData.AddRange(cmd);
}
else if (servoPort >= ServoPortEnum.AXV0 && servoPort <= ServoPortEnum.AXV50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4)
{
byte id = (byte)(servoPort - ServoPortEnum.AXV0);
int position = speed * 51;
cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4);
cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo);
byte[] cmd = DynamixelV2.ServoSpeed(id, position);
cmdData.Add((byte)cmd.Length);
cmdData.AddRange(cmd);
}
}
await _ezb.sendCommandData(0, cmdData.ToArray());
}
/// <summary>
/// Set the position of a servo
/// Uses the last speed specified
/// </summary>
public async Task SetServoPosition(Classes.ServoItem[] servos)
{
if (!_ezb.IsConnected)
{
return;
}
List <byte> cmdData = new List <byte>();
foreach (Classes.ServoItem servo in servos)
{
if (servo.Position < _servoMin[(int)servo.Port] || servo.Position > _servoMax[(int)servo.Port])
{
continue;
}
_servoPositions[(int)servo.Port] = servo.Position;
_servoReleased[(int)servo.Port] = false;
_servoLastMoveTime[(int)servo.Port] = DateTime.Now;
int tmpPosition = servo.Position + _servoFineTune[(int)servo.Port];
if (tmpPosition > SERVO_MAX)
{
tmpPosition = SERVO_MAX;
}
else if (tmpPosition < SERVO_MIN)
{
tmpPosition = SERVO_MIN;
}
if (servo.Port >= ServoPortEnum.D0 && servo.Port <= ServoPortEnum.D23)
{
cmdData.AddRange(new byte[] { (byte)(EZB.CommandEnum.CmdSetServoPosition + (byte)servo.Port), (byte)tmpPosition });
}
else if (servo.Port >= ServoPortEnum.AX0 && servo.Port <= ServoPortEnum.AX50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4)
{
byte id = (byte)(servo.Port - ServoPortEnum.AX0);
int position = (int)(tmpPosition * 5.689);
cmdData.Add((byte)EZB.CommandEnum.CmdEZBv4);
cmdData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo);
byte[] cmd1 = Dynamixel.GetDisableStatusPacketCmd(id);
byte[] cmd2 = Dynamixel.MoveServoCmd(id, position);
cmdData.Add((byte)(cmd1.Length + cmd2.Length));
cmdData.AddRange(cmd1);
cmdData.AddRange(cmd2);
}
else if (servo.Port >= ServoPortEnum.AXV0 && servo.Port <= ServoPortEnum.AXV50 && _ezb.EZBType == EZB.EZ_B_Type_Enum.ezb4)
{
List <byte> tmpData = new List <byte>();
byte id = (byte)(servo.Port - ServoPortEnum.AXV0);
int position = (int)(tmpPosition * 5.689);
tmpData.Add((byte)EZB.CommandEnum.CmdEZBv4);
tmpData.Add((byte)EZB.CmdEZBv4Enum.CmdV4SetAX12Servo);
byte[] cmd1 = DynamixelV2.MoveServoCmd(id, position);
byte[] cmd2 = DynamixelV2.GetDisableStatusPacketCmd(id);
tmpData.Add((byte)(cmd1.Length + cmd2.Length));
tmpData.AddRange(cmd1);
tmpData.AddRange(cmd2);
await _ezb.sendCommandData(0, tmpData.ToArray());
}
}
if (cmdData.Count > 0)
{
await _ezb.sendCommandData(0, cmdData.ToArray());
}
if (OnServoMove != null)
{
OnServoMove(servos);
}
}
