Update pose estimation to use SolvePNP + auto aligning

src/main/java/frc3512/robot/Constants.java

@@ -5,7 +5,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import java.util.List;
@@ -18,36 +17,25 @@ public static final class FieldConstants {
     // List of possible scoring locations as Pose2d objects
     public static final List<Pose2d> scoringPositions =
         List.of(
-            new Pose2d(
-                new Translation2d(0.555, 7.436),
-                Rotation2d.fromRadians(Math.PI)), // Red loading double station
-            new Pose2d(new Translation2d(0.555, 6.146), Rotation2d.fromRadians(Math.PI)),
-            new Pose2d(
-                new Translation2d(15.03, 5.061),
-                Rotation2d.fromDegrees(0.0)), // Red node scoring locations
-            new Pose2d(new Translation2d(15.03, 4.405), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(new Translation2d(15.03, 3.846), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(new Translation2d(15.03, 3.298), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(new Translation2d(15.03, 2.74), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(new Translation2d(15.03, 2.2), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(new Translation2d(15.03, 1.62), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(new Translation2d(15.03, 1.06), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(new Translation2d(15.03, 0.52), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(
-                new Translation2d(15.64, 7.430),
-                Rotation2d.fromDegrees(0.0)), // Blue loading double substation
-            new Pose2d(new Translation2d(15.64, 6.16), Rotation2d.fromDegrees(0.0)),
-            new Pose2d(
-                new Translation2d(1.598, 4.996),
-                Rotation2d.fromRadians(-Math.PI)), // Blue node scoring locations
-            new Pose2d(new Translation2d(1.598, 4.373), Rotation2d.fromRadians(-Math.PI)),
-            new Pose2d(new Translation2d(1.598, 3.85), Rotation2d.fromRadians(-Math.PI)),
-            new Pose2d(new Translation2d(1.598, 3.3), Rotation2d.fromRadians(-Math.PI)),
-            new Pose2d(new Translation2d(1.598, 2.75), Rotation2d.fromRadians(-Math.PI)),
-            new Pose2d(new Translation2d(1.598, 2.2), Rotation2d.fromRadians(-Math.PI)),
-            new Pose2d(new Translation2d(1.598, 1.63), Rotation2d.fromRadians(-Math.PI)),
-            new Pose2d(new Translation2d(1.598, 1.05), Rotation2d.fromRadians(-Math.PI)),
-            new Pose2d(new Translation2d(1.598, 0.5), Rotation2d.fromRadians(-Math.PI)));
+            // Red node scoring locations
+            new Pose2d(1.92, 5.00, Rotation2d.fromRadians(-Math.PI)), 
+            new Pose2d(1.92, 4.42, Rotation2d.fromRadians(-Math.PI)),
+            new Pose2d(1.92, 3.83, Rotation2d.fromRadians(-Math.PI)),
+            new Pose2d(1.92, 3.30, Rotation2d.fromRadians(-Math.PI)),
+            new Pose2d(1.92, 2.76, Rotation2d.fromRadians(-Math.PI)),
+            new Pose2d(1.92, 2.19, Rotation2d.fromRadians(-Math.PI)),
+            new Pose2d(1.92, 1.69, Rotation2d.fromRadians(-Math.PI)),
+            new Pose2d(1.92, 1.09, Rotation2d.fromRadians(-Math.PI)),
+            new Pose2d(1.92, 0.51, Rotation2d.fromRadians(-Math.PI)),
+            new Pose2d(14.70, 5.00, Rotation2d.fromDegrees(0.0)),
+            new Pose2d(14.70, 4.42, Rotation2d.fromDegrees(0.0)),
+            new Pose2d(14.70, 3.83, Rotation2d.fromDegrees(0.0)),
+            new Pose2d(14.70, 3.30, Rotation2d.fromDegrees(0.0)),
+            new Pose2d(14.70, 2.76, Rotation2d.fromDegrees(0.0)),
+            new Pose2d(14.70, 2.19, Rotation2d.fromDegrees(0.0)),
+            new Pose2d(14.70, 1.69, Rotation2d.fromDegrees(0.0)),
+            new Pose2d(14.70, 1.09, Rotation2d.fromDegrees(0.0)),
+            new Pose2d(14.70, 0.51, Rotation2d.fromDegrees(0.0)));
   }
 
   /** General robot constants */
@@ -86,8 +74,8 @@ public static final class VisionConstants {
     // Robot to camera transform
     public static final Transform3d robotToCam =
         new Transform3d(
-            new Translation3d(0.0, Units.inchesToMeters(1.5), Units.inchesToMeters(39.0)),
-            new Rotation3d(0.0, 0.0, 0.0));
+            new Translation3d(0.0, Units.inchesToMeters(0.0), Units.inchesToMeters(0.0)),
+            new Rotation3d(0.0, Units.degreesToRadians(0.0), 0.0));
   }
 
   /** Constants revolving around the elevator subsystem. */
@@ -145,7 +133,6 @@ public static final class IntakeConstants {
     public static final int currentLimit = 40;
 
     public static final double motorSpeed = 0.9;
-    public static final double intakeCurrentThreshold = 40.0;
   }
 
   /** Constants revolving around auton modes. */

src/main/java/frc3512/robot/Robot2023.java

@@ -6,13 +6,16 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc3512.robot.commands.DriveToPose;
 import frc3512.robot.subsystems.Arm;
 import frc3512.robot.subsystems.Elevator;
 import frc3512.robot.subsystems.Intake;
+import frc3512.robot.subsystems.LEDs;
 import frc3512.robot.subsystems.Superstructure;
 import frc3512.robot.subsystems.Superstructure.ScoringEnum;
 import frc3512.robot.subsystems.Swerve;
 import frc3512.robot.subsystems.Vision;
+import frc3512.robot.subsystems.LEDs.LEDColor;
 
 public class Robot2023 {
   // Robot subsystems
@@ -22,6 +25,7 @@ public class Robot2023 {
   private Arm arm = new Arm();
   private Intake intake = new Intake();
   private Superstructure superstructure = new Superstructure(swerve, elevator, arm, intake);
+  private LEDs leds = new LEDs();
 
   // Driver Control
   private final int translationAxis = XboxController.Axis.kLeftY.value;
@@ -34,6 +38,7 @@ public class Robot2023 {
   private final CommandJoystick appendage =
       new CommandJoystick(Constants.OperatorConstants.appendageControllerPort);
 
+  /** Set the swerve drive motors to brake mode or not */
   public void setMotorBrake(boolean brake) {
     swerve.setMotorBrake(brake);
   }
@@ -42,8 +47,11 @@ public void setMotorBrake(boolean brake) {
   public void configureButtonBindings() {
 
     driver.x().onTrue(new InstantCommand(() -> swerve.zeroGyro()));
+    driver.leftBumper().whileTrue(new DriveToPose(swerve));
+    driver.a().onTrue(leds.switchLEDMode(LEDColor.WHITE));
+    driver.b().onTrue(leds.switchLEDMode(LEDColor.OFF));
 
-    appendage.button(1).whileTrue(intake.stopIntake());
+    appendage.button(1).onTrue(intake.stopIntake());
     appendage.button(2).onTrue(superstructure.enableManualControl());
     appendage.button(3).whileTrue(intake.intakeGamePiece());
     appendage.button(4).whileTrue(intake.outtakeGamePiece());

src/main/java/frc3512/robot/auton/Autos.java

@@ -29,8 +29,7 @@ public final class Autos {
   private final HashMap<String, Command> eventMap;
   private final SwerveAutoBuilder autonBuilder;
 
-  public Autos(
-      Swerve swerve, Elevator elevator, Arm arm, Superstructure superstructure, Intake intake) {
+  public Autos(Swerve swerve, Elevator elevator, Arm arm, Superstructure superstructure, Intake intake) {
     this.swerve = swerve;
     this.elevator = elevator;
     this.arm = arm;

src/main/java/frc3512/robot/commands/DriveToPose.java

@@ -5,64 +5,81 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc3512.robot.Constants;
 import frc3512.robot.subsystems.Swerve;
 
 public class DriveToPose extends CommandBase {
 
   private final Swerve swerve;
-  private Pose2d desiredPose;
+  private Pose2d targetPose;
+  private Pose2d currentPose;
 
   private final ProfiledPIDController xController =
-      new ProfiledPIDController(0.6, 0.0, 0.0, new TrapezoidProfile.Constraints(1.0, 3.0));
+      new ProfiledPIDController(1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(4.0, 3.0));
   private final ProfiledPIDController yController =
-      new ProfiledPIDController(0.6, 0.0, 0.0, new TrapezoidProfile.Constraints(1.0, 3.0));
+      new ProfiledPIDController(1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(4.0, 3.0));
   private final ProfiledPIDController thetaController =
-      new ProfiledPIDController(2.2, 0.0, 0.0, new TrapezoidProfile.Constraints(1.0, 3.0));
+      new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(4.0, 3.0));
 
-  public DriveToPose(Swerve swerve, Pose2d pose) {
+  public DriveToPose(Swerve swerve) {
     this.swerve = swerve;
-    this.desiredPose = pose;
 
-    xController.setTolerance(0.5);
-    yController.setTolerance(0.5);
-    thetaController.setTolerance(Units.degreesToRadians(3.0));
+    xController.setTolerance(0.05);
+    yController.setTolerance(0.05);
+    thetaController.setTolerance(Units.degreesToRadians(0.5));
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
     addRequirements(swerve);
   }
 
   @Override
   public void initialize() {
-    var currPose = swerve.getPose();
-    xController.reset(currPose.getX());
-    yController.reset(currPose.getY());
-    thetaController.reset(currPose.getRotation().getRadians());
+    currentPose = swerve.getPose();
+    xController.reset(currentPose.getX());
+    yController.reset(currentPose.getY());
+    thetaController.reset(currentPose.getRotation().getRadians());
   }
 
   @Override
   public void execute() {
-    var currPose = swerve.getPose();
-    var targetPose = desiredPose;
+    currentPose = swerve.getPose();
+    targetPose = findClosestPose();
 
-    double xvelocity = xController.calculate(currPose.getX(), targetPose.getX());
-    double yvelocity = yController.calculate(currPose.getY(), targetPose.getY());
+    double xvelocity = xController.calculate(currentPose.getX(), targetPose.getX());
+    double yvelocity = yController.calculate(currentPose.getY(), targetPose.getY());
     double thetaVelocity =
         thetaController.calculate(
-            currPose.getRotation().getRadians(), targetPose.getRotation().getRadians());
+            currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians());
 
     if (atGoal()) {
       xvelocity = 0.0;
       yvelocity = 0.0;
       thetaVelocity = 0.0;
     }
 
-    swerve.setChassisSpeeds(
-        ChassisSpeeds.fromFieldRelativeSpeeds(
-            xvelocity, yvelocity, thetaVelocity, currPose.getRotation()));
+    swerve.setChassisSpeeds(new ChassisSpeeds(xvelocity, yvelocity, thetaVelocity));
+
+    SmartDashboard.putNumberArray("Current Pose", new double[] {currentPose.getX(), currentPose.getY(), currentPose.getRotation().getDegrees()});
+    SmartDashboard.putNumberArray("Target Pose", new double[] {targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()});
+    SmartDashboard.putBoolean("X At Goal", xController.atGoal());
+    SmartDashboard.putBoolean("Y At Goal", yController.atGoal());
+    SmartDashboard.putBoolean("Theta At Goal", thetaController.atGoal());
   }
 
   public boolean atGoal() {
     return (xController.atGoal() && yController.atGoal() && thetaController.atGoal());
   }
+
+  private Pose2d findClosestPose() {
+    Pose2d closestPose = Constants.FieldConstants.scoringPositions.get(0);
+    for (Pose2d pose : Constants.FieldConstants.scoringPositions) {
+      if (closestPose.relativeTo(currentPose).getTranslation().getNorm()
+          > pose.relativeTo(currentPose).getTranslation().getNorm()) {
+        closestPose = pose;
+      }
+    }
+    return closestPose;
+  }
 }

src/main/java/frc3512/robot/subsystems/Intake.java

@@ -5,7 +5,6 @@
 import com.revrobotics.CANSparkMaxLowLevel;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc3512.lib.logging.SpartanDoubleEntry;
 import frc3512.lib.util.CANSparkMaxUtil;
 import frc3512.lib.util.CANSparkMaxUtil.Usage;
 import frc3512.robot.Constants;
@@ -14,8 +13,6 @@ public class Intake extends SubsystemBase {
   private CANSparkMax intakeMotor =
       new CANSparkMax(
           Constants.IntakeConstants.intakeMotorID, CANSparkMaxLowLevel.MotorType.kBrushless);
-  private SpartanDoubleEntry currentEntry =
-      new SpartanDoubleEntry("/Diagnostics/Intake/Motor Current");
 
   public Intake() {
     intakeMotor.restoreFactoryDefaults();
@@ -43,25 +40,9 @@ public Command outtakeGamePiece() {
   }
 
   public Command stopIntake() {
-    return run(
+    return runOnce(
         () -> {
           intakeMotor.set(0.0);
         });
   }
-
-  public Command applyCurrentSensing(double normalSpeed, double stallSpeed) {
-    return run(
-        () -> {
-          if (intakeMotor.getOutputCurrent() >= Constants.IntakeConstants.intakeCurrentThreshold) {
-            intakeMotor.set(stallSpeed);
-          } else {
-            intakeMotor.set(normalSpeed);
-          }
-        });
-  }
-
-  @Override
-  public void periodic() {
-    currentEntry.set(intakeMotor.getOutputCurrent());
-  }
 }

src/main/java/frc3512/robot/subsystems/LEDs.java

@@ -0,0 +1,46 @@
+package frc3512.robot.subsystems;
+
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class LEDs extends SubsystemBase {
+  public enum LEDColor {
+    GREEN,
+    RED,
+    BLUE,
+    YELLOW,
+    PURPLE,
+    TEAL,
+    WHITE,
+    OFF
+  }
+
+  private final Solenoid green = new Solenoid(PneumaticsModuleType.CTREPCM, 0);
+  private final Solenoid blue = new Solenoid(PneumaticsModuleType.CTREPCM, 1);
+  private final Solenoid red = new Solenoid(PneumaticsModuleType.CTREPCM, 2);
+
+  public Command switchLEDMode(LEDColor color) {
+    return runOnce(
+        () -> {
+          if (green.get()) {
+            green.set(false);
+          } else {
+            green.set(true);
+          }
+
+          if (blue.get()) {
+            blue.set(false);
+          } else {
+            blue.set(true);
+          }
+
+          if (red.get()) {
+            red.set(false);
+          } else {
+            red.set(true);
+          }
+        });
+  }
+}