From 26ba99afab8fb5f59ecfeeb69f1ba470dab9d359 Mon Sep 17 00:00:00 2001
From: Sodi <ravenpaw5040@gmail.com>
Date: Sat, 17 Sep 2022 11:57:11 -0500
Subject: [PATCH] Tested Arm Motor and Collector Wrist, added Rack Risers.

---
 .../testing/states/PrototypeBot1.java         |  5 ++++-
 .../testing/states/PrototypeTeleOPState.java  | 19 ++++++++++++++++++-
 2 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
index 69d9648..1397ad7 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
@@ -9,7 +9,8 @@ import org.cyberarm.engine.V2.CyberarmEngine;
 
 public class PrototypeBot1 {
 
-        private CyberarmEngine engine;
+    public Servo RackRiserLeft, RackRiserRight;
+    private final CyberarmEngine engine;
 
         public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, armMotor;
 
@@ -36,6 +37,8 @@ public class PrototypeBot1 {
             collectorLeft = engine.hardwareMap.crservo.get("collector left");
             collectorRight = engine.hardwareMap.crservo.get("collector right");
             collectorWrist = engine.hardwareMap.servo.get("collector wrist");
+            RackRiserLeft = engine.hardwareMap.servo.get("RackRiserLeft");
+            RackRiserRight = engine.hardwareMap.servo.get("RackRiserRight");
 
             //motors direction and encoders
             frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
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 ce6ccbe..43901c2 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
@@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 import org.cyberarm.engine.V2.CyberarmState;
 
 import java.lang.annotation.Target;
+import java.util.Objects;
 
 public class PrototypeTeleOPState extends CyberarmState {
 
@@ -16,6 +17,7 @@ public class PrototypeTeleOPState extends CyberarmState {
     private double drivePower = 1;
     private boolean UpDPad;
     private int armTargetPosition = 0, armCollectPosition = 125, armDeliverPosition = 400;
+    private double collectorRiserPosition;
 
     public PrototypeTeleOPState(PrototypeBot1 robot) {
         this.robot = robot;
@@ -26,6 +28,7 @@ public class PrototypeTeleOPState extends CyberarmState {
         engine.telemetry.addData("Arm Power", robot.armMotor.getPower());
         engine.telemetry.addData("Arm Target Position", robot.armMotor.getTargetPosition());
         engine.telemetry.addData("Arm Current Position", robot.armMotor.getCurrentPosition());
+        engine.telemetry.addData("Wrist Current Position", robot.collectorWrist.getPosition());
     }
 
     @Override
@@ -115,7 +118,7 @@ public class PrototypeTeleOPState extends CyberarmState {
 
         if (engine.gamepad2.dpad_left) {
 
-            robot.collectorWrist.setPosition(0.2);
+            robot.collectorWrist.setPosition(0.4);
 
         }
 
@@ -164,5 +167,19 @@ public class PrototypeTeleOPState extends CyberarmState {
 
         robot.armMotor.setTargetPosition(armTargetPosition);
 
+        if (engine.gamepad1.a) {
+
+            robot.RackRiserLeft.setPosition(0);
+            robot.RackRiserRight.setPosition(1);
+
+        }
+
+        if (engine.gamepad1.x) {
+
+            robot.RackRiserLeft.setPosition(0.5);
+            robot.RackRiserRight.setPosition(1-.5);
+// .5's are originally .13's.
+        }
+
     }
 }
\ No newline at end of file