Added Aim and Shoot Command

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

@@ -20,7 +20,10 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -49,6 +52,7 @@
 import frc.robot.subsystems.shooter.HoodIOServo;
 import frc.robot.subsystems.shooter.HoodIOSim;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.shooter.ShooterIO;
 import frc.robot.subsystems.shooter.ShooterIOSim;
 import frc.robot.subsystems.shooter.ShooterIOTalonFX;
@@ -315,11 +319,27 @@ private void configureBindings() {
             intake_::isIntakeStowed
         ));
 
+
+
         // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it.
         gamepad_.leftTrigger().whileTrue(intake_.intakeSequence());
 
         // While the right trigger is held, we will shoot into the hub.
-        gamepad_.rightTrigger().whileTrue(shooter_.shootCmd(hopper_));
+        gamepad_.rightTrigger().whileTrue(Commands.parallel(DriveCommands.joystickDriveAtAngle(drivebase_,
+                () -> 0,
+                () -> 0, 
+                () -> {
+                    Translation2d hub =
+                            DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
+                            ? ShooterConstants.Positions.blueHubPose
+                            : ShooterConstants.Positions.redHubPose;
+
+                    var hubTranslation = hub.minus(drivebase_.getPose().getTranslation());
+                    var rotation = new Rotation2d(hubTranslation.getX(), hubTranslation.getY());
+
+                    return rotation;
+                }),
+            shooter_.shooterSetpointSupplier(() -> drivebase_.getPose(), hopper_)));
 
         // When the hopper isnt shooting, set it to run its idle velocity.
         hopper_.setDefaultCommand(hopper_.idleScrambler());

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

@@ -0,0 +1,28 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.commands.drive.DriveCommands;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.hopper.Hopper;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.shooter.Shooter;
+
+public class AutoCommands {
+
+
+    public static Command NeutralZoneTrenchToTrench(Drive drive, IntakeSubsystem intake, Hopper hopper, Shooter shooter){
+        return Commands.sequence(
+            Commands.runOnce(() -> drive.resetGyroCmd()),
+
+            DriveCommands.initialFollowPathCommand(drive,"TopToBottom Trench").deadlineFor(intake.intakeSequence()),
+
+            shooter.shootCmd(hopper).withTimeout(3.8),
+
+            DriveCommands.followPathCommand("BottomToTop").deadlineFor(intake.intakeSequence()),
+
+            shooter.shootCmd(hopper).withTimeout(2.5)
+        );
+    }
+
+}

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -15,12 +15,15 @@
 import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -29,6 +32,7 @@
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.Mode;
 import frc.robot.subsystems.hopper.Hopper;
+import frc.robot.subsystems.shooter.ShooterConstants.Positions.HubDistance;
 import frc.robot.util.MapleSimUtil;
 import frc.robot.util.Mechanism3d;
 
@@ -99,8 +103,7 @@ public boolean isShooterReady() {
     public Current getShooterCurrent() {
         return (shooterInputs.shooter1Current)
             .plus(shooterInputs.shooter2Current)
-            .plus(shooterInputs.shooter3Current)
-            .plus(shooterInputs.shooter4Current);
+            .plus(shooterInputs.shooter3Current);
     }
 
     private void setHoodAngle(Angle pos) {
@@ -142,7 +145,25 @@ public Command awaitShooting(Supplier<Pose2d> robotPose) {
                 return Degrees.zero();
             }
 
-            return Degrees.of(45); // TODO: replace this with whatever determines shooter angle
+            Translation2d hubTranslation =
+                DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
+                ? ShooterConstants.Positions.blueHubPose
+                : ShooterConstants.Positions.redHubPose;
+
+            Distance distanceToHub = Meters.of(pose.getTranslation().getDistance(hubTranslation));
+
+            switch(HubDistance.fromDistance(distanceToHub)) {
+                case LOW:
+                    return Degrees.of(ShooterConstants.Positions.hoodLOW);
+                case MEDIUM:
+                    return Degrees.of(ShooterConstants.Positions.hoodMEDIUM);
+                case HIGH:
+                    return Degrees.of(ShooterConstants.Positions.hoodHIGH);
+                default:  
+                    return Degrees.of(ShooterConstants.Positions.hoodLOW);  
+
+            }
+
         });
     }
 
@@ -168,6 +189,54 @@ public Command hoodToPosCmd(Angle pos) {
         return runOnce(() -> setHoodAngle(pos)).withName("Set Hood Position");
     }
 
