public IMU440(string port, int baudrate, log loger, bool debug = false)
{
lg = loger;
nav = new Serial(port, baudrate);
this.debug = debug;
nav.PackageMode = Serial.PackageModes.UseFunction;
nav.EscapeToken = new byte[0];
nav.FindPackageEnd = (bytes =>
{
int offset = 0;
for (int i = 0; i < bytes.Length - 1; i++)
{
if (bytes[i] == 0x55 && bytes[i + 1] == 0x55)
{
offset = i;
}
}
if (bytes.Length >= 5 && bytes[offset] == 0x55 && bytes[offset + 1] == 0x55)
{
if (bytes.Length > offset + 4 &&
bytes.Length >= (offset + 7 + bytes[offset + 4]))
{
return(offset + 7 + bytes[offset + 4]);
}
}
return(-1);
});
nav.PackageReceived = navCallback;
nav.Start();
}
Serial fpga = new Serial("COM7", 9600); // use 9600 for FPGA, use 57600
public Form1()
{
InitializeComponent();
//construct fpga
fpga.PackageMode = Serial.PackageModes.UseFPGA; // for FPGA
//define callback
fpga.PackageReceived = (bytes =>
{
Thread.CurrentThread.Priority = ThreadPriority.AboveNormal;
Console.WriteLine("package length: {0}", bytes.Length);
for (int i = 0; i < bytes.Length; i++)
{
Console.WriteLine("byte {0}: {1}", i, (char)bytes[i]);
}
Console.WriteLine("\n");
});
//start
fpga.Start();
// set timer event to start reading and updating from the controller
timer1.Start();
}
Serial fpga = new Serial("COM10", 9600); // use 9600 for FPGA, use 57600
public Form1()
{
InitializeComponent();
//construct fpga
fpga.PackageMode = Serial.PackageModes.UseFPGA; // for FPGA
//define callback
fpga.PackageReceived = (bytes =>
{
Thread.CurrentThread.Priority = ThreadPriority.AboveNormal;
Console.WriteLine("package length: {0}", bytes.Length);
for (int i = 0; i < bytes.Length; i++)
{
Console.WriteLine("byte {0}: {1}", i, (char)bytes[i]);
}
switch (bytes[1])
{
case 0x4B:
int temp = 0;
int MSB = bytes[3] - 0x30;
int SB = bytes[4] - 0x30;
int LSB = bytes[5] - 0x30;
temp += LSB;
temp += SB * 10;
temp += MSB * 100;
LidarDistance = temp;
PackageRecieved = true;
break;
case 0x49:
LSB = bytes[5] - 0x30;
if (LSB == 1)
{
RightEncoder += 1;
}
else
{
RightEncoder -= 1;
}
break;
case 0x4C:
LSB = bytes[5] - 0x30;
if (LSB == 1)
{
LeftEnconder += 1;
}
else
{
LeftEnconder -= 1;
}
break;
}
Console.WriteLine("\n");
});
//start
udp_camera = new UdpClientSocket(
System.Net.IPAddress.Parse("127.0.0.1"), 8008);
//define call back
udp_camera.PackageReceived = (bytes =>
{
xDir = BitConverter.ToInt32(bytes, 0);
yDir = BitConverter.ToInt32(bytes, sizeof(int));
//y_mainpayload = BitConverter.ToInt32(bytes, (sizeof(int) + sizeof(bool)));
if (track)
{
GimbalTracking(xDir, yDir);
}
});
udp_camera.Start();
fpga.Start();
// set timer event to start reading and updating from the controller
timer1.Start();
}
