From 63602ecc145c15716ca5233eddd0d9b259569051 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 11 Oct 2021 17:12:45 -0500 Subject: [PATCH] Initial Commit; Stubbed directories for FreightFrenzy, included source of UltimateGoal for quick reference, including TimeCraftersConfigurationTool library until I get it published to maven, disabled UltimateGoal engines. --- .../org/cyberarm/container/InputChecker.java | 58 ++ .../cyberarm/engine/V2/CyberarmEngine.java | 231 ++++++ .../org/cyberarm/engine/V2/CyberarmState.java | 216 +++++ .../java/org/cyberarm/engine/V2/README.txt | 28 + .../org/firstinspires/ftc/teamcode/readme.md | 121 --- .../Calibration/Engines/.gitkeep | 0 .../FreightFrenzy/Calibration/States/.gitkeep | 0 .../Competition/Autonomous/Engines/.gitkeep | 0 .../Autonomous/Engines/AutonomousEngine.java | 11 + .../Competition/Autonomous/States/.gitkeep | 0 .../Autonomous/States/StubState.java | 9 + .../Competition/Common/Robot.java | 4 + .../Competition/Common/States/.gitkeep | 0 .../Competition/TeleOp/Engines/.gitkeep | 0 .../Competition/TeleOp/States/.gitkeep | 0 .../Engines/ISO8061Engine.java | 14 + .../HardwareTesting/States/ISO8061State.java | 15 + .../library/TimeCraftersConfiguration.java | 150 ++++ .../library/backend/Config.java | 44 + .../library/backend/Settings.java | 12 + .../library/backend/TAC.java | 14 + .../library/backend/config/Action.java | 18 + .../library/backend/config/Configuration.java | 20 + .../library/backend/config/Group.java | 27 + .../library/backend/config/Presets.java | 22 + .../library/backend/config/Variable.java | 92 +++ .../serializers/ActionDeserializer.java | 33 + .../library/serializers/ActionSerializer.java | 27 + .../serializers/ConfigDeserializer.java | 34 + .../library/serializers/ConfigSerializer.java | 32 + .../ConfigurationDeserializer.java | 36 + .../serializers/ConfigurationSerializer.java | 29 + .../serializers/GroupDeserializer.java | 33 + .../library/serializers/GroupSerializer.java | 26 + .../serializers/PresetsDeserializer.java | 33 + .../serializers/PresetsSerializer.java | 25 + .../serializers/SettingsDeserializer.java | 25 + .../serializers/SettingsSerializer.java | 30 + .../serializers/VariableDeserializer.java | 28 + .../serializers/VariableSerializer.java | 23 + .../Calibration/CalibrateRingBeltLoop.java | 58 ++ .../Calibration/CalibrationEngine.java | 25 + .../Calibration/MechanumBiasCalibrator.java | 82 ++ .../UltimateGoal/Calibration/MotorCheck.java | 59 ++ .../Calibration/OdometryCalibration.java | 74 ++ .../Calibration/PerformanceTest.java | 32 + .../Competition/Autonomous/AutoEngine.java | 122 +++ .../Autonomous/DriveToCoordinates.java | 141 ++++ .../Autonomous/DriveWithColorSensor.java | 46 ++ .../Competition/Autonomous/Face.java | 95 +++ .../Competition/Autonomous/FaceAndDrive.java | 56 ++ .../Autonomous/FindWobbleGoal.java | 109 +++ .../Autonomous/LaunchDriveControl.java | 41 + .../Competition/Autonomous/Park.java | 50 ++ .../Autonomous/TensorFlowCheck.java | 94 +++ .../Competition/Autonomous/ThreadStates.java | 44 + .../Competition/Demo/DemoEngine.java | 36 + .../Competition/Demo/DemoEngineTank.java | 35 + .../Competition/Demo/DemoState.java | 194 +++++ .../Competition/Demo/DemoStateTank.java | 55 ++ .../UltimateGoal/Competition/DriveMotor.java | 45 ++ .../UltimateGoal/Competition/Launch.java | 104 +++ .../UltimateGoal/Competition/Pause.java | 50 ++ .../Competition/PreInit/FindLimitSwitch.java | 26 + .../Competition/PreInit/LoadRings.java | 35 + .../Competition/PreInit/PreInitEngine.java | 26 + .../Competition/ProgressRingBelt.java | 82 ++ .../Competition/ResetRingBelt.java | 57 ++ .../UltimateGoal/Competition/Robot.java | 757 ++++++++++++++++++ .../Competition/TeleOp/LaunchControl.java | 57 ++ .../Competition/TeleOp/LaunchSingle.java | 52 ++ .../Competition/TeleOp/Player1.java | 260 ++++++ .../Competition/TeleOp/Player2.java | 189 +++++ .../Competition/TeleOp/TeleOpEngine.java | 38 + .../Competition/TeleOp/TeleOpState.java | 43 + .../Competition/TeleOp/powerShotsControl.java | 80 ++ .../Competition/UnstickRingBelt.java | 37 + .../UltimateGoal/Competition/WobbleArm.java | 55 ++ .../UltimateGoal/Competition/WobbleGrab.java | 59 ++ .../HardwareTesting/ControlHubTest.java | 76 ++ .../HardwareTesting/DistanceSensorTest.java | 37 + .../UltimateGoal/HardwareTesting/LEDTest.java | 28 + .../HardwareTesting/MecanumFunctionTest.java | 137 ++++ .../HardwareTesting/ServoPosTest.java | 35 + .../HardwareTesting/TestingEngine.java | 19 + .../HardwareTesting/WobbleArmTest.java | 30 + 86 files changed, 5261 insertions(+), 121 deletions(-) create mode 100644 TeamCode/src/main/java/org/cyberarm/container/InputChecker.java 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/README.txt delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Calibration/Engines/.gitkeep create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Calibration/States/.gitkeep create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/.gitkeep create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/AutonomousEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/.gitkeep create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/StubState.java create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/States/.gitkeep create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/Engines/.gitkeep create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/.gitkeep create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/ISO8061Engine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/ISO8061State.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Settings.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/TAC.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Action.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Configuration.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Group.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Presets.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Variable.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionDeserializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionSerializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigDeserializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigSerializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationDeserializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationSerializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupDeserializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupSerializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsDeserializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsSerializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsDeserializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsSerializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableDeserializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableSerializer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FaceAndDrive.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WobbleArmTest.java diff --git a/TeamCode/src/main/java/org/cyberarm/container/InputChecker.java b/TeamCode/src/main/java/org/cyberarm/container/InputChecker.java new file mode 100644 index 0000000..438e476 --- /dev/null +++ b/TeamCode/src/main/java/org/cyberarm/container/InputChecker.java @@ -0,0 +1,58 @@ +package org.cyberarm.container; + +import android.util.Log; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import java.lang.reflect.Field; +import java.util.ArrayList; +import java.util.HashMap; + +public class InputChecker { + private Gamepad gamepad; + private HashMap buttons; + private ArrayList buttonList; + private byte NULL = 0, + PRESSED = 1, + RELEASED= 2; + public InputChecker(Gamepad gamepad) { + this.gamepad = gamepad; + buttons = new HashMap<>(); + buttonList = new ArrayList<>(); + + buttonList.add("a"); buttonList.add("b"); buttonList.add("x"); buttonList.add("y"); + buttonList.add("start"); buttonList.add("guide"); buttonList.add("back"); + buttonList.add("left_bumper"); buttonList.add("right_bumper"); + buttonList.add("left_stick_button"); buttonList.add("right_stick_button"); + buttonList.add("dpad_left"); buttonList.add("dpad_right"); + buttonList.add("dpad_up"); buttonList.add("dpad_down"); + } + + public void update() { + for (int i = 0; i < buttonList.size(); i++) { + try { + Field field = gamepad.getClass().getDeclaredField(buttonList.get(i)); + + if (field.getBoolean(gamepad)) { + buttons.put(buttonList.get(i), PRESSED); + } else { + if (buttons.get(buttonList.get(i)) != null && buttons.get(buttonList.get(i)) == PRESSED) { + buttons.put(buttonList.get(i), RELEASED); + } + } + } catch (NoSuchFieldException|IllegalAccessException e) { + e.printStackTrace(); + } + } + } + + public boolean check(String button) { + boolean state = false; + if (buttons.containsKey(button) && buttons.get(button) == RELEASED) { + Log.d("InputChecker","button \""+button+"\" has been released on \"gamepad"+gamepad.getGamepadId()+"\""); + state = true; + buttons.put(button, NULL); + } + return state; + } +} 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..1ac173a --- /dev/null +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -0,0 +1,231 @@ +package org.cyberarm.engine.V2; + +import android.util.Log; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +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 + private CopyOnWriteArrayList cyberarmStates = new CopyOnWriteArrayList<>(); + private int activeStateIndex = 0; + private boolean isRunning; + + private static String TAG = "PROGRAM.ENGINE"; + public boolean showStateChildrenListInTelemetry = false; + + /** + * 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; + + setup(); + + isRunning = true; + + for (CyberarmState state: cyberarmStates) { + initState(state); + } + } + + /** + * Setup states for engine to use + * For example: + * 
+   * @
+   *   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); + } + } + + /** + * 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(new Runnable() { + @Override + public void run() { + 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 + */ + 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; + } + + /** + * 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..b136a49 --- /dev/null +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java @@ -0,0 +1,216 @@ +package org.cyberarm.engine.V2; + +import android.util.Log; + +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() { + } + + /** + * 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 + */ + 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/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/FreightFrenzy/Calibration/Engines/.gitkeep b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Calibration/Engines/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Calibration/States/.gitkeep b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Calibration/States/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/.gitkeep b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/AutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/AutonomousEngine.java new file mode 100644 index 0000000..2b99171 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/AutonomousEngine.java @@ -0,0 +1,11 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.StubState; + +public class AutonomousEngine extends CyberarmEngine { + @Override + public void setup() { + addState(new StubState()); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/.gitkeep b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/StubState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/StubState.java new file mode 100644 index 0000000..2510663 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/StubState.java @@ -0,0 +1,9 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; + +public class StubState extends CyberarmState { + @Override + public void exec() { + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java new file mode 100644 index 0000000..990677c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java @@ -0,0 +1,4 @@ +package org.timecrafters.FreightFrenzy.Competition.Common; + +public class Robot { +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/States/.gitkeep b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/States/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/Engines/.gitkeep b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/Engines/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/.gitkeep b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/ISO8061Engine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/ISO8061Engine.java new file mode 100644 index 0000000..4cbd148 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/Engines/ISO8061Engine.java @@ -0,0 +1,14 @@ +package org.timecrafters.FreightFrenzy.HardwareTesting.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.HardwareTesting.States.ISO8061State; + +@TeleOp(name = "ISO8061-Is-Best", group = "testing") +public class ISO8061Engine extends CyberarmEngine { + @Override + public void setup() { + addState(new ISO8061State()); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/ISO8061State.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/ISO8061State.java new file mode 100644 index 0000000..381bebe --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/HardwareTesting/States/ISO8061State.java @@ -0,0 +1,15 @@ +package org.timecrafters.FreightFrenzy.HardwareTesting.States; + +import org.cyberarm.engine.V2.CyberarmState; + +public class ISO8061State extends CyberarmState { + @Override + public void exec() { + } + + @Override + public void telemetry() { + engine.telemetry.addLine("The ISO8061 Date Format is Best"); + engine.telemetry.addLine("YYYY-MM-DD"); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java new file mode 100644 index 0000000..5c52a36 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java @@ -0,0 +1,150 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library; + +import com.google.gson.Gson; +import com.google.gson.GsonBuilder; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.TAC; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ActionDeserializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ActionSerializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigDeserializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigSerializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigurationDeserializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigurationSerializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.GroupDeserializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.GroupSerializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.PresetsDeserializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.PresetsSerializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.SettingsDeserializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.SettingsSerializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.VariableDeserializer; +import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.VariableSerializer; + +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileReader; + +public class TimeCraftersConfiguration { + private static final String TAG = "TCT|TCConfig"; + private Config config; + + public TimeCraftersConfiguration() { + Settings settings = loadSettings(); + this.config = loadConfig(settings.config); + } + + public TimeCraftersConfiguration(String configName) { + this.config = loadConfig(configName); + } + + public Config getConfig() { + return config; + } + + public Group group(String groupName) { + for (final Group group : config.getGroups()) { + if (group.name.trim().equals(groupName.trim())) { + return group; + } + } + + throw(new RuntimeException("Failed to find a group named:\"" + groupName.trim() + "\" in config \"" + config.getName() + "\"")); + } + + public Action action(String groupName, String actionName) { + final Group group = group(groupName); + + for (Action action : group.getActions()) { + if (action.name.trim().equals(actionName.trim())) { + return action; + } + } + + throw(new RuntimeException("Failed to find an action named:\"" + actionName.trim() + "\" in group \"" + groupName.trim() + "\" in config \"" + config.getName() + "\"")); + } + + public Variable variable(String groupName, String actionName, String variableName) { + final Action action = action(groupName, actionName); + + for (Variable variable : action.getVariables()) { + if (variable.name.trim().equals(variableName.trim())) { + return variable; + } + } + + throw(new RuntimeException("Failed to find a variable named \"" + variableName.trim() + "\" in action:\"" + actionName.trim() + + "\" in group \"" + groupName.trim() + "\" in config \"" + config.getName() + "\"")); + } + + private Settings loadSettings() { + File settingsFile = new File(TAC.SETTINGS_PATH); + + if (!settingsFile.exists()) { + throw( new RuntimeException("Unable to load settings.json, file does not exist!") ); + } + + try { + return gsonForSettings().fromJson(new FileReader(settingsFile), Settings.class); + } catch (FileNotFoundException e) { + throw( new RuntimeException("Unable to load settings.json") ); + } + } + + private Config loadConfig(String name) { + if (name.equals("")) { + throw(new RuntimeException("Cannot load a config with an empty name!")); + } + + String path = TAC.CONFIGS_PATH + File.separator + name + ".json"; + File configFile = new File(path); + + if (configFile.exists() && configFile.isFile()) { + try { + Config config = gsonForConfig().fromJson(new FileReader(configFile), Config.class); + config.setName(name); + + return config; + } catch (FileNotFoundException e) { + e.printStackTrace(); + throw(new RuntimeException("Unable to find a config file named \"" + name + "\"")); + } + } else { + throw(new RuntimeException("Unable to find a config file named \"" + name + "\"")); + } + } + + private Gson gsonForSettings() { + return new GsonBuilder() + .registerTypeAdapter(Settings.class, new SettingsSerializer()) + .registerTypeAdapter(Settings.class, new SettingsDeserializer()) + .create(); + } + + public Gson gsonForConfig() { + return new GsonBuilder() + .registerTypeAdapter(Config.class, new ConfigSerializer()) + .registerTypeAdapter(Config.class, new ConfigDeserializer()) + + .registerTypeAdapter(Configuration.class, new ConfigurationSerializer()) + .registerTypeAdapter(Configuration.class, new ConfigurationDeserializer()) + + .registerTypeAdapter(Group.class, new GroupSerializer()) + .registerTypeAdapter(Group.class, new GroupDeserializer()) + + .registerTypeAdapter(Action.class, new ActionSerializer()) + .registerTypeAdapter(Action.class, new ActionDeserializer()) + + .registerTypeAdapter(Variable.class, new VariableSerializer()) + .registerTypeAdapter(Variable.class, new VariableDeserializer()) + + .registerTypeAdapter(Presets.class, new PresetsSerializer()) + .registerTypeAdapter(Presets.class, new PresetsDeserializer()) + .create(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java new file mode 100644 index 0000000..8054c72 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java @@ -0,0 +1,44 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.backend; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets; + +import java.util.ArrayList; +import java.util.Date; + +public class Config { + private String name; + private Configuration configuration; + private ArrayList groups; + private Presets presets; + + public Config(String name) { + this.name = name; + this.configuration = new Configuration(new Date(), new Date(), TAC.CONFIG_SPEC_VERSION, 0); + groups = new ArrayList<>(); + presets = new Presets(new ArrayList(), new ArrayList()); + } + + public Config(Configuration configuration, ArrayList groups, Presets presets) { + this.configuration = configuration; + this.groups = groups; + this.presets = presets; + } + + public String getName() { return name; } + public void setName(String name) { this.name = name; } + + public Configuration getConfiguration() { + return configuration; + } + + public Presets getPresets() { + return presets; + } + + public ArrayList getGroups() { + return groups; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Settings.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Settings.java new file mode 100644 index 0000000..01d3a2d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Settings.java @@ -0,0 +1,12 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.backend; + +public class Settings { + public String hostname, config; + public int port; + + public Settings(String hostname, int port, String config) { + this.hostname = hostname; + this.port = port; + this.config = config; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/TAC.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/TAC.java new file mode 100644 index 0000000..2490f09 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/TAC.java @@ -0,0 +1,14 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.backend; + +import android.os.Environment; + +import java.io.File; + +public class TAC { + // TODO: Update filesystem handling + public static final String ROOT_PATH = Environment.getExternalStorageDirectory().getAbsolutePath() + File.separator + "TimeCrafters_Configuration_Tool", + CONFIGS_PATH = ROOT_PATH + File.separator + "/configs", + SETTINGS_PATH = ROOT_PATH + File.separator + "settings.json"; + + public static final int CONFIG_SPEC_VERSION = 2; +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Action.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Action.java new file mode 100644 index 0000000..7d3eede --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Action.java @@ -0,0 +1,18 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config; + +import java.util.ArrayList; + +public class Action { + public String name, comment; + public boolean enabled; + private ArrayList variables; + + public Action(String name, String comment, boolean enabled, ArrayList variables) { + this.name = name; + this.comment = comment; + this.enabled = enabled; + this.variables = variables; + } + + public ArrayList getVariables() { return variables; } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Configuration.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Configuration.java new file mode 100644 index 0000000..547481e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Configuration.java @@ -0,0 +1,20 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config; + +import java.util.Date; + +public class Configuration { + public static final String DATE_FORMAT = "yyyy-MM-dd HH:mm:ss Z"; + + public Date createdAt, updatedAt; + private int specVersion; + public int revision; + + public Configuration(Date createdAt, Date updatedAt, int specVersion, int revision) { + this.createdAt = createdAt; + this.updatedAt = updatedAt; + this.specVersion = specVersion; + this.revision = revision; + } + + public int getSpecVersion() { return specVersion; } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Group.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Group.java new file mode 100644 index 0000000..d44bca4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Group.java @@ -0,0 +1,27 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config; + +import java.util.ArrayList; + +public class Group { + public String name; + private ArrayList actions; + + public Group(String name, ArrayList actions) { + this.name = name; + this.actions = actions; + } + + public static boolean nameIsUnique(ArrayList groups, String name) { + for (Group group: groups) { + if (group.name.equals(name)) { + return false; + } + } + + return true; + } + + public ArrayList getActions() { + return actions; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Presets.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Presets.java new file mode 100644 index 0000000..682dcad --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Presets.java @@ -0,0 +1,22 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config; + + +import java.util.ArrayList; + +public class Presets { + private ArrayList groups; + private ArrayList actions; + + public Presets(ArrayList groups, ArrayList actions) { + this.groups = groups; + this.actions = actions; + } + + public ArrayList getGroups() { + return groups; + } + + public ArrayList getActions() { + return actions; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Variable.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Variable.java new file mode 100644 index 0000000..0193c76 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/config/Variable.java @@ -0,0 +1,92 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config; + +import android.util.Log; + +import java.util.Arrays; + +public class Variable { + public String name; + private String value; + + public Variable(String name, String value) { + this.name = name; + this.value = value; + } + + public String rawValue() { + return value; + } + + public T value() { + return valueOf(value); + } + + public void setValue(String value) { + this.value = value; + } + + @SuppressWarnings("unchecked") + static public T valueOf(String value) { + String[] split = value.split("x", 2); +// Log.d("Variable", "valueOf split: " + Arrays.toString(split)); + + switch (split[0]) { + case "B": { + return (T) Boolean.valueOf(split[(split.length-1)]); + } + case "D": { + return (T) Double.valueOf(split[(split.length-1)]); + } + case "F": { + return (T) Float.valueOf(split[(split.length-1)]); + } + case "I": { + return (T) Integer.valueOf(split[(split.length-1)]); + } + case "L": { + return (T) Long.valueOf(split[(split.length-1)]); + } + case "S": { + String string = ""; + int i = 0; + for(String str : split) { + if (i == 0) { i++; continue; } + + string += str; + } + return (T) string; + } + default: { + return null; + } + } + } + + static public String typeOf(String value) { + String[] split = value.split("x"); + + switch (split[0]) { + case "B": { + return "Boolean"; + } + case "D": { + return "Double"; + } + case "F": { + return "Float"; + } + case "I": { + return "Integer"; + } + case "L": { + return "Long"; + } + case "S": { + return "String"; + } + default: { + return "=!UNKNOWN!="; + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionDeserializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionDeserializer.java new file mode 100644 index 0000000..d2c177e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionDeserializer.java @@ -0,0 +1,33 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonDeserializationContext; +import com.google.gson.JsonDeserializer; +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonParseException; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; + +import java.lang.reflect.Type; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class ActionDeserializer implements JsonDeserializer { + @Override + public Action deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException { + JsonObject jsonObject = json.getAsJsonObject(); + + final String name = jsonObject.get("name").getAsString(); + final String comment = jsonObject.get("comment").getAsString(); + final boolean enabled = jsonObject.get("enabled").getAsBoolean(); + Variable[] variablesArray = context.deserialize(jsonObject.get("variables"), Variable[].class); + + List variablesList = Arrays.asList(variablesArray); + ArrayList variables = new ArrayList<>(variablesList); + + return new Action(name, comment, enabled, variables); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionSerializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionSerializer.java new file mode 100644 index 0000000..5f8e6ed --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ActionSerializer.java @@ -0,0 +1,27 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonPrimitive; +import com.google.gson.JsonSerializationContext; +import com.google.gson.JsonSerializer; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; + +import java.lang.reflect.Type; + +public class ActionSerializer implements JsonSerializer { + @Override + public JsonElement serialize(Action action, Type type, JsonSerializationContext context) { + JsonObject container = new JsonObject(); + + container.add("name", new JsonPrimitive(action.name)); + container.add("comment", new JsonPrimitive(action.comment)); + container.add("enabled", new JsonPrimitive(action.enabled)); + container.add("variables", context.serialize(action.getVariables().toArray(), Variable[].class)); + + return container; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigDeserializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigDeserializer.java new file mode 100644 index 0000000..6766346 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigDeserializer.java @@ -0,0 +1,34 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonDeserializationContext; +import com.google.gson.JsonDeserializer; +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonParseException; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets; + +import java.lang.reflect.Type; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class ConfigDeserializer implements JsonDeserializer { + @Override + public Config deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException { + JsonObject jsonObject = json.getAsJsonObject(); + JsonObject data = jsonObject.get("data").getAsJsonObject(); + + Configuration configuration = context.deserialize(jsonObject.get("config"), Configuration.class); + Group[] groupsArray = context.deserialize(data.get("groups"), Group[].class); + List groupsList = Arrays.asList(groupsArray); + ArrayList groups = new ArrayList<>(groupsList); + + Presets presets = context.deserialize(data.get("presets"), Presets.class); + + return new Config(configuration, groups, presets); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigSerializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigSerializer.java new file mode 100644 index 0000000..21d4778 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigSerializer.java @@ -0,0 +1,32 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonSerializationContext; +import com.google.gson.JsonSerializer; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; + +import java.lang.reflect.Type; + +public class ConfigSerializer implements JsonSerializer { + @Override + public JsonElement serialize(Config config, Type type, JsonSerializationContext context) { + JsonObject container = new JsonObject(); + JsonObject result = new JsonObject(); + JsonObject presets = new JsonObject(); + container.add("config", context.serialize(config.getConfiguration(), Configuration.class)); + result.add("groups", context.serialize(config.getGroups().toArray(), Group[].class)); + + presets.add("groups", context.serialize(config.getPresets().getGroups().toArray(), Group[].class)); + presets.add("actions", context.serialize(config.getPresets().getActions().toArray(), Action[].class)); + + result.add("presets", presets); + container.add("data", result); + + return container; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationDeserializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationDeserializer.java new file mode 100644 index 0000000..590e1a5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationDeserializer.java @@ -0,0 +1,36 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonDeserializationContext; +import com.google.gson.JsonDeserializer; +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonParseException; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; + +import java.lang.reflect.Type; +import java.text.ParseException; +import java.text.SimpleDateFormat; +import java.util.Date; + +public class ConfigurationDeserializer implements JsonDeserializer { + @Override + public Configuration deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException { + JsonObject config = json.getAsJsonObject(); + + SimpleDateFormat dateFormat = new SimpleDateFormat(Configuration.DATE_FORMAT); + Date createdAt = new Date(); + Date updatedAt = new Date(); + try { + createdAt = dateFormat.parse(config.get("created_at").getAsString()); + updatedAt = dateFormat.parse(config.get("updated_at").getAsString()); + } catch (ParseException e) { + e.printStackTrace(); + } + + final int spec_version = config.get("spec_version").getAsInt(); + final int revision = config.get("revision").getAsInt(); + + return new Configuration(createdAt, updatedAt, spec_version, revision); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationSerializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationSerializer.java new file mode 100644 index 0000000..8b69916 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/ConfigurationSerializer.java @@ -0,0 +1,29 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonPrimitive; +import com.google.gson.JsonSerializationContext; +import com.google.gson.JsonSerializer; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; + +import java.lang.reflect.Type; +import java.text.SimpleDateFormat; + +public class ConfigurationSerializer implements JsonSerializer { + @Override + public JsonElement serialize(Configuration configuration, Type type, JsonSerializationContext context) { + JsonObject container = new JsonObject(); + + SimpleDateFormat dateFormat = new SimpleDateFormat(Configuration.DATE_FORMAT); + + container.add("created_at", new JsonPrimitive(dateFormat.format(configuration.createdAt))); + container.add("updated_at", new JsonPrimitive(dateFormat.format(configuration.updatedAt))); + container.add("spec_version", new JsonPrimitive(configuration.getSpecVersion())); + container.add("revision", new JsonPrimitive(configuration.revision)); + + return container; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupDeserializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupDeserializer.java new file mode 100644 index 0000000..01086bc --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupDeserializer.java @@ -0,0 +1,33 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonDeserializationContext; +import com.google.gson.JsonDeserializer; +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonParseException; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets; + +import java.lang.reflect.Type; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class GroupDeserializer implements JsonDeserializer { + @Override + public Group deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException { + JsonObject jsonObject = json.getAsJsonObject(); + + final String name = jsonObject.get("name").getAsString(); + Action[] actionsArray = context.deserialize(jsonObject.get("actions"), Action[].class); + + List actionsList = Arrays.asList(actionsArray); + ArrayList actions = new ArrayList<>(actionsList); + + return new Group(name, actions); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupSerializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupSerializer.java new file mode 100644 index 0000000..3d8a5bc --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/GroupSerializer.java @@ -0,0 +1,26 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonPrimitive; +import com.google.gson.JsonSerializationContext; +import com.google.gson.JsonSerializer; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; + +import java.lang.reflect.Type; + +public class GroupSerializer implements JsonSerializer { + @Override + public JsonElement serialize(Group group, Type type, JsonSerializationContext context) { + JsonObject container = new JsonObject(); + + container.add("name", new JsonPrimitive(group.name)); + container.add("actions", context.serialize(group.getActions().toArray(), Action[].class)); + + return container; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsDeserializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsDeserializer.java new file mode 100644 index 0000000..509db3b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsDeserializer.java @@ -0,0 +1,33 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonDeserializationContext; +import com.google.gson.JsonDeserializer; +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonParseException; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets; + +import java.lang.reflect.Type; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class PresetsDeserializer implements JsonDeserializer { + @Override + public Presets deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException { + JsonObject jsonObject = json.getAsJsonObject(); + + Group[] GroupsArray = context.deserialize(jsonObject.get("groups"), Group[].class); + Action[] actionsArray = context.deserialize(jsonObject.get("actions"), Action[].class); + + List groupsList = Arrays.asList(GroupsArray); + ArrayList groups = new ArrayList<>(groupsList); + List actionsList = Arrays.asList(actionsArray); + ArrayList actions = new ArrayList<>(actionsList); + + return new Presets(groups, actions); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsSerializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsSerializer.java new file mode 100644 index 0000000..7be89e5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/PresetsSerializer.java @@ -0,0 +1,25 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonPrimitive; +import com.google.gson.JsonSerializationContext; +import com.google.gson.JsonSerializer; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets; + +import java.lang.reflect.Type; + +public class PresetsSerializer implements JsonSerializer { + @Override + public JsonElement serialize(Presets presets, Type type, JsonSerializationContext context) { + JsonObject container = new JsonObject(); + + container.add("groups", context.serialize(presets.getGroups().toArray(), Group[].class)); + container.add("actions", context.serialize(presets.getActions().toArray(), Action[].class)); + + return container; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsDeserializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsDeserializer.java new file mode 100644 index 0000000..e08e84e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsDeserializer.java @@ -0,0 +1,25 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonDeserializationContext; +import com.google.gson.JsonDeserializer; +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonParseException; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings; + +import java.lang.reflect.Type; + +public class SettingsDeserializer implements JsonDeserializer { + @Override + public Settings deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException { + JsonObject jsonObject = json.getAsJsonObject(); + JsonObject data = jsonObject.get("data").getAsJsonObject(); + + return new Settings( + data.get("hostname").getAsString(), + data.get("port").getAsInt(), + data.get("config").getAsString() + ); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsSerializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsSerializer.java new file mode 100644 index 0000000..bd81298 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/SettingsSerializer.java @@ -0,0 +1,30 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonDeserializationContext; +import com.google.gson.JsonDeserializer; +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonParseException; +import com.google.gson.JsonPrimitive; +import com.google.gson.JsonSerializationContext; +import com.google.gson.JsonSerializer; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings; + +import java.lang.reflect.Type; + +public class SettingsSerializer implements JsonSerializer { + @Override + public JsonElement serialize(Settings settings, Type type, JsonSerializationContext context) { + JsonObject container = new JsonObject(); + JsonObject result = new JsonObject(); + result.add("hostname", new JsonPrimitive(settings.hostname)); + result.add("port", new JsonPrimitive(settings.port)); + result.add("config", new JsonPrimitive(settings.config)); + + container.add("data", result); + + return container; + } +} + diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableDeserializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableDeserializer.java new file mode 100644 index 0000000..8251c45 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableDeserializer.java @@ -0,0 +1,28 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonDeserializationContext; +import com.google.gson.JsonDeserializer; +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonParseException; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; + +import java.lang.reflect.Type; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class VariableDeserializer implements JsonDeserializer { + @Override + public Variable deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException { + JsonObject jsonObject = json.getAsJsonObject(); + + final String name = jsonObject.get("name").getAsString(); + final String value = jsonObject.get("value").getAsString(); + + return new Variable(name, value); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableSerializer.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableSerializer.java new file mode 100644 index 0000000..c5adf49 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/serializers/VariableSerializer.java @@ -0,0 +1,23 @@ +package org.timecrafters.TimeCraftersConfigurationTool.library.serializers; + +import com.google.gson.JsonElement; +import com.google.gson.JsonObject; +import com.google.gson.JsonPrimitive; +import com.google.gson.JsonSerializationContext; +import com.google.gson.JsonSerializer; + +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; + +import java.lang.reflect.Type; + +public class VariableSerializer implements JsonSerializer { + @Override + public JsonElement serialize(Variable variable, Type type, JsonSerializationContext context) { + JsonObject container = new JsonObject(); + + container.add("name", new JsonPrimitive(variable.name)); + container.add("value", new JsonPrimitive(variable.rawValue())); + + return container; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java new file mode 100644 index 0000000..8638b07 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java @@ -0,0 +1,58 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class CalibrateRingBeltLoop extends CyberarmState { + + private Robot robot; + private float startingTick; + private int currentTick; + private double ticksPerLoop; + private boolean limit; + private boolean hasSeenLimit; + private boolean limitPrevious; + + public CalibrateRingBeltLoop(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + + currentTick = robot.ringBeltMotor.getCurrentPosition(); + limit = robot.magnetSensor.isPressed(); + + if (engine.gamepad1.x || (engine.gamepad1.a && !limit)) { + robot.ringBeltMotor.setPower(0.5); + } else { + robot.ringBeltMotor.setPower(0); + } + + if (engine.gamepad1.y) { + robot.ringBeltMotor.setPower(-0.1); + } else { + robot.ringBeltMotor.setPower(0); + } + + if (engine.gamepad1.b) { + startingTick = currentTick; + } + + if (limit && !limitPrevious){ + if (hasSeenLimit) { + ticksPerLoop = currentTick - startingTick; + } + startingTick = currentTick; + hasSeenLimit = true; + } + limitPrevious = limit; + } + + @Override + public void telemetry() { + engine.telemetry.addData("ticks", currentTick - startingTick); + engine.telemetry.addData("limit swich", limit ); + engine.telemetry.addData("Clockwise", ticksPerLoop); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java new file mode 100644 index 0000000..7f1ed4d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java @@ -0,0 +1,25 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; +@Disabled +@TeleOp (name = "Calibration", group = "test") +public class CalibrationEngine extends CyberarmEngine { + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + super.init(); + } + + + @Override + public void setup() { + addState(new OdometryCalibration(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java new file mode 100644 index 0000000..5723ea8 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java @@ -0,0 +1,82 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; + +public class MechanumBiasCalibrator extends CyberarmState { + + private Robot robot; + private ArrayList Powers = new ArrayList<>(); + private double BiasFR; + private double BiasFL; + private double BiasBR; + private double BiasBL; + private boolean hasCalculated; + + + public MechanumBiasCalibrator(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + robot.updateLocation(); + + if (engine.gamepad1.x) { + double[] mecanumPowers = robot.getMecanumPowers(-45, 0.5, 0); + + + robot.driveFrontLeft.setPower(mecanumPowers[0]); + robot.driveFrontRight.setPower(mecanumPowers[1]); + robot.driveBackLeft.setPower(mecanumPowers[2]); + robot.driveBackRight.setPower(mecanumPowers[3]); + + Powers.add(mecanumPowers); + } else { + robot.setDrivePower(0,0,0,0 ); + } + + if (engine.gamepad1.y && !hasCalculated) { + hasCalculated = true; + + double sumFR = 0; + double sumFL = 0; + double sumBR = 0; + double sumBL = 0; + + for (double[] powers : Powers) { + sumFL+= Math.abs(powers[0]); + sumFR+= Math.abs(powers[1]); + sumBL+= Math.abs(powers[2]); + sumBR+= Math.abs(powers[3]); + } + + int length = Powers.size(); + BiasFR = sumFR / length; + BiasFL = sumFL / length; + BiasBR = sumBR / length; + BiasBL = sumBL / length; + + double max = Math.max(Math.max(BiasFL,BiasFR),Math.max(BiasBL,BiasBR)); + + BiasFL = BiasFL /max; + BiasFR = BiasFR /max; + BiasBL = BiasBL /max; + BiasBR = BiasBR /max; + + } else if (!engine.gamepad1.y) { + hasCalculated = false; + } + + } + + @Override + public void telemetry() { + engine.telemetry.addData("FrontLeft", BiasFL); + engine.telemetry.addData("FrontRight", BiasFR); + engine.telemetry.addData("BackLeft", BiasBL); + engine.telemetry.addData("BackRight", BiasBR); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java new file mode 100644 index 0000000..321534b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java @@ -0,0 +1,59 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Engagable; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.DriveMotor; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class MotorCheck extends CyberarmState { + + private Robot robot; + private double[] Powers; + private DcMotor[] Motors; + private int currentMotor = 0; + private double currentPower = 0.001; + + private boolean yPrevious; + + + public MotorCheck(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight}; + } + + @Override + public void start() { + Motors[currentMotor].setPower(0.2); + } + + @Override + public void exec() { + + if (currentMotor <= 3) { + + boolean y = engine.gamepad1.y; + if (y && !yPrevious) { + + Motors[currentMotor].setPower(0); + currentMotor += 1; + Motors[currentMotor].setPower(0.2); + sleep(20); + } + yPrevious = y; + } + + } + + @Override + public void telemetry() { + + engine.telemetry.addData("current Motor", Motors[currentMotor].toString()); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java new file mode 100644 index 0000000..a5a73dd --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java @@ -0,0 +1,74 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class OdometryCalibration extends CyberarmState { + + private Robot robot; + private float rotation = 0; + private float rotationPrev = 0; + private int currentTick; + private double ticksPerDegreeClockwise; + private double ticksPerDegreeCounterClockwise; + private long timePrevious; + private long timeChange; + + public OdometryCalibration(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + robot.updateLocation(); + + long timeCurrent = System.currentTimeMillis(); + timeChange = timeCurrent - timePrevious; + + if (timeChange >= 1200) { + timePrevious = timeCurrent; + + + double power = 0.25; + float imu = -robot.imu.getAngularOrientation().firstAngle; + rotation += robot.getRelativeAngle(imu, rotationPrev); + rotationPrev = imu; + + currentTick = (robot.encoderRight.getCurrentPosition() - robot.encoderLeft.getCurrentPosition()) / 2; + + if (engine.gamepad1.x) { + robot.setDrivePower(power, -power, power, -power); + ticksPerDegreeClockwise = currentTick / rotation; + } else if (engine.gamepad1.y) { + robot.setDrivePower(-power, power, -power, power); + ticksPerDegreeCounterClockwise = currentTick / rotation; + } else { + robot.setDrivePower(0, 0, 0, 0); + } + } + + if (engine.gamepad1.b) { + robot.record("Clockwise : "+ticksPerDegreeClockwise); + robot.record("Counter Clockwise : "+ticksPerDegreeCounterClockwise); + robot.saveRecording(); + setHasFinished(true); + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")"); + engine.telemetry.addData("Rotation ", robot.getRotation()); +// +// engine.telemetry.addData("degrees", rotation); + engine.telemetry.addData("ticks", robot.encoderRight.getCurrentPosition() ); + engine.telemetry.addData("Clockwise", ticksPerDegreeClockwise); + engine.telemetry.addData("CounterClockwise", ticksPerDegreeCounterClockwise); +// engine.telemetry.addData("forward", robot.forwardVector); +// engine.telemetry.addData("sideways", robot.sidewaysVector); + } + + private float round(double number,double unit) { + return (float) (Math.floor(number/unit) * unit); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java new file mode 100644 index 0000000..23ae148 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java @@ -0,0 +1,32 @@ +package org.timecrafters.UltimateGoal.Calibration; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; + +public class PerformanceTest extends CyberarmState { + + private Robot robot; + private ArrayList angles = new ArrayList(); + + + public PerformanceTest(Robot robot) { + this.robot = robot; + } + + + @Override + public void exec() { + robot.updateLocation(); +// robot.record(); + + if (engine.gamepad1.x) { + double[] powers = robot.getMecanumPowers(0, 1, 0); + robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]); + } else { + robot.setDrivePower(0,0,0,0); + } + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java new file mode 100644 index 0000000..a2044ab --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java @@ -0,0 +1,122 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Launch; +import org.timecrafters.UltimateGoal.Competition.Pause; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.WobbleArm; +import org.timecrafters.UltimateGoal.Competition.WobbleGrab; + +import java.util.ArrayList; + +@Disabled +@Autonomous(name = "Autonomous") +public class AutoEngine extends CyberarmEngine { + + private Robot robot; + private TensorFlowCheck tensorFlowCheck; + + private double launchTolerance; + private double launchPower; + private long launchBrakeTime; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + robot.webCamServo.setPosition(Robot.CAM_SERVO_DOWN); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + robot.wobbleArmMotor.setTargetPosition(0); + robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // since we've preloaded three rings, the ring belt stage is set to account for this; + robot.ringBeltStage = 3; + + //Autonomous specific Configuration Variables + float rotation = robot.stateConfiguration.variable("system", "startPos", "direction").value(); + double locationX = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "x").value()); + double locationY = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "y").value()); + robot.setLocalization(rotation,locationX,locationY); + + launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value()); + launchPower = robot.stateConfiguration.variable("auto","04_0","power").value(); + launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value(); + + super.init(); + } + + @Override + public void setup() { + //drive to view + addState(new DriveToCoordinates(robot, "auto", "01_0")); + + //face ring + addState(new Face(robot, "auto", "01_1")); + + //select scoring area + tensorFlowCheck = new TensorFlowCheck(robot, "auto", "02_0"); + addState(tensorFlowCheck); + + //drive around ring stack + addState(new DriveToCoordinates(robot, "auto", "03_0")); + + //drive to launch position + addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime)); + + //aligns to goal + addState(new Face(robot, "auto", "04_1")); + + //launch rings and drive to scoreing area. LaunchDriveControl allows makes the robot begin + //driving while the belt is resetting + CyberarmState launchState = new Launch(robot, "auto", "04_2"); + + CyberarmState driveState = new DriveToCoordinates(robot, "auto", "05_0", true); + addState(new LaunchDriveControl(robot,launchState,driveState)); + + //turn arm towards scoreing area. + ArrayList threadStates0 = new ArrayList<>(); + threadStates0.add(new Face(robot, "auto", "05_1")); + threadStates0.add(new WobbleArm(robot, "auto", "05_2",robot.wobbleDownPos)); + addState(new ThreadStates(threadStates0)); + + //open the wobble grab arm + addState(new WobbleGrab(robot, "auto", "06_0", true)); + + //drive to second wobble + addState(new DriveToCoordinates(robot, "auto","06_1")); + + addState(new DriveToCoordinates(robot, "auto", "07_0")); + addState(new Face(robot,"auto","07_1")); + + addState(new FindWobbleGoal(robot, "auto", "08_0")); + addState(new Pause(robot,"auto","09_0")); + + //drive to scoring area + ArrayList threadStates1 = new ArrayList<>(); +// threadStates1.add(new Face(robot, "auto", "09_0")); + threadStates1.add(new WobbleArm(robot, "auto", "09_1",robot.wobbleUpPos)); + threadStates1.add(new DriveToCoordinates(robot, "auto", "10_0", true)); + addState(new ThreadStates(threadStates1)); + + + + ArrayList threadStates2 = new ArrayList<>(); + threadStates2.add(new Face(robot, "auto", "11_0")); + threadStates2.add(new WobbleArm(robot, "auto", "11_1",robot.wobbleDownPos)); + addState(new ThreadStates(threadStates2)); + + //release wobble goal 2 + addState(new WobbleGrab(robot, "auto", "12_0", true)); + + //drive to park + ArrayList threadStates3 = new ArrayList<>(); + threadStates3.add(new WobbleArm(robot, "auto", "12_2", 0)); + threadStates3.add(new Park(robot,"auto","13_0")); + addState(new ThreadStates(threadStates3)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java new file mode 100644 index 0000000..8b49dc1 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java @@ -0,0 +1,141 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class DriveToCoordinates extends CyberarmState { + + private Robot robot; + private String groupName = "manual"; + private String actionName; + private double xTarget; + private double yTarget; + private float faceAngle; + private double tolerancePos; + private double basePower; + private boolean braking; + private long breakStartTime; + private long brakeTime; + private boolean autoFace; + private double decelRange; + private double decelMin; + private boolean scoringArea; + + public DriveToCoordinates(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + public DriveToCoordinates(Robot robot, String groupName, String actionName, boolean scoringArea) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + this.scoringArea = scoringArea; + } + + public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime) { + this.robot = robot; + this.xTarget = xTarget; + this.yTarget = yTarget; + this.faceAngle = faceAngle; + this.tolerancePos = tolerance; + this.basePower = power; + this.brakeTime = brakeTime; + } + + public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime, double decelRange, double decelMin) { + this.robot = robot; + this.xTarget = xTarget; + this.yTarget = yTarget; + this.faceAngle = faceAngle; + this.tolerancePos = tolerance; + this.basePower = power; + this.brakeTime = brakeTime; + this.decelRange = decelRange; + this.decelMin = decelMin; + } + + @Override + public void init() { + if (!groupName.equals("manual")) { + xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value()); + yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value()); + basePower = robot.stateConfiguration.variable(groupName, actionName, "power").value(); + tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value()); + brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value(); + + try { + faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value(); + } catch (RuntimeException e) { + autoFace = true; + } + + try { + decelRange = robot.stateConfiguration.variable(groupName, actionName, "decelR").value(); + decelMin = robot.stateConfiguration.variable(groupName, actionName, "decelM").value(); + } catch (RuntimeException e) { + decelRange = 0; + } + } + + if (decelRange > 0) { + decelRange = robot.inchesToTicks(decelRange); + } + + } + + @Override + public void start() { + if (!groupName.equals("manual")) { + setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled); + + //used to navigate towards the randomly generated scoreing area. the original target + //becomes an offset of the scoring area position. + if (scoringArea) { + xTarget += robot.wobbleScoreX; + yTarget += robot.wobbleScoreY; + } + + if (faceAngle == 360) { + faceAngle = robot.getRelativeAngle(180, robot.getAngleToPosition(xTarget,yTarget)); + } + } + + if (autoFace) { + faceAngle = robot.getAngleToPosition(xTarget,yTarget); + } + } + + @Override + public void exec() { + robot.updateLocation(); + + double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY()); + + // deceleration + double power = basePower; + if (distanceToTarget < decelRange) { + power = ((distanceToTarget / decelRange) * (basePower - decelMin)) + decelMin; + } + + double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle); + robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]); + + //Once the robot is within the acceptable range, + if (distanceToTarget > tolerancePos) { + braking = false; + } else { + long currentTime = System.currentTimeMillis(); + if (!braking) { + breakStartTime = currentTime; + braking = true; + } + if (currentTime - breakStartTime >= brakeTime) { + robot.setDrivePower(0,0,0,0); + setHasFinished(true); + } + } + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java new file mode 100644 index 0000000..1e8fb19 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java @@ -0,0 +1,46 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class DriveWithColorSensor extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private double power; + private double minimum; + private double maximum; + + public DriveWithColorSensor(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void init() { + power = robot.stateConfiguration.variable(groupName,actionName,"power").value(); + minimum = robot.stateConfiguration.variable(groupName,actionName,"min").value(); + maximum = robot.stateConfiguration.variable(groupName,actionName,"max").value(); + } + + @Override + public void start() { + setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled); + } + + @Override + public void exec() { + double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM); + if (sensorValue < minimum) { + robot.getMecanumPowers(180,power,0); + } else if (sensorValue > maximum) { + robot.getMecanumPowers(0,power,0); + } else { + robot.setDrivePower(0,0,0,0); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java new file mode 100644 index 0000000..c735ab8 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java @@ -0,0 +1,95 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class Face extends CyberarmState { + + private Robot robot; + private String groupName = "manual"; + private String actionName; + private float faceAngle; + private float toleranceFace; + private double power; + private boolean braking; + private long breakStartTime; + private long breakTime; + private boolean autoFace; + + public Face(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + public Face(Robot robot, float faceAngle, float toleranceFace, double power, long breakTime) { + this.robot = robot; + this.faceAngle = faceAngle; + this.toleranceFace = toleranceFace; + this.power = power; + this.breakTime = breakTime; + } + + @Override + public void init() { + + if (!groupName.equals("manual")) { + power = robot.stateConfiguration.variable(groupName, actionName, "power").value(); + toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value(); + breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value(); + + try { + faceAngle = robot.stateConfiguration.variable(groupName, actionName, "faceAngle").value(); + } catch (RuntimeException e) { + autoFace = true; + } + } + } + + @Override + public void start() { + + if (!groupName.equals("manual")) { + setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled); + + + if (autoFace) { + double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value()); + double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value()); + faceAngle = robot.getAngleToPosition(x, y); + } + + if (faceAngle == 360) { + double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value()); + double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value()); + faceAngle = robot.getRelativeAngle(180,robot.getAngleToPosition(x, y)); + } + } + } + + @Override + public void exec() { + robot.updateLocation(); + double[] powers = robot.getFacePowers(faceAngle,power); + robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]); + if (Math.abs(robot.getRelativeAngle(faceAngle,robot.getRotation())) > toleranceFace) { + braking = false; + } else { + long currentTime = System.currentTimeMillis(); + if (!braking) { + breakStartTime = currentTime; + braking = true; + } + if (currentTime - breakStartTime >= breakTime) { + setHasFinished(true); + robot.setDrivePower(0,0,0,0); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("target angle", faceAngle); + engine.telemetry.addData("current angle", robot.getRotation()); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FaceAndDrive.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FaceAndDrive.java new file mode 100644 index 0000000..443a3f1 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FaceAndDrive.java @@ -0,0 +1,56 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; + +public class FaceAndDrive extends CyberarmState { + + private Robot robot; + private String groupName = "manual"; + private String actionName; + private double xTarget; + private double yTarget; + private float faceAngle; + private double tolerancePos; + private double power; + private long brakeTime; + private float toleranceFace; + + public FaceAndDrive(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void init() { + + xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value()); + yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value()); + power = robot.stateConfiguration.variable(groupName, actionName, "power").value(); + tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value()); + toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value(); + brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value(); + + try { + faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value(); + } catch (NullPointerException e) { + faceAngle = robot.getAngleToPosition(xTarget,yTarget); + } + + + } + + @Override + public void start() { + addState(new Face(robot, faceAngle, toleranceFace, power, brakeTime)); + addState(new DriveToCoordinates(robot,xTarget,yTarget,faceAngle,tolerancePos,power,brakeTime)); + } + + @Override + public void exec() { + setHasFinished(true); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java new file mode 100644 index 0000000..5cc594a --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java @@ -0,0 +1,109 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +/* +The FindWobbleGoal state is used in teleOp and Autonomous to aid in capturing the wobble goal. +*/ + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class FindWobbleGoal extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private double power; + private double turnCheck; + private double driveCheck; + private double range; + private boolean cCheckPrev; + private boolean ccCheckPrev; + private float startRotation; + private boolean foundGoalRotation; + private float wobbleGoalRotation; + private long timeLimit; + + public FindWobbleGoal(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void init() { + power = robot.stateConfiguration.variable(groupName,actionName,"power").value(); + turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value(); + driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value(); + range = robot.stateConfiguration.variable(groupName,actionName,"r").value(); + timeLimit = robot.stateConfiguration.variable(groupName,actionName,"limit").value(); + } + + @Override + public void start() { + setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled); + startRotation = robot.getRotation(); + robot.setDrivePower(power,-power,power,-power); + } + + @Override + public void exec() { + robot.updateLocation(); + double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM); + + //Stage 1: scan back and forth untile the sensor is in line with the wobble goal. + if (sensorValue > turnCheck) { + + float rotation = robot.getRelativeAngle(startRotation,robot.getRotation()); + + boolean cCheck = rotation > range; + boolean ccCheck = rotation < -range; + + if (cCheck && !cCheckPrev) { + robot.setDrivePower(-power, power, -power, power); + } + + if (ccCheck && !ccCheckPrev) { + robot.setDrivePower(power, -power, power, -power); + } + cCheckPrev = cCheck; + ccCheckPrev = ccCheck; + + } else { + //Stage 2: drive toward wobble goal until it's close enough to grab + if (sensorValue > driveCheck) { + if (!foundGoalRotation) { + foundGoalRotation = true; + wobbleGoalRotation = robot.getRotation(); + } + double[] powers = robot.getMecanumPowers( + wobbleGoalRotation - 90, + power*2 , wobbleGoalRotation); + robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]); + } else { + //stage 3: grab the wobble goal && finish the state + endSearch(); + } + } + + //if the search takes too long, the robot grabs and finishes the state + if (runTime() > timeLimit) { + endSearch(); + } + } + + private void endSearch() { + robot.setDrivePower(0,0,0,0); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + setHasFinished(true); + } + + @Override + public void telemetry() { + engine.telemetry.addData("sensor", robot.wobbleColorSensor.getDistance(DistanceUnit.MM)); + engine.telemetry.addData("red", robot.wobbleColorSensor.red()); + engine.telemetry.addData("green", robot.wobbleColorSensor.green()); + engine.telemetry.addData("blue", robot.wobbleColorSensor.blue()); + } +} + diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java new file mode 100644 index 0000000..c05c032 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/LaunchDriveControl.java @@ -0,0 +1,41 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class LaunchDriveControl extends CyberarmState { + + private Robot robot; + private CyberarmState launchState; + private CyberarmState driveState; + private boolean checkConditionPrev; + + public LaunchDriveControl(Robot robot, CyberarmState launchState, CyberarmState driveState) { + this.robot = robot; + this.launchState = launchState; + this.driveState = driveState; + } + + @Override + public void start() { + addParallelState(launchState); + } + + @Override + public void exec() { + boolean checkCondition = (robot.ringBeltStage > 3); + if (checkCondition && !checkConditionPrev) { + addParallelState(driveState); + } + checkConditionPrev = checkCondition; + + setHasFinished(childrenHaveFinished()); + } + + @Override + public void telemetry() { + engine.telemetry.addData("ring belt stage", robot.ringBeltStage); + engine.telemetry.addData("check prev", checkConditionPrev); + engine.telemetry.addData("drive prev", checkConditionPrev); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java new file mode 100644 index 0000000..88bf2ff --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java @@ -0,0 +1,50 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; + +public class Park extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + double parkY; + float parkFaceAngle; + double parkTolerance; + double parkPower; + long parkBrakeTime; + double parkDecelRange; + double parkDecelMin; + + public Park(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void init() { + parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"yPos").value()); + parkFaceAngle = robot.stateConfiguration.variable(groupName,actionName,"face").value(); + parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto",actionName,"tolPos").value()); + parkPower = robot.stateConfiguration.variable(groupName,actionName,"power").value(); + parkBrakeTime = robot.stateConfiguration.variable(groupName,actionName,"brakeMS").value(); + parkDecelMin = robot.stateConfiguration.variable(groupName,actionName,"decelM").value(); + parkDecelRange = robot.stateConfiguration.variable(groupName,actionName,"decelR").value(); + } + + @Override + public void start() { + if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8)) { + addParallelState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower, parkBrakeTime,parkDecelRange,parkDecelMin)); + } + } + + @Override + public void exec() { + setHasFinished(childrenHaveFinished()); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java new file mode 100644 index 0000000..fe382a4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java @@ -0,0 +1,94 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; +import java.util.List; + +public class TensorFlowCheck extends CyberarmState { + + private Robot robot; + private String group; + private String action; + private List recognitions; + private int path = 0; + private long checkTime; + public double wobblePosX; + public double wobblePosY; + private int manualPath; + private String status; + + public TensorFlowCheck(Robot robot, String group, String action) { + this.robot = robot; + this.group = group; + this.action = action; + } + + @Override + public void init() { + checkTime = robot.stateConfiguration.variable(group,action,"time").value(); + manualPath = robot.stateConfiguration.variable(group,action,"path").value(); + } + + @Override + public void start() { + if (!robot.stateConfiguration.action(group,action).enabled) { + manualPath = -1; + } else { + robot.tfObjectDetector.activate(); + } + } + + @Override + public void exec() { + + if (runTime() < checkTime) { + if (manualPath != -1) { + recognitions = robot.tfObjectDetector.getUpdatedRecognitions(); + + if (recognitions != null) { + if (recognitions.size() == 1) { + String label = recognitions.get(0).getLabel(); + if (label.equals("Single")) { + path = 1; + } else if (label.equals("Quad")) { + path = 2; + } + } else if (recognitions.size() > 1) { + path = 1; + } + } + + } else { + path = manualPath; + } + } else { + robot.tfObjectDetector.deactivate(); + + if (path == 0) { + robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","x").value()); + robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","y").value()); + } + if (path == 1) { + robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","x").value()); + robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","y").value()); + } + if (path == 2) { + robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","x").value()); + robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","y").value()); + } + // make the servo look up once we're done using tensorFlow + robot.webCamServo.setPosition(0); + robot.launchMotor.setPower(Robot.LAUNCH_POWER); + + setHasFinished(true); + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("Chosen Path", path); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java new file mode 100644 index 0000000..2b3a119 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java @@ -0,0 +1,44 @@ +package org.timecrafters.UltimateGoal.Competition.Autonomous; + +import org.cyberarm.engine.V2.CyberarmState; + +import java.util.ArrayList; + +public class ThreadStates extends CyberarmState { + + private ArrayList states = new ArrayList(); + + public ThreadStates(ArrayList states) { + this.states = states; + } + + @Override + public void start() { + for (CyberarmState state : states) { + addParallelState(state); + } + } + + @Override + public void exec() { + int finishedStates = 0; + for (CyberarmState state : states) { + if (state.getHasFinished()) { + finishedStates += 1; + } + } + setHasFinished(finishedStates == states.size()); + + } + + @Override + public void telemetry() { + engine.telemetry.addLine("Threaded States"); + for (CyberarmState child: children) { + engine.telemetry.addLine(""+child.getClass()); + if (child.getHasFinished()) { + engine.telemetry.addLine("finished"); + } + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java new file mode 100644 index 0000000..dbb7081 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java @@ -0,0 +1,36 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.TeleOp.TeleOpState; + +@Disabled +@TeleOp (name = "Demo: Game",group = "demo") +public class DemoEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + robot.webCamServo.setPosition(0); + super.init(); + } + + @Override + public void setup() { + addState(new DemoState(robot)); + } + + @Override + public void stop() { + robot.deactivateVuforia(); + robot.saveRecording(); + super.stop(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java new file mode 100644 index 0000000..b815969 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java @@ -0,0 +1,35 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; + +@Disabled +@TeleOp (name = "Demo: Tank",group = "demo") +public class DemoEngineTank extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + robot.webCamServo.setPosition(0); + super.init(); + } + + @Override + public void setup() { + addState(new DemoStateTank(robot)); + } + + @Override + public void stop() { + robot.deactivateVuforia(); + robot.saveRecording(); + super.stop(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java new file mode 100644 index 0000000..e3007c1 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java @@ -0,0 +1,194 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +/* +The Player1 state has all the controls for player one. The Player One and Player Two controls are +kept in separate states so that the childrenHaveFinished() method can be used to easily stop things +from running at the same time that shouldn't be. +*/ + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl; + +public class DemoState extends CyberarmState { + private Robot robot; + + //normal drive control + private float leftJoystickDegrees; + private double leftJoystickMagnitude; + private float rightJoystickDegrees; + private double rightJoystickMagnitude; + private double rightJoystickMagnitudePrevious; + + private double faceControlThreshold; + private float cardinalSnapping; + private float pairSnapping; + + private float faceDirection = 0; + private double[] powers = {0,0,0,0}; + private double drivePower = 1; + private boolean lbPrev; + + private double refinePower; + + public DemoState(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + cardinalSnapping = robot.stateConfiguration.variable( + "tele","control", "cardinalSnapping").value(); + pairSnapping = robot.stateConfiguration.variable( + "tele","control", "pairSnapping").value(); + faceControlThreshold = robot.stateConfiguration.variable( + "tele","control", "faceControlT").value(); + refinePower = robot.stateConfiguration.variable( + "tele","control", "refPower").value(); + + } + + @Override + public void start() { + faceDirection = robot.getRotation(); + } + + @Override + public void exec() { + robot.updateLocation(); + + Gamepad gamepad = engine.gamepad1; + if (engine.gamepad2.right_trigger != 0) { + gamepad = engine.gamepad2; + } + + //toggle for drive speed + boolean lb = engine.gamepad2.left_stick_button; + if (lb && !lbPrev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + lbPrev = lb; + + //Calculate Joystick Vector + double leftJoystickX = gamepad.left_stick_x; + double leftJoystickY = gamepad.left_stick_y; + + leftJoystickDegrees = robot.getRelativeAngle(0, + (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY))); + leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY); + + double rightJoystickX = gamepad.right_stick_x; + double rightJoystickY = gamepad.right_stick_y; + + rightJoystickDegrees = robot.getRelativeAngle(0, + (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY))); + rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); + + //allows the the driver to indicate which direction the robot is currently looking + //so that the controller and robot can be re-synced in the event of a bad initial + //rotation. + if (gamepad.back) { + float newRotation = 0; + + if (rightJoystickMagnitude != 0) { + newRotation = rightJoystickDegrees; + } + + robot.setLocalization(newRotation, robot.getLocationX(), robot.getLocationY()); + faceDirection = newRotation; + } + + //if the driver is letting go of the face joystick, the robot should maintain it's + //current face direction. + if (rightJoystickMagnitude > faceControlThreshold || + rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) { + + //if the joystick is close to one of the cardinal directions, it is set to exactly + // the cardinal direction + faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0); + } + rightJoystickMagnitudePrevious = rightJoystickMagnitude; + + //The D-pad is used if the drivers need to get a more precise angle than they can get + //using the face joystick + if (gamepad.dpad_right) { + powers = new double[]{refinePower, -refinePower, refinePower, -refinePower}; + faceDirection = robot.getRotation(); + } else if (gamepad.dpad_left) { + powers = new double[]{-refinePower, refinePower, -refinePower, refinePower}; + faceDirection = robot.getRotation(); + } else if (leftJoystickMagnitude == 0) { + double[] facePowers = robot.getFacePowers(faceDirection, 0.4); + powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; + } else { + //drives the robot in the direction of the move joystick while facing the direction + //of the look joystick. if the move direction is almost aligned/perpendicular to the + //look joystick, + powers = robot.getMecanumPowers( + snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), + drivePower, faceDirection); + } + + robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]); + + + //LED feedback control + double ringBeltPower = robot.ringBeltMotor.getPower(); + if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - + robot.ringBeltMotor.getCurrentPosition()) > 10) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE ); + } else if (ringBeltPower < 0) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED); + } else { + if (drivePower == 1) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); + } else { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine("Controler Directions"); + engine.telemetry.addData("Right", rightJoystickDegrees); + engine.telemetry.addData("Left", leftJoystickDegrees); + + engine.telemetry.addData("face", faceDirection); + + engine.telemetry.addData("Player 1 children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } + } + + //This just checks if the wobble arm runmode is already the desired mode before setting it. + //I don't know if this is actually helpful + private void setArmMode(DcMotor.RunMode runMode) { + if (robot.wobbleArmMotor.getMode() != runMode) { + robot.wobbleArmMotor.setMode(runMode); + } + } + + private float snapToCardinal(float angle, float snapping, float offset) { + int o = (int) offset + 180; + o %= 90; + for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) { + if (angle >= cardinal-snapping && angle <= cardinal+snapping) { + angle = cardinal; + } + } + return angle; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java new file mode 100644 index 0000000..ecd65e0 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java @@ -0,0 +1,55 @@ +package org.timecrafters.UltimateGoal.Competition.Demo; + +/* +The Player1 state has all the controls for player one. The Player One and Player Two controls are +kept in separate states so that the childrenHaveFinished() method can be used to easily stop things +from running at the same time that shouldn't be. +*/ + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl; + +public class DemoStateTank extends CyberarmState { + private Robot robot; + + private double drivePower = 1; + private boolean lbPrev; + + + public DemoStateTank(Robot robot) { + this.robot = robot; + } + + @Override + public void exec() { + robot.updateLocation(); + + Gamepad gamepad = engine.gamepad1; + if (engine.gamepad2.right_bumper) { + gamepad = engine.gamepad2; + } + + //toggle for drive speed + boolean lb = engine.gamepad2.left_stick_button; + if (lb && !lbPrev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + lbPrev = lb; + + double left = -drivePower * gamepad.left_stick_y; + double right = -drivePower * gamepad.right_stick_y; + robot.setDrivePower(left,right,left,right); + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java new file mode 100644 index 0000000..43db37b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java @@ -0,0 +1,45 @@ +package org.timecrafters.UltimateGoal.Competition; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +public class DriveMotor { + + public DcMotor motor; + private int brakePosition; + private MotorConfigurationType motorType; + private DcMotorSimple.Direction direction; + static final double FINE_CORRECTION = 0.05; + static final double LARGE_CORRECTION = 0.03; + + public DriveMotor(DcMotor motor, MotorConfigurationType motorType, DcMotorSimple.Direction direction) { + this.motor = motor; + this.motorType = motorType; + this.direction = direction; + } + + public void init() { +// motor.setMotorType(motorType); +// motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); +// motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motor.setDirection(direction); + } + + public void setPower(double power) { + motor.setPower(power); + } + + public void setBrakePosition() { + brakePosition = motor.getCurrentPosition(); + } + + public void brake() { + double currentPosition = motor.getCurrentPosition(); + double relativePosition = brakePosition - currentPosition; + double breakPower = Math.pow(LARGE_CORRECTION * relativePosition, 3) + FINE_CORRECTION * relativePosition; + + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java new file mode 100644 index 0000000..da72b37 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java @@ -0,0 +1,104 @@ +package org.timecrafters.UltimateGoal.Competition; + +/* +The Launch state is used in teleOp and Autonomous. In addition to launching any rings by cycling the +ring belt, this state returns the ring belt to the starting position +*/ + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; + +public class Launch extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + boolean hasCycled; + boolean detectedPass; + private boolean reduceConditionPrev; + private double reducePos; + private long stuckStartTime; + + public Launch(Robot robot) { + this.robot = robot; + } + + public Launch(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void start() { + try { + if (robot.stateConfiguration.action(groupName, actionName).enabled) { + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); + + } else { + setHasFinished(true); + } + } catch (NullPointerException e){ + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); + } + + } + + @Override + public void exec() { + //jam counter measures + if (robot.beltIsStuck() && childrenHaveFinished()) { + long currentTime = System.currentTimeMillis(); + if (stuckStartTime == 0) { + stuckStartTime = currentTime; + } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) { + addParallelState(new UnstickRingBelt(robot)); + } + } else { + stuckStartTime = 0; + } + + //detect when limit switch is initially triggered + boolean detectingPass = robot.magnetSensor.isPressed(); + int beltPos = robot.ringBeltMotor.getCurrentPosition(); + if (detectingPass && !detectedPass) { + //finish once the ring belt has cycled all the way through and then returned to + //the first receiving position. + + if (hasCycled) { + robot.ringBeltStage = 0; + robot.ringBeltMotor.setTargetPosition(beltPos); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + if (!robot.initLauncher) { + robot.launchMotor.setPower(0); + } + + setHasFinished(true); + } else { + hasCycled = true; + reducePos = beltPos + (robot.reduceLaunchPos); + } + } + detectedPass = detectingPass; + + boolean reduceCondition = (hasCycled && beltPos > reducePos); + if (reduceCondition && !reduceConditionPrev){ + robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER); + + //the ring belt stage lets other states know that the robot has finished launching all three rings + //and is now returning to loading position. + + robot.ringBeltStage += 1; + } + reduceConditionPrev = reduceCondition; + } + + @Override + public void telemetry() { + engine.telemetry.addData("hasCycled", hasCycled); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java new file mode 100644 index 0000000..5199b93 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java @@ -0,0 +1,50 @@ +package org.timecrafters.UltimateGoal.Competition; + +import org.cyberarm.engine.V2.CyberarmState; + +public class Pause extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private long waitTime = -1; + private boolean enabled; + + public Pause(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + public Pause(Robot robot, long waitTime) { + this.robot = robot; + this.waitTime = waitTime; + } + + @Override + public void init() { + if (waitTime == -1) { + enabled = robot.stateConfiguration.action(groupName, actionName).enabled; + waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value(); + } + } + + @Override + public void start() { + if (!enabled) { + setHasFinished(true); + } + } + + @Override + public void exec() { + setHasFinished(runTime() > waitTime); + } + + @Override + public void telemetry() { + engine.telemetry.addData("runTime", runTime()); + engine.telemetry.addData("wait", waitTime); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java new file mode 100644 index 0000000..39bd52c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java @@ -0,0 +1,26 @@ +package org.timecrafters.UltimateGoal.Competition.PreInit; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class FindLimitSwitch extends CyberarmState { + + private Robot robot; + + public FindLimitSwitch(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER); + } + + @Override + public void exec() { + if (robot.magnetSensor.isPressed()) { + robot.ringBeltMotor.setPower(0); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java new file mode 100644 index 0000000..4c62b31 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java @@ -0,0 +1,35 @@ +package org.timecrafters.UltimateGoal.Competition.PreInit; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class LoadRings extends CyberarmState { + + private Robot robot; + private ProgressRingBelt ringBeltState; + private int ringCount = 0; + + public LoadRings(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + robot.collectionMotor.setPower(1); + } + + @Override + public void exec() { + ringBeltState = new ProgressRingBelt(robot); + if (engine.gamepad1.x && childrenHaveFinished()) { + addParallelState(ringBeltState); + + } + + if (robot.ringBeltStage > 2) { + robot.launchMotor.setPower(0); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java new file mode 100644 index 0000000..64ba7e9 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java @@ -0,0 +1,26 @@ +package org.timecrafters.UltimateGoal.Competition.PreInit; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Robot; +@Disabled +@Autonomous (name = "Load Rings") +public class PreInitEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + super.init(); + } + + @Override + public void setup() { + addState(new FindLimitSwitch(robot)); + addState(new LoadRings(robot)); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java new file mode 100644 index 0000000..283f72d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java @@ -0,0 +1,82 @@ +package org.timecrafters.UltimateGoal.Competition; + +/* +The ProgressRingBelt state is used in teleOp to automatically move the ring belt. +*/ + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; +import org.cyberarm.engine.V2.CyberarmState; + +public class ProgressRingBelt extends CyberarmState { + + private Robot robot; + private int targetPos; + private boolean prepLaunch; + private long stuckStartTime; + + public ProgressRingBelt(Robot robot) { + this.robot = robot; + } + + private void prep(){ + robot.ringBeltMotor.setTargetPosition(targetPos); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); + robot.ringBeltStage += 1; + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); + } + + @Override + public void start() { + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + + //The first two progressions should move to preprare for another ring. + if (robot.ringBeltStage < 2) { + targetPos = currentPos + robot.ringBeltGap; + prep(); + + //The third progression needs to only move forward enought to block the ring from + //falling out + } else if (robot.ringBeltStage == 2) { + targetPos = currentPos + 150; + prep(); + prepLaunch = !robot.initLauncher; + + //If the ring belt is already full, It does allow another progression + } else if (robot.ringBeltStage > 2) { + setHasFinished(true); + } + + + } + + @Override + public void exec() { + + //finished state when the target position is reached + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + if (currentPos >= targetPos) { + if(prepLaunch) { + robot.launchMotor.setPower(Robot.LAUNCH_POWER); + } + + setHasFinished(true); + } + + //belt jam mitigation + if (robot.beltIsStuck() && childrenHaveFinished()) { + long currentTime = System.currentTimeMillis(); + if (stuckStartTime == 0) { + stuckStartTime = currentTime; + + } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) { + + addParallelState(new UnstickRingBelt(robot)); + } + } else { + stuckStartTime = 0; + } + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java new file mode 100644 index 0000000..9088a37 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java @@ -0,0 +1,57 @@ +package org.timecrafters.UltimateGoal.Competition; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; + +public class ResetRingBelt extends CyberarmState { + + private Robot robot; + boolean detectedPass; + int reducePos; + boolean reduceConditionPrev; + + public ResetRingBelt(Robot robot) { + this.robot = robot; + } + + + @Override + public void start() { + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); + reducePos = robot.ringBeltMotor.getCurrentPosition() + robot.reduceLaunchPos; + } + + @Override + public void exec() { + //detect when limit switch is initially triggered + boolean detectingPass = robot.magnetSensor.isPressed(); + int beltPos = robot.ringBeltMotor.getCurrentPosition(); + + if (detectingPass && !detectedPass) { + //finish once the ring belt has cycled all the way through and then returned to + //the first receiving position. + + + robot.ringBeltStage = 0; + robot.ringBeltMotor.setTargetPosition(beltPos); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + robot.launchMotor.setPower(0); + setHasFinished(true); + } + detectedPass = detectingPass; + + boolean reduceCondition = (beltPos > reducePos); + if (reduceCondition && !reduceConditionPrev){ + robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER); + + //the ring belt stage lets other states know that the robot has finished launching all three rings + //and is now returning to loading position. + + } + reduceConditionPrev = reduceCondition; + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java new file mode 100644 index 0000000..ccfa4aa --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java @@ -0,0 +1,757 @@ + package org.timecrafters.UltimateGoal.Competition; + +/* +The robot object contains all the hardware and functions that are used in both teleOp and +Autonomous. This includes drive functions, localization functions, shared constants, and a few +general calculations and debugging tools. +*/ + +import android.os.Environment; +import android.util.Log; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.hardware.rev.RevTouchSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; +import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import java.io.File; +import java.io.FileWriter; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX; +import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC; + +public class Robot { + + private HardwareMap hardwareMap; + + public Robot(HardwareMap hardwareMap) { + this.hardwareMap = hardwareMap; + } + + //The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit + //variables saved on the phone, without having to re-download the whole program. This is + //especially useful for autonomous route tuning + public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration(); + + //We use the IMU to get reliable rotation and angular velocity information. Experimentation has + //demonstrated that the accelerometer and related integrations aren't as accurate. + public BNO055IMU imu; + + //The LEDs are used to provide driver feedback and for looking beautiful + public RevBlinkinLedDriver ledDriver; + + //drive and dead-wheal hardware + public DcMotor driveFrontLeft; + public DcMotor driveBackLeft; + public DcMotor driveFrontRight; + public DcMotor driveBackRight; + + public DcMotor encoderLeft; + public DcMotor encoderRight; + public DcMotor encoderBack; + + //Motion Constants + + //related graphs + //https://www.desmos.com/calculator/gndnkjndu9 + //https://www.desmos.com/calculator/w0rebnftvg + //https://www.desmos.com/calculator/qxa1rq8hrv + + static final double CUBIC_CORRECTION = 0.035; + static final double FACE_CUBIC_CORRECTION = 0.025; + static final double LINEAR_CORRECTION = 0.055; + static final double FACE_MIN_CORRECTION = 0.2; + static final double FACE_LINEAR_CORRECTION = 0.025; + static final double MOMENTUM_CORRECTION = 1.05; + static final double MOMENTUM_MAX_CORRECTION = 1.4; + static final double MOMENTUM_HORIZONTAL_CORRECTION = + -(Math.log10(MOMENTUM_MAX_CORRECTION-1) / Math.log10(MOMENTUM_CORRECTION)); + static final double FACE_MOMENTUM_MAX_CORRECTION = 1.1; + static final double FACE_MOMENTUM_CORRECTION = 1.06; + static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION = + -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1) / Math.log10(FACE_MOMENTUM_CORRECTION)); + static final double ZERO_POWER_THRESHOLD = 0.25; + + //Unit Conversion Constants + static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622; + static final int COUNTS_PER_REVOLUTION = 8192; + static final float mmPerInch = 25.4f; + static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD = 12.3; + static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD = 18.8; + static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4; + static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6; + + //Robot Localization + private static double locationX; + private static double locationY; + private static float rotation; + + private int encoderLeftPrevious = 0; + private int encoderBackPrevious = 0; + private int encoderRightPrevious = 0; + private float rotationPrevious = 0; + public float angularVelocity; + + //vuforia && tensorFlow Stuff + private WebcamName webcam; + private VuforiaLocalizer vuforia; + + // Inches Forward of axis of rotation + static final float CAMERA_FORWARD_DISPLACEMENT = 8f; + // Inches above Ground + static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f; + // Inches Left of axis of rotation + static final float CAMERA_LEFT_DISPLACEMENT = 4f; + + static final double CAMERA_DISPLACEMENT_MAG = + Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT); + static final float CAMERA_DISPLACEMENT_DIRECTION = + (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT); + + public boolean trackableVisible; + private VuforiaTrackables targetsUltimateGoal; + private List trackables = new ArrayList(); + private OpenGLMatrix lastConfirmendLocation; + + private long timeStartZeroVelocity = 0; + private long minCheckDurationMs = 500; + private int minCheckVelocity = 1; + private float vuforiaRotationCull; + + //The servo mount for our camera allows us to look down for ideal TensorFlow and look up for + //ideal Vuforia Navigation + public Servo webCamServo; + public static final double CAM_SERVO_DOWN = 0.15; + + //TensorFlow Object Detection + public TFObjectDetector tfObjectDetector; + private static final float MINIMUM_CONFIDENCE = 0.8f; + + //Launcher + public DcMotor launchMotor; + public static final double LAUNCH_POWER = 0.715; + + private static final long LAUNCH_ACCEL_TIME = 500; + //These variables were originally going to be used in both autonomous and teleOp + public double launchPositionX; + public double launchPositionY; + public float launchRotation; + public int reduceLaunchPos; + + public boolean initLauncher; + + //Ring Intake + public DcMotor collectionMotor; + + //Ring Belt + public DcMotor ringBeltMotor; + public RevTouchSensor magnetSensor; + public int ringBeltStage; + public int ringBeltGap = 700; + public static final double RING_BELT_SLOW_POWER = 0.2; + public static final double RING_BELT_NORMAL_POWER = 0.6; + private int ringBeltPrev; + public long beltMaxStopTime; + public int beltReverseTicks; + public int beltMaxStopTicks; + + //Wobble Goal Arm & Grabber + public DcMotor wobbleArmMotor; + public Servo wobbleGrabServo; + public int wobbleDownPos; + public int wobbleUpPos; + public int wobbleDropPos; + public static final double WOBBLE_SERVO_OPEN = 0; + public static final double WOBBLE_SERVO_CLOSED = 1; + public RevColorSensorV3 wobbleColorSensor; + public double wobbleScoreX; + public double wobbleScoreY; + public RevTouchSensor wobbleTouchSensor; + + //Debugging + public double totalV; + private String TestingRecord = "Raw IMU, Delta, Saved"; + public double forwardVector; + public double sidewaysVector; + public double traveledForward = 0; + public DcMotorEx motorAmpsTest; + + //All our hardware initialization in one place, for everything that is the same in TeleOp and + //Autonomous + public void initHardware() { + + //drive motors + driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft"); + driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight"); + driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft"); + driveBackRight = hardwareMap.dcMotor.get("driveBackRight"); + + driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE); + driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD); + driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE); + + //Localization encoders + encoderRight = hardwareMap.dcMotor.get("driveFrontRight"); + encoderBack = hardwareMap.dcMotor.get("driveFrontLeft"); + encoderLeft = hardwareMap.dcMotor.get("collect"); + + encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + //init wobble arm + wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm"); + wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + wobbleArmMotor.setTargetPosition(0); + wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + wobbleUpPos = stateConfiguration.variable( + "system","arm", "up").value(); + wobbleDownPos = stateConfiguration.variable( + "system","arm", "down").value(); + wobbleDropPos = stateConfiguration.variable( + "system","arm", "drop").value(); + + wobbleGrabServo = hardwareMap.servo.get("wobbleGrab"); + + wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color"); + wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch"); + + + //init collection motor + collectionMotor = hardwareMap.dcMotor.get("collect"); + collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE); + + //init ring belt + ringBeltMotor = hardwareMap.dcMotor.get("belt"); + ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + magnetSensor = hardwareMap.get(RevTouchSensor.class, "magLim"); + + beltMaxStopTime = stateConfiguration.variable( + "system","belt", "maxStopTime").value(); + beltMaxStopTicks = stateConfiguration.variable( + "system","belt", "maxStopTicks").value(); + beltReverseTicks = stateConfiguration.variable( + "system","belt", "reverseTicks").value(); + ringBeltGap = stateConfiguration.variable( + "system","belt","gap").value(); + + //init IMU + imu = hardwareMap.get(BNO055IMU.class, "imu"); + + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + + parameters.mode = BNO055IMU.SensorMode.IMU; + parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; + parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; + parameters.loggingEnabled = false; + + imu.initialize(parameters); + + Velocity startVelocity = new Velocity(); + startVelocity.xVeloc = 0; + startVelocity.yVeloc = 0; + startVelocity.zVeloc = 0; + + Position startPosition = new Position(); + startPosition.x = 0; + startPosition.y = 0; + startPosition.z = 0; + + imu.startAccelerationIntegration(startPosition,startVelocity, 10); + + //Init Localization + webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + initVuforia(); + webCamServo = hardwareMap.servo.get("look"); + webCamServo.setDirection(Servo.Direction.REVERSE ); + + minCheckVelocity =stateConfiguration.variable( + "system", "camera", "minCheckV").value(); + vuforiaRotationCull = stateConfiguration.variable( + "system", "camera", "rCull").value(); + minCheckDurationMs =stateConfiguration.variable( + "system", "camera", "minCheckMS").value(); + + //Init Launch Motor + DcMotor launcher = hardwareMap.dcMotor.get("launcher"); + + MotorConfigurationType launchMotorType = launcher.getMotorType(); + launchMotorType.setGearing(3); + launchMotorType.setTicksPerRev(84); + launchMotorType.setMaxRPM(2400); + + launchMotor = launcher; + launchMotor.setMotorType(launchMotorType); + launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + //This is a debugging option that automatically turns on the launch wheel during init. + //This can be disabled using a variable in the TimeCraftersConfiguration + initLauncher = stateConfiguration.action( + "system","initLauncher").enabled; + reduceLaunchPos = stateConfiguration.variable( + "system", "launchPos", "reducePower").value(); + + if (initLauncher) { + double launcherPower = 0; + long launchAccelStart = System.currentTimeMillis(); + while (launcherPower < LAUNCH_POWER) { + launcherPower = (double) ((System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME) * LAUNCH_POWER; + launchMotor.setPower(launcherPower); + } + } + + launchPositionX = inchesToTicks((double) stateConfiguration.variable( + "system", "launchPos","x").value()); + launchPositionY = inchesToTicks((double) stateConfiguration.variable( + "system", "launchPos","y").value()); + launchRotation = stateConfiguration.variable( + "system", "launchPos","rot").value(); + + initTensorFlow(); + + ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds"); + } + + private void initVuforia() { + + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = + new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + parameters.vuforiaLicenseKey = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; + parameters.cameraName = webcam; + parameters.useExtendedTracking = false; + + + vuforia = ClassFactory.getInstance().createVuforia(parameters); + + targetsUltimateGoal = this.vuforia.loadTrackablesFromAsset("UltimateGoal"); + VuforiaTrackable redTowerGoalTarget = targetsUltimateGoal.get(1); + redTowerGoalTarget.setName("Red Tower Goal Target"); + VuforiaTrackable redAllianceTarget = targetsUltimateGoal.get(2); + redAllianceTarget.setName("Red Alliance Target"); + VuforiaTrackable frontWallTarget = targetsUltimateGoal.get(4); + frontWallTarget.setName("Front Wall Target"); + + trackables.addAll(targetsUltimateGoal); + + final float mmTargetHeight = (6) * mmPerInch; + + final float halfField = 72 * mmPerInch; + final float quadField = 36 * mmPerInch; + + redAllianceTarget.setLocation(OpenGLMatrix + .translation(0, -halfField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix( + EXTRINSIC, XYZ, DEGREES, 90, 0, 180))); + frontWallTarget.setLocation(OpenGLMatrix + .translation(-halfField, 0, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix( + EXTRINSIC, XYZ, DEGREES, 90, 0 , 90))); + redTowerGoalTarget.setLocation(OpenGLMatrix + .translation(halfField, -quadField, mmTargetHeight) + .multiplied(Orientation.getRotationMatrix( + EXTRINSIC, XYZ, DEGREES, 90, 0, -90))); + + OpenGLMatrix robotFromCamera = OpenGLMatrix + .translation(CAMERA_FORWARD_DISPLACEMENT * mmPerInch, + CAMERA_LEFT_DISPLACEMENT * mmPerInch, + CAMERA_VERTICAL_DISPLACEMENT * mmPerInch) + .multiplied(Orientation.getRotationMatrix( + EXTRINSIC, YZX, DEGREES, -90, 0, 0)); + + for (VuforiaTrackable trackable : trackables) { + ((VuforiaTrackableDefaultListener) trackable.getListener()) + .setPhoneInformation(robotFromCamera, parameters.cameraDirection); + } + + targetsUltimateGoal.activate(); + } + + public void deactivateVuforia() { + targetsUltimateGoal.deactivate(); + } + + private void initTensorFlow() { + int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId); + parameters.minResultConfidence = stateConfiguration.variable( + "system", "camera", "minConfidence").value(); + tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia); + tfObjectDetector.loadModelFromAsset( + "UltimateGoal.tflite", "Quad", "Single"); + } + + //Localization Function! This function is represented in a flow diagram, earlier in the + //software section + //run this in every exec to track the robot's location. + public void updateLocation(){ + + // orientation is inverted to have clockwise be positive. + float imuAngle = -imu.getAngularOrientation().firstAngle; + double rotationChange = getRelativeAngle(rotationPrevious, imuAngle); + + int encoderLeftCurrent = -encoderLeft.getCurrentPosition(); + int encoderRightCurrent = encoderRight.getCurrentPosition(); + int encoderBackCurrent = encoderBack.getCurrentPosition(); + + double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious; + double encoderRightChange = encoderRightCurrent - encoderRightPrevious; + double encoderBackChange = encoderBackCurrent - encoderBackPrevious; + + encoderLeftPrevious = encoderLeftCurrent; + encoderRightPrevious = encoderRightCurrent; + encoderBackPrevious = encoderBackCurrent; + rotationPrevious = imuAngle; + + //The forward Vector has the luxury of having an odometer on both sides of the robot. + //This allows us to reduce the unwanted influence of turning the robot by averaging + //the two. unfortunatly we the current positioning of the odometers + + //Since there isn't a second wheel to remove the influence of robot rotation, we have to + //instead do this by approximating the number of ticks that were removed due to rotation + //based on a separate calibration program. + + double ticksPerDegreeForward; + double ticksPerDegreeSideways; + + if (rotationChange < 0) { + ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE; + ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD; + } else { + ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_CLOCKWISE; + ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD; + } + + forwardVector = ((encoderLeftChange+encoderRightChange)/2) - + (rotationChange* ticksPerDegreeForward); + + traveledForward += forwardVector; + sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways); + + double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector)); + double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) + + Math.atan2(sidewaysVector,forwardVector); + + double xChange = magnitude * (Math.sin(direction)); + double yChange = magnitude * (Math.cos(direction)); + + locationX += xChange; + locationY += yChange; + + Robot.rotation += rotationChange; + Robot.rotation = scaleAngleRange(Robot.rotation); + + totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + + Math.abs(encoderBackChange); + +// record(""+System.currentTimeMillis()+", "+imuAngle); + } + + //Experimentation has demonstrated that Vuforia dosen't provide good data while the camera + //is moving or rotating. This function detects if conditions are appropriate to run vuforia and + //get more accurate results + public void syncIfStationary() { + if (totalV < minCheckVelocity) { + long timeCurrent = System.currentTimeMillis(); + + if (timeStartZeroVelocity == 0) { + timeStartZeroVelocity = timeCurrent; + } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs ) { + syncWithVuforia(); + } + + } else { + timeStartZeroVelocity = 0; + } + + } + + public void syncWithVuforia() { + trackableVisible = false; + for (VuforiaTrackable trackable : trackables) { + if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) { + OpenGLMatrix robotLocation = + ((VuforiaTrackableDefaultListener)trackable.getListener()) + .getUpdatedRobotLocation(); + + //this is used for debugging purposes. + trackableVisible = true; + + if (robotLocation != null) { + lastConfirmendLocation = robotLocation; + } + + //For our tournament, it makes sense to make zero degrees towards the goal. + //Orientation is inverted to have clockwise be positive. + Orientation orientation = + Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES); + float vuforiaRotation = 90-orientation.thirdAngle; + + if (vuforiaRotation > 180) { + vuforiaRotation -= -180; + } + + if (Math.abs(rotation - vuforiaRotation) < vuforiaRotationCull) { + rotation = vuforiaRotation; + + VectorF translation = lastConfirmendLocation.getTranslation(); + double camX = -translation.get(1) / mmPerInch; + double camY = translation.get(0) / mmPerInch; + + double displaceX = CAMERA_DISPLACEMENT_MAG * + Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION); + double displaceY = CAMERA_DISPLACEMENT_MAG * + Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION); + + locationX = inchesToTicks(camX - displaceX); + locationY = inchesToTicks(camY - displaceY); + + break; + } + } + } + } + + public float getRotation() { + return Robot.rotation; + } + + public double getLocationX() { + return Robot.locationX; + } + + public double getLocationY() { + return Robot.locationY; + } + + //This is meant to only be used to indicate starting positions and to reorient the robot. + public void setLocalization(float rotation, double x, double y) { + Robot.rotation = rotation; + Robot.locationX = x; + Robot.locationY = y; + } + + + //returns the angle from the robot's current position to the given target position. + public float getAngleToPosition (double x, double y) { + double differenceX = x- getLocationX(); + double differenceY = y- getLocationY(); + double angle = Math.toDegrees(Math.atan2(differenceX, differenceY )); + + return (float) angle; + + } + + //Unit conversions + public double ticksToInches(double ticks) { + return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION); + } + + public double inchesToTicks(double inches) { + return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE); + + } + + //Returns the shortest angle between two directions, with positive angles indicating that the + // reference is to the right (clockwise) of the current. Negative angles indicate that the + // reference is to the left. + public float getRelativeAngle(float reference, float current) { + return scaleAngleRange(current - reference); + } + + //Drive Functions + public void setDrivePower(double powerFrontLeft, double powerFrontRight, + double powerBackLeft, double powerBackRight){ + driveFrontLeft.setPower(powerFrontLeft); + driveFrontRight.setPower(powerFrontRight); + driveBackLeft.setPower(powerBackLeft); + driveBackRight.setPower(powerBackRight); + } + + //returns an array of the powers necessary to execute the provided motion. + //"degreesDirectionMotion" is the angle relative to the field that the robot should drive at. + //"degreesDirectionFace" is the angle the robot should face relative to the field. The order of + //the output powers is ForwardLeft, ForwardRight, BackLeft, BackRight + public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, + float degreesDirectionFace) { + angularVelocity = imu.getAngularVelocity().xRotationRate; + + //calculating the base mecanum powers so that the robot drives along the degreesDirectionMotion + //once it is pointed towards the degreesDirectionFace + double rad = Math.toRadians(getRelativeAngle(degreesDirectionFace,degreesDirectionMotion)); + double y = Math.cos(rad); + double x = Math.sin(rad); + + double p = y + x; + double q = y - x; + + + + //calculating correction needed to steer the robot towards the degreesDirectionFace + float relativeRotation = + getRelativeAngle(degreesDirectionFace, Robot.rotation); + double turnCorrection = + Math.pow(CUBIC_CORRECTION * relativeRotation, 3) + + LINEAR_CORRECTION * relativeRotation; + + if (relativeRotation != 0) { + double momentumRelative = angularVelocity * + (relativeRotation / Math.abs(relativeRotation)); + double exponential = + Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); + double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential); + //reduces concern for momentum when the angle is far away from target + turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) * + (1 - momentumCorrection)) / 180 ); + } + + double powerForwardRight = scalar * (q + turnCorrection); + double powerForwardLeft = scalar * (p - turnCorrection); + double powerBackRight = scalar * (p + turnCorrection); + double powerBackLeft = scalar * (q - turnCorrection); + + + // The "extreme" is the power value that is furthest from zero. When this values exceed the + // -1 to 1 power range, dividing the powers by the "extreme" scales everything back into the + // workable range without altering the final motion vector. + + double extreme = Math.max( + Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)), + Math.max(Math.abs(powerBackRight),Math.abs(powerBackLeft))); + + if (extreme > 1) { + powerForwardRight = powerForwardRight/extreme; + powerForwardLeft = powerForwardLeft/extreme; + powerBackRight = powerBackRight/extreme; + powerBackLeft = powerBackLeft/extreme; + } + + double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight}; + + double totalPower = Math.abs(powerForwardLeft) + + Math.abs(powerForwardRight) + + Math.abs(powerBackLeft) + + Math.abs(powerBackRight); + if (totalPower < ZERO_POWER_THRESHOLD) { + powers = new double[] {0,0,0,0}; + } + + + return powers; + } + + //Outputs the power necessary to turn and face a provided direction + public double[] getFacePowers(float direction, double power) { + angularVelocity = imu.getAngularVelocity().xRotationRate; + double relativeAngle = getRelativeAngle(direction, Robot.rotation); + double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) + + FACE_LINEAR_CORRECTION * relativeAngle; + + if (relativeAngle > 0.5) { + scaler += FACE_MIN_CORRECTION; + } else if (relativeAngle < -0.5) { + scaler -= FACE_MIN_CORRECTION; + } + + if (relativeAngle != 0) { + double momentumRelative = angularVelocity * (relativeAngle / Math.abs(relativeAngle)); + double exponential = Math.pow(FACE_MOMENTUM_CORRECTION, + FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative); + double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential); + + scaler *= momentumCorrection; + } + + double left = -power * scaler; + double right = power *scaler; + + double[] powers = {left,right}; + + double totalPower = 2 * (Math.abs(left) + Math.abs(right)); + if (totalPower < ZERO_POWER_THRESHOLD) { + powers = new double[] {0,0}; + } + + return powers; + } + + //Used to for automated jam countermeasures + public boolean beltIsStuck() { + int ringBeltPos = ringBeltMotor.getCurrentPosition(); + boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks); + ringBeltPrev = ringBeltPos; + return notMoved; + } + + public float scaleAngleRange(float angle) { + if (angle < -180) { + angle += 360; + } + if (angle > 180) { + angle -= 360; + } + return angle; + } + + //Debugging tools + public void record(String record) { + TestingRecord+="\n"+record; + } + + public void saveRecording() { + writeToFile(Environment.getExternalStorageDirectory().getAbsolutePath()+File.separator+"TimeCrafters_TestingRecord"+File.separator+"RobotTestingRecord.txt", TestingRecord); + } + + public boolean writeToFile(String filePath, String content) { + try { + + FileWriter writer = new FileWriter(filePath); + writer.write(content); + writer.close(); + + return true; + + } catch (IOException e) { + Log.e("RecordTest", e.getLocalizedMessage()); + return false; + } + } + + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java new file mode 100644 index 0000000..180f648 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java @@ -0,0 +1,57 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Launch; +import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; +import org.timecrafters.UltimateGoal.Competition.ResetRingBelt; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class LaunchControl extends CyberarmState { + + private Robot robot; + private boolean ranLaunch; + + public LaunchControl(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + + if (robot.ringBeltStage > 2) { + if (robot.ringBeltStage > 4) { + addParallelState(new ResetRingBelt(robot)); + } else { + addParallelState(new LaunchSingle(robot)); + } + ranLaunch = true; + } + } + + @Override + public void exec() { + if (childrenHaveFinished()) { + if (!ranLaunch) { + if (robot.ringBeltStage <= 2) { + addParallelState(new ProgressRingBelt(robot)); + } else { + addParallelState(new LaunchSingle(robot)); + ranLaunch = true; + } + } else { + setHasFinished(true); + } + } + } + + @Override + public void telemetry() { + + engine.telemetry.addData("Launch Control children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java new file mode 100644 index 0000000..337b662 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java @@ -0,0 +1,52 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Robot; +import org.timecrafters.UltimateGoal.Competition.UnstickRingBelt; + +public class LaunchSingle extends CyberarmState { + + private Robot robot; + boolean hasCycled; + boolean detectedPass; + private int targetPos; + private boolean reduceConditionPrev; + private double reducePos; + private long stuckStartTime; + + public LaunchSingle(Robot robot) { + this.robot = robot; + } + + + @Override + public void start() { + robot.ringBeltStage +=1; + robot.ringBeltMotor.setTargetPosition(robot.ringBeltMotor.getCurrentPosition() + robot.ringBeltGap); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER); + } + + @Override + public void exec() { + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + if (currentPos >= targetPos) { + setHasFinished(true); + } + + if (robot.beltIsStuck() && childrenHaveFinished()) { + long currentTime = System.currentTimeMillis(); + if (stuckStartTime == 0) { + stuckStartTime = currentTime; + } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) { + addParallelState(new UnstickRingBelt(robot)); + } + } else { + stuckStartTime = 0; + } + + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java new file mode 100644 index 0000000..d25a9d9 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java @@ -0,0 +1,260 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +/* +The Player1 state has all the controls for player one. The Player One and Player Two controls are +kept in separate states so that the childrenHaveFinished() method can be used to easily stop things +from running at the same time that shouldn't be. +*/ + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class Player1 extends CyberarmState { + private Robot robot; + + //normal drive control + private float leftJoystickDegrees; + private double leftJoystickMagnitude; + private float rightJoystickDegrees; + private double rightJoystickMagnitude; + private double rightJoystickMagnitudePrevious; + + private double faceControlThreshold; + private float cardinalSnapping; + private float pairSnapping; + + private float faceDirection = 0; + private double[] powers = {0,0,0,0}; + private double drivePower = 1; + private boolean lbPrev; + + //find wobble goal control + private FindWobbleGoal findWobbleGoal; + private boolean runNextFindWobble; + private boolean findWobbleInputPrev; + private boolean aPrev; + + //Drive to launch control + private powerShotsControl powerShots; + private boolean runNextDriveToLaunch; + private boolean driveToLaunchInputPrev; + + private double endGameX; + private double endGameY; + private float endGameRot; + private int loopCount; + + private double refinePower; + + public Player1(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + cardinalSnapping = robot.stateConfiguration.variable( + "tele","control", "cardinalSnapping").value(); + pairSnapping = robot.stateConfiguration.variable( + "tele","control", "pairSnapping").value(); + faceControlThreshold = robot.stateConfiguration.variable( + "tele","control", "faceControlT").value(); + refinePower = robot.stateConfiguration.variable( + "tele","control", "refPower").value(); + + endGameX = robot.stateConfiguration.variable( + "tele","_endGameStart","x").value(); + endGameY = robot.stateConfiguration.variable( + "tele","_endGameStart","y").value(); + endGameRot = robot.stateConfiguration.variable( + "tele","_endGameStart", "r").value(); + } + + @Override + public void start() { + faceDirection = robot.getRotation(); + } + + @Override + public void exec() { + loopCount+=1; + robot.record(""+runTime()+", "+loopCount+", "+robot.getRotation()); + robot.updateLocation(); + + //toggle for drive speed + boolean lb = engine.gamepad1.left_stick_button; + if (lb && !lbPrev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + lbPrev = lb; + + //This Runs the FindWobbleGoal state as long as the button is pressed. When it's released, + //the state is interrupted. If the state finishes while the button is still pressed, + //it waits until the button is released before running the state again + runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished()); + + boolean findWobbleInput = engine.gamepad1.x; + if (findWobbleInput) { + //if the claw is open, run FindWobbleGoal + if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) { + + faceDirection = robot.getRotation(); + + if (runNextFindWobble && !findWobbleInputPrev) { + findWobbleGoal = + new FindWobbleGoal(robot, "auto", "08_0"); + addParallelState(findWobbleGoal); + } + //if the claw is closed, open the claw. + } else if (!findWobbleInputPrev) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN); + } + //if the button is released cancel the search + } else if (!runNextFindWobble) { + findWobbleGoal.setHasFinished(true); + } + findWobbleInputPrev = findWobbleInput; + + //toggles wobble grabber open and closed + boolean a = engine.gamepad1.a; + if (a && !aPrev) { + if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + } else { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN); + } + } + aPrev = a; + + //This logic works the same as the stuff above, accept instead of autonomous wobble grab, + //it + runNextDriveToLaunch = (powerShots == null || powerShots.getHasFinished()); + + boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput; + if (driveToLaunchInput) { + if (runNextDriveToLaunch && !driveToLaunchInputPrev) { + powerShots = new powerShotsControl(robot); + addParallelState(powerShots); + } + faceDirection = robot.getRotation(); + } else if (!runNextDriveToLaunch) { + powerShots.setHasFinished(true); + } + driveToLaunchInputPrev = driveToLaunchInput; + + //Normal Driver Controls + if (childrenHaveFinished()) { + + + double leftJoystickX = engine.gamepad1.left_stick_x; + double leftJoystickY = engine.gamepad1.left_stick_y; + + leftJoystickDegrees = robot.getRelativeAngle(90, + (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY))); + leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY); + + double rightJoystickX = engine.gamepad1.right_stick_x; + double rightJoystickY = engine.gamepad1.right_stick_y; + + rightJoystickDegrees = robot.getRelativeAngle(90, + (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY))); + rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); + + //allows the the driver to indicate which direction the robot is currently looking + //so that the controller and robot can be re-synced in the event of a bad initial + //rotation. + if (engine.gamepad1.back) { + float newRotation = 0; + + if (rightJoystickMagnitude != 0) { + newRotation = rightJoystickDegrees; + } + + robot.setLocalization(newRotation, robot.getLocationX(), robot.getLocationY()); + faceDirection = newRotation; + } + + //if the driver is letting go of the face joystick, the robot should maintain it's + //current face direction. + if (rightJoystickMagnitude > faceControlThreshold || + rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) { + + //if the joystick is close to one of the cardinal directions, it is set to exactly + // the cardinal direction + faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0); + } + rightJoystickMagnitudePrevious = rightJoystickMagnitude; + + //The D-pad is used if the drivers need to get a more precise angle than they can get + //using the face joystick + if (engine.gamepad1.dpad_right) { + powers = new double[]{refinePower, -refinePower, refinePower, -refinePower}; + faceDirection = robot.getRotation(); + } else if (engine.gamepad1.dpad_left) { + powers = new double[]{-refinePower, refinePower, -refinePower, refinePower}; + faceDirection = robot.getRotation(); + } else if (leftJoystickMagnitude == 0) { + double[] facePowers = robot.getFacePowers(faceDirection, 0.4); + powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; + } else { + //drives the robot in the direction of the move joystick while facing the direction + //of the look joystick. if the move direction is almost aligned/perpendicular to the + //look joystick, + powers = robot.getMecanumPowers( + snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), + drivePower, faceDirection); + } + + robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]); + } + + //LED feedback control + double ringBeltPower = robot.ringBeltMotor.getPower(); + if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - + robot.ringBeltMotor.getCurrentPosition()) > 10) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE ); + } else if (ringBeltPower < 0) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED); + } else { + if (drivePower == 1) { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); + } else { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine("Controler Directions"); + engine.telemetry.addData("Right", rightJoystickDegrees); + engine.telemetry.addData("Left", leftJoystickDegrees); + + engine.telemetry.addData("face", faceDirection); + + engine.telemetry.addData("Player 1 children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } + } + + private float snapToCardinal(float angle, float snapping, float offset) { + int o = (int) offset + 180; + o %= 90; + for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) { + if (angle >= cardinal-snapping && angle <= cardinal+snapping) { + angle = cardinal; + } + } + return angle; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java new file mode 100644 index 0000000..70123f7 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java @@ -0,0 +1,189 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +/* +The Player2 state has all the controls for player two. This includes a lot of automation and +emergency features, for if the driver wants to take control unexpectedly +*/ + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.timecrafters.UltimateGoal.Competition.Launch; +import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class Player2 extends CyberarmState { + private Robot robot; + + private boolean rbPrev; + private boolean yPrev; + private boolean xPrev; + private boolean bPrev; + private boolean wobbleArmUp = false; + private boolean wobbleGrabOpen = false; + private boolean aPrev; + private double beltPowerPrev; + private DcMotor.RunMode runModePrev; + private boolean lbPrev; + private boolean manualArmHold; + private int loops = 0; + + + + private boolean launchInput = false; + + public Player2(Robot robot) { + this.robot = robot; + } + + + @Override + public void init() { + robot.wobbleArmMotor.setTargetPosition(0); + robot.wobbleArmMotor.setPower(0.5); + } + + @Override + public void start() { + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN); + } + + @Override + public void exec() { +// loops+=1; +// robot.record(""+runTime()+", "+loops+", "+robot.getRotation()); + + //Collector control + double rt = engine.gamepad2.right_trigger; + double lt = engine.gamepad2.left_trigger; + if (rt < lt) { + robot.collectionMotor.setPower(-lt); + } else if (childrenHaveFinished()) { + robot.collectionMotor.setPower(rt); + } else { + robot.collectionMotor.setPower(0); + } + + //The childrenHaveFinished() method tracks if there are parallel states that are still + //running. + if (childrenHaveFinished()) { + //belt progression control + boolean rb = engine.gamepad2.right_bumper; + if (rb && !rbPrev) { + addParallelState(new ProgressRingBelt(robot)); + } + rbPrev = rb; + + //main launch sequence control + boolean y2 = engine.gamepad2.y; + if (y2 && !yPrev) { + addParallelState(new Launch(robot)); + } + yPrev = y2; + + + //special launch sequence for single shots + boolean x = engine.gamepad2.x; + if (x && !bPrev) { + addParallelState(new LaunchControl(robot)); + } + bPrev = x; + } + + //manually control the wobble arm for when it's initialized in an unexpected position. + double leftStickY = engine.gamepad2.left_stick_y; + + if (engine.gamepad2.dpad_right) { + if (!robot.wobbleTouchSensor.isPressed()) { + setArmMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.wobbleArmMotor.setPower(-0.2); + robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition()); + } else { + setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.wobbleArmMotor.setTargetPosition(0); + setArmMode(DcMotor.RunMode.RUN_TO_POSITION); + } + } else if (leftStickY != 0 ) { + setArmMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.wobbleArmMotor.setPower(0.15 * leftStickY); + robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition()); + } else { + setArmMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.wobbleArmMotor.setPower(0.5); + if (engine.gamepad2.dpad_up) { + robot.wobbleArmMotor.setTargetPosition(robot.wobbleUpPos); + } else if (engine.gamepad2.dpad_down) { + robot.wobbleArmMotor.setTargetPosition(robot.wobbleDownPos); + } else if (engine.gamepad2.dpad_left) { + robot.wobbleArmMotor.setTargetPosition(robot.wobbleDropPos ); + } + } + + //manually toggle the launch wheel for emergencies + boolean a = engine.gamepad2.a; + if (a && !aPrev) { + if (robot.launchMotor.getPower() == 0) { + robot.launchMotor.setPower(Robot.LAUNCH_POWER); + } else { + robot.launchMotor.setPower(0); + } + } + aPrev = a; + + + + + //allows the driver to revers the belt in the event of a jam + boolean lb = engine.gamepad2.left_bumper; + if (lb && !lbPrev) { + runModePrev = robot.ringBeltMotor.getMode(); + beltPowerPrev = robot.ringBeltMotor.getPower(); + robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER); + } + + if (!lb && lbPrev) { + robot.ringBeltMotor.setPower(beltPowerPrev); + robot.ringBeltMotor.setMode(runModePrev); + } + lbPrev = lb; + + if (engine.gamepad2.back) { + for (CyberarmState state : children) { + engine.stopState(state); + } + } + + } + + + //This just checks if the wobble arm runmode is already the desired mode before setting it. + //I don't know if this is actually helpful + private void setArmMode(DcMotor.RunMode runMode) { + if (robot.wobbleArmMotor.getMode() != runMode) { + robot.wobbleArmMotor.setMode(runMode); + } + } + + + @Override + public void telemetry() { +// engine.telemetry.addLine("belt"); +// engine.telemetry.addData("power", robot.ringBeltMotor.getPower()); +// engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition()); +// engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition()); +// +// engine.telemetry.addData("ring belt stage", robot.ringBeltStage); +// engine.telemetry.addData("Touch Sensor Pressed", robot.wobbleTouchSensor.isPressed()); +// engine.telemetry.addData(" Sensor value", robot.wobbleTouchSensor.getValue()); + engine.telemetry.addData("Player 2 children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java new file mode 100644 index 0000000..f63409c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java @@ -0,0 +1,38 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Autonomous.Face; +import org.timecrafters.UltimateGoal.Competition.Robot; +@Disabled +@TeleOp (name = "TeleOp",group = "comp") +public class TeleOpEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void init() { + robot = new Robot(hardwareMap); + robot.initHardware(); + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + robot.webCamServo.setPosition(0); + super.init(); + } + + @Override + public void setup() { + addState(new TeleOpState(robot)); + + + } + + @Override + public void stop() { + robot.deactivateVuforia(); + robot.saveRecording(); + super.stop(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java new file mode 100644 index 0000000..c32801e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java @@ -0,0 +1,43 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; +import org.timecrafters.UltimateGoal.Competition.Launch; +import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt; +import org.timecrafters.UltimateGoal.Competition.Robot; + +public class TeleOpState extends CyberarmState { + + private Robot robot; + + public TeleOpState(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + addParallelState(new Player1(robot)); + addParallelState(new Player2(robot)); + } + + @Override + public void exec() { +// setHasFinished(childrenHaveFinished()); + } + + @Override + public void telemetry() { + + engine.telemetry.addLine("Location"); + engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")"); + engine.telemetry.addData("Rotation ", robot.getRotation()); + } + + private float round(double number,double unit) { + return (float) (Math.floor(number/unit) * unit); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java new file mode 100644 index 0000000..c53dc7b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java @@ -0,0 +1,80 @@ +package org.timecrafters.UltimateGoal.Competition.TeleOp; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates; +import org.timecrafters.UltimateGoal.Competition.Autonomous.Face; +import org.timecrafters.UltimateGoal.Competition.Pause; +import org.timecrafters.UltimateGoal.Competition.Robot; + +import java.util.ArrayList; + +public class powerShotsControl extends CyberarmState { + + private Robot robot; + + private double endGameX; + private double endGameY; + private float endGameRot; + + private double endGamePower; + + private int nextState = 0; + + private ArrayList states = new ArrayList(); + + public powerShotsControl(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value(); + endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value(); + endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value(); + endGamePower = robot.stateConfiguration.variable("tele","_endGameStart", "power").value(); + + endGameX = robot.inchesToTicks(endGameX); + endGameY = robot.inchesToTicks(endGameY); + endGameRot = (float) robot.inchesToTicks(endGameRot); + + states.add(new Pause(robot, "tele","_endGameStart")); + states.add(new DriveToCoordinates(robot, "tele", "_pow1")); + states.add(new Face(robot,"tele","_faceZero")); + states.add(new LaunchControl(robot)); + states.add(new DriveToCoordinates(robot, "tele", "_pow2")); + states.add(new Face(robot,"tele","_faceZero")); + states.add(new LaunchControl(robot)); + states.add(new DriveToCoordinates(robot, "tele", "_pow3")); + states.add(new Face(robot,"tele","_faceZero")); + states.add(new LaunchControl(robot)); + } + + @Override + public void start() { + robot.setLocalization(endGameRot,endGameX,endGameY); + robot.launchMotor.setPower(endGamePower); + } + + @Override + public void exec() { + if (childrenHaveFinished()) { + if (nextState < states.size()) { + addParallelState(states.get(nextState)); + nextState += 1; + } else { + robot.launchMotor.setPower(0); + setHasFinished(true); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("Power shots children", childrenHaveFinished()); + for (CyberarmState state : children) { + if (!state.getHasFinished()) { + engine.telemetry.addLine("" + state.getClass()); + } + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java new file mode 100644 index 0000000..825d805 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java @@ -0,0 +1,37 @@ +package org.timecrafters.UltimateGoal.Competition; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; + +public class UnstickRingBelt extends CyberarmState { + + private Robot robot; + private int targetPos; + private double lastPower; + private DcMotor.RunMode lastRunMode; + + public UnstickRingBelt(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + lastPower = robot.ringBeltMotor.getPower(); + lastRunMode = robot.ringBeltMotor.getMode(); + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + targetPos = currentPos - robot.beltReverseTicks; + robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER); + } + + @Override + public void exec() { + int currentPos = robot.ringBeltMotor.getCurrentPosition(); + + if (currentPos < targetPos) { + robot.ringBeltMotor.setPower(lastPower); + setHasFinished(true); + } + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java new file mode 100644 index 0000000..4bf2ce1 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java @@ -0,0 +1,55 @@ +package org.timecrafters.UltimateGoal.Competition; + +import org.cyberarm.engine.V2.CyberarmState; + +public class WobbleArm extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private int suggestedPos; + private int targetPos = -1; + private long waitTime = -1; + + public WobbleArm(Robot robot, String groupName, String actionName, int targetPos) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + this.suggestedPos = targetPos; + } + + public WobbleArm(Robot robot, int targetPos, long waitTime) { + this.robot = robot; + this.targetPos = targetPos; + this.waitTime = waitTime; + } + + @Override + public void init() { + if (waitTime == -1) { + waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value(); + targetPos = robot.stateConfiguration.variable(groupName,actionName,"armPos").value(); + if (targetPos == -1) { + targetPos = suggestedPos; + } + } + } + + @Override + public void start() { + robot.wobbleArmMotor.setPower(0.5); + robot.wobbleArmMotor.setTargetPosition(targetPos); + } + + @Override + public void exec() { + setHasFinished(runTime() > waitTime); + } + + @Override + public void telemetry() { + engine.telemetry.addData("runTime", runTime()); + engine.telemetry.addData("target", targetPos); + engine.telemetry.addData("wait", waitTime); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java new file mode 100644 index 0000000..e24f75d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java @@ -0,0 +1,59 @@ +package org.timecrafters.UltimateGoal.Competition; + +import org.cyberarm.engine.V2.CyberarmState; + +public class WobbleGrab extends CyberarmState { + + private Robot robot; + private String groupName; + private String actionName; + private boolean open; + private long waitTime = -1; + private boolean enabled; + + public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + this.open = open; + } + + public WobbleGrab(Robot robot, boolean open, long waitTime) { + this.robot = robot; + this.open = open; + this.waitTime = waitTime; + } + + @Override + public void init() { + if (waitTime == -1) { + enabled = robot.stateConfiguration.action(groupName, actionName).enabled; + waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value(); + } + } + + @Override + public void start() { + if (enabled) { + if (open) { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN); + } else { + robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED); + } + } else { + setHasFinished(true); + } + } + + @Override + public void exec() { + setHasFinished(runTime() > waitTime); + } + + @Override + public void telemetry() { + engine.telemetry.addData("runTime", runTime()); + engine.telemetry.addData("wait", waitTime); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java new file mode 100644 index 0000000..b8b2b37 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java @@ -0,0 +1,76 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.Acceleration; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; +import org.timecrafters.UltimateGoal.Competition.Robot; + + +public class ControlHubTest extends CyberarmState { + + private Robot robot; + + private Acceleration acceleration; + private double Vx = 0; + private double Vy = 0; + private double Vz = 0; + private double aVx = 0; + private double aVy = 0; + private double aVz = 0; + + + public ControlHubTest(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + + Velocity startVelocity = new Velocity(); + startVelocity.xVeloc = 0; + startVelocity.yVeloc = 0; + startVelocity.zVeloc = 0; + + Position startPosition = new Position(); + startPosition.x = 0; + startPosition.y = 0; + startPosition.z = 0; + + robot.imu.startAccelerationIntegration(startPosition,startVelocity, 200); + } + + @Override + public void exec() { + acceleration = robot.imu.getAcceleration(); + + Vx = robot.imu.getVelocity().xVeloc; + Vy = robot.imu.getVelocity().yVeloc; + Vz = robot.imu.getVelocity().zVeloc; + + aVx = robot.imu.getAngularVelocity().xRotationRate; + aVy = robot.imu.getAngularVelocity().yRotationRate; + aVz = robot.imu.getAngularVelocity().zRotationRate; + + + } + + @Override + public void telemetry() { + engine.telemetry.addLine("Acceleration"); + engine.telemetry.addData("x", acceleration.xAccel); + engine.telemetry.addData("y", acceleration.yAccel); + engine.telemetry.addData("z", acceleration.zAccel); + + engine.telemetry.addLine("Velocity"); + engine.telemetry.addData("X", Vx); + engine.telemetry.addData("Y", Vy); + engine.telemetry.addData("Z", Vz); + + engine.telemetry.addLine("Angular Velocity"); + engine.telemetry.addData("X", aVx); + engine.telemetry.addData("Y", aVy); + engine.telemetry.addData("Z", aVz); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java new file mode 100644 index 0000000..664c945 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java @@ -0,0 +1,37 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.robotcore.hardware.ColorRangeSensor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + + +public class DistanceSensorTest extends CyberarmState { + + + private Rev2mDistanceSensor distanceSensor; +// private RevColorSensorV3 colorSensor; + private ColorRangeSensor colorSensor; + private double distance = 0; + private double color = 0; + + @Override + public void init() { + distanceSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "distance"); + colorSensor = engine.hardwareMap.get(ColorRangeSensor.class, "color"); +// colorSensor = engine.hardwareMap.get(RevColorSensorV3.class, "color"); + } + + @Override + public void exec() { + distance = distanceSensor.getDistance(DistanceUnit.MM); + color = colorSensor.getDistance(DistanceUnit.MM); + } + + @Override + public void telemetry() { + engine.telemetry.addData("distance", distance); + engine.telemetry.addData("color", color); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java new file mode 100644 index 0000000..a6d28c9 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java @@ -0,0 +1,28 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.Servo; + +import org.cyberarm.engine.V2.CyberarmState; + + +public class LEDTest extends CyberarmState { + + RevBlinkinLedDriver leds; + + @Override + public void init() { + leds = engine.hardwareMap.get(RevBlinkinLedDriver.class, "leds"); + } + + @Override + public void exec() { + setHasFinished(false); + if (engine.gamepad1.x) { + leds.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); + } else { + leds.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN ); + } + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java new file mode 100644 index 0000000..eaeaf3c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/MecanumFunctionTest.java @@ -0,0 +1,137 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; +import org.timecrafters.UltimateGoal.Competition.Robot; + + +public class MecanumFunctionTest extends CyberarmState { + + private Robot robot; + private float leftJoystickDegrees; + private double leftJoystickMagnitude; + private float rightJoystickDegrees; + private double rightJoystickMagnitude; + private double powerFrontLeft=0; + private double powerFrontRight=0; + private double powerBackLeft=0; + private double powerBackRight=0; + + private double aVx = 0; + private double aVy = 0; + private double aVz = 0; + + private int powerStep = 5; + private double POWER_SCALE = 0.5 ; + private boolean toggleSpeedInput = false; + + public MecanumFunctionTest(Robot robot) { + this.robot = robot; + } + + @Override + public void init() { + Velocity startVelocity = new Velocity(); + startVelocity.xVeloc = 0; + startVelocity.yVeloc = 0; + startVelocity.zVeloc = 0; + + Position startPosition = new Position(); + startPosition.x = 0; + startPosition.y = 0; + startPosition.z = 0; + + robot.imu.startAccelerationIntegration(startPosition,startVelocity, 10); + } + + @Override + public void exec() { + robot.updateLocation(); +// robot.record();///////////// + + AngularVelocity angularVelocity = robot.imu.getAngularVelocity(); + + aVx = angularVelocity.xRotationRate; + aVy = angularVelocity.yRotationRate; + aVz = angularVelocity.zRotationRate; + + double leftJoystickX = engine.gamepad1.left_stick_x; + double leftJoystickY = engine.gamepad1.left_stick_y; + + leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)); + leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY); + + double rightJoystickX = engine.gamepad1.right_stick_x; + double rightJoystickY = engine.gamepad1.right_stick_y; + + rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); + rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); + + powerFrontLeft = 0; + powerFrontRight = 0; + powerBackLeft = 0; + powerBackRight = 0; + + boolean a = engine.gamepad1.a; + boolean b = engine.gamepad1.b; + + if (a && !toggleSpeedInput && POWER_SCALE < 1) { + powerStep += 1; + POWER_SCALE = powerStep * 0.1; + } + + if (b && !toggleSpeedInput && POWER_SCALE > 0.1) { + powerStep -= 1; + POWER_SCALE = powerStep * 0.1; + } + + toggleSpeedInput = a || b; + + if (rightJoystickMagnitude == 0) { + if (leftJoystickMagnitude !=0) { + double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, leftJoystickDegrees); + powerFrontLeft = powers[0]; + powerFrontRight = powers[1]; + powerBackLeft = powers[2]; + powerBackRight = powers[3]; + + } + } else { + if (leftJoystickMagnitude == 0) { + double[] powers = robot.getFacePowers(rightJoystickDegrees, POWER_SCALE * rightJoystickMagnitude); + powerFrontLeft = powers[0]; + powerFrontRight = powers[1]; + powerBackLeft = powers[0]; + powerBackRight = powers[1]; + } else { + double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, rightJoystickDegrees); + powerFrontLeft = powers[0]; + powerFrontRight = powers[1]; + powerBackLeft = powers[2]; + powerBackRight = powers[3]; + } + } + + robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight); + + + } + + @Override + public void telemetry() { + engine.telemetry.addData("Scale", POWER_SCALE); + + engine.telemetry.addLine("Angular Velocity"); + engine.telemetry.addData("X", aVx); + engine.telemetry.addData("Y", aVy); + engine.telemetry.addData("Z", aVz); + + engine.telemetry.addLine("Powers"); + engine.telemetry.addData("FL", robot.driveFrontLeft.getPower()); + engine.telemetry.addData("FR", robot.driveFrontRight.getPower()); + engine.telemetry.addData("BL", robot.driveBackLeft.getPower()); + engine.telemetry.addData("BR", robot.driveBackRight.getPower()); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java new file mode 100644 index 0000000..71d5579 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java @@ -0,0 +1,35 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.robotcore.hardware.Servo; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.Acceleration; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; +import org.timecrafters.UltimateGoal.Competition.Robot; + + +public class ServoPosTest extends CyberarmState { + + private Servo servo; + private double servoPos; + + public ServoPosTest() { + } + + @Override + public void init() { + servo = engine.hardwareMap.servo.get("look"); + } + + @Override + public void exec() { + servoPos = engine.gamepad1.right_stick_y; + servo.setPosition(servoPos); + } + + @Override + public void telemetry() { + engine.telemetry.addData("Position", servoPos); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java new file mode 100644 index 0000000..2d55990 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java @@ -0,0 +1,19 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal; +import org.timecrafters.UltimateGoal.Competition.Robot; + +@TeleOp (name = "Hardware test", group = "test") +public class TestingEngine extends CyberarmEngine { + + private Robot robot; + + @Override + public void setup() { + addState(new ServoPosTest()); + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WobbleArmTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WobbleArmTest.java new file mode 100644 index 0000000..2c07e08 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WobbleArmTest.java @@ -0,0 +1,30 @@ +package org.timecrafters.UltimateGoal.HardwareTesting; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import org.cyberarm.engine.V2.CyberarmState; + + +public class WobbleArmTest extends CyberarmState { + + private int currentPos; + private DcMotor motor; + + @Override + public void init() { + motor = engine.hardwareMap.dcMotor.get("wobbleArm"); + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + @Override + public void exec() { + currentPos = motor.getCurrentPosition(); + } + + @Override + public void telemetry() { + engine.telemetry.addData("Position", currentPos); + } +}