Added Controller subsystem

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

@@ -13,10 +13,13 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.AddTunableApriltagReading;
+import frc.robot.commands.ShootButton;
 import frc.robot.commands.AddApriltagReading;
 import frc.robot.commands.AddGarbageReading;
 import frc.robot.commands.climber.ClimberDown;
 import frc.robot.commands.climber.ClimberUp;
+import frc.robot.commands.feeder.DefaultSpinFeeder;
+import frc.robot.commands.hopper.DefaultSpinHopper;
 import frc.robot.commands.hopper.SpinHopper;
 import frc.robot.commands.drive.DriveDirectionTime;
 import frc.robot.commands.feeder.SpinFeeder;
@@ -30,9 +33,12 @@
 import frc.robot.commands.sequences.IntakeUpSequence;
 import frc.robot.autochooser.AutoChooser;
 import frc.robot.commands.angler.AimAngler;
+import frc.robot.commands.angler.DefaultAnglerControl;
 import frc.robot.commands.angler.RunAnglerToReverseLimit;
+import frc.robot.commands.shooter.DefaultShooterControl;
 import frc.robot.commands.auto.ExampleAuto;
 import frc.robot.commands.shooter.SetShootingState;
+import frc.robot.commands.turret.DefaultTurretControl;
 import frc.robot.commands.turret.RunTurretToFwdLimit;
 import frc.robot.commands.turret.RunTurretToRevLimit;
 import frc.robot.commands.turret.SetTurretAngle;
@@ -43,6 +49,7 @@
 import frc.robot.constants.enums.ShootingState.ShootState;
 import frc.robot.subsystems.AnglerSubsystem;
 import frc.robot.subsystems.ApriltagSubsystem;
+import frc.robot.subsystems.ControllerSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeDeployerSubsystem;
 import frc.robot.subsystems.FeederSubsystem;
@@ -94,9 +101,11 @@ public class RobotContainer {
         private final FeederSubsystem feederSubsystem;
         private final ApriltagSubsystem apriltagSubsystem;
         private final ShooterSubsystem shooterSubsystem;
+        private final ControllerSubsystem controllerSubsystem;
         private RobotVisualizer robotVisualizer = null;
         private final HopperSubsystem hopperSubsystem;
-    private final TurretSubsystem turretSubsystem;
+
+        private final TurretSubsystem turretSubsystem;
         private final IntakeDeployerSubsystem intakeDeployer;
         private SwerveSubsystem drivebase = null;
         private GyroSubsystem gyroSubsystem = null;
@@ -142,7 +151,9 @@ public RobotContainer() {
                                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(
                                                 new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null;
                             apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase) : null;
-                        }
+                            controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
+
+            }
                         case REPLAY -> {
                                 // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo());
                                 // tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo());
@@ -159,6 +170,8 @@ public RobotContainer() {
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null;
                 apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null;
+                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
+
             }
             case SIM -> {
                 robotVisualizer = new RobotVisualizer();
@@ -177,6 +190,7 @@ public RobotContainer() {
                                                 IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null;
+                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
                 apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null;
             }
 
@@ -275,9 +289,10 @@ private void configureBindings() {
                 controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2));
                 controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB));
                 controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING));
-                steerJoystick.trigger().whileTrue((new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)));
                 driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED)));
-
+                if (controllerSubsystem != null) {
+                        steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem));
+                }
 
                 // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
                 // new Trigger(m_exampleSubsystem::exampleCondition)
@@ -290,28 +305,32 @@ private void configureBindings() {
                 // TODO: Clean this up a little - create command in method and only create the
                 // one actually needed
 
-        //example default command for angler- disabled for now
-        if (false) {
-            new AimAngler(
-                    anglerSubsystem,
-                    () -> drivebase != null ? drivebase.getPose() : null,
-                    shootState);
-        }
+                // example default command for angler- disabled for now
+                if (false) {
+                        new AimAngler(
+                                        anglerSubsystem,
+                                        () -> drivebase != null ? drivebase.getPose() : null,
+                                        shootState);
+                }
 
-                {
-                        if (!Constants.TESTBED) {
-                                SwerveInputStream driveAngularVelocity = SwerveInputStream
-                                                .of(drivebase.getSwerveDrive(),
-                                                                () -> driveJoystick.getY() * -1,
-                                                                () -> driveJoystick.getX() * -1)
-                                                .withControllerRotationAxis(steerJoystick::getX)
-                                                .deadband(Constants.DEADBAND)
-                                                .scaleTranslation(0.8)
-                                                .allianceRelativeControl(true);
-                                Command driveFieldOrientedAnglularVelocity = drivebase
-                                                .driveFieldOriented(driveAngularVelocity);
-                                drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
-                        }
+            if (controllerSubsystem != null) {
+                anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem));
+                shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem));
+                turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem));
+                hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem));
+                feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem));
+            }
+
+            if (!Constants.TESTBED) {
+                        SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
+                                        () -> driveJoystick.getY() * -1,
+                                        () -> driveJoystick.getX() * -1)
+                                        .withControllerRotationAxis(steerJoystick::getX)
+                                        .deadband(Constants.DEADBAND)
+                                        .scaleTranslation(0.8)
+                                        .allianceRelativeControl(true);
+                        Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
+                        drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
                 }
         }
 

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

