From b8c3ede618e15ee402b5aac243bc59da89d31573 Mon Sep 17 00:00:00 2001 From: Alex Resnick Date: Thu, 3 Oct 2024 22:14:37 -0500 Subject: [PATCH] kinda works --- .../frc/lib/util/swerve/SwerveModule.java | 10 +-- .../frc/lib/util/swerve/SwerveModuleIO.java | 3 +- .../frc/lib/util/swerve/SwerveModuleReal.java | 12 ++- .../frc/lib/util/swerve/SwerveModuleSim.java | 76 ++++++++++--------- src/main/java/frc/robot/Main.java | 1 - src/main/java/frc/robot/RobotContainer.java | 5 +- 6 files changed, 54 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index e7cea05..0623304 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,7 +1,6 @@ package frc.lib.util.swerve; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -21,10 +20,6 @@ public class SwerveModule { private SwerveModuleIO io; private SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged(); - private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward( - Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); - - /** * Swerve Module * @@ -86,10 +81,7 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { double power = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; io.setDriveMotorPower(power); } else { - double driveRPM = Conversions.metersPerSecondToRotationPerSecond( - desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); - double driveFF = driveFeedForward.calculate(desiredState.speedMetersPerSecond); - io.setDriveMotor(driveRPM, driveFF); + io.setDriveMotor(desiredState.speedMetersPerSecond); } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java index 9f82313..f424b13 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java @@ -11,13 +11,14 @@ public static class SwerveModuleInputs { public double driveMotorSelectedSensorVelocity; public double angleMotorSelectedPosition; public double absolutePositionAngleEncoder; + public double[] odometryTimestamps; // public double driveMotorTemp; // public double angleMotorTemp; } public default void updateInputs(SwerveModuleInputs inputs) {} - public default void setDriveMotor(double rpm, double feedforward) {} + public default void setDriveMotor(double mps) {} public default void setDriveMotorPower(double power) {} diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index bccbc00..4782117 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.geometry.Rotation2d; +import frc.lib.math.Conversions; import frc.robot.Constants; /** @@ -103,6 +104,9 @@ private void configDriveMotor() { swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP; swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI; swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD; + swerveDriveFXConfig.Slot0.kS = Constants.Swerve.driveKS; + swerveDriveFXConfig.Slot0.kV = Constants.Swerve.driveKV; + swerveDriveFXConfig.Slot0.kA = Constants.Swerve.driveKA; /* Open and Closed Loop Ramping */ swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = @@ -131,9 +135,11 @@ public void setAngleMotor(double angle) { } @Override - public void setDriveMotor(double rpm, double feedforward) { - driveVelocity.FeedForward = feedforward; - driveVelocity.Velocity = rpm; + public void setDriveMotor(double mps) { + // driveVelocity.FeedForward = feedforward; + double driveRPS = Conversions.metersPerSecondToRotationPerSecond(mps, + Constants.Swerve.wheelCircumference); + driveVelocity.Velocity = driveRPS; mDriveMotor.setControl(driveVelocity); } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java index 78c9422..1ab4c01 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java @@ -2,10 +2,13 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.lib.math.Conversions; import frc.robot.Constants; public class SwerveModuleSim implements SwerveModuleIO { @@ -15,67 +18,66 @@ public class SwerveModuleSim implements SwerveModuleIO { private FlywheelSim turnSim = new FlywheelSim(DCMotor.getFalcon500(1), Constants.Swerve.angleGearRatio, 0.004); - private final Rotation2d turnAbsoluteInitPosition = - new Rotation2d(Math.random() * 2.0 * Math.PI); - - private double turnRelativePositionRad = 0.0; - private double turnAbsolutePositionRad = Math.random() * 2.0 * Math.PI; + private double turnRelativePositionRot = 0.0; + private double turnAbsolutePositionRot = Math.random(); private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; - private final PIDController driveFeedback; - private final PIDController turnFeedback; + + private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + private PIDController driveFeedback = new PIDController(0.1, 0.0, 0.0); + private PIDController turnFeedback = new PIDController(0.05, 0.0, 0.0); public SwerveModuleSim(int moduleNumber) { this.moduleNumber = moduleNumber; System.out.println("[Init] Creating ServeModuleSim"); - driveFeedback = new PIDController(Constants.Swerve.driveKP, Constants.Swerve.driveKI, - Constants.Swerve.driveKD); - turnFeedback = new PIDController(Constants.Swerve.angleKP, Constants.Swerve.angleKI, - Constants.Swerve.angleKD); - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + turnFeedback.enableContinuousInput(-0.5, 0.5); } public void updateInputs(SwerveModuleInputs inputs) { driveSim.update(Constants.loopPeriodSecs); turnSim.update(Constants.loopPeriodSecs); - - double angleDiffRad = turnSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs; - turnRelativePositionRad += angleDiffRad; - turnAbsolutePositionRad += angleDiffRad; - while (turnAbsolutePositionRad < 0) { - turnAbsolutePositionRad += 2.0 * Math.PI; + double angleDiffRot = Units + .radiansToRotations(turnSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs); + turnRelativePositionRot += angleDiffRot; + turnAbsolutePositionRot += angleDiffRot; + while (turnAbsolutePositionRot < 0) { + turnAbsolutePositionRot += 1; } - while (turnAbsolutePositionRad > 2.0 * Math.PI) { - turnAbsolutePositionRad -= 2.0 * Math.PI; + while (turnAbsolutePositionRot > 1) { + turnAbsolutePositionRot -= 1; } - inputs.driveMotorSelectedPosition = Rotation2d - .fromRadians(inputs.driveMotorSelectedPosition - + (driveSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs)) - .getRotations(); + inputs.driveMotorSelectedPosition = + inputs.driveMotorSelectedPosition + Units.radiansToRotations( + (driveSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs)); inputs.driveMotorSelectedSensorVelocity = - Rotation2d.fromRadians(driveSim.getAngularVelocityRadPerSec()).getRotations(); + Units.radiansPerSecondToRotationsPerMinute(driveSim.getAngularVelocityRadPerSec()); - inputs.angleMotorSelectedPosition = - Rotation2d.fromRadians(turnAbsolutePositionRad).getRotations(); - inputs.absolutePositionAngleEncoder = 0; + inputs.angleMotorSelectedPosition = turnRelativePositionRot; + + inputs.absolutePositionAngleEncoder = turnAbsolutePositionRot; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; } - public void setDriveMotor(double rpm, double feedforward) { + public void setDriveMotor(double mps) { + double rpm = Conversions.metersPerSecondToRotationPerSecond(mps, + Constants.Swerve.wheelCircumference); driveFeedback.setSetpoint(rpm); - double volts = - driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec() / (2 * Math.PI)) - + feedforward; - SmartDashboard.putNumber("rpm/" + moduleNumber, rpm); - SmartDashboard.putNumber("ff/" + moduleNumber, feedforward); - SmartDashboard.putNumber("volts/" + moduleNumber, volts); + double driveFF = driveFeedforward.calculate(mps); + SmartDashboard.putNumber("ff/" + moduleNumber, driveFF); + double volts = driveFeedback.calculate(mps) + driveFF; + if (rpm == 0) { + volts = 0; + } + SmartDashboard.putNumber("Drive volts/" + moduleNumber, volts); setDriveVoltage(volts); } public void setAngleMotor(double angle) { turnFeedback.setSetpoint(angle); - double volts = turnFeedback.calculate(turnAbsolutePositionRad / (2 * Math.PI)); + double volts = turnFeedback.calculate(turnAbsolutePositionRot); + SmartDashboard.putNumber("Angle volts/" + moduleNumber, volts); setTurnVoltage(volts); } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 42cea8c..f528372 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -17,7 +17,6 @@ private Main() {} /** * Main initialization function. Do not perform any initialization here. * - * * If you change your main robot class, change the parameter type. * * @param args String args diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 12ae24f..e42fc10 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,6 +50,7 @@ import frc.robot.subsystems.swerve.Swerve; import frc.robot.subsystems.swerve.SwerveIO; import frc.robot.subsystems.swerve.SwerveReal; +import frc.robot.subsystems.swerve.SwerveSim; /** @@ -136,7 +137,7 @@ public RobotContainer(RobotRunType runtimeType) { cameras = new PhotonCameraWrapper[] {new PhotonCameraWrapper( new PhotonReal(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), - Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT),}; + Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT)}; switch (runtimeType) { case kReal: @@ -147,7 +148,7 @@ public RobotContainer(RobotRunType runtimeType) { elevatorWrist = new ElevatorWrist(new ElevatorWristReal(), operator); break; case kSimulation: - s_Swerve = new Swerve(new SwerveIO() {}, cameras); + s_Swerve = new Swerve(new SwerveSim(), cameras); shooter = new Shooter(new ShooterSim()); intake = new Intake(new IntakeIOSim()); indexer = new Indexer(new IndexerIOSim());