From db891cd4595f16c38953fae68f90fdd36db75f94 Mon Sep 17 00:00:00 2001
From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com>
Date: Thu, 12 Oct 2023 20:45:25 -0500
Subject: [PATCH] Still working on making the Controller Hub recognize my
 programs

---
 .../Engines/ProtoBotEngineSodi.java           |  5 ++--
 .../Autonomous/States/ProtoBotStateSodi.java  | 30 ++++++++++++++++++-
 .../CenterStage/Common/ProtoBotSodi.java      | 12 ++++----
 3 files changed, 38 insertions(+), 9 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 c440293..a92934f 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
@@ -1,5 +1,6 @@
 package org.timecrafters.CenterStage.Autonomous.Engines;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.timecrafters.CenterStage.Common.ProtoBotSodi;
 import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi;
@@ -7,7 +8,7 @@ import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@Autonomous(name = "Rigel", group = "Prototype Sodi")
+@TeleOp(name = "Rigel", group = "Robot")
 public class ProtoBotEngineSodi extends CyberarmEngine {
     private ProtoBotSodi robot;
     @Override
@@ -15,6 +16,6 @@ public class ProtoBotEngineSodi extends CyberarmEngine {
         this.robot = new ProtoBotSodi();
         this.robot.setup();
 
-        addState(new ProtoBotStateSodi(robot));
+        addState(new ProtoBotStateSodi());
     }
 }
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 b93e7d6..2121628 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
@@ -9,7 +9,7 @@ import org.timecrafters.CenterStage.Common.ProtoBotSodi;
 
 public class ProtoBotStateSodi extends CyberarmState {
     ProtoBotSodi robot;
-    public ProtoBotStateSodi(ProtoBotSodi robot) {
+    public ProtoBotStateSodi() {
         this.robot = robot;
     }
     public void telemetry() {
@@ -28,8 +28,36 @@ public class ProtoBotStateSodi extends CyberarmState {
     //Servos
     robot.jaw.setDirection(Servo.Direction.FORWARD);
     }
+    @Override
+    public void init() {
+        robot.flDrive.motor.setPower(0);
+        robot.frDrive.motor.setPower(0);
+        robot.blDrive.motor.setPower(0);
+        robot.brDrive.motor.setPower(0);
+        robot.bloodWorm.motor.setPower(0);
+    }
+
     @Override
     public void exec() {
 
+        if (System.currentTimeMillis() >= 500 && System.currentTimeMillis() < 2500) {
+            robot.flDrive.motor.setPower(0.5);
+            robot.frDrive.motor.setPower(0.5);
+            robot.blDrive.motor.setPower(0.5);
+            robot.brDrive.motor.setPower(0.5);
+            robot.bloodWorm.motor.setPower(0.5);
+        } else if (System.currentTimeMillis() >= 2500 && System.currentTimeMillis() < 3500) {
+            robot.flDrive.motor.setPower(-0.5);
+            robot.frDrive.motor.setPower(-0.5);
+            robot.blDrive.motor.setPower(-0.5);
+            robot.brDrive.motor.setPower(-0.5);
+            robot.bloodWorm.motor.setPower(-0.5);
+        } else {
+            robot.flDrive.motor.setPower(0);
+            robot.frDrive.motor.setPower(0);
+            robot.blDrive.motor.setPower(0);
+            robot.brDrive.motor.setPower(0);
+            robot.bloodWorm.motor.setPower(0);
+        }
     }
 }
diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java
index 4d26d10..8a54e75 100644
--- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java
+++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java
@@ -31,14 +31,14 @@ public class ProtoBotSodi extends Robot {
     @Override
     public void setup() {
 
-        configuration = new TimeCraftersConfiguration("Rigel");
+        configuration = new TimeCraftersConfiguration("Robbie");
 
         //Motors
-        flDrive = new MotorEx(hardwareMap, "FrontLeftDrive");
-        frDrive = new MotorEx(hardwareMap, "FrontRightDrive");
-        blDrive = new MotorEx(hardwareMap, "BackLeftDrive");
-        brDrive = new MotorEx(hardwareMap, "BackRightDrive");
-        bloodWorm = new MotorEx(hardwareMap, "Proboscis");
+        flDrive = new MotorEx(hardwareMap, "frontLeft");
+        frDrive = new MotorEx(hardwareMap, "frontRight");
+        blDrive = new MotorEx(hardwareMap, "backLeft");
+        brDrive = new MotorEx(hardwareMap, "backRight");
+        bloodWorm = new MotorEx(hardwareMap, "lift");
         //The motor bloodWorm is the one that raises the deliverer.
         //The reason it's called bloodworm is because of what bloodworms are. Better left unsaid.