Fix bug so that the sensitivity threshold worked correctly

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / Robot.java

package org.usfirst.frc.team3501.robot;

import org.usfirst.frc.team3501.robot.Constants.Defense;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
import org.usfirst.frc.team3501.robot.subsystems.Scaler;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends IterativeRobot {
	public static OI oi;
	public static DriveTrain driveTrain;
	public static Shooter shooter;
	public static Scaler scaler;
	public static IntakeArm intakeArm;

	// Sendable Choosers send a drop down menu to the Smart Dashboard.
	SendableChooser positionChooser;
	SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense, positionFourDefense,
			positionFiveDefense;

	@Override
	public void robotInit() {
		driveTrain = new DriveTrain();
		oi = new OI();
		shooter = new Shooter();
		scaler = new Scaler();
		intakeArm = new IntakeArm();

		// Sendable Choosers allows the driver to select the position of the
		// robot
		// and the positions of the defenses from a drop-down menu on the Smart
		// Dashboard
		// make the Sendable Choosers
		initializeSendableChoosers();
		addPositionChooserOptions();
		addDefensesToAllDefenseSendableChooosers();
		sendSendableChoosersToSmartDashboard();

	}

	private void initializeSendableChoosers() {
		positionChooser = new SendableChooser();
		positionOneDefense = new SendableChooser();
		positionTwoDefense = new SendableChooser();
		positionThreeDefense = new SendableChooser();
		positionFourDefense = new SendableChooser();
		positionFiveDefense = new SendableChooser();
	}

	private void addPositionChooserOptions() {
		positionChooser.addDefault("Position 1", 1);
		positionChooser.addObject("Position 2", 2);
		positionChooser.addObject("Position 3", 3);
		positionChooser.addObject("Position 4", 4);
		positionChooser.addObject("Position 5", 5);
	}

	private void addDefensesToAllDefenseSendableChooosers() {
		addDefenseOptions(positionOneDefense);
		addDefenseOptions(positionTwoDefense);
		addDefenseOptions(positionThreeDefense);
		addDefenseOptions(positionFourDefense);
		addDefenseOptions(positionFiveDefense);
	}

	private void addDefenseOptions(SendableChooser chooser) {
		chooser.addDefault("Portcullis", Defense.PORTCULLIS);
		chooser.addObject("Sally Port", Defense.SALLY_PORT);
		chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN);
		chooser.addObject("Low Bar", Defense.LOW_BAR);
		chooser.addObject("Cheval De Frise", Defense.CHEVAL_DE_FRISE);
		chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
		chooser.addObject("Moat", Defense.MOAT);
		chooser.addObject("Rock Wall", Defense.ROCK_WALL);
	}

	private void sendSendableChoosersToSmartDashboard() {
		SmartDashboard.putData("PositionChooser", positionChooser);
		SmartDashboard.putData("Position One Defense Chooser", positionOneDefense);
		SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense);
		SmartDashboard.putData("Position Three Defense Chooser", positionThreeDefense);
		SmartDashboard.putData("Position Four Defense Chooser", positionFourDefense);
		SmartDashboard.putData("Position Five Defense Chooser", positionFiveDefense);
	}

	@Override
	public void autonomousInit() {
		Scheduler.getInstance().run();

		// get options chosen from drop down menu
		Integer chosenPosition = (Integer) positionChooser.getSelected();
		Integer chosenDefense = 0;

		switch (chosenPosition) {
		case 1:
			chosenDefense = (Integer) positionOneDefense.getSelected();
		case 2:
			chosenDefense = (Integer) positionTwoDefense.getSelected();
		case 3:
			chosenDefense = (Integer) positionThreeDefense.getSelected();
		case 4:
			chosenDefense = (Integer) positionFourDefense.getSelected();
		case 5:
			chosenDefense = (Integer) positionFiveDefense.getSelected();
		}

		System.out.println("Chosen Position: " + chosenPosition);
		System.out.println("Chosen Defense: " + chosenDefense);
	}

	@Override
	public void autonomousPeriodic() {
		Scheduler.getInstance().run();
	}

	@Override
	public void teleopInit() {
	}

	@Override
	public void teleopPeriodic() {
		Scheduler.getInstance().run();

	}
}