From 6aefca6524459dc59b13e2b26d7750f926e00895 Mon Sep 17 00:00:00 2001
From: Thomas Griffith <tgrif81@tscstudents.net>
Date: Sat, 4 Mar 2017 15:43:50 -0500
Subject: (untested) implement choosing auto modes

---
 .../usfirst/frc/team4272/robot2017/Autonomous.java | 52 +++++++++++++++-------
 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() {
-- 
cgit v1.2.3