From adf856bb24e5a8cb7c2a3af6472c128426f7ef95 Mon Sep 17 00:00:00 2001
From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com>
Date: Tue, 9 Jan 2024 15:58:33 -0600
Subject: [PATCH] Pizza bot - Still working on the field centric drive and arm
 has been reapplied

---
 .../Common/SodiPizzaMinibotObject.java          |  5 ++---
 .../TeleOp/States/SodiPizzaTeleOPState.java     | 17 ++++++++++++++++-
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
index 25c736a..3416aec 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java
@@ -19,8 +19,7 @@ public class SodiPizzaMinibotObject extends Robot {
 
     public HardwareMap hardwareMap;
     public DcMotor leftFront, rightFront, leftBack, rightBack;
-//    public Servo shoulder, gripper;
-    public Servo launcher;
+    public Servo shoulder, gripper, launcher;
     public IMU imu;
     public Rev2mDistanceSensor distSensor;
     private String string;
@@ -71,7 +70,7 @@ public class SodiPizzaMinibotObject extends Robot {
         rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
         //Servo Defining
-//        shoulder = engine.hardwareMap.servo.get("arm");
+        shoulder = engine.hardwareMap.servo.get("shoulder");
 //        gripper = engine.hardwareMap.servo.get("gripper");
         launcher = engine.hardwareMap.servo.get("launcher");
 
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
index ae0e939..2c1839e 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
@@ -43,6 +43,7 @@ public class SodiPizzaTeleOPState extends CyberarmState {
     public float getApproxObjPos() {
         if (System.currentTimeMillis() - lastDistRead >= 500) {
             /*Pseudocode: take objData1, wait, take 2, wait, take 3*/
+
         }
         approxObjPos = (objData1 + objData2 + objData3)/3;
         return approxObjPos;
@@ -52,6 +53,9 @@ public class SodiPizzaTeleOPState extends CyberarmState {
     public void telemetry() {
         engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition());
         engine.telemetry.addData("Drone Launched?", droneLaunched);
+
+        engine.telemetry.addLine();
+        engine.telemetry.addData("Arm servo position", robot.shoulder.getPosition());
     }
 
     @Override
@@ -99,7 +103,6 @@ public class SodiPizzaTeleOPState extends CyberarmState {
             robot.leftFront.setPower(lfPower);
             robot.rightFront.setPower(rfPower);
 
-
         }
 
 
@@ -127,6 +130,18 @@ public class SodiPizzaTeleOPState extends CyberarmState {
             }
         }
 
+        if (engine.gamepad2.left_stick_y > 0.1) {
+            if (System.currentTimeMillis() - lastMoveTime >= 200) {
+                robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05);
+                lastMoveTime = System.currentTimeMillis();
+            }
+        } else if (engine.gamepad2.left_stick_y < -0.1) {
+            if (System.currentTimeMillis() - lastMoveTime >= 200) {
+                robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05);
+                lastMoveTime = System.currentTimeMillis();
+            }
+        }
+
 //        // This moves the arm to Collect position, which is at servo position 0.00.
 //        if (engine.gamepad2.a && !engine.gamepad2.start) {
 //            armPos = 1;