From be2dbd24a8bcb2dc7274468d8636058cd4bbcb85 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Tue, 25 Jan 2022 17:30:15 -0600 Subject: [PATCH] Setup new repo, implemented basic mecanum drive for minibot --- .idea/.gitignore | 3 + .idea/compiler.xml | 6 + .idea/jarRepositories.xml | 30 ++ .idea/misc.xml | 9 + .../cyberarm/engine/V2/CyberarmEngine.java | 278 ++++++++++++++++++ .../org/cyberarm/engine/V2/CyberarmState.java | 235 +++++++++++++++ .../cyberarm/engine/V2/GamepadChecker.java | 61 ++++ .../java/org/cyberarm/engine/V2/README.txt | 28 ++ .../org/firstinspires/ftc/teamcode/readme.md | 121 -------- .../minibots/cyberarm/MecanumMinibot.java | 85 ++++++ .../engines/MecanumMinibotTeleOpEngine.java | 19 ++ .../states/MecanumMinibotTeleOpState.java | 54 ++++ 12 files changed, 808 insertions(+), 121 deletions(-) create mode 100644 .idea/.gitignore create mode 100644 .idea/compiler.xml create mode 100644 .idea/jarRepositories.xml create mode 100644 .idea/misc.xml create mode 100644 TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java create mode 100644 TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java create mode 100644 TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java create mode 100644 TeamCode/src/main/java/org/cyberarm/engine/V2/README.txt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/MecanumMinibot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/MecanumMinibotTeleOpEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/compiler.xml b/.idea/compiler.xml new file mode 100644 index 0000000..fb7f4a8 --- /dev/null +++ b/.idea/compiler.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.idea/jarRepositories.xml b/.idea/jarRepositories.xml new file mode 100644 index 0000000..0380d8d --- /dev/null +++ b/.idea/jarRepositories.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..9422e84 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,9 @@ + + + + + + + + \ No newline at end of file diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java new file mode 100644 index 0000000..b96d034 --- /dev/null +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -0,0 +1,278 @@ +package org.cyberarm.engine.V2; + +import android.util.Log; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.Gamepad; + +import java.util.concurrent.CopyOnWriteArrayList; + +/** + * CyberarmEngine Version 2.0 | October 26th 2018 + * AN Experimental reimplementation of GoldfishPi's original Engine system. + * Designed to be easily maintainable, extensible, and easy to understand. + */ +public abstract class CyberarmEngine extends OpMode { + + public static CyberarmEngine instance; + //Array To Hold States + final private CopyOnWriteArrayList cyberarmStates = new CopyOnWriteArrayList<>(); + private int activeStateIndex = 0; + private boolean isRunning; + + private final static String TAG = "PROGRAM.ENGINE"; + public boolean showStateChildrenListInTelemetry = false; + + private GamepadChecker gamepadCheckerGamepad1, gamepadCheckerGamepad2; + + /** + * Called when INIT button on Driver Station is pushed + * ENSURE to call super.init() if you override this method + */ + public void init() { + CyberarmEngine.instance = this; + isRunning = false; + gamepadCheckerGamepad1 = new GamepadChecker(this, gamepad1); + gamepadCheckerGamepad2 = new GamepadChecker(this, gamepad2); + + setup(); + + isRunning = true; + + for (CyberarmState state: cyberarmStates) { + initState(state); + } + } + + /** + * Setup states for engine to use + * For example: + * 
+   * {@code
+   *   public void setup() {
+   *     addState(new TestState());
+   *     addState(new AnotherState(100, 500));
+   *   }
+   * }
+   *
+ */ + 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); + } +}