more changes

.classpath

@@ -1,7 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
 <classpath>
   <classpathentry kind="src" path="src"/>
-  <classpathentry kind="var" path="wpilib" sourcepath="wpilib.sources"/>
+  <classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER"/>
-  <classpathentry kind="var" path="networktables" sourcepath="networktables.sources"/>
+  <classpathentry kind="lib" path="~/wpilib/java/current/lib/WPILib.jar"/>
-  <classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.8"/>
+  <classpathentry kind="lib" path="~/wpilib/java/current/lib/NetworkTables.jar"/>
   <classpathentry kind="output" path="bin"/>
 </classpath>

.gitignore

@@ -1,3 +1,4 @@
 dist/FRCUserProgram.jar
 build
 bin
+sysProps.xml

autoformat.sh

@@ -17,7 +17,7 @@
 # -H             : Pad header (space after if, for, while)
 OPTS=( --mode=java -xc --style=google -j -s2 -xG -S -K -N -xn -xl -n -p -H )
 GLOB=( src/**/*.java )
-astyle $OPTS $GLOB|\grep -P '^(?!Unchanged)'
+astyle $OPTS $GLOB|\grep -vE '^Unchanged'
 # Get rid of newlines
-sed -i'' '/^\s*$/d' $GLOB
+perl -ni'' -e '/^\s*$/ || print' $GLOB
 \cd ->/dev/null

src/org/usfirst/frc/team2059/robot/OI.java

@@ -1,30 +1,73 @@
 package org.usfirst.frc.team2059.robot;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.Button;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.LogEncoder;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.PIDDrive;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.DriveStraight;
+import org.usfirst.frc.team2059.robot.commands.shooter.MoveArm;
+import org.usfirst.frc.team2059.robot.commands.shooter.ResetLower;
+import org.usfirst.frc.team2059.robot.commands.shooter.ResetUpper;
+import org.usfirst.frc.team2059.robot.commands.shooter.SetArmPosition;
+import org.usfirst.frc.team2059.robot.commands.shooter.SetShooterState;
+import org.usfirst.frc.team2059.robot.commands.shooter.SetArmStopState;
+import org.usfirst.frc.team2059.robot.commands.shooter.ShootAtSpeed;
+import org.usfirst.frc.team2059.robot.commands.shooter.SpinRollers;
+import org.usfirst.frc.team2059.robot.commands.shooter.AlignVertical;
+import org.usfirst.frc.team2059.robot.commands.visionhelper.AlignHorizontal;
+import org.usfirst.frc.team2059.robot.RobotMap;
 /**
  * This class is the glue that binds the controls on the physical operator
  * interface to the commands and command groups that allow control of the robot.
  */
 public class OI {
-  //// CREATING BUTTONS
+  JoystickButton[][] joystickButtons;
-  // One type of button is a joystick button which is any button on a joystick.
+  Joystick[] joysticks;
-  // You create one by telling it which joystick it's on and which button
+  public OI() {
-  // number it is.
+    joysticks = new Joystick[2];
-  // Joystick stick = new Joystick(port);
+    joystickButtons = new JoystickButton[2][13];
-  // Button button = new JoystickButton(stick, buttonNumber);
+    // Create joysticks
-  // There are a few additional built in buttons you can use. Additionally,
+    joysticks[0] = new Joystick(0);
-  // by subclassing Button you can create custom triggers and bind those to
+    joysticks[1] = new Joystick(1);
-  // commands the same as any other Button.
+    // Create buttons
-  //// TRIGGERING COMMANDS WITH BUTTONS
+    for (int i = 0; i < 12; i++) {
-  // Once you have a button, it's trivial to bind it to a button in one of
+      joystickButtons[0][i] = new JoystickButton(joysticks[0], i + 1);
-  // three ways:
+      joystickButtons[1][i] = new JoystickButton(joysticks[1], i + 1);
-  // Start the command when the button is pressed and let it run the command
+    }
-  // until it is finished as determined by it's isFinished method.
+    // Print log when button 1 pressed
-  // button.whenPressed(new ExampleCommand());
+    //joystickButtons[0][0].whenPressed(new LogEncoder());
-  // Run the command while the button is being held down and interrupt it once
+//    joystickButtons[0][0].whileHeld(new SetShooterState(true));
-  // the button is released.
+    joystickButtons[0][0].whileHeld(new AlignHorizontal());
-  // button.whileHeld(new ExampleCommand());
+    joystickButtons[0][1].whileHeld(new SetShooterState(true));
-  // Start the command when the button is released  and let it run the command
+    joystickButtons[0][2].whileHeld(new SpinRollers(-0.5, false));
-  // until it is finished as determined by it's isFinished method.
+    joystickButtons[0][3].whileHeld(new SpinRollers(1, false));
-  // button.whenReleased(new ExampleCommand());
+    joystickButtons[0][4].whileHeld(new DriveStraight(0.25));
+    joystickButtons[0][5].whileHeld(new DriveStraight(-0.25));
+    joystickButtons[0][6].whileHeld(new SpinRollers(1, true));
+    joystickButtons[0][9].whileHeld(new Drive());
+//    joystickButtons[0][2].whileHeld(new PIDDrive(400));
+    //joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect));
+    joystickButtons[1][0].whileHeld(new AlignVertical());
+    joystickButtons[1][1].whileHeld(new SetShooterState(true));
+    joystickButtons[1][2].whileHeld(new ResetLower(-1));
+    joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse));
+    joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot));
+    joystickButtons[1][5].whileHeld(new SpinRollers(1, false));
+//    joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetLowShot));
+    joystickButtons[1][7].whileHeld(new MoveArm(1));
+    joystickButtons[1][8].whileHeld(new MoveArm(-1));
+    joystickButtons[1][9].whileHeld(new SetArmPosition(110));
+  }
+  public Joystick[] getJoysticks() {
+    return joysticks;
+  }
+  public Joystick getJoystick(int stick) {
+    return joysticks[stick];
+  }
+  public JoystickButton[][] getJoystickButtons() {
+    return joystickButtons;
+  }
 }
 // vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/Robot.java

