import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc.team3501.robot.autons.*;
+import org.usfirst.frc.team3501.robot.commands.*;
import org.usfirst.frc.team3501.robot.subsystems.*;
+import org.usfirst.frc.team3501.util.AutonData;
public class Robot extends IterativeRobot {
public static OI oi;
- private SendableChooser autoChooser;
+ public static AutonData autonData;
+ private SendableChooser autonChooser;
private Command autonomousCommand;
public void robotInit() {
- oi = new OI();
-
drivetrain = new Drivetrain();
arm = new Arm();
claw = new Claw();
pneumatics = new Pneumatics();
+ autonData = new AutonData();
+
+ oi = new OI();
+
chooseAuto();
}
}
public void autonomousInit() {
- pneumatics.start();
+ schedule(new TurnOnCompressor());
- autonomousCommand = (Command) autoChooser.getSelected();
+ autonData.update();
+
+ autonomousCommand = (Command) autonChooser.getSelected();
autonomousCommand.start();
}
}
public void teleopInit() {
- pneumatics.start();
+ schedule(new TurnOnCompressor());
- autonomousCommand.cancel();
+ if (autonomousCommand != null)
+ autonomousCommand.cancel();
}
public void teleopPeriodic() {
LiveWindow.run();
}
- public void disabledInit() {
- pneumatics.stop();
- }
-
private void chooseAuto() {
- autoChooser = new SendableChooser();
+ autonChooser = new SendableChooser();
- autoChooser.addDefault("Drive over step", new DriveOverStep());
- autoChooser.addObject("Drive past step", new DrivePastStep());
+ autonChooser.addDefault("Pick up container", new ContainerOverStep());
+ autonChooser.addObject("Drive over step", new DriveOverStep());
+ autonChooser.addObject("Drive past step", new DrivePastStep());
+
+ SmartDashboard.putData("Auto Mode", autonChooser);
+ }
- SmartDashboard.putData("Auto Mode", autoChooser);
+ private void schedule(Command c) {
+ Scheduler.getInstance().add(c);
}
}