private void Worker()
{
try
{
while (true)
{
var commands = _commandQueue.Take(_cancellationToken);
var grSimCommands = commands.Select((c, i) => new grSim_Robot_Command
{
Id = (uint)i,
Velnormal = (float)c.VelocityNormal,
Veltangent = (float)c.VelocityTangent,
Velangular = (float)c.AngularVelocity,
Kickspeedx = (float)c.KickSpeedX,
Kickspeedz = (float)c.KickSpeedZ,
Spinner = c.EnableDribbler,
Wheelsspeed = false,
});
var grSimCommandsPacket = new grSim_Commands
{
Isteamyellow = _team == RobocupTeam.Yellow,
Timestamp = ((DateTimeOffset)DateTime.UtcNow).ToUnixTimeSeconds(),
RobotCommands = { grSimCommands }
};
var grSimPacket = new grSim_Packet
{
Commands = grSimCommandsPacket
};
var cos = new CodedOutputStream(_buffer);
grSimPacket.WriteTo(cos);
var message = new ArraySegment <byte>(_buffer, 0, (int)cos.Position);
// ReSharper disable once MethodSupportsCancellation
_socket.SendAsync(message, SocketFlags.None, _cancellationToken).AsTask().GetAwaiter().GetResult();
}
}
catch (OperationCanceledException)
{
}
}
public static void SendData(bool isYellow, int id, float w1, float w2, float w3, float w4, float kickSpeedX,
float kickSpeedZ)
{
if (_controller.SerialChecked())
{
var cmdPacket = new byte[10];
var btemp = BitConverter.GetBytes((int) Math.Abs(w1));
cmdPacket[0] = btemp[0];
btemp = BitConverter.GetBytes((int) Math.Abs(w2));
cmdPacket[1] = btemp[0];
btemp = BitConverter.GetBytes((int) Math.Abs(w3));
cmdPacket[2] = btemp[0];
btemp = BitConverter.GetBytes((int) Math.Abs(w4));
cmdPacket[3] = btemp[0];
var sign = Math.Sign(w1);
switch (sign)
{
case 1:
cmdPacket[4] = 1;
break;
case -1:
cmdPacket[4] = 0;
break;
}
sign = Math.Sign(w2);
switch (sign)
{
case 1:
cmdPacket[5] = 1;
break;
case -1:
cmdPacket[5] = 0;
break;
}
sign = Math.Sign(w3);
switch (sign)
{
case 1:
cmdPacket[6] = 1;
break;
case -1:
cmdPacket[6] = 0;
break;
}
sign = Math.Sign(w4);
switch (sign)
{
case 1:
cmdPacket[7] = 1;
break;
case -1:
cmdPacket[7] = 0;
break;
}
btemp = BitConverter.GetBytes((int) Math.Abs(kickSpeedX));
cmdPacket[8] = btemp[0];
btemp = BitConverter.GetBytes((int) Math.Abs(kickSpeedZ));
cmdPacket[9] = btemp[0];
_controller.Send(cmdPacket);
}
else
{
const float zero = (float) 0.000000000001;
var pkt = new grSim_Packet {commands = new grSim_Commands()};
var robotCmd = new grSim_Robot_Command();
pkt.commands.isteamyellow = isYellow;
pkt.commands.timestamp = 0;
robotCmd.id = (uint) id;
robotCmd.wheelsspeed = true;
robotCmd.velnormal = 0;
robotCmd.veltangent = 0;
robotCmd.velangular = 0;
robotCmd.kickspeedx = kickSpeedX;
robotCmd.kickspeedz = kickSpeedZ;
robotCmd.spinner = Spinner;
robotCmd.wheel1 = (Math.Abs(w1) < 0.00001) ? zero : w1;
robotCmd.wheel2 = (Math.Abs(w2) < 0.00001) ? zero : w2;
robotCmd.wheel3 = (Math.Abs(w3) < 0.00001) ? zero : w3;
robotCmd.wheel4 = (Math.Abs(w4) < 0.00001) ? zero : w4;
pkt.commands.robot_commands.Add(robotCmd);
var stream = new MemoryStream();
Serializer.Serialize(stream, pkt);
var array = stream.ToArray();
try
{
Client.Send(array, array.Length, new IPEndPoint(IPAddress.Broadcast, 20011));
}
catch (Exception ex)
{
_controller.PrintErrorMessage(ex);
}
}
}
