Updates and testing

Robotic-Racer14a · Robotic-Racer14a · commit db1acbc28b91 · 2024-03-20T21:44:33.000-04:00
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
 }
 
 java {
diff --git a/src/main/deploy/pathplanner/autos/Blue-SpeakerRight-C0-C3-C8-C7-C6.auto b/src/main/deploy/pathplanner/autos/Blue-SpeakerRight-C0-C3-C8-C7-C6.auto
@@ -2,8 +2,8 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 0.7693261898151704,
-      "y": 6.568323745728334
+      "x": 0.9454344324892869,
+      "y": 6.454465827910569
     },
     "rotation": 59.0
   },
diff --git a/src/main/deploy/pathplanner/autos/Red Auto.auto b/src/main/deploy/pathplanner/autos/Red Auto.auto
@@ -0,0 +1,32 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.2575195849614786,
+      "y": 5.55492627078484
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Limelight Path"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": "Testing Autos",
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/Limelight Path.path b/src/main/deploy/pathplanner/paths/Limelight Path.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2575195849614786,
+        "y": 5.55492627078484
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2575195849614786,
+        "y": 5.55492627078484
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.873019197758706,
+        "y": 5.55492627078484
+      },
+      "prevControl": {
+        "x": 1.873019197758706,
+        "y": 5.55492627078484
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 6.0,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Tests",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -252,7 +252,7 @@ public static class ShooterPosPot {
     public static final double SHOOTERPOT_HALF_COURT = 13.0;
     public static final double SHOOTERPOT_HALF_CHAIN_COURT = 12.6;
 
-    public static final double SHOOTER_AT_PICKUP = 13.7;//MIN = 9.8 MAX = 10.8      //5.7
+    public static final double SHOOTER_AT_PICKUP = 13.2;//MIN = 9.8 MAX = 10.8      //5.7
     public static final double SHOOTERPOS_RANGE = 0.5;
 
   }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -25,6 +25,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.LED.LEDRunnable;
+import frc.robot.commands.auto.AutoLimelight;
 import frc.robot.commands.auto.AutoStartDelivery;
 import frc.robot.commands.auto.AutoStartDeliveryTemp;
 import frc.robot.commands.auto.AutoStartDeliveryToSensor;
@@ -97,7 +98,7 @@ public class RobotContainer {
   private void configureBindings() {
     drivetrain.registerTelemetry(logger::telemeterize);
     drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driverJoystick, () -> getDrivetrainVelocityY(), () -> getAllianceColor()));   
-    led.setDefaultCommand(new LEDRunnable(led, ()-> intake.getIntakeSensor(), () -> delivery.getDeliveryTopSensor(), () -> shooter.getShooterUpToSpeed()).ignoringDisable(true));
+    led.setDefaultCommand(new LEDRunnable(led, ()-> intake.getIntakeSensor(), () -> delivery.getDeliveryTopSensor(), () -> delivery.getDeliveryBottomSensor(), () -> shooter.getShooterUpToSpeed()).ignoringDisable(true));
     
     //driverJoystick.back().whileTrue(new InstantCommand(() -> setMaxSpeed(Constants.Swerve.driveScale))).onFalse(new InstantCommand(() -> setMaxSpeed(1)));
     driverJoystick.povLeft().onTrue(new InstantCommand(() -> drivetrain.setRotationAngle(90)));
@@ -114,9 +115,9 @@ private void configureBindings() {
         )
       ));
 
