Note Following While Pathing

2024-Robot/src/main/java/frc/robot/commands/AutoParser.java

@@ -128,7 +128,7 @@ public AutoParser(Drive drive, Intake intake, Feeder feeder, Shooter shooter, Pe
         }
       }
       ArrayListParallelDeadlineGroup parallelDeadlineGroup = new ArrayListParallelDeadlineGroup(
-        new AutonomousFollower(drive, this.sampledPoints, previousHaltingTime, haltingTime, false),
+        new AutonomousFollower(drive, this.sampledPoints, previousHaltingTime, haltingTime, false, false),
         parallelCommands
       );
       this.autoCommands.add(parallelDeadlineGroup);
@@ -154,7 +154,7 @@ public AutoParser(Drive drive, Intake intake, Feeder feeder, Shooter shooter, Pe
       previousHaltingTime = haltingTime;
     }
     this.autoCommands.add(
-      new AutonomousFollower(drive, this.sampledPoints, previousHaltingTime, false)
+      new AutonomousFollower(drive, this.sampledPoints, previousHaltingTime, false, false)
     );
 
     addCommands(this.autoCommands);

2024-Robot/src/main/java/frc/robot/commands/AutonomousFollower.java

@@ -31,26 +31,29 @@ public class AutonomousFollower extends Command {
   private double desiredThetaChange = 0;
 
   private boolean record;
+  private boolean pickupNote;
 
   private ArrayList<double[]> recordedOdometry = new ArrayList<double[]>();
   private double pathStartTime;
   private double pathEndTime;
 
-  public AutonomousFollower(Drive drive, JSONArray pathPoints, double pathStartTime, double pathEndTime, boolean record) {
+  public AutonomousFollower(Drive drive, JSONArray pathPoints, double pathStartTime, double pathEndTime, boolean record, boolean pickupNote) {
     this.drive = drive;
     this.path = pathPoints;
     this.record = record;
     this.pathStartTime = pathStartTime;
     this.pathEndTime = pathEndTime;
+    this.pickupNote = pickupNote;
     addRequirements(drive);
   }
 
-  public AutonomousFollower(Drive drive, JSONArray pathPoints, double pathStartTime, boolean record) {
+  public AutonomousFollower(Drive drive, JSONArray pathPoints, double pathStartTime, boolean record, boolean pickupNote) {
     this.drive = drive;
     this.path = pathPoints;
     this.record = record;
     this.pathStartTime = pathStartTime;
     this.pathEndTime = path.getJSONArray(path.length() - 1).getDouble(0);
+    this.pickupNote = pickupNote;
     addRequirements(drive);
   }
 
@@ -73,7 +76,7 @@ public void execute() {
     currentTime = Timer.getFPGATimestamp() - initTime + pathStartTime;
 
     // call PIDController function
-    desiredVelocityArray = drive.pidController(odometryFusedX, odometryFusedY, odometryFusedTheta, currentTime, path);
+    desiredVelocityArray = drive.pidController(odometryFusedX, odometryFusedY, odometryFusedTheta, currentTime, path, pickupNote);
 
     // create velocity vector and set desired theta change
     Vector velocityVector = new Vector();

2024-Robot/src/main/java/frc/robot/commands/ShootWhilePathingAndIntaking.java

@@ -67,8 +67,9 @@ public class ShootWhilePathingAndIntaking extends Command {
   private double intakeRPM;
 
   private boolean haveIntakeNote;
+  private boolean pickupNote;
 
-  public ShootWhilePathingAndIntaking(Drive drive, Intake intake, Feeder feeder, Shooter shooter, Peripherals peripherals, TOF tof, JSONArray pathPoints, double intakeRPM, double feederRPM, double shootDelay) {
+  public ShootWhilePathingAndIntaking(Drive drive, Intake intake, Feeder feeder, Shooter shooter, Peripherals peripherals, TOF tof, JSONArray pathPoints, double intakeRPM, double feederRPM, double shootDelay, boolean pickupNote) {
     this.drive = drive;
     this.intake = intake;
     this.feeder = feeder;
@@ -79,6 +80,7 @@ public ShootWhilePathingAndIntaking(Drive drive, Intake intake, Feeder feeder, S
     this.intakeRPM = intakeRPM;
     this.feederRPM = feederRPM;
     this.shootDelay = shootDelay;
+    this.pickupNote = pickupNote;
     addRequirements(this.drive, this.feeder, this.shooter);
   }
 
@@ -174,7 +176,7 @@ public void execute() {
     this.odometryY = this.drive.getFusedOdometryY();
     this.odometryTheta = this.drive.getFusedOdometryTheta();
 
-    double[] desiredVelocityArray = this.drive.pidController(this.odometryX, this.odometryY, this.odometryTheta, this.currentTime, this.path);
+    double[] desiredVelocityArray = this.drive.pidController(this.odometryX, this.odometryY, this.odometryTheta, this.currentTime, this.path, this.pickupNote);
     Vector velocityVector = new Vector();
     velocityVector.setI(desiredVelocityArray[0]);
     velocityVector.setJ(desiredVelocityArray[1]);

2024-Robot/src/main/java/frc/robot/commands/autos/FivePieceAuto.java

@@ -114,7 +114,7 @@ public FivePieceAuto(Drive drive, Peripherals peripherals, Intake intake, Feeder
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3),
         new ParallelCommandGroup(
           new SequentialCommandGroup(
-            new AutonomousFollower(drive, pathJSON, 0, false),
+            new AutonomousFollower(drive, pathJSON, 0, false, false),
             new TurnToTarget(drive, peripherals)
           ),
           new AutoPrepForShot(shooter, tof, 20, 3000)
@@ -125,7 +125,7 @@ public FivePieceAuto(Drive drive, Peripherals peripherals, Intake intake, Feeder
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3),
         new ParallelCommandGroup(
           new SequentialCommandGroup(
-            new AutonomousFollower(drive, pathJSON2, 0, false),
+            new AutonomousFollower(drive, pathJSON2, 0, false, false),
             new TurnToTarget(drive, peripherals)
           ),
           new AutoPrepForShot(shooter, tof, 20, 3000)
@@ -136,15 +136,15 @@ public FivePieceAuto(Drive drive, Peripherals peripherals, Intake intake, Feeder
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3),
         new ParallelCommandGroup(
           new SequentialCommandGroup(
-            new AutonomousFollower(drive, pathJSON3, 0, false),
+            new AutonomousFollower(drive, pathJSON3, 0, false, false),
             new TurnToTarget(drive, peripherals)
           ),
           new AutoPrepForShot(shooter, tof, 20, 3000)
         )
       ),
       new AutoShoot(drive, shooter, feeder, peripherals, lights, tof, 1200, 1),
       new ParallelDeadlineGroup(
-        new AutonomousFollower(drive, pathJSON4, 0, false),
+        new AutonomousFollower(drive, pathJSON4, 0, false, false),
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600),
         new AutoPrepForShot(shooter, tof, 20, 3000)
       ),

2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceCloseAuto.java

@@ -99,7 +99,7 @@ public FourPieceCloseAuto(Drive drive, Peripherals peripherals, Intake intake, F
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3),
         new ParallelCommandGroup(
           new SequentialCommandGroup(
-            new AutonomousFollower(drive, pathJSON, 0, false),
+            new AutonomousFollower(drive, pathJSON, 0, false, false),
             new TurnToTarget(drive, peripherals)
           ),
           new AutoPrepForShot(shooter, tof, 20, 3000)
@@ -110,7 +110,7 @@ public FourPieceCloseAuto(Drive drive, Peripherals peripherals, Intake intake, F
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3),
         new ParallelCommandGroup(
           new SequentialCommandGroup(
-            new AutonomousFollower(drive, pathJSON2, 0, false),
+            new AutonomousFollower(drive, pathJSON2, 0, false, false),
             new TurnToTarget(drive, peripherals)
           ),
           new AutoPrepForShot(shooter, tof, 20, 3000)
@@ -121,7 +121,7 @@ public FourPieceCloseAuto(Drive drive, Peripherals peripherals, Intake intake, F
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600, 3),
         new ParallelCommandGroup(
           new SequentialCommandGroup(
-            new AutonomousFollower(drive, pathJSON3, 0, false),
+            new AutonomousFollower(drive, pathJSON3, 0, false, false),
             new TurnToTarget(drive, peripherals)
           ),
           new AutoPrepForShot(shooter, tof, 20, 3000)

2024-Robot/src/main/java/frc/robot/commands/autos/FourPieceFarBottomAuto.java

@@ -96,19 +96,19 @@ public FourPieceFarBottomAuto(Drive drive, Peripherals peripherals, Intake intak
         new RunIntake(intake, Constants.SetPoints.IntakePosition.kDOWN, 1200)
       ),
       new ParallelDeadlineGroup(
-        new AutonomousFollower(drive, pathJSON, 0, false),
+        new AutonomousFollower(drive, pathJSON, 0, false, false),
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600),
         new AutoPrepForShot(shooter, tof, 20, 3000)
       ),
       new AutoShoot(drive, shooter, feeder, peripherals, lights, tof, 1200, 2),
       new ParallelDeadlineGroup(
-        new AutonomousFollower(drive, pathJSON2, 0, false),
+        new AutonomousFollower(drive, pathJSON2, 0, false, false),
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600),
         new AutoPrepForShot(shooter, tof, 20, 3000)
       ),
       new AutoShoot(drive, shooter, feeder, peripherals, lights, tof, 1200, 2),
       new ParallelDeadlineGroup(
-        new AutonomousFollower(drive, pathJSON3, 0, false),
+        new AutonomousFollower(drive, pathJSON3, 0, false, false),
         new AutoIntake(intake, feeder, climber, lights, tof, Constants.SetPoints.IntakePosition.kDOWN, 1200, 600),
         new AutoPrepForShot(shooter, tof, 20, 3000)
       ),

2024-Robot/src/main/java/frc/robot/subsystems/Drive.java

@@ -916,7 +916,7 @@ public JSONArray getPathPoint(JSONArray path, double time){
   }
 
   // Autonomous algorithm
-  public double[] pidController(double currentX, double currentY, double currentTheta, double time, JSONArray pathPoints) {
+  public double[] pidController(double currentX, double currentY, double currentTheta, double time, JSONArray pathPoints, boolean pickupNote) {
     if(time < pathPoints.getJSONArray(pathPoints.length() - 1).getDouble(0)) {
         JSONArray currentPoint = pathPoints.getJSONArray(0);
         JSONArray targetPoint = pathPoints.getJSONArray(0);
@@ -965,6 +965,22 @@ public double[] pidController(double currentX, double currentY, double currentTh
           currentPointTheta = Math.PI - currentPointTheta;
         }
 
+        if (pickupNote){
+          double angleToNote =  (peripherals.getBackCamTargetTx());
+          double differenceX = Math.abs(targetX - currentPointX);
+          double differenceY = Math.abs(targetY - currentPointY);
+          double r = (Math.sqrt((differenceX * differenceX) + (differenceY * differenceY)));
+          double adjustedX = r * (Math.cos((targetTheta + Math.PI) - angleToNote));
+          double adjustedY = r * (Math.sin((targetTheta + Math.PI) - angleToNote));
+          targetX = targetX + adjustedX;
+          targetY = targetY + (3 * adjustedY);
+          System.out.println("note: " + angleToNote);
+          System.out.println("x: " + targetX);
+          System.out.println("y: " + targetY);
+          targetTheta = targetTheta - angleToNote;
+          System.out.println("theta: " + targetTheta);
+        }
+
         double feedForwardX = (targetX - currentPointX)/(targetTime - currentPointTime);
         double feedForwardY = (targetY - currentPointY)/(targetTime - currentPointTime);
         // double feedForwardTheta = -(targetTheta - currentPointTheta)/(targetTime - currentPointTime);

2024-Robot/src/main/java/frc/robot/subsystems/Peripherals.java

@@ -7,16 +7,10 @@
 
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.hardware.Pigeon2;
-import com.kauailabs.navx.frc.AHRS;
 
-import edu.wpi.first.networktables.ConnectionInfo;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.PowerDistribution;
-import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
-import edu.wpi.first.wpilibj.SPI.Port;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.commands.defaults.PeripheralsDefault;
@@ -25,6 +19,7 @@
 public class Peripherals extends SubsystemBase {
   private NetworkTable backCam = NetworkTableInstance.getDefault().getTable("limelight-back");
   private NetworkTableEntry backCamJSON = backCam.getEntry("json");
+  private NetworkTableEntry backCamTx = backCam.getEntry("tx");
   private NetworkTable frontCam = NetworkTableInstance.getDefault().getTable("limelight-front");
   private NetworkTableEntry frontCamJSON = frontCam.getEntry("json");
   private NetworkTableEntry frontCamTy = frontCam.getEntry("ty");
@@ -114,6 +109,10 @@ public JSONObject getFrontCamLatencies(){
     return latencies;
   }
 
+  public double getBackCamTargetTx(){
+    return backCamTx.getDouble(0.0) * Math.PI / 180;
+  }
+
   public void setFrontCamPipeline(int pipeline){
     frontCam.getEntry("pipeline").setNumber(pipeline);
   }