Skip to content

Commit

Permalink
Add some comments
Browse files Browse the repository at this point in the history
  • Loading branch information
BlueZeeKing committed Jan 4, 2024
1 parent 30ada95 commit 677faa7
Showing 1 changed file with 12 additions and 2 deletions.
14 changes: 12 additions & 2 deletions src/main/java/frc/robot/subsystems/DriveSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -150,24 +150,32 @@ public void drive(
* DriveConstants.MAX_SPEED_METERS_PER_SECOND,
DriveConstants.MAX_SPEED_METERS_PER_SECOND);

// If either translation magnitudes are low enough, it's direction is irrelevant
if (currentTranslationMag < 0.1) {
currentTranslationDir = inputTranslationDir;
} else if (inputTranslationMag < 0.1) {
inputTranslationDir = currentTranslationDir;
}

// If the new direction is over 90º from the current direction, slow the robot
// to a near stop before continuing. The block above activates when the robot
// has slowed down enough
if (SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) > Math.PI / 2.0) {
inputTranslationDir = currentTranslationDir;
inputTranslationMag *= -1;
}

inputTranslationDir = SwerveUtils.wrapAngle(inputTranslationDir);

// Decrease the requested speed based on how far the current translation
// direction is from the requested one. A 90º angle means the requested
// magnitude is 0, a 0º angle means the magnitude is unchanged.
inputTranslationMag *=
1
- SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir)
/ (Math.PI / 2);

// Set the direction limit to a percent of the maxiumum based on the percent of
// max speed we are currently going at. This is limited to prevent fully
// disabling steering and leaves it at 10%.
double dirLimit =
Math.max(
0,
Expand Down Expand Up @@ -198,8 +206,10 @@ public void drive(
? ChassisSpeeds.fromFieldRelativeSpeeds(
xSpeedCommanded, ySpeedCommanded, this.currentRotation, getHeading())
: new ChassisSpeeds(xSpeedCommanded, ySpeedCommanded, this.currentRotation));

SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND);

this.frontLeft.setDesiredState(swerveModuleStates[0]);
this.frontRight.setDesiredState(swerveModuleStates[1]);
this.rearLeft.setDesiredState(swerveModuleStates[2]);
Expand Down

0 comments on commit 677faa7

Please sign in to comment.