public async Task MoveAllServosAsync(ServoPositions position)
{
LastSetServoPositions = position;
byte[] byteArray = new byte[7];
byteArray[0] = (byte)ArmCommands.AllServoSet; // command number
int pwm = position.Base.RoundToInt();
byteArray[1] = (byte)((pwm >> 8) & 0xFF);
byteArray[2] = (byte)(pwm & 0xFF);
pwm = position.Shoulder.RoundToInt();
byteArray[3] = (byte)((pwm >> 8) & 0xFF);
byteArray[4] = (byte)(pwm & 0xFF);
pwm = position.Elbow.RoundToInt();
byteArray[5] = (byte)((pwm >> 8) & 0xFF);
byteArray[6] = (byte)(pwm & 0xFF);
await m_CommandBuffer.SendAsync(byteArray);
}
private async void PropagatorLoopAsync()
{
CancellationToken cancellationToken = m_CancellationTokenSource.Token;
try
{
int counter = 0;
byte[] data = new byte[6];
while (!cancellationToken.IsCancellationRequested)
{
byte[] incomingData = await m_InputBuffer.ReceiveAsync(cancellationToken);
foreach (var incomingByte in incomingData)
{
if (counter > 2)
{
data[counter - c_ServoDateIdentifier.Length] = incomingByte;
if (counter - c_ServoDateIdentifier.Length == data.Length - 1)
{
int baseAngle = (data[0] << 8) | data[1];
int shoulderAngle = (data[2] << 8) | data[3];
int elbowAngle = (data[4] << 8) | data[5];
var currentServoPosition = new ServoPositions(baseAngle, shoulderAngle, elbowAngle);
NewServoPosition?.Invoke(this, new ArmDataUpdateEvent(currentServoPosition, LastSetServoPositions));
counter = 0;
}
else
{
counter++;
}
}
else if (incomingByte == c_ServoDateIdentifier[counter])
{
counter++;
}
}
}
}
catch (TaskCanceledException)
{
}
}
public ArmDataUpdateEvent(ServoPositions current, ServoPositions lastSent)
{
Current = current;
LastSent = lastSent;
}
