public TeensyInterface(string portName, int baudRate)
{
this.serialPort = new SerialPort();
this.serialPort.PortName = portName;
this.serialPort.BaudRate = baudRate;
this.serialPort.Parity = Parity.None;
this.serialPort.DataBits = 8;
this.serialPort.StopBits = StopBits.One;
this.serialPort.Handshake = Handshake.None;
this.serialPort.WriteTimeout = 500;
this.currentParsingCommand = null;
this.incomingBytes = new byte[10];
this.nextIncomingByteIndex = 0;
this.commands = new Dictionary <byte, InCommand>();
this.commands.Add(0x00, new UpdateServoPosition(this));
this.commands.Add(0x01, new ReceiveJoystickPosition(this));
this.commands.Add(0x02, new UpdateLoadCellValue(this));
this.commands.Add(0x03, new ShutdownCommand(this));
this.commands.Add(0x04, new ModusCommand(this));
this.commands.Add(0x05, new DCMotorsCommand(this));
this.commands.Add(0x06, new ServoCommand(this));
this.commands.Add(0x07, new RemoteTimeout(this));
}
private void OnDataReceived(object sender, SerialDataReceivedEventArgs e)
{
if (resetting)
{
while (this.serialPort.BytesToRead > 0)
{
var data = (byte)this.serialPort.ReadByte();
if (data == 0xFF)
{
ServiceLocator.Get <ILogger>().LogDebug("Received handshake");
var handshake = new byte[] { 0xFF };
this.WriteBytes(handshake);
break;
}
else
{
ServiceLocator.Get <ILogger>().LogDebug("Incorrect handshake");
}
}
this.resetting = false;
return;
}
while (this.serialPort.BytesToRead > 0)
{
var data = (byte)this.serialPort.ReadByte();
if (this.currentParsingCommand == null)
{
if (!this.commands.TryGetValue(data, out var command))
{
// throw new Exception($"Command '{data}' not implemented");
continue;
}
else
{
this.currentParsingCommand = command;
}
}
else
{
this.incomingBytes[this.nextIncomingByteIndex] = data;
this.nextIncomingByteIndex++;
}
if (this.nextIncomingByteIndex == this.currentParsingCommand.GetRequiredBytes())
{
this.currentParsingCommand.Execute(this.incomingBytes);
this.currentParsingCommand = null;
this.nextIncomingByteIndex = 0;
}
}
}
