feat!: mini bot migration

Co-authored-by: Vifi5 <117041326+Vif15@users.noreply.github.com>

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/Claw.kt

@@ -8,6 +8,7 @@ import org.firstinspires.ftc.teamcode.utils.opModeSleep
 /**
  * An API for controlling the claw.
  */
+@Deprecated(message = "Claw is deprecated, this is no claw on the robot.")
 object Claw : API() {
     private lateinit var gripL: Servo
     private lateinit var gripR: Servo

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/Drone.kt

@@ -1,32 +1,28 @@
 package org.firstinspires.ftc.teamcode.api
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode
-import com.qualcomm.robotcore.hardware.DcMotor
-import com.qualcomm.robotcore.hardware.DcMotorSimple
+import com.qualcomm.robotcore.hardware.Servo
+import org.firstinspires.ftc.teamcode.utils.RobotConfig
 
 /**
  * An API to control the drone launcher.
  */
 object Drone : API() {
-    private lateinit var droneLeft: DcMotor
-    private lateinit var droneRight: DcMotor
+    private lateinit var hook: Servo
 
     override fun init(opMode: OpMode) {
         super.init(opMode)
 
-        this.droneLeft = this.opMode.hardwareMap.get(DcMotor::class.java, "droneLeft")
-        this.droneRight = this.opMode.hardwareMap.get(DcMotor::class.java, "droneRight")
+        this.hook = opMode.hardwareMap.get(Servo::class.java, "droneHook")
 
-        this.droneRight.direction = DcMotorSimple.Direction.REVERSE
+        this.reset()
     }
 
-    fun spin() {
-        this.droneLeft.power = 1.0
-        this.droneRight.power = 1.0
+    fun release() {
+        this.hook.position = RobotConfig.Drone.OPEN_PIN
     }
 
-    fun stop() {
-        this.droneLeft.power = 0.0
-        this.droneRight.power = 0.0
+    fun reset() {
+        this.hook.position = RobotConfig.Drone.CLOSE_PIN
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/Hook.kt

@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.teamcode.api
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.hardware.DcMotor
+import com.qualcomm.robotcore.hardware.DcMotorSimple
+
+/** An API for controlling the hook, used to hang the robot. */
+object Hook : API() {
+    lateinit var hook: DcMotor
+        private set
+
+    override fun init(opMode: OpMode) {
+        super.init(opMode)
+
+        this.hook = this.opMode.hardwareMap.get(DcMotor::class.java, "hook")
+
+        hook.direction = DcMotorSimple.Direction.REVERSE
+        hook.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER
+        hook.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
+    }
+
+    fun moveHook(power: Double) {
+        // TODO: Add limits based on encoders
+        hook.power = power
+    }
+
+    fun stop() {
+        hook.power = 0.0
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/LinearSlide.kt

@@ -7,6 +7,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple
 /**
  * An API for controlling the linear slide(s).
  */
+@Deprecated(message = "Linear slide is deprecated, there is no slide on the robot.")
 object LinearSlide : API() {
     // Technically not the slides themselves, but instead a motor connected to both.
     // For abstraction's sake, it's fine to treat it as the slide itself.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/PixelPlacer.kt

@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.teamcode.api
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.hardware.Servo
+import org.firstinspires.ftc.teamcode.utils.RobotConfig
+
+object PixelPlacer : API() {
+    private lateinit var pixelPlacer: Servo
+
+    override fun init(opMode: OpMode) {
+        super.init(opMode)
+
+        this.pixelPlacer = this.opMode.hardwareMap.get(Servo::class.java, "pixelPlacer")
+
+        this.reset()
+    }
+
+    fun place() {
+        pixelPlacer.position = RobotConfig.PixelPlacer.PLACE_POSITION
+    }
+
+    fun reset() {
+        pixelPlacer.position = RobotConfig.PixelPlacer.DEFAULT_POSITION
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/StatusLed.kt

@@ -8,6 +8,7 @@ import com.qualcomm.robotcore.hardware.Blinker
 /**
  * An API for manipulating the light on the control and expansion hubs.
  */
+@Deprecated(message = "StatusLed is not supported.")
 object StatusLed : API() {
     private lateinit var blinkers: List<Blinker>
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/linear/AprilMovement.kt

@@ -42,7 +42,10 @@ object AprilMovement : API() {
             (
                 abs(rangeError) > RobotConfig.AprilMovement.RANGE_ERROR ||
                     abs(headingError) > RobotConfig.AprilMovement.HEADING_ERROR ||
-                    abs(strafeError) > RobotConfig.AprilMovement.STRAFE_ERROR
+                    abs(strafeError) > RobotConfig.AprilMovement.STRAFE_ERROR ||
+                    abs(TriWheels.red.power) > 0.03 ||
+                    abs(TriWheels.green.power) > 0.03 ||
+                    abs(TriWheels.blue.power) > 0.03
             )
         ) {
             // Try scanning for the april tag

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/linear/Encoders.kt

@@ -7,6 +7,7 @@ import org.firstinspires.ftc.teamcode.api.API
 import org.firstinspires.ftc.teamcode.api.TriWheels
 import org.firstinspires.ftc.teamcode.api.linear.Encoders.driveTo
 import org.firstinspires.ftc.teamcode.api.linear.Encoders.spinTo
+import org.firstinspires.ftc.teamcode.utils.MotorController
 import org.firstinspires.ftc.teamcode.utils.MotorControllerGroup
 import org.firstinspires.ftc.teamcode.utils.RobotConfig
 import kotlin.math.abs
@@ -179,27 +180,31 @@ object Encoders : API() {
     ) {
         TriWheels.stopAndResetMotors()
 
-        val (left, right, _) = this.defineWheels(direction)
+        val (left, right, back) = this.defineWheels(direction)
         val ticks = this.inchesToTick(inches)
 
         val controllers = MotorControllerGroup(ticks, left, right)
+        val backController = MotorController(0, back)
 
         try {
             right.direction = DcMotorSimple.Direction.REVERSE
 
-            while (!controllers.isDone() && linearOpMode.opModeIsActive()) {
+            while (!(controllers.isDone() && backController.isDone()) && linearOpMode.opModeIsActive()) {
                 controllers.update()
+                backController.update()
 
                 with(linearOpMode.telemetry) {
                     addData("Status", "Encoder Driving")
 
                     addData("Left Power", left.power)
                     addData("Right Power", right.power)
+                    addData("Back Power", back.power)
 
                     addData("Target", ticks)
 
-                    addData("Left Current", left.currentPosition)
-                    addData("Right Current", right.currentPosition)
+                    addData("Left Pos", left.currentPosition)
+                    addData("Right Pos", right.currentPosition)
+                    addData("Back Pos", back.currentPosition)
 
                     update()
                 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/MoreJankAuto.kt

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.opmode.autonomous
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.api.Telemetry
+import org.firstinspires.ftc.teamcode.api.TriWheels
+import org.firstinspires.ftc.teamcode.api.linear.Encoders
+import org.firstinspires.ftc.teamcode.utils.RobotConfig
+
+@Autonomous(name = "Most Jank Auto")
+class MoreJankAuto : LinearOpMode() {
+    private val forward =
+        when (RobotConfig.model) {
+            RobotConfig.Model.RobotA -> Encoders.Direction.Green
+            RobotConfig.Model.RobotB -> Encoders.Direction.Red
+        }
+
+    override fun runOpMode() {
+        Telemetry.init(this)
+        TriWheels.init(this)
+        Encoders.init(this)
+
+        Telemetry.sayInitialized()
+
+        waitForStart()
+
+        Telemetry.sayStarted()
+
+        Encoders.driveTo(
+            forward,
+            42.0,
+        )
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/TeleOpMain.kt

@@ -2,11 +2,8 @@ package org.firstinspires.ftc.teamcode.opmode.teleop
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
-import com.qualcomm.robotcore.hardware.DcMotor
-import org.firstinspires.ftc.teamcode.api.Claw
 import org.firstinspires.ftc.teamcode.api.Drone
-import org.firstinspires.ftc.teamcode.api.GamepadEx
-import org.firstinspires.ftc.teamcode.api.LinearSlide
+import org.firstinspires.ftc.teamcode.api.Hook
 import org.firstinspires.ftc.teamcode.api.Telemetry
 import org.firstinspires.ftc.teamcode.api.TriWheels
 import org.firstinspires.ftc.teamcode.utils.RobotConfig
@@ -16,36 +13,19 @@ import kotlin.math.sqrt
 
 @TeleOp(name = "TeleOpMain")
 class TeleOpMain : OpMode() {
-    // Whether the linear slide should remain locked in place or not.
-    private var slideLocked = false
+    private var hanging = false
 
-    // init will run once
     override fun init() {
-        // Setup special telemetry
         Telemetry.init(this)
-
-        // Triangle wheel controls
         TriWheels.init(this)
-
-        LinearSlide.init(this)
-
-        // Advanced gamepad inputs
-        GamepadEx.init(this)
-
-        // Claw controls
-        Claw.init(this)
-
-        // Drone controls
+        Hook.init(this)
         Drone.init(this)
+        // PixelPlacer.init(this)
 
         // Log that we are initialized
         Telemetry.sayInitialized()
-
-        // Claw controls
-        Claw.init(this)
     }
 
-    // loop will run repetitively overtime while the robot runs
     override fun loop() {
         // alias gamepad1
         val gamepad = this.gamepad1
@@ -56,60 +36,38 @@ class TeleOpMain : OpMode() {
 
         // angle and strength
         // PI / 3 because 0 radians is right, not forward
-        val joyRadians = atan2(joyY, joyX) - (PI / 3.0)
+        val joyRadians = atan2(joyY, joyX) - (PI / 3.0) - (2.0 * PI / 3.0)
 
         val joyMagnitude = sqrt(joyY * joyY + joyX * joyX)
 
         val rotationPower = -gamepad.right_stick_x.toDouble()
 
-        // Lock or unlock the slide "brake"
         if (gamepad.dpad_left) {
-            this.slideLocked = true
-
-            with(LinearSlide.slide) {
-                targetPosition = currentPosition
-                mode = DcMotor.RunMode.RUN_TO_POSITION
-            }
+            this.hanging = true
         } else if (gamepad.dpad_right) {
-            this.slideLocked = false
-
-            LinearSlide.slide.mode = DcMotor.RunMode.RUN_USING_ENCODER
-            LinearSlide.stop()
+            this.hanging = false
         }
 
-        // Move linear slide
-        if (this.slideLocked) {
-            LinearSlide.slide.power = 0.4
+        if (hanging) {
+            // Robot's weight counteracts hook's constant force
+            Hook.hook.power = -1.0
+            telemetry.addData("Hook", "Hanging")
         } else {
-            // Manually move slide up and down
-            if (gamepad.dpad_down) {
-                LinearSlide.power(-RobotConfig.TeleOpMain.SLIDE_DOWN_POWER)
-            } else if (gamepad.dpad_up) {
-                LinearSlide.power(RobotConfig.TeleOpMain.SLIDE_UP_POWER)
+            if (gamepad.a) {
+                Hook.moveHook(1.0)
+            } else if (gamepad.b) {
+                Hook.moveHook(-1.0)
             } else {
-                LinearSlide.stop()
+                Hook.stop()
             }
-        }
 
-        // Inputs for the gripper on the claw
-        if (gamepad.left_bumper) {
-            Claw.closeLR()
-        } else if (gamepad.right_bumper) {
-            Claw.openLR()
+            telemetry.addData("Hook", "Not hanging")
         }
 
-        // Inputs for the movement of the box
-        if (gamepad.a) {
-            Claw.dropClaw()
-        } else if (gamepad.b) {
-            Claw.raiseClaw()
-        }
-
-        // Input to launch drone.
         if (gamepad.x && gamepad.y) {
-            Drone.spin()
-        } else {
-            Drone.stop()
+            Drone.release()
+        } else if (gamepad.y) {
+            Drone.reset()
         }
 
         // movement of all wheels

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/MotorController.kt

@@ -51,5 +51,5 @@ class MotorController(
         previousError = currentError
     }
 
-    fun isDone() = abs(previousError) < 5
+    fun isDone() = abs(previousError) < 3 && abs(motor.power) < 0.03
 }