Merge pull request #29 from pietroglyph/master

Fix throttle and straight driving. We're shipping!

src/main/java/com/spartronics4915/atlas/OI.java

@@ -153,4 +153,9 @@ private void initDrivetrainOI()
         // Initalize the drivetrain
     }
 
+    public static double getScaledThrottle()
+    {
+        return Math.max(Math.min(1-OI.sDriveStick.getZ(),1),0.4);
+    }
+
 }

src/main/java/com/spartronics4915/atlas/Robot.java

@@ -46,8 +46,7 @@ public void robotInit()
     @Override
     public void robotPeriodic()
     {
-        // This is invoked in both Autonomous and TeleOp, in
-        // addition to the autonomousPeriodic and TeleOp periodic.
+        SmartDashboard.putNumber("IMU Heading", mDrivetrain.getIMUHeading().getDegrees());
     }
 
     @Override

src/main/java/com/spartronics4915/atlas/commands/DriveStraightDrivetrain.java

@@ -18,9 +18,9 @@
 public class DriveStraightDrivetrain extends Command implements PIDSource, PIDOutput
 {
 
-    private static final double kP = 0.5;
-    private static final double kI = 0;
-    private static final double kD = 10;
+    private static final double kP = 0; // Was 0.4
+    private static final double kI = 0.2;
+    private static final double kD = 0;
     private static final double kF = 0;
     private static final double kAllowedError = 0; // In degrees
 
@@ -55,8 +55,7 @@ protected void initialize()
     protected void execute()
     {
         mPIDController.enable();
-
-        SmartDashboard.putNumber("IMU Heading", mDrivetrain.getIMUHeading().getDegrees());
+        SmartDashboard.putNumber("Error", mPIDController.getError());
     }
 
     @Override
@@ -119,6 +118,6 @@ public double pidGet()
     @Override
     public void pidWrite(double output)
     {
-        mDrivetrain.arcadeDrive(OI.sDriveStick.getZ(), output);
+        mDrivetrain.arcadeDrive(-1*OI.getScaledThrottle(), output);
     }
 }

src/main/java/com/spartronics4915/atlas/commands/QuickTurnDrivetrain.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * QuickTurnDrivetrain turns the drivetrain 180 degrees in a closed
@@ -17,7 +18,7 @@
 public class QuickTurnDrivetrain extends Command implements PIDSource, PIDOutput
 {
 
-    private static final double kP = 0.01;
+    private static final double kP = 0.02;
     private static final double kI = 0;
     private static final double kD = 0;
     private static final double kF = 0;

src/main/java/com/spartronics4915/atlas/commands/TeleopDrivetrain.java

@@ -31,7 +31,7 @@ protected void initialize()
     protected void execute()
     {
         // getZ is the throttle
-        mDrivetrain.arcadeDrive(OI.sDriveStick.getY()*Math.min(1-OI.sDriveStick.getZ(),1), OI.sDriveStick.getX()*-1); // Steering is reversed!
+        mDrivetrain.arcadeDrive(OI.sDriveStick.getY()*OI.getScaledThrottle(), OI.sDriveStick.getX()*-1); // Steering is reversed!
     }
 
     @Override

src/main/java/com/spartronics4915/atlas/subsystems/Drivetrain.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.Victor;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * The subsystem that controls the Drivetrain.
@@ -28,6 +29,9 @@ public class Drivetrain extends SpartronicsSubsystem
 
     private DifferentialDrive mDifferentialDrive;
 
+    // CTRE does a pass-by-reference thing, so we might as well not allocate a new array every time
+    private double[] yawPitchRoll = new double[3];
+
     // Motors
     public Victor mLeftMotor;
     public Victor mRightMotor;
@@ -58,6 +62,9 @@ private Drivetrain()
             //Initialize IMU
             mIMU = new PigeonIMU(RobotMap.kDriveTrainIMUID);
 
+            // Do a song and dance to make the IMU work
+            mIMU.clearStickyFaults(0); // 0 timeout means no timeout
+
             // Setup the differential drive
             mDifferentialDrive = new DifferentialDrive(mLeftMotor, mRightMotor);
 
@@ -87,9 +94,9 @@ public void stop()
 
     public Rotation2d getIMUHeading()
     {
-        double[] ypr = new double[3];
-        mIMU.getYawPitchRoll(ypr);
-        return Rotation2d.fromDegrees(ypr[0]);
+        mIMU.getYawPitchRoll(yawPitchRoll);
+        SmartDashboard.putString("YawPitchRoll", "[0]: "+yawPitchRoll[0]+"; [1]: "+yawPitchRoll[1]+"; [2]: "+yawPitchRoll[2]+"; Fused: "+mIMU.getFusedHeading());
+        return Rotation2d.fromDegrees(yawPitchRoll[0]);
     }
 
     public void arcadeDrive(double speed, double rotation)