From cc1b40b833a72e6915551abb87da78ed271d2756 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Sun, 3 Nov 2024 10:52:31 -0500 Subject: [PATCH] Demo mode (#197) * Add skeleton of AutomatedTeleop command * Make AutomatedTeleop command drive automatically * Update secretary and add diagram for AutomatedTeleop command * add rotation override when align state is set to aligning * forgot format * remove align state manual in driveToPose / driveToPath * format * add demo mode to drivetrain * remove automated teleop drive to path with demo * add demo shooting * adjust demo constants and logic for shooter ready --------- Co-authored-by: aidnem <99768676+aidnem@users.noreply.github.com> Co-authored-by: minhnguyenbhs --- doc/AutomatedTeleop.csv | 6 + doc/AutomatedTeleop.md | 43 ++++++ src/main/java/frc/robot/Constants.java | 25 ++++ src/main/java/frc/robot/RobotContainer.java | 10 ++ .../frc/robot/commands/AutomatedTeleop.java | 133 ++++++++++++++++++ .../drive/CommandSwerveDrivetrain.java | 64 +++++---- .../subsystems/scoring/ScoringSubsystem.java | 89 ++++++++---- .../java/frc/robot/utils/AllianceUtil.java | 12 ++ util/secretary/driver.py | 3 + util/secretary/secretary.py | 60 +++++++- 10 files changed, 392 insertions(+), 53 deletions(-) create mode 100644 doc/AutomatedTeleop.csv create mode 100644 doc/AutomatedTeleop.md create mode 100644 src/main/java/frc/robot/commands/AutomatedTeleop.java diff --git a/doc/AutomatedTeleop.csv b/doc/AutomatedTeleop.csv new file mode 100644 index 00000000..eec270ad --- /dev/null +++ b/doc/AutomatedTeleop.csv @@ -0,0 +1,6 @@ +↓ Prev | Next → ,START,DRIVE_TO_SOURCE,ACQUIRE_NOTE,DRIVE_TO_SPEAKER,SHOOT_NOTE +START,default,!hasNote(),Not allowed,hasNote(),Not allowed +DRIVE_TO_SOURCE,Not allowed,default,Near Source? || Note Detected?,Not allowed,Not allowed +ACQUIRE_NOTE,Not allowed,Not allowed,default,hasNote(),Not allowed +DRIVE_TO_SPEAKER,Not allowed,!hasNote(),Not allowed,default,In range? +SHOOT_NOTE,Not allowed,!hasNote(),Not allowed,Not allowed,default \ No newline at end of file diff --git a/doc/AutomatedTeleop.md b/doc/AutomatedTeleop.md new file mode 100644 index 00000000..790eb9c4 --- /dev/null +++ b/doc/AutomatedTeleop.md @@ -0,0 +1,43 @@ +# AutomatedTeleop Subsystem +```mermaid +flowchart TD + +classDef state font-size:40px,padding:10px + +node0:::state +node0([START]) +node1:::state +node1([DRIVE_TO_SOURCE]) +node2:::state +node2([ACQUIRE_NOTE]) +node3:::state +node3([DRIVE_TO_SPEAKER]) +node4:::state +node4([SHOOT_NOTE]) +node0 --> node5 +node5 -.->|false| node1 +node5{"hasNote()"} +node5 -.->|true| node3 +node1 --> node6 +node6{"Near Source?"} +node6 -.->|true| node2 +node6 -.->|false| node7 +node7{"Note Detected?"} +node7 -.->|true| node2 +node7 -.->|false| node1 +node2 --> node8 +node8{"hasNote()"} +node8 -.->|true| node3 +node8 -.->|false| node2 +node3 --> node9 +node9{"!hasNote()"} +node9 -.->|true| node1 +node9 -.->|false| node10 +node10{"In range?"} +node10 -.->|true| node4 +node10 -.->|false| node3 +node4 --> node11 +node11{"!hasNote()"} +node11 -.->|true| node1 +node11 -.->|false| node4 +``` diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 81f37a60..35ef86a1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,6 +56,7 @@ public static final class FeatureFlags { public static final boolean runScoring = true; public static final boolean runEndgame = true; public static final boolean runDrive = true; + public static final boolean demoMode = true; public static final boolean enableLEDS = true; } @@ -114,6 +115,17 @@ public static final class DriveConstants { public static final double vYkD = 0.0; } + public static final class AutomatedTeleopConstants { + // TODO: Find constants for automated teleop command + public static final double intakeTimeoutSeconds = 0.5; + public static final double retryIntakeWaitSeconds = 0.5; + + public static final double shootRangeMeters = 3.0; + + // The distance from the source at which to stop driving the robot + public static final double sourceRangeMeters = 1.5; + } + public static final class FieldConstants { public static final double lengthM = 16.451; public static final double widthM = 8.211; @@ -173,6 +185,16 @@ public static final class FieldConstants { public static final Pose2d robotAgainstRedAmpZone = new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90)); + + public static final Pose2d robotAgainstBlueSource = + new Pose2d(14.82, 0.69, Rotation2d.fromDegrees(-60)); + + public static final Pose2d robotAgainstRedSource = + new Pose2d(1.63, 0.69, Rotation2d.fromDegrees(60)); + + // TODO: Find actual coordinates of shop source + public static final Pose2d robotAgainstShopSource = + new Pose2d(8.30, 6.80, Rotation2d.fromDegrees(60)); } public static final class VisionConstants { @@ -553,6 +575,9 @@ public static final class ScoringConstants { public static final double hoodMaxVelocity = 0.5; public static final double hoodMaxAcceleration = 0.5; + public static final double demoShooterRPM = 1000; + public static final double demoAimAngle = 0.4; + // NOTE - This should be monotonically increasing // Key - Distance in meters // Value - Aimer angle in radians diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1650dc72..3c40f975 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -135,6 +135,9 @@ public void configureSubsystems() { if (FeatureFlags.runDrive) { driveTelemetry = new Telemetry(DriveConstants.MaxSpeedMetPerSec, new TelemetryIOLive()); + if (FeatureFlags.demoMode) { + drivetrain.setDemo(true); + } } if (FeatureFlags.runScoring) { @@ -143,6 +146,9 @@ public void configureSubsystems() { new ShooterIOTalon(), new AimerIORoboRio(), new HoodIOSparkFlex()); + if (FeatureFlags.demoMode) { + scoringSubsystem.setDemo(true); + } } if (FeatureFlags.runEndgame) { @@ -393,6 +399,7 @@ private void configureBindings() { } if (FeatureFlags.runScoring) { + if(!FeatureFlags.demoMode) { scoringSubsystem.setDefaultCommand(new ShootWithGamepad( () -> rightJoystick.getHID().getRawButton(4), controller.getHID()::getRightBumper, @@ -401,6 +408,9 @@ private void configureBindings() { controller.getHID()::getAButton, controller.getHID()::getBButton, scoringSubsystem, FeatureFlags.runDrive ? drivetrain::getAlignTarget : () -> AlignTarget.NONE)); + } else { + controller.y().onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.SHOOT))).onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.WAIT))); + } rightJoystick.button(11).onTrue(new InstantCommand(() -> scoringSubsystem.setArmDisabled(true))); rightJoystick.button(16).onTrue(new InstantCommand(() -> scoringSubsystem.setArmDisabled(false))); diff --git a/src/main/java/frc/robot/commands/AutomatedTeleop.java b/src/main/java/frc/robot/commands/AutomatedTeleop.java new file mode 100644 index 00000000..aa2695e8 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutomatedTeleop.java @@ -0,0 +1,133 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.AutomatedTeleopConstants; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.drive.CommandSwerveDrivetrain; +import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.intake.IntakeSubsystem.IntakeAction; +import frc.robot.subsystems.scoring.ScoringSubsystem; +import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringAction; +import frc.robot.telemetry.Telemetry; +import java.util.function.Supplier; + +public class AutomatedTeleop extends Command { + ScoringSubsystem scoringSubsystem; + IntakeSubsystem intakeSubsystem; + CommandSwerveDrivetrain drivetrain; + + private Supplier poseSupplier = () -> new Pose2d(); + + Telemetry telemetry; + + private enum State { + START, + DRIVE_TO_SOURCE, + ACQUIRE_NOTE, + DRIVE_TO_SPEAKER, + SHOOT_NOTE, + } + + private State state; + + public AutomatedTeleop( + ScoringSubsystem scoringSubsystem, + IntakeSubsystem intakeSubsystem, + CommandSwerveDrivetrain drivetrain, + Telemetry telemetry, + Supplier poseSupplier) { + this.scoringSubsystem = scoringSubsystem; + this.intakeSubsystem = intakeSubsystem; + this.drivetrain = drivetrain; + this.telemetry = telemetry; + + this.poseSupplier = poseSupplier; + + addRequirements(scoringSubsystem, intakeSubsystem, drivetrain); + } + + @Override + public void initialize() { + this.state = State.START; + + scoringSubsystem.setAction(ScoringAction.WAIT); + scoringSubsystem.forceHood(false); + } + + private double findDistanceToSource() { + // NOTE: Leave one of the following lines commented out depending on the scenario: + + // Uncomment this line for actual production code: + // Pose2d sourcePose = AllianceUtil.getPoseAgainstSource(); + + // Uncomment this line for testing with shop source + Pose2d sourcePose = FieldConstants.robotAgainstShopSource; + + Pose2d robotPose = poseSupplier.get(); + double distancetoGoal = + Math.sqrt( + Math.pow(Math.abs(robotPose.getX() - sourcePose.getX()), 2) + + Math.pow(Math.abs(robotPose.getY() - sourcePose.getY()), 2)); + return distancetoGoal; + } + + @Override + public void execute() { + switch (state) { + case START: + if (intakeSubsystem.hasNote()) { + state = State.DRIVE_TO_SPEAKER; + } else { + state = State.DRIVE_TO_SOURCE; + } + break; + case DRIVE_TO_SOURCE: + drivetrain.driveToSource(); + + scoringSubsystem.setAction(ScoringAction.WAIT); + // Once robot reaches the source, stop driving and try to acquire a note + if (findDistanceToSource() + < AutomatedTeleopConstants.sourceRangeMeters /* || note detected */) { + state = State.ACQUIRE_NOTE; + } + break; + case ACQUIRE_NOTE: + // TODO: Fully implement automation of intake + + drivetrain.stopDriveToPose(); + intakeSubsystem.run(IntakeAction.INTAKE); + + if (intakeSubsystem.hasNote()) { + state = State.DRIVE_TO_SPEAKER; + } + break; + case DRIVE_TO_SPEAKER: + // If we don't have a note, go get one + // This will work for if we drop our note or for after we've shot + if (!intakeSubsystem.hasNote()) { + // TODO: Use note vision to pick up a nearby note rather than going to source? + state = State.DRIVE_TO_SOURCE; + } + + drivetrain.driveToSpeaker(); + scoringSubsystem.setAction(ScoringAction.AIM); + + // Once we are in range, shoot + if (scoringSubsystem.findDistanceToGoal() + < AutomatedTeleopConstants.shootRangeMeters) { + state = State.SHOOT_NOTE; + } + break; + case SHOOT_NOTE: + if (!intakeSubsystem.hasNote()) { + state = State.DRIVE_TO_SOURCE; + } + + drivetrain.driveToSpeaker(); + scoringSubsystem.setAction(ScoringAction.SHOOT); + + break; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index 6423aafc..99a77efc 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -10,14 +10,15 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -42,6 +43,7 @@ import frc.robot.utils.AllianceUtil; import frc.robot.utils.GeomUtil; import frc.robot.utils.InterpolateDouble; +import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -120,12 +122,17 @@ public enum AlignState { private Notifier simNotifier = null; private double lastSimTime; + private String lastCommandedPath = ""; private Command pathfindCommand = null; private Pose2d pathfindPose = new Pose2d(); private ChassisSpeeds stopSpeeds = new ChassisSpeeds(0, 0, 0); + private Rotation2d desiredHeading = new Rotation2d(); + + private boolean demo = false; + public CommandSwerveDrivetrain( SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, @@ -195,6 +202,10 @@ public void setVelocitySupplier(Supplier getRobotVelocity) { this.getRobotVelocity = getRobotVelocity; } + public void setDemo(boolean demo) { + this.demo = demo; + } + public void configurePathPlanner() { double driveBaseRadius = 0; for (var moduleLocation : m_moduleLocations) { @@ -235,7 +246,7 @@ public void configurePathPlanner() { }, // Change this if the path needs to be flipped on red vs blue this); // Subsystem for requirements - // PPHolonomicDriveController.setRotationTargetOverride(this::getRotationTargetOverride); + PPHolonomicDriveController.setRotationTargetOverride(this::getOverrideRotation); // autoChooser = AutoBuilder.buildAutoChooser(); autoChooser.setDefaultOption("Default (nothing)", Commands.none()); // S1-W1-W2-W3 @@ -330,8 +341,8 @@ public void setTuningVolts(double tuningVolts) { private void controlDrivetrain() { Pose2d pose = getFieldToRobot.get(); - Rotation2d desiredHeading = pose.getRotation(); - if (alignState == AlignState.ALIGNING) { + desiredHeading = pose.getRotation(); + if (alignState == AlignState.ALIGNING && !demo) { switch (alignTarget) { case AMP: desiredHeading = AllianceUtil.getAmpHeading(); @@ -458,18 +469,18 @@ private void controlDrivetrain() { } else if (!fieldCentric) { setControl( driveRobotCentric - .withVelocityX(vx) - .withVelocityY(vy) - .withRotationalRate(omega) + .withVelocityX(demo ? MathUtil.clamp(vx, 0, 1) : vx) + .withVelocityY(demo ? MathUtil.clamp(vy, 0, 1) : vy) + .withRotationalRate(demo ? omega * 0.50 : omega) .withDeadband(0.0) .withRotationalDeadband(0.0) .withDriveRequestType(DriveRequestType.Velocity)); } else { setControl( driveFieldCentric - .withVelocityX(vx) - .withVelocityY(vy) - .withRotationalRate(omega) + .withVelocityX(demo ? MathUtil.clamp(vx, 0, 1) : vx) + .withVelocityY(demo ? MathUtil.clamp(vy, 0, 1) : vy) + .withRotationalRate(demo ? omega * 0.5 : omega) .withDeadband(0.0) .withRotationalDeadband(0.0) .withDriveRequestType(DriveRequestType.Velocity)); @@ -545,21 +556,21 @@ public boolean atPathfindPose() { } public void driveToPose(Pose2d targetPose) { - this.setAlignState(AlignState.MANUAL); - - pathfindCommand = getPathfindCommand(targetPose); - pathfindCommand.schedule(); + if (!demo) { + pathfindCommand = getPathfindCommand(targetPose); + pathfindCommand.schedule(); + } } public void driveToPath(String pathName) { - this.setAlignState(AlignState.MANUAL); + if (pathName == lastCommandedPath || demo) { + return; + } else { + lastCommandedPath = pathName; + } PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - // if(DriverStation.getAlliance() === Alliance.Blue) { - // path.flipPath(); - // } - PathConstraints constraints = new PathConstraints( 3.0, @@ -567,20 +578,25 @@ public void driveToPath(String pathName) { Constants.ConversionConstants.kDegreesToRadians * 540, Constants.ConversionConstants.kDegreesToRadians * 720); - PathPlannerLogging.setLogTargetPoseCallback( - (target) -> { - Logger.recordOutput("targetPose", target); - }); - pathfindCommand = AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0); pathfindCommand.schedule(); } + public Optional getOverrideRotation() { + if (alignState == AlignState.ALIGNING) { + return Optional.of(desiredHeading); + } else { + return Optional.empty(); + } + } + public void stopDriveToPose() { if (pathfindCommand != null) { pathfindCommand.cancel(); } + lastCommandedPath = ""; + setGoalChassisSpeeds(stopSpeeds, true); } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 60399735..2d9c7bf5 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -64,6 +64,7 @@ public class ScoringSubsystem extends SubsystemBase implements Tunable { private boolean overrideShoot = false; private boolean overrideStageAvoidance = false; private boolean overrideBeamBreak = false; + private boolean demo = false; private boolean hoodForced = false; @@ -240,31 +241,55 @@ private void spit() { } private void prime() { + double shooterRPM; + double aimAngle; double distanceToGoal = findDistanceToGoal(); - Logger.recordOutput("scoring/aimGoal", getAimerAngle(distanceToGoal)); - shooterIo.setShooterVelocityRPM(shooterInterpolated.getValue(distanceToGoal)); - aimerIo.setAimAngleRad(getAimerAngle(distanceToGoal), false); + if (demo) { + shooterRPM = ScoringConstants.demoShooterRPM; + aimAngle = ScoringConstants.demoAimAngle; + } else { + shooterRPM = shooterInterpolated.getValue(distanceToGoal); + aimAngle = getAimerAngle(distanceToGoal); + } + Logger.recordOutput("scoring/aimGoal", aimAngle); + shooterIo.setShooterVelocityRPM(shooterRPM); + aimerIo.setAimAngleRad(aimAngle, false); if (!overrideBeamBreak) { shooterIo.setKickerVolts(hasNote() ? 0.0 : ScoringConstants.kickerIntakeVolts); } - - boolean shooterReady = - shooterInputs.shooterLeftVelocityRPM - < (shooterInputs.shooterLeftGoalVelocityRPM - + ScoringConstants.shooterUpperVelocityMarginRPM) - && shooterInputs.shooterLeftVelocityRPM - > (shooterInputs.shooterLeftGoalVelocityRPM - - ScoringConstants.shooterLowerVelocityMarginRPM); - boolean aimReady = - Math.abs(aimerInputs.aimAngleRad - aimerInputs.aimGoalAngleRad) - < aimerAngleTolerance.getValue(distanceToGoal) - && Math.abs(aimerInputs.aimVelocityErrorRadPerSec) - < ScoringConstants.aimAngleVelocityMargin; - boolean driveReady = driveAllignedSupplier.get(); + boolean shooterReady; + boolean aimReady; + if (demo) { + shooterReady = + shooterInputs.shooterLeftVelocityRPM + < (shooterInputs.shooterLeftGoalVelocityRPM + + ScoringConstants.shooterUpperVelocityMarginRPM) + && shooterInputs.shooterLeftVelocityRPM + > (shooterInputs.shooterLeftGoalVelocityRPM + - ScoringConstants.shooterLowerVelocityMarginRPM); + aimReady = + Math.abs(aimerInputs.aimAngleRad - aimerInputs.aimGoalAngleRad) < 0.1 + && Math.abs(aimerInputs.aimVelocityErrorRadPerSec) + < ScoringConstants.aimAngleVelocityMargin; + } else { + shooterReady = + shooterInputs.shooterLeftVelocityRPM + < (shooterInputs.shooterLeftGoalVelocityRPM + + ScoringConstants.shooterUpperVelocityMarginRPM) + && shooterInputs.shooterLeftVelocityRPM + > (shooterInputs.shooterLeftGoalVelocityRPM + - ScoringConstants.shooterLowerVelocityMarginRPM); + aimReady = + Math.abs(aimerInputs.aimAngleRad - aimerInputs.aimGoalAngleRad) + < aimerAngleTolerance.getValue(distanceToGoal) + && Math.abs(aimerInputs.aimVelocityErrorRadPerSec) + < ScoringConstants.aimAngleVelocityMargin; + } + boolean driveReady = demo || driveAllignedSupplier.get(); boolean fieldLocationReady = true; if (!DriverStation.getAlliance().isPresent()) { fieldLocationReady = true; - } else { + } else if (!demo) { switch (DriverStation.getAlliance().get()) { case Blue: fieldLocationReady = @@ -322,11 +347,16 @@ private void ampPrime() { } private void shoot() { - double distancetoGoal = findDistanceToGoal(); - - double shootRPM = shooterInterpolated.getValue(distancetoGoal); - shooterIo.setShooterVelocityRPM(shootRPM); - double aimAngleRad = aimerInterpolated.getValue(distancetoGoal) - 0.015; + double shootRPM; + double aimAngleRad; + if (!demo) { + double distancetoGoal = findDistanceToGoal(); + shootRPM = shooterInterpolated.getValue(distancetoGoal); + aimAngleRad = aimerInterpolated.getValue(distancetoGoal); + } else { + shootRPM = ScoringConstants.demoShooterRPM; + aimAngleRad = ScoringConstants.demoAimAngle; + } aimerIo.setAimAngleRad(aimAngleRad, false); shooterIo.setKickerVolts(10); @@ -407,7 +437,7 @@ private void temporarySetpoint() { } } - private double findDistanceToGoal() { + public double findDistanceToGoal() { Translation2d speakerPose = AllianceUtil.getFieldToSpeaker(); Pose2d robotPose = poseSupplier.get(); double distancetoGoal = @@ -423,7 +453,6 @@ public boolean hasNote() { public boolean aimerAtIntakePosition() { return aimerInputs.aimAngleRad > ScoringConstants.intakeAngleToleranceRadians; - // return true;\][] } public boolean canIntake() { @@ -619,6 +648,10 @@ public void setOverrideBeamBrake(boolean overrideBeamBrake) { this.overrideBeamBreak = overrideBeamBrake; } + public void setDemo(boolean demo) { + this.demo = demo; + } + public void setArmDisabled(boolean disabled) { aimerIo.setMotorDisabled(disabled); } @@ -688,15 +721,15 @@ public void setVolts(double volts, int slot) { switch (slot) { // Aimer case 0: - aimerIo.setOverrideVolts(volts); + aimerIo.setOverrideVolts(demo ? volts * 0.5 : volts); break; // Hood case 1: - hoodIo.setOverrideVolts(volts); + hoodIo.setOverrideVolts(demo ? volts * 0.5 : volts); break; // Shooter case 2: - shooterIo.setOverrideVolts(volts); + shooterIo.setOverrideVolts(demo ? volts * 0.5 : volts); break; default: throw new IllegalArgumentException("Invalid slot"); diff --git a/src/main/java/frc/robot/utils/AllianceUtil.java b/src/main/java/frc/robot/utils/AllianceUtil.java index 278a1ec3..17998b28 100644 --- a/src/main/java/frc/robot/utils/AllianceUtil.java +++ b/src/main/java/frc/robot/utils/AllianceUtil.java @@ -88,6 +88,18 @@ public static Pose2d getPoseAgainstAmpZone() { return FieldConstants.robotAgainstRedAmpZone; } + public static Pose2d getPoseAgainstSource() { + if (!DriverStation.getAlliance().isEmpty()) { + switch (DriverStation.getAlliance().get()) { + case Blue: + return FieldConstants.robotAgainstRedSource; + case Red: + return FieldConstants.robotAgainstBlueSource; + } + } + return FieldConstants.robotAgainstRedSource; + } + public static Rotation2d getSourceHeading() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { diff --git a/util/secretary/driver.py b/util/secretary/driver.py index f89f15cd..5f12f984 100644 --- a/util/secretary/driver.py +++ b/util/secretary/driver.py @@ -4,6 +4,9 @@ import subprocess def main(): + print("WARNING: Secretary does not work consistently at the moment.") + print("\tBe sure to verify that generated diagrams look correct before committing.") + secretary = os.path.join(os.path.dirname(__file__), "secretary.py"); if not os.path.isfile(secretary): print(f"[ERROR] secretary not found at {secretary}") diff --git a/util/secretary/secretary.py b/util/secretary/secretary.py index b94987b9..93c1eb35 100644 --- a/util/secretary/secretary.py +++ b/util/secretary/secretary.py @@ -73,7 +73,62 @@ def render_single(condition: str, in_node: str, out_true: str, out_false: str): print(f"{in_node} -.->|true| {out_true}") print(f"{in_node} -.->|false| {out_false}") -NON_TRANSITIONS = ['default', 'Not allowed', 'TBD', 'N/A', 'not possible'] +def render_one_check(row: list[str], states_row: list[str], in_node: str): + for i, condition in enumerate(row[1:]): + if condition in NON_TRANSITIONS: + continue + next_state = states_row[i] + next_state_node = get_node_id(next_state) + + if condition.startswith('!'): + print(f"{in_node} -.->|false| {next_state_node}") + else: + print(in_node + '{"' + condition + '"}') + print(f"{in_node} -.->|true| {next_state_node}") + +NON_TRANSITIONS = ['default', 'Not allowed', 'TBD'] + +def is_one_check(row: list[str]) -> bool: + condition = "" + found_true = False + found_false = False + + for transition in row[1:]: + if transition in NON_TRANSITIONS: + continue + if transition.startswith("!"): + if found_false: + # We've already found one condition starting with `!` + # This means this row can't be one check + return False + elif found_true: + if transition[1:] == condition: + found_false = True + else: + # The `!...` condition doesn't match the `...` condition + # Can't be one check + return False + else: + found_false = True + condition = transition[1:] + else: + if found_true: + # We've already found one condition not starting with `!` + # This means this row can't be one check + return False + elif found_false: + if transition == condition: + found_true = True + else: + # The `!...` condition doesn't match the `...` condition + # Can't be one check + return False + else: + found_true = True + condition = transition + + # If we found both a true and false without duplicates, it must be a one check + return found_true and found_false def render_table(rows: list[list[str]], combine: bool=False): states_row = rows[0][1:] # Chop off first, empty square @@ -102,6 +157,9 @@ def render_table(rows: list[list[str]], combine: bool=False): if num_transitions > 0: in_node = get_node_id() print(f"{current_state_node} --> {in_node}") + if is_one_check(row): + render_one_check(row, states_row, in_node) + continue for i, condition in enumerate(row[1:]): if condition in NON_TRANSITIONS: continue