Example #1
0
        private static void TestPS4Controller(ICrazyradioDriver crazyradioDriver)
        {
            if (crazyradioDriver != null)
            {
                var crazyRadioMessenger = new CrazyflieMessenger(crazyradioDriver);

                var stopMotorsCommanderPacket = new CommanderPacket(roll: 0, pitch: 0, yaw: 0, thrust: 0);

                try
                {
                    // Init
                    float  roll   = 0;
                    float  pitch  = 0;
                    float  yaw    = 0;
                    ushort thrust = 0;

                    // Max/min values
                    float  rollRange   = 50;
                    float  pitchRange  = 50;
                    float  yawRange    = 100;
                    ushort thrustRange = 50000;

                    // Stick ranges
                    int stickRange = 1000;

                    // Get first attached game controller found
                    var directInput = new DirectInput();
                    var attahcedGameControllerDevices = directInput.GetDevices(DeviceClass.GameController, DeviceEnumerationFlags.AttachedOnly);
                    if (!attahcedGameControllerDevices.Any())
                    {
                        throw new ApplicationException("No available game controllers found.");
                    }
                    var attachedDeviceInstance = attahcedGameControllerDevices.First();
                    var joystick = new Joystick(directInput, attachedDeviceInstance.InstanceGuid);

                    foreach (DeviceObjectInstance doi in joystick.GetObjects(ObjectDeviceType.Axis))
                    {
                        joystick.GetObjectPropertiesById((int)doi.ObjectType).SetRange(-1 * stickRange, stickRange);
                    }

                    joystick.Properties.AxisMode = DeviceAxisMode.Absolute;
                    joystick.Acquire();
                    var joystickState = new JoystickState();

                    var loop = true;
                    while (loop)
                    {
                        if (Console.KeyAvailable)
                        {
                            switch (Console.ReadKey().Key)
                            {
                            // end
                            case ConsoleKey.Escape:
                                loop = false;
                                break;

                            // pause
                            case ConsoleKey.Spacebar:
                                Log.InfoFormat("Paused...Hit SPACE to resume, ESC to quit.");

                                thrust = 0;
                                pitch  = 0;
                                yaw    = 0;
                                roll   = 0;
                                crazyRadioMessenger.SendMessage(stopMotorsCommanderPacket);

                                var pauseLoop = true;
                                while (pauseLoop)
                                {
                                    if (Console.KeyAvailable)
                                    {
                                        switch (Console.ReadKey().Key)
                                        {
                                        // resume
                                        case ConsoleKey.Spacebar:
                                            pauseLoop = false;
                                            break;

                                        // end
                                        case ConsoleKey.Escape:
                                            pauseLoop = loop = false;
                                            break;
                                        }
                                    }
                                }
                                break;

                            default:
                                Log.InfoFormat("Invalid key for action.");
                                break;
                            }
                        }

                        // Poll the device and get state
                        joystick.Poll();
                        joystick.GetCurrentState(ref joystickState);

                        // Get buttons pressed info
                        var stringWriter      = new StringWriter();
                        var buttons           = joystickState.GetButtons();
                        var anyButtonsPressed = buttons.Any(b => b == true);
                        if (anyButtonsPressed)
                        {
                            for (int buttonNumber = 0; buttonNumber < buttons.Length; buttonNumber++)
                            {
                                if (buttons[buttonNumber] == true)
                                {
                                    stringWriter.Write(string.Format("{0}", buttonNumber));
                                }
                            }
                        }
                        var buttonsPressedString = stringWriter.ToString().Trim();

                        // Joystick info
                        var leftStickX  = joystickState.X;
                        var leftStickY  = joystickState.Y;
                        var rightStickX = joystickState.RotationX;
                        var rightStickY = joystickState.RotationY;

                        roll   = rollRange * rightStickX / stickRange;
                        pitch  = pitchRange * rightStickY / stickRange;
                        yaw    = yawRange * leftStickX / stickRange;
                        thrust = (ushort)(leftStickY > 0 ? 0 : thrustRange * -1 * leftStickY / stickRange);

                        var infoString = String.Format("LX:{0,7}, LY:{1,7}, RX:{2,7}, RY:{3,7}, Buttons:{4,7}.\tRoll:{5, 7}, Pitch:{6, 7}, Yaw:{7, 7}, Thrust:{8, 7}.", leftStickX, leftStickY, rightStickX, rightStickY, buttonsPressedString, roll, pitch, yaw, thrust);
                        Console.WriteLine(infoString);

                        var commanderPacket = new CommanderPacket(roll, pitch, yaw, thrust);
                        crazyRadioMessenger.SendMessage(commanderPacket);
                    }
                }
                catch (Exception)
                {
                    try
                    {
                        crazyRadioMessenger.SendMessage(CommanderPacket.ZeroAll);
                    }
                    catch (Exception)
                    {
                    }

                    throw;
                }

                crazyRadioMessenger.SendMessage(CommanderPacket.ZeroAll);
            }
        }
