diff --git a/TeamCode/src/main/java/org/timecrafters/testing/engine/SpeedrunEngine.java b/TeamCode/src/main/java/org/timecrafters/testing/engine/SpeedrunEngine.java new file mode 100644 index 0000000..7031fc2 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/testing/engine/SpeedrunEngine.java @@ -0,0 +1,53 @@ +package org.timecrafters.testing.engine; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.cyberarm.engine.V2.GamepadChecker; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; +import org.timecrafters.testing.states.PhoenixBot1; +import org.timecrafters.testing.states.TeleOPSpeedrunState; + +@TeleOp (name = "Speedrun Engine") +public class SpeedrunEngine extends CyberarmEngine { + GamepadChecker gamepadChecker; + PhoenixBot1 robot; + + @Override + public void setup() { + TimeCraftersConfiguration configuration = new TimeCraftersConfiguration(); + robot = new PhoenixBot1(this); + + gamepadChecker = new GamepadChecker(this, this.gamepad1); + +// setupFromConfig( +// configuration, +// "org.timecrafters.testing.states", +// robot, +// PhoenixBot1.class, +// "SpeedRun" +// ); + +// addState(new TeleOPSpeedrunState(robot, "SpeedRun", "00-0")); +// addState(new TeleOPSpeedrunState(robot, "SpeedRun", "01-0")); + } + + @Override + public void loop() { + super.loop(); + + gamepadChecker.update(); + } + + @Override + protected void buttonUp(Gamepad gamepad, String button) { + if (gamepad1 == gamepad) { + if (button.equals("a")) { + addState(new TeleOPSpeedrunState(robot, "SpeedRun", "00-0")); + } else if (button.equals("y")) { + addState(new TeleOPSpeedrunState(robot, "SpeedRun", "01-0")); + } + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java index 6c184df..4ad241f 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java @@ -54,13 +54,13 @@ public class PhoenixBot1 { public PhoenixBot1(CyberarmEngine engine) { this.engine = engine; - initVuforia(); - initTfod(); +// initVuforia(); +// initTfod(); setupRobot(); } private void setupRobot () { - collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); +// collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); @@ -75,7 +75,7 @@ public class PhoenixBot1 { imu.startAccelerationIntegration(new Position(), new Velocity(), 10); configuration = new TimeCraftersConfiguration("Phoenix"); - AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit"); +// AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit"); //motors configuration frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left"); @@ -147,7 +147,7 @@ public class PhoenixBot1 { // Instantiate the Vuforia engine - vuforia = ClassFactory.getInstance().createVuforia(parameters); +// vuforia = ClassFactory.getInstance().createVuforia(parameters); } private void initTfod() { diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/TeleOPSpeedrunState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/TeleOPSpeedrunState.java new file mode 100644 index 0000000..de34dd7 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/TeleOPSpeedrunState.java @@ -0,0 +1,38 @@ +package org.timecrafters.testing.states; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +public class TeleOPSpeedrunState extends CyberarmState { + PhoenixBot1 robot; + double power; + int distance; + + public TeleOPSpeedrunState(PhoenixBot1 robot, String groupName, String actionName) { + this.robot = robot; + TimeCraftersConfiguration configuration = new TimeCraftersConfiguration(); + + this.power = configuration.variable(groupName, actionName, "Power").value(); + this.distance = configuration.variable(groupName, actionName, "Distance").value(); + } + + @Override + public void start() { + this.robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + this.robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + @Override + public void exec() { + robot.backLeftDrive.setPower(power); + robot.backRightDrive.setPower(power); + if (Math.abs(robot.backLeftDrive.getCurrentPosition()) > Math.abs(distance)) { + power = 0; + robot.backLeftDrive.setPower(power); + robot.backRightDrive.setPower(power); + setHasFinished(true); + } + } +}