/// <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);
}
}
public async void DoEvent(EZ_B.Classes.RecordingEvent recordingEvent)
{
await _ezb.sendCommandData(0, recordingEvent.CmdData);
}
