Box Movement

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

@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.api
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode
+import com.qualcomm.robotcore.hardware.Servo
+
+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() {
+ boxL.position = 0.5
+ boxR.position = 0.5
+ }
+
+ /**
+ * Moves box to upright position
+ */
+ fun pickUpBox() {
+ boxL.position = 0.0
+ boxL.position = 0.0
+ }
+
+ /**
+ * Moves the grip to keep pixels in place when [pickUpBox] the box
+ */
+ fun gripIn() {
+ grip.position = 0.2
+ }
+
+ /**
+ * Resets the grip to starting position
+ */
+ fun gripOut() {
+ grip.position = 0.0
+ }
+}

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.TeleOpState
 import org.firstinspires.ftc.teamcode.api.TriWheels
 import kotlin.math.PI
@@ -43,10 +44,26 @@ object TeleOpMovement : Component {
  var joyMagnitude = sqrt(joyY * joyY + joyX * joyX)
 
  // if the right bumper is pressed move slower
- if (gamepad.right_bumper) {
+ if (gamepad.y) {
  joyMagnitude *= SLOW_MULTIPLIER
  }
 
+ // Inputs for the gripper on the box
+ if (gamepad.left_bumper) {
+ Box.gripIn()
+ }
+ if (gamepad.right_bumper) {
+ Box.gripOut()
+ }
+
+ // Inputs for the movement of the box
+ if (gamepad.a) {
+ Box.dropBox()
+ }
+ if (gamepad.b) {
+ Box.pickUpBox()
+ }
+
  // movement of all wheels
  TriWheels.driveWithRotation(joyRadians, joyMagnitude, rotationPower)
  }