Merge pull request #33 from 2491-NoMythic/CollectNoteTesting

Collect note testing

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

@@ -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<String> climbSpotChooser;
+  private SendableChooser<Command> 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)));}
   }
 

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,37 +20,41 @@
 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.
   }
 
   // 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);
-    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); 
+  }
 }

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

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));