Merge pull request #98 from strykeforce/profiledPIDAutoPlace

Final Drive Practice Fixes for Lakeview

Author: mwitcpalek

src/main/java/frc/robot/Constants.java

@@ -221,7 +221,7 @@ public static Translation2d[] getWheelLocationMeters() {
 
     public static final double kAutoBalanceSlowDriveVel = 0.6; // 0.5
     public static final double kAutoBalanceFinalDriveVel = 1.0; // 0.5 0.75
-    public static final double kAutoBalanceSlowdownTimeSec = 1.15; // 2.3  1.6 1.3
+    public static final double kAutoBalanceSlowdownTimeSec = 1.0; // 1.3-> 1.15
     public static final double kAutoBalanceStopThresholdDegrees = 1.5; // 1 0.6 1.5
     public static final double kAutoBalanceEdgeTriggerThreshold = 3; // 5
     public static final double kAutoBalanceAvgRollCount = 7; // 5 10 7

src/main/java/frc/robot/RobotContainer.java

@@ -571,6 +571,10 @@ private void configureMatchDashboard() {
         .addBoolean("Is Elbow Ok?", () -> armSubsystem.isElbowOk())
         .withSize(1, 1)
         .withPosition(8, 0);
+    Shuffleboard.getTab("Match")
+        .addBoolean("Is Navx Connected", () -> driveSubsystem.isNavxWorking())
+        .withSize(1, 1)
+        .withPosition(8, 1);
   }
 
   private void configurePitDashboard() {

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -190,7 +190,7 @@ public DriveSubsystem(Constants constants) {
 
       swerveModules[i].loadAndSetAzimuthZeroReference();
     }
-    ahrs = new AHRS(SerialPort.Port.kUSB1, SerialDataType.kProcessedData, (byte) 200);
+    ahrs = new AHRS(SerialPort.Port.kUSB, SerialDataType.kProcessedData, (byte) 200);
     swerveDrive = new SwerveDrive(ahrs, swerveModules);
     swerveDrive.resetGyro();
     swerveDrive.setGyroOffset(Rotation2d.fromDegrees(0));
@@ -743,6 +743,10 @@ public double getGyroPitch() {
     return ahrs.getPitch();
   }
 
+  public boolean isNavxWorking() {
+    return ahrs.isConnected();
+  }
+
   public void lockZero() {
     SwerveModule[] swerveModules = swerveDrive.getSwerveModules();
     for (int i = 0; i < 4; i++) {