Change default unit in Chrono.kt to MILLISECONDS, transfer between opmodes and ClearData opmode.

Author: Braedeng0

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

@@ -61,9 +61,7 @@ class Bot private constructor(
     }
 
     fun updateAll(dt: Double) {
-        hardwareComponents.values.forEach { it.update() }
-        pinpoint?.update(dt)
-        // TODO: Add other update methods (ie. localizer, follower)
+        hardwareComponents.values.forEach { it.update(dt) }
     }
 
     // Companion for builder and fromType

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

@@ -123,22 +123,23 @@ object Constants {
         /** The maximum angular acceleration in rad per second squared. */
         const val MAX_ANGULAR_ACCELERATION = 10.0
 
+        // --- Standard Follower PID Tuned to Stay on the Path ---
         // X-axis PID coefficients for the trajectory follower
-        @JvmField var X_KP = 7.5
+        @JvmField var X_KP = 5.0 // was 7.0
         @JvmField var X_KI = 0.0
         @JvmField var X_KD = 0.0
 
         // Y-axis PID coefficients for the trajectory follower
-        @JvmField var Y_KP = 7.5
+        @JvmField var Y_KP = 5.0 // was 7.0
         @JvmField var Y_KI = 0.0
         @JvmField var Y_KD = 0.0
 
         // Theta PID coefficients for heading interpolation
-        @JvmField var THETA_KP = 5.0
+        @JvmField var THETA_KP = 3.0 // was 5.0
         @JvmField var THETA_KI = 0.0
         @JvmField var THETA_KD = 0.0
 
-        // Position PID coefficients for final pose correction
+        // --- Position PID coefficients tuned for final pose correction ---
         @JvmField var POS_X_KP = 0.0
         @JvmField var POS_X_KI = 0.0
         @JvmField var POS_X_KD = 0.0
@@ -208,8 +209,24 @@ object Constants {
     }
 
     object TransferData {
+        fun reset() {
+            allianceColor = AllianceColor.NEUTRAL
+            pose = Pose()
+            turretMotorTicks = 0
+            spindexerMotorTicks = 0
+        }
+
         var allianceColor = AllianceColor.NEUTRAL
-        var turretPositionTicks = 0
-        var spindexerPositionTicks = 0
+        var pose = Pose()
+        var turretMotorTicks = 0
+        var spindexerMotorTicks = 0
+    }
+
+    @Config
+    object Flywheel {
+        @JvmField var KP = 0.0075
+        @JvmField var KI = 0.0
+        @JvmField var KD = 0.0
+        @JvmField var KF = 0.000415
     }
 }

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

@@ -45,13 +45,13 @@ enum class GoalTag(
     val height: Double
         get() = position[2].toDouble()
 
-    val shootingHeight = height + 30.0
+    val shootingHeight = height + 50.0
 
     val shootingPose: Pose
         get() =
             when (this) {
-                BLUE -> this.pose + Pose(x = -32.0, y = 30.0) // -X +Y
-                RED -> this.pose + Pose(x = 32.0, y = 30.0) // +X +Y
+                BLUE -> this.pose + Pose(x = -37.5, y = 32.5) // -X +Y
+                RED -> this.pose + Pose(x = 37.5, y = 32.5) // +X +Y
             }
 }
 

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

@@ -4,14 +4,11 @@ import com.qualcomm.robotcore.hardware.DcMotor
 import com.qualcomm.robotcore.hardware.DcMotorEx
 import com.qualcomm.robotcore.hardware.DcMotorSimple
 import com.qualcomm.robotcore.hardware.HardwareMap
-import com.qualcomm.robotcore.hardware.PIDFCoefficients
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit
 import pioneer.Constants
-import pioneer.decode.GoalTag
-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
@@ -21,14 +18,21 @@ class Flywheel(
     private val motorName: String = Constants.HardwareNames.FLYWHEEL,
 ) : HardwareComponent {
     private lateinit var flywheel: DcMotorEx
+    private val motorPID = PIDController(
+        Constants.Flywheel.KP,
+        Constants.Flywheel.KI,
+        Constants.Flywheel.KD,
+    )
 
     val motor: DcMotorEx
         get() = flywheel
 
+    var targetVelocity = 0.0
+
     var velocity
         get() = flywheel.velocity
         set(value) {
-            flywheel.velocity = value
+            targetVelocity = value
         }
 
     val current
@@ -42,16 +46,16 @@ class Flywheel(
                 zeroPowerBehavior = DcMotor.ZeroPowerBehavior.FLOAT
                 direction = DcMotorSimple.Direction.FORWARD
             }
-        FileLogger.info(name, flywheel.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER).toString())
-        flywheel.setPIDFCoefficients(
-            DcMotor.RunMode.RUN_USING_ENCODER,
-            PIDFCoefficients(
-                50.0,
-                3.0,
-                0.0,
-                0.0,
-            ),
-        )
+    }
+
+    override fun update(dt: Double) {
+        if (targetVelocity == 0.0) {
+            flywheel.power = 0.0
+            return
+        }
+        val correction = motorPID.update(targetVelocity - velocity, dt)
+        flywheel.power = Constants.Flywheel.KF * targetVelocity + correction
+//        FileLogger.debug("Flywheel","Set Power: ${Constants.Flywheel.KF * targetVelocity + correction}")
     }
 
     // https://www.desmos.com/calculator/uofqeqqyn1

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

