// Update is called once per frame
void Update()
{
float leftPower, rightPower, centerPower;
if (tankDrive)
{
leftPower = Input.GetAxis("P" + playerNumber + " Left");
rightPower = Input.GetAxis("P" + playerNumber + " Right");
centerPower = Input.GetAxis("P" + playerNumber + " Center");
}
else
{
float drive = Input.GetAxis("P" + playerNumber + " Vertical");
float steer = Input.GetAxis("P" + playerNumber + " Horizontal");
leftPower = Mathf.Clamp(drive + steer, -1, 1);
rightPower = Mathf.Clamp(drive - steer, -1, 1);
centerPower = Input.GetAxis("P" + playerNumber + " Center");
}
robot.SetMotors(leftPower, rightPower, centerPower);
float auxiliary = Input.GetAxis("P" + playerNumber + " auxiliary");
robot.SetAuxiliaryMotor(auxiliary);
float auxiliary2 = Input.GetAxis("P" + playerNumber + " auxiliary2");
robot.SetAuxiliary2Motor(auxiliary2);
float intake = Input.GetAxis("P" + playerNumber + " intake");
robot.SetIntake(intake);
float gripper = Input.GetAxis("P" + playerNumber + " Gripper");
if (gripper > 0)
{
robot.SetIntakeArm(true);
}
else if (gripper < 0)
{
robot.SetIntakeArm(false);
}
//if (Input.GetButton("P" + playerNumber + " Actuate"))
// robot.Actuate();
//TODO: robot.ShootPower = Mathf.Clamp01(robot.ShootPower + Input.GetAxis ("P" + playerNumber + " Shoot Power") * powerChangeSpeed * Time.deltaTime);
bool launchButton = Input.GetButton("P" + playerNumber + " Launch");
if (launchButton && launchIsPressed == false)
{
robot.Launch();
}
launchIsPressed = launchButton;
//if (Input.GetButton("P" + playerNumber + " Launch2"))
// robot.Launch2();
}
// Update is called once per frame
void Update()
{
float leftPower, rightPower, centerPower;
if (tankDrive)
{
leftPower = Input.GetAxis("P" + playerNumber + " Left");
rightPower = Input.GetAxis("P" + playerNumber + " Right");
centerPower = Input.GetAxis("P" + playerNumber + " Center");
}
else
{
float drive = Input.GetAxis("P" + playerNumber + " Vertical");
float steer = -1 * Input.GetAxis("P" + playerNumber + " Horizontal");
leftPower = Mathf.Clamp(drive + steer, -1, 1);
rightPower = Mathf.Clamp(drive - steer, -1, 1);
centerPower = Input.GetAxis("P" + playerNumber + " Center");
}
robot.SetMotors(leftPower, rightPower, centerPower);
float intake = Input.GetAxis("P" + playerNumber + " intake");
robot.SetIntake(intake);
float gripper = Input.GetAxis("P" + playerNumber + " Gripper");
//if (gripper > 0)
//robot.SetGripper(true);
//else if (gripper < 0)
//robot.SetGripper(false);
robot.SetIntakeArm(gripper);
if (Input.GetButton("P" + playerNumber + " Actuate"))
{
robot.Actuate();
}
//TODO: robot.ShootPower = Mathf.Clamp01(robot.ShootPower + Input.GetAxis ("P" + playerNumber + " Shoot Power") * powerChangeSpeed * Time.deltaTime);
if (Input.GetButton("P" + playerNumber + " Launch") && buttonIsPressed == false && robot.numBalls > 0)
{
robot.Launch();
//-1 ball
robot.numBalls--;
}
buttonIsPressed = Input.GetButton("P" + playerNumber + " Launch");
if (Input.GetButton("P" + playerNumber + " Launch2"))
{
robot.Launch2();
}
}
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;
}
}
void FixedUpdate()
{
if (DateTime.Now - lastFeedback > TimeSpan.FromMilliseconds(1))
{
lastFeedback = DateTime.Now;
int[] values = new int[100];
long timestamp = (long)(Time.time * 1000);
values[TIMESTAMP_LSW] = (int)timestamp;
values[TIMESTAMP_MSW] = (int)(timestamp >> 32);
values[ROBOT_X] = (int)(robot.transform.position.x * 1000);
values[ROBOT_Y] = (int)(robot.transform.position.z * 1000);
values[LEFT_ENCODER] = robot.LeftEncoder;
values[RIGHT_ENCODER] = robot.RightEncoder;
values[MECHANISM_ENCODER] = robot.MechanismEncoder;
values[HEADING] = (int)robot.GyroAngle;
values[HEADING_PRECISE] = (int)(robot.GyroAngle * 10);
values[HEADING_RATE] = (int)(robot.GyroRate * 100);
values[GYRO_PITCH] = (int)(robot.GyroPitch * 10);
values[GYRO_ROLL] = (int)(robot.GyroRoll * 10);
values[BALL_PRESENCE] = robot.BallPresence ? 1 : 0;
if (robot.lineSensor1 != null)
{
values[LINE_SENSOR_1] = robot.lineSensor1.IsDetecting ? 1 : 0;
}
if (robot.lineSensor2 != null)
{
values[LINE_SENSOR_2] = robot.lineSensor2.IsDetecting ? 1 : 0;
}
if (robot.lineSensor3 != null)
{
values[LINE_SENSOR_3] = robot.lineSensor3.IsDetecting ? 1 : 0;
}
switch (gameGui.RobotMode)
{
case RobotMode.Disabled:
values[ROBOT_MODE] = DISABLED_MODE;
break;
case RobotMode.Auton:
values[ROBOT_MODE] = AUTON_MODE;
break;
case RobotMode.Teleop:
values[ROBOT_MODE] = TELEOP_MODE;
break;
}
for (var j = 0; j < NumJoysticks; ++j)
{
for (var a = 0; a < AxesPerJoystick; ++a)
{
values[j * AxesPerJoystick + a + JoystickAxisStart] = (int)(oi.joysticks[j].axis[a] * 100);
}
for (var b = 0; b < ButtonsPerJoystick; b++)
{
values[j * ButtonsPerJoystick + b + JoystickButtonStart] = oi.joysticks[j].button[b] ? 1 : 0;
}
}
byte[] sendBytes = new byte[values.Length * sizeof(int)];
Buffer.BlockCopy(values, 0, sendBytes, 0, sendBytes.Length);
try {
udpClient.Send(sendBytes, sendBytes.Length);
} catch (SocketException ex) {
if (gameGui.haveRobotCode || (Time.realtimeSinceStartup - lastConnectException > EXCEPTION_LOG_PERIOD))
{
lastConnectException = Time.realtimeSinceStartup;
Debug.LogException(ex, this);
}
}
}
if (udpClient.Available > 0)
{
IPEndPoint e = new IPEndPoint(IPAddress.Any, COMMANDS_PORT);
Byte[] receiveBytes = null;
try {
receiveBytes = udpClient.Receive(ref e);
} catch (IOException ex) {
Debug.LogException(ex, this);
}
if (receiveBytes != null)
{
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;
* }*/
robot.SetMotors(commands[LEFT_MOTOR] / 512.0f, commands[RIGHT_MOTOR] / 512.0f, commands[CENTER_MOTOR] / 512.0f);
robot.SetAuxiliaryMotor(commands[CENTER_MOTOR] / 512.0f);
robot.SetAuxiliary2Motor(commands[AUX2_MOTOR] / 512.0f);
robot.SetIntake(commands[INTAKE] / 512.0f);
robot.SetIntakeArm(commands[INTAKE_ARM] > 0);
if (commands[LAUNCH] >= 256)
{
if (!launched)
{
robot.Launch();
}
launched = true;
}
else
{
launched = false;
}
}
}
lastCommand = DateTime.Now;
}
}
gameGui.haveRobotCode = DateTime.Now - lastCommand < TimeSpan.FromSeconds(1);
}
