Merge pull request #53 from 4201VitruvianBots/field-constants

Field constants

Author: gavinskycastle

src/main/java/frc/robot/Constants.java

@@ -14,6 +14,7 @@
 import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.derive;
 
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.signals.GravityTypeValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.pathplanner.lib.config.PIDConstants;
@@ -121,8 +122,8 @@ public Angle getAngle() {
   }
 
   public class CAN {
-    public static final String rioCanbus = "rio";
-    public static final String driveBaseCanbus = "drivebase";
+    public static final CANBus roboRIO = new CANBus("rio");
+    public static final CANBus driveBase = new CANBus("drivebase");
 
     public static final int pigeon = 9;
 

src/main/java/frc/robot/Robot.java

@@ -63,6 +63,8 @@ public void robotPeriodic() {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    m_robotContainer.robotPeriodic();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */

src/main/java/frc/robot/RobotContainer.java

@@ -30,10 +30,15 @@
 import frc.robot.commands.intake.RunIntake;
 import frc.robot.commands.shooter.Shoot;
 import frc.robot.commands.shooter.ShootManualFlywheel;
+import frc.robot.constants.FIELD;
 import frc.robot.generated.WoodBotConstants;
 import frc.robot.simulation.Robot2d;
 import frc.robot.subsystems.*;
 import frc.robot.subsystems.Climber;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Indexer;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Uptake;
 import frc.team4201.lib.simulation.FieldSim;
 import frc.team4201.lib.utils.HubTracker;
 import frc.team4201.lib.utils.Telemetry;
@@ -88,13 +93,13 @@ public boolean isHubActive() {
   }
 
   @NotLogged
-  private double MaxSpeed =
-      WoodBotConstants.kSpeedAt12Volts.in(MetersPerSecond); // Kspeed at 12 volts desired top speed
+  private final double MaxSpeed =
+      WoodBotConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeed at 12 volts desired top speed
 
   private Boolean m_flipToRight = false;
 
   @NotLogged
-  private double MaxAngularRate =
+  private final double MaxAngularRate =
       RotationsPerSecond.of(SWERVE.kMaxRotationRadiansPerSecond)
           .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
 
@@ -106,7 +111,7 @@ public boolean isHubActive() {
 
   private Robot2d m_robotSim;
   private final Telemetry m_telemetry = new Telemetry(MaxSpeed, SWERVE.kModuleTranslations);
-  private final FieldSim m_fieldSim = new FieldSim();
+  private FieldSim m_fieldSim = new FieldSim();
 
   @Logged(name = "AutoChooser")
   private final SendableChooser<Command> m_autoChooser = new SendableChooser<>();
@@ -116,6 +121,8 @@ public boolean isHubActive() {
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the trigger bindings
+    FIELD.initializeConstants();
+    FIELD.updateConstants();
     initializeSubSystems();
     configureBindings();
     initSmartDashboard();
@@ -145,6 +152,7 @@ private void initializeSubSystems() {
                       rotationRate); // Drive counterclockwise with negative X (left)
               return drive;
             }));
+    m_fieldSim = new FieldSim();
     m_flywheel = new Flywheel();
     m_controls = new Controls();
     m_vision = new Vision(m_controls);
@@ -162,6 +170,8 @@ private void initializeSubSystems() {
     m_led.setDefaultCommand(new UpdateLEDs(m_led, m_swerveDrive, m_intake, m_climber, m_uptake));
 
     if (Robot.isSimulation()) {
+      FIELD.plotAllPositions(m_fieldSim);
+
       m_robotSim = new Robot2d();
       m_robotSim.registerSubsystems(m_flywheel, m_hood, m_indexer, m_intake, m_uptake);
     }
@@ -177,8 +187,8 @@ private void configureBindings() {
                   new AutoAlignDrive(
                       m_swerveDrive,
                       m_vision,
-                      () -> m_driverController.getLeftY(),
-                      () -> m_driverController.getLeftX()),
+                      m_driverController::getLeftY,
+                      m_driverController::getLeftX),
                   new Shoot(m_flywheel, m_vision)));
     }
 
