feat: object detection auto correction

src/main/java/com/frcteam3636/frc2024/Robot.kt

@@ -1,14 +1,19 @@
 package com.frcteam3636.frc2024
 
 import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain
+import com.frcteam3636.frc2024.subsystems.drivetrain.LocalADStarAK
 import com.frcteam3636.frc2024.subsystems.intake.Intake
+import com.frcteam3636.frc2024.subsystems.intake.Intake.intake
 import com.frcteam3636.frc2024.subsystems.shooter.Shooter
 import com.frcteam3636.frc2024.subsystems.shooter.speakerTranslation
 import com.pathplanner.lib.auto.AutoBuilder
 import com.pathplanner.lib.auto.NamedCommands
+import com.pathplanner.lib.path.PathConstraints
+import com.pathplanner.lib.pathfinding.Pathfinding
 import edu.wpi.first.hal.FRCNetComm.tInstances
 import edu.wpi.first.hal.FRCNetComm.tResourceType
 import edu.wpi.first.hal.HAL
+import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.networktables.NetworkTableInstance
 import edu.wpi.first.units.Units
 import edu.wpi.first.wpilibj.*
@@ -56,6 +61,8 @@ object Robot : LoggedRobot() {
             tResourceType.kResourceType_Language, tInstances.kLanguage_Kotlin, 0, WPILibVersion.Version
         )
 
+        Pathfinding.setPathfinder(LocalADStarAK())
+
         if (isReal()) {
             Logger.addDataReceiver(WPILOGWriter("/U")) // Log to a USB stick
             Logger.addDataReceiver(NT4Publisher()) // Publish data to NetworkTables
@@ -270,11 +277,32 @@ object Robot : LoggedRobot() {
     }
 }
 
