added more autos

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

@@ -23,6 +23,7 @@ public class Robot extends IterativeRobot {
     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());

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

@@ -9,8 +9,8 @@ public class RobotMap {
   public static int driveLeftEncoderB = 1;
   public static int gyro = 1;
   //Arm
-  public static double zeroDegrees = 1.520;
+  public static double zeroDegrees = 1.469;
-  public static double ninetyDegrees = 3.335;
+  public static double ninetyDegrees = 3.274;
   public static int armPot = 0;
   public static int armLeftMotor = 5;
   public static int armRightMotor = 6;

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

@@ -1,6 +1,7 @@
 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
@@ -21,7 +22,7 @@ public class AutoDrive extends CommandBase {
   }
   protected void execute() {
     if (startDriving) {
-      driveBase.pidDrive(distance);
+      driveBase.pidDrive(distance,SmartDashboard.getNumber("GyroCorrection"));
     }
     startDriving = false;
   }

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/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/drivetrain/PIDDrive.java

@@ -1,5 +1,6 @@
 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;
 /**
  *
@@ -15,7 +16,7 @@ public class PIDDrive extends CommandBase {
   }
   // Called repeatedly when this Command is scheduled to run
   protected void execute() {
-    driveBase.pidDrive(count);
+    driveBase.pidDrive(count,SmartDashboard.getNumber("GyroCorrection"));
   }
   // Make this return true when this Command no longer needs to run execute()
   protected boolean isFinished() {

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

@@ -21,7 +21,7 @@ public class ResetLower extends CommandBase {
   // 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();
+    return mainArm.getBottomPressed();
   }
   // Called once after isFinished returns true
   protected void end() {

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

@@ -15,10 +15,11 @@ public class DriveBase extends Subsystem {
   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);
+  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;
   public void initDefaultCommand() {
     setDefaultCommand(new Drive());
   }
@@ -41,10 +42,11 @@ public class DriveBase extends Subsystem {
     rightMotorOne.set((y + -gyro.getAngle()*correction));
     rightMotorTwo.set((y + -gyro.getAngle()*correction));
   }
-  public void pidDrive(double setpoint) {
+  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());
@@ -63,16 +65,19 @@ public class DriveBase extends Subsystem {
   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);
+      leftMotorOne.pidWrite(output + -encoderGyroCorrection);
-      leftMotorTwo.pidWrite(output);
+      leftMotorTwo.pidWrite(output + -encoderGyroCorrection);
-      rightMotorOne.pidWrite(-output);
+      rightMotorOne.pidWrite(-output + -encoderGyroCorrection);
-      rightMotorTwo.pidWrite(-output);
+      rightMotorTwo.pidWrite(-output + -encoderGyroCorrection);
     }
   }
   public class MotorsPIDOutputReversed implements PIDOutput {

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

@@ -79,11 +79,12 @@ public class MainArm extends PIDSubsystem {
       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;
     }
+//    } else if (getTopPressed()) {
+//      System.out.println("Calibrating top to: " + getRaw());
+//      max = getRaw();
+//      return true;
+//    }
     return false;
   }
 }