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);
}
}
//Password to module wifi is "password1"
/** entry point of the application */
public static void Main()
{
ds = new CTRE.FRC.DriverStation(wifiport); //Create new Driver station object at port 4
_gamepad = new CTRE.Controller.GameController(ds, 0); //Set controller to look at DS with ID 0
//Create two TalonSrx objects and set their control mode
left1 = new CTRE.TalonSrx(1);
right1 = new CTRE.TalonSrx(2);
left1.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
right1.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus);
//Set IP module looks for and is configured with, Computer must set to Static IP
ds.SendIP(new byte[] { 10, 0, 33, 2 }, new byte[] { 10, 0, 33, 5 });
while (true)
{
//Must be called for DS to function
ds.update();
//Send Battery Voltage information to Driver Station
ds.SendBattery(left1.GetBusVoltage());
if (ds.IsAuton() && ds.IsEnabled())
{
//Auton Code while enabled
}
else if (ds.IsAuton() && !ds.IsEnabled())
{
//Auton Code while disabled
}
else if (!ds.IsAuton() && ds.IsEnabled())
{
//Teleop Code while enabled
Drive();
Debug.Print(stringBuilder.ToString());
ds.SendUDP(2550, Encoding.UTF8.GetBytes(stringBuilder.ToString()));
stringBuilder.Clear();
}
else if (!ds.IsAuton() && !ds.IsEnabled())
{
//Teleop Code while disabled
}
}
}
