/// <summary>
/// Main Cartesion move method
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
/// <returns></returns>
public override async Task MoveToCartesianAsync(double x, double y, double z)
{
ServoPositions servoPositions = CalculateServosFromPosition(x, y, z);
ServoPositions convertedServoPositions = ConvertToAbsoluteServoAnglesOrPwm(servoPositions);
await MoveToConvertedAnglesOrPwmAsync(convertedServoPositions);
}
/// <summary>
/// Main realtive move method
/// </summary>
/// <param name="baseAngle"></param>
/// <param name="distance"></param>
/// <param name="z"></param>
/// <returns></returns>
public override async Task MoveToRelativeAsync(double baseAngle, double distance, double z)
{
VerticalServoPositions verticalServoPositions = CalculateVerticalServoPositions(distance, z);
ServoPositions convertedServoAnglesOrPwm = ConvertToAbsoluteServoAnglesOrPwm(new ServoPositions(baseAngle, verticalServoPositions));
await MoveToConvertedAnglesOrPwmAsync(convertedServoAnglesOrPwm);
}
/// <summary>
/// procedure to convert from information from the arm to wanted angles
/// </summary>
/// <param name="servoPositions"></param>
/// <returns></returns>
private ServoPositions ConvertToAnglesFromServoOrPwm(ServoPositions servoPositions)
{
double baseAngle = m_Configuration.PwmToAngleForBase(servoPositions.Base);
double shoulderAngle = m_Configuration.PwmToAngleForShoulder(servoPositions.Shoulder);
double elbowAngle = m_Configuration.PwmToAngleForElbow(servoPositions.Elbow);
return(new ServoPositions(baseAngle, shoulderAngle, elbowAngle));
}
/// <summary>
/// procedure to convert idel angles to current arm angles
/// </summary>
/// <param name="servoPositions"></param>
/// <returns></returns>
private ServoPositions ConvertToAbsoluteServoAnglesOrPwm(ServoPositions servoPositions)
{
double basePwm = m_Configuration.AngleToPwmForBase(servoPositions.Base);
double shoulderPwm = m_Configuration.AngleToPwmForShoulder(servoPositions.Shoulder);
double elbowPwm = m_Configuration.AngleToPwmForElbow(servoPositions.Elbow);
return(new ServoPositions(basePwm, shoulderPwm, elbowPwm));
}
private async Task MoveToConvertedAnglesOrPwmAsync(ServoPositions servoPwmOrAngles)
{
TaskCompletionSource <bool> completionSource = new TaskCompletionSource <bool>();
EventHandler <ArmDataUpdateEvent> callback = null;
callback = (sender, armData) =>
{
if (!armData.LastSent.RelativeEquals(servoPwmOrAngles))
{
completionSource.SetResult(false);
m_ArmConnector.NewServoPosition -= callback;
}
else if (armData.Current.RelativeEquals(servoPwmOrAngles))
{
completionSource.SetResult(true);
m_ArmConnector.NewServoPosition -= callback;
}
};
await m_ArmConnector.MoveAllServosAsync(servoPwmOrAngles);
m_ArmConnector.NewServoPosition += callback;
await completionSource.Task;
}
