add high interpolating map

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -217,16 +217,6 @@ object RobotContainer {
     ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand())
 
     ControlBoard.targetAmp.whileTrue(
-      runOnce({
-        val currentRotation = drivetrain.odomTRobot.rotation
-        setAmpAngle =
-          if (currentRotation > 0.0.degrees && currentRotation < 180.degrees) {
-            90.degrees
-          } else {
-            270.degrees
-          }
-      })
-        .andThen(
           TargetAngleCommand(
             driver = Ryan(),
             { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
@@ -236,7 +226,6 @@ object RobotContainer {
             drivetrain,
             ampAngle
           )
-        )
     )
 
     ControlBoard.climbAlignFar.whileTrue(

src/main/kotlin/com/team4099/robot2023/config/constants/SuperstructureConstants.kt

@@ -49,6 +49,49 @@ object SuperstructureConstants {
       Pair(198.15.inches, -2.2.degrees)
     )
 
+
+  val highDistanceFlywheelSpeedTableReal =
+    listOf(
+      Pair(
+        FieldConstants.subwooferX + FieldConstants.edgeOfBumperToCenter,
+        3000.rotations.perMinute
+      ),
+      Pair(55.55.inches, 2500.rotations.perMinute),
+      Pair(63.89.inches, 2500.rotations.perMinute),
+      Pair(75.7.inches, 2500.rotations.perMinute),
+      Pair(84.25.inches, 2500.rotations.perMinute),
+      Pair(92.6.inches, 2500.rotations.perMinute),
+      Pair(103.7.inches, 2500.rotations.perMinute),
+      Pair(113.2.inches, 2500.rotations.perMinute),
+      Pair(122.1.inches, 3000.rotations.perMinute),
+      Pair(134.inches, 3000.rotations.perMinute),
+      Pair(144.8.inches, 3000.rotations.perMinute),
+      Pair(156.8.inches, 3500.rotations.perMinute),
+      Pair(172.2.inches, 3500.rotations.perMinute),
+      Pair(185.6.inches, 4000.rotations.perMinute),
+      Pair(198.15.inches, 4000.rotations.perMinute)
+    )
+
+  val highDistanceWristAngleTableReal =
+    listOf(
+      Pair(FieldConstants.subwooferX + FieldConstants.edgeOfBumperToCenter, -34.5.degrees),
+      Pair(55.55.inches, -30.degrees),
+      Pair(63.89.inches, -25.degrees),
+      Pair(75.5.inches, -20.degrees),
+      Pair(84.25.inches, -17.degrees),
+      Pair(92.6.inches, -15.25.degrees),
+      Pair(103.7.inches, -13.degrees),
+      Pair(113.23.inches, -11.degrees),
+      Pair(122.0.inches, -10.5.degrees),
+      Pair(134.inches, -9.degrees),
+      Pair(143.8.inches, -6.75.degrees),
+      Pair(156.8.inches, -5.5.degrees),
+      Pair(166.9.inches, -4.5.degrees),
+      Pair(172.2.inches, -4.375.degrees),
+      Pair(185.6.inches, -2.95.degrees),
+      Pair(198.15.inches, -2.2.degrees)
+    )
+
   val distanceFlywheelSpeedTableSim =
     listOf(
       Pair(1.meters, 3000.rotations.perMinute),

src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/AutoAim.kt

@@ -33,6 +33,9 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) {
   val flywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap()
   val wristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap()
 
+  val highFlywheelSpeedRPMInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap()
+  val highWristAngleDegreesInterpolationTable: InterpolatingDoubleTreeMap = InterpolatingDoubleTreeMap()
+
   val tunableFlywheelInterpolationTable:
     List<Pair<LoggedTunableValue<Meter>, LoggedTunableValue<Velocity<Radian>>>>
 
@@ -108,6 +111,14 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) {
             )
           )
         }
+
+      SuperstructureConstants.highDistanceFlywheelSpeedTableReal.forEach {
+        highWristAngleDegreesInterpolationTable.put(it.first.inMeters, it.second.inRotationsPerMinute)
+      }
+      SuperstructureConstants.highDistanceWristAngleTableReal.forEach {
+        highWristAngleDegreesInterpolationTable.put(it.first.inMeters, it.second.inDegrees)
+      }
+
     }
 
     updateFlywheelInterpolationTable()
