diff --git a/src/main/deploy/pathplanner/autos/testCollectNote.auto b/src/main/deploy/pathplanner/autos/testCollectNote.auto new file mode 100644 index 00000000..2c9a9871 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/testCollectNote.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.1808522786677846, + "y": 7.040713298508448 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "testCollectNotepath1" + } + }, + { + "type": "named", + "data": { + "name": "pring HIIII" + } + }, + { + "type": "path", + "data": { + "pathName": "testCollectNotepath2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/testCollectNotepath1.path b/src/main/deploy/pathplanner/paths/testCollectNotepath1.path new file mode 100644 index 00000000..4f240390 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/testCollectNotepath1.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.6580237193689102, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.794358416712089, + "y": 6.982284142504229 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.057289618731077, + "y": 7.0 + }, + "prevControl": { + "x": 1.9209549213878978, + "y": 7.001760527838968 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/testCollectNotepath2.path b/src/main/deploy/pathplanner/paths/testCollectNotepath2.path new file mode 100644 index 00000000..c476616e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/testCollectNotepath2.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.793721758262023, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 4.930056455605202, + "y": 6.982284142504229 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.319584162299998, + "y": 7.0 + }, + "prevControl": { + "x": 5.183249464956819, + "y": 7.001760527838968 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d69423c9..4077ee9e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -89,6 +89,7 @@ public class RobotContainer { private final boolean climberExists = Preferences.getBoolean("Climber", true); private final boolean lightsExist = Preferences.getBoolean("Lights", true); private final boolean indexerExists = Preferences.getBoolean("Indexer", true); + private final boolean useDetectorLimelight = Preferences.getBoolean("Detector Limelight", true); private DrivetrainSubsystem driveTrain; private IntakeSubsystem intake; @@ -104,6 +105,7 @@ public class RobotContainer { private IndexCommand defaulNoteHandlingCommand; private IndexerSubsystem indexer; private SendableChooser climbSpotChooser; + private SendableChooser autoChooser; // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -114,7 +116,8 @@ public RobotContainer() { Preferences.initBoolean("Shooter", false); Preferences.initBoolean("Lights", false); Preferences.initBoolean("Indexer", false); - + Preferences.initBoolean("Detector Limelight", false); + driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); @@ -130,6 +133,7 @@ public RobotContainer() { if(lightsExist) {lightsInst();} if(indexerExists) {indexInit();} if(intakeExists && shooterExists && indexerExists) {indexCommandInst();} + Limelight.useDetectorLimelight(useDetectorLimelight); autoInit(); // Configure the trigger bindings configureBindings(); @@ -177,8 +181,9 @@ private void indexCommandInst() { private void autoInit() { configureDriveTrain(); - SmartDashboard.putData("Auto Chooser", AutoBuilder.buildAutoChooser()); registerNamedCommands(); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); } private void limelightInit() { limelight = Limelight.getInstance(); @@ -213,6 +218,8 @@ private void configureBindings() { () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), driverController::getR1Button)); + new Trigger(driverController::getCircleButton).onTrue(new CollectNote(driveTrain, limelight)); + new Trigger(driverController::getTouchpadPressed).onTrue(new InstantCommand(driveTrain::stop, driveTrain)); if(shooterExists) {new Trigger(operatorController::getCircleButton).onTrue(new ManualShoot(shooter));} if(climberExists) { @@ -240,7 +247,7 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); + return autoChooser.getSelected(); } private double modifyAxis(double value, double deadband) { @@ -275,17 +282,19 @@ private void configureDriveTrain() { new Translation2d(DRIVETRAIN_TRACKWIDTH_METERS / 2.0, DRIVETRAIN_WHEELBASE_METERS / 2.0).getNorm(), //drive base radius new ReplanningConfig() ), - ()->DriverStation.getAlliance().equals(Alliance.Red), + ()->DriverStation.getAlliance().get().equals(Alliance.Red), driveTrain ); } private void registerNamedCommands() { NamedCommands.registerCommand("stopDrivetrain", new InstantCommand(driveTrain::stop, driveTrain)); + if(shooterExists) {NamedCommands.registerCommand("shooterOn", new InstantCommand(()->shooter.shootThing(1), shooter)); + NamedCommands.registerCommand("stopFeedingShooter", new InstantCommand(indexer::off, indexer));} - if(indexerExists) {NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.on(), indexer));} - if(intakeExists) {NamedCommands.registerCommand("autoPickup", new CollectNote(driveTrain, intake)); + if(indexerExists) {NamedCommands.registerCommand("feedShooter", new InstantCommand(indexer::on, indexer));} + if(intakeExists) {NamedCommands.registerCommand("autoPickup", new CollectNote(driveTrain, limelight)); NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1)));} } diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index 64f0c67a..cb0949dd 100644 --- a/src/main/java/frc/robot/commands/CollectNote.java +++ b/src/main/java/frc/robot/commands/CollectNote.java @@ -7,8 +7,10 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.DriveConstants; +import frc.robot.settings.Constants.Vision; import frc.robot.settings.LimelightDetectorData; import frc.robot.subsystems.DrivetrainSubsystem; import static frc.robot.settings.Constants.DriveConstants.*; @@ -18,19 +20,22 @@ public class CollectNote extends Command { DrivetrainSubsystem drivetrain; - IntakeSubsystem intake; LimelightDetectorData detectorData; + Limelight limelight; + double runsInvalid; PIDController txController; - PIDController taController; + PIDController tyController; + SlewRateLimiter tyLimiter; double tx; - double ta; + double ty; /** Creates a new CollectNote. */ - public CollectNote(DrivetrainSubsystem drivetrain, IntakeSubsystem intake) { - addRequirements(drivetrain, intake); + public CollectNote(DrivetrainSubsystem drivetrain, Limelight limelight) { + addRequirements(drivetrain); this.drivetrain = drivetrain; - this.intake = intake; + this.limelight = limelight; + runsInvalid = 0; // Use addRequirements() here to declare subsystem dependencies. } @@ -38,17 +43,18 @@ public CollectNote(DrivetrainSubsystem drivetrain, IntakeSubsystem intake) { @Override public void initialize() { txController = new PIDController( - K_DETECTOR_TX_P, - K_DETECTOR_TX_I, - K_DETECTOR_TX_D); - taController = new PIDController( - K_DETECTOR_TA_P, - K_DETECTOR_TA_I, - K_DETECTOR_TA_D); + Vision.K_DETECTOR_TX_P, + Vision.K_DETECTOR_TX_I, + Vision.K_DETECTOR_TX_D); + tyController = new PIDController( + Vision.K_DETECTOR_TY_P, + Vision.K_DETECTOR_TY_I, + Vision.K_DETECTOR_TY_D); + tyLimiter = new SlewRateLimiter( 1, -1, 0); txController.setSetpoint(0); - taController.setSetpoint(0.5); - txController.setTolerance(1, 0.25); - taController.setTolerance(1, 0.25); + tyController.setSetpoint(0); + txController.setTolerance(3.5, 0.25); + tyController.setTolerance(1, 0.25); } @@ -63,31 +69,39 @@ public void execute() { return; } if (!detectorData.isResultValid) { - - drivetrain.stop(); + if (runsInvalid <= 10) { // don't stop imediately, in case only a couple frames were missed + drivetrain.drive(new ChassisSpeeds(tyLimiter.calculate(0), 0, 0)); + } else { + drivetrain.stop(); + } System.err.println("invalidDetectorData"); + runsInvalid++; return; + } else { + runsInvalid = 0; } tx = detectorData.tx; - ta = detectorData.ta; - - if (taController.atSetpoint() && txController.atSetpoint()) { - drivetrain.stop(); - } else { - //drives the robot forward faster if the object is taking up less of the screen, and turns it more based on how far away the object is from x=0 - drivetrain.drive(new ChassisSpeeds(1/taController.calculate(ta), 0, txController.calculate(tx))); - } + ty = detectorData.ty; + + // drives the robot forward faster if the object is higher up on the screen, and turns it more based on how far away the object is from x=0 + drivetrain.drive(new ChassisSpeeds( + tyLimiter.calculate(-tyController.calculate(ty)), + 0, + txController.calculate(tx))); } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + drivetrain.stop(); + } // Returns true when the command should end. @Override public boolean isFinished() { - return (taController.atSetpoint() && txController.atSetpoint()); } + return ((tyController.atSetpoint() && txController.atSetpoint()) || detectorData == null || runsInvalid>30); + } } diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 73189cf7..0cd17b3a 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -237,14 +237,6 @@ private DriveConstants() { public static final double k_BALANCE_TOLORANCE_DEGREES = 10.0; public static final double k_BALANCE_TOLORANCE_DEG_PER_SEC = 1; - public static final double K_DETECTOR_TA_P = 1; - public static final double K_DETECTOR_TA_I = 0; - public static final double K_DETECTOR_TA_D = 0; - - public static final double K_DETECTOR_TX_P = 1; - public static final double K_DETECTOR_TX_I = 0; - public static final double K_DETECTOR_TX_D = 0; - public static final PathConstraints DEFAUL_PATH_CONSTRAINTS = new PathConstraints(2, 1.5, Math.toRadians(360), Math.toRadians(360)); public static final double k_PICKUP_NOTE_ta_P = 1; @@ -412,9 +404,9 @@ public final class Vision{ public static final double K_DETECTOR_TX_I = 0; public static final double K_DETECTOR_TX_D = 0; - public static final double K_DETECTOR_TA_P = 0.1; - public static final double K_DETECTOR_TA_I = 0; - public static final double K_DETECTOR_TA_D = 0; + public static final double K_DETECTOR_TY_P = 0.1; + public static final double K_DETECTOR_TY_I = 0; + public static final double K_DETECTOR_TY_D = 0; } public final class PathConstants{ //Welcome, to Pathconstantic Park diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 6137d8f1..a4176b04 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -31,10 +31,10 @@ public static void useAprilTagLimelight(boolean enabled) { aprilTagEnabled = enabled; } public static void forceTrustAprilTag(boolean enabled) { - aprilTagEnabled = enabled; + aprilTagForceTrust = enabled; } public static void useDetectorLimelight(boolean enabled) { - aprilTagEnabled = enabled; + detectorEnabled = enabled; } public LimelightValues getLimelightValues(String name) { return new LimelightValues(LimelightHelpers.getLatestResults(name).targetingResults, LimelightHelpers.getTV(name));