public void UdpTest()
{
object monitor = new object();
lock (monitor) {
int payloadCounter = 0;
Console.WriteLine("Starting server");
UdpServerSocket server = null;
server = new UdpServerSocket((sender, buffer, size) => {
lock (monitor) {
Console.WriteLine("SRec " + size);
server.Send(sender, buffer, 0, size);
}
}, Port);
byte[] sendBuffer = new byte[DataSize];
_random.NextBytes(sendBuffer);
Console.WriteLine("Starting client");
UdpClientSocket client = null;
client = new UdpClientSocket((buffer, size) => {
lock (monitor) {
Console.WriteLine("CRec " + size);
AssertArrayContentsEqualInFirstArrayLengthRange(sendBuffer, buffer);
if (++payloadCounter == PayloadCount)
{
Monitor.Pulse(monitor);
}
else
{
_random.NextBytes(sendBuffer);
client.Send(sendBuffer, 0, sendBuffer.Length);
}
}
});
client.Start(Ip, Port);
Console.WriteLine("Client sending first data");
client.Send(sendBuffer, 0, sendBuffer.Length);
Console.WriteLine("Main thread waiting");
Monitor.Wait(monitor);
Console.WriteLine("Closing client");
client.Close();
Console.WriteLine("Closing server");
server.Close();
}
}
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();
}
public Form1()
{
InitializeComponent();
//construct fpga
fpga.PackageMode = Serial.PackageModes.UseFPGA; // for FPGA
tempfpga.PackageMode = Serial.PackageModes.UseFPGA;
//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_CameraMode = new UdpClientSocket(
* System.Net.IPAddress.Parse("127.0.0.1"), 6800);
* udp_CameraMode.Start();*/
udp_bottle = new UdpClientSocket(
System.Net.IPAddress.Parse("127.0.0.1"), 6789);
//define call back
udp_bottle.PackageReceived = (bytes =>
{
BottleData = BitConverter.ToInt32(bytes, 0);
BottleX = BitConverter.ToInt32(bytes, sizeof(int));
BottleY = BitConverter.ToInt32(bytes, 2 * sizeof(int));
if (track)
{
GimbalTracking(BottleX, BottleY);
}
});
udp_bottle.Start();
udp_ball = new UdpClientSocket(
System.Net.IPAddress.Parse("127.0.0.1"), 6790);
udp_ball.PackageReceived = (bytes =>
{
BallX = BitConverter.ToInt32(bytes, 0);
BallY = BitConverter.ToInt32(bytes, sizeof(int));
});
udp_ball.Start();
///DYNAMIXEL CODE
dynamixel.packetHandler();
dynamixel.openPort(port_num);
dynamixel.setBaudRate(port_num, BAUDRATE);
//ENABLE TORQUE
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, GIMBALPITCH, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, GIMBALYAW, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
// dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, TURRENT, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, FRONTWHEEL, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, BACKWHEEL, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE);
//SET START POS
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, GIMBALPITCH, ADDR_MX_GOAL_POSITION, GIMBALPITCHSTART);
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, GIMBALYAW, ADDR_MX_GOAL_POSITION, GIMBALYAWSTART);
// dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, TURRENT, ADDR_MX_GOAL_POSITION, TURRENTSTART);
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, FRONTWHEEL, ADDR_MX_GOAL_POSITION, FRONTWHEELSTART);
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, BACKWHEEL, ADDR_MX_GOAL_POSITION, BACKWHEELSTART);
///DYNAMIXEL CODE
//tempfpga.Start();
//UNCOMMENT WHEN TESTING FPGA
//fpga.Start();
// set timer event to start reading and updating from the controller
timer1.Start();
}
