Update

swervelib/SwerveDrive.java

@@ -101,6 +101,10 @@ public class SwerveDrive
    * Whether to correct heading when driving translationally. Set to true to enable.
    */
   public        boolean                  headingCorrection                               = false;
+  /**
+   * Amount of seconds the duration of the timestep the speeds should be applied for.
+   */
+  private        double                   discretizationdtSeconds                         = 0.02;
   /**
    * Deadband for speeds in heading correction.
    */
@@ -185,12 +189,12 @@ public class SwerveDrive
     setMaximumSpeed(maxSpeedMPS);
 
     // Initialize Telemetry
-    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
+    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
     {
       SmartDashboard.putData("Field", field);
     }
 
-    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
+    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
     {
       SwerveDriveTelemetry.maxSpeed = maxSpeedMPS;
       SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity;
@@ -342,6 +346,19 @@ public class SwerveDrive
     HEADING_CORRECTION_DEADBAND = deadband;
   }
 
+  /**
+   * Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same time. 
+   * The inputs are added together so this is not intneded to be used to give the driver both methods of control.
+   * 
+   * @param fieldOrientedVelocity The field oriented velocties to use
+   * @param robotOrientedVelocity The robot oriented velocties to use
+   */
+  public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity, ChassisSpeeds robotOrientedVelocity)
+  {
+    ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()).plus(robotOrientedVelocity);
+    drive(TotalVelocties);
+  }
+
   /**
    * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use.
    *
@@ -464,7 +481,7 @@ public class SwerveDrive
     // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
     if (chassisVelocityCorrection)
     {
-      velocity = ChassisSpeeds.discretize(velocity, 0.02);
+      velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
     }
 
     // Heading Angular Velocity Deadband, might make a configuration option later.
@@ -485,11 +502,11 @@ public class SwerveDrive
     }
 
     // Display commanded speed for testing
-    if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
+    if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO)
     {
       SmartDashboard.putString("RobotVelocity", velocity.toString());
     }
-    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
+    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
     {
       SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond;
       SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond;
@@ -502,7 +519,6 @@ public class SwerveDrive
     setRawModuleStates(swerveModuleStates, isOpenLoop);
   }
 
-
   /**
    * Set the maximum speeds for desaturation.
    *
@@ -660,7 +676,7 @@ public class SwerveDrive
    */
   public void postTrajectory(Trajectory trajectory)
   {
-    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
+    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
     {
       field.getObject("Trajectory").setTrajectory(trajectory);
     }
@@ -871,7 +887,7 @@ public class SwerveDrive
     {
       SwerveModuleState desiredState =
           new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle());
-      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
+      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
       {
         SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] =
             desiredState.angle.getDegrees();
@@ -932,7 +948,7 @@ public class SwerveDrive
       swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
 
       // Update angle accumulator if the robot is simulated
-      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
+      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
       {
         Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
         if (SwerveDriveTelemetry.isSimulation)
@@ -951,7 +967,7 @@ public class SwerveDrive
         SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees();
       }
 
-      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
+      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
       {
         field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
       }
@@ -967,7 +983,7 @@ public class SwerveDrive
           SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees());
           SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees());
         }
-        if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
+        if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
         {
           SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees();
           SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond;
@@ -983,7 +999,7 @@ public class SwerveDrive
         moduleSynchronizationCounter = 0;
       }
 
-      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
+      if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
       {
         SwerveDriveTelemetry.updateData();
       }
@@ -1122,11 +1138,11 @@ public class SwerveDrive
    * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the
    * internal offsets to prevent double offsetting.
    */
-  public void pushOffsetsToControllers()
+  public void pushOffsetsToEncoders()
   {
     for (SwerveModule module : swerveModules)
     {
-      module.pushOffsetsToControllers();
+      module.pushOffsetsToEncoders();
     }
   }
 
@@ -1156,4 +1172,15 @@ public class SwerveDrive
     }
   }
 
+  /**
+   * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction
+   * 
+   * @param enable
+   * @param dtSeconds
+   */
+  public void setChassisDiscretization(boolean enable, double dtSeconds){
+    chassisVelocityCorrection = enable;
+    discretizationdtSeconds = dtSeconds;
+  }
+
 }

swervelib/SwerveModule.java

@@ -74,6 +74,10 @@ public class SwerveModule
    * NT3 Raw drive motor.
    */
   private final String                 rawDriveName;
+  /**
+   * NT3 Raw drive motor.
+   */
+  private final String                 rawDriveVelName;
   /**
    * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
    */
@@ -207,6 +211,7 @@ public class SwerveModule
     absoluteEncoderIssueName = "Module[" + configuration.name + "] Absolute Encoder Read Issue";
     rawAngleName = "Module[" + configuration.name + "] Raw Angle Encoder";
     rawDriveName = "Module[" + configuration.name + "] Raw Drive Encoder";
