+ */
+ public abstract void setup();
+
+ /**
+ * Called when START button on Driver Station is pushed
+ * ENSURE to call super.start() if you override this method
+ */
+ public void start() {
+ if (cyberarmStates.size() > 0) {
+ runState(cyberarmStates.get(0));
+ }
+ }
+
+ /**
+ * Engine main loop
+ * ENSURE to call super.loop() if you override this method
+ */
+ public void loop() {
+ CyberarmState state;
+
+ // Try to set state to the current state, if it fails assume that there are no states to run
+ try {
+ state = cyberarmStates.get(activeStateIndex);
+ } catch(IndexOutOfBoundsException e) {
+ // The engine is now out of states.
+ stop();
+
+ telemetry.addLine("" + this.getClass().getSimpleName() + " is out of states to run!");
+ telemetry.addLine();
+ return;
+ }
+
+ // Add telemetry to show currently running state
+ telemetry.addLine("Running state: " +state.getClass().getSimpleName() + ". State: " + activeStateIndex + " of " + (cyberarmStates.size()-1));
+ if (showStateChildrenListInTelemetry && state.hasChildren()) {
+ for(CyberarmState child: state.children) {
+ telemetry.addLine(" Child: " + child.getClass().getSimpleName() + " [" + child.children.size() + "] grandchildren");
+ }
+ }
+ telemetry.addLine();
+
+ if (state.getHasFinished() && state.childrenHaveFinished()) {
+ activeStateIndex++;
+
+ try {
+ state = cyberarmStates.get(activeStateIndex);
+ runState(state);
+ } catch(IndexOutOfBoundsException e) { /* loop will handle this in a few milliseconds */ }
+
+ } else {
+ stateTelemetry(state);
+ }
+
+ gamepadCheckerGamepad1.update();
+ gamepadCheckerGamepad2.update();
+ }
+
+ /**
+ * Stops every known state
+ */
+ @Override
+ public void stop() {
+ for (CyberarmState state: cyberarmStates) {
+ stopState(state);
+ }
+
+
+ }
+
+ /**
+ * Recursively calls telemetry() on states
+ * @param state State to get telemetry
+ */
+ private void stateTelemetry(CyberarmState state) {
+ if (!state.getHasFinished()) {
+ state.telemetry();
+ }
+
+ for(CyberarmState childState : state.children) {
+ if (!childState.getHasFinished()) {
+ stateTelemetry(childState);
+ }
+ }
+ }
+
+ /**
+ * Called when INIT button on Driver Station is pressed
+ * Recursively initiates states
+ * @param state State to initiate
+ */
+ private void initState(CyberarmState state) {
+ state.init();
+
+ for(CyberarmState childState : state.children) {
+ initState(childState);
+ }
+ }
+
+ /**
+ * Called when programs ends or STOP button on Driver Station is pressed
+ * Recursively stop states
+ * @param state State to stop
+ */
+ public void stopState(CyberarmState state) {
+ state.setHasFinished(true);
+ state.stop();
+
+ for(CyberarmState childState : state.children) {
+ stopState(childState);
+ }
+ }
+
+ /**
+ * Recursively start up states
+ * @param state State to run
+ */
+ protected void runState(CyberarmState state) {
+ final CyberarmState finalState = state;
+// if (state.isRunning()) { return; } // Assume that we have already started running this state
+
+ new Thread(() -> {
+ finalState.prestart();
+ finalState.start();
+ finalState.startTime = System.currentTimeMillis();
+ finalState.run();
+ }).start();
+
+ for (CyberarmState kid : state.children) {
+ runState(kid);
+ }
+ }
+
+ /**
+ * Add state to queue, will call init() on state if engine is running
+ * @param state State to add to queue
+ */
+ public CyberarmState addState(CyberarmState state) {
+ Log.i(TAG, "Adding cyberarmState "+ state.getClass());
+ cyberarmStates.add(state);
+
+ if (isRunning()) { initState(state); }
+
+ return state;
+ }
+
+ /**
+ * Inserts state after the query state plus an offset to ensure logical insertion
+ * @param query State to add state after
+ * @param state State to be inserted
+ * @return CyberarmState
+ */
+ public CyberarmState insertState(CyberarmState query, CyberarmState state) {
+ int index = cyberarmStates.indexOf(query) + query.insertOffset;
+ Log.i(TAG, "Adding cyberarmState "+ state.getClass());
+
+ cyberarmStates.add(index, state);
+ query.insertOffset++;
+
+ if (isRunning()) { initState(state); }
+
+ return state;
+ }
+
+ private void buttonDownForStates(CyberarmState state, Gamepad gamepad, String button) {
+ state.buttonDown(gamepad, button);
+
+ for (CyberarmState child : state.children) {
+ child.buttonDown(gamepad, button);
+ }
+ }
+
+ private void buttonUpForStates(CyberarmState state, Gamepad gamepad, String button) {
+ state.buttonUp(gamepad, button);
+
+ for (CyberarmState child : state.children) {
+ child.buttonUp(gamepad, button);
+ }
+ }
+
+ /**
+ * Called by GamepadChecker when it detects that a gamepad button has been pressed
+ * @param gamepad Gamepad
+ * @param button String
+ */
+ protected void buttonDown(Gamepad gamepad, String button) {
+ try {
+ buttonDownForStates(cyberarmStates.get(activeStateIndex), gamepad, button);
+ } catch(IndexOutOfBoundsException e){
+ /* loop will handle this in a few milliseconds */
+ }
+ }
+
+ /**
+ * Called by GamepadChecker when it detects that a gamepad button has been released
+ * @param gamepad Gamepad
+ * @param button String
+ */
+ protected void buttonUp(Gamepad gamepad, String button) {
+ try {
+ buttonUpForStates(cyberarmStates.get(activeStateIndex), gamepad, button);
+ } catch(IndexOutOfBoundsException e){
+ /* loop will handle this in a few milliseconds */
+ }
+ }
+
+ /**
+ * This will return false while Engine.setup() is executing, and be true after.
+ * @return Whether the engine main loop is running
+ */
+ public boolean isRunning() {
+ return isRunning;
+ }
+
+ /**
+ *
+ * @return The index used to lookup the current state from cyberarmStates
+ */
+ public int getActiveStateIndex() {
+ return activeStateIndex;
+ }
+}
diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java
new file mode 100644
index 0000000..9d28f0d
--- /dev/null
+++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java
@@ -0,0 +1,235 @@
+package org.cyberarm.engine.V2;
+
+import android.util.Log;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import java.util.concurrent.CopyOnWriteArrayList;
+
+/**
+ * A State for use with CyberarmEngineV2
+ */
+public abstract class CyberarmState implements Runnable {
+
+ private volatile boolean isRunning, hasFinished;
+ public static String TAG = "PROGRAM.STATE";
+ public CyberarmEngine engine = CyberarmEngine.instance;
+ public CopyOnWriteArrayList children = new CopyOnWriteArrayList<>();
+ public long startTime = 0;
+ public int insertOffset = 1;
+
+ /**
+ * Called when INIT button on Driver Station is pushed
+ */
+ public void init() {
+ }
+
+ /**
+ * Called just before start to ensure state is in correct state
+ */
+ protected void prestart() {
+ isRunning = true;
+ }
+
+ /**
+ * Called when state has begin to run
+ */
+ public void start() {
+ }
+
+ /**
+ * Called while State is running
+ */
+ public abstract void exec();
+
+ /**
+ * State's main loop, calls exec() until hasFinished is true
+ * DO NO OVERRIDE
+ */
+ @Override
+ public void run() {
+ while (!hasFinished) {
+ exec();
+ }
+ isRunning = false;
+ }
+
+ /**
+ * Place telemetry calls in here instead of inside exec() to have them displayed correctly on the Driver Station
+ * (States update thousands of times per second, resulting in missing or weirdly formatted telemetry if telemetry is added in exec())
+ */
+ public void telemetry() {
+ }
+
+ /**
+ * Called when Engine is finished
+ */
+ public void stop() {
+ }
+
+ /**
+ * Called when GamepadChecker detects that a gamepad button has been pressed
+ * @param gamepad Gamepad
+ * @param button String
+ */
+ public void buttonDown(Gamepad gamepad, String button) {
+ }
+
+
+ /**
+ * Called when GamepadChecker detects that a gamepad button has been released
+ * @param gamepad Gamepad
+ * @param button String
+ */
+ public void buttonUp(Gamepad gamepad, String button) {
+ }
+
+ /**
+ * Add a state which runs in parallel with this one
+ */
+ public CyberarmState addParallelState(CyberarmState state) {
+ Log.i(TAG, "Adding " + state.getClass() + " to " + this.getClass());
+ children.add(state);
+
+ if (isRunning()) {
+ state.init();
+ engine.runState(state);
+ Log.i(TAG, "Started " + state.getClass() + " in " + this.getClass());
+ }
+
+ return state;
+ }
+
+ /**
+ * Add a state to engine which will run after this one finishes
+ */
+ public CyberarmState addState(CyberarmState state) {
+ engine.insertState(this, state);
+
+ return state;
+ }
+
+ /**
+ * Returns whether or not state has children
+ * @return True if state has children, false otherwise
+ */
+ public boolean hasChildren() {
+ return (children.size() > 0);
+ }
+
+ /**
+ * Have all of the states children finished running themselves?
+ * @return Whether or not all children have finished running
+ */
+ public boolean childrenHaveFinished() {
+ return childrenHaveFinished(children);
+ }
+
+ /**
+ * Have all of the states children finished running themselves?
+ * @param kids CopyOnWriteArrayList of children to check for hasFinished()
+ * @return Whether or not all children have finished running
+ */
+ public boolean childrenHaveFinished(CopyOnWriteArrayList kids) {
+ boolean allDone = true;
+
+ for (CyberarmState state : kids) {
+ if (!state.hasFinished) {
+ allDone = false;
+ break;
+ } else {
+ if (!state.childrenHaveFinished()) {
+ allDone = false;
+ break;
+ }
+ }
+ }
+
+ return allDone;
+ }
+
+ /**
+ *
+ * @return The number of milliseconds this state has been running for
+ */
+ public double runTime() {
+ return (System.currentTimeMillis() - startTime);
+ }
+
+ /**
+ * Set whether state has finished or not
+ * @param value boolean
+ */
+ public void setHasFinished(boolean value) {
+ hasFinished = value;
+ }
+
+ /**
+ *
+ * @return Get value of hasFinished
+ */
+ public boolean getHasFinished() {
+ return hasFinished;
+ }
+
+ /**
+ *
+ * @return Get value of isRunning
+ */
+ public boolean isRunning() {
+ return isRunning;
+ }
+
+ /**
+ *
+ * @param timems How long to sleep in milliseconds
+ */
+ public void sleep(long timems) {
+ try {
+ Thread.sleep(timems);
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ }
+ }
+
+ /**
+ *
+ * @param width How many characters wide to be
+ * @param percentCompleted Number between 0.0 and 100.0
+ * @param bar What character to draw the completion bar with
+ * @param padding What character to draw non-completed bar with
+ * @return A string
+ */
+ public String progressBar(int width, double percentCompleted, String bar, String padding) {
+ String percentCompletedString = "" + Math.round(percentCompleted) + "%";
+ double activeWidth = (width - 2) - percentCompletedString.length();
+
+ String string = "[";
+ double completed = (percentCompleted / 100.0) * activeWidth;
+
+ for (int i = 0; i <= ((int) activeWidth); i++) {
+ if (i == ((int) activeWidth) / 2) {
+ string += percentCompletedString;
+ } else {
+ if (i <= (int) completed && (int) completed > 0) {
+ string += bar;
+ } else {
+ string += padding;
+ }
+ }
+ }
+
+ string += "]";
+ return string;
+ }
+
+ /**
+ *
+ * @param width How many characters wide to be
+ * @param percentCompleted Number between 0.0 and 100.0
+ * @return A string
+ */
+ public String progressBar(int width, double percentCompleted) {
+ return progressBar(width, percentCompleted, "=", " ");
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java
new file mode 100644
index 0000000..8a82ffa
--- /dev/null
+++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java
@@ -0,0 +1,61 @@
+package org.cyberarm.engine.V2;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import java.lang.reflect.Field;
+import java.util.HashMap;
+
+public class GamepadChecker {
+ private final CyberarmEngine engine;
+ private final Gamepad gamepad;
+ private final HashMap buttons = new HashMap<>();
+
+ public GamepadChecker(CyberarmEngine engine, Gamepad gamepad) {
+ this.engine = engine;
+ this.gamepad = gamepad;
+
+ buttons.put("a", false);
+ buttons.put("b", false);
+ buttons.put("x", false);
+ buttons.put("y", false);
+
+ buttons.put("start", false);
+ buttons.put("guide", false);
+ buttons.put("back", false);
+
+ buttons.put("left_bumper", false);
+ buttons.put("right_bumper", false);
+
+ buttons.put("left_stick_button", false);
+ buttons.put("right_stick_button", false);
+
+ buttons.put("dpad_left", false);
+ buttons.put("dpad_right", false);
+ buttons.put("dpad_up", false);
+ buttons.put("dpad_down", false);
+ }
+
+ public void update() {
+ for (String btn : buttons.keySet()) {
+ try {
+ Field field = gamepad.getClass().getDeclaredField(btn);
+
+ if (field.getBoolean(gamepad)) {
+ if (!buttons.get(btn)) {
+ engine.buttonDown(gamepad, btn);
+ }
+
+ buttons.put(btn, true);
+ } else {
+ if (buttons.get(btn)) {
+ engine.buttonUp(gamepad, btn);
+ }
+
+ buttons.put(btn, false);
+ }
+ } catch (NoSuchFieldException|IllegalAccessException e) {
+ e.printStackTrace();
+ }
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/README.txt b/TeamCode/src/main/java/org/cyberarm/engine/V2/README.txt
new file mode 100644
index 0000000..7632dc5
--- /dev/null
+++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/README.txt
@@ -0,0 +1,28 @@
+CyberarmEngine V2 Architecture
+
+Engine
+ -> [States]
+ -> [ParallelStates]
+
+Start with an Engine and override setup():
+
+public class Engine extends CyberarmEngineV2 {
+ public void setup() {
+ addState(new State(arguments));
+ }
+}
+
+NOTE: states do not need to be passed the instance of Engine as they have a field 'cyberarmEngine'
+ which is set to CyberarmEngineV2.instance when they are created.
+
+States can have 'children' which are also States, which run in parallel with their parent. There is
+no fixed limit to how many grandchildren can exist (Children themselves can have children.):
+
+public class State extends CyberarmEngineStateV2 {
+ public init() {
+ addParallelState(new ParallelState(arguments));
+ }
+
+ public exec() {
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
deleted file mode 100644
index 2f7e3a4..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
+++ /dev/null
@@ -1,121 +0,0 @@
-## TeamCode Module
-
-Welcome!
-
-This module, TeamCode, is the place where you will write/paste the code for your team's
-robot controller App. This module is currently empty (a clean slate) but the
-process for adding OpModes is straightforward.
-
-## Creating your own OpModes
-
-The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
-
-Sample opmodes exist in the FtcRobotController module.
-To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
-
-Expand the following tree elements:
- FtcRobotController / java / org.firstinspires.ftc.robotcontroller / external / samples
-
-A range of different samples classes can be seen in this folder.
-The class names follow a naming convention which indicates the purpose of each class.
-The full description of this convention is found in the samples/sample_convention.md file.
-
-A brief synopsis of the naming convention is given here:
-The prefix of the name will be one of the following:
-
-* Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
- of a particular style of OpMode. These are bare bones examples.
-* Sensor: This is a Sample OpMode that shows how to use a specific sensor.
- It is not intended as a functioning robot, it is simply showing the minimal code
- required to read and display the sensor values.
-* Hardware: This is not an actual OpMode, but a helper class that is used to describe
- one particular robot's hardware devices: eg: for a Pushbot. Look at any
- Pushbot sample to see how this can be used in an OpMode.
- Teams can copy one of these to create their own robot definition.
-* Pushbot: This is a Sample OpMode that uses the Pushbot robot structure as a base.
-* Concept: This is a sample OpMode that illustrates performing a specific function or concept.
- These may be complex, but their operation should be explained clearly in the comments,
- or the header should reference an external doc, guide or tutorial.
-* Library: This is a class, or set of classes used to implement some strategy.
- These will typically NOT implement a full OpMode. Instead they will be included
- by an OpMode to provide some stand-alone capability.
-
-Once you are familiar with the range of samples available, you can choose one to be the
-basis for your own robot. In all cases, the desired sample(s) needs to be copied into
-your TeamCode module to be used.
-
-This is done inside Android Studio directly, using the following steps:
-
- 1) Locate the desired sample class in the Project/Android tree.
-
- 2) Right click on the sample class and select "Copy"
-
- 3) Expand the TeamCode / java folder
-
- 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
-
- 5) You will be prompted for a class name for the copy.
- Choose something meaningful based on the purpose of this class.
- Start with a capital letter, and remember that there may be more similar classes later.
-
-Once your copy has been created, you should prepare it for use on your robot.
-This is done by adjusting the OpMode's name, and enabling it to be displayed on the
-Driver Station's OpMode list.
-
-Each OpMode sample class begins with several lines of code like the ones shown below:
-
-```
- @TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
- @Disabled
-```
-
-The name that will appear on the driver station's "opmode list" is defined by the code:
- ``name="Template: Linear OpMode"``
-You can change what appears between the quotes to better describe your opmode.
-The "group=" portion of the code can be used to help organize your list of OpModes.
-
-As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
- ``@Disabled`` annotation which has been included.
-This line can simply be deleted , or commented out, to make the OpMode visible.
-
-
-
-## ADVANCED Multi-Team App management: Cloning the TeamCode Module
-
-In some situations, you have multiple teams in your club and you want them to all share
-a common code organization, with each being able to *see* the others code but each having
-their own team module with their own code that they maintain themselves.
-
-In this situation, you might wish to clone the TeamCode module, once for each of these teams.
-Each of the clones would then appear along side each other in the Android Studio module list,
-together with the FtcRobotController module (and the original TeamCode module).
-
-Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
-prior to clicking to the green Run arrow.
-
-Warning: This is not for the inexperienced Software developer.
-You will need to be comfortable with File manipulations and managing Android Studio Modules.
-These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
-
-Also.. Make a full project backup before you start this :)
-
-To clone TeamCode, do the following:
-
-Note: Some names start with "Team" and others start with "team". This is intentional.
-
-1) Using your operating system file management tools, copy the whole "TeamCode"
- folder to a sibling folder with a corresponding new name, eg: "Team0417".
-
-2) In the new Team0417 folder, delete the TeamCode.iml file.
-
-3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
- to a matching name with a lowercase 'team' eg: "team0417".
-
-4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
- package="org.firstinspires.ftc.teamcode"
- to be
- package="org.firstinspires.ftc.team0417"
-
-5) Add: include ':Team0417' to the "/settings.gradle" file.
-
-6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/MecanumMinibot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/MecanumMinibot.java
new file mode 100644
index 0000000..24ea658
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/MecanumMinibot.java
@@ -0,0 +1,85 @@
+package org.timecrafters.minibots.cyberarm;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+
+public class MecanumMinibot {
+ public static final int TURN_LEFT = 0;
+ public static final int TURN_RIGHT = 1;
+ public static final int STRAFE_LEFT = 0;
+ public static final int STRAFE_RIGHT = 1;
+
+ private CyberarmEngine engine;
+
+ public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
+
+ public MecanumMinibot(CyberarmEngine engine) {
+ this.engine = engine;
+
+ setupDrivetrain();
+ }
+
+ private void setupDrivetrain() {
+ frontLeftDrive = engine.hardwareMap.dcMotor.get("frontLeft");
+ frontRightDrive = engine.hardwareMap.dcMotor.get("frontRight");
+ backLeftDrive = engine.hardwareMap.dcMotor.get("backLeft");
+ backRightDrive = engine.hardwareMap.dcMotor.get("backRight");
+
+ frontLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
+ frontRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
+ backLeftDrive.setDirection(DcMotorSimple.Direction.REVERSE);
+ backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
+ }
+
+ public void driveAll(double power) {
+ frontLeftDrive.setPower(power);
+ frontRightDrive.setPower(power);
+ backLeftDrive.setPower(power);
+ backRightDrive.setPower(power);
+ }
+
+ public void driveStop() {
+ driveAll(0);
+ }
+
+ public void driveTurn(int direction, double power) {
+ if (direction == TURN_LEFT) {
+ frontLeftDrive.setPower(-power);
+ backLeftDrive.setPower(-power);
+
+ frontRightDrive.setPower(power);
+ backRightDrive.setPower(power);
+
+ } else if (direction == TURN_RIGHT) {
+ frontLeftDrive.setPower(power);
+ backLeftDrive.setPower(power);
+
+ frontRightDrive.setPower(-power);
+ backRightDrive.setPower(-power);
+ } else {
+ throw new RuntimeException("Invalid direction for driveTurn()");
+ }
+ }
+
+ public void driveStrafe(int direction, double power) {
+ if (direction == STRAFE_LEFT) {
+ frontLeftDrive.setPower(power);
+ frontRightDrive.setPower(-power);
+
+ backLeftDrive.setPower(-power);
+ backRightDrive.setPower(power);
+
+ } else if (direction == STRAFE_RIGHT) {
+ frontLeftDrive.setPower(-power);
+ frontRightDrive.setPower(power);
+
+ backLeftDrive.setPower(power);
+ backRightDrive.setPower(-power);
+ } else {
+ throw new RuntimeException("Invalid direction for driveStrafe()");
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/MecanumMinibotTeleOpEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/MecanumMinibotTeleOpEngine.java
new file mode 100644
index 0000000..4e8df45
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/MecanumMinibotTeleOpEngine.java
@@ -0,0 +1,19 @@
+package org.timecrafters.minibots.cyberarm.engines;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.minibots.cyberarm.MecanumMinibot;
+import org.timecrafters.minibots.cyberarm.states.MecanumMinibotTeleOpState;
+
+@TeleOp(name = "MecanumMinibot TeleOp", group = "minibot")
+public class MecanumMinibotTeleOpEngine extends CyberarmEngine {
+ MecanumMinibot robot;
+
+ @Override
+ public void setup() {
+ robot = new MecanumMinibot(this);
+
+ addState(new MecanumMinibotTeleOpState(robot));
+ }
+}
diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java
new file mode 100644
index 0000000..adc2ff8
--- /dev/null
+++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java
@@ -0,0 +1,54 @@
+package org.timecrafters.minibots.cyberarm.states;
+
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.cyberarm.engine.V2.CyberarmState;
+import org.timecrafters.minibots.cyberarm.MecanumMinibot;
+
+public class MecanumMinibotTeleOpState extends CyberarmState {
+ private final MecanumMinibot robot;
+ private float speed;
+
+ public MecanumMinibotTeleOpState(MecanumMinibot robot) {
+ this.robot = robot;
+ }
+
+ @Override
+ public void exec() {
+ speed = 1.0f - engine.gamepad1.left_trigger;
+
+ if (engine.gamepad1.y) {
+ robot.driveAll(speed);
+ } else if (engine.gamepad1.a) {
+ robot.driveAll(-speed);
+ } else if (engine.gamepad1.x) {
+ robot.driveStrafe(MecanumMinibot.STRAFE_LEFT, speed);
+ } else if (engine.gamepad1.b) {
+ robot.driveStrafe(MecanumMinibot.STRAFE_RIGHT, speed);
+
+ } else if (engine.gamepad1.left_bumper) {
+ robot.driveTurn(MecanumMinibot.TURN_LEFT, speed);
+
+ } else if (engine.gamepad1.right_bumper) {
+ robot.driveTurn(MecanumMinibot.TURN_RIGHT, speed);
+ } else {
+ robot.driveStop();
+ }
+ }
+
+ @Override
+ public void buttonDown(Gamepad gamepad, String button) {
+ super.buttonDown(gamepad, button);
+ }
+
+ @Override
+ public void buttonUp(Gamepad gamepad, String button) {
+ super.buttonUp(gamepad, button);
+ }
+
+ @Override
+ public void telemetry() {
+ engine.telemetry.addData("speed", speed);
+ }
+}