Box Movement

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

@@ -0,0 +1,51 @@
+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 boxL: Servo
+    private lateinit var boxR: Servo
+    private lateinit var grip: Servo
+
+    override fun init(opMode: OpMode) {
+        super.init(opMode)
+
+        this.boxL = this.hardwareMap.get(Servo::class.java, "boxL")
+        this.boxR = this.hardwareMap.get(Servo::class.java, "boxR")
+        this.grip = this.hardwareMap.get(Servo::class.java, "grip")
+    }
+
+    /**
+     * Moves the box to the ground
+     */
+    fun dropBox() {
+        this.boxL.position = 0.4
+        this.boxR.position = 0.35
+    }
+
+    /**
+     * Moves box to upright position
+     */
+    fun pickUpBox() {
+        this.boxL.position = 0.76
+        this.boxR.position = 0.0
+    }
+
+    /**
+     * Moves the grip to keep pixels in place when  [pickUpBox] the box
+     */
+    fun gripIn() {
+        this.grip.position = 0.23
+    }
+
+    /**
+     * Resets the grip to starting position
+     */
+    fun gripOut() {
+        this.grip.position = 0.11
+    }
+}

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

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.components
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import org.firstinspires.ftc.teamcode.api.Box
 import org.firstinspires.ftc.teamcode.api.TriWheels
 import org.firstinspires.ftc.teamcode.utils.RobotConfig
 import kotlin.math.PI
@@ -30,11 +31,25 @@ object TeleOpMovement : Component {
         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) {
+        // if the y  button is pressed move slower
+        if (gamepad.y) {
             joyMagnitude *= SLOW_MULTIPLIER
         }
 
+        // 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.Telemetry
 import org.firstinspires.ftc.teamcode.api.TriWheels
@@ -24,6 +25,9 @@ class TeleOpMain : OpMode() {
         // Advanced gamepad inputs
         GamepadEx.init(this)
 
+        // Box controls
+        Box.init(this)
+
         // Log that we are initialized
         Telemetry.sayInitialized()
     }