From 6eae3fa3052ff1b22a3f731cb89977cc369bd0c1 Mon Sep 17 00:00:00 2001
From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com>
Date: Thu, 26 Oct 2023 20:29:13 -0500
Subject: [PATCH] Begin useful stuff, need to work on game strategy

---
 .../Engines/ProtoBotEngineSodi.java           |  4 ++--
 .../Autonomous/States/ProtoBotStateSodi.java  | 24 +++++++++----------
 2 files changed, 13 insertions(+), 15 deletions(-)

diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java
index 91dd568..c147779 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java
@@ -7,12 +7,12 @@ import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@Autonomous(name = "Autonomous- Sodi", group = "Simple Test")
+@Autonomous(name = "Autonomous- Sodi", group = "Arm Sequence")
 public class ProtoBotEngineSodi extends CyberarmEngine {
     private ProtoBotSodi robot;
     @Override
     public void setup() {
-        this.robot = new ProtoBotSodi("Autonomous - Sodi");
+        this.robot = new ProtoBotSodi("Autonomous- Sodi");
         this.robot.setup();
 
         addState(new ProtoBotStateSodi(robot));
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java
index b66ec61..ed2851a 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java
@@ -10,15 +10,16 @@ import org.timecrafters.CenterStage.Common.ProtoBotSodi;
 
 public class ProtoBotStateSodi extends CyberarmState {
     ProtoBotSodi robot;
-    private double avgVelocity, avgDrivePower;
     private long lastTimeChecked;
     private int testSequence;
+    private int targetPos;
+    private int currentPos;
+    private int totalDist;  //
+
     public ProtoBotStateSodi(ProtoBotSodi robot) {
         this.robot = robot;
     }
     public void telemetry() {
-//    engine.telemetry.addData("Avg Drive Velocity", avgVelocity);
-//    engine.telemetry.addData("Avg Drive Power", avgDrivePower);
     engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity());
     engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity());
     engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity());
@@ -30,16 +31,6 @@ public class ProtoBotStateSodi extends CyberarmState {
 
     }
 
-//    public double getAvgDrivePower() {
-//        avgDrivePower = (robot.flDrive.motor.getPower() + robot.frDrive.motor.getPower() + robot.blDrive.motor.getPower() + robot.brDrive.motor.getPower())/4;
-//        return avgDrivePower;
-//    }
-
-//    public double getAvgVelocity() {
-//        avgVelocity = (robot.flDrive.getVelocity() + robot.frDrive.getVelocity() + robot.blDrive.getVelocity() + robot.brDrive.getVelocity())/4;
-//        return avgVelocity;
-//    }
-
     @Override
     public void init() {
         robot.flDrive.motor.setPower(0);
@@ -59,10 +50,14 @@ public class ProtoBotStateSodi extends CyberarmState {
         testSequence = 0;
 
 
+
+
     }
 
     @Override
     public void exec() {
+
+        currentPos = robot.liftMotor.motor.getCurrentPosition();
 //
 //        if (System.currentTimeMillis() - lastTimeChecked >= 500 && System.currentTimeMillis() - lastTimeChecked < 2500) {
 //            robot.flDrive.motor.setPower(0.5);
@@ -99,11 +94,14 @@ public class ProtoBotStateSodi extends CyberarmState {
 
         switch (testSequence) {
             case 1:
+                robot.liftMotor.motor.setPower(0.4);
+                robot.liftMotor.motor.setTargetPosition(targetPos);
 
                 //lift motor go up for some way
                 //wait for about 0.25
 
             case 2:
+                robot.liftMotor.motor.setTargetPosition(targetPos);
 
                 //lift motor go down
                 //repeat