public InPort(CSerialPort serial_port, byte port, SensorType type = SensorType.Switch)
{
this.serial_port = serial_port;
this.port = port;
state = new byte[8];
Set(type, SensorMode.RawMode);
}
public OutPort(CSerialPort serial_port, MotorPort motor_port)
{
this.serial_port = serial_port;
this.motor_port = motor_port;
}
public Details(CSerialPort serial_port)
{
this.serial_port = serial_port;
}
public Direct_Commands(CSerialPort serial_port)
{
this.serial_port = serial_port;
}
public static bool Read(CSerialPort serial_port, byte port, SonarCommands command, ref byte[] reply)
{
if (!Read(serial_port, port, command))
return false;
return LSRead(serial_port, port, ref reply);
}
public static bool Read(CSerialPort serial_port, byte port, SonarCommands command)
{
byte[] data = { 0x02, (byte)command };
switch (command)
{
case SonarCommands.Version:
case SonarCommands.ProductID:
case SonarCommands.SensorType:
return LSWrite(serial_port, port, data, 8);
case SonarCommands.MeasurementUnits:
return LSWrite(serial_port, port, data, 7);
default:
return LSWrite(serial_port, port, data, 1);
}
}
public static bool LSRead(CSerialPort serial_port, byte port, ref byte[] rx_data)
{
byte[] message = new byte[3];
message[0] = 0x00;
message[1] = (byte)DirectCommand.LSRead;
message[2] = port;
byte[] reply = new byte[20];
if (!serial_port.Send(message, ref reply))
return false;
if (reply.Length != 20)
return false;
Array.Resize(ref rx_data, reply[3]);
Array.Copy(reply, 4, rx_data, 0, reply[3]);
return true;
}
public static bool LSWrite(CSerialPort serial_port, byte port, byte[] tx_data, byte rx_data_length)
{
byte[] message = new byte[tx_data.Length + 5];
message[0] = 0x00;
message[1] = (byte)DirectCommand.LSWrite;
message[2] = port;
message[3] = (byte)tx_data.Length;
message[4] = rx_data_length;
tx_data.CopyTo(message, 5);
return serial_port.Send(message);
}
public static byte LSGetStatus(CSerialPort serial_port, byte port)
{
byte[] message = new byte[3];
message[0] = 0x00;
message[1] = (byte)DirectCommand.LSGetStatus;
message[2] = port;
byte[] reply = new byte[4];
if (!serial_port.Send(message, ref reply))
return 0;
if (reply.Length != 4)
return 0;
return reply[3];
}
public static bool I2CSetByte(CSerialPort serial_port, byte port, byte address, byte data)
{
byte[] message = { 0x02, address, data };
return LSWrite(serial_port, port, message, 0);
}
public static bool I2CGetByte(CSerialPort serial_port, byte port, byte address, ref byte data)
{
byte[] message = { 0x02, address };
if (!LSWrite(serial_port, port, message, 1))
return false;
byte[] reply = { 0 };
LSRead(serial_port, port, ref reply);
foreach (byte value in reply)
Console.Write("{0:x2} ", value);
Console.WriteLine("");
return true;
}
public CDeviceInfo(CSerialPort serial_port)
{
device_info = new byte[30];
this.serial_port = serial_port;
}
/// <summary>
/// Will try and connect to a specific ComPort
/// </summary>
/// <param name="port_name"></param>
/// <returns></returns>
private bool Connect(string port_name)
{
serial_port = new CSerialPort();
DeviceInfo = new CDeviceInfo(serial_port);
InPort1 = new InPort(serial_port, 0, SensorType.Switch);
InPort2 = new InPort(serial_port, 1, SensorType.SoundDB);
InPort3 = new InPort(serial_port, 2, SensorType.LightActive);
InPort4 = new InPort(serial_port, 3, SensorType.Sonar);
OutPortA = new OutPort(serial_port, MotorPort.MotorA);
OutPortB = new OutPort(serial_port, MotorPort.MotorB);
OutPortC = new OutPort(serial_port, MotorPort.MotorC);
OutPortAll = new OutPort(serial_port, MotorPort.MotorAll);
comPort = port_name;
Info = new Details(serial_port);
DirectCommands = new Direct_Commands(serial_port);
if (!serial_port.Connect(port_name))
return false;
if (!serial_port.Send(DirectCommand.KeepAlive))
{
NXTDisconnect();
return false;
}
connected = true;
return true;
}