public static void Main()
{
float turn = 0;
timer.Start();
bool leftLast = false;
bool rightLast = false;
while (true)
{
ds.update();
ds.SendBattery(12.34f);
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);
}
}
/** Return the heading from the Pigeon*/
public float GetImuHeading()
{
_pidgey.GetYawPitchRoll(YPR);
return(YPR[0]);
}