Robot.java

// RobotBuilder Version: 1.5
//
// 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.frc5122.Fred2;

import org.usfirst.frc5122.Fred2.commands.*;
import org.usfirst.frc5122.Fred2.subsystems.*;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Preferences;
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.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 autonomousCommand;
    SendableChooser autoChooser;
    //CameraServer server;

    public static OI oi;
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static Drive drive;
    public static Lift lift;
    public static Grabber grabber;
    public static Lights lights;
    public static CanGrabber canGrabber;

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    public static Preferences prefs;

    private oi_Grabber grabbercmd;
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
    	System.out.println("Blake's Robot Code");
    	
    	prefs = Preferences.getInstance();
    	
    	RobotMap.init();
    	//server = CameraServer.getInstance();
    	//server.setQuality(50);
    	//server.startAutomaticCapture("cam0");
    	
    	
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        drive = new Drive();
        lift = new Lift();
        grabber = new Grabber();
        lights = new Lights();
        canGrabber = new CanGrabber();

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTORS
        // 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();
        grabbercmd = new oi_Grabber();

        // instantiate the command used for the autonomous period
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS
        autoChooser = new SendableChooser();
        autoChooser.addObject("Default Auto - Lift up 26in", new auto_liftup());
        autoChooser.addDefault("Lift up 12in", new auto_liftup_2());
        autoChooser.addObject("Robotset: fwd, right 90", new auto_robotset());
        autoChooser.addObject("auto_3totes", new auto_3totes());
        autoChooser.addObject("Stack 3 totes, partners remove RCs", new auto_3totes_norc());
        autoChooser.addObject("Stack bin on tote P1", new auto_rc_totenm3());
        autoChooser.addObject("1tote_pos3", new auto_1tote_pos3());
        autoChooser.addObject("1tote_pos2", new auto_1tote_pos2());
        autoChooser.addObject("1tote_pos1_lm", new auto_1tote_pos1());
        autoChooser.addObject("1tote_pos1_lm2", new auto_1tote_pos1_lm2());
        autoChooser.addObject("2tote_pos1", new auto_2tote_pos1());
        autoChooser.addObject("1RC_bump", new auto_1rc_bump());
        autoChooser.addObject("1tote_bump", new auto_1tote_bump());
        autoChooser.addObject("CanGrab", new auto_canGraabber());
        autoChooser.addObject("3tote with 1 RC", new auto_3tote_rc());
        SmartDashboard.putData("Auto Chooser", autoChooser);
        
    }

    /**
     * This function is called when the disabled button is hit.
     * You can use it to reset subsystems before shutting down.
     */
    public void disabledInit(){
    	Robot.lights.LightBar.set(15, 0, 7);
    	Robot.lights.LiftLights.set(5, 15, 0);
    }
    public void alwaysPerodic() {
//    	SmartDashboard.putNumber("Gyro", Robot.drive.getGyroAngle());
//    	SmartDashboard.putNumber("Gyro NavX", Robot.drive.imu.getYaw());
//        SmartDashboard.putBoolean("isConnected", Robot.drive.imu.isConnected());
//        SmartDashboard.putNumber("UpdateCount", Robot.drive.imu.getUpdateCount());
//        SmartDashboard.putBoolean("isCalibrating", Robot.drive.imu.isCalibrating());
//        SmartDashboard.putNumber("FusedHeading", Robot.drive.imu.getFusedHeading());
//        SmartDashboard.putNumber("Rate", Robot.drive.gyro1.getRate());
//        SmartDashboard.putNumber("Gyro Analog", Robot.drive.gyro1.getAngle());
//        
//        SmartDashboard.putNumber("Roll", Robot.drive.imu.getRoll());
//        SmartDashboard.putNumber("Pitch", Robot.drive.imu.getPitch());
//        SmartDashboard.putNumber("Yaw", Robot.drive.imu.getYaw());
//        SmartDashboard.putBoolean("GrabberLimit", Robot.canGrabber.isAtTop());
//        
//        SmartDashboard.putNumber("leftEndoder", Robot.drive.leftDistance());
        

//        double current = RobotMap.pdp.getCurrent(14);
        //SmartDashboard.putNumber("Lift Current",  current);
    }

    public void disabledPeriodic() {
        Scheduler.getInstance().run();
        alwaysPerodic();
//        SmartDashboard.putNumber("IMU", Robot.drive.getOldGyroAngle());
        Robot.lift.reset();
    }

    public void autonomousInit() {
//    	Robot.lights.LightBar.set(0, 5, 2);
//    	Robot.lights.LiftLights.set(15, 0, 0);
    	grabbercmd.cancel();
    	autonomousCommand = (Command) autoChooser.getSelected(); //= new auto_3totes();
        // schedule the autonomous command (example)
        if (autonomousCommand != null) autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
    	alwaysPerodic();
    	SmartDashboard.putNumber("Gyro", Robot.drive.getGyroAngle());
        Scheduler.getInstance().run();
    }

    public void teleopInit() {
    	grabbercmd.start();
    	
    	
        // 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() {
    	alwaysPerodic();    	
        Scheduler.getInstance().run();

    }

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