From 3424549d61ad10db960aa229d38cf0a5b5b7a5ee Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Tue, 13 Feb 2024 20:50:45 -0600 Subject: [PATCH 1/2] Back to robot-centric :( --- .../TeleOp/States/SodiPizzaTeleOPState.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java index ba37ac0..d14ff1a 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java @@ -85,7 +85,7 @@ public class SodiPizzaTeleOPState extends CyberarmState { // lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); gp1checker = new GamepadChecker(engine, engine.gamepad1); - gp2checker = new GamepadChecker(engine, engine.gamepad1); + gp2checker = new GamepadChecker(engine, engine.gamepad2); lastMoveTime = System.currentTimeMillis(); lastDistRead = System.currentTimeMillis(); @@ -138,6 +138,12 @@ public class SodiPizzaTeleOPState extends CyberarmState { robot.imu.resetYaw(); } + if (Math.abs(robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES)) > 0.000001 && + Math.abs(robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES)) > 0.000001 && + Math.abs(robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES)) > 0.000001) { + robot.imu.resetYaw(); + } + if (engine.gamepad1.left_stick_button) { if (System.currentTimeMillis() - lastMoveTime >= 200) { robot.launcher.setPosition(0.98); @@ -163,12 +169,12 @@ public class SodiPizzaTeleOPState extends CyberarmState { } if (engine.gamepad1.dpad_up) { - if (System.currentTimeMillis() - lastMoveTime >= 200) { + if (System.currentTimeMillis() - lastMoveTime >= 150) { robot.shoulder.setPosition(robot.shoulder.getPosition() + 0.05); lastMoveTime = System.currentTimeMillis(); } } else if (engine.gamepad1.dpad_down) { - if (System.currentTimeMillis() - lastMoveTime >= 200) { + if (System.currentTimeMillis() - lastMoveTime >= 150) { robot.shoulder.setPosition(robot.shoulder.getPosition() - 0.05); lastMoveTime = System.currentTimeMillis(); } From 704708f907f11f7a2cf597fd4509c76a5b328152 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Tue, 13 Feb 2024 20:59:09 -0600 Subject: [PATCH 2/2] RedCrab: Make LED rail show launch confirmation progress in teleop, add null check for localizer loadConstants function --- .../minibots/red_crab/RedCrabMinibot.java | 24 ++++++++++++++++++- .../minibots/red_crab/states/Pilot.java | 23 ++++++++---------- 2 files changed, 33 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index fb01e87..edc2a83 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -105,6 +105,9 @@ public class RedCrabMinibot { public TimeCraftersConfiguration config; private final PIDFController clawArmPIDFController; public final String webcamName = "Webcam 1"; + public boolean droneLaunchRequested = false; + public double droneLastLaunchRequestStartMS = 0; + public boolean droneLaunchAuthorized = false; private long lastClawArmOverCurrentAnnounced = 0; private boolean clawArmOverCurrent = false; @@ -387,7 +390,8 @@ public class RedCrabMinibot { RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION = config.variable("Robot", "DroneLauncher_Tuning", "launch_position").value(); RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value(); - localizer.loadConfigConstants(); + if (RedCrabMinibot.localizer != null) + RedCrabMinibot.localizer.loadConfigConstants(); } public void resetDeadWheels() { @@ -653,6 +657,24 @@ public class RedCrabMinibot { ledAnimateProgress(LED_ON, LED_OFF, (engine.runTime() - 23_000.0) / 5_000.0); } } else { + // Show progress of drone launch authorization + if (droneLaunchRequested) { + if (droneLaunchAuthorized) { + ledTopRed.setState(LED_OFF); + ledTopGreen.setState(LED_ON); + + ledSetRailRedLEDs(LED_ON); + ledSetRailRedLEDs(LED_ON); + } else { + ledTopRed.setState(LED_ON); + ledTopGreen.setState(LED_ON); + + ledAnimateProgress(LED_ON, LED_ON, (engine.runTime() - droneLastLaunchRequestStartMS) / RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS); + } + + return; + } + if (engine.runTime() >= 90_000.0) { // LAUNCH DRONE and DO CHIN UP ledTopRed.setState(LED_OFF); ledTopGreen.setState(LED_ON); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java index 265f4e7..4061871 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java @@ -17,9 +17,6 @@ public class Pilot extends CyberarmState { private boolean rightClawOpen = false; private int clawArmPosition = RedCrabMinibot.ClawArm_INITIAL; private boolean hookArmUp = false; - private boolean droneLaunchAuthorized = false; - private boolean droneLaunchRequested = false; - private double droneLastLaunchRequestStartMS = 0; private double odometryResetRequestStartMS = 0; private boolean odometryResetRequested = false; private boolean odometryResetRequestLeftStick = false; @@ -42,7 +39,7 @@ public class Pilot extends CyberarmState { clawArmAndWristController(); clawController(); droneLatchController(); - hookArmController(); // disabled for swrist debug + hookArmController(); winchController(); odometryDebugController(); @@ -54,8 +51,8 @@ public class Pilot extends CyberarmState { if (gamepad == engine.gamepad1) { switch (button) { case "y": - droneLaunchRequested = true; - droneLastLaunchRequestStartMS = runTime(); + robot.droneLaunchRequested = true; + robot.droneLastLaunchRequestStartMS = runTime(); break; case "x": hookArmUp = true; @@ -117,8 +114,8 @@ public class Pilot extends CyberarmState { if (gamepad == engine.gamepad1) { switch (button) { case "y": - droneLaunchRequested = false; - droneLastLaunchRequestStartMS = runTime(); + robot.droneLaunchRequested = false; + robot.droneLastLaunchRequestStartMS = runTime(); break; case "left_stick_button": odometryResetRequestLeftStick = false; @@ -234,16 +231,16 @@ public class Pilot extends CyberarmState { } private void droneLatchController() { - if (droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS) - droneLaunchAuthorized = true; + if (robot.droneLaunchRequested && runTime() - robot.droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS) + robot.droneLaunchAuthorized = true; - if (droneLaunchAuthorized) { + if (robot.droneLaunchAuthorized) { robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_LAUNCH_POSITION); } // Auto reset drone latch after DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1 second - if (!droneLaunchRequested && runTime() - droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) { - droneLaunchAuthorized = false; + if (!robot.droneLaunchRequested && runTime() - robot.droneLastLaunchRequestStartMS >= RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS + 1_000) { + robot.droneLaunchAuthorized = false; robot.droneLatch.setPosition(RedCrabMinibot.DRONE_LATCH_INITIAL_POSITION); }