feat: Acceleration limit (#37)

* implemented xy acceleration limit * Code has error * Revert "Code has error" This reverts commit 8a7a53f. * fix close to 0 bug * fix: max acceleration --------- Co-authored-by: Dave Korhumel <[email protected]> Co-authored-by: LeonardAbbas <[email protected]>
SaintsRobotics · Mar 25, 2023 · d5a5146 · d5a5146
1 parent 7338717
commit d5a5146
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -112,6 +112,7 @@ public static final class DriveConstants {
     public static final double kMaxAngularSpeedRadiansPerSecond = 15.24/3;
 
     public static final double kTurningStopTime = 0.2; // TODO: tune heading correction stop time
+    public static final double kSpeedIncreasePerPeriod = 0.15;
 
     public static final double kPSnapRotate = 6;
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -90,15 +90,15 @@ public RobotContainer() {
                     * (1 - m_driverController
                         .getLeftTriggerAxis()
                         * OIConstants.kSlowModeScalar)
-                    / 1.5,
+                    * 0.8,
                 MathUtil.applyDeadband(
                     -m_driverController.getLeftX(),
                     OIConstants.kControllerDeadband)
                     * DriveConstants.kMaxSpeedMetersPerSecond
                     * (1 - m_driverController
                         .getLeftTriggerAxis()
                         * OIConstants.kSlowModeScalar)
-                    / 1.5,
+                    * 0.8,
                 MathUtil.applyDeadband(
                     -m_driverController.getRightX(),
                     OIConstants.kControllerDeadband)

diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -78,6 +78,9 @@ public class DriveSubsystem extends SubsystemBase {
 
   private final Field2d m_field = new Field2d();
 
+  private double m_currentXSpeed = 0;
+  private double m_currentYSpeed = 0;
+
   /** Creates a new {@link DriveSubsystem}. */
   public DriveSubsystem() {
     SmartDashboard.putData("Field", m_field);
@@ -176,17 +179,21 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ
 
     double currentAngle = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians());
 
-    if ((xSpeed == 0 && ySpeed == 0) || m_headingCorrectionTimer.get() < DriveConstants.kTurningStopTime) {
+    m_currentXSpeed = calculateAcceleration(m_currentXSpeed, xSpeed);
+    m_currentYSpeed = calculateAcceleration(m_currentYSpeed, ySpeed);
+
+    if ((m_currentXSpeed == 0 && m_currentYSpeed == 0)
+        || m_headingCorrectionTimer.get() < DriveConstants.kTurningStopTime) {
       m_headingCorrectionPID.setSetpoint(currentAngle);
     } else {
       rotation = m_headingCorrectionPID.calculate(currentAngle);
     }
 
     var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
         fieldRelative
-            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotation,
+            ? ChassisSpeeds.fromFieldRelativeSpeeds(m_currentXSpeed, m_currentYSpeed, rotation,
                 Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))
-            : new ChassisSpeeds(xSpeed, ySpeed, rotation));
+            : new ChassisSpeeds(m_currentXSpeed, m_currentYSpeed, rotation));
     setModuleStates(swerveModuleStates);
   }
 
@@ -223,4 +230,22 @@ public void zeroHeading() {
   public double getGyroPitch() {
     return m_gyro.getPitch();
   }
-}
+
+  /**
+   * Slowly accelerates the bot to the desired speed.
+   * 
+   * @param currentSpeed The current speed.
+   * @param desiredSpeed The desired speed.
+   * 
+   * @return The new speed to use.
+   */
+  private double calculateAcceleration(double currentSpeed, double desiredSpeed) {
+    if (Math.abs(currentSpeed - desiredSpeed) < DriveConstants.kSpeedIncreasePerPeriod) {
+      return desiredSpeed;
+    } else if (currentSpeed > desiredSpeed) {
+      return currentSpeed - DriveConstants.kSpeedIncreasePerPeriod;
+    } else {
+      return currentSpeed + DriveConstants.kSpeedIncreasePerPeriod;
+    }
+  }
+}