updated auto and buttons

.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="var" path="networktables" sourcepath="networktables.sources"/>
-  <classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.8"/>
+  <classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER"/>
+  <classpathentry kind="lib" path="~/wpilib/java/current/lib/WPILib.jar"/>
+  <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,60 @@
 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.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.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
-  // One type of button is a joystick button which is any button on a joystick.
-  // You create one by telling it which joystick it's on and which button
-  // number it is.
-  // Joystick stick = new Joystick(port);
-  // Button button = new JoystickButton(stick, buttonNumber);
-  // There are a few additional built in buttons you can use. Additionally,
-  // by subclassing Button you can create custom triggers and bind those to
-  // commands the same as any other Button.
-  //// TRIGGERING COMMANDS WITH BUTTONS
-  // Once you have a button, it's trivial to bind it to a button in one of
-  // three ways:
-  // 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.
-  // button.whenPressed(new ExampleCommand());
-  // Run the command while the button is being held down and interrupt it once
-  // the button is released.
-  // button.whileHeld(new ExampleCommand());
-  // Start the command when the button is released  and let it run the command
-  // until it is finished as determined by it's isFinished method.
-  // button.whenReleased(new ExampleCommand());
+  JoystickButton[][] joystickButtons;
+  Joystick[] joysticks;
+  public OI() {
+    joysticks = new Joystick[2];
+    joystickButtons = new JoystickButton[2][13];
+    // Create joysticks
+    joysticks[0] = new Joystick(0);
+    joysticks[1] = new Joystick(1);
+    // Create buttons
+    for (int i = 0; i < 12; i++) {
+      joystickButtons[0][i] = new JoystickButton(joysticks[0], i + 1);
+      joystickButtons[1][i] = new JoystickButton(joysticks[1], i + 1);
+    }
+    // Print log when button 1 pressed
+    //joystickButtons[0][0].whenPressed(new LogEncoder());
+    joystickButtons[0][0].whileHeld(new SetShooterState(true));
+    joystickButtons[0][3].whileHeld(new SpinRollers(1));
+//    joystickButtons[0][2].whileHeld(new PIDDrive(400));
+    joystickButtons[1][0].whileHeld(new MoveArm(1));
+    joystickButtons[1][1].whileHeld(new MoveArm(-1));
+    joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect));
+    joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse));
+    joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot));
+    joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetFarShot));
+    joystickButtons[1][7].whileHeld(new SetArmStopState(true));
+//    joystickButtons[1][10].whileHeld(new ResetLower(-1));
+//    joystickButtons[1][11].whileHeld(new ResetUpper(1));
+  }
+  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,93 +1,65 @@
 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.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;
-  /**
-   * 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());
+    chooser.addDefault("Time based drive", new RoutineDriveTime());
+    chooser.addObject("Time based defense", new RoutineDefenseTime());
     SmartDashboard.putData("Auto mode", chooser);
+    SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());
+    SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController());
+    SmartDashboard.putBoolean("CompressorEnabled", true);
+    //Automatically determine if rolling in or rolling out
+    SmartDashboard.putBoolean("SmartRollers", true);
+    //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());
   }
   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(true);
   }
-  /**
-   * This function is called periodically during operator control
-   */
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+    SmartDashboard.putNumber("ArmAngleRaw", CommandBase.mainArm.getRaw());
+    SmartDashboard.putNumber("ArmAngleDegrees", CommandBase.mainArm.getDegrees());
+    SmartDashboard.putNumber("tmpRotations", CommandBase.driveBase.getLeftRotations());
+    CommandBase.pneumatics.setCompressorEnabled(SmartDashboard.getBoolean("CompressorEnabled"));
+    System.out.println(CommandBase.mainArm.getDegrees());
   }
-  /**
-   * This function is called periodically during test mode
-   */
   public void testPeriodic() {
     LiveWindow.run();
   }
 }
-// vim: sw=2:ts=2:sts=2
+//  vim: sw=2:ts=2:sts=2

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

