From 7442efa1c9f9bf040027e444f2d9233451e4c9ce Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Thu, 20 Oct 2022 19:32:40 -0500
Subject: [PATCH] Configuration

---
 .../Autonomous/States/LowerArm.java           | 21 +++++++++++++++++++
 .../testing/states/PrototypeBot1.java         |  5 ++++-
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java
index e66a597..974c2b7 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java
@@ -1,11 +1,32 @@
 package org.timecrafters.Autonomous.States;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.testing.states.PrototypeBot1;
 
 public class LowerArm extends CyberarmState {
 
+    PrototypeBot1 robot;
+    double LowerRiserRightPos, LowerRiserLeftPos;
+    long time;
+    long lastStepTime = 0;
+
+    public LowerArm(PrototypeBot1 robot, String groupName, String actionName) {
+        this.robot = robot;
+        this.LowerRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value();
+        this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value();
+        this.time = robot.configuration.variable(groupName, actionName, "time").value();
+    }
+
     @Override
     public void exec() {
+        if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos) {
+            if (System.currentTimeMillis() - lastStepTime >= time) {
+                lastStepTime = System.currentTimeMillis();
+                robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - LowerRiserLeftPos);
+                robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - LowerRiserRightPos);
 
+
+            }
+        }
     }
 }
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 988f55d..bbeb323 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
@@ -9,6 +9,7 @@ import com.qualcomm.robotcore.hardware.Servo;
 import org.cyberarm.engine.V2.CyberarmEngine;
 import org.firstinspires.ftc.robotcore.external.navigation.Position;
 import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
 
 public class PrototypeBot1 {
 
@@ -21,6 +22,8 @@ public class PrototypeBot1 {
 
          public BNO055IMU imu;
 
+         public TimeCraftersConfiguration configuration;
+
 //        public Servo collectorWrist;
 
         public PrototypeBot1(CyberarmEngine engine) {
@@ -42,7 +45,7 @@ public class PrototypeBot1 {
 
             imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
 
-
+            configuration = new TimeCraftersConfiguration("Phoenix");
 
             //motors configuration
             frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left");