Shoot while moving updates

TeamCode/src/main/kotlin/pioneer/Constants.kt

@@ -104,7 +104,7 @@ object Constants {
     // -------- Follower (path following) --------
     object Follower {
         /** The threshold in cm to consider the target reached. */
-        const val POSITION_THRESHOLD = 1.25
+        const val POSITION_THRESHOLD = 1.75
 
         /** The threshold in radians to consider the target heading reached. */
         const val ROTATION_THRESHOLD = 0.06
@@ -136,7 +136,7 @@ object Constants {
         @JvmField var Y_KD = 0.0
 
         // Theta PID coefficients for heading interpolation
-        @JvmField var THETA_KP = 3.0 // was 5.0
+        @JvmField var THETA_KP = 5.0 // was 5.0
         @JvmField var THETA_KI = 0.0
         @JvmField var THETA_KD = 0.0
 
@@ -173,28 +173,32 @@ object Constants {
         // val distortionCoefficients = floatArrayOf(0.0573F, 2.0205F, -0.0331F, 0.0021F, -14.6155F, 0F, 0F, 0F)
     }
 
+    @Config
     object Spindexer {
-        // External Encoder
-        @JvmField var KP = 0.00475
-        @JvmField var KI = 0.00005
-        @JvmField var KD = 0.0125
+        @JvmField var KP = 0.004
+        @JvmField var KI = 0.0
+        @JvmField var KD = 0.00125
 
         @JvmField var KS_START = 0.04
 
+        @JvmField var SHOOT_POWER = 0.2
+
         @JvmField var MAX_POWER_RATE = 100.0
 
         @JvmField var MOTOR_TOLERANCE_TICKS = 0 // was 75 // stops moving within tolerance (in outtake for magnets)
 
         const val SHOOTING_TOLERANCE_TICKS = 4
         const val DETECTION_TOLERANCE_TICKS = 18
         const val VELOCITY_TOLERANCE_TPS = 50
+        const val ALLOWED_REVERSE_TICKS = 50 // How far spindexer can reverse without doing a 360
         const val TICKS_PER_REV = 537.7
 
         // Time required to confirm an artifact has been intaken (ms)
-        const val CONFIRM_INTAKE_MS = 67.0
+        const val CONFIRM_INTAKE_MS = 25.0
 
         // Max time the artifact can disappear without resetting confirmation (ms)
-        const val CONFIRM_LOSS_MS = 10
+        const val CONFIRM_LOSS_MS = 0.0
+        const val MAX_VELOCITY = 2400 // tps
     }
 
     object Turret {
@@ -239,14 +243,17 @@ object Constants {
         var spindexerMotorTicks = 0
     }
 
+    @Config
     object Flywheel {
-        @JvmField var KP = 0.0075
+        @JvmField var KP = 0.0001
         @JvmField var KI = 0.0
-        @JvmField var KD = 0.0
+        @JvmField var KD = 0.001
         @JvmField var KF = 0.000415
 
         val idleVelocity = 300.0
 
+        @JvmField var VELOCITY_COMPENSATION_FACTOR = 1.0
+
         enum class FlywheelOperatingMode{
             ALWAYS_IDLE,
             FULL_OFF,

TeamCode/src/main/kotlin/pioneer/decode/Points.kt

@@ -40,7 +40,7 @@ class Points(
     val START_GOAL = Pose(133.0, 134.0, theta = 0.67).T(color)
     val START_FAR = Pose(43.0, -157.0, theta = 0.0).T(color)
 
-    val SHOOT_GOAL_CLOSE = Pose(55.0, 30.0, theta = -PI/2).T(color)
+    val SHOOT_GOAL_CLOSE = Pose(55.0, 25.0, theta = -PI/2).T(color)
     val SHOOT_GOAL_FAR = Pose(43.0, -140.0, theta = -PI/2).T(color)
 
     val LEAVE_POSITION = Pose(60.0, -60.0).T(color)
@@ -53,8 +53,8 @@ class Points(
     val PREP_COLLECT_MID = Pose(prepCollectX, collectY(1), theta=collectTheta).T(color)
     val PREP_COLLECT_AUDIENCE = Pose(prepCollectX, collectY(2), theta=collectTheta).T(color)
 
-    val PREP_COLLECT_START_VELOCITY = Pose(0.0, -75.0).T(color)
-    val PREP_COLLECT_END_VELOCITY = Pose(75.0, 0.0).T(color)
+    val PREP_COLLECT_START_VELOCITY = Pose(0.0, 0.0).T(color)
+    val PREP_COLLECT_END_VELOCITY = Pose(80.0, 0.0).T(color)
     val GOTO_SHOOT_VELOCITY = Pose(-175.0, 0.0).T(color)
 
     private val collectX = 125.0

TeamCode/src/main/kotlin/pioneer/hardware/Flywheel.kt

@@ -11,6 +11,7 @@ import pioneer.helpers.FileLogger
 import pioneer.helpers.PIDController
 import pioneer.helpers.Pose
 import kotlin.math.cos
+import kotlin.math.pow
 import kotlin.math.sin
 import kotlin.math.sqrt
 import kotlin.math.tan
@@ -88,29 +89,31 @@ class Flywheel(
 
         //TODO Double check AprilTag height
         val groundDistance = shootPose distanceTo target
-        return estimateVelocity(groundDistance, targetHeight)
+        return estimateVelocity(groundDistance, targetHeight, pose)
     }
 
-    fun estimateVelocity(targetDistance: Double, targetHeight: Double) : Double {
+    fun estimateVelocity(targetDistance: Double, targetHeight: Double, pose: Pose = Pose()) : Double {
         val heightDiff = targetHeight - Constants.Turret.HEIGHT
         //Real world v0 of the ball
         val v0 =
             (targetDistance) / (
                     cos(
                         Constants.Turret.THETA,
                     ) * sqrt((2.0 * (heightDiff - tan(Constants.Turret.THETA) * (targetDistance))) / (-980))
-                )
+                    )
         //Regression to convert real world velocity to flywheel speed
 //        val flywheelVelocity = 1.583 * v0 - 9.86811 // From 12/22 testing
 //        val flywheelVelocity = 1.64545 * v0 - 51.56276 // From 2/4 testing
-        val flywheelVelocity = 2.05204 * v0 - 290.74829 // From 2/9 testing
+//        val flywheelVelocity = 2.05204 * v0 - 290.74829 // From 2/9 testing (Old flywheel)
+        val flywheelVelocity = 4.43524 * v0 - 1109.14177 // From 2/15 Testing (Lighter flywheel)
+        //https://www.desmos.com/calculator/6jwkhfmibd
         //Adjust for velocity of the bot when moving
-//        val thetaToTarget = -(shootPose angleTo target)
-//        val newTargetVelocityX = sin(thetaToTarget) * flywheelVelocity - pose.vx
-//        val newTargetVelocityY = cos(thetaToTarget) * flywheelVelocity - pose.vy
-//        val newTargetVelocity = sqrt(newTargetVelocityX.pow(2) + newTargetVelocityY.pow(2))
+        val shootPose = pose + Pose(x = Constants.Turret.OFFSET * sin(-pose.theta), y = Constants.Turret.OFFSET * cos(-pose.theta)) + Pose(pose.vx * Constants.Turret.LAUNCH_TIME, pose.vy * Constants.Turret.LAUNCH_TIME)
+        val thetaToTarget = -(shootPose angleTo Pose(0.0, 0.0))
+        val newTargetVelocityX = sin(thetaToTarget) * flywheelVelocity - pose.vx * Constants.Flywheel.VELOCITY_COMPENSATION_FACTOR
+        val newTargetVelocityY = cos(thetaToTarget) * flywheelVelocity - pose.vy * Constants.Flywheel.VELOCITY_COMPENSATION_FACTOR
+        val newTargetVelocity = sqrt(newTargetVelocityX.pow(2) + newTargetVelocityY.pow(2))
 
-        val newTargetVelocity = flywheelVelocity
         return newTargetVelocity
     }
-}
+}

TeamCode/src/main/kotlin/pioneer/hardware/Intake.kt

@@ -19,7 +19,7 @@ class Intake(
             intake.power = value
         }
 
-    var defaultPower: Double = 1.0
+    var defaultPower: Double = 0.9
 
     override fun init() {
         intake =

TeamCode/src/main/kotlin/pioneer/hardware/spindexer/ArtifactIndexer.kt

@@ -3,6 +3,7 @@ package pioneer.hardware.spindexer
 import com.qualcomm.robotcore.util.ElapsedTime
 import pioneer.Constants
 import pioneer.decode.Artifact
+import pioneer.decode.Motif
 import pioneer.helpers.FileLogger
 
 class ArtifactIndexer {
@@ -29,14 +30,52 @@ class ArtifactIndexer {
         get() = artifacts.count { it != null }
 
     // --- Index Helpers --- //
-    fun nextOpenIntakeIndex(): Int? =
-        artifacts.indexOfFirst { it == null }.takeIf { it != -1 }
+    fun nextOpenIntakeIndex(currentIndex: Int): Int? = run {
+        for (i in 0..2) {
+            // -i for reversed direction
+            val index = Math.floorMod(currentIndex - i, 3)
+            if (artifacts[index] == null) return index
+        }
+        null
+    }
 
     fun findOuttakeIndex(target: Artifact? = null): Int? =
         target?.let {
             artifacts.indexOfFirst { a -> a == it }.takeIf { i -> i != -1 }
         } ?: artifacts.indexOfFirst { it != null }.takeIf { it != -1 }
 
+    fun motifStartIndex(motif: Motif?, currentIndex: Int): Int? {
+        // Find the best shift to match motif
+        if (motif == null || !motif.isValid()) return null
+
+        val pattern = motif.getPattern()
+
+        var bestShift: Int? = null
+        var bestScore = -1
+
+        // Try all 3 rotations
+        for (offset in 0 until 3) {
+            // -offset for reversed direction, -2 to account for shooting vs intake difference
+            val shift = Math.floorMod((currentIndex - offset - 2), 3)
+            var score = 0
+
+            // Score this rotation based on how many positions match the motif pattern
+            for (i in 0 until 3) {
+                val rotated = artifacts[(i + shift) % 3]
+                if (rotated == pattern[i]) {
+                    score++
+                }
+            }
+
+            if (score > bestScore) {
+                bestScore = score
+                bestShift = shift
+            }
+        }
+
+        return bestShift?.minus(2)
+    }
+
     // --- Intake Logic --- //
 
     /**
@@ -71,6 +110,7 @@ class ArtifactIndexer {
     private fun handleLoss() {
         if (!artifactWasSeenRecently) return
 
+        // FIXME: whatttt
         if (artifactLostTimer.milliseconds() == 0.0) {
             artifactLostTimer.reset()
         }