diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveTrainConstants.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveTrainConstants.java index 4297b89..bc13b72 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveTrainConstants.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/constants/DriveTrainConstants.java @@ -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} diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java index 44dc81b..e04f692 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java @@ -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; @@ -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); @@ -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(); @@ -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() { diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java index d395733..06a12c6 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java @@ -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; diff --git a/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java b/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java index bb2c639..ad47b64 100644 --- a/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java +++ b/example/5516-2024-OffSeason/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java @@ -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; diff --git a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java index 44dc81b..e04f692 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java @@ -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; @@ -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); @@ -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(); @@ -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() { diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java index d395733..06a12c6 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java @@ -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; diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java index bb2c639..ad47b64 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/OpponentRobotSimulation.java @@ -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;