public static void Disconnect()
{
if (serial == null)
throw new Exception("Not connected");
Log.Info("Disconnecting robot serial");
ControllerInput.StopAlive(); //may not actually get through in time
serial.Close();
serial = null;
}
private static ControllerPacket ReadPacket(DiffBotSerial serial, bool warn)
{
while (true) {
if (serial.ReadByte() != 0xAA || serial.ReadByte() != 0x55) continue;
var p = new ControllerPacket();
serial.ReadBytes(p.bytes);
if (p.CRC()) return p;
if (warn) Log.Warn("CRC failed");
}
}
public static void Connect(string portName)
{
if (serial != null)
throw new Exception("Already connected");
Log.Info("Connecting robot serial on "+portName);
try {
packetQueue = new BlockingCollection<byte[]>();
serial = new DiffBotSerial(new SerialPort(portName, 57600, Parity.None, 8, StopBits.One));
serial.AddThread(WriteThread);
serial.AddThread(HeartbeatThread);
serial.Open();
} catch (Exception e) {
Log.Error("Exception connecting controller serial on "+portName, e);
}
}
public static void ClosePort(DiffBotSerial serial)
{
lock (serials) {
DiffBotSerial stored;
if(serials.TryGetValue(serial.port.PortName, out stored) && stored == serial) {
serials.Remove(serial.port.PortName);
ControllerPortForm.Instance.Remove(serial.port.PortName);
}
}
try {
serial.Close();
Log.Info("Closed controller port " + serial.port.PortName);
}
catch (Exception e) {
Log.Error("Exception closing controller port "+serial.port.PortName, e);
}
}
private static void HeartbeatThread(DiffBotSerial serial)
{
try {
var ct = serial.CancellationToken();
var stopwatch = new Stopwatch();
ControllerInput.ResetAlive();
while (!ct.IsCancellationRequested) {
stopwatch.Restart();
ControllerInput.SendKeepAlive();
var wait = heartbeatInterval - stopwatch.Elapsed;
if (wait > TimeSpan.Zero)
ct.WaitHandle.WaitOne(wait);
}
} catch (Exception e) {
Log.Error("Exception in Heartbeat Thread", e);
}
}
private static void ReadThread(DiffBotSerial serial)
{
try {
while (true) ControllerInput.Handle(ReadPacket(serial, true));
}
catch (OperationCanceledException) {}
catch (InvalidOperationException e) {
Log.Error(serial.port.PortName + " Read Thread: " + e.Message);
} catch (IOException e) {
Log.Error(serial.port.PortName + " Read Thread: " + e.Message);
} catch (AccessViolationException e) {
Log.Error(serial.port.PortName + " Read Thread: " + e.Message);
} catch (Exception e) {
Log.Error(serial.port.PortName + " Read Thread", e);
} finally {
Task.Run(() => ClosePort(serial));
}
}
private static void _AddPort(DiffBotSerial serial)
{
lock (serials) {
ClosePort(serial.port.PortName);
serials[serial.port.PortName] = serial;
ControllerPortForm.Instance.Add(serial.port.PortName);
}
Log.Info("Opened controller port " + serial.port.PortName);
}
private static void WriteThread(DiffBotSerial serial)
{
try {
var ct = serial.CancellationToken();
while (true) serial.SendPacket(packetQueue.Take(ct));
}
catch (OperationCanceledException) {}
catch (Exception e) {
Log.Error("Exception in Write Thread", e);
}
}