@@ -150,6 +161,17 @@ class AutoAim(val drivetrain: Drivetrain, val vision: Vision) {
     return wristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).degrees
   }
 
+  fun calculateHighFlywheelSpeed(): AngularVelocity {
+    return highFlywheelSpeedRPMInterpolationTable.get(calculateDistanceFromSpeaker().inMeters)
+      .rotations
+      .perMinute
+  }
+
+  fun calculateHighWristAngle(): Angle {
+    return highWristAngleDegreesInterpolationTable.get(calculateDistanceFromSpeaker().inMeters).degrees
+  }
+
+
   fun updateFlywheelInterpolationTable() {
     flywheelSpeedRPMInterpolationTable.clear()
     tunableFlywheelInterpolationTable.forEach {

src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt

@@ -315,12 +315,7 @@ class Superstructure(
             nextState = SuperstructureStates.EJECT_GAME_PIECE_PREP
           }
           is Request.SuperstructureRequest.PrepScoreAmp -> {
-            val currentRotation = drivetrain.odomTRobot.rotation
-            if ((currentRotation > 0.0.degrees && currentRotation < 180.degrees)) {
-              nextState = SuperstructureStates.WRIST_AMP_PREP
-            } else {
               nextState = SuperstructureStates.ELEVATOR_AMP_PREP
-            }
           }
           is Request.SuperstructureRequest.ScoreSpeaker -> {
             if (feeder.hasNote) {
@@ -443,6 +438,30 @@ class Superstructure(
         }
       }
       SuperstructureStates.AUTO_AIM -> {
+        val targetFlywheelSpeed = aimer.calculateHighFlywheelSpeed()
+        val targetWristAngle = aimer.calculateHighWristAngle()
+
+        Logger.recordOutput("AutoAim/FlywheelSpeed", targetFlywheelSpeed.inRotationsPerMinute)
+        Logger.recordOutput("AutoAim/WristAngle", targetWristAngle.inDegrees)
+
+        flywheel.currentRequest = Request.FlywheelRequest.TargetingVelocity(targetFlywheelSpeed)
+        wrist.currentRequest = Request.WristRequest.TargetingPosition(targetWristAngle)
+
+        when (currentRequest) {
+          is Request.SuperstructureRequest.Idle -> {
+            nextState = SuperstructureStates.IDLE
+          }
+          is Request.SuperstructureRequest.ScoreSpeaker -> {
+            nextState = SuperstructureStates.SCORE_SPEAKER
+            shootStartTime = Clock.fpgaTime
+          }
+          is Request.SuperstructureRequest.PrepScoreSpeakerHigh -> {
+            nextState = SuperstructureStates.SCORE_SPEAKER_HIGH_PREP
+          }
+        }
+      }
+
+      SuperstructureStates.HIGH_AIM -> {
         val targetFlywheelSpeed = aimer.calculateFlywheelSpeed()
         val targetWristAngle = aimer.calculateWristAngle()
 
@@ -640,6 +659,8 @@ class Superstructure(
         when (currentRequest) {
           is Request.SuperstructureRequest.Idle -> {
             nextState = SuperstructureStates.IDLE
+          } is Request.SuperstructureRequest.AutoAim -> {
+            nextState = SuperstructureStates.HIGH_AIM
           }
         }
       }
@@ -956,7 +977,8 @@ class Superstructure(
 
   fun autoAimCommand(): Command {
     val returnCommand =
-      runOnce { currentRequest = Request.SuperstructureRequest.AutoAim() }.until {
+      runOnce {
+        currentRequest = Request.SuperstructureRequest.AutoAim() }.until {
         isAtRequestedState && currentState == SuperstructureStates.AUTO_AIM
       }
     returnCommand.name = "AutoAim"
@@ -1066,7 +1088,8 @@ class Superstructure(
       EJECT_GAME_PIECE_PREP,
       PASSING_SHOT_PREP,
       PASSING_SHOT,
-      AUTO_AIM
+      AUTO_AIM,
+      HIGH_AIM
     }
   }
 }