added basic pid distance driving functionality

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

@@ -4,6 +4,7 @@ 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.LogEncoder;
+import org.usfirst.frc.team2059.robot.commands.PIDDrive;
 import org.usfirst.frc.team2059.robot.commands.MoveArm;
 import org.usfirst.frc.team2059.robot.commands.SetArmPosition;
 import org.usfirst.frc.team2059.robot.commands.SetShooterState;
@@ -33,6 +34,7 @@ public class OI {
     //joystickButtons[0][0].whenPressed(new LogEncoder());
     joystickButtons[0][0].whileHeld(new SpinRollers(1));
     joystickButtons[0][1].whileHeld(new SetShooterState(true));
+//    joystickButtons[0][2].whileHeld(new PIDDrive(400));
 
     joystickButtons[1][0].whileHeld(new MoveArm(0.5));
     joystickButtons[1][1].whileHeld(new MoveArm(-0.5));

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

@@ -16,6 +16,7 @@ public class Robot extends IterativeRobot {
     chooser = new SendableChooser();
     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);
@@ -45,6 +46,7 @@ public class Robot extends IterativeRobot {
     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());
   }

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

@@ -5,11 +5,11 @@ public class RobotMap {
   public static int driveLeftMotorTwo = 2;
   public static int driveRightMotorOne = 3;
   public static int driveRightMotorTwo = 4;
-  public static int driveRightEncoder = 0;
-  public static int driveLeftEncoder = 1;
+  public static int driveLeftEncoderA = 0;
+  public static int driveLeftEncoderB = 1;
   //Arm
-  public static double zeroDegrees = 2.08;
-  public static double ninetyDegrees = 3.54;
+  public static double zeroDegrees = 2.019;
+  public static double ninetyDegrees = 3.512;
   public static int armPot = 0;
   public static int armLeftMotor = 5;
   public static int armRightMotor = 6;
@@ -25,7 +25,7 @@ public class RobotMap {
   public static int armStopSolenoidOne = 4;
   public static int armStopSolenoidTwo = 5;
   //Misc
-  public static int mainArmPresetCollect = -3;
+  public static int mainArmPresetCollect = -5;
   public static int mainArmPresetTraverse = 11;
   public static int mainArmPresetCloseShot = 90;
   public static int mainArmPresetFarShot = 70;

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

@@ -0,0 +1,32 @@
+package org.usfirst.frc.team2059.robot.commands;
+import org.usfirst.frc.team2059.robot.commands.CommandBase;
+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);
+  }
+  // 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/SetArmPosition.java

@@ -19,7 +19,7 @@ public class SetArmPosition extends CommandBase {
     //Move the arm stop
     if(pos==RobotMap.mainArmPresetCollect){
       pneumatics.setArmStopState(true);
-    }else if((pos!=RobotMap.mainArmPresetCollect)&&(mainArm.getDegrees()>15)){
+    }else if((pos!=RobotMap.mainArmPresetCollect)&&(mainArm.getDegrees()>2)){
       pneumatics.setArmStopState(false);
     }
     mainArm.enable();

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

@@ -2,12 +2,19 @@ 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.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());
   }
@@ -17,5 +24,26 @@ public class DriveBase extends Subsystem {
     rightMotorOne.set(y + (x + z));
     rightMotorTwo.set(y + (x + z));
   }
+  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/EncoderBase.java

@@ -9,7 +9,7 @@ public class EncoderBase extends Subsystem {
   public EncoderBase() {
   }
   //Encoder enc = new Encoder(0, 1, false, Encoder.EncodingType.k1X);
-  Encoder enc = new Encoder(0, 1);
+  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