diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 141c3d42..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,22 @@ 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, 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 +455,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 +468,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 +1047,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..0f4d45f5 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); } @@ -73,10 +74,37 @@ public void stop() { * @return the optimized state */ 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()); + // 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(); + double forceNewtons = Math.hypot(forceXNewtons, forceYNewtons) * Math.cos(thetaRads); + 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) {