void Update()
{
if (DateTime.Now - lastFeedback > TimeSpan.FromMilliseconds(20))
{
lastFeedback = DateTime.Now;
int[] values = new int[64];
long timestamp = (long)(DateTime.UtcNow - UnixEpoch).TotalMilliseconds;
values[TIMESTAMP] = (int)timestamp;
values[LEFT_ENCODER] = robot.LeftEncoder;
values[RIGHT_ENCODER] = robot.RightEncoder;
values[HEADING] = (int)robot.Gyro;
values[INTAKE_STATE] = robot.GripperState ? 1 : -1;
values[BALL_PRESENCE] = robot.BallPresence ? 1 : 0;
values[ROBOT_MODE] = Time.timeSinceLevelLoad < 15? Auton : Teleop;
for (var j = 0; j < 2; ++j)
{
for (var a = 0; a < AxisPerJoystick; ++a)
{
values[j * AxisPerJoystick + a + JoystickStart] = (int)(Input.GetAxis("j" + j + "a" + a) * 100);
}
for (var b = 0; b < ButtonsPerJoystick; b++)
{
values[j * ButtonsPerJoystick + b + JoystickButtonStart] = (int)(Input.GetKey("joystick " + (j + 1) + " button " + b) ? 1 : 0);
}
}
byte[] sendBytes = new byte[values.Length * sizeof(int)];
Buffer.BlockCopy(values, 0, sendBytes, 0, sendBytes.Length);
udpClient.Send(sendBytes, sendBytes.Length);
}
if (udpClient.Available > 0)
{
IPEndPoint e = new IPEndPoint(IPAddress.Any, commandsPort);
Byte[] receiveBytes = udpClient.Receive(ref e);
if (receiveBytes.Length % sizeof(int) == 0)
{
int[] commands = new int[receiveBytes.Length / sizeof(int)];
Buffer.BlockCopy(receiveBytes, 0, commands, 0, receiveBytes.Length);
if (commands.Length >= 14)
{
/*if (commands[RESET_SIM] > 0) {
* SceneManager.LoadScene(SceneManager.GetActiveScene().buildIndex);
* Debug.Log("Reset");
* commands[RESET_SIM] = 0;
* }*/
teleop.enabled = false;
robot.SetMotors(commands[LEFT_MOTOR] / 512.0f, commands[RIGHT_MOTOR] / 512.0f, commands[CENTER_MOTOR] / 512.0f);
robot.SetGripper(commands[INTAKE] >= 0);
if (commands[LAUNCH] >= 256)
{
robot.Launch();
}
}
}
lastCommand = DateTime.Now;
}
else if (DateTime.Now - lastCommand > TimeSpan.FromSeconds(1))
{
teleop.enabled = true;
}
}