Merge branch 'main' into AngleShooterSubsystem

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
+}

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
+}

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
+}

src/main/java/frc/robot/RobotContainer.java

@@ -44,6 +44,7 @@
 import frc.robot.commands.goToPose.GoToAmp;
 import frc.robot.commands.goToPose.GoToClimbSpot;
 import frc.robot.commands.climber_commands.AutoClimb;
+import frc.robot.commands.climber_commands.ClimberPullDown;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.Lights;
@@ -89,6 +90,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;
@@ -105,6 +107,7 @@ public class RobotContainer {
   private IndexCommand defaulNoteHandlingCommand;
   private IndexerSubsystem indexer;
   private SendableChooser<String> climbSpotChooser;
+  private SendableChooser<Command> autoChooser;
   // Replace with CommandPS4Controller or CommandJoystick if needed
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
@@ -115,7 +118,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);
@@ -131,6 +135,7 @@ public RobotContainer() {
     if(lightsExist) {lightsInst();}
     if(indexerExists) {indexInit();}
     if(intakeExists && shooterExists && indexerExists) {indexCommandInst();}
+    Limelight.useDetectorLimelight(useDetectorLimelight);
     autoInit();
     // Configure the trigger bindings
     configureBindings();
@@ -179,8 +184,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();
@@ -215,12 +221,16 @@ 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) {
       new Trigger(operatorController::getCrossButton).onTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));
       new Trigger(operatorController::getTriangleButton).onTrue(new InstantCommand(()-> climber.climberGo(ClimberConstants.CLIMBER_SPEED_UP))).onFalse(new InstantCommand(()-> climber.climberStop()));
-      new Trigger(operatorController::getCrossButtonPressed).onTrue(new AutoClimb(climber));
+      new Trigger(operatorController::getSquareButton).onTrue(new ClimberPullDown(climber));
     }
+
     // //Intake bindings
     // new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE));
    // new Trigger(operatorController::getL2Button).onTrue(new IntakeCommand(intake, iDirection.OUTAKE));
@@ -240,7 +250,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,23 +285,26 @@ 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));}
-    if(indexerExists) {NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.feederFeed(0.5), indexer));
-    NamedCommands.registerCommand("stopFeedingShooter", new InstantCommand(indexer::feederOff, indexer));}
-    if(intakeExists) {NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1)));};
-    if(intakeExists) {NamedCommands.registerCommand("autoPickup", new CollectNote(driveTrain, intake, limelight));}
+
+    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, limelight));
+    NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1)));}
   }
-  
+
   public void teleopPeriodic() {
     SmartDashboard.putData(driveTrain.getCurrentCommand());
     driveTrain.calculateSpeakerAngle();
+    SmartDashboard.putNumber("Is Note Seen?", limelight.getNeuralDetectorValues().ta);
   }
 
   public void disabledPeriodic() {

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,38 +20,40 @@
 public class CollectNote extends Command {
 
   DrivetrainSubsystem drivetrain;
-  IntakeSubsystem intake;
   LimelightDetectorData detectorData;
   Limelight limelight;
+  double runsInvalid;
 
   PIDController txController;
   PIDController tyController;
+  SlewRateLimiter tyLimiter;
 
   double tx;
   double ty;
   /** Creates a new CollectNote. */
-  public CollectNote(DrivetrainSubsystem drivetrain, IntakeSubsystem intake, Limelight limelight) {
-    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.
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     txController = new PIDController(
-        K_DETECTOR_TX_P,
-        K_DETECTOR_TX_I,
-        K_DETECTOR_TX_D);
+        Vision.K_DETECTOR_TX_P,
+        Vision.K_DETECTOR_TX_I,
+        Vision.K_DETECTOR_TX_D);
     tyController = new PIDController(
-        K_DETECTOR_TA_P,
-        K_DETECTOR_TA_I,
-        K_DETECTOR_TA_D);
+        Vision.K_DETECTOR_TY_P,
+        Vision.K_DETECTOR_TY_I,
+        Vision.K_DETECTOR_TY_D);
+    tyLimiter = new SlewRateLimiter( 1, -1, 0);
     txController.setSetpoint(0);
     tyController.setSetpoint(0);
-    txController.setTolerance(1, 0.25);
+    txController.setTolerance(3.5, 0.25);
     tyController.setTolerance(1, 0.25);
 
   }
@@ -65,19 +69,25 @@ 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;
     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
+    // 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,
-      -tyController.calculate(ty),
       txController.calculate(tx)));
   }
 
@@ -92,6 +102,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return ((tyController.atSetpoint() && txController.atSetpoint()) || detectorData == null || !detectorData.isResultValid); 
+    return ((tyController.atSetpoint() && txController.atSetpoint()) || detectorData == null || runsInvalid>30); 
   }
 }

src/main/java/frc/robot/commands/IndexCommand.java

@@ -42,15 +42,15 @@ public void initialize() {
   public void execute() {
     if (m_Indexer.getInchesFromSensor()>8) {
       intake.intakeYes(1);
-      m_Indexer.holderRetrieve(0.5);
+      m_Indexer.on();
       shooter.turnOff();
     } else {
       intake.intakeOff();
-      m_Indexer.holderOff();
+      m_Indexer.off();
       shooter.shootThing(1);
     }
     if (shootButtonSupplier.getAsBoolean()) {
-      m_Indexer.feederFeed(0.5);
+      m_Indexer.on();
     }
   }