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);
}
}
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);
}
}