kinda works

Frc5572 · Oct 4, 2024 · b8c3ede · b8c3ede
1 parent 1d5e7a4
commit b8c3ede
Show file tree

Hide file tree

Showing 6 changed files with 54 additions and 53 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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());
-Original file line number
+Diff line change
@@ Expand Up / @@ -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
@@ Expand Down @@