WPI branch

src/main/java/frc/robot/Drive.java

@@ -9,9 +9,9 @@
 
 
 public class Drive extends SubsystemBase{
-    private final PIDController xController = new PIDController(.01, 0.0, 0.0);
-    private final PIDController yController = new PIDController(.01, 0.0, 0.0);
-    private final PIDController headingController = new PIDController(.01, 0.0, 0.0);
+    private final PIDController xController = new PIDController(10, 0.0, 0.0);
+    private final PIDController yController = new PIDController(10, 0.0, 0.0);
+    private final PIDController headingController = new PIDController(5, 0.0, 0.0);
     private SwerveSubsystem subsystem;
 
     public Drive(SwerveSubsystem subsystem) {
@@ -29,7 +29,7 @@ public void followTrajectory(SwerveSample sample) {
             );
 
         ChassisSpeeds robotRelativeSpeeds =
-                ChassisSpeeds.fromRobotRelativeSpeeds(fieldRelativeSpeeds, pose.getRotation());
+                ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, pose.getRotation());
             subsystem.setChassisSpeeds(robotRelativeSpeeds);
 
     }

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

@@ -336,6 +336,16 @@ private void configureBindings() {
         }
 
         public void putShuffleboardCommands() {
+                                SmartDashboard.putData(
+                                        "intakedeployer/Deployment State: UP",
+                                        new SetDeploymentState(intakeDeployer, DeploymentState.UP));
+                        SmartDashboard.putData(
+                                        "intakedeployer/Deployment State: DOWN",
+                                        new SetDeploymentState(intakeDeployer, DeploymentState.DOWN));
+                                        SmartDashboard.putData(
+                                        "Shooting State: Auto aim",
+                                        new SetShootingState(shootState, ShootState.AUTO_AIM));
+
                 if (Constants.DEBUG) {
                         SmartDashboard.putNumber(RunDashboardShotTest.ANGLER_TARGET_POSITION_KEY, 0.0);
                         SmartDashboard.putNumber(RunDashboardShotTest.SHOOTER_TARGET_RPM_KEY, Constants.SHOOTER_SPEED);
@@ -508,12 +518,6 @@ public void putShuffleboardCommands() {
                         SmartDashboard.putData(
                                         "Shooting State: Shuttling",
                                         new SetShootingState(shootState, ShootState.SHUTTLING));
-                        SmartDashboard.putData(
-                                        "intakedeployer/Deployment State: UP",
-                                        new SetDeploymentState(intakeDeployer, DeploymentState.UP));
-                        SmartDashboard.putData(
-                                        "intakedeployer/Deployment State: DOWN",
-                                        new SetDeploymentState(intakeDeployer, DeploymentState.DOWN));
                         SmartDashboard.putData(
                                         "intakedeployer/Deployment State: STOPPED",
                                         new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED));

src/main/java/frc/robot/apriltags/SimApriltagIO.java

@@ -44,8 +44,8 @@ public void simReadings() {
                     if (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) {
                         VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0);
                         Vector<N3> stdDevs = truster.calculateTrust(measurement);
-                        double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0);
-                        double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1);
+                        double readingX = pose.getX(); //+ random.nextGaussian() * stdDevs.get(0);
+                        double readingY = pose.getY(); //+ random.nextGaussian() * stdDevs.get(1);
                         double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2);
                         Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw));
                         distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation());

src/main/java/frc/robot/autochooser/AutoAction.java