@@ -1,91 +1,97 @@
 package org.usfirst.frc.team2059.robot;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.commands.autonomous.*;
 import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.CameraServer;
 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 {
   public static OI oi;
   Command autonomousCommand;
   SendableChooser chooser;
-  /**
+//  CameraServer cameraServer;
-   * This function is run when the robot is first started up and should be
-   * used for any initialization code.
-   */
   public void robotInit() {
+    CommandBase.init();
     oi = new OI();
     chooser = new SendableChooser();
-//        chooser.addObject("My Auto", new MyAutoCommand());
+//   cameraServer = CameraServer.getInstance();
-    SmartDashboard.putData("Auto mode", chooser);
+//    cameraServer.setQuality(50);
+//    cameraServer.startAutomaticCapture("cam0");
+    try {
+      new ProcessBuilder("/home/lvuser/grip").inheritIO().start();
+    } catch (Exception e) {
+      System.out.println("Error starting GRIP");
+    }
+    chooser.addDefault("Nothing", new RoutineNothing());
+    chooser.addObject("Time based low bar", new RoutineDriveTime());
+    chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime());
+    chooser.addObject("Distance based low bar straight", new RoutineDriveStraightDistance());
+    chooser.addObject("Time based defense", new RoutineDefenseTime());
+    chooser.addObject("Shot Auto", new RoutineShoot());
+    chooser.addObject("Two Ball Drop off", new RoutineTwoBallDropOff());
+    SmartDashboard.putData("Auto mode", chooser);
+    SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());
+    SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController());
+    SmartDashboard.putData("gyroPID", CommandBase.driveBase.getGyroController());
+    SmartDashboard.putNumber("GyroCorrection", 0.13);
+    SmartDashboard.putBoolean("CompressorEnabled", true);
+    //Automatically determine if rolling in or rolling out
+    SmartDashboard.putBoolean("SmartRollers", false);
+    //Use the limit swithces on the shooter
+    SmartDashboard.putBoolean("UseLimitSwitches", true);
   }
-  /**
-    * This function is called once each time the robot enters Disabled mode.
-    * You can use it to reset any subsystem information you want to clear when
-   * the robot is disabled.
-    */
   public void disabledInit() {
   }
   public void disabledPeriodic() {
     Scheduler.getInstance().run();
   }
-  /**
-   * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
-   * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
-   * Dashboard, remove all of the chooser code and uncomment the getString code to get the auto name from the text box
-   * below the Gyro
-   *
-   * You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
-   * or additional comparisons to the switch structure below with additional strings & commands.
-   */
   public void autonomousInit() {
     autonomousCommand = (Command) chooser.getSelected();
-    /* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
-    switch(autoSelected) {
-    case "My Auto":
-    	autonomousCommand = new MyAutoCommand();
-    	break;
-    case "Default Auto":
-    default:
-    	autonomousCommand = new ExampleCommand();
-    	break;
-    } */
-    // schedule the autonomous command (example)
     if (autonomousCommand != null) {
       autonomousCommand.start();
     }
   }
-  /**
-   * This function is called periodically during autonomous
-   */
   public void autonomousPeriodic() {
     Scheduler.getInstance().run();
+    SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
+    SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
+    SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations());
+    SmartDashboard.putNumber("horizontalError", CommandBase.visionHelper.getHorizontalError());
+    SmartDashboard.putNumber("verticalError", CommandBase.visionHelper.getVerticalError());
+    SmartDashboard.putNumber("horizontalErrorCorrected", CommandBase.visionHelper.getHorizontalError() + 5);
   }
   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();
     }
+    CommandBase.pneumatics.setCompressorEnabled(true);
+    CommandBase.pneumatics.setArmStopState(false);
   }
-  /**
-   * This function is called periodically during operator control
-   */
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+    SmartDashboard.putNumber("GyroAngle", CommandBase.driveBase.getGyro().getAngle());
+    SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
+    SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
+    SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations());
+    SmartDashboard.putNumber("centerX", CommandBase.visionHelper.getCenterX());
+    SmartDashboard.putNumber("centerContoursX", CommandBase.visionHelper.getCenterContourX());
+    SmartDashboard.putNumber("horizontalError", CommandBase.visionHelper.getHorizontalError());
+    SmartDashboard.putNumber("horizontalErrorCorrected", CommandBase.visionHelper.getHorizontalError() + 5);
+    SmartDashboard.putNumber("sonarDistance", CommandBase.mainArm.getSonarDistance());
+    SmartDashboard.putNumber("sonarDistanceRaw", CommandBase.mainArm.getSonarDistanceRaw());
+    SmartDashboard.putNumber("shooterAngleError", CommandBase.mainArm.getShooterAngleError());
+    SmartDashboard.putNumber("shooterAngleErrorFixed", CommandBase.mainArm.getShooterAngleError()+50);
+    SmartDashboard.putNumber("goalDistance", 54 / Math.tan((Math.PI/180) * (60 - CommandBase.visionHelper.getVerticalError())));
+    if (Robot.oi.getJoysticks()[1].getRawButton(3)) {
+      CommandBase.pneumatics.setArmStopState(true);
+    } else {
+      CommandBase.pneumatics.setArmStopState(false);
+    }
+    CommandBase.pneumatics.setCompressorEnabled(SmartDashboard.getBoolean("CompressorEnabled"));
   }
-  /**
-   * This function is called periodically during test mode
-   */
   public void testPeriodic() {
     LiveWindow.run();
   }

src/org/usfirst/frc/team2059/robot/RobotMap.java

