//Test & Example to move robots
static async Task TestMotionCommands(IUArm uArm)
{
Console.WriteLine("NOTE: Before testing the motion of the robot, ");
Console.WriteLine(" - Move the robot arm to some position, such that the robot can move to any direction, by 50mm");
Console.WriteLine(" - Recommend to detach the heavy accessories like laser module, (for the case of test failure)");
Console.WriteLine(" - Check that the hand does not hit the robot itself with any rotation (no extremely large hand equipped)");
Console.WriteLine("Press ENTER to off the motor (be careful: the arm go down by the gravity!)");
Console.ReadLine();
await uArm.DetachAllMotorAsync();
await Task.Delay(500);
Console.WriteLine("After moving the robot, press ENTER to on the motor (*robot does not move immediately, there is one more confirmation step)");
Console.ReadLine();
await uArm.AttachAllMotorAsync();
await Task.Delay(500);
Console.WriteLine("Take a distance from robot, then press ENTER to start the motion test");
Console.ReadLine();
Console.WriteLine("Start motion test..");
//Get base state
var currentPos = await uArm.GetPositionAsync();
var currentAngles = await uArm.GetAllServoAnglesAsync();
Console.WriteLine($"Current Pos : {currentPos}");
Console.WriteLine($"Current Angle: {currentAngles}");
#region Relative XYZ
Console.WriteLine("Move by relative XYZ motion");
Console.WriteLine("+X");
await uArm.MoveRelativeAsync(new Position(50, 0, 0), 500);
await Task.Delay(100);
Console.WriteLine("-X");
await uArm.MoveRelativeAsync(new Position(-50, 0, 0), 500);
await Task.Delay(100);
Console.WriteLine("+Y");
await uArm.MoveRelativeAsync(new Position(0, 50, 0), 500);
await Task.Delay(100);
Console.WriteLine("-Y");
await uArm.MoveRelativeAsync(new Position(0, -50, 0), 500);
await Task.Delay(100);
Console.WriteLine("-Z");
await uArm.MoveRelativeAsync(new Position(0, 0, 50), 500);
await Task.Delay(100);
Console.WriteLine("-Z");
await uArm.MoveRelativeAsync(new Position(0, 0, -50), 500);
await Task.Delay(100);
Console.WriteLine($"Current Pos : {await uArm.GetPositionAsync()}");
Console.WriteLine($"Current Angle: {await uArm.GetAllServoAnglesAsync()}");
#endregion
#region Relative Polar(SRH)
Console.WriteLine("Move by relative Polar(SRH) motion");
Console.WriteLine("+S");
await uArm.MoveRelativeAsync(new Polar(50, 0, 0), 500);
await Task.Delay(100);
Console.WriteLine("-S");
await uArm.MoveRelativeAsync(new Polar(-50, 0, 0), 500);
await Task.Delay(100);
Console.WriteLine("+R");
await uArm.MoveRelativeAsync(new Polar(0, 10, 0), 500);
await Task.Delay(100);
Console.WriteLine("-R");
await uArm.MoveRelativeAsync(new Polar(0, -10, 0), 500);
await Task.Delay(100);
Console.WriteLine("-H");
await uArm.MoveRelativeAsync(new Polar(0, 0, 50), 500);
await Task.Delay(100);
Console.WriteLine("-H");
await uArm.MoveRelativeAsync(new Polar(0, 0, -50), 500);
await Task.Delay(100);
Console.WriteLine($"Current Pos : {await uArm.GetPositionAsync()}");
Console.WriteLine($"Current Angle: {await uArm.GetAllServoAnglesAsync()}");
#endregion
#region Absolute Angle(Bottom, Left, Right, Hand)
Console.WriteLine("Move by absolute angle(BLR, and Hand) input");
Console.WriteLine("Bottom +/-");
await uArm.MoveServoAngleAsync(Servos.Bottom, currentAngles.Bottom + 10.0f);
await Task.Delay(500);
await uArm.MoveServoAngleAsync(Servos.Bottom, currentAngles.Bottom);
await Task.Delay(500);
Console.WriteLine("Left +/-");
await uArm.MoveServoAngleAsync(Servos.Left, currentAngles.Left + 10.0f);
await Task.Delay(500);
await uArm.MoveServoAngleAsync(Servos.Left, currentAngles.Left);
await Task.Delay(500);
Console.WriteLine("Right +/-");
await uArm.MoveServoAngleAsync(Servos.Right, currentAngles.Right + 10.0f);
await Task.Delay(500);
await uArm.MoveServoAngleAsync(Servos.Right, currentAngles.Right);
await Task.Delay(500);
Console.WriteLine("Hand 0deg, 45deg, 135deg, 180deg, and 90deg");
await uArm.MoveServoAngleAsync(Servos.Hand, 0);
await Task.Delay(500);
await uArm.MoveServoAngleAsync(Servos.Hand, 45);
await Task.Delay(500);
await uArm.MoveServoAngleAsync(Servos.Hand, 135);
await Task.Delay(500);
await uArm.MoveServoAngleAsync(Servos.Hand, 180);
await Task.Delay(500);
await uArm.MoveServoAngleAsync(Servos.Hand, 90);
await Task.Delay(500);
#endregion
#region Absolute XYZ / Polar
Console.WriteLine("Move by absolute XYZ motion");
await uArm.MoveAsync(new Position(currentPos.X + 50, currentPos.Y + 50, currentPos.Z + 50), 500);
await Task.Delay(100);
await uArm.MoveAsync(currentPos, 500);
await Task.Delay(100);
Console.WriteLine("Move by absolute Polar motion");
var currentPolar = await uArm.GetPolarAsync();
await uArm.MoveAsync(
new Polar(currentPolar.Stretch + 50, currentPolar.Rotation + 10, currentPolar.Height + 50), 500
);
await Task.Delay(100);
await uArm.MoveAsync(currentPolar, 500);
await Task.Delay(100);
#endregion
}