//Test & Example to call getter commands
//NOTE: this test only reads current status of the robot, so does not include output commands
static async Task TestQueryCommands(IUArm uArm)
{
// System information
Console.WriteLine("Dev Name: " + await uArm.GetDeviceNameAsync());
Console.WriteLine("H/W ver : " + await uArm.GetHardwareVersionAsync());
Console.WriteLine("S/W ver : " + await uArm.GetSoftwareVersionAsync());
Console.WriteLine("API ver : " + await uArm.GetAPIVersionAsync());
Console.WriteLine("UID : " + await uArm.GetUIDAsync());
// Position-related information
// API to get single servo angle
Console.WriteLine($"Angle (single): B = {await uArm.GetServoAngleAsync(Servos.Bottom)}");
Console.WriteLine($"Angle (single): L = {await uArm.GetServoAngleAsync(Servos.Left)}");
Console.WriteLine($"Angle (single): R = {await uArm.GetServoAngleAsync(Servos.Right)}");
// API to get all servo angle (except hand)
ServoAngles angles = await uArm.GetAllServoAnglesAsync();
Console.WriteLine($"Angles: B={angles.Bottom}, L={angles.Left}, R={angles.Right}");
Position pos = await uArm.GetPositionAsync();
Console.WriteLine($"Pos : X={pos.X}, Y={pos.Y}, Z={pos.Z}");
Polar polar = await uArm.GetPolarAsync();
Console.WriteLine($"Polar : S={polar.Stretch}, R={polar.Rotation}, H={polar.Height}");
// NOTE: this data is NOT the angle (seems to be pulse value)
ServoAngles defaultPulse = await uArm.GetDefaultValueOfAS5600Async();
Console.WriteLine($"Default Pulse: B={defaultPulse.Bottom}, L={defaultPulse.Left}, R={defaultPulse.Right}");
// I/O-related information
Console.WriteLine($"Pump state : {await uArm.GetPumpStatusAsync()}");
Console.WriteLine($"Gripper state : {await uArm.GetGripperStatusAsync()}");
Console.WriteLine($"Digital In 0 High? : {await uArm.GetDigitalPinStateAsync(0)}");
Console.WriteLine($"Analog In 0 ADC : {await uArm.GetAnalogPinStateAsync(0)}");
Console.WriteLine($"Limited Switch Triggered? : {await uArm.CheckLimitedSwitchTriggeredAsync()}");
// "Check" commands, classified in checking
Console.WriteLine($"Robot is Moving? : {await uArm.CheckIsMovingAsync()}");
Console.Write("Motor Attached? ");
Console.Write($"M0: {await uArm.CheckMotorAttachedAsync(Servos.Bottom)}, ");
Console.Write($"M1: {await uArm.CheckMotorAttachedAsync(Servos.Left)}, ");
Console.Write($"M2: {await uArm.CheckMotorAttachedAsync(Servos.Right)}, ");
Console.WriteLine($"M3: {await uArm.CheckMotorAttachedAsync(Servos.Hand)}");
}