It works with canandgyro

Frc5572 · Dec 9, 2024 · d4ee169 · d4ee169
1 parent 1f981bf
commit d4ee169
Show file tree

Hide file tree

Showing 6 changed files with 65 additions and 12 deletions.
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java
@@ -110,6 +110,19 @@ public SwerveModuleState getState() {
             Rotation2d.fromRotations(inputs.angleMotorSelectedPosition));
     }
 
+    /**
+     * Get the current Swerve Module State
+     *
+     * @return The current {@link SwerveModuleState}
+     */
+    public SwerveModuleState getStateAbs() {
+        return new SwerveModuleState(
+            Conversions.rotationPerSecondToMetersPerSecond(inputs.driveMotorSelectedSensorVelocity,
+                Constants.Swerve.wheelCircumference),
+            Rotation2d.fromRotations(
+                inputs.absolutePositionAngleEncoder - this.angleOffset.getRotations()));
+    }
+
     /**
      * Get the current Swerve Module Position
      *

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.util.Color;
 import frc.lib.util.FieldConstants;
+import frc.robot.subsystems.swerve.Swerve.SwerveGyro;
 
 /**
  * Constants file.
@@ -188,7 +189,11 @@ public static final class Swerve {
 
         public static final edu.wpi.first.wpilibj.SPI.Port navXID =
             edu.wpi.first.wpilibj.SPI.Port.kMXP;
-        public static final boolean invertGyro = true;
+
+        public static final SwerveGyro selectedGyro =
+            frc.robot.subsystems.swerve.Swerve.SwerveGyro.CANAND;
+        public static final boolean invertGyro = true; // Not inverted to use with CANand Gyro,
+                                                       // invert for NavX
         public static final boolean isFieldRelative = true;
         public static final boolean isOpenLoop = false;
 
@@ -215,7 +220,8 @@ public static final class Swerve {
         public static final double angleGearRatio = (12.8 / 1.0); // (150 / 7) : 1
 
         /* Motor Inverts */
-        public static final InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive;
+        public static final InvertedValue angleMotorInvert =
+            InvertedValue.CounterClockwise_Positive;
         public static final InvertedValue driveMotorInvert =
             InvertedValue.CounterClockwise_Positive;
 
@@ -259,8 +265,8 @@ public static final class Swerve {
 
         /* Swerve Profiling Values */
         /** Meters per Second */
-        public static final double maxSpeed = 10.0;
-        public static final double AUTO_MAX_SPEED = 3.0;
+        public static final double maxSpeed = 1;
+        public static final double AUTO_MAX_SPEED = 1;
         /** Radians per Second */
         public static final double maxAngularVelocity = 15.0;
 
@@ -278,7 +284,7 @@ public static final class Mod0 {
             public static final int angleMotorID = 6;
             public static final int canCoderID = 4;
             // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(183.955078125);
-            public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.376221 + 0.75);
+            public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.37451171875);
 
         }
 
@@ -290,7 +296,7 @@ public static final class Mod1 {
             public static final int angleMotorID = 5;
             public static final int canCoderID = 3;
             // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(325.01953125);
-            public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.355957 + 0.25);
+            public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.146728515625);
 
         }
 
@@ -302,7 +308,7 @@ public static final class Mod2 {
             public static final int angleMotorID = 7;
             public static final int canCoderID = 2;
             // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(124.62890625);
-            public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.223145 + 0.75);
+            public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.219970703125);
 
         }
 
@@ -314,7 +320,7 @@ public static final class Mod3 {
             public static final int angleMotorID = 8;
             public static final int canCoderID = 1;
             // public static final Rotation2d angleOffset = Rotation2d.fromDegrees(295.400390625);
-            public static final Rotation2d angleOffset = Rotation2d.fromRotations(0.095459 + 0.25);
+            public static final Rotation2d angleOffset = Rotation2d.fromRotations(-0.40771484375);
         }
 
         public static final HolonomicPathFollowerConfig pathFollowerConfig =

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -53,7 +53,7 @@ public RobotContainer(RobotRunType runtimeType) {
         s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver,
             Constants.Swerve.isFieldRelative, Constants.Swerve.isOpenLoop));
         // Configure the button bindings
-        CanandEventLoop.getInstance();
+        // CanandEventLoop.getInstance();
         configureButtonBindings();
     }
 

diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
@@ -35,6 +35,10 @@ public class Swerve extends SubsystemBase {
     private SwerveIO swerveIO;
 
 
+    public enum SwerveGyro {
+        NAVX, CANAND
+    }
+
     /**
      * Swerve Subsystem
      */
@@ -132,6 +136,15 @@ public SwerveModuleState[] getModuleStates() {
         return states;
     }
 
+    @AutoLogOutput(key = "Absolute States")
+    public SwerveModuleState[] gModuleStates() {
+        SwerveModuleState[] states = new SwerveModuleState[4];
+        for (SwerveModule mod : swerveMods) {
+            states[mod.moduleNumber] = mod.getStateAbs();
+        }
+        return states;
+    }
+
     /**
      * Get Swerve Module Positions
      *
@@ -180,9 +193,21 @@ public Rotation2d getHeading() {
      * @return Current rotation/yaw of gyro as {@link Rotation2d}
      */
     public Rotation2d getGyroYaw() {
-        float yaw = inputs.yaw;
-        return (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(-yaw)
-            : Rotation2d.fromDegrees(yaw);
+        Rotation2d robotYaw = switch (Constants.Swerve.selectedGyro) {
+            case NAVX -> {
+                float yaw = inputs.yaw;
+                yield (Constants.Swerve.invertGyro) ? Rotation2d.fromDegrees(-yaw)
+                    : Rotation2d.fromDegrees(yaw);
+
+            }
+            case CANAND -> {
+                double yaw = inputs.newyaw;
+                yield Rotation2d.fromRotations(yaw);
+
+            }
+            default -> new Rotation2d();
+        };
+        return robotYaw;
     }
 
     /**

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
@@ -15,6 +15,10 @@ public static class SwerveInputs {
         public float yaw;
         public float roll;
         public float pitch;
+
+        public double newyaw;
+        public double newroll;
+        public double newpitch;
     }
 
     public default void updateInputs(SwerveInputs inputs) {}

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems.swerve;
 
 import com.kauailabs.navx.frc.AHRS;
+import com.reduxrobotics.sensors.canandgyro.Canandgyro;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.lib.util.swerve.SwerveModule;
 import frc.lib.util.swerve.SwerveModuleReal;
@@ -10,6 +11,7 @@
 public class SwerveReal implements SwerveIO {
 
     private AHRS gyro = new AHRS(Constants.Swerve.navXID);
+    private Canandgyro newGyro = new Canandgyro(1);
 
     /** Real Swerve Initializer */
     public SwerveReal() {}
@@ -19,6 +21,9 @@ public void updateInputs(SwerveInputs inputs) {
         inputs.yaw = gyro.getYaw();
         inputs.pitch = gyro.getPitch();
         inputs.roll = gyro.getRoll();
+        inputs.newyaw = newGyro.getYaw();
+        inputs.newpitch = newGyro.getPitch();
+        inputs.newroll = newGyro.getRoll();
 
     }