summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java52
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Robot.java1
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() {