From 250db12317aede9fa1607035416ce7a659e66485 Mon Sep 17 00:00:00 2001
From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com>
Date: Tue, 8 Nov 2022 20:33:43 -0600
Subject: [PATCH] LeftSideAutonomous has drive backward and forward states
 after rotating towards both of the junctions for allignment. and it is
 organized

---
 .../Engines/LeftSideAutonomousEngine.java     |  2 ++
 .../Engines/RightSideAutonomousEngine.java    |  2 ++
 .../States/CollectorDistanceState.java        | 19 ++++++++++++++++---
 3 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
index fabe850..1a65f8d 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
@@ -27,6 +27,8 @@ public class LeftSideAutonomousEngine extends CyberarmEngine {
         addState(new DriverState(robot, "LeftSideAutonomous", "01-0"));
         //turn towards high pole
         addState(new RotationState(robot, "LeftSideAutonomous", "02-0"));
+        //drive back
+        addState(new DriverState(robot, "LeftSideAutonomous", "02-1"));
         //lift the upper arm
         addState(new TopArm(robot, "LeftSideAutonomous", "03-0"));
         //lift the lower arm
diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
index 5bcd405..bc37d82 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
@@ -27,6 +27,8 @@ public class RightSideAutonomousEngine extends CyberarmEngine {
         addState(new DriverState(robot, "RightSideAutonomous", "01-0"));
         //turn towards high pole
         addState(new RotationState(robot, "RightSideAutonomous", "02-0"));
+        // drive back
+        addState(new DriverState(robot, "RightSideAutonomous", "02-1"));
         //lift the upper arm
         addState(new TopArm(robot, "RightSideAutonomous", "03-0"));
         //lift the lower arm
diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
index b6982e4..4e3588b 100644
--- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
+++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
@@ -20,6 +20,9 @@ public class CollectorDistanceState extends CyberarmState {
     private double distanceDelta;
     private double debugRunTime;
     private String debugStatus = "?";
+    private boolean inRange = false;
+    private float collectTime;
+    private double inRangeTime;
 
 
     public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
@@ -28,6 +31,7 @@ public class CollectorDistanceState extends CyberarmState {
         this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
         this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
         this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
+        this.collectTime = robot.configuration.variable(groupName, actionName, "collectTime").value();
 
     }
 
@@ -147,9 +151,18 @@ public class CollectorDistanceState extends CyberarmState {
         robot.frontLeftDrive.setPower(0);
         robot.backRightDrive.setPower(0);
         robot.backLeftDrive.setPower(0);
-        robot.collectorRight.setPower(0);
-        robot.collectorLeft.setPower(0);
-        setHasFinished(true);
+
+        if (!inRange){
+            inRange = true;
+            inRangeTime = runTime();
+        } else {
+
+            if (runTime() - inRangeTime >= collectTime){
+                robot.collectorRight.setPower(0);
+                robot.collectorLeft.setPower(0);
+                setHasFinished(true);
+            }
+        }
 
             }