+    rawDriveVelName = "Module[" + configuration.name + "] Raw Drive Velocity";
   }
 
   /**
@@ -252,7 +257,7 @@ public class SwerveModule
     this.antiJitterEnabled = antiJitter;
     if (antiJitter)
     {
-      pushOffsetsToControllers();
+      pushOffsetsToEncoders();
     } else
     {
       restoreInternalOffset();
@@ -366,7 +371,7 @@ public class SwerveModule
       simModule.updateStateAndPosition(desiredState);
     }
 
-    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal())
+    if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
     {
       SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees();
       SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity;
@@ -580,7 +585,7 @@ public class SwerveModule
   /**
    * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset.
    */
-  public void pushOffsetsToControllers()
+  public void pushOffsetsToEncoders()
   {
     if (absoluteEncoder != null && angleOffset == configuration.angleOffset)
     {
@@ -633,7 +638,7 @@ public class SwerveModule
     }
     SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition());
     SmartDashboard.putNumber(rawDriveName, driveMotor.getPosition());
-    SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition());
+    SmartDashboard.putNumber(rawDriveVelName, driveMotor.getVelocity()); SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition());
     SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0);
   }
 }

swervelib/encoders/PWMDutyCycleEncoderSwerve.java

@@ -26,10 +26,6 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
    * An {@link Alert}  for if the encoder cannot report accurate velocities.
    */
   private       Alert            inaccurateVelocities;
-    /**
-   * An {@link Alert}  for if the encoder is disconnected.
-   */
-  private       Alert            disconnected;
 
   /**
    * Constructor for the PWM duty cycle encoder.
@@ -43,10 +39,6 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
         "Encoders",
         "The PWM Duty Cycle encoder may not report accurate velocities!",
         Alert.AlertType.WARNING_TRACE);
-    inaccurateVelocities = new Alert(
-        "Encoders",
-        "The swerve encoder on port " + pin + "is disconnected!",
-        Alert.AlertType.ERROR_TRACE);
 
   }
 
@@ -69,7 +61,6 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
   @Override
   public double getAbsolutePosition()
   {
-    disconnected.set(!encoder.isConnected());
     return (isInverted ? -1.0 : 1.0) * encoder.getAbsolutePosition() * 360;
   }
 
@@ -92,7 +83,6 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
   @Override
   public double getVelocity()
   {
-    disconnected.set(!encoder.isConnected());
     inaccurateVelocities.set(true);
     return encoder.get();
   }

swervelib/encoders/SwerveAbsoluteEncoder.java

@@ -49,7 +49,7 @@ public abstract class SwerveAbsoluteEncoder
   /**
    * Sets the Absolute Encoder offset at the Encoder Level.
    *
-   * @param offset the offset the Absolute Encoder uses as the zero point.
+   * @param offset the offset the Absolute Encoder uses as the zero point in degrees.
    * @return if setting Absolute Encoder Offset was successful or not.
    */
   public abstract boolean setAbsoluteEncoderOffset(double offset);

swervelib/motors/SparkMaxSwerve.java

@@ -52,11 +52,6 @@ public class SparkMaxSwerve extends SwerveMotor
    */
   private Supplier<Double>      position;
 
-  /**
-   * An {@link Alert} for if there is an error configuring the motor.
-   */
-  private Alert              failureConfiguringAlert;
-
   /**
    * Initialize the swerve motor.
    *
@@ -79,9 +74,6 @@ public class SparkMaxSwerve extends SwerveMotor
     position = encoder::getPosition;
     // Spin off configurations in a different thread.
     // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
-    failureConfiguringAlert = new Alert("Motors",
-                                        "Failure configuring motor " + motor.getDeviceId(),
-                                        Alert.AlertType.WARNING_TRACE);
   }
 
   /**
@@ -109,7 +101,7 @@ public class SparkMaxSwerve extends SwerveMotor
         return;
       }
     }
-    failureConfiguringAlert.set(true);
+    DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
   }
 
   /**

swervelib/motors/TalonSRXSwerve.java

@@ -228,7 +228,6 @@ public class TalonSRXSwerve extends SwerveMotor
   @Override
   public void setInverted(boolean inverted)
   {
-    Timer.delay(1);
     motor.setInverted(inverted);
   }
 

swervelib/telemetry/SwerveDriveTelemetry.java

@@ -125,6 +125,14 @@ public class SwerveDriveTelemetry
      * Low telemetry data, only post the robot position on the field.
      */
     LOW,
+    /**
+     * Medium telemetry data, swerve directory
+     */
+    INFO,
+    /**
+     * Info level + field info
+     */
+    POSE,
     /**
      * Full swerve drive data is sent back in both human and machine readable forms.
      */