@@ -189,8 +199,8 @@ private void configureBindings() {
               new AutoAlignDrive(
                   m_swerveDrive,
                   m_vision,
-                  () -> m_driverController.getLeftY(),
-                  () -> m_driverController.getLeftX()));
+                  m_driverController::getLeftY,
+                  m_driverController::getLeftX));
     }
 
     if (m_swerveDrive != null && m_flywheel != null && m_vision != null) {
@@ -201,7 +211,7 @@ private void configureBindings() {
       m_driverController.y().whileTrue(new ShootManualFlywheel(m_flywheel));
     }
 
-    // I forsee a state machine in the future...
+    // I foresee a state machine in the future...
     if (m_uptake != null && m_indexer != null && m_intake != null) {
       m_driverController
           .a()
@@ -244,10 +254,7 @@ private void initSideChooser() {
 
     m_autoSide.addOption("Depot", false);
     m_autoSide.addOption("Outpost", true);
-    m_autoSide.onChange(
-        (Boolean selected) -> {
-          m_flipToRight = selected;
-        });
+    m_autoSide.onChange((Boolean selected) -> m_flipToRight = selected);
   }
 
   private void initSmartDashboard() {
@@ -278,4 +285,8 @@ public Command getAutonomousCommand() {
     // An example command will be run in autonomous
     return m_autoChooser.getSelected();
   }
+
+  public void robotPeriodic() {
+    FIELD.updateCurrentSector(m_swerveDrive.getState().Pose);
+  }
 }

src/main/java/frc/robot/commands/AutoAlignDrive.java

@@ -1,28 +1,26 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Rectangle2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.SWERVE;
 import frc.robot.constants.FIELD;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
-import frc.robot.subsystems.Controls;
 import frc.robot.subsystems.Vision;
 import java.util.function.DoubleSupplier;
 
 public class AutoAlignDrive extends Command {
   private final CommandSwerveDrivetrain m_swerveDrivetrain;
   private final Vision m_vision;
-  Translation2d m_goal = new Translation2d();
+  private Translation2d m_goal = Translation2d.kZero;
 
   private Double kTeleP_Theta = 7.4;
   private Double kTeleD_Theta = 0.3;
   public static final double kTeleI_Theta = 0.0;
 
-  private PIDController m_PidController =
+  private final PIDController m_PidController =
       new PIDController(kTeleP_Theta, kTeleI_Theta, kTeleD_Theta);
 
   private final DoubleSupplier m_throttleInput;
@@ -49,18 +47,14 @@ public void initialize() {
     kTeleP_Theta = m_vision.m_kPAutoAlignSubscriber.getAsDouble();
     kTeleD_Theta = m_vision.m_kDAutoAlignSubscriber.getAsDouble();
     m_PidController.reset();
-    Rectangle2d m_goalZone;
 
-    // If we're outside of our own zone, then we align to pass.
-    if (m_vision.isInOpposingAllianceZone() || m_vision.isInNeutralZone()) {
-      m_goalZone =
-          Controls.isBlueAlliance()
-              ? (m_vision.isInLeftHalf() ? FIELD.blueZoneLeft : FIELD.blueZoneRight)
-              : (m_vision.isInLeftHalf() ? FIELD.redZoneLeft : FIELD.redZoneRight);
-      m_goal = m_goalZone.getCenter().getTranslation();
+    // If we're outside our own zone, then we align to pass.
+    if (m_vision.isInOpposingAllianceSector() || m_vision.isInNeutralSector()) {
+      // TODO: Add pass position as goal
+      m_goal = Translation2d.kZero;
       // If we're in our own zone, then we align to the hub
     } else {
-      m_goal = Controls.isBlueAlliance() ? FIELD.blueAutoHub : FIELD.redAutoHub;
+      m_goal = FIELD.HUB.GOAL.getTargetPosition().toTranslation2d();
     }
   }
 

src/main/java/frc/robot/commands/shooter/Shoot.java

@@ -11,7 +11,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.FLYWHEEL.Shot;
 import frc.robot.constants.FIELD;
-import frc.robot.subsystems.Controls;
 import frc.robot.subsystems.Flywheel;
 import frc.robot.subsystems.Vision;
 
@@ -54,17 +53,13 @@ public Shoot(Flywheel flywheel, Vision vision) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    if (Controls.isBlueAlliance()) {
-      m_goal = FIELD.blueHub;
-    } else {
-      m_goal = FIELD.redHub;
-    }
+    m_goal = FIELD.HUB.GOAL.getTargetPosition().toTranslation2d();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    Shot shot = distanceToShotMap.get(m_vision.getDistancetoHub());
+    Shot shot = distanceToShotMap.get(m_vision.getDistanceToHub());
     m_flywheel.setRPMOutputFOC(shot.shooterRPM);
     // m_shooterHood.setPosition(shot.hoodPosition);
   }