Merge pull request #195 from Technologyman00/dev-AddCommentsandMoveVa…

…riablesToConstants

Move Maximum Speed to Constants.java

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

@@ -24,6 +24,7 @@ public final class Constants
   public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound
   public static final Matter CHASSIS    = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS);
   public static final double LOOP_TIME  = 0.13; //s, 20ms + 110ms sprk max velocity lag
+  public static final double MAX_SPEED = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration.
 
   public static final class AutonConstants
   {

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

@@ -42,6 +42,15 @@ public RobotContainer()
     // Configure the trigger bindings
     configureBindings();
 
+
+
+    // Applies deadbands and inverts controls because joysticks
+    // are back-right positive while robot
+    // controls are front-left positive
+    // left stick controls translation
+    // right stick controls the rotational velocity 
+    // buttons are quick rotation positions to different ways to face
+    // WARNING: default buttons are on the same buttons as the ones defined in configureBindings
     AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase,
                                                                    () -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
                                                                                                 OperatorConstants.LEFT_Y_DEADBAND),

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -26,6 +26,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import frc.robot.Constants;
 import frc.robot.Constants.AutonConstants;
 import java.io.File;
 import java.util.function.DoubleSupplier;
@@ -48,10 +49,6 @@ public class SwerveSubsystem extends SubsystemBase
    * Swerve drive object.
    */
   private final SwerveDrive swerveDrive;
-  /**
-   * Maximum speed of the robot in meters per second, used to limit acceleration.
-   */
-  public        double      maximumSpeed = Units.feetToMeters(14.5);
 
   /**
    * Initialize {@link SwerveDrive} with the directory provided.
@@ -78,7 +75,7 @@ public SwerveSubsystem(File directory)
     SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
     try
     {
-      swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed);
+      swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED);
       // Alternative method if you don't want to supply the conversion factor via JSON files.
       // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor);
     } catch (Exception e)
@@ -98,7 +95,7 @@ public SwerveSubsystem(File directory)
    */
   public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg)
   {
-    swerveDrive = new SwerveDrive(driveCfg, controllerCfg, maximumSpeed);
+    swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED);
   }
 
   /**
@@ -516,7 +513,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headin
                                                         headingX,
                                                         headingY,
                                                         getHeading().getRadians(),
-                                                        maximumSpeed);
+                                                        Constants.MAX_SPEED);
   }
 
   /**
@@ -536,7 +533,7 @@ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d an
                                                         yInput,
                                                         angle.getRadians(),
                                                         getHeading().getRadians(),
-                                                        maximumSpeed);
+                                                        Constants.MAX_SPEED);
   }
 
   /**

src/main/java/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.
    */
@@ -342,6 +346,19 @@ public void setHeadingCorrection(boolean state, double deadband)
     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 void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
     // 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.
@@ -502,7 +519,6 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
     setRawModuleStates(swerveModuleStates, isOpenLoop);
   }
 
-
   /**
    * Set the maximum speeds for desaturation.
    *
@@ -1122,11 +1138,11 @@ public void resetDriveEncoders()
    * 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 void setCosineCompensator(boolean enabled)
     }
   }
 
+  /**
+   * 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;
+  }
+
 }

src/main/java/swervelib/SwerveModule.java

@@ -257,7 +257,7 @@ public void setAntiJitter(boolean antiJitter)
     this.antiJitterEnabled = antiJitter;
     if (antiJitter)
     {
-      pushOffsetsToControllers();
+      pushOffsetsToEncoders();
     } else
     {
       restoreInternalOffset();
@@ -585,7 +585,7 @@ public SwerveModuleConfiguration getConfiguration()
   /**
    * 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)
     {