From ff864b2668acf8511474cd609b99083cbee6bab2 Mon Sep 17 00:00:00 2001
From: Cyberarm <matthewlikesrobots@gmail.com>
Date: Thu, 18 Aug 2022 20:41:53 -0500
Subject: [PATCH] Sodi's minibot teleop

---
 .../testing/engine/SodiEngine.java            | 14 +++++
 .../testing/states/SodiState.java             | 52 +++++++++++++++++++
 2 files changed, 66 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/timecrafters/testing/engine/SodiEngine.java
 create mode 100644 TeamCode/src/main/java/org/timecrafters/testing/states/SodiState.java

diff --git a/TeamCode/src/main/java/org/timecrafters/testing/engine/SodiEngine.java b/TeamCode/src/main/java/org/timecrafters/testing/engine/SodiEngine.java
new file mode 100644
index 0000000..0d4b325
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/testing/engine/SodiEngine.java
@@ -0,0 +1,14 @@
+package org.timecrafters.testing.engine;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.testing.states.SodiState;
+
+@TeleOp(name = "Wheel")
+public class SodiEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new SodiState());
+    }
+}
diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/SodiState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/SodiState.java
new file mode 100644
index 0000000..8fccc45
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/SodiState.java
@@ -0,0 +1,52 @@
+package org.timecrafters.testing.states;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class SodiState extends CyberarmState {
+    CRServo Wheel, Rack;
+    DcMotor ForePort, ForeStar, HindPort, HindStar;
+    Servo Zygo;
+    @Override
+    public void init() {
+        super.init();
+
+        Wheel=engine.hardwareMap.crservo.get("Wheel");
+        Rack=engine.hardwareMap.crservo.get("Rack");
+        ForePort=engine.hardwareMap.dcMotor.get("ForePort");
+        ForeStar=engine.hardwareMap.dcMotor.get("ForeStar");
+        HindPort=engine.hardwareMap.dcMotor.get("HindPort");
+        HindStar=engine.hardwareMap.dcMotor.get("HindStar");
+        Zygo=engine.hardwareMap.servo.get("Zygo");
+    }
+
+    @Override
+    public void exec() {
+        if (engine.gamepad1.x) {
+            Wheel.setPower(1.0);
+        } else if(engine.gamepad1.b) {
+            Wheel.setPower(-1.0);
+    } else {
+        Wheel.setPower(0);
+    }
+    if (engine.gamepad1.y) {
+        Rack.setPower(1.0);
+    } else if(engine.gamepad1.a) {
+        Rack.setPower(-1.0);
+    } else {
+        Rack.setPower(0);
+    }
+        ForePort.setPower(engine.gamepad1.left_stick_y * 1);
+        HindPort.setPower(engine.gamepad1.left_stick_y * 1);
+        ForeStar.setPower(engine.gamepad1.right_stick_y * 1);
+        HindStar.setPower(engine.gamepad1.right_stick_y * 1);
+        if (engine.gamepad1.left_bumper) {
+            Zygo.setPosition(1.0);
+        } else {
+            Zygo.setPosition(0);
+        }
+    }
+}