@@ -0,0 +1,31 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.ControllerSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class ShootButton extends LoggableCommand{
+
+   private final ControllerSubsystem controllerSubsystem;
+
+    public ShootButton(ControllerSubsystem controllerSubsystem) {
+        this.controllerSubsystem = controllerSubsystem;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        controllerSubsystem.setDriverActivatedShooting(false); 
+    }
+
+    @Override
+    public void initialize() {
+        controllerSubsystem.setDriverActivatedShooting(true);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+
+
+}

src/main/java/frc/robot/commands/angler/DefaultAnglerControl.java

@@ -0,0 +1,27 @@
+package frc.robot.commands.angler;
+
+import frc.robot.subsystems.AnglerSubsystem;
+import frc.robot.subsystems.ControllerSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class DefaultAnglerControl extends LoggableCommand {
+
+    private final AnglerSubsystem anglerSubsystem;
+    private final ControllerSubsystem controllerSubsystem;
+
+    public DefaultAnglerControl(AnglerSubsystem anglerSubsystem, ControllerSubsystem controllerSubsystem) {
+        this.anglerSubsystem = anglerSubsystem;
+        this.controllerSubsystem = controllerSubsystem;
+        addRequirements(anglerSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        anglerSubsystem.setAngle(controllerSubsystem.getTargetAnglerAngleDegrees());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/frc/robot/commands/feeder/DefaultSpinFeeder.java

@@ -0,0 +1,46 @@
+package frc.robot.commands.feeder;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.ControllerSubsystem;
+import frc.robot.subsystems.FeederSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class DefaultSpinFeeder extends LoggableCommand {
+
+    private final FeederSubsystem subsystem;
+    private final Timer timer;
+    private final ControllerSubsystem controllerSubsystem;
+
+    public DefaultSpinFeeder(FeederSubsystem subsystem, ControllerSubsystem controllerSubsystem) {
+        this.subsystem = subsystem;
+        this.controllerSubsystem = controllerSubsystem;
+        timer = new Timer();
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void initialize() {
+        timer.restart();
+    }  
+
+    @Override
+    public void execute() {
+        if (controllerSubsystem.shouldFeederSpin()) {
+            subsystem.setSpeed(Constants.FEEDER_SPEED);
+        } else {
+            subsystem.stopMotors();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        subsystem.stopMotors();
+    }
+
+}

src/main/java/frc/robot/commands/feeder/SpinFeeder.java

@@ -3,11 +3,9 @@
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.FeederSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.utils.logging.TimeoutLogger;
 import frc.robot.utils.logging.commands.LoggableCommand;
 
-public class SpinFeeder  extends LoggableCommand {
+public class SpinFeeder extends LoggableCommand {
 
     private final FeederSubsystem subsystem;
     private final Timer timer;

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

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

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

@@ -5,11 +5,11 @@
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.utils.logging.commands.LoggableCommand;
 
-public class SpinHopper extends LoggableCommand{
+public class SpinHopper extends LoggableCommand {
 
     public final HopperSubsystem subsystem;
     public final Timer timer;
-    
+
 
     public SpinHopper(HopperSubsystem subsystem){
         timer = new Timer();
@@ -43,5 +43,5 @@ public boolean isFinished() {
 
     }
 
-    
+
 }

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

@@ -0,0 +1,32 @@
+package frc.robot.commands.shooter;
+
+import frc.robot.subsystems.ControllerSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class DefaultShooterControl extends LoggableCommand {
+
+    private final ShooterSubsystem shooterSubsystem;
+    private final ControllerSubsystem controllerSubsystem;
+
+    public DefaultShooterControl(ShooterSubsystem shooterSubsystem, ControllerSubsystem controllerSubsystem) {
+        this.shooterSubsystem = shooterSubsystem;
+        this.controllerSubsystem = controllerSubsystem;
+        addRequirements(shooterSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        double targetShooterVelocity = controllerSubsystem.getTargetShooterVelocityRpm();
+        if (targetShooterVelocity > 0.0) {
+            shooterSubsystem.setPidVelocity(targetShooterVelocity);
+        } else {
+            shooterSubsystem.stopMotors();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/frc/robot/commands/turret/DefaultTurretControl.java

@@ -0,0 +1,27 @@
+package frc.robot.commands.turret;
+
+import frc.robot.subsystems.ControllerSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class DefaultTurretControl extends LoggableCommand {
+
+    private final TurretSubsystem turretSubsystem;
+    private final ControllerSubsystem controllerSubsystem;
+
+    public DefaultTurretControl(TurretSubsystem turretSubsystem, ControllerSubsystem controllerSubsystem) {
+        this.turretSubsystem = turretSubsystem;
+        this.controllerSubsystem = controllerSubsystem;
+        addRequirements(turretSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        turretSubsystem.setAngle(controllerSubsystem.getTargetTurretAngleDegrees());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

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

@@ -102,6 +102,7 @@ public enum Mode {
     public static final double ANGLER_ANGLE_LOW = 0; //Lowest angle position of Angler
     public static final double ANGLER_ANGLE_HIGH = 45; //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.2;