fix nullpointerexception in old led strip, put goodTrajectory to smar…

…t dashboard, reset pose on start button, auto intake on xbox controller

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

@@ -45,8 +45,8 @@ public class Constants {
   // TODO: cammera offsets
   public static final Transform3d FRONT_APRILTAG_CAMERA_OFFSET = new Transform3d(0, 0, 0.7112, new Rotation3d(0, Math.toRadians(-20), 0));
   public static final Transform3d REAR_APRILTAG_CAMERA_OFFSET = new Transform3d(0, 0, 0, new Rotation3d());
-  public static final Transform3d FRONT_NOTE_CAMERA_OFFSET = new Transform3d(Units.inchesToMeters(7.8971), Units.inchesToMeters(-12.99815), Units.inchesToMeters(18.3493), new Rotation3d(0, Math.toRadians(-20), Math.toRadians(20)));
-  public static final double FRONT_NOTE_CAMERA_HFOV = 53;
+  public static final Transform3d INTAKE_NOTE_CAMERA_OFFSET = new Transform3d(Units.inchesToMeters(7.8971), Units.inchesToMeters(-12.99815), Units.inchesToMeters(18.3493), new Rotation3d(0, Math.toRadians(-20), Math.toRadians(20)));
+  public static final double INTAKE_NOTE_CAMERA_HFOV = 53;
 
   public static final double INTAKE_RADIUS = 0.5;
 

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

@@ -13,6 +13,8 @@
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -26,6 +28,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.arm.SetArmState;
+import frc.robot.commands.drive.AutoIntake;
 import frc.robot.commands.drive.AutoPickupNote;
 import frc.robot.commands.drive.DriveSwerveWithXbox;
 import frc.robot.commands.intake.SetIntakeState;
@@ -105,7 +108,7 @@ public static double getRobotRoll() {
   public static final OldLEDStrip leds = new OldLEDStrip();
 
   // Cameras
-  public static final NoteDetectorCamera frontNoteCamera = new NoteDetectorCamera("NoteCamera", Constants.FRONT_NOTE_CAMERA_OFFSET);
+  public static final NoteDetectorCamera intakeCamera = new NoteDetectorCamera("NoteCamera", Constants.INTAKE_NOTE_CAMERA_OFFSET);
   public static final AprilTagCamera frontAprilTagCamera = new AprilTagCamera("FrontCamera", Constants.FRONT_APRILTAG_CAMERA_OFFSET);
   public static final AprilTagCamera rearAprilTagCamera = new AprilTagCamera("RearCamera", Constants.REAR_APRILTAG_CAMERA_OFFSET);
 
@@ -136,8 +139,12 @@ private void configureAutomatedBindings() {
 
   /** Binds commands to xbox controller buttons. */
   private void configureXboxControllerBindings() {
-    Trigger resetNavx = driverController.start();
-    resetNavx.onTrue(new InstantCommand(() -> swerveDrive.resetNavx(), RobotContainer.swerveDrive));
+    Trigger resetHeading = driverController.start().debounce(1);
+    resetHeading.onTrue(new InstantCommand(() -> {
+      RobotContainer.swerveDrive.resetNavx();
+      var pose = RobotContainer.swerveDrive.getPose();
+      RobotContainer.swerveDrive.setPose(new Pose2d(pose.getX(), pose.getY(), Rotation2d.fromDegrees(0)));
+    }, RobotContainer.swerveDrive));
 
     // go slow is an exception - doesn't really need to "require" the swerve drive
     Trigger slowDrive = driverController.leftTrigger(0.3);
@@ -150,6 +157,9 @@ private void configureXboxControllerBindings() {
       SwerveDrive.kMaxAngularSpeedRadiansPerSecond = 2 * Math.PI;
       SwerveDrive.kMaxSpeedMetersPerSecond = 4.5;
     }));
+
+    Trigger autoIntakeNote = driverController.leftBumper();
+    autoIntakeNote.whileTrue(new AutoIntake());
   }
 
   /** Binds commands to the operator stick. */

src/main/java/frc/robot/commands/drive/AutoIntake.java

@@ -13,9 +13,9 @@
 
 public class AutoIntake extends Command {
   private final Timer timer = new Timer();
-  // TODO: test this and find a better number
-  private final double kTimeForBreamBreakTrippedToCompleteInSeconds = 0.10;
+  private final double kTimeForBreamBreakTrippedToCompleteInSeconds = 0.05;
 
+  private boolean hasNoteInitially;
   private Optional<Note> target = Optional.empty();
   private Pose2d robotTarget;
   private Pose2d robotPose;
@@ -27,8 +27,11 @@ public AutoIntake() {
   // Called just before this Command runs the first time
   @Override
   public void initialize() {
-    RobotContainer.intake.intake();
-    RobotContainer.shooter.index();
+    hasNoteInitially = RobotContainer.shooter.hasNote();
+    if(hasNoteInitially) {
+      RobotContainer.intake.intake();
+      RobotContainer.shooter.index();
+    }
   }
 
   // Called repeatedly when this Command is scheduled to run
@@ -37,8 +40,7 @@ public void execute() {
     this.robotPose = RobotContainer.swerveDrive.getPose();
 
     // update target based on camera
-    this.target = RobotContainer.frontNoteCamera.getNearestNote();
-
+    this.target = RobotContainer.intakeCamera.getNearestNote();
     if (this.target.isEmpty()) {
       return;
     }
@@ -57,22 +59,20 @@ public void execute() {
     this.robotTarget = new Pose2d(vec.plus(robotPose.getTranslation()), angleToNote);
 
     // drive to robotTarget
-    // TODO: we hardcode fieldRelative here - does it matter true vs false? set it to one of them that makes sense and take no input from the joystick here
-    var fieldRelative = (RobotContainer.driverController.getRightTriggerAxis() < 0.3);
-    RobotContainer.swerveDrive.goToPose(robotTarget, fieldRelative);
+    RobotContainer.swerveDrive.goToPose(robotTarget, true);
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   public boolean isFinished() {
-    var hasNote = RobotContainer.intake.hasNoteInside();
+    var hasNote = RobotContainer.shooter.hasNote();
     if (hasNote) {
       timer.start();
     } else {
       timer.reset();
     }
 
-    return hasNote && timer.hasElapsed(kTimeForBreamBreakTrippedToCompleteInSeconds);
+    return hasNoteInitially || (hasNote && timer.hasElapsed(kTimeForBreamBreakTrippedToCompleteInSeconds));
   }
 
   // Called once after isFinished returns true

src/main/java/frc/robot/commands/drive/TargetPointWhileDriving.java

@@ -20,13 +20,13 @@ public TargetPointWhileDriving(Translation2d target) {
   // Called just before this Command runs the first time
   @Override
   public void initialize() {
-    targetPoint = RobotContainer.frontNoteCamera.notes.get(0).position;
+    targetPoint = RobotContainer.intakeCamera.notes.get(0).position;
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   public void execute() {
-    targetPoint = RobotContainer.frontNoteCamera.notes.get(0).position;
+    targetPoint = RobotContainer.intakeCamera.notes.get(0).position;
 
     Pose2d robotPose = RobotContainer.swerveDrive.getPose();
     Translation2d vec = targetPoint.minus(robotPose.getTranslation());

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

@@ -14,29 +14,38 @@
 import frc.robot.subsystems.drive.SwerveDrive;
 
 public class ShootSpeaker extends Command {
+  SwerveDrive swerveDrive;
   Shooter shooter;
   Alliance color;
-  boolean hasAlliance;
+  boolean hasAlliance, hasNoteInitially;
   double targetX, targetY, targetZ;
   final double v0 = 20;
   boolean goodTrajectory = true;
   Timer timer;
 
   public ShootSpeaker() {
     this.addRequirements(RobotContainer.shooter, RobotContainer.swerveDrive);
+    swerveDrive = RobotContainer.swerveDrive;
     shooter = RobotContainer.shooter;
     timer = new Timer();
   }
 
   // Called just before this Command runs the first time
   @Override
   public void initialize() {
+    if (!shooter.hasNote()) {
+      hasNoteInitially = false;
+      return;
+    }
+    hasNoteInitially = true;
+
     Optional<Alliance> alliance = DriverStation.getAlliance();
     hasAlliance = alliance.isPresent();
-    if (hasAlliance) {
-      color = alliance.get();
+    if (!hasAlliance) {
+      return;
     }
 
+    color = alliance.get();
     if (color == Alliance.Blue) {
       targetX = 0.4572 / 2;
       targetY = 8.001 - 2.063394 - (1.05 / 2);
@@ -46,15 +55,13 @@ public void initialize() {
       targetY = 8.001 - 2.063394 - (1.05 / 2);
       targetZ = 2.05;
     }
-
-    //new LEDShoot().schedule();
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   public void execute() {
     shooter.setVelocity(v0);
-    Pose2d robotPose = RobotContainer.swerveDrive.getPose();
+    Pose2d robotPose = swerveDrive.getPose();
 
     // theta phi time
     double[] optimalParams = shooter.optimizeShooterOrientation(1,
@@ -65,8 +72,8 @@ public void execute() {
     Translation3d shooterPose = shooter.shooterExitFieldRelative(robotPose, shooterPoseRobotRelative);
 
     double[] in = { shooterPose.getX(), shooterPose.getY(), shooterPose.getZ(),
-        RobotContainer.swerveDrive.vX + (v0 * Math.sin(Math.PI / 2 - optimalParams[0]) * Math.cos(optimalParams[1])),
-        RobotContainer.swerveDrive.vY + (v0 * Math.sin(Math.PI / 2 - optimalParams[0]) * Math.sin(optimalParams[1])),
+        swerveDrive.vX + (v0 * Math.sin(Math.PI / 2 - optimalParams[0]) * Math.cos(optimalParams[1])),
+        swerveDrive.vY + (v0 * Math.sin(Math.PI / 2 - optimalParams[0]) * Math.sin(optimalParams[1])),
         v0 * Math.cos(Math.PI / 2 - optimalParams[0]) };
 
     // we may not want to actually do any of this if it can't run in real time
@@ -89,44 +96,40 @@ public void execute() {
     // negative values when we push forward.
     var xSpeed = 0.0;
     if (Math.abs(RobotContainer.driverController.getLeftY()) > 0.05) {
-      xSpeed = RobotContainer.swerveDrive.sensControl(-RobotContainer.driverController.getLeftY()) * SwerveDrive.kMaxSpeedMetersPerSecond;
+      xSpeed = swerveDrive.sensControl(-RobotContainer.driverController.getLeftY()) * SwerveDrive.kMaxSpeedMetersPerSecond;
     }
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
     var ySpeed = 0.0;
     if (Math.abs(RobotContainer.driverController.getLeftX()) > 0.05) {
-      ySpeed = RobotContainer.swerveDrive.sensControl(-RobotContainer.driverController.getLeftX()) * SwerveDrive.kMaxSpeedMetersPerSecond;
+      ySpeed = swerveDrive.sensControl(-RobotContainer.driverController.getLeftX()) * SwerveDrive.kMaxSpeedMetersPerSecond;
     }
     boolean fieldRelative = (RobotContainer.driverController.getRightTriggerAxis() < 0.3);
 
 
-    RobotContainer.swerveDrive.holdAngleWhileDriving(xSpeed, ySpeed, angleSetpoint, fieldRelative);
+    swerveDrive.holdAngleWhileDriving(xSpeed, ySpeed, angleSetpoint, fieldRelative);
+    shooter.goodTrajectory = goodTrajectory;
 
     if (goodTrajectory && Math.abs(robotPose.getRotation().getRadians() - optimalParams[1]) < 0.3 && Math.abs(shooter.getPosition() - optimalParams[0]) < 0.3) {
       shooter.feed();
       timer.start();
     }
-
-
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   public boolean isFinished() {
-    if (!hasAlliance || timer.hasElapsed(1) || !shooter.hasNote()) {
-      return true;
-    }
-    return false;
+    return !hasNoteInitially || !hasAlliance || timer.hasElapsed(0.33);
   }
 
   // Called once after isFinished returns true
   @Override
   public void end(boolean interrupted) {
-    shooter.setVelocity(0);
+    shooter.stopShooting();
     shooter.stopIndexer();
-    RobotContainer.swerveDrive.stop();
+    swerveDrive.stop();
     timer.stop();
     timer.reset();
   }

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

@@ -125,6 +125,7 @@ public void periodic() {
     SmartDashboard.putNumber("IntakeAngle", getPosition());
     SmartDashboard.putNumber("IntakeSetpoint", rotationSetpoint);
     SmartDashboard.putNumber("IntakeError", Math.abs(getPosition() - rotationSetpoint));
+    SmartDashboard.putBoolean("IntakeHasNote", hasNoteInside());
 
     // kP = SmartDashboard.getNumber("kp", kP);
     // kI = SmartDashboard.getNumber("ki", kI);

src/main/java/frc/robot/subsystems/OldLEDStrip.java

@@ -1,15 +1,18 @@
 package frc.robot.subsystems;
 
-import java.util.Optional;
-
 import java.awt.Color;
 
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants;
 import frc.robot.RobotContainer;
 import frc.robot.RobotMap;
+import frc.robot.subsystems.Arm.ArmState;
 
 public class OldLEDStrip {
   Thread t;
@@ -19,8 +22,9 @@ public class OldLEDStrip {
   int numberSections;
   int counter;
   int ticksPerSecond = 50;
-  LEDState state = LEDState.kDisabled;
-  Optional<Alliance> alliance;
+
+  private LEDState state = LEDState.kDisabled;
+  private final Notifier updateLedState = new Notifier(this::updateState);
 
   float speedFactor = 1f;
   float sections = 5f;
@@ -64,6 +68,7 @@ public OldLEDStrip() {
 
     this.numberSections = Constants.LED_LENGTH;
     t.start();
+    updateLedState.startPeriodic(0.02);
   }
 
   public void setRGB(int index, int r, int g, int b) {
@@ -193,6 +198,7 @@ public void runSilly() {
   // run square wave of alliance color
   public void runDisabled() {
     ticksPerSecond = 50;
+    var alliance = DriverStation.getAlliance();
     if (alliance.isPresent()) {
       if (alliance.get() == Alliance.Blue) {
         runSquareWave(Color.BLUE, -0.1f, 5f);
@@ -209,6 +215,7 @@ public void runDisabled() {
   // if there is a note flash on and off really fast, if there's not a note run disabled pattern
   public void runIntake() {
     ticksPerSecond = 50;
+    var alliance = DriverStation.getAlliance();
     if (RobotContainer.shooter.hasNote()) {
       if (counter % 25 == 0) {
         for (int i = 0; i < Constants.LED_LENGTH; i++) {
@@ -232,6 +239,9 @@ public void runIntake() {
 
   // if there's a note, run square wave really fast, when the note leaves flash on and off really fast
   public void runShoot() {
+    var atSetpoint = RobotContainer.shooter.isShooterRampedUp(1);
+    var goodTrajectory = RobotContainer.shooter.goodTrajectory;
+
     if (RobotContainer.shooter.hasNote()) {
       runSquareWave(new Color(255, 100, 0), 1f, 8f);
     } else {
@@ -312,13 +322,31 @@ public void update() {
         break;
     }
 
-    runBlue();
-
     strip.setData(buffer);
     // strip2.setData(buffer);
     counter++;
   }
 
+  private void updateState() {
+    var isDisabled = RobotState.isDisabled();
+    var isScoringSpeaker = RobotContainer.shooter.veloctiySetpoint != 0;
+    var isScoringAmp = Math.abs(RobotContainer.arm.getPosition() - ArmState.kAmp.rotations) < 0.02;
+    var hasIntakeDeployed = RobotContainer.intake.isIntakeDown();
+
+    if (isDisabled)
+      state = LEDState.kDisabled;
+    else if(isScoringSpeaker)
+      state = LEDState.kShoot;
+    else if(isScoringAmp)
+      state = LEDState.kAmp;
+    else if(hasIntakeDeployed)
+      state = LEDState.kIntake;
+    else
+      state = LEDState.kStowed;
+
+    SmartDashboard.putString("LED State", state.toString());
+  }
+
   public enum LEDState {
     kDisabled,
     kIntake,

src/main/java/frc/robot/subsystems/Shooter.java

@@ -84,6 +84,7 @@ public class Shooter extends SubsystemBase {
 
   public double veloctiySetpoint;
   public double rotationSetpoint = ShooterState.kIntake.rotations;
+  public boolean goodTrajectory;
 
   public double shooterFlatAngle;
 
@@ -209,6 +210,7 @@ public boolean isShooterRampedUp(double tolerance) {
   public void stopShooting() {
     shootMotor.stopMotor();
     this.veloctiySetpoint = 0;
+    goodTrajectory = false;
   }
 
   public boolean atAngleSetpoint(double tolerance) {
@@ -398,6 +400,7 @@ public void periodic() {
     SmartDashboard.putNumber("shooterVelocityError", (Math.abs(getVelocity() - (-veloctiySetpoint))));
     SmartDashboard.putBoolean("shooterAtSetpoint", isShooterRampedUp(1));
     SmartDashboard.putNumber("shooterSetpoint", veloctiySetpoint);
+    SmartDashboard.putBoolean("shooterGoodTrajectory", goodTrajectory);
 
     goToSetpoint(rotationSetpoint);
   }