public void Run()
{
//// Place the gear.
//AutoFunctions.Drive(1.6418); // Drive forward 1.6418 meters.
//AutoFunctions.TurnToAngle(-30); // Turn to -30 degrees.
//AutoFunctions.Drive(0.96); // Drive forward 0.96 meters to align at gear peg.
//AutoFunctions.PlaceGear(); // Place the gear. This might incorporate vision. Currently does not.
//// Drive to the boiler and shoot.
//AutoFunctions.Drive(-0.96); // Drive 0.96 meters backwards away from gear peg.
//AutoFunctions.TurnToAngle(0); // Turn to 0 degrees.
//AutoFunctions.Drive(-0.7); // Drive backwards -0.7 meters to clear airship for driving.
//AutoFunctions.TurnToAngle(133.74); // Turn to 133.74 degrees.
//AutoFunctions.Drive(6.5); // Drive 6.5 meters forward to the boiler. NEEDS CLARIFICATION FROM LOGAN.
//AutoFunctions.ShootFuel(); // Shoot fuel. This might incorporate vision. Currently does not.
//// Drive to a hopper.
//AutoFunctions.Drive(-2.1); // Back out of the boiler by driving back 2.1 meters.
//AutoFunctions.TurnToAngle(33.03); // Turn to 33.03 degrees.
//AutoFunctions.Drive(1.5); // Drive forward 1.5 meteres to the hopper.
//AutoFunctions.TurnToAngle(90); // Turn to 90 degrees, which is perpendicular to the perimeter.
//AutoFunctions.Drive(1.275); // Drive forward 1.275 meters to the hopper.
AutoFunctions.Drive(2.8);
Stopwatch stop = new Stopwatch();
stop.Start();
while (AutoFunctions.OnTarget() || stop.ElapsedMilliseconds < 8000)
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
RobotMap.GearSlot.Set(true);
AutoFunctions.DriveFast(0.35);
Stopwatch sw = new Stopwatch();
sw.Start();
while (sw.ElapsedMilliseconds < 5000)
{
;
}
AutoFunctions.Drive(-1.75);
while (AutoFunctions.OnTarget())
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
AutoFunctions.TurnToAngle(160);
}
public void Run()
{
//AutoFunctions.DriveStraight(5000);
//RobotMap.GearSlot.Set(true);
// Place the gear.
//AutoFunctions.Drive(0.52); // Drive forward 0.52 meters.
//AutoFunctions.TurnToAngle(-32.70); // Turn to 60.25 degrees.
//AutoFunctions.Drive(0.72); // Drive forward 0.72 meters.
//AutoFunctions.TurnToAngle(0); // Turn to 0 degrees.
//AutoFunctions.Drive(1.00); // Drive forward one meter to place the gear.
//AutoFunctions.PlaceGear(); // Place the gear.
AutoFunctions.Drive(2.08);
Stopwatch sw = new Stopwatch();
while (AutoFunctions.OnTarget() || sw.ElapsedMilliseconds < 5000)
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
RobotMap.GearSlot.Set(true);
sw.Start();
while (sw.ElapsedMilliseconds < 5000)
{
;
}
AutoFunctions.Drive(-1.75);
while (AutoFunctions.OnTarget())
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
AutoFunctions.TurnToAngle(90);
AutoFunctions.Drive(3);
while (AutoFunctions.OnTarget())
{
SmartDashboard.PutBoolean("In loop?", true);
SmartDashboard.PutNumber("Left Position", RobotMap.Left1.GetEncoderPosition());
SmartDashboard.PutNumber("Right Position", RobotMap.Right1.GetEncoderPosition());
}
SmartDashboard.PutBoolean("In loop?", false);
}
