From f1ac853a4bc00e1beac648f72905966093ead3c6 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sat, 28 Feb 2026 11:41:51 -0800 Subject: [PATCH] a --- .../paths/#1 Left under the trench.path | 22 +++++ .../paths/#1(2) Right - Under the trench.path | 90 +++++++++++++++---- .../#2 Left(No SOTM) under the trench.path | 22 +++++ .../paths/#2(2) Right - Under the trench.path | 27 +++++- .../#3(No SOTM) Left under the trench.path | 8 +- src/main/java/frc/robot/RobotContainer.java | 17 ++-- .../robot/commands/gpm/AutoShootCommand.java | 6 +- .../frc/robot/commands/gpm/RunSpindexer.java | 31 +++++++ .../robot/commands/gpm/Superstructure.java | 18 ++-- .../controls/PS5ControllerDriverConfig.java | 7 +- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../frc/robot/subsystems/turret/Turret.java | 6 +- .../java/frc/robot/util/PhaseManager.java | 5 +- .../TrenchAssist/TrenchAssistConstants.java | 4 +- 14 files changed, 210 insertions(+), 55 deletions(-) create mode 100644 src/main/java/frc/robot/commands/gpm/RunSpindexer.java diff --git a/src/main/deploy/pathplanner/paths/#1 Left under the trench.path b/src/main/deploy/pathplanner/paths/#1 Left under the trench.path index 587ab10..fa5e4eb 100644 --- a/src/main/deploy/pathplanner/paths/#1 Left under the trench.path +++ b/src/main/deploy/pathplanner/paths/#1 Left under the trench.path @@ -119,6 +119,28 @@ ] } } + }, + { + "name": "Hood Down", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Hood Down" + } + } + }, + { + "name": "Stop Hood Down", + "waypointRelativePos": 0.5534308211473522, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/#1(2) Right - Under the trench.path b/src/main/deploy/pathplanner/paths/#1(2) Right - Under the trench.path index 4470532..d1bb09c 100644 --- a/src/main/deploy/pathplanner/paths/#1(2) Right - Under the trench.path +++ b/src/main/deploy/pathplanner/paths/#1(2) Right - Under the trench.path @@ -3,13 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.5467141162514837, - "y": 0.6942704626334513 + "x": 4.396690391459075, + "y": 0.43604982206405685 }, "prevControl": null, "nextControl": { - "x": 5.83681954887218, - "y": 0.7017406015037613 + "x": 6.580806642941875, + "y": 0.823380782918149 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.301672597864768, + "y": 0.823380782918149 + }, + "prevControl": { + "x": 6.72798848994848, + "y": 0.776996631189148 + }, + "nextControl": { + "x": 7.911232317280483, + "y": 0.8726655898756941 }, "isLocked": false, "linkedName": null @@ -20,12 +36,12 @@ "y": 1.5158029801324506 }, "prevControl": { - "x": 8.742932937101976, - "y": 0.1380740879492448 + "x": 8.57960288807054, + "y": 0.5564572982632667 }, "nextControl": { - "x": 8.5023178807947, - "y": 2.1111175496688723 + "x": 8.571257413997628, + "y": 2.265112692763938 }, "isLocked": false, "linkedName": null @@ -36,8 +52,8 @@ "y": 3.57 }, "prevControl": { - "x": 8.519890563503097, - "y": 2.960165562913907 + "x": 8.62505338078292, + "y": 2.275871886120997 }, "nextControl": null, "isLocked": false, @@ -46,19 +62,19 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.42628774422735277, + "waypointRelativePos": 0.85, "rotationDegrees": 29.999999999999996 }, { - "waypointRelativePos": 0.6975023126734518, + "waypointRelativePos": 1.4, "rotationDegrees": 90.84924216679515 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.7792207792207767, - "maxWaypointRelativePos": 1.6989130434782616, + "minWaypointRelativePos": 1.55, + "maxWaypointRelativePos": 2.6989130434782616, "constraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -71,7 +87,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.7151943462897536, + "maxWaypointRelativePos": 1.45, "constraints": { "maxVelocity": 6.0, "maxAcceleration": 5.0, @@ -83,7 +99,49 @@ } ], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Extend Intake" + } + }, + { + "type": "named", + "data": { + "name": "Spin Intake Rollers" + } + }, + { + "type": "named", + "data": { + "name": "Hood Down" + } + } + ] + } + } + }, + { + "name": "Stop Hood Down", + "waypointRelativePos": 0.65, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path b/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path index 0ae7192..0e4cf18 100644 --- a/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path +++ b/src/main/deploy/pathplanner/paths/#2 Left(No SOTM) under the trench.path @@ -114,6 +114,28 @@ ] } } + }, + { + "name": "Hood Down", + "waypointRelativePos": 0.9111361079864995, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Hood Down" + } + } + }, + { + "name": "Stop Hood Down", + "waypointRelativePos": 1.7885264341957245, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Stop Hood Down" + } + } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/#2(2) Right - Under the trench.path b/src/main/deploy/pathplanner/paths/#2(2) Right - Under the trench.path index d854b68..2e7f080 100644 --- a/src/main/deploy/pathplanner/paths/#2(2) Right - Under the trench.path +++ b/src/main/deploy/pathplanner/paths/#2(2) Right - Under the trench.path @@ -66,7 +66,32 @@ } ], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Retract Intake", + "waypointRelativePos": 0.5354330708661407, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Retract Intake" + } + }, + { + "type": "named", + "data": { + "name": "Hood Down" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path b/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path index 53e7d77..0bec0ed 100644 --- a/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path +++ b/src/main/deploy/pathplanner/paths/#3(No SOTM) Left under the trench.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 0.9893708609271523, - "y": 6.002997790234502 + "x": 0.925765932523468, + "y": 5.968318426038064 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.966 }, "prevControl": { - "x": 1.3625978647686838, - "y": 5.944756820877817 + "x": 1.497518014658156, + "y": 5.9695580132966395 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 35478b0..926877b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -92,7 +92,7 @@ public class RobotContainer { SmartDashboard.putString("RobotID", robotId.toString()); // Filling the SendableChooser on SmartDashboard - autoChooserInit(); + //autoChooserInit(); // dispatch on the robot switch (robotId) { @@ -141,12 +141,13 @@ public class RobotContainer { PathGroupLoader.loadPathGroups(); // Load the auto command try { - String leftSideAuto = "Left Side Auto"; + String leftSideAuto = "Left(No SOTM) - Under Trench"; PathPlannerAuto.getPathGroupFromAutoFile(leftSideAuto); auto = new PathPlannerAuto(leftSideAuto); } catch (IOException | ParseException e) { e.printStackTrace(); } + if(turret != null){ turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer)); } @@ -218,9 +219,8 @@ public class RobotContainer { } if (hood != null){ - Command hoodDown = new InstantCommand(()-> {hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0);}, hood); - NamedCommands.registerCommand("Hood Down", new InstantCommand(()->{hoodDown.schedule();})); - NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(()-> {hoodDown.cancel();})); + NamedCommands.registerCommand("Hood Down", new InstantCommand(()->{hood.forceHoodDown(true);})); + NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(()-> {hood.forceHoodDown(false);})); } @@ -268,11 +268,8 @@ public class RobotContainer { } } - public Command getAutoCommand() { - // get the selected Command - Command autoSelected = autoChooser.getSelected(); - - return autoSelected; + public Command getAutoCommand(){ + return auto; } public void logComponents() { diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 3aa1ee9..7cae9fc 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -190,9 +190,9 @@ public class AutoShootCommand extends Command { hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); - SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint); - SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint); - SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel()); + Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint); + Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint); + Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel()); /** Spindexer Stuff!! */ if(spindexer != null){ diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java new file mode 100644 index 0000000..7ffc366 --- /dev/null +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java @@ -0,0 +1,31 @@ +package frc.robot.commands.gpm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.spindexer.Spindexer; +import frc.robot.subsystems.turret.Turret; + +public class RunSpindexer extends Command { + private Spindexer spindexer; + private Turret turret; + + public RunSpindexer(Spindexer spindexer, Turret turret){ + this.spindexer = spindexer; + this.turret = turret; + + addRequirements(spindexer); + } + + @Override + public void execute() { + //if (turret.atSetpoint()){ + spindexer.maxSpindexer(); + // } else{ + // spindexer.stopSpindexer(); + // } + } + + @Override + public void end(boolean interrupted) { + spindexer.stopSpindexer(); + } +} diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 0de2673..6112b51 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -124,7 +124,7 @@ public class Superstructure extends Command { Translation2d.kZero, target3d.minus(lookahead3d), target == FieldConstants.getHubTranslation().toTranslation2d() ? - 2.0 : 5.0); + 2.0 : 2.0); double TOFAdjustment = 0.75; timeOfFlight = goalState.timeOfFlight() * TOFAdjustment; @@ -213,7 +213,7 @@ public class Superstructure extends Command { SmartDashboard.putNumber("Turret Offset", turretOffset); // Phase manager stuff phaseManager.update(drivepose, shooter, turret); - target = phaseManager.getTarget(); + target = phaseManager.getTarget(drivepose); updateDrivePose(); updateSetpoints(drivepose); @@ -223,11 +223,11 @@ public class Superstructure extends Command { } else { turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); - if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){ - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); - } else{ + // if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){ + // hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); + // } else{ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity); - } + // } shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); @@ -238,9 +238,9 @@ public class Superstructure extends Command { // } } - SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint); - SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint); - SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel()); + Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint); + Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint); + Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel()); SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString()); } diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index ecc2bd1..aa53754 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -16,6 +16,7 @@ import frc.robot.commands.gpm.AutoShootCommand; import frc.robot.commands.gpm.ClimbDriveCommand; import frc.robot.commands.gpm.IntakeMovementCommand; import frc.robot.commands.gpm.ReverseMotors; +import frc.robot.commands.gpm.RunSpindexer; import frc.robot.commands.gpm.Superstructure; import frc.robot.constants.Constants; import frc.robot.subsystems.Climb.LinearClimb; @@ -83,7 +84,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { () -> false, getDrivetrain()).withTimeout(2)); // Trench align - controller.get(DPad.LEFT).whileTrue(new StartEndCommand( + controller.get(PS5Button.CIRCLE).whileTrue(new StartEndCommand( () -> { getDrivetrain().setTrenchAssist(true); getDrivetrain().setTrenchAlign(true); @@ -95,7 +96,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // Reverse motors if (intake != null && spindexer != null) { - controller.get(PS5Button.CIRCLE).whileTrue(new ReverseMotors(intake, spindexer)); controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake, spindexer)); } @@ -141,8 +141,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // Spindexer if (spindexer != null) { // Will only run if we are not calling default shoot command - controller.get(PS5Button.LEFT_TRIGGER).onTrue(new InstantCommand(() -> spindexer.maxSpindexer())) - .onFalse(new InstantCommand(() -> spindexer.stopSpindexer())); + controller.get(PS5Button.LEFT_TRIGGER).whileTrue(new RunSpindexer(spindexer, turret)); } // Auto shoot diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ebe389f..9b1a45a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -30,7 +30,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - double powerModifier = .75; + double powerModifier = 1.0; public Shooter(){ updateInputs(); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 3a83e8d..773cb78 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -146,10 +146,10 @@ public class Turret extends SubsystemBase implements TurretIO{ } /** - * @return If the turret is at setpoint with tolerance of 5 degrees + * @return If the turret is at setpoint with tolerance of 10 degrees */ public boolean atSetpoint() { - return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(5.0); + return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(10.0); } /** @@ -210,7 +210,7 @@ public class Turret extends SubsystemBase implements TurretIO{ double motorGoalRotations = Units.radiansToRotations(best) * TurretConstants.GEAR_RATIO; // Clamp position setpoint to min and max angles - motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(180) * TurretConstants.GEAR_RATIO); + motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(TurretConstants.MIN_ANGLE) * TurretConstants.GEAR_RATIO, Units.degreesToRotations(TurretConstants.MAX_ANGLE) * TurretConstants.GEAR_RATIO); // Multiply goal velocity by kV double robotTurnCompensation = goalVelocityRadPerSec * TurretConstants.FEEDFORWARD_KV; diff --git a/src/main/java/frc/robot/util/PhaseManager.java b/src/main/java/frc/robot/util/PhaseManager.java index 9eb3931..c5e4451 100644 --- a/src/main/java/frc/robot/util/PhaseManager.java +++ b/src/main/java/frc/robot/util/PhaseManager.java @@ -87,7 +87,8 @@ public class PhaseManager { public Translation2d getTarget(Pose2d drivePose) { return wantedState == WantedState.SHOOTING ? FieldConstants.getHubTranslation().toTranslation2d() : (FieldConstants.isOnLeftSideOfField(drivePose.getTranslation()) - ? FieldConstants.getAllianceSideTranslation(true).toTranslation2d() - : FieldConstants.getAllianceSideTranslation(false).toTranslation2d()); + //TODO: reversed for sm reason + ? FieldConstants.getAllianceSideTranslation(false).toTranslation2d() + : FieldConstants.getAllianceSideTranslation(true).toTranslation2d()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java index c6e1208..f54bf47 100644 --- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java +++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java @@ -22,8 +22,8 @@ public class TrenchAssistConstants { }; public static final double[] SLIDE_LATITUDES = new double[] { - FieldConstants.FIELD_WIDTH - Units.inchesToMeters(30.0), - Units.inchesToMeters(30.0), // should be accurate, i think our field is slightly too small + FieldConstants.FIELD_WIDTH - Units.inchesToMeters(27.0), + Units.inchesToMeters(27.0), // should be accurate, i think our field is slightly too small // 6.550, // 0.668, -- 2.39.5