+    /**
+     * Calculates Velocity and Hood Angle based on distance and Shoots
+     * 
+     * 
+     */
+    public Command shooterSetpointSupplier(Supplier<Pose2d> pose, Hopper hopper) {
+
+        return Commands.parallel(
+                defer(() -> {
+
+                    // constructing
+                    Translation2d hubTranslation =
+                        DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
+                        ? ShooterConstants.Positions.blueHubPose
+                        : ShooterConstants.Positions.redHubPose;
+
+                    ShooterConstants.Positions.initMap();
+
+                    return runDynamicSetpoints(() -> {
+                        Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation));
+                        var vel = ShooterConstants.Positions.distMap.get(distanceToHub.baseUnitMagnitude());
+
+                        return RotationsPerSecond.of(vel);
+                    },
+
+                    () -> {
+                        // periodic
+                        Distance distanceToHub = Meters.of(pose.get().getTranslation().getDistance(hubTranslation));
+
+                        switch(HubDistance.fromDistance(distanceToHub)) {
+                            case LOW:
+                                return Degrees.of(ShooterConstants.Positions.hoodLOW);
+
+                            case MEDIUM:
+                                return Degrees.of(ShooterConstants.Positions.hoodMEDIUM);
+
+                            case HIGH:
+                                return Degrees.of(ShooterConstants.Positions.hoodHIGH);
+
+                            default: 
+                                return Degrees.of(ShooterConstants.Positions.hoodMEDIUM);
+                        }   
+                });
+            }),
+            hopper.forwardFeed()
+        );
+    }
+
     /**
      * Runs supplied setpoints until the command ends, then stops.
      * @param vel

src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java

@@ -4,6 +4,8 @@
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
 
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Time;
@@ -24,31 +26,119 @@ public class ShooterConstants {
     public static final Distance allowedTrenchDistance = Meters.of(1.0);
 
     public class PID {
-        // shooter
-        public static final double shooterkP = 0.0;
-        public static final double shooterkI = 0.0;
-        public static final double shooterkD = 0.0;
-        public static final double shooterkV = 0.117;
-        public static final double shooterkA = 0.0;
-        public static final double shooterkG = 0.0;
-        public static final double shooterkS = 0.0;
-    }
+            // shooter
+            public static final double shooterkP = 0.5; 
+            public static final double shooterkI = 0.0;
+            public static final double shooterkD = 0.0;
+            public static final double shooterkV = 0.0;
+            public static final double shooterkA = 0.0;
+            public static final double shooterkG = 0.0;
+            public static final double shooterkS = 0.0;
 
-    public class MotionMagic {
+            // hood
+            public static final double hoodkP = 0.0; 
+            public static final double hoodkI = 0.0;
+            public static final double hoodkD = 0.0;
+            public static final double hoodkV = 0.0;
+            public static final double hoodkA = 0.0;
+            public static final double hoodkG = 0.0;
+            public static final double hoodkS = 0.0;
+        }
 
-        // shooter
-        public static final double shooterkMaxVelocity = 1000.0;
-        public static final double shooterkMaxAcceleration = 3000.0;
-        public static final double shooterkJerk = 0.0;
-    }
+        public class MotionMagic {
+
+            // shooter
+            public static final double shooterkMaxVelocity = 1000.0;
+            public static final double shooterkMaxAcceleration = 3000.0;
+            public static final double shooterkJerk = 0.0;
+
+            // hood
+            public static final double hoodkMaxVelocity = 0.0;
+            public static final double hoodkMaxAcceleration = 300.0;
+            public static final double hoodkJerk = 0.0;
+        }
+
+        public class SoftwareLimits {
+            public static final double hoodMaxAngle = 0.0;
+            public static final double hoodMinAngle = 0.0;
+        }
+
+        public class Positions {
+            public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536);
+            public static final Translation2d redHubPose = new Translation2d(11.938,4.034536);
+
+            // Hood Setpoints
+            public static final double hoodLOW = 0;
+            public static final double hoodMEDIUM = 1;
+            public static final double hoodHIGH = 2;
+
+            // Tested Distances 
+            public static final double dist1 = 1;
+            public static final double dist2 = 2;
+            public static final double dist3 = 3;
+
+            public static final double dist4 = 4;
+            public static final double dist5 = 5;
+            public static final double dist6 = 6;
+
+            public static final double dist7 = 7;
+            public static final double dist8 = 8;
+            public static final double dist9 = 9;
 
-    public class SoftwareLimits {
-        public static final double hoodMaxAngle = 0.0;
-        public static final double hoodMinAngle = 0.0;
+            // Tested Velocities
+            public static final double low1 = 10;
+            public static final double low2 = 20;
+            public static final double low3 = 30;
+
+            public static final double med1 = 40;
+            public static final double med2 = 50;
+            public static final double med3 = 60;
+
+            public static final double high1 = 70;
+            public static final double high2 = 80;
+            public static final double high3 = 90;
+
+            public static final InterpolatingDoubleTreeMap distMap = new InterpolatingDoubleTreeMap();
+
+            public static void initMap() {
+                distMap.put(dist1, low1);
+                distMap.put(dist2, low2);
+                distMap.put(dist3, low3);
+                distMap.put(dist4, med1);
+                distMap.put(dist5, med2);
+                distMap.put(dist6, med3);
+                distMap.put(dist7, high1);
+                distMap.put(dist8, high2);
+                distMap.put(dist9, high3);
+            }
+
+            public enum HubDistance {
+                LOW(Meters.of(2)), // 0-2 m
+                MEDIUM(Meters.of(3)), //  in
+                HIGH(Meters.of(4)); // 96+ in
+
+                private final Distance maxDistance;
+
+                private HubDistance(Distance maxDistance) {
+                    this.maxDistance = maxDistance;
+                }
+
+                public Distance maxDistance() { return maxDistance; }
+
+                public static HubDistance fromDistance(Distance pos) {
+                    for(HubDistance vol : values()) {
+                        if (pos.baseUnitMagnitude() <= vol.maxDistance().baseUnitMagnitude()) {
+                            return vol;
+                        } 
+                    }
+                    return HubDistance.HIGH;
+                }
+            }
     }
 
     public class HoodPWMs {
-        public static final int hoodLeftPWMPort = 2;
-        public static final int hoodRightPWMPort = 0;
+            public static final int hoodLeftPWMPort = 2;
+            public static final int hoodRightPWMPort = 0;
     }
+
 }

src/main/java/frc/robot/subsystems/shooter/ShooterIO.java

@@ -29,11 +29,6 @@ public static class ShooterIOInputs {
         public Current shooter3Current = Amps.zero();
         public AngularVelocity shooter3Velocity = RadiansPerSecond.zero();
 
-        public boolean shooter4Ready = false;
-        public Voltage shooter4Voltage = Volts.zero();
-        public Current shooter4Current = Amps.zero();
-        public AngularVelocity shooter4Velocity = RadiansPerSecond.zero();
-
         public AngularVelocity wheelVelocity = RadiansPerSecond.zero();
     }