-    driverJoystick.a().whileTrue(new InstantCommand(() -> drivetrain.setVisionRotate(true))
+    driverJoystick.a().whileTrue(new VisionRotate(drivetrain, driverJoystick, "limelight-blue", () -> allianceColor)
       .alongWith(new SetShooterPosByVision(shooterPot, () -> drivetrain.getPose(), () -> getAllianceColor(), () -> getDrivetrainVelocityX(), () -> delivery.getDeliveryTopSensor())));
-    driverJoystick.a().onFalse(new InstantCommand(() -> drivetrain.setVisionRotate(false)));
+    //driverJoystick.a().onFalse(new InstantCommand(() -> drivetrain.setVisionRotate(false)));
     //driverJoystick.leftStick().whileTrue(new SetShooterPosByVision(shooterPot, () -> drivetrain.getPose(), () -> getAllianceColor(), () -> getDrivetrainVelocityX(), () -> delivery.getDeliveryTopSensor()));
     // driverJoystick.b().onTrue(new InstantCommand(() -> delivery.engageNoteStop()));
     // driverJoystick.b().onFalse(new InstantCommand(() -> delivery.disengageNoteStop()));
@@ -127,12 +128,12 @@ private void configureBindings() {
     driverJoystick.rightBumper().onTrue(new InstantCommand(() -> drivetrain.setAngleToZero()));
     driverJoystick.rightBumper().onFalse(new InstantCommand(() -> drivetrain.setDriveAtAngleFalse()));
 
-    driverJoystick.leftTrigger().whileTrue(Commands.parallel(new HalfCourt(shooter, () -> drivetrain.getState().Pose.getY()),
+    driverJoystick.leftTrigger().whileTrue(Commands.parallel(new HalfCourt(shooter, () -> isChainShot()),
       new InstantCommand(() -> drivetrain.setDriveAtAngleFalse()),
       new ConditionalCommand(
         new InstantCommand(() -> shooterPot.setSetpoint(Constants.ShooterPosPot.SHOOTERPOT_HALF_COURT)),
         new InstantCommand(() -> shooterPot.setSetpoint(Constants.ShooterPosPot.SHOOTERPOT_HALF_CHAIN_COURT)),
-        () -> (drivetrain.getState().Pose.getY() >= Constants.FieldElements.cartman && drivetrain.getState().Pose.getY() <= Constants.FieldElements.longwood)
+        () -> (isChainShot())
       ),
       // new DelayedDelivery(delivery, Constants.Delivery.DELIVERY_FORWARD_SPEED, () -> shooter.getShooterUpToSpeed()),
       new ConditionalCommand(
@@ -192,7 +193,7 @@ private void configureBindings() {
       //operatorJoystick.back().whileTrue(new SetClimbSpeed(climb, () -> Utilities.deadband(operatorJoystick.getRightY(), 0.1)));
     //*************Operator Station *****************/
     //operatorStation.blueButton.onTrue(new InstantCommand(() -> climb.setClimberPosition(-65)));
-    operatorStation.blueButton.whileTrue(new SetClimbSpeed(climb, () -> -0.6));
+    //operatorStation.blueButton.whileTrue(new SetClimbSpeed(climb, () -> -0.6));
     operatorStation.whiteButton.whileTrue(new SetClimbSpeed(climb, () -> -0.6));
     operatorStation.yellowButton.whileTrue(new SetClimbSpeed(climb, () -> 0.6));
     operatorStation.blackButton.whileTrue(new InstantCommand(() -> drivetrain.setRotationAngle(getAmpRotationAngle()))
@@ -205,6 +206,7 @@ private void configureBindings() {
     operatorStation.clearSwitch.whileTrue(new DeliveryServoOverride(servo));
     operatorStation.redLeftSwitch.onTrue(new InstantCommand(() -> drivetrain.setRotationAngle(getEndgameRotationAngleLeft())));
     operatorStation.redRightSwitch.onTrue(new InstantCommand(() -> drivetrain.setRotationAngle(getEndgameRotationAngleRight())));
+    operatorStation.blueButton.whileTrue(new AutoLimelight(() -> startNoteDetecton()));
     
      //************* Test Joystick *****************/
      // testJoystick.a().onTrue(new InstantCommand(() -> climb.setClimberSetpoint(10)));
@@ -297,6 +299,8 @@ public RobotContainer() {
     NamedCommands.registerCommand("StopShooter", new InstantCommand(() -> shooter.setShooterDutyCycleZero()));
     NamedCommands.registerCommand("UseLimelight", new InstantCommand(() -> useLimelight()));
     NamedCommands.registerCommand("DontUseLimelight", new InstantCommand(() -> dontUseLimelight()));
+    NamedCommands.registerCommand("AutoDetect", new AutoLimelight(() -> startNoteDetecton()));
+    
 
     
     /* 
@@ -435,6 +439,26 @@ public PathPlannerPath goToAmp() {
     return path;
   }
 
+  public boolean isChainShot() {
+    return drivetrain.getState().Pose.getY() >= Constants.FieldElements.cartman && drivetrain.getState().Pose.getY() <= Constants.FieldElements.longwood;
+  }
+
+  public boolean startNoteDetecton() {
+    if (allianceColor == "blue") {
+      if (drivetrain.getState().Pose.getX() >= 5) {
+        return true;
+      } else {
+        return false;
+      }
+    } else {
+      if (drivetrain.getState().Pose.getX() <= 12) {
+        return true;
+      } else {
+        return false;
+      }
+    }
+  }
+
   public boolean doWeHaveNote() {
     return(intake.getIntakeSensor() || delivery.getDeliveryBottomSensor() || delivery.getDeliveryTopSensor());
   }
diff --git a/src/main/java/frc/robot/commands/LED/LEDRunnable.java b/src/main/java/frc/robot/commands/LED/LEDRunnable.java
@@ -9,13 +9,14 @@
 
 public class LEDRunnable extends Command {
   private final LED led;
-  private Supplier<Boolean> intakeSensor, deliveryTopSensor, upToSpeed;
+  private Supplier<Boolean> intakeSensor, deliveryTopSensor, deliveryBottomSensor, upToSpeed;
   
 
-  public LEDRunnable(LED led, Supplier<Boolean> intakeSensor, Supplier<Boolean> deliveryTopSensor, Supplier<Boolean> upToSpeed) {
+  public LEDRunnable(LED led, Supplier<Boolean> intakeSensor, Supplier<Boolean> deliveryTopSensor, Supplier<Boolean> deliveryBottomSensor, Supplier<Boolean> upToSpeed) {
     this.led = led;
     this.intakeSensor = intakeSensor;
     this.deliveryTopSensor = deliveryTopSensor;
+    this.deliveryBottomSensor = deliveryBottomSensor;
     this.upToSpeed = upToSpeed;
 
     addRequirements(led);
@@ -26,7 +27,7 @@ public void execute() {
         led.setColor(Color.kGreen);
     } else if (DriverStation.isTeleopEnabled() && deliveryTopSensor.get()) {
         led.setColor(Color.kBlue);
-    }  else if (DriverStation.isTeleopEnabled() && intakeSensor.get()) {
+    }  else if (DriverStation.isTeleopEnabled() && (intakeSensor.get() || deliveryBottomSensor.get())) {
         led.setColor(Color.kRed);
     } else if (DriverStation.isTeleopEnabled()) {
         led.setColor(Color.kBlack);
diff --git a/src/main/java/frc/robot/commands/auto/AutoLimelight.java b/src/main/java/frc/robot/commands/auto/AutoLimelight.java
@@ -0,0 +1,36 @@
+package frc.robot.commands.auto;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.nerdyfiles.vision.LimelightHelpers;
+
+public class AutoLimelight extends Command{
+    private boolean seeNote = false;
+    private Supplier<Boolean> startDetection;
+    
+    public AutoLimelight(Supplier<Boolean> startDetection) {
+        this.startDetection = startDetection;
+    }
+
+    @Override
+    public void initialize() {
+    }
+    
+    @Override
+    public void execute() {
+        if (startDetection.get()) {
+            var lastResult = LimelightHelpers.getLatestResults("limelight-coral").targetingResults;
+            seeNote = lastResult.valid;
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    @Override
+    public boolean isFinished() {
+        return seeNote; 
+    }
+}
diff --git a/src/main/java/frc/robot/commands/shooter/HalfCourt.java b/src/main/java/frc/robot/commands/shooter/HalfCourt.java
@@ -3,15 +3,15 @@
 import java.util.function.Supplier;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
 import frc.robot.subsystems.Shooter;
 
 public class HalfCourt extends Command {
     private Shooter shooter;
-    private Supplier<Double> robotY;
+    private Supplier<Boolean> chainShot;
 
-    public HalfCourt(Shooter shooter, Supplier<Double> robotY) {
+    public HalfCourt(Shooter shooter, Supplier<Boolean> chainShot) {
         this.shooter = shooter;
+        this.chainShot = chainShot;
         addRequirements(shooter);
     }
 
@@ -22,11 +22,11 @@ public void initialize() {
     
     @Override
     public void execute() {
-        // if (robotY.get() >= Constants.FieldElements.cartman && robotY.get() <= Constants.FieldElements.longwood) {
-        //     shooter.halfCourtChain();
-        // } else {
+        if (chainShot.get()) {
             shooter.halfCourt();
-        // }
+        } else {
+            shooter.halfCourtChain();
+        }
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/shooterPosition/SetShooterPosByVision.java b/src/main/java/frc/robot/commands/shooterPosition/SetShooterPosByVision.java
@@ -42,6 +42,7 @@ public void initialize() {
        } else {
             LimelightHelpers.setPriorityTagID("limelight-blue", 4);
        }
+
     }
     
     @Override
@@ -83,13 +84,21 @@ public void execute() {
         // SmartDashboard.putNumber("Shooter/New Position Setpoint", newSetpoint);
         // SmartDashboard.putNumber("Shooter/Mod New Position Setpoint", modNewSetpoint);
         // SmartDashboard.putNumber("Shooter/X Velocity", xVelocity.get());
-        if (currentY <= Constants.FieldElements.midFieldInMeters && topSensor.get()) { //TODO  
+        if (midfield() && topSensor.get()) { //TODO  
             shooterPosPot.setSetpoint(modNewSetpoint);
         } else {
             shooterPosPot.setSetpoint(Constants.ShooterPosPot.SHOOTER_AT_PICKUP);
         }
     }
 
+    public boolean midfield() {
+        if (allianceColor.get() == "blue") {
+            return currentY <= Constants.FieldElements.midFieldInMeters;
+        } else {
+            return currentY >= Constants.FieldElements.midFieldInMeters;
+        }
+    }
+
     @Override
     public void end(boolean interrupted) {
         shooterPosPot.getAndSetSetPoint();
diff --git a/src/main/java/frc/robot/commands/swerve/SwerveDriveCommand.java b/src/main/java/frc/robot/commands/swerve/SwerveDriveCommand.java
@@ -62,10 +62,10 @@ public void execute() {
         forward = Utilities.deadband(-driverJoystick.getLeftY(), Constants.Swerve.driveDeadband) * (Constants.Swerve.MaxSpeed/Constants.Swerve.driveAdjustment);
         strafe = Utilities.deadband(-driverJoystick.getLeftX(), Constants.Swerve.driveDeadband) * (Constants.Swerve.MaxSpeed/Constants.Swerve.driveAdjustment);
         rotation = Utilities.deadband(-driverJoystick.getRightX(), Constants.Swerve.driveDeadband) * Constants.Swerve.MaxAngularRate;
-        if (allianceColor.get() == "red") {
-            forward = -forward;
-            strafe = -strafe;
-        }
+        // if (allianceColor.get() == "red") {
+        //     forward = -forward;
+        //     strafe = -strafe;
+        // }
         //If we have set an angle to drive at and driver has hit drive at angle button, drive at that angle
         if ((drivetrain.rotationAngle != 0) && (drivetrain.driveAtAngle)) {
             swerveRequest = driveFacingAngle
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -20,6 +20,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -46,6 +47,13 @@ public class Drivetrain extends SwerveDrivetrain implements Subsystem {
     public boolean autoModLimelight = false;
     public String allianceColor = "Undefined";
 
+    /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+    private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0);
+    /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+    private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180);
+    /* Keep track if we've ever applied the operator perspective before or not */
+    private boolean hasAppliedOperatorPerspective = false;
+
     private final SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds();
 
     public Drivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) {
@@ -214,6 +222,14 @@ public void periodic() {
         //TODO: If auto doesn't work, uncomment
         //field.setRobotPose(pose);
       });
+      if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
+            DriverStation.getAlliance().ifPresent((allianceColor) -> {
+                this.setOperatorPerspectiveForward(
+                        allianceColor == Alliance.Red ? RedAlliancePerspectiveRotation
+                                : BlueAlliancePerspectiveRotation);
+                hasAppliedOperatorPerspective = true;
+            });
+        }
       if (DriverStation.isAutonomous() && !DriverStation.isDisabled()) {//TODO: Alliance flip
         if (allianceColor == "Undefined") {
             if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json

Original file line number	Diff line number	Diff line change
`@@ -1,6 +1,6 @@`
`1`	`1`	`plugins {`
`2`	`2`	`id "java"`
`3`		`- id "edu.wpi.first.GradleRIO" version "2024.3.1"`
	`3`	`+ id "edu.wpi.first.GradleRIO" version "2024.3.2"`
`4`	`4`	`}`
`5`	`5`
`6`	`6`	`java {`
Original file line number	Diff line number	Diff line change
`@@ -252,7 +252,7 @@ public static class ShooterPosPot {`
`252`	`252`	`public static final double SHOOTERPOT_HALF_COURT = 13.0;`
`253`	`253`	`public static final double SHOOTERPOT_HALF_CHAIN_COURT = 12.6;`
`254`	`254`
`255`		`- public static final double SHOOTER_AT_PICKUP = 13.7;//MIN = 9.8 MAX = 10.8 //5.7`
	`255`	`+ public static final double SHOOTER_AT_PICKUP = 13.2;//MIN = 9.8 MAX = 10.8 //5.7`
`256`	`256`	`public static final double SHOOTERPOS_RANGE = 0.5;`
`257`	`257`
`258`	`258`	`}`