diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index c3173164..da7a5d31 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -48,13 +48,13 @@ public class CommandConstants { AutonomousConstants.ROBOT_CONFIG.moduleLocations, RobotContainer.SWERVE::getDriveWheelPositionsRadians, () -> RobotContainer.SWERVE.getHeading().getRadians(), - (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null), + (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), RobotContainer.SWERVE ), CALCULATE_CAMERA_POSITION_COMMAND = new CameraPositionCalculationCommand( RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose, Rotation2d.fromDegrees(0), - (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null), + (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), RobotContainer.SWERVE ); diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 8e74ec3e..ae69acd7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -34,7 +34,6 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.Subs DISABLED_TRIGGER.onFalse(new InstantCommand(() -> { setAllSubsystemsBrakeAsync(true); IS_BRAKING = true; - RobotContainer.SWERVE.resetSetpoint(); }).ignoringDisable(true)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 84aff59b..8789cf5d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -2,7 +2,6 @@ import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.swerve.SwerveSetpoint; -import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -19,7 +18,6 @@ import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants; -import lib.hardware.RobotHardwareStats; import lib.hardware.phoenix6.Phoenix6SignalThread; import lib.hardware.phoenix6.pigeon2.Pigeon2Gyro; import lib.hardware.phoenix6.pigeon2.Pigeon2Signal; @@ -33,10 +31,8 @@ public class Swerve extends MotorSubsystem { private final Pigeon2Gyro gyro = SwerveConstants.GYRO; private final SwerveModule[] swerveModules = SwerveConstants.SWERVE_MODULES; private final Phoenix6SignalThread phoenix6SignalThread = Phoenix6SignalThread.getInstance(); - private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(AutonomousConstants.ROBOT_CONFIG, SwerveModuleConstants.MAXIMUM_MODULE_ROTATIONAL_SPEED_RADIANS_PER_SECOND); public Pose2d targetPathPlannerPose = new Pose2d(); public boolean isPathPlannerDriving = false; - private SwerveSetpoint previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules)); public Swerve() { setName("Swerve"); @@ -53,7 +49,7 @@ public void setBrake(boolean brake) { public void sysIDDrive(double targetCurrent) { SwerveModuleState[] a = SwerveConstants.KINEMATICS.toSwerveModuleStates(new ChassisSpeeds(0, 0, 2)); for (int i = 0; i < 4; i++) { - swerveModules[i].setDriveMotorTargetCurrent(targetCurrent); + swerveModules[i].setDriveMotorTargetVoltage(targetCurrent); swerveModules[i].setTargetAngle(a[i].angle); } } @@ -176,22 +172,15 @@ public void drivePathPlanner(ChassisSpeeds targetPathPlannerFeedforwardSpeeds, b * Used for PathPlanner because it generates setpoints automatically. * * @param targetSpeeds the pre generated robot relative target speeds - * @param feedforwards the feedforwards to use */ - public void selfRelativeDriveWithoutSetpointGeneration(ChassisSpeeds targetSpeeds, DriveFeedforwards feedforwards) { + public void selfRelativeDriveWithoutSetpointGeneration(ChassisSpeeds targetSpeeds) { // if (isStill(targetSpeeds)) { // stop(); // return; // } final SwerveModuleState[] swerveModuleStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(targetSpeeds); - final double[] targetForcesNm; - if (feedforwards == null) - targetForcesNm = new double[]{0, 0, 0, 0}; - else - targetForcesNm = feedforwards.linearForcesNewtons(); - - setTargetModuleStates(swerveModuleStates, targetForcesNm); + setTargetModuleStates(swerveModuleStates); } public Rotation2d getDriveRelativeAngle() { @@ -199,10 +188,6 @@ public Rotation2d getDriveRelativeAngle() { return Flippable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; } - public void resetSetpoint() { - previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules)); - } - public void initializeDrive(boolean shouldUseClosedLoop) { setClosedLoop(shouldUseClosedLoop); resetRotationController(); @@ -279,23 +264,18 @@ void selfRelativeDrive(double xPower, double yPower, FlippableRotation2d targetA * @param targetSpeeds the desired robot-relative targetSpeeds */ public void selfRelativeDrive(ChassisSpeeds targetSpeeds) { - previousSetpoint = setpointGenerator.generateSetpoint( - previousSetpoint, - targetSpeeds, - RobotHardwareStats.getPeriodicTimeSeconds() - ); - -// if (isStill(previousSetpoint.robotRelativeSpeeds())) { +// if (isStill(targetSpeeds) { // stop(); // return; // } - setTargetModuleStates(previousSetpoint.moduleStates(), previousSetpoint.feedforwards().linearForcesNewtons()); + final SwerveModuleState[] swerveModuleStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(targetSpeeds); + setTargetModuleStates(swerveModuleStates); } - private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates, double[] targetForcesNm) { + private void setTargetModuleStates(SwerveModuleState[] swerveModuleStates) { for (int i = 0; i < swerveModules.length; i++) - swerveModules[i].setTargetState(swerveModuleStates[i], targetForcesNm[i]); + swerveModules[i].setTargetState(swerveModuleStates[i]); } private void setClosedLoop(boolean shouldUseClosedLoop) { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java index dcaf72e3..f53c9a6c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java @@ -1,8 +1,7 @@ package frc.trigon.robot.subsystems.swerve.swervemodule; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -25,9 +24,8 @@ public class SwerveModule { private final CANcoderEncoder steerEncoder; private final PositionVoltage steerPositionRequest = new PositionVoltage(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC); private final double wheelDiameter; - private final VelocityTorqueCurrentFOC driveVelocityRequest = new VelocityTorqueCurrentFOC(0).withUpdateFreqHz(1000); + private final VelocityVoltage driveVelocityRequest = new VelocityVoltage(0).withUpdateFreqHz(1000); private final VoltageOut driveVoltageRequest = new VoltageOut(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC); - private final TorqueCurrentFOC driveTorqueCurrentFOCRequest = new TorqueCurrentFOC(0); private boolean shouldDriveMotorUseClosedLoop = true; private SwerveModuleState targetState = new SwerveModuleState(); private double[] @@ -50,15 +48,14 @@ public SwerveModule(int moduleID, double offsetRotations, double wheelDiameter) configureHardware(offsetRotations); } - public void setTargetState(SwerveModuleState targetState, double targetForceNm) { + public void setTargetState(SwerveModuleState targetState) { if (willOptimize(targetState)) { targetState.optimize(getCurrentAngle()); - targetForceNm *= -1; } this.targetState = targetState; setTargetAngle(targetState.angle); - setTargetVelocity(targetState.speedMetersPerSecond, targetForceNm); + setTargetVelocity(targetState.speedMetersPerSecond); } public void setBrake(boolean brake) { @@ -96,8 +93,8 @@ public void shouldDriveMotorUseClosedLoop(boolean shouldDriveMotorUseClosedLoop) this.shouldDriveMotorUseClosedLoop = shouldDriveMotorUseClosedLoop; } - public void setDriveMotorTargetCurrent(double targetCurrent) { - driveMotor.setControl(driveTorqueCurrentFOCRequest.withOutput(targetCurrent)); + public void setDriveMotorTargetVoltage(double targetVoltage) { + driveMotor.setControl(driveVoltageRequest.withOutput(targetVoltage)); } public void setTargetAngle(Rotation2d angle) { @@ -145,21 +142,19 @@ private boolean willOptimize(SwerveModuleState state) { * The target velocity is set using either closed loop or open loop depending on {@link this#shouldDriveMotorUseClosedLoop}. * * @param targetVelocityMetersPerSecond the target velocity, in meters per second - * @param targetForceNm the target force of the module in newton meters */ - private void setTargetVelocity(double targetVelocityMetersPerSecond, double targetForceNm) { + private void setTargetVelocity(double targetVelocityMetersPerSecond) { if (shouldDriveMotorUseClosedLoop) { - setTargetClosedLoopVelocity(targetVelocityMetersPerSecond, targetForceNm); + setTargetClosedLoopVelocity(targetVelocityMetersPerSecond); return; } setTargetOpenLoopVelocity(targetVelocityMetersPerSecond); } - private void setTargetClosedLoopVelocity(double targetVelocityMetersPerSecond, double targetForceNm) { + private void setTargetClosedLoopVelocity(double targetVelocityMetersPerSecond) { final double targetVelocityRotationsPerSecond = metersToDriveWheelRotations(targetVelocityMetersPerSecond); - final double targetAccelerationRotationsPerSecondSquared = metersToDriveWheelRotations(targetForceNm); - driveMotor.setControl(driveVelocityRequest.withVelocity(targetVelocityRotationsPerSecond).withAcceleration(targetAccelerationRotationsPerSecondSquared)); + driveMotor.setControl(driveVelocityRequest.withVelocity(targetVelocityRotationsPerSecond)); } private void setTargetOpenLoopVelocity(double targetVelocityMetersPerSecond) { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index e1f0c6c7..b7432bf2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -58,7 +58,7 @@ static SimpleMotorSimulation createSteerMotorSimulation() { return new SimpleMotorSimulation(STEER_MOTOR_GEARBOX, REAR_STEER_MOTOR_GEAR_RATIO, STEER_MOMENT_OF_INERTIA); } - static TalonFXConfiguration generateDriveMotorConfiguration() { + public static TalonFXConfiguration generateDriveMotorConfiguration() { final TalonFXConfiguration config = new TalonFXConfiguration(); config.Audio.BeepOnBoot = false; @@ -77,12 +77,12 @@ static TalonFXConfiguration generateDriveMotorConfiguration() { config.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; config.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.1; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 50 : 50; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 50; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.4708 : 5.25; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : DRIVE_MOTOR_GEAR_RATIO / (1 / DCMotor.getKrakenX60Foc(1).KtNMPerAmp); + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 5.25; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.020691 : 0; config.Feedback.VelocityFilterTimeConstant = 0;