implement pigeon which both robots will run

frc5417 · Jan 11, 2025 · 10dff8a · 10dff8a
1 parent ac89702
commit 10dff8a
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 29 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -5,7 +5,8 @@
 package frc.robot;
 
 
-import com.studica.frc.AHRS;
+import com.ctre.phoenix6.hardware.Pigeon2;
+
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -15,7 +16,6 @@
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.AutonLoader;
 import frc.robot.commands.TeleopDrive;
@@ -35,10 +35,10 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
 
 
-  public static AHRS ahrs = new AHRS(null);
+  public static Pigeon2 pigeon = new Pigeon2(59, "canivore");
 
-  public static Kinematics kinematics = new Kinematics(ahrs);
-  public static DriveBase driveBase = new DriveBase(kinematics, ahrs);
+  public static Kinematics kinematics = new Kinematics(pigeon);
+  public static DriveBase driveBase = new DriveBase(kinematics, pigeon);
   // public static Elevator elevator = new Elevator();
   public static Bezier bezier = new Bezier();
 

diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java
@@ -1,7 +1,7 @@
 package frc.robot.subsystems;
 import java.util.function.Supplier;
 
-import com.studica.frc.AHRS;
+import com.ctre.phoenix6.hardware.Pigeon2;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -21,7 +21,7 @@ public class DriveBase extends SubsystemBase {
 
     private static Module.ModuleState targetModuleStates[];
     private final Kinematics m_kinematics;
-    private final AHRS m_ahrs;
+    private final Pigeon2 m_pigeon;
 
     public static Module[] moduleGroup;
 
@@ -51,9 +51,6 @@ public class DriveBase extends SubsystemBase {
 
     ChassisSpeeds autoSetSpeed = new ChassisSpeeds();
 
-    private PIDController snapToNearestTheta = new PIDController(1, 0, 0);
-    private boolean snappingOn = false;
-
     private double[] chassisSpeed_pub = {0.0, 0.0, 0.0};
 
     public Supplier<double[]> chassisSpeed_supp = ()->chassisSpeed_pub;
@@ -63,9 +60,9 @@ public class DriveBase extends SubsystemBase {
     public Supplier<String> alliancecolor_supp = ()->alliancecolor_pub;
 
 
-    public DriveBase(Kinematics kinematics, AHRS ahrs) {
+    public DriveBase(Kinematics kinematics, Pigeon2 pigeon) {
         m_kinematics = kinematics;
-        m_ahrs = ahrs;
+        m_pigeon = pigeon;
 
         moduleGroup = new Module[4];
         for (int i = 0; i < 4; i++) {
@@ -80,7 +77,7 @@ public DriveBase(Kinematics kinematics, AHRS ahrs) {
             targetModuleStates[i] = new Module.ModuleState(0, Constants.ModuleConstants.motorDegrees[i] * (Math.PI/180));
 
         m_sdkOdom = new SwerveDriveOdometry(
-            m_skdKine, m_ahrs.getRotation2d(), new SwerveModulePosition[] {
+            m_skdKine, m_pigeon.getRotation2d(), new SwerveModulePosition[] {
                 new SwerveModulePosition(odomDeltas[2], new Rotation2d(odomAngles[2])),
                 new SwerveModulePosition(odomDeltas[0], new Rotation2d(odomAngles[0])),
                 new SwerveModulePosition(odomDeltas[3], new Rotation2d(odomAngles[3])),
@@ -132,7 +129,7 @@ public void resetOdometry(Pose2d pose) {
                 new SwerveModulePosition(odomDeltas[3], new Rotation2d(odomAngles[3])),
                 new SwerveModulePosition(odomDeltas[1], new Rotation2d(odomAngles[1]))
         };
-        m_sdkOdom.resetPosition(m_ahrs.getRotation2d(), modulePositions, inverted);
+        m_sdkOdom.resetPosition(m_pigeon.getRotation2d(), modulePositions, inverted);
     }
 
     public void setHardStates(Module.ModuleState[] targetState) {
@@ -171,10 +168,6 @@ public double smallestAngle(double largeAngle) {
         }
     }
 
-    public void setSnapping(boolean on_or_off) {
-        snappingOn = on_or_off;
-    }
-
     @Override
     public void periodic() {
         for (int i = 0; i < 4; i++) {
@@ -183,17 +176,12 @@ public void periodic() {
             odomAngles[i] = smallestAngle(moduleGroup[i].getAngleInRadians());
         }
 
-        m_sdkOdom.update(m_ahrs.getRotation2d(), new SwerveModulePosition[] {
+        m_sdkOdom.update(m_pigeon.getRotation2d(), new SwerveModulePosition[] {
             new SwerveModulePosition(Math.abs(odomDeltas[2]), new Rotation2d(odomAngles[2])),
             new SwerveModulePosition(Math.abs(odomDeltas[0]), new Rotation2d(odomAngles[0])),
             new SwerveModulePosition(Math.abs(odomDeltas[3]), new Rotation2d(odomAngles[3])),
             new SwerveModulePosition(Math.abs(odomDeltas[1]), new Rotation2d(odomAngles[1]))
         });
-
-        if (snappingOn) {
-            snapToNearestTheta.setSetpoint(m_ahrs.getYaw()-(m_ahrs.getYaw()%90.0));
-            setDriveSpeed(RobotContainer.getSaturatedSpeeds(0, 0, snapToNearestTheta.calculate(m_ahrs.getYaw())));
-        }
 
         field.setRobotPose(getCurrentPose());
     }

diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java
@@ -6,7 +6,7 @@
 
 import frc.robot.Constants;
 
-import com.studica.frc.AHRS;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 
 public class Kinematics {
@@ -15,12 +15,12 @@ public class Kinematics {
   private double[] theta = new double[4];
   public boolean fieldCentric;
   private double gyro = 0.0;
-  private final AHRS m_ahrs;
+  private final Pigeon2 m_pigeon;
 
 
-  public Kinematics(AHRS ahrs) {
+  public Kinematics(Pigeon2 pigeon) {
     this.fieldCentric = Constants.OperatorConstants.fieldCentric;
-    m_ahrs = ahrs;
+    m_pigeon = pigeon;
   }
 
   private double[][] computeStrafe(double joy_x, double joy_y) {
@@ -91,7 +91,7 @@ public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisS
     double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond; 
 
     if (fieldCentric) {
-      this.gyro = this.m_ahrs.getYaw();
+      this.gyro = this.m_pigeon.getYaw().getValueAsDouble();
       this.gyro *= Math.PI/180;
     } else {
       this.gyro = 0;