From: moo Date: Sun, 19 Apr 2026 23:52:00 +0000 (-0700) Subject: april 19th at 971 X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=2737f0c5241c70af8c8b81772efa1f15029f6fa4;p=FRC2026.git april 19th at 971 --- diff --git a/src/main/deploy/pathplanner/autos/LeftShallowDoubleSwipe.auto b/src/main/deploy/pathplanner/autos/LeftShallowDoubleSwipe.auto new file mode 100644 index 0000000..e4002fa --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftShallowDoubleSwipe.auto @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "W5 Left Trench Start" + } + }, + { + "type": "named", + "data": { + "name": "Spin Intake Rollers" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Swipe3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + }, + { + "type": "named", + "data": { + "name": "Stop Intake Seizure" + } + }, + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "named", + "data": { + "name": "Stop Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Reset Spindexer" + } + }, + { + "type": "named", + "data": { + "name": "Hood Down" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Swipe2" + } + }, + { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + }, + { + "type": "named", + "data": { + "name": "Start Spindexer" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Start Intake Seizure" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto deleted file mode 100644 index d4dfd7b..0000000 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Stop Hood Down" - } - }, - { - "type": "named", - "data": { - "name": null - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Swipe1.path b/src/main/deploy/pathplanner/paths/Left Swipe1.path index 33bf814..ca33895 100644 --- a/src/main/deploy/pathplanner/paths/Left Swipe1.path +++ b/src/main/deploy/pathplanner/paths/Left Swipe1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.341153730575683, - "y": 7.339067227185437 + "x": 6.651298524925124, + "y": 7.607193541852019 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 6.924453642384106 }, "prevControl": { - "x": 8.031951473995347, - "y": 7.123753266445548 + "x": 8.025456588617843, + "y": 7.118663887579298 }, "nextControl": { - "x": 8.333810115408626, - "y": 6.725154018322663 + "x": 8.456464624342907, + "y": 6.586940219527121 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 5.668485099337747 }, "prevControl": { - "x": 8.618618682530846, - "y": 5.7310165129163355 + "x": 8.453398729051012, + "y": 5.774889075676877 }, "nextControl": { - "x": 7.627292843251787, - "y": 5.626913168266943 + "x": 7.668717493102982, + "y": 5.580825559331089 }, "isLocked": false, "linkedName": null @@ -52,12 +52,12 @@ "y": 6.27831953642384 }, "prevControl": { - "x": 7.433678730378213, - "y": 5.997765220428201 + "x": 7.455955837425071, + "y": 6.060525942967513 }, "nextControl": { - "x": 7.0916784354147495, - "y": 6.784302377129385 + "x": 7.167700799412434, + "y": 6.495691704464595 }, "isLocked": false, "linkedName": null @@ -68,12 +68,12 @@ "y": 7.408007117437722 }, "prevControl": { - "x": 5.877242530931041, - "y": 7.381065257813595 + "x": 5.861909734801985, + "y": 7.3911421043858505 }, "nextControl": { - "x": 4.284691670946883, - "y": 7.543120509860877 + "x": 3.5628490080812623, + "y": 7.546590977304791 }, "isLocked": false, "linkedName": null @@ -84,8 +84,8 @@ "y": 7.5 }, "prevControl": { - "x": 4.397534345753931, - "y": 7.5120104256577624 + "x": 4.366224335486623, + "y": 7.4554835538530675 }, "nextControl": null, "isLocked": false, @@ -110,10 +110,10 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0.7761529808773961, - "maxWaypointRelativePos": 1.642294713160863, + "maxWaypointRelativePos": 2.8908886389201145, "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 2.0, + "maxVelocity": 1.67, + "maxAcceleration": 1.5, "maxAngularVelocity": 200.0, "maxAngularAcceleration": 150.0, "nominalVoltage": 12.0, @@ -122,30 +122,7 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Stop Hood Down", - "waypointRelativePos": 4.968503937007888, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Stop Hood Down" - } - } - }, - { - "name": "Start Spindexer", - "waypointRelativePos": 5.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Start Spindexer" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 2.5, diff --git a/src/main/deploy/pathplanner/paths/Left Swipe3.path b/src/main/deploy/pathplanner/paths/Left Swipe3.path new file mode 100644 index 0000000..daad02e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Swipe3.path @@ -0,0 +1,168 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.5, + "y": 7.623 + }, + "prevControl": null, + "nextControl": { + "x": 6.25, + "y": 7.623 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.999810201660737, + "y": 5.890960854092527 + }, + "prevControl": { + "x": 6.114457884162435, + "y": 6.884367069543684 + }, + "nextControl": { + "x": 5.907758101386918, + "y": 5.093342214431054 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.258030842230131, + "y": 4.449228944246738 + }, + "prevControl": { + "x": 5.653713425517404, + "y": 4.619355154982582 + }, + "nextControl": { + "x": 6.687014708632405, + "y": 4.3284622781876605 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.892823250296561, + "y": 5.546666666666668 + }, + "prevControl": { + "x": 6.892823250296561, + "y": 4.907866997335997 + }, + "nextControl": { + "x": 6.892823250296561, + "y": 6.2753696930420535 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.258030842230131, + "y": 6.977639383155397 + }, + "prevControl": { + "x": 6.423269381273585, + "y": 6.7900328128685725 + }, + "nextControl": { + "x": 6.092792303186677, + "y": 7.165245953442221 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.5 + }, + "prevControl": { + "x": 5.5, + "y": 7.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.2433392539964518, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0373001776198882, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 4.129662522202486, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9111361079864955, + "maxWaypointRelativePos": 2.125984251968506, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Stop Hood Down", + "waypointRelativePos": 5.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + } + }, + { + "name": "Start Spindexer", + "waypointRelativePos": 5.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Start Spindexer" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 200.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "week 5 (new stuff)", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Swipe1.path b/src/main/deploy/pathplanner/paths/Right Swipe1.path index 3bd63f6..c30a7e0 100644 --- a/src/main/deploy/pathplanner/paths/Right Swipe1.path +++ b/src/main/deploy/pathplanner/paths/Right Swipe1.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 8.155370348167532, - "y": 0.8386737269319345 + "y": 0.8386737269319344 }, "nextControl": { "x": 8.45722898958081, @@ -122,30 +122,7 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Stop Hood Down", - "waypointRelativePos": 4.968503937007888, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Stop Hood Down" - } - } - }, - { - "name": "Start Spindexer", - "waypointRelativePos": 5, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Start Spindexer" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3, "maxAcceleration": 2.5, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 71933a1..ca5ec03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.commands.LogCommand; +import frc.robot.commands.auto_comm.DynamicAutoBuilder; import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.gpm.AutoShootCommand; import frc.robot.commands.gpm.ClimbDriveCommand; @@ -280,6 +281,17 @@ public class RobotContainer { } } + public void addAuto(String name, Command auto) { + try { + autoChooser.addOption(name, auto); + } + // is this the right one?? + catch (AutoBuilderException e) { + e.printStackTrace(); + System.out.println("HELLOOOO AUTO \"" + name + "\" NOT FOUND"); + } + } + /** * Initialize the SendableChooser on the SmartDashboard. * Fill the SendableChooser with available Commands. @@ -297,6 +309,9 @@ public class RobotContainer { String rightDoNothing = "Right Do Nothing"; String centerDoNothing = "Center Do Nothing"; + String leftShallowDoubleSwipe = "LeftShallowDoubleSwipe"; + String rightShallowDoubleSwipe = "RightShallowDoubleSwipe"; + autoChooser.setDefaultOption("Default", getDefaultAuto()); addAuto(leftSideAuto); addAuto(rightSideAuto); @@ -308,6 +323,16 @@ public class RobotContainer { addAuto(leftDoNothing); addAuto(rightDoNothing); addAuto(centerDoNothing); + addAuto(leftShallowDoubleSwipe); + addAuto(rightShallowDoubleSwipe); + + + DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake, breaker); + + String leftDynamicDoubleLiberalSwipe = "LeftDynamicDoubleLiberalSwipe"; + String rightDynamicDoubleLiberalSwipe = "RightDynamicDoubleLiberalSwipe"; + addAuto(leftDynamicDoubleLiberalSwipe, dynamicAutoBuilder.getLeftDynamicDoubleLiberalSwipe()); + addAuto(rightDynamicDoubleLiberalSwipe, dynamicAutoBuilder.getRightDynamicDoubleLiberalSwipe()); // put the Chooser on the SmartDashboard SmartDashboard.putData("Auto chooser", autoChooser); diff --git a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java new file mode 100644 index 0000000..5aebc5f --- /dev/null +++ b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java @@ -0,0 +1,97 @@ +package frc.robot.commands.auto_comm; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.*; +import frc.robot.commands.gpm.IntakeCommand; +import frc.robot.commands.gpm.RunSpindexer; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.PowerControl.BreakerConstants; +import frc.robot.subsystems.PowerControl.EMABreaker; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.turret.Turret; + +import com.pathplanner.lib.commands.PathPlannerAuto; + +public class DynamicAutoBuilder { + + private final Spindexer spindexer; + private final Turret turret; + private final Hood hood; + private final Intake intake; + private final EMABreaker breaker; + + public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake, EMABreaker breaker) { + this.spindexer = spindexer; + this.turret = turret; + this.hood = hood; + this.intake = intake; + this.breaker = breaker; + } + + public Command getLeftDynamicDoubleLiberalSwipe() { + return new SequentialCommandGroup( + new InstantCommand(() -> { + hood.forceHoodDown(true); + intake.extend(); + intake.spinStart(); + }), + + new PathPlannerAuto("LeftSwipeOne"), + + new InstantCommand(() -> hood.forceHoodDown(false)), + runSpindexerWithAbort(), + new InstantCommand(() -> hood.forceHoodDown(true)), + + new PathPlannerAuto("LeftSwipeTwo"), + new InstantCommand(() -> hood.forceHoodDown(false)), + runSpindexerWithAbort(), + new InstantCommand(() -> hood.forceHoodDown(true)), + + new PathPlannerAuto("LeftSwipeTwo"), + new InstantCommand(() -> hood.forceHoodDown(false)), + runSpindexerWithAbort(), + new InstantCommand(() -> hood.forceHoodDown(true)), + + new PathPlannerAuto("LeftSwipeTwo"), + new InstantCommand(() -> hood.forceHoodDown(false)), + runSpindexerWithAbort(), + new InstantCommand(() -> hood.forceHoodDown(true)) + ); + } + + public Command getRightDynamicDoubleLiberalSwipe() { + return new SequentialCommandGroup( + new InstantCommand(() -> hood.forceHoodDown(true)), + + new PathPlannerAuto("RightSwipeOne"), + runSpindexerWithAbort(), + + new PathPlannerAuto("RightSwipeTwo"), + runSpindexerWithAbort(), + + new PathPlannerAuto("RightSwipeTwo"), + runSpindexerWithAbort(), + + new PathPlannerAuto("RightSwipeTwo"), + runSpindexerWithAbort()); + } + + private Command runSpindexerWithAbort() { + // should race: when a command finnishes (will always be the wait until command) + // then we will end the command + // return new RunSpindexer(spindexer, turret, hood, intake).raceWith(new + // WaitCommand(5.0)); + + var timer = new Timer(); + timer.start(); + + return new RunSpindexer(spindexer, turret, hood, intake).raceWith( + new WaitUntilCommand(() -> breaker.getAverageCurrentDraw(BreakerConstants.SPINDEXER_PORTS) < 5.0 && timer.hasElapsed(3.0))); + + // return new ParallelDeadlineGroup(new WaitUntilCommand(() -> + // spindexer.spinningAir()), new RunSpindexer(spindexer, turret, hood, intake)); + // return new ParallelDeadlineGroup(new WaitCommand(5.0), new + // RunSpindexer(spindexer, turret, hood, intake)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index b370ae4..12f283d 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -85,6 +85,8 @@ public class Superstructure extends Command { 8.0); // Random initial goalState to prevent it being null addRequirements(turret, shooter); + + // SmartDashboard.putNumber("targetVelocity", 0.0); } public void updateSetpoints(Pose2d drivepose) { @@ -239,6 +241,8 @@ public class Superstructure extends Command { turretOffset -= 2.5; //2.5 deg } + // LoggedNetworkNumber velocity = new LoggedNetworkNumber("targetVelocity", 0.0); + @Override public void execute() { // Phase manager stuff @@ -249,7 +253,7 @@ public class Superstructure extends Command { updateSetpoints(drivepose); turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset); - SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset); + // SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset); if (phaseManager.isIdle()) { underLadder(); @@ -280,6 +284,7 @@ public class Superstructure extends Command { // different maps for shuttling vs shooting. Less powerful when shuttling. if (shuttling) { shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget)); + // shooter.setShooter(-SmartDashboard.getNumber("targetVelocity", 0.0)); } else { shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); } diff --git a/src/main/java/frc/robot/constants/ShuttleInterpolation.java b/src/main/java/frc/robot/constants/ShuttleInterpolation.java index 3d8074e..fe86227 100644 --- a/src/main/java/frc/robot/constants/ShuttleInterpolation.java +++ b/src/main/java/frc/robot/constants/ShuttleInterpolation.java @@ -22,7 +22,7 @@ public class ShuttleInterpolation { shooterVelocityMap.put(4.0, 12.8); shooterVelocityMap.put(7.6, 19.0); shooterVelocityMap.put(11.4, 28.1); // was 25.2 before - shooterVelocityMap.put(16.54, 44.8); // untested + shooterVelocityMap.put(16.54, 70.0); // untested // always shoot at low angle to ground. newHoodMap.put(0.0, 60.0); diff --git a/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java b/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java index ca915a3..979fee0 100644 --- a/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java +++ b/src/main/java/frc/robot/subsystems/PowerControl/EMABreaker.java @@ -86,7 +86,7 @@ public class EMABreaker extends SubsystemBase { public double getAverageCurrentDraw(int[] ports) { double sum = 0; for (int number : ports) { - sum += subsystems.get(number - 1).average; // the list starts at zero, so ports will be shifted + sum += subsystems.get(number).average; // the list starts at zero, so ports will be shifted } return sum; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index bca47d2..fa9620d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -62,7 +62,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { ); CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); - limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT; + limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_STATOR_CURRENT_LIMIT; limitConfig.StatorCurrentLimitEnable = true; shooterMotorLeft.getConfigurator().apply(limitConfig); shooterMotorRight.getConfigurator().apply(limitConfig); @@ -105,10 +105,10 @@ public class Shooter extends SubsystemBase implements ShooterIO { SmartDashboard.putBoolean("Shooter Running", shooterTargetSpeed > 0); } powerModifier = SmartDashboard.getNumber("OPERATOR: Shooter Power Modifier", powerModifier); - SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier); + // SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier); // keep this - SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST"); + Logger.recordOutput("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST"); } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 29191c9..807bcc4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,4 +6,5 @@ public class ShooterConstants { public static final double SHOOTER_VELOCITY = 1.0; public static final double SHOOTER_LAUNCH_DIAMETER = Units.inchesToMeters(4.0); public static final double SHOOTER_CURRENT_LIMIT = 40.0; + public static final double SHOOTER_STATOR_CURRENT_LIMIT = 120.0; }