Fix dt issues; each hardware component now keeps track of its own dt.

Author: Braedeng0

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

@@ -60,8 +60,8 @@ class Bot private constructor(
         )
     }
 
-    fun updateAll(dt: Double) {
-        hardwareComponents.values.forEach { it.update(dt) }
+    fun updateAll() {
+        hardwareComponents.values.forEach { it.update() }
     }
 
     // Companion for builder and fromType

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

@@ -194,7 +194,8 @@ object Constants {
 
         @JvmField var MAX_POWER_RATE = 50.0
 
-        const val POSITION_TOLERANCE_TICKS = 40
+        const val POSITION_TOLERANCE_TICKS = 50
+        const val PID_TOLERANCE_TICKS = 20
         const val VELOCITY_TOLERANCE_TPS = 10
         const val TICKS_PER_REV = 8192
 
@@ -215,8 +216,8 @@ object Constants {
     }
 
     object ServoPositions {
-        const val LAUNCHER_REST = 0.065
-        const val LAUNCHER_TRIGGERED = 0.315
+        const val LAUNCHER_REST = 0.3
+        const val LAUNCHER_TRIGGERED = 0.7
     }
 
     object TransferData {

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

@@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple
 import com.qualcomm.robotcore.hardware.HardwareMap
 import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit
 import pioneer.Constants
+import pioneer.helpers.Chrono
 import pioneer.helpers.PIDController
 import pioneer.helpers.Pose
 import kotlin.math.cos
@@ -24,6 +25,8 @@ class Flywheel(
         Constants.Flywheel.KD,
     )
 
+    private val chrono = Chrono()
+
     val motor: DcMotorEx
         get() = flywheel
 
@@ -48,12 +51,12 @@ class Flywheel(
             }
     }
 
-    override fun update(dt: Double) {
+    override fun update() {
         if (targetVelocity == 0.0) {
             flywheel.power = 0.0
             return
         }
-        val correction = motorPID.update(targetVelocity - velocity, dt)
+        val correction = motorPID.update(targetVelocity - velocity, chrono.dt)
         flywheel.power = Constants.Flywheel.KF * targetVelocity + correction
 //        FileLogger.debug("Flywheel","Set Power: ${Constants.Flywheel.KF * targetVelocity + correction}")
     }

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

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

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

@@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotorEx
 import com.qualcomm.robotcore.hardware.DcMotorSimple
 import com.qualcomm.robotcore.hardware.HardwareMap
 import pioneer.Constants
+import pioneer.helpers.FileLogger
 
 class Intake(
     private val hardwareMap: HardwareMap,

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

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

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

@@ -6,6 +6,7 @@ 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
@@ -114,8 +115,7 @@ class Spindexer(
     val reachedTarget: Boolean
         get() {
             // Compute circular error
-            val error = wrapTicks(targetMotorTicks - currentMotorTicks)
-            return abs(error) < Constants.Spindexer.POSITION_TOLERANCE_TICKS && (motor.velocity < Constants.Spindexer.VELOCITY_TOLERANCE_TPS)
+            return abs(motorTicksError) < Constants.Spindexer.POSITION_TOLERANCE_TICKS && (motor.velocity < Constants.Spindexer.VELOCITY_TOLERANCE_TPS)
         }
 
     // Getters for artifact storage status
@@ -144,6 +144,9 @@ class Spindexer(
     val targetMotorTicks: Int
         get() = (motorState.radians * ticksPerRadian).toInt()
 
+    val motorTicksError: Int
+        get() = wrapTicks(targetMotorTicks - currentMotorTicks)
+
     val currentScannedArtifact: Artifact?
         get() = scanArtifact()
 
@@ -172,6 +175,7 @@ class Spindexer(
         listOf(MotorPosition.OUTTAKE_1, MotorPosition.OUTTAKE_2, MotorPosition.OUTTAKE_3)
     private lateinit var motor: DcMotorEx
     private lateinit var intakeSensor: RevColorSensor
+    private val chrono = Chrono(false)
 
     /**
      * Returns the index of the current motor position in the intake/outtake lists.
@@ -207,8 +211,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(dt: Double) {
-        runMotorToState(dt)
+    override fun update() {
+        runMotorToState()
         if (checkingForNewArtifacts) checkForArtifact()
     }
 
@@ -322,21 +326,21 @@ class Spindexer(
         return lastPower
     }
 
-    private fun runMotorToState(dt: Double) {
+    private fun runMotorToState() {
         if (manualMove) return
 
-        val rawError = targetMotorTicks - currentMotorTicks
-        val error = wrapTicks(rawError)
-
         // PID
-        var power = motorPID.update(error.toDouble(), dt)
+        chrono.update()
+        var power = motorPID.update(motorTicksError.toDouble(), chrono.dt)
 
         // Ramp power to prevent sudden acceleration
-        power = rampPower(power, dt)
+        power = rampPower(power, chrono.dt)
 
         // Static constant
         val adjustedKS = Constants.Spindexer.KS_START + Constants.Spindexer.KS_STEP * numStoredArtifacts
-        power += if (abs(error) > 100) adjustedKS * sign(error.toDouble()) else 0.0
+        power += if (abs(motorTicksError) > 100) adjustedKS * sign(motorTicksError.toDouble()) else 0.0
+
+        if (abs(motorTicksError) < Constants.Spindexer.PID_TOLERANCE_TICKS) power = 0.0
 
         // Apply power
         val maxPower = 1.0

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

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

TeamCode/src/main/kotlin/pioneer/localization/localizers/Pinpoint.kt

@@ -7,6 +7,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit
 import org.firstinspires.ftc.robotcore.external.navigation.Pose2D
 import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit
 import pioneer.Constants
+import pioneer.helpers.Chrono
 import pioneer.helpers.Pose
 import pioneer.localization.Localizer
 
@@ -21,6 +22,8 @@ class Pinpoint(
 ) : Localizer {
     override val name = "PinpointLocalizer"
 
+    private val chrono = Chrono(false)
+
     override var pose: Pose = startPose
     override var prevPose: Pose = startPose.copy()
 
@@ -40,7 +43,7 @@ class Pinpoint(
         pinpoint.update()
     }
 
-    override fun update(dt: Double) {
+    override fun update() {
         pinpoint.update()
 
         // Coordinate conversion: robot_x = -pinpoint_y, robot_y = pinpoint_x
@@ -49,14 +52,16 @@ class Pinpoint(
         val vx = -pinpoint.getVelY(DistanceUnit.CM)
         val vy = pinpoint.getVelX(DistanceUnit.CM)
 
+        chrono.update() // Update before using dt
+
         // Numerical differentiation for acceleration
-        val ax = (vx - prevPose.vx) / dt
-        val ay = (vy - prevPose.vy) / dt
+        val ax = (vx - prevPose.vx) / chrono.dt
+        val ay = (vy - prevPose.vy) / chrono.dt
 
         // Angular motion with coordinate conversion
         val theta = pinpoint.getHeading(AngleUnit.RADIANS)
         val omega = pinpoint.getHeadingVelocity(UnnormalizedAngleUnit.RADIANS)
-        val alpha = (omega - prevPose.omega) / dt
+        val alpha = (omega - prevPose.omega) / chrono.dt
 
         prevPose = pose
         pose = Pose(x, y, vx, vy, ax, ay, theta, omega, alpha)

TeamCode/src/main/kotlin/pioneer/localization/localizers/ThreeWheelOdometry.kt

@@ -49,7 +49,7 @@ class ThreeWheelOdometry(
     override val encoderYTicks: Int
         get() = prevCenterTicks
 
-    override fun update(dt: Double) {
+    override fun update() {
         // Get current encoder values
         val dLeftCM = odoLeft.toCentimeters()
         val dRightCM = odoRight.toCentimeters()