// RobotBuilder Version: 2.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.

package org.usfirst.frc3219.TREAD;

import org.usfirst.frc3219.TREAD.subsystems.GearSlot;
import org.usfirst.frc3219.TREAD.commands.autonomous.StandardAutonomous;
import org.usfirst.frc3219.TREAD.commands.shooter.AimRight;
import org.usfirst.frc3219.TREAD.subsystems.Ballfeeder;
import org.usfirst.frc3219.TREAD.subsystems.Drive;
import org.usfirst.frc3219.TREAD.subsystems.BallIntake;
import org.usfirst.frc3219.TREAD.subsystems.Turntable;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc3219.TREAD.subsystems.*;
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;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */

public class Robot extends IterativeRobot {
	
	// Command Declarations
	Command autonomousCommand;
	private static SendableChooser posChooser;
	public static String position = "Default";
	
	public static boolean blueAlliance = true;
	
	// Subsystem Declarations
	public static OI oi;
	public static Climber climber;
	public static Turntable turntable;
	public static Drive drive;
	public static BallIntake intake;
	public static Ballfeeder ballfeeder;
	public static GearSlot gearSlot;
	public static Shooter shooter;
	public static Sensors sensors;
	
	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	public void robotInit() {
		RobotMap.init();
		
		// Subsystem Construction, OI must be last.
		climber = new Climber();
		turntable = new Turntable();
		drive = new Drive();
		intake = new BallIntake();
		ballfeeder = new Ballfeeder();
		gearSlot = new GearSlot();
		shooter= new Shooter();
		sensors = new Sensors();
		
		// OI must be constructed after subsystems. If the OI creates Commands
		// (which it very likely will), subsystems are not guaranteed to be
		// constructed yet. Thus, their requires() statements may grab null
		// pointers. Bad news. Don't move it.
		oi = new OI();

		// instantiate the command chooser used for selecting autonomous
		posChooser = new SendableChooser();
		posChooser.addDefault("Middle", "Middle");
		posChooser.addObject("Side", "Diag");
		SmartDashboard.putData("Position", posChooser);
		
		autonomousCommand = new StandardAutonomous();
	}

	/**
	 * This function is called when the disabled button is hit. You can use it
	 * to reset subsystems before shutting down.
	 */
	public void disabledInit() {

	}

	public void disabledPeriodic() {
		Scheduler.getInstance().run();
	}

	public void autonomousInit() {
		DriverStation.Alliance alliance = DriverStation.getInstance().getAlliance();
		blueAlliance = alliance.equals(DriverStation.Alliance.Blue);
		position = (String) posChooser.getSelected();
		autonomousCommand = new StandardAutonomous();
		// schedule the autonomous command (example)
		if (autonomousCommand != null)
			autonomousCommand.start();
	}

	/**
	 * This function is called periodically during autonomous
	 */
	public void autonomousPeriodic() {
		Scheduler.getInstance().run();
	}

	public void teleopInit() {
		DriverStation.Alliance alliance = DriverStation.getInstance().getAlliance();
		blueAlliance = alliance.equals(DriverStation.Alliance.Blue);
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to
		// continue until interrupted by another command, remove
		// this line or comment it out.
		if (autonomousCommand != null) autonomousCommand.cancel();
	}

	/**
	 * This function is called periodically during operator control
	 */
	public void teleopPeriodic() {
		Scheduler.getInstance().run();
	}

	/**
	 * This function is called periodically during test mode
	 */
	public void testPeriodic() {
		LiveWindow.run();
	}

	public static void addCommand(Command command) {
		Scheduler.getInstance().add(command);
	}
}

@@ -3100,20 +3100,62 @@
 ct(%22
-Side%22, %22Diag
+Left%22, %22Left%22);%0A%09%09posChooser.addObject(%22Right%22, %22Right
 %22);%0A

Fix for position chooser