Skip to content

Commit b09950c

Browse files
mebrahimaleemmebrahimaleem
committed
feat: acceleration limits
1 parent ec8acaf commit b09950c

File tree

3 files changed

+131
-31
lines changed

3 files changed

+131
-31
lines changed

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

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,9 @@
44

55
package frc.robot;
66

7+
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
8+
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
9+
710
import edu.wpi.first.math.VecBuilder;
811
import edu.wpi.first.math.Vector;
912
import edu.wpi.first.math.geometry.Pose3d;
@@ -12,6 +15,8 @@
1215
import edu.wpi.first.math.geometry.Translation3d;
1316
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
1417
import edu.wpi.first.math.numbers.N3;
18+
import edu.wpi.first.units.measure.AngularAcceleration;
19+
import edu.wpi.first.units.measure.LinearAcceleration;
1520

1621
/**
1722
* The Constants class provides a convenient place for teams to hold robot-wide
@@ -28,6 +33,7 @@
2833
public final class Constants {
2934

3035
public static final double kFastPeriodicPeriod = 0.01; // 100Hz, 10ms
36+
public static final double kFastPeriodicFreq = 1.0 / kFastPeriodicPeriod;
3137

3238
/**
3339
* Input/Output constants
@@ -83,6 +89,9 @@ public static final class DriveConstants {
8389
// TODO: Tune this PID before running on a robot on the ground
8490
public static final double kPModuleTurningController = 0.3;
8591

92+
// TODO: tune PID
93+
public static final double[] kDrivePIDConstants = new double[] {1, 0, 0};
94+
8695
public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
8796
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
8897
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
@@ -98,6 +107,10 @@ public static final class DriveConstants {
98107
public static final double kHeadingCorrectionTurningStopTime = 0.2;
99108
// TODO: Tune this PID before running on a robot on the ground
100109
public static final double kPHeadingCorrectionController = 5;
110+
111+
// TODO: tune values
112+
public static final LinearAcceleration kMaxLinearAcceleration = MetersPerSecondPerSecond.of(1);
113+
public static final AngularAcceleration kMaxAngularAcceleration = RadiansPerSecondPerSecond.of(3);
101114
}
102115

103116
public static final class VisionConstants {

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

Lines changed: 114 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -4,19 +4,27 @@
44

55
package frc.robot.subsystems;
66

7+
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
8+
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
9+
710
import com.studica.frc.AHRS;
811
import com.studica.frc.AHRS.NavXComType;
912

1013
import edu.wpi.first.math.MathUtil;
1114
import edu.wpi.first.math.VecBuilder;
15+
import edu.wpi.first.math.Vector;
1216
import edu.wpi.first.math.controller.PIDController;
1317
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
18+
import edu.wpi.first.math.filter.SlewRateLimiter;
1419
import edu.wpi.first.math.geometry.Pose2d;
1520
import edu.wpi.first.math.geometry.Rotation2d;
1621
import edu.wpi.first.math.kinematics.ChassisSpeeds;
1722
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
1823
import edu.wpi.first.math.kinematics.SwerveModulePosition;
1924
import edu.wpi.first.math.kinematics.SwerveModuleState;
25+
import edu.wpi.first.math.numbers.N2;
26+
import edu.wpi.first.units.measure.AngularAcceleration;
27+
import edu.wpi.first.units.measure.LinearAcceleration;
2028
import edu.wpi.first.wpilibj.Timer;
2129
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
2230
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -73,6 +81,13 @@ public class DriveSubsystem extends SubsystemBase {
7381

7482
private final Field2d m_field = new Field2d();
7583

84+
// meters per second (per second)
85+
private Vector<N2> m_prevTranSpeed;
86+
private double m_maxLinearAcceleration;
87+
88+
// radians per second
89+
private SlewRateLimiter m_rotationRateFilter;
90+
7691
/** Creates a new DriveSubsystem. */
7792
public DriveSubsystem() {
7893
this.zeroHeading();
@@ -97,12 +112,17 @@ public DriveSubsystem() {
97112
LimelightHelpers.SetIMUMode(VisionConstants.kLimelightName, VisionConstants.kIMUMode);
98113
}
99114

115+
m_prevTranSpeed = VecBuilder.fill(0, 0);
116+
m_maxLinearAcceleration = DriveConstants.kMaxLinearAcceleration.in(MetersPerSecondPerSecond);
117+
118+
m_rotationRateFilter = new SlewRateLimiter(DriveConstants.kMaxAngularAcceleration.in(RadiansPerSecondPerSecond));
119+
100120
m_desiredStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(new ChassisSpeeds());
101121
}
102122

