diff --git a/src/main/java/frc/robot/constants/DriveControlLoops.java b/src/main/java/frc/robot/constants/DriveControlLoops.java index 3900d69..43d56a7 100644 --- a/src/main/java/frc/robot/constants/DriveControlLoops.java +++ b/src/main/java/frc/robot/constants/DriveControlLoops.java @@ -11,7 +11,7 @@ public class DriveControlLoops { Math.toRadians(60), 0.02, Math.toRadians(3), - 0.05, + 0.12, true, 0 ); diff --git a/src/main/java/frc/robot/constants/DriveTrainConstants.java b/src/main/java/frc/robot/constants/DriveTrainConstants.java index 5a284e5..97a191a 100644 --- a/src/main/java/frc/robot/constants/DriveTrainConstants.java +++ b/src/main/java/frc/robot/constants/DriveTrainConstants.java @@ -8,8 +8,6 @@ /** * stores the constants and PID configs for chassis * because we want an all-real simulation for the chassis, the numbers are required to be considerably precise - * TODO: you need to change some of these numbers to fit your robot! - * TODO: If you are using REV Chassis, please remove ALL the references to {@link TunerConstants} and enter your numbers by hand * */ public class DriveTrainConstants { /* dead configs, don't change them */ @@ -29,6 +27,7 @@ public class DriveTrainConstants { /** + * numbers that needs to be changed to fit each robot * TODO: change these numbers to match your robot * for REV chassis, replace ALL references to {@link TunerConstants} with actual numbers * */ @@ -40,27 +39,12 @@ public class DriveTrainConstants { ROBOT_MASS_KG = 40, // with bumpers STEER_FRICTION_VOLTAGE = TunerConstants.kSteerFrictionVoltage, DRIVE_FRICTION_VOLTAGE = TunerConstants.kDriveFrictionVoltage, + DRIVE_INERTIA = 0.01, STEER_INERTIA = 0.01; - /* don't change the equations */ - private static final double GRAVITY_CONSTANT = 9.8; - public static final double - DRIVE_BASE_RADIUS = Math.hypot(TunerConstants.kFrontLeftXPosInches, TunerConstants.kFrontLeftYPosInches), - /* friction_force = normal_force * coefficient_of_friction */ - MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION, - /* force = torque / distance */ - MAX_PROPELLING_FORCE_NEWTONS = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, - /* floor_speed = wheel_angular_velocity * wheel_radius */ - CHASSIS_MAX_VELOCITY = DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS , // calculate using choreo - CHASSIS_MAX_ACCELERATION_MPS_SQ = Math.min( - MAX_FRICTION_ACCELERATION, // cannot exceed max friction - MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG - ), - CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS, - CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS, - CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION = MAX_FRICTION_ACCELERATION / DRIVE_BASE_RADIUS; - - /* translations of the modules to the robot center, in FL, FR, BL, BR */ + /** + * translations of the modules to the robot center, in FL, FR, BL, BR + * */ public static final Translation2d[] MODULE_TRANSLATIONS = new Translation2d[] { new Translation2d( Units.inchesToMeters(TunerConstants.kFrontLeftXPosInches), @@ -80,6 +64,25 @@ public class DriveTrainConstants { ) }; + /* equations that calculates some constants for the simulator (don't modify) */ + private static final double GRAVITY_CONSTANT = 9.8; + public static final double + DRIVE_BASE_RADIUS = MODULE_TRANSLATIONS[0].getNorm(), + /* friction_force = normal_force * coefficient_of_friction */ + MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION, + MAX_FRICTION_FORCE_PER_MODULE = MAX_FRICTION_ACCELERATION * ROBOT_MASS_KG / MODULE_TRANSLATIONS.length, + /* force = torque / distance */ + MAX_PROPELLING_FORCE_NEWTONS = DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS, + /* floor_speed = wheel_angular_velocity * wheel_radius */ + CHASSIS_MAX_VELOCITY = DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS , // calculate using choreo + CHASSIS_MAX_ACCELERATION_MPS_SQ = Math.min( + MAX_FRICTION_ACCELERATION, // cannot exceed max friction + MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG + ), + CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS, + CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS, + CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION = MAX_FRICTION_ACCELERATION / DRIVE_BASE_RADIUS; + public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics(MODULE_TRANSLATIONS); /* for collision detection in simulation */ 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 38e2be7..bf0bb8d 100644 --- a/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/IO/ModuleIOSim.java @@ -74,14 +74,35 @@ public void updateSim(double periodSecs) { steerSim.update(periodSecs); } - public double getDriveAppliedVolts() { + public double getSimulationTorque() { + return DRIVE_MOTOR.getTorque(DRIVE_MOTOR.getCurrent( + physicsSimulationResults.driveWheelFinalVelocityRadPerSec * DRIVE_GEAR_RATIO, + driveAppliedVolts + )); + } + + public double getAppliedVolts() { return driveAppliedVolts; } - public Rotation2d getSteerFacing() { + public Rotation2d getSimulationSteerFacing() { return Rotation2d.fromRadians(steerSim.getAngularPositionRad()); } + public SwerveModuleState getSimulationSwerveState() { + return new SwerveModuleState( + physicsSimulationResults.driveWheelFinalVelocityRadPerSec * WHEEL_RADIUS_METERS, + getSimulationSteerFacing() + ); + } + + public SwerveModuleState getDesiredSwerveState() { + return new SwerveModuleState( + driveAppliedVolts * CHASSIS_MAX_VELOCITY, + getSimulationSteerFacing() + ); + } + /** * this replaces DC Motor Sim for drive wheels * */ diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java index 7c2ba00..8b5a2b3 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java @@ -17,6 +17,7 @@ import frc.robot.utils.Alert; import frc.robot.utils.CustomMaths.SwerveStateProjection; import frc.robot.utils.CustomPIDs.MaplePIDController; +import org.littletonrobotics.junction.Logger; import static frc.robot.constants.DriveControlLoops.*; @@ -54,7 +55,7 @@ public SwerveModule(ModuleIO io, String name) { public void updateOdometryInputs() { io.updateInputs(inputs); - // Logger.processInputs("Drive/Module-" + name, inputs); + Logger.processInputs("Drive/Module-" + name, inputs); this.hardwareFaultAlert.setActivated(!inputs.hardwareConnected); } 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 fb62cb1..46763f4 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/HolonomicChassisSimulation.java @@ -93,16 +93,16 @@ protected void simulateTranslationalFrictionNoMotion() { super.setLinearVelocity(new Vector2()); } - private void simulateChassisRotationalBehavior(double desiredRotationalMotionPercent) { + protected void simulateChassisRotationalBehavior(double desiredRotationalMotionPercent) { final double maximumTorque = this.profile.maxAngularAcceleration * super.getMass().getInertia(); - if (Math.abs(desiredRotationalMotionPercent) > 0.03) { + if (Math.abs(desiredRotationalMotionPercent) > 0.01) { super.applyTorque(desiredRotationalMotionPercent * maximumTorque); return; } final double actualRotationalMotionPercent = Math.abs(getAngularVelocity() / profile.maxAngularVelocity), frictionalTorqueMagnitude = this.profile.angularFrictionAcceleration * super.getMass().getInertia(); - if (actualRotationalMotionPercent > 0.03) + if (actualRotationalMotionPercent > 0.01) super.applyTorque(Math.copySign(frictionalTorqueMagnitude, -super.getAngularVelocity())); else super.setAngularVelocity(0); @@ -158,10 +158,10 @@ public RobotSimulationProfile(double robotMaxVelocity, double robotMaxAccelerati this.robotMass = robotMass; this.propellingForce = robotMaxAcceleration * robotMass; this.frictionForce = DriveTrainConstants.MAX_FRICTION_ACCELERATION * robotMass; - this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity; + this.linearVelocityDamping = robotMaxAcceleration / robotMaxVelocity * 0.3; this.maxAngularVelocity = maxAngularVelocity; this.maxAngularAcceleration = robotMaxAcceleration / (Math.hypot(width, height) / 2); - this.angularDamping = maxAngularAcceleration / maxAngularVelocity; + this.angularDamping = maxAngularAcceleration / maxAngularVelocity * 0.3; this.angularFrictionAcceleration = DriveTrainConstants.CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION; this.width = width; this.height = height; diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java index bb23714..a1ff3c2 100644 --- a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java +++ b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulations/SwerveDriveSimulation.java @@ -1,16 +1,20 @@ package frc.robot.utils.CompetitionFieldUtils.Simulations; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import frc.robot.Robot; import frc.robot.constants.LogPaths; import frc.robot.subsystems.drive.IO.GyroIOSim; import frc.robot.subsystems.drive.IO.ModuleIOSim; import frc.robot.subsystems.drive.IO.OdometryThread; +import frc.robot.utils.CustomMaths.GeometryConvertor; import frc.robot.utils.MapleTimeUtils; +import org.dyn4j.geometry.Vector2; import java.util.Arrays; import java.util.function.Consumer; @@ -55,40 +59,112 @@ public void resetOdometryToActualRobotPose() { public void updateSimulationSubTick(int tickNum, double tickSeconds) { for (int i = 0; i < modules.length; i++) moduleSimulationSubTick( + getObjectOnFieldPose2d(), modules[i], MODULE_TRANSLATIONS[i], tickNum, tickSeconds ); - final ChassisSpeeds chassisFreeSpeeds = DRIVE_KINEMATICS.toChassisSpeeds(Arrays.stream(modules).map(( - moduleIOSim -> new SwerveModuleState( - moduleIOSim.physicsSimulationResults.driveWheelFinalVelocityRadPerSec * WHEEL_RADIUS_METERS, - moduleIOSim.getSteerFacing() - ))).toArray(SwerveModuleState[]::new)); - - // TODO: here we apply the friction force that brings the chassis from its floor speed to free speed - + simulateFrictionForce(); gyroSimulationSubTick( - gyroIOSim, super.getObjectOnFieldPose2d().getRotation(), super.getAngularVelocity(), tickNum ); } - private static void moduleSimulationSubTick( + private void moduleSimulationSubTick( + Pose2d robotWorldPose, ModuleIOSim module, - Translation2d moduleTranslation, + Translation2d moduleTranslationOnRobot, int tickNum, double tickPeriodSeconds ) { - // TODO: here, apply the propelling force of each module on the robot - // and feed its odometry drive position and velocity back to the results + /* update the DC motor simulation of the steer */ + module.updateSim(tickPeriodSeconds); + + /* simulate the propelling force of the module */ + final Rotation2d moduleWorldFacing = module.getSimulationSteerFacing().plus(robotWorldPose.getRotation()); + final Vector2 moduleWorldPosition = GeometryConvertor.toDyn4jVector2( + robotWorldPose.getTranslation() + .plus(moduleTranslationOnRobot.rotateBy(robotWorldPose.getRotation())) + ); + double actualPropellingForceOnFloorNewtons = module.getSimulationTorque() / WHEEL_RADIUS_METERS; + final boolean skidding = Math.abs(actualPropellingForceOnFloorNewtons) > MAX_FRICTION_FORCE_PER_MODULE; + if (skidding) + actualPropellingForceOnFloorNewtons = Math.copySign(MAX_FRICTION_FORCE_PER_MODULE, actualPropellingForceOnFloorNewtons); + super.applyForce( + Vector2.create(actualPropellingForceOnFloorNewtons, moduleWorldFacing.getRadians()), + moduleWorldPosition + ); + + + final Vector2 floorVelocity = super.getLinearVelocity(moduleWorldPosition); + final double floorVelocityProjectionOnWheelDirectionMPS = floorVelocity.getMagnitude() * + Math.cos(floorVelocity.getAngleBetween(new Vector2(moduleWorldFacing.getRadians()))); + + if (skidding) + /* if the chassis is skidding, the toque will cause the wheels to spin freely */ + module.physicsSimulationResults.driveWheelFinalVelocityRadPerSec += module.getSimulationTorque() / DRIVE_INERTIA * tickPeriodSeconds; + else + /* otherwise, the floor velocity is projected to the wheel */ + module.physicsSimulationResults.driveWheelFinalVelocityRadPerSec = floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS; + + module.physicsSimulationResults.odometrySteerPositions[tickNum] = module.getSimulationSteerFacing(); + module.physicsSimulationResults.driveWheelFinalRevolutions += Units.radiansToRotations( + module.physicsSimulationResults.driveWheelFinalVelocityRadPerSec * tickPeriodSeconds + ); + module.physicsSimulationResults.odometryDriveWheelRevolutions[tickNum] = module.physicsSimulationResults.driveWheelFinalRevolutions; } - private static void gyroSimulationSubTick( - GyroIOSim gyroIOSim, + private void simulateFrictionForce() { + final ChassisSpeeds speedsDifference = getDifferenceBetweenFloorAndFreeSpeed(); + final Translation2d translationalSpeedsDifference = new Translation2d(speedsDifference.vxMetersPerSecond, speedsDifference.vyMetersPerSecond); + final double forceMultiplier = Math.min(translationalSpeedsDifference.getNorm() * 3, 1); + super.applyForce(Vector2.create( + forceMultiplier * MAX_FRICTION_ACCELERATION * ROBOT_MASS_KG, + translationalSpeedsDifference.getAngle().getRadians() + )); + + if (Math.abs(getDesiredSpeedsFieldRelative().omegaRadiansPerSecond) + / CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC + < 0.01) + simulateChassisRotationalBehavior(0); + } + + private ChassisSpeeds getDifferenceBetweenFloorAndFreeSpeed() { + ChassisSpeeds chassisFreeSpeedsFieldRelative = getFreeSpeedsFieldRelative(); + + final double freeSpeedMagnitude = Math.hypot(chassisFreeSpeedsFieldRelative.vxMetersPerSecond, chassisFreeSpeedsFieldRelative.vyMetersPerSecond), + floorSpeedMagnitude = Math.hypot(getMeasuredChassisSpeedsFieldRelative().vxMetersPerSecond, getMeasuredChassisSpeedsFieldRelative().vyMetersPerSecond); + if (freeSpeedMagnitude > floorSpeedMagnitude) + chassisFreeSpeedsFieldRelative = chassisFreeSpeedsFieldRelative.times(floorSpeedMagnitude / freeSpeedMagnitude); + + return chassisFreeSpeedsFieldRelative.minus(getMeasuredChassisSpeedsFieldRelative()); + } + + private ChassisSpeeds getFreeSpeedsFieldRelative() { + return ChassisSpeeds.fromRobotRelativeSpeeds( + DRIVE_KINEMATICS.toChassisSpeeds( + Arrays.stream(modules) + .map(ModuleIOSim::getSimulationSwerveState) + .toArray(SwerveModuleState[]::new)), + getObjectOnFieldPose2d().getRotation() + ); + } + + private ChassisSpeeds getDesiredSpeedsFieldRelative() { + return ChassisSpeeds.fromRobotRelativeSpeeds( + DRIVE_KINEMATICS.toChassisSpeeds( + Arrays.stream(modules) + .map(ModuleIOSim::getDesiredSwerveState) + .toArray(SwerveModuleState[]::new)), + getObjectOnFieldPose2d().getRotation() + ); + } + + private void gyroSimulationSubTick( Rotation2d currentFacing, double angularVelocityRadPerSec, int tickNum