Box Movement

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

@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.api
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.hardware.Servo
+
+/**
+ * An API for controlling the Box on the robot.
+ */
+object Box : API() {
+    private lateinit var box: Servo
+    private lateinit var grip: Servo
+
+    override fun init(opMode: OpMode) {
+        super.init(opMode)
+
+        this.box = this.hardwareMap.get(Servo::class.java, "box")
+        this.grip = this.hardwareMap.get(Servo::class.java, "grip")
+    }
+
+    /**
+     * Moves the box to the ground
+     */
+    fun dropBox() {
+        this.box.position = 0.5
+    }
+
+    /**
+     * Moves box to upright position
+     */
+    fun pickUpBox() {
+        this.box.position = 1.0
+    }
+
+    /**
+     * Moves the grip to keep pixels in place when  [pickUpBox] the box
+     */
+    fun gripIn() {
+        this.grip.position = 0.4
+    }
+
+    /**
+     * Resets the grip to starting position
+     */
+    fun gripOut() {
+        this.grip.position = 0.11
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/components/TeleOpMovement.kt

@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.components
 import com.qualcomm.robotcore.eventloop.opmode.OpMode
 import com.qualcomm.robotcore.hardware.DcMotor
 import org.firstinspires.ftc.teamcode.api.LinearSlide
+import org.firstinspires.ftc.teamcode.api.Box
 import org.firstinspires.ftc.teamcode.api.TriWheels
 import org.firstinspires.ftc.teamcode.utils.Resettable
 import org.firstinspires.ftc.teamcode.utils.RobotConfig
@@ -13,7 +14,7 @@ import kotlin.math.sqrt
 /**
  * Moves the robot wheels based on gamepad input.
  *
- * Requires the [TriWheels] API.
+ * Requires the [TriWheels], [Box], and [LinearSlide] APIs.
  */
 object TeleOpMovement : Component {
     private var slideLocked: Boolean by Resettable { false }
@@ -30,12 +31,7 @@ object TeleOpMovement : Component {
         // angle and strength
         // PI / 3 because 0 radians is right, not forward
         val joyRadians = atan2(joyY, joyX) - (PI / 3.0)
-        var joyMagnitude = sqrt(joyY * joyY + joyX * joyX)
-
-        // if the right bumper is pressed move slower
-        if (gamepad.right_bumper) {
-            joyMagnitude *= RobotConfig.TeleOpMovement.SLOW_MULTIPLIER
-        }
+        val joyMagnitude = sqrt(joyY * joyY + joyX * joyX)
 
         // Lock or unlock the slide "brake"
         if (gamepad.dpad_left) {
@@ -52,6 +48,7 @@ object TeleOpMovement : Component {
             LinearSlide.stop()
         }
 
+        // Move linear slide
         if (slideLocked) {
             LinearSlide.slide.power = 0.4
         } else {
@@ -65,6 +62,20 @@ object TeleOpMovement : Component {
             }
         }
 
+        // Inputs for the gripper on the box
+        if (gamepad.left_bumper) {
+            Box.gripIn()
+        } else if (gamepad.right_bumper) {
+            Box.gripOut()
+        }
+
+        // Inputs for the movement of the box
+        if (gamepad.a) {
+            Box.dropBox()
+        } else if (gamepad.b) {
+            Box.pickUpBox()
+        }
+
         // movement of all wheels
         TriWheels.driveWithRotation(
             joyRadians,

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

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.opmode.teleop
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
+import org.firstinspires.ftc.teamcode.api.Box
 import org.firstinspires.ftc.teamcode.api.GamepadEx
 import org.firstinspires.ftc.teamcode.api.LinearSlide
 import org.firstinspires.ftc.teamcode.api.Telemetry
@@ -23,6 +24,9 @@ class TeleOpMain : OpMode() {
         // Advanced gamepad inputs
         GamepadEx.init(this)
 
+        // Box controls
+        Box.init(this)
+
         // Log that we are initialized
         Telemetry.sayInitialized()
     }