refactor auto to use definite reef align locations (define the left/r…

…ight side of reef to align to)

src/main/kotlin/frc/team449/ControllerBindings.kt

@@ -3,16 +3,14 @@ package frc.team449
 import com.ctre.phoenix6.SignalLogger
 import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
-import edu.wpi.first.units.Measure
 import edu.wpi.first.units.Units.*
 import edu.wpi.first.units.measure.Voltage
 import edu.wpi.first.wpilibj.DriverStation
-import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.ConditionalCommand
 import edu.wpi.first.wpilibj2.command.InstantCommand
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
-import frc.team449.commands.driveAlign.SimpleReefAlign
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism
 import frc.team449.subsystems.RobotConstants
 import frc.team449.subsystems.drive.swerve.SwerveSim
 import frc.team449.subsystems.superstructure.SuperstructureGoal
@@ -47,7 +45,10 @@ class ControllerBindings(
   private fun nonRobotBindings() {
     // slowDrive()
 
-    if (RobotBase.isSimulation()) resetOdometrySim()
+    /** NOTE: If you want to see simulated vision convergence times with this function, go to simulationPeriodic in
+     * RobotBase and change the passed in pose to it.simulationPeriodic to robot.drive.odometryPose
+     */
+//    if (RobotBase.isSimulation()) resetOdometrySim()
 
     resetGyro()
   }
@@ -58,31 +59,27 @@ class ControllerBindings(
     )
   }
 
-//  private fun score_l1() {
-//    driveController.a().onTrue(
-//      robot.superstructureManager.requestGoal(SuperstructureGoal.L1)
-//        .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
-//    )
-//  }
+  private fun score_l1() {
+    driveController.a().onTrue(
+      robot.superstructureManager.requestGoal(SuperstructureGoal.L1)
+    )
+  }
 
   private fun score_l2() {
     driveController.x().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L2)
-        .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
   }
 
   private fun score_l3() {
     driveController.b().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L3)
-        .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
   }
 
   private fun score_l4() {
     driveController.y().onTrue(
       robot.superstructureManager.requestGoal(SuperstructureGoal.L4)
-        .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
     )
   }
 
@@ -153,112 +150,98 @@ class ControllerBindings(
   }
 
   /** Characterization functions */