@@ -6,5 +6,5 @@ interface HardwareComponent {
 
     fun init()
 
-    fun update() { }
+    fun update(dt: Double) {}
 }

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

@@ -36,7 +36,7 @@ class LaunchServos(
         timer.reset()
     }
 
-    override fun update() {
+    override fun update(dt: Double) {
         if (timer.milliseconds() < 250) {
             if (launch) {
                 setPower(1.0)

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

@@ -6,12 +6,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap
 import com.qualcomm.robotcore.util.ElapsedTime
 import pioneer.Constants
 import pioneer.decode.Artifact
-import pioneer.helpers.Chrono
+import pioneer.helpers.FileLogger
 import pioneer.helpers.PIDController
 import kotlin.math.PI
 import kotlin.math.abs
 import kotlin.math.sign
-import kotlin.time.DurationUnit
 
 /*
 Positive ROT: CW due to under-mounted motor
@@ -79,7 +78,7 @@ class Spindexer(
 
             for (position in MotorPosition.entries) {
                 val targetTicks = (position.radians * ticksPerRadian).toInt()
-                val error = wrapTicks(targetTicks - currentMotorPosition).toDouble()
+                val error = wrapTicks(targetTicks - currentMotorTicks).toDouble()
                 if (abs(error) < abs(smallestError)) {
                     smallestError = error
                     closestPosition = position
@@ -94,7 +93,7 @@ class Spindexer(
             // Check if motor position matches any outtake position
             for (position in outtakePositions) {
                 val targetTicks = (position.radians * ticksPerRadian).toInt()
-                val error = wrapTicks(targetTicks - currentMotorPosition)
+                val error = wrapTicks(targetTicks - currentMotorTicks)
                 if (abs(error) < 300.0) {
                     return true
                 }
@@ -115,7 +114,7 @@ class Spindexer(
     val reachedTarget: Boolean
         get() {
             // Compute circular error
-            val error = wrapTicks(targetMotorPosition - currentMotorPosition)
+            val error = wrapTicks(targetMotorTicks - currentMotorTicks)
             return abs(error) < Constants.Spindexer.POSITION_TOLERANCE_TICKS && (motor.velocity < Constants.Spindexer.VELOCITY_TOLERANCE_TPS)
         }
 
@@ -130,21 +129,28 @@ class Spindexer(
         get() = _artifacts.count { it != null }
 
     // Motor position accessors
-    val currentMotorPosition: Int
-        get() = motor.currentPosition - offsetTicks
+    private val rawMotorTicks: Int
+        get() {
+            check(::motor.isInitialized)
+            return motor.currentPosition
+        }
+
+    val currentMotorTicks: Int
+        get() = rawMotorTicks + offsetTicks
 
-    val targetMotorPosition: Int
+    val targetMotorTicks: Int
         get() = (motorState.radians * ticksPerRadian).toInt()
 
     val currentScannedArtifact: Artifact?
         get() = scanArtifact()
 
     var checkingForNewArtifacts = true
 
+    var manualMove = false
+
     // Private variables
     private var offsetTicks = 0
     private var lastPower = 0.0
-    private val chrono = Chrono(autoUpdate = false, units = DurationUnit.MILLISECONDS)
     private val ticksPerRadian: Int = (Constants.Spindexer.TICKS_PER_REV / (2 * PI)).toInt()
     private val motorPID =
         PIDController(
@@ -157,7 +163,6 @@ class Spindexer(
     private var artifactSeen = false
     private var artifactWasSeenRecently = false
     private var lastStoredIndex = 0
-    private var manualMove = false
     private val intakePositions =
         listOf(MotorPosition.INTAKE_1, MotorPosition.INTAKE_2, MotorPosition.INTAKE_3)
     private val outtakePositions =
@@ -199,9 +204,8 @@ class Spindexer(
      * Updates the motor position to match the desired motor state.
      * Checks for new artifacts if in an intake position.
      */
-    override fun update() {
-        chrono.update()
-        runMotorToState()
+    override fun update(dt: Double) {
+        runMotorToState(dt)
         if (checkingForNewArtifacts) checkForArtifact()
     }
 
@@ -261,6 +265,7 @@ class Spindexer(
         _artifacts[index] = null
         // Automatically move to intake if empty
         if (isEmpty) moveToNextOpenIntake()
+        FileLogger.info("Spindexer", "Removed $artifact")
         return artifact
     }
 
@@ -314,17 +319,17 @@ class Spindexer(
         return lastPower
     }
 
-    private fun runMotorToState() {
+    private fun runMotorToState(dt: Double) {
         if (manualMove) return
 
-        val rawError = targetMotorPosition - currentMotorPosition
+        val rawError = targetMotorTicks - currentMotorTicks
         val error = wrapTicks(rawError)
 
         // PID
-        var power = motorPID.update(error.toDouble(), chrono.dt)
+        var power = motorPID.update(error.toDouble(), dt)
 
         // Ramp power to prevent sudden acceleration
-        power = rampPower(power, chrono.dt)
+        power = rampPower(power, dt)
 
         // Static constant
         val adjustedKS = Constants.Spindexer.KS_START + Constants.Spindexer.KS_STEP * numStoredArtifacts
@@ -418,6 +423,9 @@ class Spindexer(
             if (artifactVisibleTimer.milliseconds() >= Constants.Spindexer.CONFIRM_INTAKE_MS) {
                 storeArtifact(artifact)
 
+                // Log stored artifact
+                FileLogger.info("Spindexer", "Stored $artifact")
+
                 // Reset state
                 artifactSeen = false
                 artifactWasSeenRecently = false

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

@@ -34,7 +34,13 @@ class Turret(
     val currentTicks: Int
         get() {
             check(::turret.isInitialized)
-            return turret.currentPosition - offsetTicks
+            return turret.currentPosition + offsetTicks
+        }
+
+    val rawTicks: Int
+        get() {
+            check(::turret.isInitialized)
+            return turret.currentPosition
         }
 
     val reachedTarget: Boolean
@@ -61,11 +67,12 @@ class Turret(
         get() = currentTicks / ticksPerRadian
 
     val targetAngle: Double
-        get() = turret.targetPosition / ticksPerRadian
+        get() = (turret.targetPosition + offsetTicks) / ticksPerRadian
 
     fun resetMotorPosition(resetTicks: Int = 0) {
         turret.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
         offsetTicks = resetTicks
+        turret.mode = DcMotor.RunMode.RUN_USING_ENCODER
     }
 
     fun gotoAngle(
@@ -84,13 +91,14 @@ class Turret(
             desiredAngle = MathUtils.normalizeRadians(angle, motorRange)
         }
 
-        val currentAngle = currentTicks / ticksPerRadian
-
-        val delta = desiredAngle - currentAngle
-        val targetTicks = (currentTicks + delta * ticksPerRadian).toInt()
+        // Logical ticks uses offset
+        val logicalCurrentTicks = currentTicks
+        val logicalTargetTicks =
+            (desiredAngle * ticksPerRadian).toInt()
+        val rawTargetTicks = logicalTargetTicks - offsetTicks
 
         with(turret) {
-            targetPosition = targetTicks
+            targetPosition = rawTargetTicks
             mode = DcMotor.RunMode.RUN_TO_POSITION
             this.power = power
         }
@@ -102,7 +110,7 @@ class Turret(
     ) {
         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)
-        // General Angle(From robot 0 to target):
+        // General Angle (From robot 0 to target):
         val targetTheta = (shootPose angleTo target)
         val turretTheta = (PI / 2 + targetTheta) - shootPose.theta
         gotoAngle(MathUtils.normalizeRadians(turretTheta), 0.85)

TeamCode/src/main/kotlin/pioneer/helpers/Chrono.kt

@@ -14,7 +14,7 @@ import kotlin.time.TimeSource
 class Chrono(
     private val autoUpdate: Boolean = true,
     private val source: TimeSource = TimeSource.Monotonic,
-    private val units: DurationUnit = DurationUnit.SECONDS,
+    private val units: DurationUnit = DurationUnit.MILLISECONDS,
 ) {
     private var last = source.markNow()
 

TeamCode/src/main/kotlin/pioneer/localization/Localizer.kt

@@ -17,7 +17,7 @@ interface Localizer : HardwareComponent {
      * Updates the pose of the robot based on sensor data
      * @param dt The time since the last update in seconds
      */
-    fun update(dt: Double)
+    override fun update(dt: Double)
 
     /**
      * Resets the localizer to a specific pose