From ff471bce8bf7c2b69d24b73fa1e617a992f4db0c Mon Sep 17 00:00:00 2001
From: Sodi <ravenpaw5040@gmail.com>
Date: Mon, 12 Sep 2022 20:18:02 -0500
Subject: [PATCH] Arm Motor and Collector Wheels (Arm need reinforcement,
 Cayden)

---
 .../testing/states/PrototypeTeleOPState.java  | 29 ++++++++++++++++++-
 1 file changed, 28 insertions(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
index e37f705..4243c61 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
@@ -1,5 +1,7 @@
 package org.timecrafters.testing.states;
 
+import com.qualcomm.robotcore.hardware.DcMotor;
+
 import org.cyberarm.engine.V2.CyberarmState;
 
 public class PrototypeTeleOPState extends CyberarmState {
@@ -10,11 +12,27 @@ public class PrototypeTeleOPState extends CyberarmState {
     public boolean Y;
     private boolean bprev;
     private double drivePower = 1;
+    private boolean UpDPad;
 
     public PrototypeTeleOPState(PrototypeBot1 robot) {
         this.robot = robot;
     }
 
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Arm Position", robot.armMotor.getCurrentPosition());
+    }
+
+    @Override
+    public void init() {
+        robot.armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        robot.armMotor.setTargetPosition(600);
+        robot.armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+
+    }
+
     @Override
     public void exec() {
 
@@ -23,6 +41,7 @@ public class PrototypeTeleOPState extends CyberarmState {
         A = engine.gamepad1.a;
         X = engine.gamepad1.x;
         Y = engine.gamepad1.y;
+        UpDPad = engine.gamepad1.dpad_up;
 
 
         //drive speed toggle
@@ -71,7 +90,7 @@ public class PrototypeTeleOPState extends CyberarmState {
 
         }
 
-        if (engine.gamepad1.a) {
+        if (engine.gamepad1.b) {
 
             robot.collectorLeft.setPower(1);
             robot.collectorRight.setPower(-1);
@@ -85,5 +104,13 @@ public class PrototypeTeleOPState extends CyberarmState {
 
         }
 
+        if (engine.gamepad1.dpad_up) {
+
+            robot.armMotor.setPower(1);
+        } else {
+            robot.armMotor.setPower(0);
+
+
+        }
     }
 }
\ No newline at end of file