feat: hook hanging

BotsBurgh · Feb 23, 2024 · 8757e26 · 8757e26
1 parent e263bca
commit 8757e26
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 11 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/Hook.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/api/Hook.kt
@@ -2,25 +2,29 @@ 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() {
-    private lateinit var hanger: DcMotor
+    lateinit var hook: DcMotor
+        private set
 
     override fun init(opMode: OpMode) {
         super.init(opMode)
 
-        this.hanger = this.opMode.hardwareMap.get(DcMotor::class.java, "hanger")
+        this.hook = this.opMode.hardwareMap.get(DcMotor::class.java, "hanger")
 
-        hanger.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.BRAKE
+        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
-        hanger.power = power
+        hook.power = power
     }
 
     fun stop() {
-        hanger.power = 0.0
+        hook.power = 0.0
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/TeleOpMain.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/TeleOpMain.kt
@@ -13,6 +13,8 @@ import kotlin.math.sqrt
 
 @TeleOp(name = "TeleOpMain")
 class TeleOpMain : OpMode() {
+    private var hanging = false
+
     override fun init() {
         Telemetry.init(this)
         TriWheels.init(this)
@@ -39,13 +41,26 @@ class TeleOpMain : OpMode() {
 
         val rotationPower = -gamepad.right_stick_x.toDouble()
 
-        // Inputs for the movement of the box
-        if (gamepad.a) {
-            Hook.moveHook(0.5)
-        } else if (gamepad.b) {
-            Hook.moveHook(-0.5)
+        if (gamepad.dpad_left) {
+            this.hanging = true
+        } else if (gamepad.dpad_right) {
+            this.hanging = false
+        }
+
+        if (hanging) {
+            // Robot's weight counteracts hook's constant force
+            Hook.hook.power = -1.0
+            telemetry.addData("Hook", "Hanging")
         } else {
-            Hook.stop()
+            if (gamepad.a) {
+                Hook.moveHook(1.0)
+            } else if (gamepad.b) {
+                Hook.moveHook(-1.0)
+            } else {
+                Hook.stop()
+            }
+
+            telemetry.addData("Hook", "Not hanging")
         }
 
         if (gamepad.x && gamepad.y) {