added basic gyro auto

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

@@ -27,7 +27,8 @@ public class Robot extends IterativeRobot {
     SmartDashboard.putData("Auto mode", chooser);
     SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController());
     SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController());
-    SmartDashboard.putNumber("GyroCorrection",0.0);
+    SmartDashboard.putData("gyroPID", CommandBase.driveBase.getGyroController());
+    SmartDashboard.putNumber("GyroCorrection",0.187);
     SmartDashboard.putBoolean("CompressorEnabled", true);
     //Automatically determine if rolling in or rolling out
     SmartDashboard.putBoolean("SmartRollers", false);

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

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

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

@@ -5,6 +5,8 @@ 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;
@@ -15,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.0,0.0,0.0, gyro, new MotorsPIDOutput());
+  PIDController gyroController = new PIDController(0.082,0.002,0.07, new GyroPIDSource(), new MotorsPIDOutputReversed());
   PIDController leftEncoderController = new PIDController(0.02, 0.002, 0.017, leftEncoder, new MotorsPIDOutput());
   public void initDefaultCommand() {
     setDefaultCommand(new Drive());
@@ -34,25 +36,36 @@ public class DriveBase extends Subsystem {
   }
   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));
+    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) {
     leftEncoder.reset();
     leftEncoderController.enable();
     leftEncoderController.setSetpoint(setpoint);
   }
+  public void rotateAngle(double angle){
+    SmartDashboard.putNumber("GyroAngle",gyro.getAngle());
+    gyroController.enable();
+    gyroController.setSetpoint(angle);
+  }
   public PIDController getLeftController() {
     return leftEncoderController;
   }
+  public PIDController getGyroController(){
+    return gyroController;
+  }
   public double getLeftRotations() {
     return leftEncoder.get();
   }
   public void resetGyro(){
     gyro.reset();
   }
+  public AnalogGyro getGyro(){
+    return gyro;
+  }
   public class MotorsPIDOutput implements PIDOutput {
     @Override
     public void pidWrite(double output) {
@@ -62,5 +75,24 @@ public class DriveBase extends Subsystem {
       rightMotorTwo.pidWrite(-output);
     }
   }
+  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