-private fun doIntakeSequence(): Command =
+fun generatePathToTargetThenIntakeAndReturnToPreviousPosition(target: Pose2d): Command {
+    val intialPosition = Drivetrain.estimatedPose
+    val constraints = PathConstraints(3.0, 1.0, edu.wpi.first.math.util.Units.degreesToRadians(540.0), edu.wpi.first.math.util.Units.degreesToRadians(720.0))
+    val pathfindToTargetCommand = AutoBuilder.pathfindToPose(
+        target,
+        constraints,
+        0.0,
+        0.0
+    )
+    val pathfindToPreviousPositionCommand = AutoBuilder.pathfindToPose(
+        intialPosition,
+        constraints,
+        0.0,
+        0.0
+    )
+    return Commands.sequence(
+        pathfindToTargetCommand,
+        autoIntake(),
+        pathfindToPreviousPositionCommand
+    )
+}
+
+fun autoIntake(): Command =
     Commands.sequence(
-        Intake.intake(),
+        intake(),
         Commands.runOnce({ Note.state = Note.State.HANDOFF }),
-        Commands.waitUntil(Shooter.Pivot.isStowed),
         Commands.race(
             Commands.parallel(
                 Intake.index(),
@@ -293,6 +321,15 @@ private fun doIntakeSequence(): Command =
                 Commands.runOnce({ Note.state = Note.State.SHOOTER })
             )
         )
+    ).withTimeout(1.5)
+
+private fun doIntakeSequence(): Command =
+    Commands.sequence(
+        Commands.waitUntil(Shooter.Pivot.isStowed),
+        autoIntake(),
+        Commands.either(Intake.inputs.target?.let { generatePathToTargetThenIntakeAndReturnToPreviousPosition(it) }, Commands.none()) {
+            Note.state == Note.State.NONE && Intake.inputs.target != null
+        }
     )
 
 object Note {

src/main/java/com/frcteam3636/frc2024/subsystems/intake/Intake.kt

@@ -1,9 +1,18 @@
 package com.frcteam3636.frc2024.subsystems.intake
 
+import com.frcteam3636.frc2024.Note
 import com.frcteam3636.frc2024.Robot
-import edu.wpi.first.wpilibj2.command.*
+import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain
+import com.pathplanner.lib.auto.AutoBuilder
+import com.pathplanner.lib.path.PathConstraints
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.util.Units
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Commands
+import edu.wpi.first.wpilibj2.command.Subsystem
 import org.littletonrobotics.junction.Logger
 
+
 object Intake : Subsystem {
     private var io: IntakeIO = when (Robot.model) {
         Robot.Model.SIMULATION -> IntakeIOSim()
@@ -38,10 +47,14 @@ object Intake : Subsystem {
             },
             {
                 io.setUnderBumperRoller(0.0)
+                if (inputs.isIntaking)
+                    Note.state = Note.State.HANDOFF
 //                io.setOverBumperRoller(0.0)
             }
         ).until(inputs::isIntaking)
 
+
+
     fun index(): Command =
         Commands.sequence(
             Commands.runOnce({ io.setUnderBumperRoller(0.5) }),

src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt

@@ -4,12 +4,17 @@ import com.frcteam3636.frc2024.CANSparkFlex
 import com.frcteam3636.frc2024.REVMotorControllerId
 import com.frcteam3636.frc2024.Robot
 import com.revrobotics.CANSparkLowLevel
-import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.*
 import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.DigitalInput
+import edu.wpi.first.wpilibj.DigitalOutput
 import edu.wpi.first.wpilibj.simulation.FlywheelSim
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
+import org.photonvision.PhotonCamera
+import org.photonvision.PhotonUtils
+import org.photonvision.targeting.PhotonPipelineResult
 
 interface IntakeIO {
     class IntakeInputs : LoggableInputs {
@@ -19,6 +24,7 @@ interface IntakeIO {
         var utbCurrent: Double = 0.0
         var isIntaking: Boolean = false
         var beamBreak: Boolean = false
+        var target: Pose2d? = null
 
         override fun toLog(table: LogTable?) {
             table?.put("OTB Roller Velocity", otbRollerVelocity)
@@ -60,6 +66,7 @@ class IntakeIOReal : IntakeIO {
             CANSparkLowLevel.MotorType.kBrushless
         )
     private var beamBreakSensor: DigitalInput = DigitalInput(Constants.BEAM_BREAK_PORT)
+    private var camera: PhotonCamera = PhotonCamera("photonvision")
 
     override fun updateInputs(inputs: IntakeIO.IntakeInputs) {
 //        inputs.otbRollerVelocity = Rotation2d(otbRollers.encoder.velocity)
@@ -68,6 +75,26 @@ class IntakeIOReal : IntakeIO {
         inputs.utbCurrent = utbRollers.outputCurrent
         inputs.beamBreak = beamBreakSensor.get()
         inputs.isIntaking = !inputs.beamBreak
+        val result: PhotonPipelineResult = camera.latestResult
+        if (result.hasTargets()) {
+            // Calculate Pose2d of the note
+            val distance: Double = PhotonUtils.calculateDistanceToTargetMeters(
+                CAMERA_POSE.z,
+                0.0,
+                CAMERA_POSE.rotation.y,
+                Units.degreesToRadians(result.bestTarget.pitch)
+            )
+            val cameraRelative: Translation3d = Translation3d(
+                distance,
+                Rotation3d(
+                    0.0, Units.degreesToRadians(result.bestTarget.pitch), Units.degreesToRadians(result.bestTarget.yaw)
+                )
+            )
+            val fieldRelative: Translation3d = cameraRelative.rotateBy(CAMERA_POSE.rotation).plus(CAMERA_POSE.translation)
+            inputs.target = Pose2d(fieldRelative.toTranslation2d(), Rotation2d())
+        } else {
+            inputs.target = null
+        }
     }
 
     override fun setOverBumperRoller(speed: Double) {
@@ -81,6 +108,10 @@ class IntakeIOReal : IntakeIO {
     internal companion object Constants {
         const val BEAM_BREAK_PORT = 0
         const val BEAM_BREAK_CURRENT_THRESHOLD = 50.0
+        val CAMERA_POSE = Transform3d(
+                Translation3d(0.1175, 0.0, Units.inchesToMeters(18.0)),
+                Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)
+            )
     }
 }