103-
@Override
104-
public void periodic() {
105-
// This method will be called once per scheduler run
123+
@Override
124+
public void periodic() {
125+
// This method will be called once per scheduler run
106126

107127
m_swerveModulePositions = new SwerveModulePosition[] {
108128
m_frontLeft.getPosition(),
@@ -213,20 +233,72 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe
213233
calculatedRotation = m_headingCorrectionPID.calculate(currentAngle);
214234
}
215235

216-
// Depending on whether the robot is being driven in field relative, calculate
217-
// the desired states for each of the modules
218-
m_desiredStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
219-
fieldRelative
220-
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation,
221-
Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))
222-
: new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation));
223-
}
236+
// Depending on whether the robot is being driven in field relative, calculate
237+
// the desired states for each of the modules
238+
m_desiredStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
239+
fieldRelative
240+
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation,
241+
Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))
242+
: new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation));
224243

225-
/**
226-
* Resets the odometry to the specified pose.
227-
*
228-
* @param pose The pose to which to set the odometry.
229-
*/
244+
SwerveDriveKinematics.desaturateWheelSpeeds(
245+
m_desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
246+
247+
// acceleration limits
248+
249+
// convert to chassis speeds for calculations
250+
ChassisSpeeds originalSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(m_desiredStates);
251+
252+
// slew rate limiter only works in one dimension so it can't be used for
253+
// translation
254+
255+
// calculate speed vector delta magnitude
256+
final Vector<N2> dv = VecBuilder
257+
.fill(originalSpeeds.vxMetersPerSecond, originalSpeeds.vyMetersPerSecond)
258+
.minus(m_prevTranSpeed);
259+
260+
final double dvMagnitude = dv.norm();
261+
262+
// calculate maximum speed vector detla magnitude
263+
final double maxdvMagnitude = m_maxLinearAcceleration * Robot.kDefaultPeriod;
264+
265+
// if dv exceeds maxdv, then add maxdv to the old velocity in the same direction as dv
266+
if (dvMagnitude > maxdvMagnitude) {
267+
// calculate a vector with direction of dv (normalized) and magnitude maxdv
268+
final Vector<N2> dvLimited = dv.times(maxdvMagnitude/dvMagnitude);
269+
270+
// calculate new velocity vector
271+
final Vector<N2> newV = m_prevTranSpeed.plus(dvLimited);
272+
273+
// correct chassis speeds
274+
originalSpeeds.vxMetersPerSecond = newV.get(0);
275+
originalSpeeds.vyMetersPerSecond = newV.get(1);
276+
277+
// inverse kinematics back to module states
278+
m_desiredStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(originalSpeeds);
279+
}
280+
281+
// recalculate chassis speed for rotation limits
282+
originalSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(m_desiredStates);
283+
284+
// calculate new rotation speed
285+
final double limitedOmega = m_rotationRateFilter.calculate(originalSpeeds.omegaRadiansPerSecond);
286+
287+
// scale all modules to limit rotation
288+
final double rotationScalar = originalSpeeds.omegaRadiansPerSecond == 0 ? 1
289+
: limitedOmega / originalSpeeds.omegaRadiansPerSecond;
290+
for (SwerveModuleState state : m_desiredStates) {
291+
state.speedMetersPerSecond *= rotationScalar;
292+
}
293+
294+
m_prevTranSpeed = VecBuilder.fill(originalSpeeds.vxMetersPerSecond, originalSpeeds.vyMetersPerSecond);
295+
}
296+
297+
/**
298+
* Resets the odometry to the specified pose.
299+
*
300+
* @param pose The pose to which to set the odometry.
301+
*/
230302
public void resetOdometry(Pose2d pose) {
231303
m_poseEstimator.resetPosition(
232304
Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
@@ -249,18 +321,31 @@ public void addVisionMeasurement(Pose2d pose, double timestamp) {
249321
m_poseEstimator.addVisionMeasurement(pose, timestamp);
250322
}
251323

252-
/** Sets the module states every 10ms (100Hz), faster than the regular periodic loop */
253-
public void fastPeriodic() {
254-
SwerveDriveKinematics.desaturateWheelSpeeds(
255-
m_desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
256-
m_frontLeft.setDesiredState(m_desiredStates[0]);
257-
m_frontRight.setDesiredState(m_desiredStates[1]);
258-
m_rearLeft.setDesiredState(m_desiredStates[2]);
259-
m_rearRight.setDesiredState(m_desiredStates[3]);
260-
261-
// Takes the integral of the rotation speed to find the current angle for the
262-
// simulator
263-
m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(m_desiredStates).omegaRadiansPerSecond
264-
* Constants.kFastPeriodicPeriod;
324+
/**
325+
* Changes the acceleration limits
326+
* @param linearAccelerationLimit translation acceleration limit
327+
* @param angularAccelerationLimit rotation acceleration limit
328+
*/
329+
public void setAccelerationLimits(LinearAcceleration linearAccelerationLimit,
330+
AngularAcceleration angularAccelerationLimit) {
331+
m_maxLinearAcceleration = linearAccelerationLimit.in(MetersPerSecondPerSecond);
332+
m_rotationRateFilter = new SlewRateLimiter(angularAccelerationLimit.in(RadiansPerSecondPerSecond),
333+
angularAccelerationLimit.in(RadiansPerSecondPerSecond), m_rotationRateFilter.lastValue());
265334
}
335+
336+
/**
337+
* Sets the module states every 10ms (100Hz), faster than the regular periodic
338+
* loop
339+
*/
340+
public void fastPeriodic() {
341+
m_frontLeft.setDesiredState(m_desiredStates[0]);
342+
m_frontRight.setDesiredState(m_desiredStates[1]);
343+
m_rearLeft.setDesiredState(m_desiredStates[2]);
344+
m_rearRight.setDesiredState(m_desiredStates[3]);
345+
346+
// Takes the integral of the rotation speed to find the current angle for the
347+
// simulator
348+
m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(m_desiredStates).omegaRadiansPerSecond
349+
* Constants.kFastPeriodicPeriod;
350+
}
266351
}

‎src/main/java/frc/robot/subsystems/SwerveModule.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ public class SwerveModule {
3030
private final CANcoder m_turningEncoder;
3131

3232
private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0, 0, Constants.kFastPeriodicPeriod);
33+
private final PIDController m_drivingPIDController = new PIDController(DriveConstants.kDrivePIDConstants[0], DriveConstants.kDrivePIDConstants[1], DriveConstants.kDrivePIDConstants[2], Constants.kFastPeriodicPeriod);
3334

3435
private SwerveModuleState m_state = new SwerveModuleState();
3536
private double m_distance;
@@ -57,6 +58,8 @@ public SwerveModule(
5758
// converts default units to meters per second
5859
m_driveMotorConfig.encoder.positionConversionFactor(
5960
DriveConstants.kWheelDiameterMeters * Math.PI / DriveConstants.kDrivingGearRatio);
61+
m_driveMotorConfig.encoder.velocityConversionFactor(
62+
DriveConstants.kWheelDiameterMeters * Math.PI / DriveConstants.kDrivingGearRatio);
6063
m_driveMotorConfig.inverted(driveMotorReversed);
6164

6265
m_turningMotorConfig.idleMode(IdleMode.kBrake);
@@ -95,12 +98,11 @@ public SwerveModulePosition getPosition() {
9598
public void setDesiredState(SwerveModuleState desiredState) {
9699
m_state = desiredState;
97100
m_state.optimize(getEncoderAngle(m_turningEncoder));
98-
driveOutput = m_state.speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond;
99101

100102
turnOutput = -m_turningPIDController.calculate(getEncoderAngle(m_turningEncoder).getRadians(),
101103
m_state.angle.getRadians());
102104

103-
m_driveMotor.set(driveOutput);
105+
m_driveMotor.set(m_drivingPIDController.calculate(m_driveMotor.getEncoder().getVelocity(), m_state.speedMetersPerSecond));
104106
m_turningMotor.set(turnOutput);
105107
}
106108

0 commit comments

Comments
 (0)