-  private fun characterizationExample() {
-    /** Example
-     *
-     val exampleSubsystemRoutine = SysIdRoutine(
-     SysIdRoutine.Config(
-     Volts.of(0.5).per(Seconds.of(1.0)),
-     Volts.of(3.0),
-     Seconds.of(10.0)
-     ) { state -> SignalLogger.writeString("state", state.toString()) },
-     Mechanism(
-     { voltage: Measure<Voltage> ->
-     run { robot.shooter.setVoltage(voltage.`in`(Volts)) }
-     },
-     null,
-     robot.shooter,
-     "shooter"
-     )
-     )
-
-     // Quasistatic Forwards
-     driveController.povUp().onTrue(
-     exampleSubsystemRoutine.quasistatic(SysIdRoutine.Direction.kForward)
-     )
-
-     // Quasistatic Reverse
-     driveController.povDown().onTrue(
-     exampleSubsystemRoutine.quasistatic(SysIdRoutine.Direction.kReverse)
-     )
-
-     // Dynamic Forwards
-     driveController.povRight().onTrue(
-     exampleSubsystemRoutine.dynamic(SysIdRoutine.Direction.kForward)
-     )
-
-     // Dynamic Reverse
-     driveController.povLeft().onTrue(
-     exampleSubsystemRoutine.dynamic(SysIdRoutine.Direction.kReverse)
-     )
-     */
+  private fun driveCharacterization() {
+    val driveRoutine = SysIdRoutine(
+      SysIdRoutine.Config(),
+      Mechanism(
+        { voltage: Voltage -> robot.drive.setVoltage(voltage.`in`(Volts)) },
+        null,
+        robot.drive
+      )
+    )
+
+    // Quasistatic Forwards
+    driveController.povUp().onTrue(
+      driveRoutine.quasistatic(SysIdRoutine.Direction.kForward)
+    )
+
+    // Quasistatic Reverse
+    driveController.povDown().onTrue(
+      driveRoutine.quasistatic(SysIdRoutine.Direction.kReverse)
+    )
+
+    // Dynamic Forwards
+    driveController.povRight().onTrue(
+      driveRoutine.dynamic(SysIdRoutine.Direction.kForward)
+    )
+
+    // Dynamic Reverse
+    driveController.povLeft().onTrue(
+      driveRoutine.dynamic(SysIdRoutine.Direction.kReverse)
+    )
   }
 
   private fun elevatorCharacterizaton() {
-    val elevatorSubsystemRoutine = SysIdRoutine(
+    val elevatorRoutine = SysIdRoutine(
       SysIdRoutine.Config(
         Volts.of(0.5).per(Second),
-        Volts.of(3.0),
+        Volts.of(2.0),
         Seconds.of(10.0)
       ) { state -> SignalLogger.writeString("state", state.toString()) },
-      SysIdRoutine.Mechanism(
-        { voltage: Voltage ->
-          run { robot.elevator.setVoltage(voltage.`in`(Volts)) }
-        },
+      Mechanism(
+        { voltage: Voltage -> robot.elevator.setVoltage(voltage.`in`(Volts)) },
         null,
         robot.elevator,
         "elevator"
       )
     )
 
     driveController.povUp().onTrue(
-      elevatorSubsystemRoutine.quasistatic(SysIdRoutine.Direction.kForward)
+      elevatorRoutine.quasistatic(SysIdRoutine.Direction.kForward)
     )
 
     driveController.povDown().onTrue(
-      elevatorSubsystemRoutine.quasistatic(SysIdRoutine.Direction.kReverse)
+      elevatorRoutine.quasistatic(SysIdRoutine.Direction.kReverse)
     )
 
     driveController.povRight().onTrue(
-      elevatorSubsystemRoutine.dynamic(SysIdRoutine.Direction.kForward)
+      elevatorRoutine.dynamic(SysIdRoutine.Direction.kForward)
     )
 
     driveController.povLeft().onTrue(
-      elevatorSubsystemRoutine.dynamic(SysIdRoutine.Direction.kReverse)
+      elevatorRoutine.dynamic(SysIdRoutine.Direction.kReverse)
     )
   }
 
   fun pivotCharacterizaton() {
-    val pivotSubsystemRoutine = SysIdRoutine(
+    val pivotRoutine = SysIdRoutine(
       SysIdRoutine.Config(
         Volts.of(0.5).per(Second),
         Volts.of(3.0),
         Seconds.of(10.0)
       ) { state -> SignalLogger.writeString("state", state.toString()) },
-      SysIdRoutine.Mechanism(
-        { voltage: Voltage ->
-          run { robot.pivot.setVoltage(voltage.`in`(Volts)) }
-        },
+      Mechanism(
+        { voltage: Voltage -> robot.pivot.setVoltage(voltage.`in`(Volts)) },
         null,
         robot.pivot,
         "elevator"
       )
     )
 
     driveController.povUp().onTrue(
-      pivotSubsystemRoutine.quasistatic(SysIdRoutine.Direction.kForward)
+      pivotRoutine.quasistatic(SysIdRoutine.Direction.kForward)
     )
 
     driveController.povDown().onTrue(
-      pivotSubsystemRoutine.quasistatic(SysIdRoutine.Direction.kReverse)
+      pivotRoutine.quasistatic(SysIdRoutine.Direction.kReverse)
     )
 
     driveController.povRight().onTrue(
-      pivotSubsystemRoutine.dynamic(SysIdRoutine.Direction.kForward)
+      pivotRoutine.dynamic(SysIdRoutine.Direction.kForward)
     )
 
     driveController.povLeft().onTrue(
-      pivotSubsystemRoutine.dynamic(SysIdRoutine.Direction.kReverse)
+      pivotRoutine.dynamic(SysIdRoutine.Direction.kReverse)
     )
   }
 

src/main/kotlin/frc/team449/RobotLoop.kt

@@ -167,7 +167,7 @@ class RobotLoop : TimedRobot() {
     robot.drive as SwerveSim
 
     VisionConstants.ESTIMATORS.forEach {
-      it.simulationPeriodic(robot.drive.odometryPose)
+      it.simulationPeriodic(robot.poseSubsystem.pose)
     }
 
     VisionConstants.VISION_SIM.debugField.getObject("EstimatedRobot").pose = robot.poseSubsystem.pose

src/main/kotlin/frc/team449/auto/routines/ThreeL4.kt

@@ -3,14 +3,15 @@ package frc.team449.auto.routines
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.InstantCommand
 import edu.wpi.first.wpilibj2.command.WaitCommand
-import edu.wpi.first.wpilibj2.command.WaitUntilCommand
 import frc.team449.Robot
 import frc.team449.auto.AutoUtil
 import frc.team449.auto.choreo.ChoreoRoutine
 import frc.team449.auto.choreo.ChoreoRoutineStructure
 import frc.team449.auto.choreo.ChoreoTrajectory
 import frc.team449.commands.driveAlign.SimpleReefAlign
+import frc.team449.subsystems.FieldConstants
 import frc.team449.subsystems.superstructure.SuperstructureGoal
+import java.util.Optional
 
 class ThreeL4(
   robot: Robot,
@@ -30,11 +31,11 @@ class ThreeL4(
         4 to premoveL4(robot),
       ),
       stopEventMap = hashMapOf(
-        1 to scoreL4(robot),
+        1 to scoreL4(robot, if (isRedAlliance && isRedStage || !isRedAlliance && !isRedStage) FieldConstants.ReefSide.RIGHT else FieldConstants.ReefSide.LEFT),
         2 to intakeSubstation(robot),
-        3 to scoreL4(robot),
+        3 to scoreL4(robot, if (isRedAlliance && isRedStage || !isRedAlliance && !isRedStage) FieldConstants.ReefSide.LEFT else FieldConstants.ReefSide.RIGHT),
         4 to intakeSubstation(robot),
-        5 to scoreL4(robot)
+        5 to scoreL4(robot, if (isRedAlliance && isRedStage || !isRedAlliance && !isRedStage) FieldConstants.ReefSide.RIGHT else FieldConstants.ReefSide.LEFT)
           .andThen(robot.superstructureManager.requestGoal(SuperstructureGoal.STOW)),
       ),
       debug = false,
@@ -51,19 +52,19 @@ class ThreeL4(
       .alongWith(robot.intake.intakeCoral())
   }
 
-  private fun scoreL4(robot: Robot): Command {
+  private fun scoreL4(robot: Robot, reefSide: FieldConstants.ReefSide): Command {
     return robot.superstructureManager.requestGoal(SuperstructureGoal.L4)
-      .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem))
+      .alongWith(SimpleReefAlign(robot.drive, robot.poseSubsystem, leftOrRight = Optional.of(reefSide)))
       .andThen(robot.intake.outtakeCoral())
-      .andThen(WaitUntilCommand { !robot.intake.infrared.get() })
+//      .andThen(WaitUntilCommand { !robot.intake.infrared.get() })
       .andThen(WaitCommand(0.15))
   }
 
   private fun intakeSubstation(robot: Robot): Command {
     return InstantCommand(robot.drive::stop)
       .andThen(robot.intake.intakeCoral())
       .andThen(robot.superstructureManager.requestGoal(SuperstructureGoal.SUBSTATION_INTAKE))
-      .andThen(WaitUntilCommand { !robot.intake.infrared.get() })
+//      .andThen(WaitUntilCommand { !robot.intake.infrared.get() })
       .andThen(WaitCommand(0.15))
       .andThen(robot.intake.stop())
   }

src/main/kotlin/frc/team449/commands/driveAlign/SimpleReefAlign.kt

@@ -14,6 +14,7 @@ import frc.team449.subsystems.FieldConstants
 import frc.team449.subsystems.RobotConstants
 import frc.team449.subsystems.drive.swerve.SwerveDrive
 import frc.team449.subsystems.vision.PoseSubsystem
+import java.util.Optional
 import kotlin.math.PI
 import kotlin.math.abs
 import kotlin.math.min
@@ -27,15 +28,16 @@ class SimpleReefAlign(
   translationSpeedLim: Double = 0.825 * RobotConstants.MAX_LINEAR_SPEED,
   translationAccelLim: Double = 5.75,
   headingSpeedLim: Double = PI,
-  headingAccelLim: Double = 6 * PI,
-  translationPID: Triple<Double, Double, Double> = Triple(7.5, 0.0, 0.0),
+  headingAccelLim: Double = 16 * PI,
+  translationPID: Triple<Double, Double, Double> = Triple(8.0, 0.0, 0.0),
   headingPID: Triple<Double, Double, Double> = Triple(7.5, 0.0, 0.0),
-  private val translationTolerance: Double = Units.inchesToMeters(0.75),
-  private val headingTolerance: Double = Units.degreesToRadians(1.25),
+  private val translationTolerance: Double = Units.inchesToMeters(1.0),
+  private val headingTolerance: Double = Units.degreesToRadians(1.5),
   private val speedTol: Double = 0.10,
   private val speedTolRot: Double = PI / 16,
   private val ffMinRadius: Double = 0.2,
-  private val ffMaxRadius: Double = 0.8
+  private val ffMaxRadius: Double = 0.65,
+  private val leftOrRight: Optional<FieldConstants.ReefSide> = Optional.empty()
 ) : Command() {
   init {
     addRequirements(drive)
@@ -64,8 +66,17 @@ class SimpleReefAlign(
     translationController.setTolerance(translationTolerance, speedTol)
     headingController.setTolerance(headingTolerance, speedTolRot)
 
-    targetPose = if (FieldConstants.REEF_LOCATIONS.isNotEmpty()) poseSubsystem.pose.nearest(FieldConstants.REEF_LOCATIONS) else Pose2d()
     val currentPose = poseSubsystem.pose
+
+    if (leftOrRight.isEmpty) {
+      targetPose = if (FieldConstants.REEF_LOCATIONS.isNotEmpty()) currentPose.nearest(FieldConstants.REEF_LOCATIONS) else Pose2d()
+    } else {
+      val closestReef = if (FieldConstants.REEF_LOCATIONS.isNotEmpty()) currentPose.nearest(FieldConstants.REEF_CENTER_LOCATIONS) else Pose2d()
+      val index = FieldConstants.REEF_CENTER_LOCATIONS.indexOf(closestReef)
+
+      targetPose = FieldConstants.REEF_LOCATIONS[2 * index + if (leftOrRight.get() == FieldConstants.ReefSide.LEFT) 0 else 1]
+    }
+
     val fieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(
       drive.currentSpeeds.vxMetersPerSecond,
       drive.currentSpeeds.vyMetersPerSecond,

src/main/kotlin/frc/team449/subsystems/FieldConstants.kt

@@ -11,6 +11,12 @@ object FieldConstants {
   const val fieldWidth = 8.05
 
   val REEF_LOCATIONS = arrayListOf<Pose2d>()
+  val REEF_CENTER_LOCATIONS = arrayListOf<Pose2d>()
+
+  enum class ReefSide {
+    LEFT,
+    RIGHT
+  }
 
   fun configureReef(alliance: Alliance) {
     val allianceComp = alliance == Alliance.Red
@@ -44,6 +50,24 @@ object FieldConstants {
         REEF_L
       )
     )
+
+    val REEF_1 = findPose(3.192615509033203, (4.189684867858887 + 3.8614695072174072) / 2, 0.0, allianceComp)
+    val REEF_2 = findPose((3.695124626159668 + 3.9832611083984375) / 2, (2.985105037689209 + 2.820899248123169) / 2, PI / 3, allianceComp)
+    val REEF_3 = findPose((4.9979729652404785 + 5.282362937927246) / 2, (2.8225479125976562 + 2.989065647125244) / 2, 2 * PI / 3, allianceComp)
+    val REEF_4 = findPose(5.78605842590332, (3.860325813293457 + 4.188675880432129) / 2, PI, allianceComp)
+    val REEF_5 = findPose((4.9979729652404785 + 5.282362937927246) / 2, (5.065289497375488 + 5.229397296905518) / 2, -2 * PI / 3, allianceComp)
+    val REEF_6 = findPose((3.695124626159668 + 3.9832611083984375) / 2, (5.231619358062744 + 5.066085338592529) / 2, -PI / 3, allianceComp)
+
+    REEF_CENTER_LOCATIONS.addAll(
+      listOf(
+        REEF_1,
+        REEF_2,
+        REEF_3,
+        REEF_4,
+        REEF_5,
+        REEF_6,
+      )
+    )
   }
 
   private fun findPose(x: Double, y: Double, angle: Double, isRed: Boolean): Pose2d {

src/main/kotlin/frc/team449/subsystems/drive/swerve/SwerveDrive.kt

@@ -1,7 +1,6 @@
 package frc.team449.subsystems.drive.swerve
 
 import dev.doglog.DogLog
-import edu.wpi.first.epilogue.Logged
 import edu.wpi.first.math.geometry.Translation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics
@@ -23,7 +22,6 @@ import kotlin.math.hypot
  * @param maxRotSpeed The maximum rotation speed of the chassis.
  * @param field The SmartDashboard [Field2d] widget that shows the robot's pose.
  */
-@Logged
 open class SwerveDrive(
   val modules: List<SwerveModule>,
   var maxLinearSpeed: Double,