From f2ed06c01f8c3012d991fe40a53bb4045ce792f8 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 1 Apr 2026 12:29:04 -0700 Subject: [PATCH 1/3] Add force ffs --- .../subsystems/swerve/SwerveSubsystem.java | 46 +++++++++++++++++-- .../subsystems/swerve/module/Module.java | 19 +++++++- .../swerve/module/ModuleIOReal.java | 12 ++++- 3 files changed, 71 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 141c3d42..b81c4ec2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -432,7 +432,11 @@ private void updateVision() { * @param openLoop boolean for if the drivetrain should run with feedforward control (open loop) * or with feedback control (closed loop) */ - private void drive(ChassisSpeeds speeds, boolean openLoop) { + private void drive( + ChassisSpeeds speeds, + boolean openLoop, + double[] moduleForcesXNetwon, + double[] moduleForcesYNewton) { // Converts time continuous chassis speeds to setpoints after the specified time (dtSeconds) speeds = ChassisSpeeds.discretize(speeds, 0.02); @@ -446,6 +450,9 @@ private void drive(ChassisSpeeds speeds, boolean openLoop) { SwerveModuleState[] optimizedStates = new SwerveModuleState[modules.length]; + // Instead of storing velocity here we store Newtons of force + SwerveModuleState[] forceSetpoints = new SwerveModuleState[modules.length]; + for (int i = 0; i < optimizedStates.length; i++) { if (openLoop) { // Heuristic to enable/disable FOC @@ -456,14 +463,41 @@ private void drive(ChassisSpeeds speeds, boolean openLoop) { + Math.pow(this.getVelocityRobotRelative().vyMetersPerSecond, 2)) < SWERVE_CONSTANTS.getMaxLinearSpeed() * 0.9; // 0.9 is 90% of drivetrain max speed optimizedStates[i] = modules[i].runOpenLoop(states[i], focEnable); + + // Needs some value to update + forceSetpoints[i] = new SwerveModuleState(); } else { - optimizedStates[i] = modules[i].runClosedLoop(states[i]); + + double robotRelForceX = + moduleForcesXNetwon[i] * getRotation().unaryMinus().getCos() + - moduleForcesYNewton[i] * getRotation().unaryMinus().getSin(); + double robotRelForceY = + moduleForcesXNetwon[i] * getRotation().unaryMinus().getSin() + + moduleForcesYNewton[i] * getRotation().unaryMinus().getCos(); + + forceSetpoints[i] = + new SwerveModuleState( + Math.hypot(robotRelForceX, robotRelForceY), + new Rotation2d(robotRelForceX, robotRelForceY)); + + optimizedStates[i] = modules[i].runClosedLoop(states[i], robotRelForceX, robotRelForceY); } } - + Logger.recordOutput("SwerveStates/Force Setpoints", forceSetpoints); Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedStates); } + /** + * Runs the modules to the specified ChassisSpeeds (robot velocity) + * + * @param speeds the ChassisSpeeds to run the drivetrain at + * @param openLoop boolean for if the drivetrain should run with feedforward control (open loop) + * or with feedback control (closed loop) + */ + private void drive(ChassisSpeeds speeds, boolean openLoop) { + drive(speeds, openLoop, new double[] {0.0, 0.0, 0.0, 0.0}, new double[] {0.0, 0.0, 0.0, 0.0}); + } + /** * Drive closed-loop at robot relative speeds * @@ -1008,7 +1042,11 @@ public Consumer choreoDriveController() { sample.getChassisSpeeds().plus(feedback), getPose().getRotation()); Logger.recordOutput("Choreo/Target Speeds Robot Relative", speeds); - this.drive(speeds, false); + // These are field relative i believe + double[] moduleForcesX = sample.moduleForcesX(); + double[] moduleForcesY = sample.moduleForcesY(); + + this.drive(speeds, false, moduleForcesX, moduleForcesY); }; } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/Module.java b/src/main/java/frc/robot/subsystems/swerve/module/Module.java index f6c8490d..244fa8ea 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/Module.java @@ -73,10 +73,27 @@ public void stop() { * @return the optimized state */ public SwerveModuleState runClosedLoop(SwerveModuleState state) { + return runClosedLoop(state, 0.0, 0.0); + } + + public SwerveModuleState runClosedLoop( + SwerveModuleState state, double forceXNewtons, double forceYNewtons) { state.optimize(getAngle()); + // Force on the motor is the total force vector projected onto the velocity vector + // to project a onto b take ||a||*cos(theta) where theta is the angle between the two vectors + // We want the magnitude of the projection, so we can ignore the direction of this later + final var theta = Math.atan2(forceYNewtons, forceXNewtons) - inputs.turnPosition.getRadians(); + double forceNewtons = Math.hypot(forceXNewtons, forceYNewtons) * Math.cos(theta); + if (Math.signum(forceNewtons) * Math.signum(state.speedMetersPerSecond) < 0) { + forceNewtons = 0; + } + io.setTurnPositionSetpoint(state.angle); - io.setDriveVelocitySetpoint(state.speedMetersPerSecond); + io.setDriveVelocitySetpoint(state.speedMetersPerSecond, forceNewtons); + + Logger.recordOutput( + "Swerve/" + constants.prefix + " Module/Force Feedforward Newtons", forceNewtons); return state; } diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java index 13ec9556..e7213c83 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOReal.java @@ -214,7 +214,17 @@ public void setDriveVoltage(double volts) { } public void setDriveVelocitySetpoint(double setpointMetersPerSecond) { - driveTalon.setControl(driveVelocityControl.withVelocity(setpointMetersPerSecond)); + setDriveVelocitySetpoint(setpointMetersPerSecond, 0.0); + } + + public void setDriveVelocitySetpoint(double setpointMetersPerSecond, double forceNewtons) { + // Convert the requested force to the current required to output that force + // Conversion factor: Stall torque / Stall current (aka kT) + double forceFFAmps = forceNewtons * 9.37 / 483; + // Kelpie had no conversion from force to torque. maybe we should add this?? idk? comment claims + // its handled in sensor to mech ratio + driveTalon.setControl( + driveVelocityControl.withVelocity(setpointMetersPerSecond).withFeedForward(forceFFAmps)); } public void setTurnVoltage(double volts) { From f48ed6abc201b5ddeb1a7192ac8d79e0ada616b7 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 1 Apr 2026 13:29:44 -0700 Subject: [PATCH 2/3] Doc comments --- .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 4 +++- .../frc/robot/subsystems/swerve/module/Module.java | 12 ++++++++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index b81c4ec2..4772de50 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -426,11 +426,13 @@ private void updateVision() { } /** - * Runs the modules to the specified ChassisSpeeds (robot velocity) + * Runs the modules to the specified ChassisSpeeds (robot velocity) and with the specified force feedforwards * * @param speeds the ChassisSpeeds to run the drivetrain at * @param openLoop boolean for if the drivetrain should run with feedforward control (open loop) * or with feedback control (closed loop) + * @param moduleForcesXNewton the x-components of the force feedforwards. Should be in order FL, FR, BL, BR. Should have the same length as the modules array + * @param moduleForcesYNewton the y-components of the force feedforwards. Same order and length as x-components */ private void drive( ChassisSpeeds speeds, diff --git a/src/main/java/frc/robot/subsystems/swerve/module/Module.java b/src/main/java/frc/robot/subsystems/swerve/module/Module.java index 244fa8ea..19594fc8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/Module.java @@ -62,6 +62,7 @@ public void setTurnVoltage(double volts) { io.setTurnVoltage(volts); } + /** Stops the module */ public void stop() { io.setDriveVoltage(0); } @@ -76,6 +77,13 @@ public SwerveModuleState runClosedLoop(SwerveModuleState state) { return runClosedLoop(state, 0.0, 0.0); } + /** + * Runs closed loop to the specified state, with a force feedforward. Used for automated actions, like auto. + * @param state setpoint state + * @param forceXNewtons the x-component of the requested force vector + * @param forceYNewtons the y-component of the requested force vector + * @return the optimized state + */ public SwerveModuleState runClosedLoop( SwerveModuleState state, double forceXNewtons, double forceYNewtons) { state.optimize(getAngle()); @@ -83,8 +91,8 @@ public SwerveModuleState runClosedLoop( // Force on the motor is the total force vector projected onto the velocity vector // to project a onto b take ||a||*cos(theta) where theta is the angle between the two vectors // We want the magnitude of the projection, so we can ignore the direction of this later - final var theta = Math.atan2(forceYNewtons, forceXNewtons) - inputs.turnPosition.getRadians(); - double forceNewtons = Math.hypot(forceXNewtons, forceYNewtons) * Math.cos(theta); + final double thetaRads = Math.atan2(forceYNewtons, forceXNewtons) - inputs.turnPosition.getRadians(); + double forceNewtons = Math.hypot(forceXNewtons, forceYNewtons) * Math.cos(thetaRads); if (Math.signum(forceNewtons) * Math.signum(state.speedMetersPerSecond) < 0) { forceNewtons = 0; } From a9e48a65cc5bf287df2b959398a37b80bcdf8cb0 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 1 Apr 2026 13:33:33 -0700 Subject: [PATCH 3/3] Spotless --- .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 9 ++++++--- .../java/frc/robot/subsystems/swerve/module/Module.java | 7 +++++-- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 4772de50..d52d564c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -426,13 +426,16 @@ private void updateVision() { } /** - * Runs the modules to the specified ChassisSpeeds (robot velocity) and with the specified force feedforwards + * Runs the modules to the specified ChassisSpeeds (robot velocity) and with the specified force + * feedforwards * * @param speeds the ChassisSpeeds to run the drivetrain at * @param openLoop boolean for if the drivetrain should run with feedforward control (open loop) * or with feedback control (closed loop) - * @param moduleForcesXNewton the x-components of the force feedforwards. Should be in order FL, FR, BL, BR. Should have the same length as the modules array - * @param moduleForcesYNewton the y-components of the force feedforwards. Same order and length as x-components + * @param moduleForcesXNewton the x-components of the force feedforwards. Should be in order FL, + * FR, BL, BR. Should have the same length as the modules array + * @param moduleForcesYNewton the y-components of the force feedforwards. Same order and length as + * x-components */ private void drive( ChassisSpeeds speeds, diff --git a/src/main/java/frc/robot/subsystems/swerve/module/Module.java b/src/main/java/frc/robot/subsystems/swerve/module/Module.java index 19594fc8..0f4d45f5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/Module.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/Module.java @@ -78,7 +78,9 @@ public SwerveModuleState runClosedLoop(SwerveModuleState state) { } /** - * Runs closed loop to the specified state, with a force feedforward. Used for automated actions, like auto. + * Runs closed loop to the specified state, with a force feedforward. Used for automated actions, + * like auto. + * * @param state setpoint state * @param forceXNewtons the x-component of the requested force vector * @param forceYNewtons the y-component of the requested force vector @@ -91,7 +93,8 @@ public SwerveModuleState runClosedLoop( // Force on the motor is the total force vector projected onto the velocity vector // to project a onto b take ||a||*cos(theta) where theta is the angle between the two vectors // We want the magnitude of the projection, so we can ignore the direction of this later - final double thetaRads = Math.atan2(forceYNewtons, forceXNewtons) - inputs.turnPosition.getRadians(); + final double thetaRads = + Math.atan2(forceYNewtons, forceXNewtons) - inputs.turnPosition.getRadians(); double forceNewtons = Math.hypot(forceXNewtons, forceYNewtons) * Math.cos(thetaRads); if (Math.signum(forceNewtons) * Math.signum(state.speedMetersPerSecond) < 0) { forceNewtons = 0;