@@ -8,6 +8,7 @@
 public enum AutoAction {
   DO_NOTHING("Do Nothing"),
   SHOOT("Shoot"),
+  FAST_SHOOT("Shoot depot"),
   //SHOOT_PICKUP("Shoot and Pickup"),
   //DISTURBANCE("Disturbance"),
   //NEUTRAL_ZONE("Shoot and Neutral Zone Pickup"),

src/main/java/frc/robot/autochooser/AutoChooser.java

@@ -11,6 +11,8 @@
 import frc.robot.Robot;
 import frc.robot.commands.auto.neutral.DepotNeutral;
 import frc.robot.commands.auto.neutral.OutpostNeutral;
+import frc.robot.commands.auto.newauto.FastDepotRed;
+import frc.robot.commands.auto.newauto.FastDepotBlue;
 import frc.robot.commands.auto.shoot.ShootBlue;
 import frc.robot.commands.auto.shoot.ShootRed;
 import frc.robot.commands.auto.disturbance.DepotDisturbance;
@@ -82,9 +84,9 @@ private void populateChoosers() {
             switch (location) {
                 case INVALID -> {} // Skip the invalid case.
                 case ZERO -> { // Default
-                    locationChooser.addDefaultOption(location.toString(), location);
+                    locationChooser.addDefaultOption(location.getShuffleboardName(), location);
                 }
-                default -> {locationChooser.addOption(location.toString(), location);}
+                default -> {locationChooser.addOption(location.getShuffleboardName(), location);}
             };
         }
         for (AutoAction action : AutoAction.values()) {
@@ -125,6 +127,12 @@ private void populateCommandMap() {
             new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake));
         commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE, Alliance.Blue),
             new ShootBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake));
