added more autos that require a gyro

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

@@ -24,11 +24,13 @@ public class Robot extends IterativeRobot {
     chooser.addObject("Time based low bar", new RoutineDriveTime());
     chooser.addObject("Time based straight low bar", new RoutineDriveStraightTime());
     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.187);
+    SmartDashboard.putNumber("GyroCorrection",0.13);
     SmartDashboard.putBoolean("CompressorEnabled", true);
     //Automatically determine if rolling in or rolling out
     SmartDashboard.putBoolean("SmartRollers", false);

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

@@ -10,7 +10,7 @@ public class RobotMap {
   public static int gyro = 1;
   //Arm
   public static double zeroDegrees = 1.520;
-  public static double ninetyDegrees = 3.419;
+  public static double ninetyDegrees = 3.335;
   public static int armPot = 0;
   public static int armLeftMotor = 5;
   public static int armRightMotor = 6;

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

@@ -3,15 +3,17 @@ 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 {
-  public AutoDriveStraightTime(double time) {
+  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(-0.5,SmartDashboard.getNumber("GyroCorrection"));
+    driveBase.driveStraight(-speed,SmartDashboard.getNumber("GyroCorrection"));
   }
   protected void end() {
     driveBase.stopDriving();

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/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/RoutineDriveStraightTime.java

@@ -6,9 +6,6 @@ public class RoutineDriveStraightTime extends CommandGroup {
   public RoutineDriveStraightTime() {
     addSequential(new AutoSetArmStopState(true));
     addSequential(new AutoResetLower(-1));
-    addSequential(new AutoDriveStraightTime(1));
-    addSequential(new AutoRotate(45));
-    addParallel(new AutoSetArmPosition(90));
-    addSequential(new AutoDriveStraightTime(1.5));
+    addSequential(new AutoDriveStraightTime(0.5,1));
   }
 }

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

@@ -0,0 +1,13 @@
+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,20 @@
+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));
+    addParallel(new AutoSetArmPosition(90));
+    addSequential(new AutoRotate(45));
+    addSequential(new AutoDriveStraightTime(0.4,1.5));
+    addSequential(new AutoSpinRollers(1,2));
+    addSequential(new AutoSetShooterState(true));
+    addSequential(new AutoSetArmStopState(false));
+    addParallel(new AutoSetArmPosition(10));
+    addSequential(new AutoRotate(180));
+    addSequential(new AutoSetShooterState(false));
+  }
+}

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/subsystems/DriveBase.java

@@ -17,7 +17,7 @@ public class DriveBase extends Subsystem {
   CANTalon rightMotorTwo = new CANTalon(RobotMap.driveRightMotorTwo);
   Encoder leftEncoder = new Encoder(RobotMap.driveLeftEncoderA, RobotMap.driveLeftEncoderB, false, Encoder.EncodingType.k2X);
   AnalogGyro gyro = new AnalogGyro(RobotMap.gyro);
-  PIDController gyroController = new PIDController(0.082,0.002,0.07, new GyroPIDSource(), new MotorsPIDOutputReversed());
+  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());
   public void initDefaultCommand() {
     setDefaultCommand(new Drive());