public static void Main(string[] args)
{
if (args[1] == "--LSF")
{
isLSF = true;
}
else if (args[1] == "--ABA")
{
isABA = true;
targetHeight = double.Parse(args[2]);
}
else
{
Console.WriteLine("Unknown assistance method: {0}.", args[1]);
return;
}
ConnectBufferBCI(args[0]);
running = true;
while (running)
{
var sec = bci_client.WaitForEvents(lastEvent, 5000);
if (sec.NumEvents > lastEvent)
{
var events = bci_client.GetEvents(lastEvent, sec.NumEvents - 1);
lastEvent = sec.NumEvents;
foreach (var evt in events)
{
if (isLSF && evt.Type.ToString() == "classifier.prediction")
{
ApplyLSF(evt.Value.ToString());
}
else if (isABA)
{
if (evt.Type.ToString() == "classifier.prediction")
{
ApplyABA(evt.Value.ToString());
}
else if (evt.Type.ToString() == "drone.altitude")
{
UpdateABA(evt.Value.ToString());
}
}
}
}
}
}
/// <summary>
/// Handles receiving data from blackboard.
/// </summary>
private void RecvThread()
{
while (true)
{
var sec = clock.WaitForEvents(lastEvent, 5000);
if (sec.NumEvents > lastEvent)
{
BufferEvent[] events = clock.GetEvents(lastEvent, sec.NumEvents - 1);
lastEvent = sec.NumEvents;
foreach (var evt in events)
{
string evttype = evt.Type.ToString();
if (evttype == "classifier.prediction")
{
try
{
double val = double.Parse(evt.Value.ToString());
Console.WriteLine(">> " + val);
if (val > 0)
{
OnCommandReceived(new Vector2D(0, 0.2));
}
else
{
OnCommandReceived(new Vector2D(0, -0.2));
}
}
catch (Exception)
{
Console.WriteLine("Couldn't convert '{0}' to double.", evt.Value);
}
}
else
{
Console.WriteLine(" " + evt);
}
}
}
else
{
Console.WriteLine("Timeout while waiting for events.");
}
}
}
static void FlyDroneBuffer(string heightEvtType)
{
ConnectBufferBCI();
bool flying = false;
bool running = true;
float targetHeight = 1, targetVelocity = 2;
float maxHeight = 3, minHeight = 0.1f;
float gaz, roll, pitch, yaw;
gaz = roll = pitch = yaw = 0;
// Thread to read events from the buffer.
new Thread(
() =>
{
while (running)
{
var sec = bci_client.WaitForEvents(lastEvent, 5000);
if (sec.NumEvents > lastEvent)
{
var events = bci_client.GetEvents(lastEvent, sec.NumEvents - 1);
lastEvent = sec.NumEvents;
foreach (var evt in events)
{
string evttype = evt.Type.ToString();
Console.WriteLine("{0}: {1}", evttype, evt.Value);
// BCI event type.
if (evttype == heightEvtType)
{
var val = double.Parse(evt.Value.ToString());
if (val > 0 && targetHeight < maxHeight)
{
targetHeight += 0.1f;
}
else if (val < 0 && targetHeight < minHeight)
{
targetHeight -= 0.1f;
}
}
// Joystick
else if (evttype == "joystick")
{
var raw_val = evt.Value.ToString();
if (raw_val == "Button0")
{
if (flying)
{
drone.Land();
}
else
{
drone.Takeoff();
}
flying = !flying;
}
}
// exit
else if (evttype == "exit")
{
running = false;
}
}
}
}
}
).Start();
// Event handler (probably called from another thread) to receive navigation data from the drone.
drone.NavigationDataAcquired +=
navData =>
{
// Correction for height.
if (navData.Altitude < targetHeight - 0.1)
{
gaz = Math.Min(targetHeight - drone.NavigationData.Altitude, 1.0f);
}
else if (navData.Altitude > targetHeight + 0.1)
{
gaz = -Math.Min(drone.NavigationData.Altitude - targetHeight, 1.0f);
}
else
{
gaz = 0;
}
// Correction for velocity.
if (navData.Velocity.X > targetVelocity + 0.5)
{
pitch = navData.Pitch - 0.025f;
}
else if (navData.Velocity.X < targetHeight - 0.5)
{
pitch = navData.Pitch + 0.025f;
}
else
{
pitch = navData.Pitch;
}
};
// Infinite loop to send data to the drone.
while (running)
{
if (flying)
{
drone.Progress(FlightMode.Hover, roll, pitch, yaw, gaz);
}
Thread.Sleep(250);
}
// Land when done, if required.
if (flying)
{
drone.Land();
}
}