@@ -3,16 +3,19 @@ public class RobotMap {
   //Drive
   public static int driveLeftMotorOne = 1;
   public static int driveLeftMotorTwo = 2;
-  public static int driveRightMotorOne = 1;
+  public static int driveRightMotorOne = 3;
-  public static int driveRightMotorTwo = 2;
+  public static int driveRightMotorTwo = 4;
-  public static int driveRightEncoder = 0;
+  public static int driveLeftEncoderA = 0;
-  public static int driveLeftEncoder = 1;
+  public static int driveLeftEncoderB = 1;
+  public static int gyro = 1;
   //Arm
-  public static double zeroDegrees = 0.1;
+  public static double zeroDegrees = 1.28;
-  public static double ninetyDegrees = 0.7;
+  public static double ninetyDegrees = 3.1; 
-  public static int armPot = 0;
+  public static int armPot = 3;
   public static int armLeftMotor = 5;
   public static int armRightMotor = 6;
+  public static int armBottomLim = 4;
+  public static int armTopLim = 5;
   //Shooter
   public static int shooterLeftMotor = 7;
   public static int shooterRightMotor = 8;
@@ -24,10 +27,22 @@ public class RobotMap {
   public static int portcullisSolenoidTwo = 3;
   public static int armStopSolenoidOne = 4;
   public static int armStopSolenoidTwo = 5;
+  //Camera
+  //public static double fWidth = 2.8;
+  // Camera: Axis M1013
+  // FOV = 67deg
+  // f = pixelSize/(2*tan(FOV/2))
+  public static double imageWidth = 320;
+  public static double imageHeight = 240;
+  public static double fWidth = 483.467261958;
+  public static double fHeight = 362.600446468;
+  public static int sonar = 2;
+  public static int goalHeight = 85;
   //Misc
-  public static int mainArmPresetCollect = 0;
+  public static int mainArmPresetCollect = -5;
-  public static int mainArmPresetTraverse = 5;
+  public static int mainArmPresetTraverse = 10;
-  public static int mainArmPresetCloseShot = 95;
+  public static int mainArmPresetLowShot = 18;
-  public static int mainArmPresetFarShot = 85;
+  public static int mainArmPresetCloseShot = 90;
+  public static int mainArmPresetFarShot = 70;
 }
 // vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/CommandBase.java

@@ -0,0 +1,25 @@
+package org.usfirst.frc.team2059.robot.commands;
+import org.usfirst.frc.team2059.robot.subsystems.DriveBase;
+import org.usfirst.frc.team2059.robot.subsystems.EncoderBase;
+import org.usfirst.frc.team2059.robot.subsystems.MainArm;
+import org.usfirst.frc.team2059.robot.subsystems.Pneumatics;
+import org.usfirst.frc.team2059.robot.subsystems.Shooter;
+import org.usfirst.frc.team2059.robot.subsystems.VisionHelper;
+import edu.wpi.first.wpilibj.command.Command;
+public abstract class CommandBase extends Command {
+  public static EncoderBase encoderBase;
+  public static DriveBase driveBase;
+  public static MainArm mainArm;
+  public static Pneumatics pneumatics;
+  public static Shooter shooter;
+  public static VisionHelper visionHelper;
+  public static void init() {
+    encoderBase = new EncoderBase();
+    driveBase = new DriveBase();
+    mainArm = new MainArm();
+    pneumatics = new Pneumatics();
+    shooter = new Shooter();
+    visionHelper = new VisionHelper();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDrive.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+public class AutoDrive extends CommandBase {
+  private double distance;
+  // Determines if we should start driving
+  // Will be false if we already started driving
+  private boolean startDriving = true;
+  public AutoDrive(double distance, double timeout) {
+    requires(driveBase);
+    this.distance = distance;
+    setTimeout(timeout);
+  }
+  public AutoDrive(double distance) {
+    requires(driveBase);
+    this.distance = distance;
+    // Make the default timeout 2s
+    setTimeout(2.0d);
+  }
+  protected void initialize() {
+  }
+  protected void execute() {
+    if (startDriving) {
+      driveBase.pidDrive(distance, SmartDashboard.getNumber("GyroCorrection"));
+    }
+    startDriving = false;
+  }
+  protected void end() {
+    startDriving = true;
+    driveBase.stopDriving();
+  }
+  protected void interrupted() {
+    end();
+  }
+  protected boolean isFinished() {
+    return isTimedOut();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightDistance.java

@@ -0,0 +1,30 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+public class AutoDriveStraightDistance extends CommandBase {
+  double distance;
+  public AutoDriveStraightDistance(double distance, double timeout) {
+    requires(driveBase);
+    setTimeout(timeout);
+    this.distance = distance;
+  }
+  protected void initialize() {
+    driveBase.resetGyro();
+    driveBase.getLeftEncoder().reset();
+  }
+  protected void execute() {
+    driveBase.pidDrive(distance, SmartDashboard.getNumber("GyroCorrection"));
+  }
+  protected void end() {
+    driveBase.getLeftController().disable();
+    driveBase.stopDriving();
+  }
+  protected void interrupted() {
+    end();
+  }
+  protected boolean isFinished() {
+    return isTimedOut() || Math.abs(distance - driveBase.getLeftRotations()) <= 3;
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveStraightTime.java

@@ -0,0 +1,28 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+public class AutoDriveStraightTime extends CommandBase {
+  double speed;
+  public AutoDriveStraightTime(double speed, double time) {
+    requires(driveBase);
+    setTimeout(time);
+    this.speed = speed;
+  }
+  protected void initialize() {
+    driveBase.resetGyro();
+  }
+  protected void execute() {
+    driveBase.driveStraight(-speed, SmartDashboard.getNumber("GyroCorrection"));
+  }
+  protected void end() {
+    driveBase.stopDriving();
+  }
+  protected void interrupted() {
+    end();
+  }
+  protected boolean isFinished() {
+    return isTimedOut();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java

@@ -0,0 +1,26 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+public class AutoDriveTime extends CommandBase {
+  double power;
+  public AutoDriveTime(double timeout, double power) {
+    requires(driveBase);
+    this.power = power;
+    setTimeout(timeout);
+  }
+  protected void initialize() {
+  }
+  protected void execute() {
+    driveBase.driveArcade(0, -power, 0, 1);
+  }
+  protected void end() {
+    driveBase.stopDriving();
+  }
+  protected void interrupted() {
+    end();
+  }
+  protected boolean isFinished() {
+    return isTimedOut();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoMoveArm.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+/**
+ *
+ */
+public class AutoMoveArm extends CommandBase {
+  double speed;
+  public AutoMoveArm(double s, double timeout) {
+    requires(mainArm);
+    speed = s;
+    setTimeout(timeout);
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    mainArm.disable();
+    mainArm.moveArm(speed);
+    System.out.println(speed);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return isTimedOut();
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.moveArm(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class AutoResetLower extends CommandBase {
+  double speed;
+  public AutoResetLower(double s) {
+    requires(mainArm);
+    speed = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    System.out.println("test");
+    mainArm.disable();
+    mainArm.resetLower(speed);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when bottom limit switch is hit
+    return mainArm.getBottomPressed();
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.moveArm(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoRotate.java

@@ -0,0 +1,31 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import edu.wpi.first.wpilibj.AnalogGyro;
+public class AutoRotate extends CommandBase {
+  double degrees;
+  public AutoRotate(double degrees) {
+    requires(driveBase);
+    this.degrees = degrees;
+    setTimeout(2.5);
+  }
+  protected void initialize() {
+    driveBase.getGyro().reset();
+  }
+  protected void execute() {
+    System.out.println("rotating");
+    driveBase.rotateAngle(degrees);
+  }
+  protected void end() {
+    driveBase.stopDriving();
+    driveBase.getGyroController().disable();
+  }
+  protected void interrupted() {
+    end();
+  }
+  protected boolean isFinished() {
+    //return Math.abs(driveBase.getGyro().getAngle()-degrees) <= 0.5;
+    return isTimedOut();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java

@@ -0,0 +1,43 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import org.usfirst.frc.team2059.robot.RobotMap;
+/**
+ *
+ */
+public class AutoSetArmPosition extends CommandBase {
+  double pos;
+  public AutoSetArmPosition(double p) {
+    requires(mainArm);
+    pos = p;
+    setTimeout(3);
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    mainArm.enable();
+    mainArm.setSetpoint(pos);
+    System.out.println(pos - mainArm.getDegrees());
+    System.out.println(mainArm.getDegrees());
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when either limit switch is hit
+    System.out.println("AutoSetArmPosition.isFinished(): " + (Math.abs(pos - mainArm.getDegrees()) <= 1));
+    System.out.println("  pos                  : " + pos);
+    System.out.println("  mainArm.getDegrees() : " + mainArm.getDegrees());
+    return (Math.abs(pos - mainArm.getDegrees()) <= 1) || isTimedOut();
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.disable();
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmStopState.java

@@ -0,0 +1,32 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class AutoSetArmStopState extends CommandBase {
+  boolean state;
+  public AutoSetArmStopState(boolean s) {
+    state = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    pneumatics.setArmStopState(state);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return true;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetShooterState.java

@@ -0,0 +1,32 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class AutoSetShooterState extends CommandBase {
+  boolean state;
+  public AutoSetShooterState(boolean s) {
+    state = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    pneumatics.setShooterState(state);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return true;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSpinRollers.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class AutoSpinRollers extends CommandBase {
+  double speed;
+  boolean smartrollers;
+  public AutoSpinRollers(double s, double timeout) {
+    speed = s;
+    setTimeout(timeout);
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    shooter.shootAtSpeed(speed);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return isTimedOut();
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    shooter.shootAtSpeed(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java

@@ -0,0 +1,11 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineDefenseTime extends CommandGroup {
+  public RoutineDefenseTime() {
+    addSequential(new AutoSetArmStopState(false));
+    addSequential(new AutoSetArmPosition(6));
+    addSequential(new AutoDriveTime(2.5, 0.8));
+  }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightDistance.java

@@ -0,0 +1,11 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineDriveStraightDistance extends CommandGroup {
+  public RoutineDriveStraightDistance() {
+    addSequential(new AutoSetArmStopState(true));
+    addSequential(new AutoResetLower(-1));
+    addSequential(new AutoDriveStraightDistance(200, 5));
+  }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveStraightTime.java

@@ -0,0 +1,11 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineDriveStraightTime extends CommandGroup {
+  public RoutineDriveStraightTime() {
+    addSequential(new AutoSetArmStopState(true));
+    addSequential(new AutoResetLower(-1));
+    addSequential(new AutoDriveStraightTime(0.5, 1));
+  }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java

@@ -0,0 +1,11 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineDriveTime extends CommandGroup {
+  public RoutineDriveTime() {
+    addSequential(new AutoSetArmStopState(true));
+    addSequential(new AutoResetLower(-1));
+    addSequential(new AutoDriveTime(2.5, 0.75));
+  }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineNothing.java

@@ -0,0 +1,8 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineNothing extends CommandGroup {
+  public RoutineNothing() {
+  }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineRaiseArmFire.java

@@ -0,0 +1,15 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineRaiseArmFire extends CommandGroup {
+  public RoutineRaiseArmFire() {
+    addSequential(new AutoSetArmPosition(90));
+    //Raise the arm a little before spinning rollers
+    try {
+      wait(2);
+    } catch (Exception e) { }
+    addParallel(new AutoSpinRollers(0.5, 5));
+    addSequential(new AutoSetShooterState(true));
+  }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineShoot.java

@@ -0,0 +1,15 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineShoot extends CommandGroup {
+  public RoutineShoot() {
+    addSequential(new AutoSetArmStopState(true));
+    addSequential(new AutoResetLower(-1));
+    addSequential(new AutoDriveStraightTime(0.5, 1));
+    addSequential(new AutoSetArmPosition(90));
+    //addSequential(new AutoRotate(45));
+    //addSequential(new AutoDriveStraightTime(0.4, 1.5));
+    addSequential(new VisionAlignHorizontal());
+  }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineTwoBallDropOff.java

@@ -0,0 +1,26 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import org.usfirst.frc.team2059.robot.Robot;
+public class RoutineTwoBallDropOff extends CommandGroup {
+  public RoutineTwoBallDropOff() {
+    addSequential(new AutoSetArmStopState(true));
+    addSequential(new AutoResetLower(-1));
+    addSequential(new AutoDriveStraightTime(0.5, 2));
+    addSequential(new AutoMoveArm(0.5, 1));
+    addSequential(new AutoSpinRollers(1, 2));
+    addSequential(new AutoSetShooterState(true));
+    addSequential(new AutoResetLower(-1));
+    addSequential(new AutoRotate(180));
+    addSequential(new AutoSetShooterState(false));
+    addParallel(new AutoSpinRollers(-0.5, 3));
+    addSequential(new AutoDriveStraightTime(0.5, 2));
+    addSequential(new AutoRotate(180));
+    addSequential(new AutoDriveStraightTime(0.5, 2));
+    addSequential(new AutoMoveArm(0.5, 1));
+    addSequential(new AutoSpinRollers(1, 2));
+    addSequential(new AutoSetShooterState(true));
+    addSequential(new AutoRotate(180));
+    addSequential(new AutoSetShooterState(false));
+  }
+}

src/org/usfirst/frc/team2059/robot/commands/autonomous/VisionAlignHorizontal.java

@@ -0,0 +1,29 @@
+package org.usfirst.frc.team2059.robot.commands.autonomous;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+public class VisionAlignHorizontal extends CommandBase {
+  double error;
+  public VisionAlignHorizontal() {
+  }
+  protected void initialize() {
+    error = visionHelper.getHorizontalError();
+    driveBase.resetGyro();
+  }
+  protected boolean isFinished() {
+    return false;
+  }
+  protected void execute() {
+    if (visionHelper.getHorizontalError() == 420) {
+      driveBase.resetGyro();
+      driveBase.rotateAngle(5);
+    } else {
+//      driveBase.rotateAngle(visionHelper.getHorizontalError());
+    }
+  }
+  protected void end() {
+    driveBase.getGyroController().disable();
+  }
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/drivetrain/Drive.java

@@ -0,0 +1,39 @@
+package org.usfirst.frc.team2059.robot.commands.drivetrain;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class Drive extends CommandBase {
+  double sensitivity = 0.5;
+  public Drive() {
+    requires(driveBase);
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    double x = Robot.oi.getJoysticks()[0].getRawAxis(0);
+    double y = Robot.oi.getJoysticks()[0].getRawAxis(1);
+    double z = Robot.oi.getJoysticks()[0].getRawAxis(2);
+    if (Robot.oi.getJoysticks()[0].getRawButton(2) || Robot.oi.getJoysticks()[0].getRawButton(11)) {
+      sensitivity = 1;
+    } else {
+      sensitivity = 0.7;
+    }
+    driveBase.driveArcade(x, y, z, sensitivity);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/drivetrain/DriveStraight.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc.team2059.robot.commands.drivetrain;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class DriveStraight extends CommandBase {
+  double speed;
+  public DriveStraight(double speed) {
+    requires(driveBase);
+    this.speed = speed;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    driveBase.resetGyro();
+    driveBase.driveStraight(speed, SmartDashboard.getNumber("GyroCorrection"));
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    driveBase.driveStraight(0, SmartDashboard.getNumber("GyroCorrection"));
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/drivetrain/LogEncoder.java

@@ -0,0 +1,28 @@
+package org.usfirst.frc.team2059.robot.commands.drivetrain;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.structs.EncoderValues;
+public class LogEncoder  extends CommandBase {
+  public LogEncoder() {
+    requires(encoderBase);
+  }
+  protected void initialize() {
+  }
+  protected boolean isFinished() {
+    EncoderValues values = encoderBase.getValues();
+    System.out.println("==== Encoder log ====");
+    System.out.println("Count     : " + values.getCount());
+    System.out.println("Distance  : " + values.getDistance());
+    System.out.println("Period    : " + values.getPeriod());
+    System.out.println("Rate      : " + values.getRate());
+    System.out.println("Direction : " + values.getDirection());
+    System.out.println("Stopped   : " + values.getStopped());
+    return true;
+  }
+  protected void execute() {
+  }
+  protected void end() {
+  }
+  protected void interrupted() {
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/drivetrain/PIDDrive.java

@@ -0,0 +1,33 @@
+package org.usfirst.frc.team2059.robot.commands.drivetrain;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class PIDDrive extends CommandBase {
+  double count;
+  public PIDDrive(double c) {
+    requires(driveBase);
+    count = c;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    driveBase.pidDrive(count, SmartDashboard.getNumber("GyroCorrection"));
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/drivetrain/ResetEncoder.java

@@ -0,0 +1,20 @@
+package org.usfirst.frc.team2059.robot.commands.drivetrain;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+public class ResetEncoder extends CommandBase {
+  public ResetEncoder() {
+    requires(encoderBase);
+  }
+  protected void initialize() {
+  }
+  protected boolean isFinished() {
+    return true;
+  }
+  protected void execute() {
+    encoderBase.resetEncoder();
+  }
+  protected void end() {
+  }
+  protected void interrupted() {
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/AlignVertical.java

@@ -0,0 +1,35 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import org.usfirst.frc.team2059.robot.RobotMap;
+/**
+ *
+ */
+public class AlignVertical extends CommandBase {
+  public AlignVertical() {
+    requires(mainArm);
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    mainArm.setSetpoint(mainArm.getShooterAngleError()+50);
+    mainArm.enable();
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when either limit switch is hit
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.disable();
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/MoveArm.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+/**
+ *
+ */
+public class MoveArm extends CommandBase {
+  double speed;
+  public MoveArm(double s) {
+    requires(mainArm);
+    speed = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    mainArm.disable();
+    mainArm.moveArm(speed);
+    System.out.println(speed);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    //TODO: I don't know if this should go here
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.moveArm(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java

@@ -0,0 +1,36 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class ResetLower extends CommandBase {
+  double speed;
+  public ResetLower(double s) {
+    requires(mainArm);
+    speed = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    mainArm.disable();
+    mainArm.resetLower(speed);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when either limit switch is hit
+    return mainArm.getBottomPressed();
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.moveArm(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/ResetUpper.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class ResetUpper extends CommandBase {
+  double speed;
+  public ResetUpper(double s) {
+    requires(mainArm);
+    speed = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    System.out.println("testUpper");
+    mainArm.disable();
+    mainArm.resetUpper(speed);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when either limit switch is hit
+    return mainArm.getBottomPressed() || mainArm.getTopPressed();
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.moveArm(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java

@@ -0,0 +1,38 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import org.usfirst.frc.team2059.robot.RobotMap;
+/**
+ *
+ */
+public class SetArmPosition extends CommandBase {
+  double pos;
+  public SetArmPosition(double p) {
+    requires(mainArm);
+    pos = p;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    //Move the arm stop
+    mainArm.enable();
+    mainArm.setSetpoint(pos);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    // Stop when either limit switch is hit
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    mainArm.disable();
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmStopState.java

@@ -1,16 +1,20 @@
-package org.usfirst.frc.team2059.robot.commands;
+package org.usfirst.frc.team2059.robot.commands.shooter;
-import edu.wpi.first.wpilibj.command.Command;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
 /**
  *
  */
-public class Drive extends Command {
+public class SetArmStopState extends CommandBase {
-  public Drive() {
+  boolean state;
+  public SetArmStopState(boolean s) {
+    state = s;
   }
   // Called just before this Command runs the first time
   protected void initialize() {
   }
   // Called repeatedly when this Command is scheduled to run
   protected void execute() {
+    pneumatics.setArmStopState(state);
   }
   // Make this return true when this Command no longer needs to run execute()
   protected boolean isFinished() {
@@ -18,10 +22,12 @@ public class Drive extends Command {
   }
   // Called once after isFinished returns true
   protected void end() {
+    pneumatics.setArmStopState(!state);
   }
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   protected void interrupted() {
+    end();
   }
 }
 // vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/SetShooterState.java

@@ -0,0 +1,33 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class SetShooterState extends CommandBase {
+  boolean state;
+  public SetShooterState(boolean s) {
+    state = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    pneumatics.setShooterState(state);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    pneumatics.setShooterState(!state);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/ShootAtSpeed.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+import org.usfirst.frc.team2059.robot.RobotMap;
+/**
+ *
+ */
+public class ShootAtSpeed extends CommandBase {
+  double speed;
+  public ShootAtSpeed(double s) {
+    speed = s;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    shooter.shootAtSpeed(speed);
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    shooter.shootAtSpeed(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/shooter/SpinRollers.java

@@ -0,0 +1,40 @@
+package org.usfirst.frc.team2059.robot.commands.shooter;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+import org.usfirst.frc.team2059.robot.Robot;
+/**
+ *
+ */
+public class SpinRollers extends CommandBase {
+  double speed;
+  boolean smartrollers;
+  public SpinRollers(double s, boolean sr) {
+    speed = s;
+    smartrollers = sr;
+  }
+  // Called just before this Command runs the first time
+  protected void initialize() {
+  }
+  // Called repeatedly when this Command is scheduled to run
+  protected void execute() {
+    if (smartrollers && (mainArm.getDegrees() < 5 || mainArm.getBottomPressed())) {
+      shooter.shootAtSpeed(-.5);
+    } else {
+      shooter.shootAtSpeed(speed);
+    }
+  }
+  // Make this return true when this Command no longer needs to run execute()
+  protected boolean isFinished() {
+    return false;
+  }
+  // Called once after isFinished returns true
+  protected void end() {
+    shooter.shootAtSpeed(0);
+  }
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/commands/vision/AlignHorizontal.java

@@ -0,0 +1,31 @@
+package org.usfirst.frc.team2059.robot.commands.visionhelper;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+public class AlignHorizontal extends CommandBase {
+  double error;
+  public AlignHorizontal() {
+  }
+  protected void initialize() {
+    error = visionHelper.getHorizontalError();
+    driveBase.setDriveEnabled(false);
+    driveBase.resetGyro();
+    mainArm.setSetpoint(110);
+    mainArm.enable();
+  }
+  protected boolean isFinished() {
+    return false;
+  }
+  protected void execute() {
+    if(mainArm.getDegrees() >=45){
+      driveBase.rotateAngle(error);
+    }
+  }
+  protected void end() {
+    driveBase.getGyroController().disable();
+    driveBase.setDriveEnabled(true);
+    mainArm.disable();
+  }
+  protected void interrupted() {
+    end();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/structs/EncoderValues.java

@@ -0,0 +1,33 @@
+package org.usfirst.frc.team2059.robot.structs;
+public class EncoderValues {
+  private int count;
+  private double distance, period, rate;
+  private boolean direction, stopped;
+  public EncoderValues(int count, double distance, double period, double rate, boolean direction, boolean stopped) {
+    this.count = count;
+    this.distance = distance;
+    this.period = period;
+    this.rate = rate;
+    this.direction = direction;
+    this.stopped = stopped;
+  }
+  public int getCount() {
+    return count;
+  }
+  public double getDistance() {
+    return distance;
+  }
+  public double getPeriod() {
+    return period;
+  }
+  public double getRate() {
+    return rate;
+  }
+  public boolean getDirection() {
+    return direction;
+  }
+  public boolean getStopped() {
+    return stopped;
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/subsystems/DriveBase.java

@@ -0,0 +1,109 @@
+package org.usfirst.frc.team2059.robot.subsystems;
+import org.usfirst.frc.team2059.robot.RobotMap;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.CANTalon;
+public class DriveBase extends Subsystem {
+  CANTalon leftMotorOne = new CANTalon(RobotMap.driveLeftMotorOne);
+  CANTalon leftMotorTwo = new CANTalon(RobotMap.driveLeftMotorTwo);
+  CANTalon rightMotorOne = new CANTalon(RobotMap.driveRightMotorOne);
+  CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo);
+  Encoder leftEncoder = new Encoder(RobotMap.driveLeftEncoderA, RobotMap.driveLeftEncoderB, false, Encoder.EncodingType.k4X);
+  AnalogGyro gyro = new AnalogGyro(RobotMap.gyro);
+  PIDController gyroController = new PIDController(0.07, 0.002, 0.08, new GyroPIDSource(), new MotorsPIDOutputReversed());
+  PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
+  double encoderGyroCorrection = 1;
+  boolean driveEnabled = true;
+  public void initDefaultCommand() {
+    setDefaultCommand(new Drive());
+  }
+  public void stopDriving() {
+    leftMotorOne.set(0);
+    leftMotorTwo.set(0);
+    rightMotorOne.set(0);
+    rightMotorTwo.set(0);
+  }
+  public void driveArcade(double x, double y, double z, double sensitivity) {
+    if(driveEnabled){
+      leftMotorOne.set((-y + (x + z)) * sensitivity);
+      leftMotorTwo.set((-y + (x + z)) * sensitivity);
+      rightMotorOne.set((y + (x + z)) * sensitivity);
+      rightMotorTwo.set((y + (x + z)) * sensitivity);
+    }
+  }
+  public void driveStraight(double y, double correction) {
+    SmartDashboard.putNumber("GyroAngle", gyro.getAngle());
+    leftMotorOne.set((-y + -gyro.getAngle() * correction));
+    leftMotorTwo.set((-y + -gyro.getAngle() * correction));
+    rightMotorOne.set((y + -gyro.getAngle() * correction));
+    rightMotorTwo.set((y + -gyro.getAngle() * correction));
+  }
+  public void pidDrive(double setpoint, double correction) {
+    leftEncoder.reset();
+    leftEncoderController.enable();
+    leftEncoderController.setSetpoint(setpoint);
+    encoderGyroCorrection = -gyro.getAngle() * correction;
+  }
+  public void rotateAngle(double angle) {
+    SmartDashboard.putNumber("GyroAngle", gyro.getAngle());
+    gyroController.enable();
+    gyroController.setSetpoint(angle);
+  }
+  public void setDriveEnabled(boolean enabled){
+    driveEnabled = enabled;
+  }
+  public PIDController getLeftController() {
+    return leftEncoderController;
+  }
+  public PIDController getGyroController() {
+    return gyroController;
+  }
+  public double getLeftRotations() {
+    return leftEncoder.get();
+  }
+  public void resetGyro() {
+    gyro.reset();
+  }
+  public Encoder getLeftEncoder() {
+    return leftEncoder;
+  }
+  public AnalogGyro getGyro() {
+    return gyro;
+  }
+  public class MotorsPIDOutput implements PIDOutput {
+    @Override
+    public void pidWrite(double output) {
+      leftMotorOne.pidWrite(output + -encoderGyroCorrection);
+      leftMotorTwo.pidWrite(output + -encoderGyroCorrection);
+      rightMotorOne.pidWrite(-output + -encoderGyroCorrection);
+      rightMotorTwo.pidWrite(-output + -encoderGyroCorrection);
+    }
+  }
+  public class MotorsPIDOutputReversed implements PIDOutput {
+    @Override
+    public void pidWrite(double output) {
+      leftMotorOne.pidWrite(output);
+      leftMotorTwo.pidWrite(output);
+      rightMotorOne.pidWrite(output);
+      rightMotorTwo.pidWrite(output);
+    }
+  }
+  public class GyroPIDSource implements PIDSource {
+    public double pidGet() {
+      return gyro.getAngle();
+    }
+    public PIDSourceType getPIDSourceType() {
+      return PIDSourceType.kDisplacement;
+    }
+    public void setPIDSourceType(PIDSourceType type) {
+    }
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/subsystems/Drivebase.java

@@ -1,21 +0,0 @@
-package org.usfirst.frc.team2059.robot.subsystems;
-import org.usfirst.frc.team2059.robot.RobotMap;
-import org.usfirst.frc.team2059.robot.commands.Drive;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.CANTalon;
-public class Drivebase extends Subsystem {
-  CANTalon leftMotorOne = new CANTalon(RobotMap.driveLeftMotorOne);
-  CANTalon leftMotorTwo = new CANTalon(RobotMap.driveLeftMotorTwo);
-  CANTalon rightMotorOne = new CANTalon(RobotMap.driveRightMotorOne);
-  CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorOne);
-  public void initDefaultCommand() {
-    setDefaultCommand(new Drive());
-  }
-  public void driveArcade(double x, double y, double z, double sensitivity) {
-    leftMotorOne.set(-y + (x + z));
-    leftMotorTwo.set(-y + (x + z));
-    rightMotorOne.set(y + (x + z));
-    rightMotorTwo.set(y + (x + z));
-  }
-}
-// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/subsystems/EncoderBase.java

@@ -0,0 +1,33 @@
+package org.usfirst.frc.team2059.robot.subsystems;
+import org.usfirst.frc.team2059.robot.RobotMap;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.Encoder;
+import org.usfirst.frc.team2059.robot.structs.EncoderValues;
+public class EncoderBase extends Subsystem {
+  //TODO: Might not need a constructor
+  public EncoderBase() {
+  }
+  //Encoder enc = new Encoder(0, 1, false, Encoder.EncodingType.k1X);
+  Encoder enc = new Encoder(8, 9);
+  public void initDefaultCommand() {
+    //TODO: Not sure if we need a default command, not setting one
+    //TODO: These are just defaults, they wil lneed to be changed
+    //enc.setMaxPeriod(.1);
+    //enc.setMinRate(10);
+    //enc.setDistancePerPulse(5);
+    //enc.setReverseDirection(false);
+    //enc.setSamplesToAverage(7);
+  }
+  public void resetEncoder() {
+    enc.reset();
+  }
+  public EncoderValues getValues() {
+    //TODO: There are two ways to get distance:
+    //enc.getDistance();
+    //enc.getRaw();
+    //figure out which to use
+    return new EncoderValues(enc.get() , enc.getRaw() , enc.getPeriod() , enc.getRate() , enc.getDirection() , enc.getStopped());
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java

@@ -0,0 +1,104 @@
+package org.usfirst.frc.team2059.robot.subsystems;
+import org.usfirst.frc.team2059.robot.RobotMap;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.CANTalon;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+public class MainArm extends PIDSubsystem {
+  CANTalon armMotorLeft = new CANTalon(RobotMap.armLeftMotor);
+  CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor);
+  AnalogInput pot = new AnalogInput(RobotMap.armPot);
+  AnalogInput sonar = new AnalogInput(RobotMap.sonar);
+  DigitalInput limitSwitchBottom = new DigitalInput(RobotMap.armBottomLim);
+  DigitalInput limitSwitchTop = new DigitalInput(RobotMap.armTopLim);
+  private double min = RobotMap.zeroDegrees;
+  private double max = RobotMap.ninetyDegrees;
+  public MainArm() {
+    super("MainArm", 0.06, 0.0, 0.002);
+    getPIDController().setContinuous(false);
+    setSetpoint(70);
+//    enable();
+    disable();
+  }
+  public void initDefaultCommand() {
+  }
+  public void moveArm(double speed) {
+    // Calibrate the arm, but don't do anything about it
+    calibrate();
+    System.out.println(getTopPressed());
+    armMotorLeft.set(-speed);
+    armMotorRight.set(speed);
+  }
+  protected double returnPIDInput() {
+    return getDegrees();
+  }
+  protected void usePIDOutput(double output) {
+    moveArm(output);
+  }
+  public double getRaw() {
+    return pot.getAverageVoltage();
+  }
+  public double getDegrees() {
+    return potToDegrees(getRaw());
+  }
+  public void resetLower(double speed) {
+    if (getBottomPressed()) {
+      moveArm(0);
+      return;
+    } else {
+      moveArm(speed);
+    }
+  }
+  public boolean getBottomPressed() {
+    return !limitSwitchBottom.get();
+  }
+  public void resetUpper(double speed) {
+    if (getTopPressed()) {
+      moveArm(0);
+      return;
+    } else {
+      moveArm(speed);
+    }
+  }
+  public boolean getTopPressed() {
+    return !limitSwitchTop.get();
+  }
+  private double potToDegrees(double pot) {
+    //System.out.println((pot - min) / (Math.abs(min - max) / 90));
+    return (pot - min) / (Math.abs(min - max) / 90);
+  }
+  /**
+   * Calibrates the arm if it hits the upper or lower limit switch
+   * @return True if it was calibrated (if either limit switch was pressed)
+   */
+  private boolean calibrate() {
+    // It can't be calibrated if the limit swithces are disabled
+    //if (!SmartDashboard.getBoolean("UseLimitSwitches")) {
+    //  return false;
+    //}
+    if (getBottomPressed()) {
+      System.out.println("Calibrating bottom to: " + getRaw());
+      min = getRaw();
+      return true;
+    }
+ //   } else if (getTopPressed()) {
+ //     System.out.println("Calibrating top to: " + getRaw());
+ //     max = getRaw();
+ //     return true;
+ //   }
+    return false;
+  }
+  public double getShooterAngleError(){
+    double correctedHeight = (RobotMap.goalHeight + 16) - 44; //44 is the distance between the ground and the sonar
+    return (180/Math.PI) * Math.atan(correctedHeight/(getSonarDistance()-1));
+  }
+  public double getSonarDistanceRaw(){
+     return sonar.getVoltage();
+  }
+  public double getSonarDistance(){
+     return (getSonarDistanceRaw()/0.000976562f)/25.4f;
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/subsystems/Pneumatics.java

@@ -0,0 +1,43 @@
+package org.usfirst.frc.team2059.robot.subsystems;
+import org.usfirst.frc.team2059.robot.RobotMap;
+import org.usfirst.frc.team2059.robot.commands.drivetrain.Drive;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.Compressor;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+public class Pneumatics extends Subsystem {
+  Compressor compressor = new Compressor(RobotMap.pcmID);
+  DoubleSolenoid shooterSolenoid = new DoubleSolenoid(RobotMap.pcmID, RobotMap.shooterSolenoidOne, RobotMap.shooterSolenoidTwo);
+  DoubleSolenoid armStopSolenoid = new DoubleSolenoid(RobotMap.pcmID, RobotMap.armStopSolenoidOne, RobotMap.armStopSolenoidTwo);
+  boolean shooterState, armStopState;
+  public void initDefaultCommand() {
+  }
+  public void setCompressorEnabled(boolean state) {
+    compressor.setClosedLoopControl(state);
+  }
+  public void setShooterState(boolean state) {
+    if (state) {
+      shooterSolenoid.set(DoubleSolenoid.Value.kForward);
+    } else {
+      shooterSolenoid.set(DoubleSolenoid.Value.kReverse);
+    }
+    shooterState = state;
+  }
+  public void setArmStopState(boolean state) {
+    if (state) {
+      armStopSolenoid.set(DoubleSolenoid.Value.kForward);
+    } else {
+      armStopSolenoid.set(DoubleSolenoid.Value.kReverse);
+    }
+    armStopState = state;
+  }
+  public boolean getShooterState() {
+    return shooterState;
+  }
+  public boolean getArmStopState() {
+    return armStopState;
+  }
+  public boolean getCompressorEnabled() {
+    return compressor.enabled();
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/subsystems/Shooter.java

@@ -0,0 +1,15 @@
+package org.usfirst.frc.team2059.robot.subsystems;
+import org.usfirst.frc.team2059.robot.RobotMap;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.CANTalon;
+public class Shooter extends Subsystem {
+  CANTalon shooterMotorLeft = new CANTalon(RobotMap.shooterLeftMotor);
+  CANTalon shooterMotorRight = new CANTalon(RobotMap.shooterRightMotor);
+  public void initDefaultCommand() {
+  }
+  public void shootAtSpeed(double speed) {
+    shooterMotorRight.set(speed);
+    shooterMotorLeft.set(speed);
+  }
+}
+// vim: sw=2:ts=2:sts=2

src/org/usfirst/frc/team2059/robot/subsystems/VisionHelper.java

@@ -0,0 +1,95 @@
+package org.usfirst.frc.team2059.robot.subsystems;
+import org.usfirst.frc.team2059.robot.RobotMap;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.CANTalon;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+public class VisionHelper {
+  NetworkTable linesTable, contoursTable;
+  double imageWidth = 640;
+  double imageHeight = 480;
+  public VisionHelper() {
+    linesTable = NetworkTable.getTable("GRIP/lines");
+    contoursTable = NetworkTable.getTable("GRIP/contours");
+  }
+  public double getCenterContourX(){
+    int highestAreaIndex = 0;
+    int index = 0;
+    double areas[] = linesTable.getNumberArray("area", new double[0]);
+    for(double area : areas){
+       if(area >= areas[highestAreaIndex]){
+          highestAreaIndex=index;
+       }
+       index++;
+    }
+     try{
+        return contoursTable.getNumberArray("centerX",new double[0])[highestAreaIndex];
+     }catch(Exception e){
+        return 0;
+     }
+  }
+  public double getSlope(){
+    int highestLengthIndex = 0;
+    int index = 0;
+    double lengths[] = linesTable.getNumberArray("length", new double[0]);
+    try{
+    double x1s[] = linesTable.getNumberArray("x1", new double[0]);
+    double y1s[] = linesTable.getNumberArray("y1", new double[0]);
+    double x2s[] = linesTable.getNumberArray("x2", new double[0]);
+    double y2s[] = linesTable.getNumberArray("y2", new double[0]);
+    for(double length : lengths){
+       if(length >= lengths[highestLengthIndex]){
+          highestLengthIndex=index;
+       }
+       index++;
+    }
+    return ((y1s[highestLengthIndex]-y2s[highestLengthIndex])/(x1s[highestLengthIndex]-x2s[highestLengthIndex]));
+    }catch(Exception e){
+       return 0;
+    }
+  }
+  public double getCenterX() {
+    int highestLengthIndex = 0;
+    int index = 0;
+    double lengths[] = linesTable.getNumberArray("length", new double[0]);
+    double x1s[] = linesTable.getNumberArray("x1", new double[0]);
+    double x2s[] = linesTable.getNumberArray("x2", new double[0]);
+    for(double length : lengths){
+       if(length >= lengths[highestLengthIndex]){
+          highestLengthIndex=index;
+       }
+       index++;
+    }
+    try {
+      return (x1s[highestLengthIndex]+x2s[highestLengthIndex])/2;
+    } catch (Exception e) {
+      return 0;
+    }
+  }
+  public double getCenterY() {
+    int highestLengthIndex = 0;
+    int index = 0;
+    double lengths[] = linesTable.getNumberArray("length", new double[0]);
+    double y1s[] = linesTable.getNumberArray("y1", new double[0]);
+    double y2s[] = linesTable.getNumberArray("y2", new double[0]);
+    for(double length : lengths){
+       if(length >= lengths[highestLengthIndex]){
+          highestLengthIndex=index;
+       }
+       index++;
+    }
+    try {
+      return (y1s[highestLengthIndex]+y2s[highestLengthIndex])/2;
+    } catch (Exception e) {
+      return 0;
+    }
+  }
+  public double getHorizontalError() {
+    return ((180/Math.PI) * (Math.atan((getCenterContourX() - (RobotMap.imageWidth / 2)) / RobotMap.fWidth)))+3.5;
+  }
+  public double getVerticalError() {
+    return (180/Math.PI) * (Math.atan((getCenterY() - (RobotMap.imageHeight / 2)) / RobotMap.fHeight));
+  }
+}