diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 52 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Robot.java | 1 |
2 files changed, 38 insertions, 15 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index 7220d89..35a2e1d 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -28,6 +28,7 @@ package org.usfirst.frc.team4272.robot2017; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; public class Autonomous { private static interface Command { @@ -148,25 +149,46 @@ public class Autonomous { private final HwRobot robot; private final Command[] commands; private int step = 0; + private static final SendableChooser modeChooser = new SendableChooser(); + + public static void robotInit() { + modeChooser.addObject("Center Peg", "Center Peg"); + modeChooser.addObject("Left Peg", "Left Peg"); + modeChooser.addObject("Right Peg", "Right Peg"); + SmartDashboard.putData("Autonomous Mode", modeChooser); + } public Autonomous(HwRobot robot) { this.robot = robot; - // Cross line / center peg - commands = new Command[]{ - driveDistance(robot, 0.4, 8.75), - stop(robot), - }; - - // Left peg - /* - commands = new Command[]{ - driveDistance(robot, 0.4, 3.5), - turnDegrees(robot, 0.4, 60), - driveDistance(robot, 0.4, 8.3), - stop(robot), - }; - */ + String mode = (String) modeChooser.getSelected(); + + switch (mode) { + case "Center Peg": + commands = new Command[]{ + driveDistance(robot, 0.4, 8.75), + stop(robot), + }; + break; + case "Left Peg": + commands = new Command[]{ + driveDistance(robot, 0.4, 3.5), + turnDegrees(robot, 0.4, 60), + driveDistance(robot, 0.4, 8.3), + stop(robot), + }; + break; + case "Right Peg": + commands = new Command[]{ + driveDistance(robot, 0.4, 3.5), + turnDegrees(robot, 0.4, -60), + driveDistance(robot, 0.4, 8.3), + stop(robot), + }; + break; + default: + commands = new Command[]{stop(robot)}; + } } public Control run(Control c) { diff --git a/src/org/usfirst/frc/team4272/robot2017/Robot.java b/src/org/usfirst/frc/team4272/robot2017/Robot.java index ef732f0..b65ce35 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Robot.java +++ b/src/org/usfirst/frc/team4272/robot2017/Robot.java @@ -48,6 +48,7 @@ public class Robot extends IterativeRobot { * used for any initialization code. */ public void robotInit() { + Autonomous.robotInit(); } public void autonomousInit() { |