From 0b74a5dde52e10e1caaafb401fa08c609ea56eaa Mon Sep 17 00:00:00 2001
From: Sodi <ravenpaw5040@gmail.com>
Date: Thu, 1 Sep 2022 18:58:12 -0500
Subject: [PATCH] Tank drive w/ bumper strafe, playing with LEDS and sensors.

---
 .../testing/engine/SodiEngine.java            |  6 +-
 .../testing/states/LaserState.java            | 37 +++++++++++
 .../testing/states/SodiLEDState.java          | 62 +++++++++++++++++++
 3 files changed, 104 insertions(+), 1 deletion(-)
 create mode 100644 TeamCode/src/main/java/org/timecrafters/testing/states/LaserState.java
 create mode 100644 TeamCode/src/main/java/org/timecrafters/testing/states/SodiLEDState.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
index 0d4b325..93fe81a 100644
--- a/TeamCode/src/main/java/org/timecrafters/testing/engine/SodiEngine.java
+++ b/TeamCode/src/main/java/org/timecrafters/testing/engine/SodiEngine.java
@@ -3,12 +3,16 @@ package org.timecrafters.testing.engine;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.testing.states.LaserState;
+import org.timecrafters.testing.states.SodiLEDState;
 import org.timecrafters.testing.states.SodiState;
 
 @TeleOp(name = "Wheel")
 public class SodiEngine extends CyberarmEngine {
     @Override
     public void setup() {
-        addState(new SodiState());
+//        addState(new SodiState());
+//        addState(new SodiLEDState());
+        addState(new LaserState());
     }
 }
diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/LaserState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/LaserState.java
new file mode 100644
index 0000000..349815d
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/LaserState.java
@@ -0,0 +1,37 @@
+package org.timecrafters.testing.states;
+
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.hardware.rev.RevTouchSensor;
+import com.qualcomm.robotcore.hardware.ColorSensor;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+public class LaserState extends CyberarmState {
+    Rev2mDistanceSensor laser;
+    RevTouchSensor Tack;
+    RevTouchSensor Mag;
+    ColorSensor Speck;
+
+
+    @Override
+    public void init() {
+        laser = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Zappy");
+        Tack = engine.hardwareMap.get(RevTouchSensor.class, "Pokey");
+        Mag = engine.hardwareMap.get(RevTouchSensor.class, "Electro");
+        Speck = engine.hardwareMap.get(ColorSensor.class, "Chroma");
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Laser Distance", laser.getDistance(DistanceUnit.CM));
+        engine.telemetry.addData("The Feels", Tack.isPressed());
+        engine.telemetry.addData("Eel", Mag.isPressed());
+        engine.telemetry.addData("Pretty Colors", ", Red " + Speck.red() + ", Green " + Speck.green() + ", Blue " + Speck.blue()  );
+    }
+
+    @Override
+    public void exec() {
+
+    }
+}
diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/SodiLEDState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/SodiLEDState.java
new file mode 100644
index 0000000..2a7b9cb
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/testing/states/SodiLEDState.java
@@ -0,0 +1,62 @@
+package org.timecrafters.testing.states;
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.minibots.cyberarm.engines.Common;
+
+public class SodiLEDState extends CyberarmState {
+    
+    RevBlinkinLedDriver LEDs;
+
+    @Override
+    public void init() {
+        LEDs = engine.hardwareMap.get(RevBlinkinLedDriver.class, "lights");
+        LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
+    }
+
+    @Override
+    public void exec() {
+
+        if (engine.gamepad1.a) {
+
+            LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
+
+        }
+        else if (engine.gamepad1.dpad_up) {
+
+            LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.CP1_LIGHT_CHASE);
+
+        }
+        if (engine.gamepad1.x) {
+
+            LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
+
+        }
+        else if (engine.gamepad1.dpad_up) {
+
+            LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.CP1_LIGHT_CHASE);
+
+        }
+        if (engine.gamepad1.y) {
+
+            LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW);
+
+        }
+        else if (engine.gamepad1.dpad_up) {
+
+            LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.CP1_LIGHT_CHASE);
+
+        }
+        if (engine.gamepad1.b) {
+
+            LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED);
+
+        }
+        else if (engine.gamepad1.dpad_up) {
+
+            LEDs.setPattern(RevBlinkinLedDriver.BlinkinPattern.CP1_LIGHT_CHASE);
+
+        }
+    }
+}