tested auton (forgot to push)

src/main/deploy/pathplanner/autos/Actuate In.auto → src/main/deploy/pathplanner/autos/2 Piece Auton.auto

@@ -2,15 +2,21 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.28126094090026,
-      "y": 4.751777431482791
+      "x": 1.308995050422533,
+      "y": 5.509843091758257
     },
     "rotation": 180.0
   },
   "command": {
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot"
+          }
+        },
         {
           "type": "path",
           "data": {

src/main/deploy/pathplanner/paths/REAL AUTON IN.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.9746136789570874,
-        "y": 4.095403506122328
+        "x": 2.29,
+        "y": 4.11
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.3458343999061597,
-        "y": 3.9716988909377653
+        "x": 1.8361613352382946,
+        "y": 4.11
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.28,
-        "y": 4.75
+        "x": 1.3459738631188973,
+        "y": 5.4821089822359825
       },
       "prevControl": {
-        "x": 1.1053783204258805,
-        "y": 4.766568109803206
+        "x": 1.1713521835447778,
+        "y": 5.4986770920391885
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
   "eventMarkers": [
     {
       "name": "New Event Marker",
-      "waypointRelativePos": 0.45,
+      "waypointRelativePos": 0.15,
       "command": {
         "type": "sequential",
         "data": {

src/main/deploy/pathplanner/paths/REAL AUTON OUT.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.28,
-        "y": 4.751777431482791
+        "x": 1.308995050422533,
+        "y": 5.509843091758257
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.5493573329488999,
-        "y": 4.724043321960518
+        "x": 1.5783523833714328,
+        "y": 5.482108982235983
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.0393266011757247,
+        "x": 2.29,
         "y": 4.113892912470511
       },
       "prevControl": {
-        "x": 1.898865075387808,
-        "y": 4.1824987471596575
+        "x": 1.4672214175058977,
+        "y": 4.261808163255966
       },
       "nextControl": null,
       "isLocked": false,

src/main/java/frc/robot/Constants.java

@@ -130,8 +130,8 @@ public static class PID {
 
 
   public static class Presets {
-    public static final double CLOSE_SHOOTER_ANGLE = 0;
-    public static final double CLOSE_ELEVATOR_HEIGHT = 0;
+    public static final double CLOSE_SHOOTER_ANGLE = 50.12;
+    public static final double CLOSE_ELEVATOR_HEIGHT = 0.06;
 
     public static final double ACTUATE_SHOOTER_ANGLE = 52;
 

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

@@ -83,10 +83,16 @@ public class RobotContainer {
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
+    NamedCommands.registerCommand("brake", new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
+
     NamedCommands.registerCommand("actuateOut", new SetShooter(_shooterSubsystem, () -> Presets.ACTUATE_SHOOTER_ANGLE).andThen(
       new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE)
     ));
-    NamedCommands.registerCommand("actuateIn", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED).alongWith(new SpinShooter(_shooterSubsystem, ShooterState.SHOOT, true)));
+
+    NamedCommands.registerCommand("actuateIn", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.INTAKE).alongWith(
+      new SpinShooter(_shooterSubsystem, ShooterState.SHOOT, true)
+    ));
+
     NamedCommands.registerCommand("shoot", new AutonShoot(_shooterSubsystem, _elevatorSubsystem, _ledSubsystem, _swerveSubsystem, _intakeSubsystem));
 
     // Drive/Operate default commands
@@ -137,6 +143,11 @@ private void configureBindings() {
     _operatorController.triangle().whileTrue(new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.OUTTAKE));
     _operatorController.cross().whileTrue(new FeedActuate(_intakeSubsystem, FeedMode.INTAKE));
 
+    // test for the presets
+    _driveController.square().onTrue(
+      new AutonShoot(_shooterSubsystem, _elevatorSubsystem, _ledSubsystem, _swerveSubsystem, _intakeSubsystem)
+    );
+
     // driver bindings
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));
     _driveController.L1().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetPose(new Pose2d()), _swerveSubsystem));

src/main/java/frc/robot/commands/shooter/AutoAim.java

@@ -142,9 +142,11 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.out.println("AIMING");
+
     double currentSwerveHeading = _swerve.getHeading().getDegrees();
 
-    if (_overrideDesired) {
+    if (!_overrideDesired) {
       double[] setpoints = _swerve.speakerSetpoints();
 
       _desiredSwerveHeading = setpoints[0];

src/main/java/frc/robot/commands/shooter/AutonShoot.java

@@ -4,12 +4,16 @@
 
 package frc.robot.commands.shooter;
 
+import java.util.concurrent.locks.Condition;
+
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants.Presets;
 import frc.robot.commands.intake.FeedActuate;
+import frc.robot.commands.swerve.BrakeSwerve;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
@@ -24,6 +28,7 @@
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class AutonShoot extends SequentialCommandGroup {
   private double _headingPreset;
+  private boolean _isAimed = false;
 
   /** Creates a new AutonShoot. */
   public AutonShoot(
@@ -38,14 +43,16 @@ public AutonShoot(
     addCommands(
       Commands.runOnce(() -> {_headingPreset = (UtilFuncs.GetAlliance() == Alliance.Red) ? 0 : 180;}),
 
-      // pre-shooting (rev up if needed, while auto aiming, while feeding in for squish)
       new ParallelCommandGroup(
-        new SpinShooter(shooter, ShooterState.SHOOT).unless(() -> shooter.isState(ShooterState.SHOOT)).withTimeout(3),
-        new AutoAim(shooter, elevator, leds, swerve, Presets.CLOSE_SHOOTER_ANGLE, Presets.CLOSE_ELEVATOR_HEIGHT, _headingPreset).withTimeout(5),
-        new FeedActuate(intake, FeedMode.INTAKE).withTimeout(3)
+        new SpinShooter(shooter, ShooterState.SHOOT).unless(() -> shooter.isState(ShooterState.SHOOT)).withTimeout(1.5),
+        new AutoAim(shooter, elevator, leds, swerve, Presets.CLOSE_SHOOTER_ANGLE, Presets.CLOSE_ELEVATOR_HEIGHT, 180).onlyIf(
+          () -> !_isAimed
+        ).withTimeout(3).andThen(() -> _isAimed = true),
+        new FeedActuate(intake, FeedMode.INTAKE).unless(() -> intake.isFeedMode(FeedMode.INTAKE)).withTimeout(1)
       ),
+      // pre-shooting (rev up if needed, while auto aiming, while feeding in for squish)
       // new WaitUntilCommand(() -> UtilFuncs.InRange(shooter.getVelocity(), Encoders.SHOOTER_SHOOT_VEL, 1)),
-      new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(2),
+      new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(0.5),
       new SpinShooter(shooter, ShooterState.NONE, true)
     );
   }

src/main/java/frc/robot/commands/shooter/SpinShooter.java

@@ -38,6 +38,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.out.println("SPINNING");
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/swerve/BrakeSwerve.java

@@ -27,7 +27,7 @@ public BrakeSwerve(SwerveDriveSubsystem swerveDrive, LEDSubsystem leds) {
     _swerveDrive = swerveDrive;
     _leds = leds;
 
-    addRequirements(_swerveDrive, _leds);
+    addRequirements(_swerveDrive);
   }
 
   /**

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

@@ -41,7 +41,7 @@ public ElevatorSubsystem() {
 
     _leftMotor.getConfigurator().apply(softLimits);
 
-    _heightController.setTolerance(0.02);
+    _heightController.setTolerance(0.01);
 
     SmartDashboard.putData("ELEVATOR PID", _heightController);
   }

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -44,6 +44,9 @@ public enum ActuatorState {
     STOWED, OUT, NONE
   }
 
+  private FeedMode _feedMode = FeedMode.NONE;
+  private ActuatorState _actuatorState = ActuatorState.NONE;
+
   /** Creates a new IntakeSubsystem. */
   public IntakeSubsystem() {
     _feedMotor = new CANSparkMax(Constants.CAN.INTAKE_FEED, MotorType.kBrushless);
@@ -84,8 +87,12 @@ public IntakeSubsystem() {
   //   return false;
   // }
 
-  public void work() {
-    _actuatorMotor.set(0.1);
+  public boolean isFeedMode(FeedMode feedMode) {
+    return _feedMode == feedMode;
+  }
+
+  public boolean isActuatorState(ActuatorState actuatorState) {
+    return _actuatorState == actuatorState;
   }
 
   /**
@@ -112,6 +119,8 @@ public double getActuator() {
    */
   public void actuate(ActuatorState actuatorState) {
     double out = 0;
+
+    _actuatorState = actuatorState;
 
     switch (actuatorState) {
       case STOWED :
@@ -161,6 +170,8 @@ public void actuate(ActuatorState actuatorState) {
    *            How to feed.
    */
   public void feed(FeedMode feedMode) {
+    _feedMode = feedMode;
+
     switch (feedMode) {
       case INTAKE :
         _feedMotor.set(Constants.Speeds.INTAKE_FEED_SPEED);

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -72,6 +72,8 @@ public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putNumber("SHOOTER ANGLE ENCODER", _angleMotor.getPosition().getValueAsDouble());
     SmartDashboard.putNumber("SHOOTER ANGLE", getAngle());
+
+    SmartDashboard.putNumber("SHOOTER PERCENT OUTPUT", _leftMotor.get());
   }
 
   /**

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -86,7 +86,7 @@ public class SwerveDriveSubsystem extends SubsystemBase {
       Constants.Physical.SWERVE_KINEMATICS, getHeadingRaw(),
       new SwerveModulePosition[]{_frontLeft.getPosition(), _frontRight.getPosition(), _backRight.getPosition(),
           _backLeft.getPosition()},
-      new Pose2d(), VecBuilder.fill(0.01, 0.01, 0.01), VecBuilder.fill(0.9, 0.9, 0.9));
+      new Pose2d(), VecBuilder.fill(0.01, 0.01, 0.01), VecBuilder.fill(0.9, 0.9, 1.2));
 
   // OTHER POSSIBLE STD VALUES:
   // VecBuilder.fill(0.006, 0.006, 0.007), VecBuilder.fill(0.52, 0.52, 1.35)
@@ -168,6 +168,8 @@ public void periodic() {
       _backLeft.getPosition()
     });
 
+    SmartDashboard.putBoolean("IN RANGE", _visionSubsystem.isValid());
+
     if (_visionSubsystem.isValid()) { // TODO: make sure this works
       Optional<Pose2d> visionBotpose = _visionSubsystem.getBotpose();
       if (visionBotpose.isPresent()) {

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -98,8 +98,9 @@ public boolean isValid() {
     JsonNode tags = _limelight.getTags();
 
     for (JsonNode tag : tags) {
-      double distance = ((ArrayNode) tag.get("t6t_rs")).get(0).asDouble();
+      double distance = ((ArrayNode) tag.get("t6t_cs")).get(2).asDouble();
       if (distance <= FieldConstants.TAG_DISTANCE_THRESHOLD) {
+        SmartDashboard.putNumber("TAG DISTANCE", distance);
         return true;
       }
     }