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