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); } } }