Hold arm in position attempt #1

tysuh66 · Feb 4, 2022 · 150ba46 · 150ba46
1 parent 3b48182
commit 150ba46
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Lilboi.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Lilboi.java
@@ -8,6 +8,8 @@
 @TeleOp(name = "Lil Main Drive")
 public class Lilboi extends LinearOpMode {
 
+    private int position = 0;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -24,7 +26,10 @@ public void runOpMode() throws InterruptedException {
         //set motor direction
         motorFrontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
         motorBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        // Setup shoulder
         shoulderMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        shoulderMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 
         waitForStart();
 
@@ -58,8 +63,10 @@ public void runOpMode() throws InterruptedException {
 
             if(gamepad1.dpad_up){
                 shoulderPower = 0.5;
+                position += 20;
             }else if(gamepad1.dpad_down){
                 shoulderPower = -0.5;
+                position += 20;
             }else{
                 shoulderPower = 0;
             }
@@ -72,6 +79,7 @@ public void runOpMode() throws InterruptedException {
                 elbowPower = 0;
             }
 
+            shoulderMotor.setTargetPosition(position);
             shoulderMotor.setPower(shoulderPower);
             elbowMotor.setPower(elbowPower);
             spinMotor.setPower(spin);