Merge pull request #37 from Pioneer-Robotics/bot-refactor

Bot refactor

Author: milopeters

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

@@ -1,82 +1,89 @@
 package pioneer
 
 import com.qualcomm.robotcore.hardware.HardwareMap
-import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor
-import pioneer.constants.Drive
-import pioneer.hardware.AprilTag
 import pioneer.hardware.BatteryMonitor
 import pioneer.hardware.Camera
 import pioneer.hardware.Flywheel
+import pioneer.hardware.HardwareComponent
 import pioneer.hardware.LaunchServos
 import pioneer.hardware.MecanumBase
-import pioneer.hardware.MockHardware
-import pioneer.localization.Localizer
-import pioneer.localization.localizers.LocalizerMock
 import pioneer.localization.localizers.Pinpoint
 import pioneer.pathing.follower.Follower
-import pioneer.constants.Camera as CameraConstants
 
-enum class BotType(
-    val supportsLocalizer: Boolean,
-) {
-    BASIC_MECANUM_BOT(false),
-    MECANUM_BOT(true),
-    GOBILDA_STARTER_BOT(true),
+enum class BotType {
+    MECANUM_BOT,
+    GOBILDA_STARTER_BOT,
+    CUSTOM,
 }
 
-class Bot(
-    val botType: BotType,
-    hardwareMap: HardwareMap,
+class Bot private constructor(
+    val type: BotType,
+    private val hardwareComponents: Map<Class<out HardwareComponent>, HardwareComponent>,
 ) {
-    // Basic hardware components
-    var mecanumBase: MecanumBase = MecanumBase(MockHardware())
-    var localizer: Localizer = LocalizerMock()
-    var batteryMonitor: BatteryMonitor = BatteryMonitor(MockHardware())
+    // Type-safe access
+    internal inline fun <reified T : HardwareComponent> get(): T? = hardwareComponents[T::class.java] as? T
 
-    // GoBilda starter bot specific components
-    var flywheel: Flywheel = Flywheel(MockHardware())
-    var launchServos: LaunchServos = LaunchServos(MockHardware())
+    // Check if a component is present
+    internal inline fun <reified T : HardwareComponent> has(): Boolean = hardwareComponents.containsKey(T::class.java)
 
-    // Other hardware components
-    var aprilTagProcessor: AprilTagProcessor = AprilTag().processor
-    var camera: Camera = Camera(MockHardware())
+    fun initAll() {
+        hardwareComponents.values.forEach { it.init() }
+    }
 
-    // Path follower
-    var follower = Follower(this)
+    // Property-style access for known components
+    val mecanumBase get() = get<MecanumBase>()
+    val pinpoint get() = get<Pinpoint>()
+    val launchServos get() = get<LaunchServos>()
+    val flywheel get() = get<Flywheel>()
+    val camera get() = get<Camera>()
+    val batteryMonitor get() = get<BatteryMonitor>()
 
-    init {
-        when (botType) {
-            BotType.BASIC_MECANUM_BOT -> {
-                // Initialize hardware components for Basic Mecanum Bot
-                mecanumBase = MecanumBase(hardwareMap)
-                batteryMonitor = BatteryMonitor(hardwareMap)
-            }
+    // Follower is lazily initialized (only if accessed)
+    // and will error if localizer or mecanumBase is missing
+    val follower: Follower by lazy {
+        Follower(
+            localizer = get<Pinpoint>()!!,
+            mecanumBase = get<MecanumBase>()!!,
+        )
+    }
 
-            BotType.MECANUM_BOT -> {
-                // Initialize hardware components for Basic Mecanum Bot
-                mecanumBase = MecanumBase(hardwareMap, Drive.MOTOR_CONFIG)
-                localizer = Pinpoint(hardwareMap)
-                batteryMonitor = BatteryMonitor(hardwareMap)
-            }
+    // Companion for builder and fromType
+    companion object {
+        fun builder() = Builder()
 
-            BotType.GOBILDA_STARTER_BOT -> {
-                // Initialize hardware components for GoBilda Starter Bot
-                mecanumBase = MecanumBase(hardwareMap)
-                localizer = Pinpoint(hardwareMap)
-                batteryMonitor = BatteryMonitor(hardwareMap)
-                flywheel = Flywheel(hardwareMap)
-                launchServos = LaunchServos(hardwareMap)
-                aprilTagProcessor =
-                    AprilTag(
-                        CameraConstants.POSITION_CM,
-                        CameraConstants.ORIENTATION_RAD,
-                    ).processor
-                camera =
-                    Camera(
-                        hardwareMap,
-                        processors = arrayOf(aprilTagProcessor),
-                    )
+        fun fromType(
+            type: BotType,
+            hardwareMap: HardwareMap,
+        ): Bot =
+            when (type) {
+                BotType.MECANUM_BOT ->
+                    builder()
+                        .add(MecanumBase(hardwareMap))
+                        .add(Pinpoint(hardwareMap))
+                        .add(BatteryMonitor(hardwareMap))
+                        .build()
+                BotType.GOBILDA_STARTER_BOT ->
+                    builder()
+                        .add(MecanumBase(hardwareMap))
+                        .add(Pinpoint(hardwareMap))
+                        .add(LaunchServos(hardwareMap))
+                        .add(Flywheel(hardwareMap))
+                        .add(Camera(hardwareMap, processors = arrayOf(Camera.createAprilTagProcessor())))
+                        .add(BatteryMonitor(hardwareMap))
+                        .build()
+                BotType.CUSTOM -> throw IllegalArgumentException("Use Bot.builder() to create a custom bot")
             }
+    }
+
+    class Builder {
+        private val components = mutableMapOf<Class<out HardwareComponent>, HardwareComponent>()
+
+        fun <T : HardwareComponent> add(component: T): Builder {
+            // TODO: Possibly allow interfaces such as Localizer vs Pinpoint
+            components[component::class.java] = component
+            return this
         }
+
+        fun build(): Bot = Bot(BotType.CUSTOM, components.toMap())
     }
 }

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

@@ -8,56 +8,60 @@ import com.qualcomm.robotcore.hardware.VoltageSensor
  * Provides battery voltage information for power management and telemetry.
  */
 class BatteryMonitor(
-    hardwareMap: HardwareMap,
-) {
-    private val voltageSensors: List<VoltageSensor> = hardwareMap.voltageSensor.toList()
+    private val hardwareMap: HardwareMap,
+) : HardwareComponent {
+    override val name = "BatteryMonitor"
+
+    private lateinit var voltageSensors: List<VoltageSensor>
+
+    override fun init() {
+        voltageSensors = hardwareMap.voltageSensor.toList()
+    }
 
     /**
-     * Gets the lowest voltage reading from all available sensors.
-     * @return Minimum voltage in volts, or 0.0 if no valid readings
+     * Minimum voltage in volts, or 0.0 if no valid readings
      */
-    fun getVoltage(): Double =
-        voltageSensors
-            .mapNotNull { sensor -> sensor.voltage.takeIf { it > 0.0 } }
-            .minOrNull() ?: 0.0
+    val voltage: Double
+        get() {
+            return voltageSensors
+                .mapNotNull { sensor -> sensor.voltage.takeIf { it > 0.0 } }
+                .minOrNull() ?: 0.0
+        }
 
     /**
-     * Gets the highest voltage reading from all available sensors.
-     * @return Maximum voltage in volts, or 0.0 if no valid readings
+     * Maximum voltage in volts, or 0.0 if no valid readings
      */
-    fun getMaxVoltage(): Double =
-        voltageSensors
-            .mapNotNull { sensor -> sensor.voltage.takeIf { it > 0.0 } }
-            .maxOrNull() ?: 0.0
+    val maxVoltage: Double
+        get() {
+            return voltageSensors
+                .mapNotNull { sensor -> sensor.voltage.takeIf { it > 0.0 } }
+                .maxOrNull() ?: 0.0
+        }
 
     /**
-     * Gets average voltage from all valid sensors.
-     * @return Average voltage in volts, or 0.0 if no valid readings
+     * Average voltage in volts, or 0.0 if no valid readings
      */
-    fun getAverageVoltage(): Double {
-        val validVoltages =
-            voltageSensors
-                .mapNotNull { sensor -> sensor.voltage.takeIf { it > 0.0 } }
+    val averageVoltage: Double
+        get() {
+            val validVoltages =
+                voltageSensors
+                    .mapNotNull { sensor -> sensor.voltage.takeIf { it > 0.0 } }
+            return if (validVoltages.isNotEmpty()) validVoltages.average() else 0.0
+        }
 
-        return if (validVoltages.isNotEmpty()) {
-            validVoltages.average()
-        } else {
-            0.0
+    /**
+     * List of valid voltage readings
+     */
+    val allVoltages: List<Double>
+        get() {
+            return voltageSensors
+                .mapNotNull { sensor -> sensor.voltage.takeIf { it > 0.0 } }
         }
-    }
 
     /**
      * Checks if battery voltage is critically low.
      * @param threshold Minimum acceptable voltage (default: 11.0V)
      * @return True if voltage is below threshold
      */
-    fun isVoltageLow(threshold: Double): Boolean = getVoltage() < threshold
-
-    /**
-     * Gets all valid voltage readings for debugging.
-     * @return List of valid voltage readings
-     */
-    fun getAllVoltages(): List<Double> =
-        voltageSensors
-            .mapNotNull { sensor -> sensor.voltage.takeIf { it > 0.0 } }
+    fun isVoltageLow(threshold: Double): Boolean = voltage < threshold
 }

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

@@ -2,23 +2,58 @@ package pioneer.hardware
 
 import com.qualcomm.robotcore.hardware.HardwareMap
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit
+import org.firstinspires.ftc.robotcore.external.navigation.Position
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles
 import org.firstinspires.ftc.vision.VisionPortal
 import org.firstinspires.ftc.vision.VisionProcessor
+import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase
+import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor
 import pioneer.constants.HardwareNames
 import kotlin.jvm.java
 
 class Camera(
-    hardwareMap: HardwareMap,
-    name: String = HardwareNames.WEBCAM,
-    processors: Array<VisionProcessor> = emptyArray(),
-) {
-    val portal: VisionPortal =
-        VisionPortal
-            .Builder()
-            .setCamera(hardwareMap.get(WebcamName::class.java, name))
-            .apply {
-                if (processors.isNotEmpty()) {
-                    addProcessors(*processors)
-                }
-            }.build()
+    private val hardwareMap: HardwareMap,
+    private val cameraName: String = HardwareNames.WEBCAM,
+    private val processors: Array<VisionProcessor> = emptyArray(),
+) : HardwareComponent {
+    override val name = "Camera"
+
+    private lateinit var portal: VisionPortal
+
+    override fun init() {
+        portal =
+            VisionPortal
+                .Builder()
+                .setCamera(hardwareMap.get(WebcamName::class.java, cameraName))
+                .apply {
+                    if (processors.isNotEmpty()) {
+                        addProcessors(*processors)
+                    }
+                }.build()
+    }
+
+    companion object {
+        fun createAprilTagProcessor(
+            position: Position = Position(DistanceUnit.CM, 0.0, 0.0, 0.0, 0),
+            orientation: YawPitchRollAngles = YawPitchRollAngles(AngleUnit.RADIANS, 0.0, 0.0, 0.0, 0),
+        ): VisionProcessor {
+            val library: AprilTagLibrary =
+                AprilTagLibrary
+                    .Builder()
+                    .addTags(AprilTagGameDatabase.getCurrentGameTagLibrary())
+                    .build()
+
+            val processor: AprilTagProcessor =
+                AprilTagProcessor
+                    .Builder()
+                    .setTagLibrary(library)
+                    .setCameraPose(position, orientation)
+                    .build()
+
+            return processor
+        }
+    }
 }

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

@@ -7,22 +7,24 @@ import com.qualcomm.robotcore.hardware.HardwareMap
 import pioneer.constants.HardwareNames
 
 class Flywheel(
-    hardwareMap: HardwareMap,
-    name: String = HardwareNames.FLYWHEEL,
-) {
-    private val flywheel: DcMotorEx = hardwareMap.get(DcMotorEx::class.java, name)
+    private val hardwareMap: HardwareMap,
+    private val motorName: String = HardwareNames.FLYWHEEL,
+) : HardwareComponent {
+    override val name = "Flywheel"
 
-    val velocity
-        get() = flywheel.getVelocity()
+    private lateinit var flywheel: DcMotorEx
 
-    init {
+    var velocity
+        get() = flywheel.velocity
+        set(value) {
+            flywheel.velocity = value
+        }
+
+    override fun init() {
+        flywheel = hardwareMap.get(DcMotorEx::class.java, motorName)
         flywheel.mode = DcMotor.RunMode.STOP_AND_RESET_ENCODER
         flywheel.mode = DcMotor.RunMode.RUN_USING_ENCODER
         flywheel.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.FLOAT
         flywheel.direction = DcMotorSimple.Direction.FORWARD
     }
-
-    fun setSpeed(velocity: Double) {
-        flywheel.setVelocity(velocity)
-    }
 }

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

@@ -0,0 +1,7 @@
+package pioneer.hardware
+
+interface HardwareComponent {
+    val name: String
+
+    fun init()
+}