예제 #1
0
        //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)}");
        }
예제 #2
0
        //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
        }