TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-12 23:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2024-02-16 15:16:23 -06:00
parent 3b868f10a0 704708f907
commit ed993eff76
3 changed files with 42 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java
View File

@@ -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);

23
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java
View File

@@ -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);
}

12
TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java
View File

@@ -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();
}