Example #2
0
        private static void TestCRTP(ICrazyradioDriver crazyradioDriver)
        {
            if (crazyradioDriver != null)
            {
                var crazyRadioMessenger = new CrazyflieMessenger(crazyradioDriver);

                try
                {
                    IAckPacket ackPacket = null;

                    Log.InfoFormat("Ping Packet Request: {0}", PingPacket.Instance);
                    ackPacket = crazyRadioMessenger.SendMessage(PingPacket.Instance);
                    Log.InfoFormat("ACK Response: {0}", ackPacket);


                    ushort thrustIncrements = 1000;
                    float  pitchIncrements  = 5;
                    float  yawIncrements    = 2;
                    float  rollIncrements   = 5;
                    ushort thrust           = 10000;
                    float  pitch            = 0;
                    float  yaw  = 0;
                    float  roll = 0;

                    var loop = true;
                    while (loop)
                    {
                        Log.InfoFormat("Thrust: {0}, Pitch: {1}, Roll: {2}, Yaw: {3}.", thrust, pitch, roll, yaw);

                        if (Console.KeyAvailable)
                        {
                            switch (Console.ReadKey().Key)
                            {
                            // end
                            case ConsoleKey.Escape:
                                loop = false;
                                break;

                            // pause
                            case ConsoleKey.Spacebar:
                                var commanderPacket = new CommanderPacket(roll, pitch, yaw, thrust = 10000);
                                Log.InfoFormat("Commander Packet Request: {0}", commanderPacket);
                                ackPacket = crazyRadioMessenger.SendMessage(CommanderPacket.ZeroAll);
                                ackPacket = crazyRadioMessenger.SendMessage(commanderPacket);
                                Log.InfoFormat("ACK Response: {0}", ackPacket);

                                Log.InfoFormat("Paused...Hit SPACE to resume, ESC to quit.");

                                var pauseLoop = true;
                                while (pauseLoop)
                                {
                                    if (Console.KeyAvailable)
                                    {
                                        switch (Console.ReadKey().Key)
                                        {
                                        // resume
                                        case ConsoleKey.Spacebar:
                                            pauseLoop = false;
                                            break;

                                        // end
                                        case ConsoleKey.Escape:
                                            pauseLoop = loop = false;
                                            break;
                                        }
                                    }
                                }
                                break;

                            // thrust up
                            case ConsoleKey.UpArrow:
                                thrust += thrustIncrements;
                                break;

                            // thrust down
                            case ConsoleKey.DownArrow:
                                thrust -= thrustIncrements;
                                break;

                            // yaw right
                            case ConsoleKey.RightArrow:
                                yaw += yawIncrements;
                                break;

                            // yaw left
                            case ConsoleKey.LeftArrow:
                                yaw -= yawIncrements;
                                break;

                            // pitch backward
                            case ConsoleKey.S:
                                pitch += pitchIncrements;
                                break;

                            // pitch forward
                            case ConsoleKey.W:
                                pitch -= pitchIncrements;
                                break;

                            // roll right
                            case ConsoleKey.D:
                                roll += rollIncrements;
                                break;

                            // roll left
                            case ConsoleKey.A:
                                roll -= rollIncrements;
                                break;

                            default:
                                Log.InfoFormat("Invalid key for action.");
                                break;
                            }
                        }

                        {
                            var commanderPacket = new CommanderPacket(roll, pitch, yaw, thrust);
                            Log.InfoFormat("Commander Packet Request: {0}", commanderPacket);
                            ackPacket = crazyRadioMessenger.SendMessage(commanderPacket);
                            Log.InfoFormat("ACK Response: {0}", ackPacket);
                        }
                    }
                }
                catch (Exception)
                {
                    try
                    {
                        crazyRadioMessenger.SendMessage(CommanderPacket.ZeroAll);
                    }
                    catch (Exception)
                    {
                    }

                    throw;
                }

                crazyRadioMessenger.SendMessage(CommanderPacket.ZeroAll);
            }
        }