import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import org.usfirst.frc.team3501.robot.commands.*;
+import org.usfirst.frc.team3501.robot.autons.*;
import org.usfirst.frc.team3501.robot.subsystems.*;
public class Robot extends IterativeRobot {
public static OI oi;
- Command autonomousCommand;
+ private SendableChooser autoChooser;
+
+ private Command autonomousCommand;
public void robotInit() {
oi = new OI();
- double time = 1.2;
- double speed = 0.7;
- autonomousCommand = new DriveForward(time, speed);
+ chooseAuto();
}
public void disabledPeriodic() {
}
public void autonomousInit() {
+ autonomousCommand = (Command) autoChooser.getSelected();
autonomousCommand.start();
}
public void teleopPeriodic() {
Scheduler.getInstance().run();
-
-
}
public void testPeriodic() {
LiveWindow.run();
}
+
+ private void chooseAuto() {
+ autoChooser = new SendableChooser();
+
+ autoChooser.addDefault("Drive over step", new DriveOverStep());
+ autoChooser.addObject("Drive past step", new DrivePastStep());
+
+ SmartDashboard.putData("Auto Mode", autoChooser);
+ }
}