
package org.usfirst.frc.team2471.robot;


import org.usfirst.frc.team2471.robot.commands.BunnyBotAuto;
import org.usfirst.frc.team2471.robot.subsystems.Drive;
import org.usfirst.frc.team2471.robot.subsystems.Sucker;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
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 {

	public static OI oi;
	public static Drive drive;
	public static Sucker sucker;

    Command autonomousCommand;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
    	RobotMap.init();
		drive = new Drive();
		sucker = new Sucker();
		oi = new OI();
        // instantiate the command used for the autonomous period
		autonomousCommand = new BunnyBotAuto();
		//autonomousCommand = new DoNothing();
    }
	
	public void disabledPeriodic() {
		Scheduler.getInstance().run();
		
		/*System.out.println("Right Encoder: " + RobotMap.rightE.get());
		System.out.println("Left Encoder: " + RobotMap.leftE.get());
		System.out.println("Limit Top: " + RobotMap.ltop.get());
		System.out.println("Limit Bot: " + RobotMap.lbottom.get());*/
		
		SmartDashboard.putNumber("Right Encoder: ", RobotMap.rightE.getDistance());
        SmartDashboard.putNumber("Left Encoder: ", RobotMap.leftE.getDistance());
        SmartDashboard.putBoolean("Limit Top: ", RobotMap.ltop.get());
        SmartDashboard.putBoolean("Limit Bot: ", RobotMap.lbottom.get());
		drive.onDisabled();
	}

    public void autonomousInit() {
        // 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() {
		// 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();
        RobotMap.fextend.set(true);
        
    }

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

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
        
        //System.out.println("Hello world");
        
        /*System.out.println("Right Encoder: " + RobotMap.rightE.get());
		System.out.println("Left Encoder: " + );
		System.out.println("Limit Top: " + );
		System.out.println("Limit Bot: " + RobotMap.lbottom.get());*/
        
		//SmartDashboard.putNumber("Right Encoder: ", RobotMap.rightE.getRate());
        //SmartDashboard.putNumber("Left Encoder: ", RobotMap.leftE.getRate());
        SmartDashboard.putBoolean("Limit Top: ", RobotMap.ltop.get());
        SmartDashboard.putBoolean("Limit Bot: ", RobotMap.lbottom.get());
        SmartDashboard.putNumber("Voltage Output ", RobotMap.pdp.getCurrent(5));
        

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

@@ -83,28 +83,283 @@
 ommands.
-BunnyBotAuto
+AutoDoNothing;%0Aimport org.usfirst.frc.team2471.robot.commands.BunnyBotAuto;%0Aimport org.usfirst.frc.team2471.robot.commands.BunnyBotAutoLeft;%0Aimport org.usfirst.frc.team2471.robot.commands.BunnyBotAutoRight;%0Aimport org.usfirst.frc.team2471.robot.commands.RotateCommand
 ;%0Aimport
@@ -657,16 +657,77 @@
 Window;%0A
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;%0A
 import e
@@ -1233,16 +1233,61 @@
 ucker;%0A%0A
+%09public static SendableChooser autoChooser;%0A%0A
     Comm
@@ -1627,16 +1627,589 @@
  period%0A
+%09%09autoChooser = new SendableChooser();%0A        //autoChooser.addDefault(%223 Tote Pick Up %22, new AutoThreeTote());%0A        autoChooser.addObject(%22Nothing%22, new AutoDoNothing());%0A        autoChooser.addDefault(%22Drive Straight%22, new BunnyBotAuto());%0A        autoChooser.addObject(%22Left%22, new BunnyBotAutoLeft());%0A        autoChooser.addObject(%22Right%22, new BunnyBotAutoRight());%0A        autoChooser.addObject(%22Turn Only%22, new RotateCommand(180, 3));%0A        %0A//        autoChooser.addObject(%22Name%22, new Command());%0A%09%09%0A        SmartDashboard.putData(%22AutoChooser%22, autoChooser);%0A
 %09%09autono
@@ -3035,16 +3035,75 @@
 example)
+%0A%0A%09%09autonomousCommand = (Command)autoChooser.getSelected();
 %0A       
@@ -3887,21 +3887,54 @@
 Init()%7B%0A
-
     %09
+RobotMap.bunnyGrabber.set(false);
 %0A    %7D%0A%0A

Add auto chooser and set bunny grab state on disable.