Skip to content

Commit

Permalink
better current limit simulation
Browse files Browse the repository at this point in the history
  • Loading branch information
@catr1xLiu
catr1xLiu committed Sep 11, 2024
1 parent 47a7464 commit 4aabcba
Show file tree
Hide file tree
Showing 7 changed files with 48 additions and 40 deletions.
8 changes: 6 additions & 2 deletions example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveTrainConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,16 @@ public class DriveTrainConstants {
WHEEL_COEFFICIENT_OF_FRICTION = 0.95,
ROBOT_MASS_KG = 37.35; // with bumpers

private static final DCMotor KRAKEN_X60 = new DCMotor(
12, 7.09, 366, 2, Units.rotationsPerMinuteToRadiansPerSecond(4800), 1);
private static final DCMotor FALCON_500 = new DCMotor(
12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(5104), 1);
/**
* TODO: change motor type to match your robot
* */
public static final DCMotor
DRIVE_MOTOR = DCMotor.getKrakenX60(1),
STEER_MOTOR = DCMotor.getFalcon500(1);
DRIVE_MOTOR = KRAKEN_X60,
STEER_MOTOR = FALCON_500;

/**
* numbers imported from {@link TunerConstants}
Expand Down
29 changes: 15 additions & 14 deletions example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
import org.littletonrobotics.junction.Logger;

import java.util.Arrays;

Expand All @@ -23,7 +24,7 @@
public class ModuleIOSim implements ModuleIO {
public final SwerveModulePhysicsSimulationResults physicsSimulationResults;
private final DCMotorSim steerSim;
private double driveDesiredVolts = 0.0, steerAppliedVolts = 0.0;
private double driveDesiredVolts = 0.0, driveAppliedVolts = 0.0, driveActualCurrent = 0.0, steerAppliedVolts = 0.0;

public ModuleIOSim() {
this.steerSim = new DCMotorSim(STEER_MOTOR, STEER_GEAR_RATIO, STEER_INERTIA);
Expand All @@ -35,11 +36,8 @@ public ModuleIOSim() {
public void updateInputs(ModuleIOInputs inputs) {
inputs.driveWheelFinalRevolutions = physicsSimulationResults.driveWheelFinalRevolutions;
inputs.driveWheelFinalVelocityRevolutionsPerSec = Units.radiansToRotations(physicsSimulationResults.driveWheelFinalVelocityRadPerSec);
inputs.driveMotorAppliedVolts = driveDesiredVolts;
inputs.driveMotorCurrentAmps = Math.abs(DRIVE_MOTOR.getCurrent(
physicsSimulationResults.driveWheelFinalVelocityRadPerSec,
driveDesiredVolts
));
inputs.driveMotorAppliedVolts = driveAppliedVolts;
inputs.driveMotorCurrentAmps = Math.abs(driveActualCurrent);

inputs.steerFacing = Rotation2d.fromRadians(steerSim.getAngularPositionRad());
inputs.steerVelocityRadPerSec = steerSim.getAngularVelocityRadPerSec();
Expand Down Expand Up @@ -80,24 +78,27 @@ public double getSimulationTorque() {
* DRIVE_GEAR_RATIO;
final double currentAtDesiredVolts = DRIVE_MOTOR.getCurrent(
driveMotorRotterSpeedRadPerSec,
driveDesiredVolts
driveAppliedVolts
);

// apply smart current limit
final double SMART_CURRENT_LIMIT_MINIMUM_VOLTAGE_RATE = 0.5;
double actualAppliedVolts = driveDesiredVolts;
driveAppliedVolts = driveDesiredVolts;
if (Math.abs(currentAtDesiredVolts) > DRIVE_CURRENT_LIMIT
&& driveDesiredVolts * physicsSimulationResults.driveWheelFinalVelocityRadPerSec > 0) {
final double currentLimitRate = DRIVE_CURRENT_LIMIT / Math.abs(currentAtDesiredVolts);
actualAppliedVolts *= Math.max(currentLimitRate, SMART_CURRENT_LIMIT_MINIMUM_VOLTAGE_RATE);
final double limitedCurrent = Math.copySign(DRIVE_CURRENT_LIMIT, currentAtDesiredVolts);
driveAppliedVolts = DRIVE_MOTOR.getVoltage(
DRIVE_MOTOR.getTorque(limitedCurrent),
driveMotorRotterSpeedRadPerSec
);
}

final double actualCurrent = DRIVE_MOTOR.getCurrent(
driveActualCurrent = DRIVE_MOTOR.getCurrent(
driveMotorRotterSpeedRadPerSec,
actualAppliedVolts
driveAppliedVolts
);

return DRIVE_MOTOR.getTorque(actualCurrent);

return DRIVE_MOTOR.getTorque(driveActualCurrent) * DRIVE_GEAR_RATIO;
}

public Rotation2d getSimulationSteerFacing() {
Expand Down
9 changes: 5 additions & 4 deletions ...in/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,20 +149,21 @@ public RobotSimulationProfile() {
DriveTrainConstants.ROBOT_MASS_KG,
DriveTrainConstants.BUMPER_WIDTH_METERS,
DriveTrainConstants.BUMPER_LENGTH_METERS,
0.1
0.05,
0.05
);
}

public RobotSimulationProfile(double robotMaxVelocity, double robotMaxAcceleration, double maxAngularVelocity, double robotMass, double width, double height, double dampingCoefficient) {
public RobotSimulationProfile(double robotMaxVelocity, double robotMaxAcceleration, double maxAngularVelocity, double robotMass, double width, double height, double translationalDampingCoefficient, double rotationalDampingCoefficient) {
this.robotMaxVelocity = robotMaxVelocity;
this.robotMaxAcceleration = robotMaxAcceleration;
this.robotMass = robotMass;
this.propellingForce = robotMaxAcceleration * robotMass;
this.frictionForce = DriveTrainConstants.MAX_FRICTION_ACCELERATION * robotMass;
this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity * dampingCoefficient;
this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity * translationalDampingCoefficient;
this.maxAngularVelocity = maxAngularVelocity;
this.maxAngularAcceleration = robotMaxAcceleration / (Math.hypot(width, height) / 2);
this.angularDamping = maxAngularAcceleration / maxAngularVelocity * dampingCoefficient;
this.angularDamping = maxAngularAcceleration / maxAngularVelocity * rotationalDampingCoefficient;
this.angularFrictionAcceleration = DriveTrainConstants.CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION;
this.width = width;
this.height = height;
Expand Down
2 changes: 1 addition & 1 deletion .../main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ public class OpponentRobotSimulation extends HolonomicChassisSimulation implemen
Units.lbsToKilograms(125),
DriveTrainConstants.BUMPER_WIDTH_METERS,
DriveTrainConstants.BUMPER_LENGTH_METERS,
1
1, 1
);

private final int robotID;
Expand Down
29 changes: 15 additions & 14 deletions src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;
import org.littletonrobotics.junction.Logger;

import java.util.Arrays;

Expand All @@ -23,7 +24,7 @@
public class ModuleIOSim implements ModuleIO {
public final SwerveModulePhysicsSimulationResults physicsSimulationResults;
private final DCMotorSim steerSim;
private double driveDesiredVolts = 0.0, steerAppliedVolts = 0.0;
private double driveDesiredVolts = 0.0, driveAppliedVolts = 0.0, driveActualCurrent = 0.0, steerAppliedVolts = 0.0;

public ModuleIOSim() {
this.steerSim = new DCMotorSim(STEER_MOTOR, STEER_GEAR_RATIO, STEER_INERTIA);
Expand All @@ -35,11 +36,8 @@ public ModuleIOSim() {
public void updateInputs(ModuleIOInputs inputs) {
inputs.driveWheelFinalRevolutions = physicsSimulationResults.driveWheelFinalRevolutions;
inputs.driveWheelFinalVelocityRevolutionsPerSec = Units.radiansToRotations(physicsSimulationResults.driveWheelFinalVelocityRadPerSec);
inputs.driveMotorAppliedVolts = driveDesiredVolts;
inputs.driveMotorCurrentAmps = Math.abs(DRIVE_MOTOR.getCurrent(
physicsSimulationResults.driveWheelFinalVelocityRadPerSec,
driveDesiredVolts
));
inputs.driveMotorAppliedVolts = driveAppliedVolts;
inputs.driveMotorCurrentAmps = Math.abs(driveActualCurrent);

inputs.steerFacing = Rotation2d.fromRadians(steerSim.getAngularPositionRad());
inputs.steerVelocityRadPerSec = steerSim.getAngularVelocityRadPerSec();
Expand Down Expand Up @@ -80,24 +78,27 @@ public double getSimulationTorque() {
* DRIVE_GEAR_RATIO;
final double currentAtDesiredVolts = DRIVE_MOTOR.getCurrent(
driveMotorRotterSpeedRadPerSec,
driveDesiredVolts
driveAppliedVolts
);

// apply smart current limit
final double SMART_CURRENT_LIMIT_MINIMUM_VOLTAGE_RATE = 0.5;
double actualAppliedVolts = driveDesiredVolts;
driveAppliedVolts = driveDesiredVolts;
if (Math.abs(currentAtDesiredVolts) > DRIVE_CURRENT_LIMIT
&& driveDesiredVolts * physicsSimulationResults.driveWheelFinalVelocityRadPerSec > 0) {
final double currentLimitRate = DRIVE_CURRENT_LIMIT / Math.abs(currentAtDesiredVolts);
actualAppliedVolts *= Math.max(currentLimitRate, SMART_CURRENT_LIMIT_MINIMUM_VOLTAGE_RATE);
final double limitedCurrent = Math.copySign(DRIVE_CURRENT_LIMIT, currentAtDesiredVolts);
driveAppliedVolts = DRIVE_MOTOR.getVoltage(
DRIVE_MOTOR.getTorque(limitedCurrent),
driveMotorRotterSpeedRadPerSec
);
}

final double actualCurrent = DRIVE_MOTOR.getCurrent(
driveActualCurrent = DRIVE_MOTOR.getCurrent(
driveMotorRotterSpeedRadPerSec,
actualAppliedVolts
driveAppliedVolts
);

return DRIVE_MOTOR.getTorque(actualCurrent);

return DRIVE_MOTOR.getTorque(driveActualCurrent) * DRIVE_GEAR_RATIO;
}

public Rotation2d getSimulationSteerFacing() {
Expand Down
9 changes: 5 additions & 4 deletions ...in/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,20 +149,21 @@ public RobotSimulationProfile() {
DriveTrainConstants.ROBOT_MASS_KG,
DriveTrainConstants.BUMPER_WIDTH_METERS,
DriveTrainConstants.BUMPER_LENGTH_METERS,
0.1
0.05,
0.05
);
}

public RobotSimulationProfile(double robotMaxVelocity, double robotMaxAcceleration, double maxAngularVelocity, double robotMass, double width, double height, double dampingCoefficient) {
public RobotSimulationProfile(double robotMaxVelocity, double robotMaxAcceleration, double maxAngularVelocity, double robotMass, double width, double height, double translationalDampingCoefficient, double rotationalDampingCoefficient) {
this.robotMaxVelocity = robotMaxVelocity;
this.robotMaxAcceleration = robotMaxAcceleration;
this.robotMass = robotMass;
this.propellingForce = robotMaxAcceleration * robotMass;
this.frictionForce = DriveTrainConstants.MAX_FRICTION_ACCELERATION * robotMass;
this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity * dampingCoefficient;
this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity * translationalDampingCoefficient;
this.maxAngularVelocity = maxAngularVelocity;
this.maxAngularAcceleration = robotMaxAcceleration / (Math.hypot(width, height) / 2);
this.angularDamping = maxAngularAcceleration / maxAngularVelocity * dampingCoefficient;
this.angularDamping = maxAngularAcceleration / maxAngularVelocity * rotationalDampingCoefficient;
this.angularFrictionAcceleration = DriveTrainConstants.CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION;
this.width = width;
this.height = height;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ public class OpponentRobotSimulation extends HolonomicChassisSimulation implemen
Units.lbsToKilograms(125),
DriveTrainConstants.BUMPER_WIDTH_METERS,
DriveTrainConstants.BUMPER_LENGTH_METERS,
1
1, 1
);

private final int robotID;
Expand Down

0 comments on commit 4aabcba

Please sign in to comment.