public static void Main()
{
float turn = 0;
timer.Start();
bool leftLast = false;
bool rightLast = false;
while(true)
{
ds.update();
ds.SendBattery(leftF.GetBusVoltage());
float t = deltaTime();
CTRE.FRC.DriverStation.State s = ds.GetState();
if (ds.IsEnabled())
{
float x = controller.GetAxis(0);
float y = controller.GetAxis(1);
float twist = -controller.GetAxis(4);
if (twist * twist < 0.005)
twist = 0;
turn += twist * t * 180;
if (controller.GetButton(6) && !leftLast) turn -= 90;
if (controller.GetButton(5) && !rightLast) turn += 90;
leftLast = controller.GetButton(6);
rightLast = controller.GetButton(5);
drive(x, y, turn);
}
else
{
float[] ypr = new float[3];
pigeon.GetYawPitchRoll(ypr);
turn = ypr[0];
}
byte[] b = new byte[] { (byte)'h', (byte)'e', (byte)'l', (byte)'l', (byte)'o' };
ds.SendUDP(2550, b);
}
}