Minor Updates to code

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

@@ -2,16 +2,22 @@
 
 import org.photonvision.PhotonCamera;
 
+import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import frc.lib.zylve.Controller;
+import frc.robot.auto.AmpScore;
 import frc.robot.auto.FollowAprilTag;
 import frc.robot.auto.PhotonRunnable;
 import frc.robot.auto.PoseEstimatorSubsystem;
+import frc.robot.auto.ShootNote;
 import frc.robot.constants.OperatorConstants;
+import frc.robot.elevator.Elevator;
+import frc.robot.intake.Intake;
+import frc.robot.shooter.Shooter;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc.robot.swerve.SwerveDrive;
@@ -21,11 +27,19 @@ public class RobotContainer {
 
     private final SwerveDrive swerveDrive = new SwerveDrive();
 
+    private final Elevator elevator = new Elevator();
+    private final Intake intake = new Intake();
+    private final Shooter shooter = new Shooter();
+
+
     private final PhotonCamera photonCamera  = new PhotonCamera("Limelight");
     private final PhotonRunnable photonRunnable = new PhotonRunnable(photonCamera);
     private final PoseEstimatorSubsystem poseEstimator = new PoseEstimatorSubsystem(swerveDrive::getRotation2d, swerveDrive::getModulePositions, photonRunnable);
     private final FollowAprilTag followAprilTag = new FollowAprilTag(swerveDrive, photonCamera, poseEstimator::getCurrentPose);
 
+    private final AmpScore AmpScore = new AmpScore(elevator, intake);
+    private final ShootNote shootnotefeed = new ShootNote(shooter, intake);
+
     // @SuppressWarnings("unused")
     // private final Elevator elevator = new Elevator();
 
@@ -44,6 +58,10 @@ public RobotContainer() {
         autoSelector.addOption("Right Auto", new PathPlannerAuto("Right Auto"));
         autoSelector.setDefaultOption("Middle Auto", new PathPlannerAuto("Middle Auto"));
 
+        NamedCommands.registerCommand("Amp Score", AmpScore);
+        NamedCommands.registerCommand("Shoot Note", shootnotefeed);
+
+
         Shuffleboard.getTab("auto").add(autoSelector);
 
         swerveDrive.setDefaultCommand(

src/main/java/frc/robot/auto/AmpScore.java

@@ -0,0 +1,18 @@
+package frc.robot.auto;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.elevator.Elevator;
+import frc.robot.elevator.commands.ElevatorDown;
+import frc.robot.elevator.commands.ElevatorUp;
+import frc.robot.intake.Intake;
+import frc.robot.intake.commands.IntakeAmpScore;
+
+public class AmpScore extends SequentialCommandGroup {
+    public AmpScore(Elevator elevator, Intake intake) {
+        addCommands(
+            new ElevatorUp(elevator),
+            new IntakeAmpScore(intake),
+            new ElevatorDown(elevator)
+        );
+    }
+}

src/main/java/frc/robot/auto/ShootNote.java

@@ -0,0 +1,19 @@
+package frc.robot.auto;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.intake.Intake;
+
+import frc.robot.intake.commands.IntakeShooterFeed;
+import frc.robot.shooter.Shooter;
+import frc.robot.shooter.commands.ActivateShooter;
+
+public class ShootNote extends ParallelCommandGroup {
+    public ShootNote(Shooter shooter, Intake intake) {
+        addCommands(
+            new ActivateShooter(shooter),
+
+            new IntakeShooterFeed(intake)
+
+        );
+    }
+}

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

@@ -13,7 +13,7 @@ public class IntakeConstants {
 
     public static final double SHOOTER_SPEED = 1;
     public static final double SHOOTER_FEED_DURATION = 0.25;
+    public static final double SHOOTER_FEED_DELAY = 2;
 
     public static final double P = 0.1;
-}
-
+}

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

@@ -6,26 +6,26 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ShooterConstants;
 
-public class Shooter extends SubsystemBase { //similar to making a scratch sprite
-    private final CANSparkMax shooterMotor;
+public class Shooter extends SubsystemBase {
+    private final CANSparkMax motor;
 
     public Shooter() {
-        shooterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless);
+        motor = new CANSparkMax(ShooterConstants.SHOOTER_ID, MotorType.kBrushless);
     }
 
     @Override
     public void periodic() {
     }
 
-    public void shooterMotorOn() {
-        shooterMotor.set(ShooterConstants.SHOOTER_ON);
+    public void shooterRun() {
+        motor.set(ShooterConstants.SHOOTER_ON);
     }
 
-    public void shooterMotorOff() {
-        shooterMotor.set(ShooterConstants.SHOOTER_OFF);
+    public void shooterOff() {
+        motor.set(ShooterConstants.SHOOTER_OFF);
     }
 
     public void inputShooterMotor(double inputShooterMotorValue) {
-        shooterMotor.set(inputShooterMotorValue);
+        motor.set(inputShooterMotorValue);
     }
 }

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

@@ -0,0 +1,36 @@
+package frc.robot.shooter.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.ShooterConstants;
+import frc.robot.shooter.Shooter;
+
+public class ActivateShooter extends Command {
+    private final Shooter shooter;
+    private double startTime;
+
+    public ActivateShooter(Shooter shooter) {
+        this.shooter = shooter;
+        addRequirements(shooter);
+    } 
+
+    @Override
+    public void initialize() {
+        startTime = Timer.getFPGATimestamp();
+        shooter.shooterRun();
+    }
+
+    @Override
+    public void execute() {
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        shooter.shooterOff();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Timer.getFPGATimestamp() - startTime > ShooterConstants.DELAY;
+    }
+}