[Robot] Seven Rivers Changes

Robot/src/main/java/com/swrobotics/robot/RobotContainer.java

@@ -120,18 +120,18 @@ public RobotContainer() {
 
  // Create a chooser to select the autonomous
  List<AutoEntry> autos = buildPathPlannerAutos();
- autos.add(new AutoEntry("Drive backward", Commands.run(
+ autos.add(new AutoEntry("Drive backward", createNonPathPlannerAuto(Commands.run(
  () -> drive.drive(new DriveRequest(
  SwerveDrive.AUTO_PRIORITY,
  new Translation2d(-0.5, 0),
  DriveRequestType.OpenLoopVoltage)),
  drive
- ).withTimeout(5)));
+ ).withTimeout(5))));
  autos.sort(Comparator.comparing(AutoEntry::name, String.CASE_INSENSITIVE_ORDER));
 
  SendableChooser<Command> autoChooser = new SendableChooser<>();
- autoChooser.setDefaultOption("Just Shoot", RobotCommands.aimAndShoot(this, false));
- autoChooser.addOption("None", Commands.none());
+ autoChooser.setDefaultOption("Just Shoot", createNonPathPlannerAuto(RobotCommands.justShoot(this)));
+ autoChooser.addOption("None", createNonPathPlannerAuto(Commands.none()));
  for (AutoEntry auto : autos)
  autoChooser.addOption(auto.name(), auto.cmd());
  autoSelector = new LoggedDashboardChooser<>("Auto Selection", autoChooser);
@@ -174,6 +174,17 @@ public RobotContainer() {
 
  private static final record AutoEntry(String name, Command cmd) {}
 
+ private Command createNonPathPlannerAuto(Command autoCmd) {
+ return Commands.sequence(
+ Commands.runOnce(() -> {
+ if (!drive.hasSeenWhereWeAre()) {
+ drive.setPose(FieldView.startingPosition.getPose());
+ }
+ }),
+ autoCmd
+ );
+ }
+
  private static List<AutoEntry> buildPathPlannerAutos() {
  if (!AutoBuilder.isConfigured()) {
  throw new RuntimeException(

Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java

@@ -10,6 +10,16 @@
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 
 public final class RobotCommands {
+ public static Command justShoot(RobotContainer robot) {
+ return Commands.sequence(
+ Commands.waitUntil(robot.shooter::isCalibrated),
+ Commands.waitUntil(robot.shooter::isReadyToShoot)
+ .withTimeout(NTData.SHOOTER_AUTO_READY_TIMEOUT.get()),
+ Commands.waitSeconds(NTData.SHOOTER_AUTO_AFTER_READY_DELAY.get()),
+ new IndexerFeedCommand(robot.indexer)
+ );
+ }
+
  public static Command aimAndShoot(RobotContainer robot, boolean waitForNote) {
  AimTowardsSpeakerCommand aim = new AimTowardsSpeakerCommand(robot.drive, robot.shooter);
  Command shootSeq = Commands.sequence(

Robot/src/main/java/com/swrobotics/robot/config/NTData.java

@@ -87,7 +87,7 @@ public final class NTData {
  public static final NTEntry<Double> SHOOTER_PIVOT_ANGLE_ADJUST_RED = new NTDouble("Shooter/Pivot/Angle Adjust Red (deg)", -2).setPersistent();
  public static final NTEntry<Double> SHOOTER_PIVOT_REVERSE_ANGLE = new NTDouble("Shooter/Pivot/Reverse Angle (deg)", 52).setPersistent();
 
- public static final NTEntry<Double> SHOOTER_LOB_POWER_COEFFICIENT = new NTDouble("Shooter/Lob/Power Coefficient", 1).setPersistent(); // To go from real velocity to flywheel velocity
+ public static final NTEntry<Double> SHOOTER_LOB_POWER_COEFFICIENT = new NTDouble("Shooter/Lob/Power Coefficient", 1.5).setPersistent(); // To go from real velocity to flywheel velocity
  public static final NTEntry<Double> SHOOTER_LOB_TALL_HEIGHT_METERS = new NTDouble("Shooter/Lob/Tall Lob Height (m)", 5).setPersistent();
  public static final NTEntry<Double> SHOOTER_LOB_SHORT_HEIGHT_METERS = new NTDouble("Shooter/Lob/Short Lob Height (m)", 1.3).setPersistent();
  public static final NTEntry<Double> SHOOTER_LOB_DRIVE_ANGLE_CORRECTION_BLUE = new NTDouble("Shooter/Lob/Drive Angle Correction Blue (deg)", -30).setPersistent();

Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java

@@ -380,9 +380,9 @@ else if (operatorWantsShoot)
 
  pieceRumbleNt.set(pieceRumble);
 
- driver.setRumble(pieceRumble ? 0.6 : (tooFar && (driverWantsAim() || driverWantsFlywheels()) ? 0.5 : 0));
+ driver.setRumble(pieceRumble ? 1.0 : (tooFar && (driverWantsAim() || driverWantsFlywheels()) ? 0.5 : 0));
  boolean shooterReady = robot.shooter.isReadyToShoot();
- operator.setRumble(pieceRumble ? 0.6 : (shooterReady ? 0.5 : 0));
+ operator.setRumble(pieceRumble ? 1.0 : (shooterReady ? 0.5 : 0));
 
  this.shooterReady.set(shooterReady);
  this.tooFar.set(tooFar && (driverWantsAim() || driverWantsFlywheels()));

Robot/src/main/java/com/swrobotics/robot/logging/FieldView.java

@@ -1,5 +1,6 @@
 package com.swrobotics.robot.logging;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -13,6 +14,11 @@ public final class FieldView {
  public static final FieldObject2d pathPlannerPath = field.getObject("PathPlanner path");
  public static final FieldObject2d pathPlannerSetpoint = field.getObject("PathPlanner setpoint");
 
+ public static final FieldObject2d startingPosition = field.getObject("Preload Position");
+ static {
+ startingPosition.setPose(-1, 0, new Rotation2d());
+ }
+
  public static void publish() {
  SmartDashboard.putData("Field View", field);
  }

Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java

@@ -106,11 +106,11 @@ public void periodic() {
  targetAim = aim;
  aimCalculator = tableAimCalculator;
 
+ isPreparing = false;
  if (DriverStation.isDisabled() || !pivot.hasCalibrated())
  return;
 
  // TODO: Make this not a mess
- isPreparing = false;
  if ((aim != null) && DriverStation.isAutonomous()) {
  // Have the shooter be constantly active during auto
  isPreparing = true;

Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/aim/TableAimCalculator.java

@@ -6,6 +6,8 @@
 import com.swrobotics.mathlib.MathUtil;
 import com.swrobotics.robot.config.NTData;
 
+import edu.wpi.first.math.util.Units;
+
 public final class TableAimCalculator implements AimCalculator {
  public static final TableAimCalculator INSTANCE = new TableAimCalculator();
 
@@ -24,20 +26,24 @@ public TableAimCalculator() {
 // addCalibrationSample(3.0, 34, 60);
 // addCalibrationSample(3.8, 29, 70);
 
+ double fieldWrongness = Units.inchesToMeters(1 + 5.0/8); 
+
  // MURA 3-16, 3|1 wheel shooter configuration
- addCalibrationSample(1.224203 - .2, 58, 55);
- addCalibrationSample(1.657 - .2, 52, 55);
- addCalibrationSample(2.239 - .2, 44, 55);
- addCalibrationSample(2.875 - .2, 36, 55);
- addCalibrationSample(2.993, 34, 63);
- addCalibrationSample(3.506, 30, 63);
- addCalibrationSample(3.903, 28, 67);
+ addCalibrationSample(fieldWrongness + 1.224203 - .2, 58, 55);
+ addCalibrationSample(fieldWrongness + 1.657 - .2, 52, 55);
+ addCalibrationSample(fieldWrongness + 2.239 - .2, 44, 55);
+ addCalibrationSample(fieldWrongness + 2.875 - .2, 36, 55);
+ addCalibrationSample(fieldWrongness + 2.993, 34, 63);
+ addCalibrationSample(fieldWrongness + 3.506, 30, 63);
+ addCalibrationSample(fieldWrongness + 3.903, 28, 67);
 
  // MURA 3-23, 3|1 wheel shooter
- addCalibrationSample(4.34, 28, 67);
- addCalibrationSample(4.67, 27, 67);
- addCalibrationSample(5.06, 25, 67);
- addCalibrationSample(5.58, 24, 70);
+ addCalibrationSample(fieldWrongness + 4.34, 28 + 1, 67);
+ addCalibrationSample(fieldWrongness + 4.67, 27 + 1, 67);
+ addCalibrationSample(fieldWrongness + 5.06, 25 + 1, 67);
+ addCalibrationSample(fieldWrongness + 5.58, 24 + 1, 70);
+
+ addCalibrationSample(fieldWrongness + 6.6, 22.5, 82);
 
 // addCalibrationSample(3.37, 29, 60);
 

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java

@@ -332,4 +332,8 @@ public TalonFX getTurnMotor(int module) {
  public void setEstimatorIgnoreVision(boolean ignoreVision) {
  estimator.setIgnoreVision(ignoreVision);
  }
+
+ public boolean hasSeenWhereWeAre() {
+ return estimator.hasSeenWhereWeAre();
+ }
 }

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java

@@ -1,6 +1,8 @@
 package com.swrobotics.robot.subsystems.swerve;
 
 import com.swrobotics.lib.field.FieldInfo;
+import com.swrobotics.lib.net.NTBoolean;
+import com.swrobotics.lib.net.NTInteger;
 import com.swrobotics.mathlib.MathUtil;
 import com.swrobotics.robot.logging.FieldView;
 import com.swrobotics.robot.subsystems.tagtracker.TagTrackerInput;
@@ -33,6 +35,9 @@ public final class SwerveEstimator {
 
  private boolean ignoreVision;
 
+ private final NTInteger numberOfVisionUpdates = new NTInteger("Debug/Number of Vision Updates", 0);
+ private final NTBoolean seenWhereWeAre = new NTBoolean("Debug/Has Seen Where We Are", false);
+
  public SwerveEstimator(FieldInfo field) {
  double halfFrameL = 0.77 / 2;
  double halfFrameW = 0.695 / 2;
@@ -115,6 +120,9 @@ public void update(Twist2d driveTwist) {
  List<TagTrackerInput.VisionUpdate> visionData = tagTracker.getNewUpdates();
  updates.put(Timer.getFPGATimestamp(), new PoseUpdate(driveTwist, new ArrayList<>()));
 
+ numberOfVisionUpdates.set(numberOfVisionUpdates.get() + visionData.size());
+ seenWhereWeAre.set(hasSeenWhereWeAre());
+
  List<Pose2d> tagPoses = new ArrayList<>();
  for (Pose3d tagPose3d : tagTracker.getEnvironment().getAllPoses()) {
  tagPoses.add(tagPose3d.toPose2d());
@@ -228,4 +236,9 @@ public Pose2d apply(Pose2d prevPose, Matrix<N3, N1> q) {
  return pose;
  }
  }
+
+ public boolean hasSeenWhereWeAre() {
+ // 1 second of frames from both cameras
+ return numberOfVisionUpdates.get() > 100;
+ }
 }