protected override async void Execute()
{
nt = NetworkTables.NetworkTable.GetTable("Forgiving/Vision");
nt.PutString("turn", "null");
nt.PutNumber("angle", 0.0);
double angle;
switch (System.Math.Abs(robotLocation))
{
case 1:
System.Console.WriteLine("Initial Location set to " + ((robotLocation == -1)?"LEFT":"RIGHT"));
FRC2017c.operateSys.gearIntake(0.3);
//int test=0;
FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed, 0, RobotMap.drivingSquaredInput);
int autonomousRushing1Delay = System.Convert.ToInt32(WPILib.SmartDashboard.SmartDashboard.GetNumber("AutonomousRushing1Delay", 900));
System.Threading.Thread.Sleep(autonomousRushing1Delay);
FRC2017c.operateSys.gearUp(-1);
System.Threading.Thread.Sleep(600);
//test++;System.Console.WriteLine("orz: "+test.ToString());
FRC2017c.driveSys.arcadeDrive(-0.3, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(340);
FRC2017c.operateSys.gearUp(0);
FRC2017c.driveSys.arcadeDrive(0, 0, RobotMap.drivingSquaredInput);
nt.PutString("turn", "null");
nt.PutNumber("angle", 0.0);
await amazingAutoTurn(RobotMap.autonomousAutoGearAngle, -robotLocation, 1);
FRC2017c.operateSys.gearUp(-1);
FRC2017c.powerSys.stallDetectionDelay(RobotMap.pdpMotorGearUp, RobotMap.stallMotorGearUp);
System.Console.WriteLine("Stalled!");
FRC2017c.operateSys.gearUp(0);
System.Threading.Thread.Sleep(300);
FRC2017c.operateSys.gearUp(1);
System.Threading.Thread.Sleep(500);
FRC2017c.operateSys.gearUp(0);
/*
* FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed*0.7,0,RobotMap.drivingSquaredInput);
* System.Threading.Thread.Sleep(100);
* FRC2017c.driveSys.arcadeDrive(0,0,RobotMap.drivingSquaredInput);
* System.Threading.Thread.Sleep(500);
*/
System.Threading.Thread.Sleep(1080);
angle = nt.GetNumber("angle", 0.0);
if (System.Math.Abs(angle) < 0.1)
{
System.Threading.Thread.Sleep(300);
angle = nt.GetNumber("angle", 0.0);
}
await amazingAutoTurn(System.Math.Abs(angle), 0.57 *((nt.GetString("turn", "null") == "right") ? 1 : -1), 1);
System.Threading.Thread.Sleep(180);
FRC2017c.operateSys.gearUp(1);
System.Threading.Thread.Sleep(600);
FRC2017c.operateSys.gearUp(0);
FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed * 0.64, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(950);
FRC2017c.driveSys.arcadeDrive(0.2, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(440);
FRC2017c.operateSys.gearIntake(-0.16);
System.Threading.Thread.Sleep(6000);
FRC2017c.operateSys.gearIntake(0);
FRC2017c.driveSys.arcadeDrive(0, 0, RobotMap.drivingSquaredInput);
break;
case 0:
System.Console.WriteLine("Initial Location set to CENTER");
FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed * 0.82, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(600);
FRC2017c.driveSys.arcadeDrive(0, 0, RobotMap.drivingSquaredInput);
FRC2017c.operateSys.gearUp(1);
FRC2017c.powerSys.stallDetectionDelay(RobotMap.pdpMotorGearUp, RobotMap.stallMotorGearUpSmall);
System.Console.WriteLine("Stalled!");
FRC2017c.operateSys.gearUp(-1);
FRC2017c.driveSys.arcadeDrive(RobotMap.autonomousAutoGearStraightSpeed * 0.6, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(1000);
FRC2017c.operateSys.gearUp(0);
System.Threading.Thread.Sleep(860);
FRC2017c.driveSys.arcadeDrive(0.2, 0, RobotMap.drivingSquaredInput);
System.Threading.Thread.Sleep(300);
FRC2017c.operateSys.gearIntake(-0.16);
System.Threading.Thread.Sleep(6000);
FRC2017c.operateSys.gearIntake(0);
FRC2017c.driveSys.arcadeDrive(0, 0, RobotMap.drivingSquaredInput);
break;
}
}