+
+        commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT, Alliance.Red),
+            new FastDepotRed(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake));
+        commandMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT, Alliance.Blue),
+            new FastDepotBlue(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake));
+
 
         /*
         //shoot-pickup
@@ -164,6 +172,8 @@ private void populateDescriptionMap() {
             "shoot from the middle"); 
         descriptionMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE),
             "shoot from the outpost"); 
+        descriptionMap.put(new AutoEvent(AutoAction.FAST_SHOOT, FieldLocation.FAST_DEPOT),
+            "depot and shoot from the depot"); 
     /*
         descriptionMap.put(new AutoEvent(AutoAction.SHOOT_PICKUP, FieldLocation.DEPOT_SIDE),
             "shoot and pickup from the depot");
@@ -204,7 +214,7 @@ public String getCommandDescription() {
             return "NO AUTO";
         } else {
             String commandDescription = descriptionMap.get(event.withoutColor());
-            return event.getAction() + " at " + event.getLocation() + " → " + commandDescription + ".";
+            return event.getActionName() + " at " + event.getLocation() + " -> " + commandDescription + ".";
         }
     }
 

src/main/java/frc/robot/autochooser/AutoEvent.java

@@ -40,6 +40,9 @@ public AutoEvent(AutoAction action, FieldLocation location, Alliance color) {
   public AutoAction getAction() {
     return action;
   }
+  public String getActionName() {
+    return action.name();
+  }
 
   public FieldLocation getLocation() {
     return location;

src/main/java/frc/robot/autochooser/FieldLocation.java

@@ -17,7 +17,8 @@ public enum FieldLocation {
   INVALID(-1, -1, -1, "INVALID"),
   DEPOT_SIDE(3.599, 7.33, 180, "Depot side - Driver Station LEFT"),  //robot is 24 cm away from the wall
   MID(3.599, 4.029, 180, "Middle"),
-  OUTPOST_SIDE(3.599, 0.67, 180, "Outpost side - Driver Station RIGHT"); //robot is 24 cm away from the wall
+  OUTPOST_SIDE(3.599, 0.67, 180, "Outpost side - Driver Station RIGHT"), //robot is 24 cm away from the wall
+  FAST_DEPOT(3.596, 5.06, 180, "Depot side - for the fast auto"); //robot's back left corner is next to hopper corner
 
   private static final double RED_X_POS = 9.338; // meters
   public static final double HEIGHT_OF_FIELD = 8.043;

src/main/java/frc/robot/commands/auto/AutoShoot.java

@@ -1,6 +1,7 @@
 package frc.robot.commands.auto;
 
 import frc.robot.commands.feeder.SpinFeeder;
+import frc.robot.commands.hopper.AutoSpinHopper;
 import frc.robot.commands.hopper.SpinHopper;
 import frc.robot.subsystems.FeederSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
@@ -10,7 +11,7 @@
 public class AutoShoot extends LoggableRaceCommandGroup{
     public AutoShoot(HopperSubsystem hopperSubsystem, FeederSubsystem feederSubsystem, double time){
         super(
-            new SpinHopper(hopperSubsystem),
+            new AutoSpinHopper(hopperSubsystem),
             new SpinFeeder(feederSubsystem),
             new LoggableWaitCommand(time)
         );

src/main/java/frc/robot/commands/auto/newauto/FastDepotBlue.java

@@ -0,0 +1,47 @@
+package frc.robot.commands.auto.newauto;
+
+import choreo.auto.AutoFactory;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.auto.AutoReset;
+import frc.robot.commands.auto.AutoShoot;
+import frc.robot.commands.drive.DriveSwerve;
+import frc.robot.commands.intakeDeployment.ToggleDeployment;
+import frc.robot.commands.shooter.SetShootingState;
+import frc.robot.constants.enums.DriveDirection;
+import frc.robot.constants.enums.ShootingState;
+import frc.robot.constants.enums.ShootingState.ShootState;
+import frc.robot.subsystems.*;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommandWrapper;
+import frc.robot.utils.logging.commands.LoggableParallelCommandGroup;
+import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup;
+import frc.robot.utils.logging.commands.LoggableWaitCommand;
+
+public class FastDepotBlue extends LoggableSequentialCommandGroup {
+    public FastDepotBlue(
+            SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate,
+            HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler,
+            ControllerSubsystem controller, IntakeDeployerSubsystem intake) {
+        super(  
+            new AutoReset(shootstate, turret, angler),
+            new SetShootingState(shootstate, ShootState.AUTO_AIM),
+            new LoggableParallelCommandGroup(
+                LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")),
+                LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast"))
+            ),
+            new AutoShoot(hopper, feeder, 1),
+            new ToggleDeployment(intake, controller),
+            new LoggableParallelCommandGroup(
+                new AutoShoot(hopper, feeder, 20),
+                new LoggableSequentialCommandGroup(
+                    new DriveSwerve(drivetrain, DriveDirection.FORWARD, 6, 0.2),
+                    new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 0.5, 0.5),
+                    new DriveSwerve(drivetrain, DriveDirection.FORWARD, 7, 0.2),
+                    new ToggleDeployment(intake, controller)
+                )
+            )
+
+        );
+    }
+}

src/main/java/frc/robot/commands/auto/newauto/FastDepotRed.java

@@ -0,0 +1,50 @@
+package frc.robot.commands.auto.newauto;
+
+import choreo.auto.AutoFactory;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.auto.AutoReset;
+import frc.robot.commands.auto.AutoShoot;
+import frc.robot.commands.drive.DriveSwerve;
+import frc.robot.commands.intakeDeployment.ToggleDeployment;
+import frc.robot.commands.shooter.SetShootingState;
+import frc.robot.constants.enums.DriveDirection;
+import frc.robot.constants.enums.ShootingState;
+import frc.robot.constants.enums.ShootingState.ShootState;
+import frc.robot.subsystems.*;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommandWrapper;
+import frc.robot.utils.logging.commands.LoggableParallelCommandGroup;
+import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup;
+import frc.robot.utils.logging.commands.LoggableWaitCommand;
+
+public class FastDepotRed extends LoggableSequentialCommandGroup {
+    public FastDepotRed(
+            SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate,
+            HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler,
+            ControllerSubsystem controller, IntakeDeployerSubsystem intake) {
+        super(  
+
+            new LoggableParallelCommandGroup(
+                new LoggableSequentialCommandGroup(
+                new AutoReset(shootstate, turret, angler),
+                new SetShootingState(shootstate, ShootState.AUTO_AIM)
+                ),
+                LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Fast")),
+                LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Fast"))
+            ),
+            new AutoShoot(hopper, feeder, 1),
+            new ToggleDeployment(intake, controller),
+            new LoggableParallelCommandGroup(
+                new AutoShoot(hopper, feeder, 20),
+                new LoggableSequentialCommandGroup(
+                    new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 6, 0.2),
+                    new DriveSwerve(drivetrain, DriveDirection.FORWARD, 0.5, 0.5),
+                    new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 7, 0.2),
+                    new ToggleDeployment(intake, controller)
+                )
+            )
+
+        );
+    }
+}

src/main/java/frc/robot/commands/hopper/AutoSpinHopper.java

@@ -0,0 +1,47 @@
+package frc.robot.commands.hopper;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class AutoSpinHopper extends LoggableCommand {
+
+    public final HopperSubsystem subsystem;
+    public final Timer timer;
+
+
+    public AutoSpinHopper(HopperSubsystem subsystem){
+        timer = new Timer();
+        this.subsystem = subsystem;
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void initialize() {
+      timer.restart();
+    }
+
+    @Override
+    public void execute() {
+            subsystem.setSpeed(Constants.HOPPER_AUTO_SPEED);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        subsystem.stopMotors();
+    }
+
+    @Override
+    public boolean isFinished() {
+        if (timer.hasElapsed(Constants.HOPPER_TIMEOUT)){
+            return true;
+        }
+        else{
+            return false;
+        }
+
+    }
+
+
+}

src/main/java/frc/robot/commands/lightStrip/SetLed.java

@@ -57,6 +57,9 @@ public void execute() {
             case SHOOTING_HUB:
                 lightStrip.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE);
                 break;
+            case AUTO_AIM:
+                lightStrip.setPattern(BlinkinPattern.RAINBOW_LAVA_PALETTE);
+                break;
             case SHUTTLING:
                 lightStrip.setPattern(BlinkinPattern.GREEN);
                 break;

src/main/java/frc/robot/constants/GameConstants.java

@@ -36,7 +36,7 @@ public enum Mode {
     public static final boolean ENABLE_LOGGING = true;
 
     //Debugs
-    public static final boolean DEBUG = false;
+    public static final boolean DEBUG = true;
     public static final boolean ARM_DEBUG = true;
 
     //Joystick
@@ -47,7 +47,8 @@ public enum Mode {
     //Speeds
     public static final double INTAKE_SPEED = -0.7;
     public static final double INTAKE_REVERSE_SPEED = 0.5;
-    public static final double HOPPER_SPEED = 0.75;
+    public static final double HOPPER_SPEED = 0.6;
+    public static final double HOPPER_AUTO_SPEED = 0.35;
     public static final double CLIMBER_SPEED_UP = 0.1;
     public static final double CLIMBER_SPEED_DOWN = -0.1;
     public static final double FEEDER_SPEED = 1;
@@ -101,12 +102,12 @@ public enum Mode {
     public static final double ANGLER_FF = 0.0;
     public static final double ANGLER_HOME_ROTATIONS = 0.0;
     public static final double ANGLER_ENCODER_LOW = 0; //Lowest encoder position of Angler
-    public static final double ANGLER_ENCODER_HIGH = 272; //Highest encoder position of Angler
-    public static final double ANGLER_ANGLE_LOW = 16; //Lowest angle position of Angler
-    public static final double ANGLER_ANGLE_HIGH = 37; //Highest angle position of Angler
+    public static final double ANGLER_ENCODER_HIGH = 265; //Highest encoder position of Angler
+    public static final double ANGLER_ANGLE_LOW = 17; //Lowest angle position of Angler
+    public static final double ANGLER_ANGLE_HIGH = 38; //Highest angle position of Angler
     public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState
     public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState
-    public static final double ANGLER_LIMIT_SPEED = 0.3;
+    public static final double ANGLER_LIMIT_SPEED = 0.6;
 
 
  // turret (pan angle) PID
@@ -121,8 +122,8 @@ public enum Mode {
     public static final double TURRET_ENCODER_MIN = 0; //Lowest encoder position of Turret
     public static final double TURRET_ENCODER_MAX = 77; //Highest encoder position of Turret
     public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward
-    public static final double TURRET_MIN_ANGLE = -92;
-    public static final double TURRET_MAX_ANGLE = 92;
+    public static final double TURRET_MIN_ANGLE = -96;
+    public static final double TURRET_MAX_ANGLE = 96;
     public static final double TURRET_LIMIT_SPEED = 0.2;
     public static final double TURRET_OUT_OF_RANGE_FLOP_RPM = -1500.0;
     public static final double TURRET_PID_DISTANCE_THRESHOLD = 10;

src/main/java/frc/robot/constants/enums/ShootingState.java

@@ -8,7 +8,8 @@ public enum ShootState {
         FIXED,          // Shoot from a known location
         FIXED_2,        // Shoot from another known location
         SHOOTING_HUB,   // Auto-aim into the hub
-        SHUTTLING       // Auto-aim into alliance zone
+        SHUTTLING,     // Auto-aim into alliance zone
+        AUTO_AIM        //auto aim without hopper and feeder
 
     }