Move poseestimator into swerve drive subsystem

Creekside7688 · Feb 11, 2024 · 824a05f · 824a05f
1 parent b0f1881
commit 824a05f
Show file tree

Hide file tree

Showing 5 changed files with 162 additions and 256 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -10,7 +10,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import frc.lib.zylve.Controller;
 import frc.robot.auto.PhotonRunnable;
-import frc.robot.auto.PoseEstimatorSubsystem;
 import frc.robot.auto.commands.AmpAlign;
 import frc.robot.auto.commands.FollowAprilTag;
 import frc.robot.auto.commands.SpeakerAlign;
@@ -32,21 +31,22 @@
 
 public class RobotContainer {
     Controller controller = new Controller(OperatorConstants.CONTROLLER_PORT);
-    private final PhotonCamera photonCamera = new PhotonCamera("Limelight");
     private final PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev);
 
-    private final SwerveDrive swerveDrive = new SwerveDrive();
+    private final PhotonCamera photonCamera = new PhotonCamera("Limelight");
+    private final PhotonRunnable photonRunnable = new PhotonRunnable(photonCamera);
+    // private final PoseEstimatorSubsystem poseEstimator = new PoseEstimatorSubsystem(swerveDrive::getRotation2d, swerveDrive::getModulePositions, photonRunnable);
+
+    private final SwerveDrive swerveDrive = new SwerveDrive(photonRunnable);
     private final Elevator elevator = new Elevator();
     private final Intake intake = new Intake();
     private final Shooter shooter = new Shooter();
 
-    private final PhotonRunnable photonRunnable = new PhotonRunnable(photonCamera);
-    private final PoseEstimatorSubsystem poseEstimator = new PoseEstimatorSubsystem(swerveDrive::getRotation2d, swerveDrive::getModulePositions, photonRunnable);
-    private final FollowAprilTag followAprilTag = new FollowAprilTag(swerveDrive, photonCamera, poseEstimator::getCurrentPose);
+    // private final FollowAprilTag followAprilTag = new FollowAprilTag(swerveDrive, photonCamera, poseEstimator::getCurrentPose);
 
     private final SequentialCommandGroup ampSuperCommand = new SequentialCommandGroup(
         new ParallelCommandGroup(
-            new AmpAlign(swerveDrive, poseEstimator::getCurrentPose),
+            // new AmpAlign(swerveDrive, poseEstimator::getCurrentPose),
             new ElevatorUp(elevator)
         ),
 
@@ -56,7 +56,7 @@ public class RobotContainer {
 
     private final SequentialCommandGroup shooterSuperCommand = new SequentialCommandGroup(
         new ParallelCommandGroup(
-            new SpeakerAlign(swerveDrive, poseEstimator::getCurrentPose),
+            // new SpeakerAlign(swerveDrive, poseEstimator::getCurrentPose),
             new ShooterSpinUp(shooter)
         ),
 
@@ -118,7 +118,7 @@ private void configureSwerveDriveCommands() {
                 )
             );
 
-        controller.getA().whileTrue(followAprilTag);
+        // controller.getA().whileTrue(followAprilTag);
     }
 
     private void configureSuperCommands() {
@@ -130,6 +130,6 @@ public Command getAutonomousCommand() {
     }
 
     public void onAllianceChanged(Alliance alliance) {
-        poseEstimator.setAlliance(alliance);
+        // poseEstimator.setAlliance(alliance);
     }
 }
diff --git a/src/main/java/frc/robot/auto/PoseEstimatorSubsystem.java b/src/main/java/frc/robot/auto/PoseEstimatorSubsystem.java
diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java
@@ -18,7 +18,7 @@ public class ModuleConstants {
 
     // Bevel 45T, First Stage 22T, Bevel Pinion 15T
     public static final double DRIVE_MOTOR_REDUCTION = (45.0 * 22) / (DRIVE_PINION_TEETH * 15);
-    public static final double DRIVE_WHEEL_FREE_SPEED_RPS = (DRIVE_MOTOR_FREE_SPEED_RPS * WHEEL_CIRCUMFERENCE_METRES) / DRIVE_MOTOR_REDUCTION;
+    public static final double DRIVE_WHEEL_FREE_SPEED = (DRIVE_MOTOR_FREE_SPEED_RPS * WHEEL_CIRCUMFERENCE_METRES) / DRIVE_MOTOR_REDUCTION;
 
     public static final double DRIVE_ENCODER_POSITION_FACTOR = (WHEEL_DIAMETER_METRES * Math.PI) / DRIVE_MOTOR_REDUCTION; // Metres
     public static final double DRIVE_ENCODER_VELOCITY_FACTOR = DRIVE_ENCODER_POSITION_FACTOR / 60.0; // Metres per second
@@ -32,12 +32,12 @@ public class ModuleConstants {
     public static final double DRIVE_P = 0.04;
     public static final double DRIVE_I = 0;
     public static final double DRIVE_D = 0;
-    public static final double DRIVE_FF = 1 / DRIVE_WHEEL_FREE_SPEED_RPS;
+    public static final double DRIVE_FF = 1 / DRIVE_WHEEL_FREE_SPEED;
 
     public static final double DRIVE_MINIMUM_OUTPUT = -1;
     public static final double DRIVE_MAXIMUM_OUTPUT = 1;
 
-    public static final double TURN_P = 0.75;
+    public static final double TURN_P = 1;
     public static final double TURN_I = 0;
     public static final double TURN_D = 0;
     public static final double TURN_FF = 0;