@@ -3,16 +3,18 @@ public class RobotMap {
   //Drive
   public static int driveLeftMotorOne = 1;
   public static int driveLeftMotorTwo = 2;
-  public static int driveRightMotorOne = 1;
-  public static int driveRightMotorTwo = 2;
-  public static int driveRightEncoder = 0;
-  public static int driveLeftEncoder = 1;
+  public static int driveRightMotorOne = 3;
+  public static int driveRightMotorTwo = 4;
+  public static int driveLeftEncoderA = 0;
+  public static int driveLeftEncoderB = 1;
   //Arm
-  public static double zeroDegrees = 0.1;
-  public static double ninetyDegrees = 0.7;
+  public static double zeroDegrees = 1.622;
+  public static double ninetyDegrees = 3.234;
   public static int armPot = 0;
   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;
@@ -25,9 +27,9 @@ public class RobotMap {
   public static int armStopSolenoidOne = 4;
   public static int armStopSolenoidTwo = 5;
   //Misc
-  public static int mainArmPresetCollect = 0;
-  public static int mainArmPresetTraverse = 5;
-  public static int mainArmPresetCloseShot = 95;
-  public static int mainArmPresetFarShot = 85;
+  public static int mainArmPresetCollect = -5;
+  public static int mainArmPresetTraverse = 11;
+  public static int mainArmPresetCloseShot = 90;
+  public static int mainArmPresetFarShot = 70;
 }
-// vim: sw=2:ts=2:sts=2
+// vim: sw=2:ts=2:sts=2

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

@@ -0,0 +1,22 @@
+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 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 void init() {
+    encoderBase = new EncoderBase();
+    driveBase = new DriveBase();
+    mainArm = new MainArm();
+    pneumatics = new Pneumatics();
+    shooter = new Shooter();
+  }
+}
+// vim: sw=2:ts=2:sts=2

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

@@ -0,0 +1,39 @@
+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 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);
+    }
+    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/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, 0);
+  }
+  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/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/AutoSetArmPosition.java

@@ -0,0 +1,42 @@
+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;
+  }
+  // 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;
+  }
+  // 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/RoutineDefenseTime.java

@@ -0,0 +1,14 @@
+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(true));
+    addSequential(new AutoResetLower(-1));
+    addSequential(new AutoSetArmPosition(15));
+    addSequential(new AutoSetArmStopState(false));
+    addSequential(new AutoSetArmPosition(11));
+//      addSequential(new AutoDriveTime(2,.5));
+  }
+}

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 AutoResetLower(-1));
+    addSequential(new AutoSetArmStopState(true));
+    addSequential(new AutoDriveTime(2, .5));
+  }
+}

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)){
+      sensitivity = 1;
+    }else{
+      sensitivity = 0.5;
+    }
+    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/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

@@ -1,16 +1,21 @@
-package org.usfirst.frc.team2059.robot.commands;
-import edu.wpi.first.wpilibj.command.Command;
+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 Command {
-  public Drive() {
+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);
   }
   // Make this return true when this Command no longer needs to run execute()
   protected boolean isFinished() {
@@ -24,4 +29,4 @@ public class Drive extends Command {
   protected void interrupted() {
   }
 }
-// vim: sw=2:ts=2:sts=2
+// 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/MoveArm.java

@@ -0,0 +1,43 @@
+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
+    if (SmartDashboard.getBoolean("UseLimitSwitches")) {
+      if (mainArm.getTopPressed() || mainArm.getBottomPressed()) {
+        return true;
+      }
+    }
+    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,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 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() {
+    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 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/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,43 @@
+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
+    if (pos == RobotMap.mainArmPresetCollect) {
+      pneumatics.setArmStopState(true);
+    } else if ((pos != RobotMap.mainArmPresetCollect) && (mainArm.getDegrees() > 2)) {
+      pneumatics.setArmStopState(false);
+    }
+    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

@@ -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 SetArmStopState extends CommandBase {
+  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() {
+    return false;
+  }
+  // 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,38 @@
+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;
+  public SpinRollers(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() {
+    if (SmartDashboard.getBoolean("SmartRollers") && mainArm.getDegrees() < 5) {
+      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/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,52 @@
+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.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+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.k2X);
+  PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
+  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) {
+    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 pidDrive(double setpoint) {
+    leftEncoder.reset();
+    leftEncoderController.enable();
+    leftEncoderController.setSetpoint(setpoint);
+  }
+  public PIDController getLeftController() {
+    return leftEncoderController;
+  }
+  public double getLeftRotations() {
+    return leftEncoder.get();
+  }
+  public class MotorsPIDOutput implements PIDOutput {
+    @Override
+    public void pidWrite(double output) {
+      leftMotorOne.pidWrite(output);
+      leftMotorTwo.pidWrite(output);
+      rightMotorOne.pidWrite(-output);
+      rightMotorTwo.pidWrite(-output);
+    }
+  }
+}
+// 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,89 @@
+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);
+  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.3, 0.0, 0.4);
+    getPIDController().setContinuous(false);
+    enable();
+  }
+  public void initDefaultCommand() {
+  }
+  public void moveArm(double speed) {
+    // Calibrate the arm, but don't do anything about it
+    calibrate();
+    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;
+  }
+}
+// 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