From 800d7d1be9a660e99bb2e2b70006093401388c39 Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Thu, 5 Oct 2023 20:31:26 -0500
Subject: [PATCH] congfiguration added for depositor variables

---
 .../CenterStage/Common/PrototypeRobot.java    | 17 ++++++++
 .../States/PrototypeRobotDrivetrainState.java | 41 +++++++++++++------
 2 files changed, 46 insertions(+), 12 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
index a37f3b3..e7996c6 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java
@@ -12,11 +12,16 @@ import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.timecrafters.Library.Robot;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
 public class PrototypeRobot extends Robot {
 
+    public float ELBOW_COLLECT;
+    public float ELBOW_DEPOSIT;
+    public float SHOULDER_COLLECT;
+    public float SHOULDER_DEPOSIT;
     private HardwareMap hardwareMap;
     public MotorEx frontLeft, frontRight, backLeft, backRight, lift;
     public IMU imu;
@@ -25,10 +30,19 @@ public class PrototypeRobot extends Robot {
     private String string;
     private CyberarmEngine engine;
 
+    public TimeCraftersConfiguration configuration;
+
     public PrototypeRobot(String string) {
         this.string = string;
     }
 
+    public void initConstants(){
+            ELBOW_DEPOSIT = configuration.variable("Robot", "Tuning", "ELBOW_DEPOSIT").value();
+            ELBOW_COLLECT = configuration.variable("Robot", "Tuning", "ELBOW_COLLECT").value();
+            SHOULDER_DEPOSIT = configuration.variable("Robot", "Tuning", "SHOULDER_DEPOSIT").value();
+            SHOULDER_COLLECT = configuration.variable("Robot", "Tuning", "SHOULDER_COLLECT").value();
+        }
+
     @Override
     public void setup() {
         System.out.println("Bacon: " + this.string);
@@ -44,6 +58,9 @@ public class PrototypeRobot extends Robot {
         backLeft = new MotorEx(hardwareMap, "backLeft");
         lift = new MotorEx(hardwareMap, "lift");
 
+        configuration = new TimeCraftersConfiguration("Robbie");
+
+        initConstants();
 
         frontRight.motor.setDirection(DcMotorSimple.Direction.FORWARD);
         backRight.motor.setDirection(DcMotorSimple.Direction.FORWARD);
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java
index 30e0258..354aef6 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java
@@ -14,22 +14,22 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
         this.robot = robot;
     }
 
-    @Override
-    public void exec() {
-        // drivetrain
-        robot.driveTrainTeleOp();
-
+    // --------------------------------------------------------------------------------------------------------- Depositor control function
+    private void depositorTeleOp(){
         // flip arms
         if (engine.gamepad1.x) {
-            robot.depositorShoulder.setPosition(0.75);
+            // shoulder deposit
+            robot.depositorShoulder.setPosition(robot.SHOULDER_DEPOSIT);
+            // shoulder collect
         } else if (engine.gamepad1.a) {
-            robot.depositorShoulder.setPosition(0.05);
+            robot.depositorShoulder.setPosition(robot.SHOULDER_COLLECT);
         }
-
         if (engine.gamepad1.y){
-            robot.depositorElbow.setPosition(0.33);
+            // elbow deposit
+            robot.depositorElbow.setPosition(robot.ELBOW_DEPOSIT);
+            // elbow collect
         } else if (engine.gamepad1.b){
-            robot.depositorElbow.setPosition(0);
+            robot.depositorElbow.setPosition(robot.ELBOW_COLLECT); // Collect / transfer = 0
         }
 
         // depositor
@@ -39,8 +39,9 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
             robot.depositor.setPosition(0.2);
         }
 
-
-        // lift
+    }
+    // --------------------------------------------------------------------------------------------------------- Slider control function
+    private void sliderTeleOp(){
         if (engine.gamepad1.right_trigger != 0){
             if (robot.lift.getCurrentPosition() >= maxExtension){
                 robot.lift.motor.setPower(0);
@@ -61,6 +62,18 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
         } else {
             robot.lift.motor.setPower(0);
         }
+    }
+
+    @Override
+    public void exec() {
+        // drivetrain
+        robot.driveTrainTeleOp();
+
+        // depositor
+        depositorTeleOp();
+
+        // lift
+        sliderTeleOp();
 
     }
 
@@ -69,5 +82,9 @@ public class PrototypeRobotDrivetrainState extends CyberarmState {
     public void telemetry() {
         engine.telemetry.addData("Lift Encoder Pos", robot.lift.motor.getCurrentPosition());
         engine.telemetry.addData("imu", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        engine.telemetry.addData("Elbow Collect", robot.ELBOW_COLLECT);
+        engine.telemetry.addData("Elbow Deposit", robot.ELBOW_DEPOSIT);
+        engine.telemetry.addData("Shoulder Collect", robot.SHOULDER_COLLECT);
+        engine.telemetry.addData("Shoulder Deposit", robot.SHOULDER_DEPOSIT);
     }
 }