High frequency pose estimation and improved traction control

lasarobotics · Jul 7, 2024 · a14f4fb · a14f4fb
1 parent 5ce4ef5
commit a14f4fb
Show file tree

Hide file tree

Showing 13 changed files with 248 additions and 194 deletions.
diff --git a/.gitignore b/.gitignore
@@ -177,6 +177,9 @@ simgui-ds.json
 # Build Constants
 src/main/java/frc/robot/BuildConstants.java
 
+# Logs
+logs/
+
 # Battery Tracker
 previous_battery.txt
 

diff --git a/build.gradle b/build.gradle
@@ -97,7 +97,7 @@ dependencies {
   nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
   simulationRelease wpi.sim.enableRelease()
 
-  implementation 'com.github.lasarobotics:PurpleLib:90fa55fbf4'
+  implementation 'com.github.lasarobotics:PurpleLib:c34eb28828'
 
   implementation 'org.apache.commons:commons-math3:3.+'
 

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -8,6 +8,7 @@
 import java.util.List;
 import java.util.Map;
 import java.util.Map.Entry;
+import java.util.Optional;
 
 import org.apache.commons.math3.analysis.interpolation.SplineInterpolator;
 import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction;
@@ -18,8 +19,11 @@
 import org.lasarobotics.hardware.revrobotics.SparkPIDConfig;
 import org.lasarobotics.led.LEDStrip;
 import org.lasarobotics.utils.PIDConstants;
+import org.lasarobotics.vision.AprilTagCamera.Resolution;
 
 import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -34,8 +38,6 @@
 import edu.wpi.first.units.Units;
 import frc.robot.subsystems.drive.PurplePathPose;
 import frc.robot.subsystems.shooter.ShooterSubsystem.State;
-import frc.robot.subsystems.vision.AprilTagCamera.Resolution;
-import frc.robot.subsystems.vision.VisionSubsystem;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
@@ -51,12 +53,11 @@
  */
 public final class Constants {
   public static class Field {
-    public static final double FIELD_WIDTH = 8.21;
-    public static final double FIELD_LENGTH = 16.54;
+    public static final AprilTagFieldLayout FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+    public static final Translation2d CENTER = new Translation2d(FIELD_LAYOUT.getFieldLength() / 2, FIELD_LAYOUT.getFieldWidth() / 2);
 
-    public static final Translation2d CENTER = new Translation2d(FIELD_LENGTH / 2, FIELD_WIDTH / 2);
-    public static final AprilTag BLUE_SPEAKER = VisionSubsystem.getInstance().getTag(7).get();
-    public static final AprilTag RED_SPEAKER = VisionSubsystem.getInstance().getTag(4).get();
+    public static final AprilTag BLUE_SPEAKER = getTag(7).get();
+    public static final AprilTag RED_SPEAKER = getTag(4).get();
 
     public static final PurplePathPose AMP = new PurplePathPose(
       new Pose2d(Units.Meters.of(1.85), Units.Meters.of(7.77), Rotation2d.fromDegrees(-90.0)),
@@ -70,6 +71,15 @@ public static class Field {
       Units.Meters.of(0.5),
       true
     );
+
+    /**
+     * Get AprilTag from field
+     * @param id Tag ID
+     * @return AprilTag matching ID
+     */
+    public static Optional<AprilTag> getTag(int id) {
+      return FIELD_LAYOUT.getTags().stream().filter((tag) -> tag.ID == id).findFirst();
+    }
   }
 
   public static class HID {
@@ -106,7 +116,8 @@ public static class AutoNames {
 
   public static class Drive {
     public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(8.0, 0.0, 0.3, 0.0, 0.0);
-    public static final double DRIVE_SLIP_RATIO = 0.08;
+    public static final Measure<Dimensionless> DRIVE_SLIP_RATIO = Units.Percent.of(8.0);
+    public static final Measure<Dimensionless> FRICTION_COEFFICIENT = Units.Value.of(1.1);
     public static final double DRIVE_TURN_SCALAR = 60.0;
     public static final double DRIVE_LOOKAHEAD = 6;
 
@@ -156,7 +167,7 @@ public static class Shooter {
     );
     public static final TrapezoidProfile.Constraints ANGLE_MOTION_CONSTRAINT = new TrapezoidProfile.Constraints(
       Units.DegreesPerSecond.of(360.0),
-      Units.DegreesPerSecond.of(360.0 * 10).per(Units.Second)
+      Units.DegreesPerSecond.of(360.0 * 6).per(Units.Second)
     );
     public static final List<Entry<Measure<Distance>,State>> SHOOTER_MAP = Arrays.asList(
       Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(14.90),    Units.Degrees.of(53.0))),

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -10,7 +10,6 @@
 import org.lasarobotics.utils.GlobalConstants;
 import org.littletonrobotics.junction.LoggedRobot;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -61,7 +61,7 @@ public class RobotContainer {
     IntakeSubsystem.initializeHardware(),
     Constants.Intake.ROLLER_VELOCITY
   );
-  private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance();
+  //private static final VisionSubsystem VISION_SUBSYSTEM = VisionSubsystem.getInstance();
 
   private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT);
 
@@ -93,8 +93,6 @@ public RobotContainer() {
     NamedCommands.registerCommand(Constants.NamedCommands.AUTO_SHOOT_LONG_COMMAND_NAME, shootCommand().withTimeout(2.0));
     NamedCommands.registerCommand(Constants.NamedCommands.AUTO_INTAKE_COMMAND_NAME, aimAndIntakeObjectCommand());
 
-    VISION_SUBSYSTEM.setPoseSupplier(() -> DRIVE_SUBSYSTEM.getPose());
-
     // Bind buttons and triggers
     configureBindings();
 
@@ -111,7 +109,7 @@ private void configureBindings() {
 
     // Right trigger button - aim and shoot at speaker, shooting only if speaker tag is visible and robot is in range
     // Click DPAD down to override and shoot now
-    PRIMARY_CONTROLLER.rightTrigger().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.b().getAsBoolean()));
+    PRIMARY_CONTROLLER.y().whileTrue(shootCommand(() -> PRIMARY_CONTROLLER.b().getAsBoolean()));
 
     // Right bumper button - amp score, also use for outtake
     PRIMARY_CONTROLLER.rightBumper().whileTrue(SHOOTER_SUBSYSTEM.scoreAmpCommand());
@@ -158,7 +156,7 @@ private void configureBindings() {
     PRIMARY_CONTROLLER.x().whileTrue(SHOOTER_SUBSYSTEM.shootSpeakerCommand());
 
     // Y button - spit out note
-    PRIMARY_CONTROLLER.y().whileTrue(outtakeCommand());
+    //PRIMARY_CONTROLLER.y().whileTrue(outtakeCommand());
 
     // DPAD up - shoot manual
     PRIMARY_CONTROLLER.povUp().whileTrue(SHOOTER_SUBSYSTEM.shootManualCommand(() -> dashboardStateSupplier()));
@@ -252,7 +250,7 @@ private Command shootCommand(BooleanSupplier override) {
         () -> PRIMARY_CONTROLLER.getRightX(),
         () -> speakerSupplier().pose.getTranslation().toTranslation2d(),
         true,
-        true
+        false
       ),
       SHOOTER_SUBSYSTEM.shootCommand(() -> DRIVE_SUBSYSTEM.isAimed(), override)
     );
@@ -282,20 +280,20 @@ private Command feedThroughCommand() {
    * Command to aim at detected game object automatically, driving normally if none is detected
    * @return Command to aim at object
    */
-  private Command aimAtObject() {
-    return DRIVE_SUBSYSTEM.aimAtPointRobotCentric(
-      () -> PRIMARY_CONTROLLER.getLeftY(),
-      () -> PRIMARY_CONTROLLER.getLeftX(),
-      () -> PRIMARY_CONTROLLER.getRightX(),
-      () -> {
-        return VISION_SUBSYSTEM.getObjectLocation().isPresent()
-                ? VISION_SUBSYSTEM.getObjectLocation().get()
-                : null;
-      },
-      false,
-      false
-    );
-  }
+  // private Command aimAtObject() {
+  //   return DRIVE_SUBSYSTEM.aimAtPointRobotCentric(
+  //     () -> PRIMARY_CONTROLLER.getLeftY(),
+  //     () -> PRIMARY_CONTROLLER.getLeftX(),
+  //     () -> PRIMARY_CONTROLLER.getRightX(),
+  //     () -> {
+  //       return VISION_SUBSYSTEM.getObjectLocation().isPresent()
+  //               ? VISION_SUBSYSTEM.getObjectLocation().get()
+  //               : null;
+  //     },
+  //     false,
+  //     false
+  //   );
+  // }
 
    /**
     * Automatically aim robot heading at object, drive, and intake a game object
@@ -314,15 +312,15 @@ private Command aimAndIntakeObjectCommand() {
     //       false,
     //       false).until(() -> VISION_SUBSYSTEM.shouldIntake()),
     //  Commands.parallel(
-      DRIVE_SUBSYSTEM.aimAtPointCommand(
-        () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getCos() * 0.75,
-        () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getSin() * 0.75,
-        () -> 0,
-        () -> {
-          return VISION_SUBSYSTEM.getObjectLocation().orElse(null);
-        },
-        false,
-        false)
+      // DRIVE_SUBSYSTEM.aimAtPointCommand(
+      //   () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getCos() * 0.75,
+      //   () -> -DRIVE_SUBSYSTEM.getPose().getRotation().plus(new Rotation2d(VISION_SUBSYSTEM.getObjectHeading().orElse(Units.Degrees.of(0)))).getSin() * 0.75,
+      //   () -> 0,
+      //   () -> {
+      //     return VISION_SUBSYSTEM.getObjectLocation().orElse(null);
+      //   },
+      //   false,
+      //   false)
 
         //  INTAKE_SUBSYSTEM.intakeCommand(),
         //  SHOOTER_SUBSYSTEM.intakeCommand()