Exemple #1
0
        //Test & Example to call setter commands, except robot's motion
        // CAUTION: some operation includes output
        // - Beep (outputs some sound)
        // - Motor ON/OFF
        // - Suction cup ON/OFF
        // - Gripper ON/OFF
        // - Digital Out 0 ON/OFF
        static async Task TestNoMotionCommands(IUArm uArm)
        {
            await uArm.BeepAsync(440, 50);

            Console.WriteLine("Detach All Motors");
            await uArm.DetachAllMotorAsync();

            await Task.Delay(500);

            Console.WriteLine("Attach All Motors");
            await uArm.AttachAllMotorAsync();

            await Task.Delay(500);

            Console.WriteLine("Detach All Motors");
            await uArm.DetachMotorAsync(Servos.Bottom);

            await uArm.DetachMotorAsync(Servos.Left);

            await uArm.DetachMotorAsync(Servos.Right);

            await Task.Delay(500);

            Console.WriteLine("Attach All Motors");
            await uArm.AttachMotorAsync(Servos.Bottom);

            await uArm.AttachMotorAsync(Servos.Left);

            await uArm.AttachMotorAsync(Servos.Right);

            await Task.Delay(500);

            // Pos -> Angles by IK -> Pos by FK (Math)
            var pos    = new Position(100, 100, 100);
            var angles = await uArm.GetIKAsync(pos);

            Console.WriteLine($"IK, src: X={pos.X}, Y={pos.Y}, Z={pos.Z}");
            Console.WriteLine($"IK, res: L={angles.Bottom}, B={angles.Left}, R={angles.Right}");

            pos = await uArm.GetFKAsync(angles);

            Console.WriteLine($"FK, src: L={angles.Bottom}, B={angles.Left}, R={angles.Right}");
            Console.WriteLine($"FK, res: X={pos.X}, Y={pos.Y}, Z={pos.Z}");

            // Can Reach or not
            pos = new Position(100, 100, 100);
            Console.WriteLine($"Can reach to (x,y,z)=({pos.X}, {pos.Y}, {pos.Z})? : {await uArm.CanReachAsync(pos)}");
            pos = new Position(300, 300, 100);
            Console.WriteLine($"Can reach to (x,y,z)=({pos.X}, {pos.Y}, {pos.Z})? : {await uArm.CanReachAsync(pos)}");

            var polar = new Polar(100, 45, 100);

            Console.WriteLine($"Can reach to (s,r,h)=({polar.Stretch}, {polar.Rotation}, {polar.Height})? : {await uArm.CanReachAsync(polar)}");
            polar = new Polar(400, 200, 100);
            Console.WriteLine($"Can reach to (s,r,h)=({polar.Stretch}, {polar.Rotation}, {polar.Height})? : {await uArm.CanReachAsync(polar)}");

            Console.WriteLine("Activate and deactivate the pump");
            await Task.Delay(1000);

            await uArm.SetPumpStateAsync(true);

            await Task.Delay(1000);

            await uArm.SetPumpStateAsync(false);

            Console.WriteLine("Activate and deactivate the gripper");
            await Task.Delay(1000);

            await uArm.SetGripperStateAsync(true);

            await Task.Delay(1000);

            await uArm.SetGripperStateAsync(false);

            // NO bluetooth operation, because when using this, serial connection will be shut down!
            //Console.WriteLine("Activate and deactivate the bluetooth");
            //await Task.Delay(1000);
            //await uArm.SetBluetoothStateAsync(true);
            //await Task.Delay(1000);
            //await uArm.SetBluetoothStateAsync(false);

            Console.WriteLine("Change digital Out 0 to high, and low");
            await Task.Delay(1000);

            await uArm.SetDigitalPinOutputAsync(0, true);

            await Task.Delay(1000);

            await uArm.SetDigitalPinOutputAsync(0, false);

            Console.WriteLine("Change ArmMode to Normal, Laser, Printing, Universal Holder, and then Normal again");
            await Task.Delay(500);

            await uArm.SetArmModeAsync(ArmModes.Normal);

            await Task.Delay(500);

            await uArm.SetArmModeAsync(ArmModes.Laser);

            await Task.Delay(500);

            await uArm.SetArmModeAsync(ArmModes.Printing);

            await Task.Delay(500);

            await uArm.SetArmModeAsync(ArmModes.UniversalHolder);

            await Task.Delay(500);

            await uArm.SetArmModeAsync(ArmModes.Normal);

            Console.WriteLine("Set Current Position to the reference position");
            try
            {
                await uArm.UpdateReferencePoint();
            }
            catch (UArmException ex)
            {
                Console.WriteLine("Expected error, message: " + ex.Message);
            }

            Console.WriteLine("Set Height zero Point");
            await uArm.UpdateHeightZeroPoint();

            Console.WriteLine("Set End Effector Height 100mm, and 0mm");
            await uArm.SetEndEffectorHeight(100);

            await uArm.SetEndEffectorHeight(0);
        }