instant commands for testing

src/main/java/frc4388/robot/RobotContainer.java

@@ -33,6 +33,8 @@ import frc4388.robot.constants.Constants.SimConstants.Mode;
 import frc4388.robot.constants.FieldConstants;
 // Subsystems
 import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.intake.Intake;
+import frc4388.robot.subsystems.shooter.Shooter;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.DeferredBlock;
@@ -58,9 +60,10 @@ public class RobotContainer {
     /* Subsystems */
     public final LED m_robotLED = new LED();
     //Testing of Colors
-    
     public final Vision m_vision = new Vision();
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
+    public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
+    public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO);
     private Boolean operatorManualMode = false;
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
@@ -86,13 +89,21 @@ public class RobotContainer {
     private SendableChooser<String> autoChooser;
     private Command autoCommand;
 
+    // private Command RobotShoot = new SequentialCommandGroup(
+    //     new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
+    //     new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED),
+    //     new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
+    //     new WaitCommand(5),
+    //     new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED),
+    //     new InstantCommand(() -> System.out.println(m_robotLED.getMode()))
+    // );
+
     private Command RobotShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
+        new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter)
-        new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED),
+    );
-        new InstantCommand(() -> System.out.println(m_robotLED.getMode())),
+
-        new WaitCommand(5),
+    private Command RobotIntake = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED),
+        new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake)
-        new InstantCommand(() -> System.out.println(m_robotLED.getMode()))
     );
 
     public RobotContainer() {
@@ -144,7 +155,12 @@ public class RobotContainer {
         //     .onTrue(new RotTo45(m_robotSwerveDrive));
 
         new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
-            .onTrue(RobotShoot);
+            .onTrue(RobotShoot)
+            .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter));
+
+        new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
+            .onTrue(RobotIntake)
+            .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter));
             
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); 
@@ -157,11 +173,11 @@ public class RobotContainer {
 
        
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
-            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
+        //     .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
-            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
+        //     .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
 
         // IF the driver is holding the aim button, aim the robot towards the hub
         new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)

src/main/java/frc4388/robot/RobotMap.java

@@ -61,9 +61,9 @@ public class RobotMap {
     /* Swreve Drive Subsystem */
     public final SwerveIO swerveDrivetrain;
 
-    // /* Elevator Subsystem */
+    // /* Shooter and Intake Subsystem */
-    // public final ShooterIO shooterIO;
+    public final ShooterIO shooterIO;
-    // public final IntakeIO intakeIO;
+    public final IntakeIO intakeIO;
 
     public RobotMap(SimConstants.Mode mode) {
         switch (mode) {
@@ -91,23 +91,23 @@ public class RobotMap {
 
                 swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
 
-                // Configure Shooter
+                // Configure Shooter 22,23,24
 
                 TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id);
                 TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id);
                 TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id);
                 
-                //Configure Intake
+                //Configure Intake 20,21
-                // TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id);
+                TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id);
-                // TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id);
+                TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id);
 
                 // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
                 // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
                 // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
-                // shooterIO = new ShooterReal(shooter1, shooter2, indexer);
+                shooterIO = new ShooterReal(shooter1, shooter2, indexer);
 
-                // intakeIO = new IntakeReal(arm, roller);
+                intakeIO = new IntakeReal(arm, roller);
 
                 // Fault
                 FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
@@ -116,8 +116,8 @@ public class RobotMap {
                 FaultTalonFX.addDevice(shooter1, "Shooter1");
                 FaultTalonFX.addDevice(shooter2, "Shooter2");
                 FaultTalonFX.addDevice(indexer, "Indexer");
-                // FaultTalonFX.addDevice(arm, "Arm");
+                FaultTalonFX.addDevice(arm, "Arm");
-                // FaultTalonFX.addDevice(roller, "Roller");
+                FaultTalonFX.addDevice(roller, "Roller");
                 
                 FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
                 FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
@@ -139,8 +139,8 @@ public class RobotMap {
                 // leftCamera = new VisionIO() {};
                 // rightCamera = new VisionIO() {};
                 swerveDrivetrain = new SwerveIO() {};
-                // intakeIO = new IntakeIO() {};
+                intakeIO = new IntakeIO() {};
-                // shooterIO = new ShooterIO() {};
+                shooterIO = new ShooterIO() {};
                 break;
         }
     }

src/main/java/frc4388/robot/subsystems/intake/Intake.java

@@ -18,11 +18,11 @@ public class Intake extends SubsystemBase {
     Supplier<Pose2d> m_swervePoseSupplier;
 
     public Intake(
-        IntakeIO io,
+        IntakeIO io
-        Supplier<Pose2d> swervePoseSupplier
+        // Supplier<Pose2d> swervePoseSupplier
     ) {
         this.io = io;
-        this.m_swervePoseSupplier = swervePoseSupplier;
+        // this.m_swervePoseSupplier = swervePoseSupplier;
     }
 
     public enum IntakeMode {
@@ -33,11 +33,11 @@ public class Intake extends SubsystemBase {
     public void setMode(IntakeMode mode) {
         switch (mode) {
             case Up:
-                io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
+                // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
                 io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
                 break;
             case Down:
-                io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
+                // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
                 io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
                 break;
         }

src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java

@@ -23,7 +23,6 @@ public class IntakeConstants {
 
     //IDs
 
-
     public static final CanDevice ARM_ID   = new CanDevice("SHOOTER 1", 20);
     public static final CanDevice ROLLER_ID   = new CanDevice("SHOOTER 2", 21);
         
@@ -31,8 +30,11 @@ public class IntakeConstants {
 
     // 0 is the forward angle on the robot.
     // negative is left, positive is right
-    public static final Angle ARM_LIMIT_LOWER = Degrees.of(-180);
+
-    public static final Angle ARM_LIMIT_UPPER = Degrees.of(180);
+    //when testing the negative output of 10% made the robot put the intake up
+
+    public static final Angle ARM_LIMIT_LOWER = Degrees.of(90);
+    public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90);
     public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0);
     public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0);
 

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

@@ -14,15 +14,15 @@ public class Shooter extends SubsystemBase {
     ShooterIO io;
     ShooterStateAutoLogged state = new ShooterStateAutoLogged();
 
-    Supplier<Pose2d> m_swervePoseSupplier;
+    // Supplier<Pose2d> m_swervePoseSupplier;
 
 
     public Shooter(
-        ShooterIO io,
+        ShooterIO io
-        Supplier<Pose2d> swervePoseSupplier
+        // Supplier<Pose2d> swervePoseSupplier
     ) {
         this.io = io;
-        this.m_swervePoseSupplier = swervePoseSupplier;
+        // this.m_swervePoseSupplier = swervePoseSupplier;
     }
 
     public enum FieldZone {
@@ -80,8 +80,8 @@ public class Shooter extends SubsystemBase {
 
         Logger.processInputs("Shooter", state);
 
-        Pose2d pose = m_swervePoseSupplier.get();
+        // Pose2d pose = m_swervePoseSupplier.get();
-        Angle robotRot = pose.getRotation().getMeasure();
+        // Angle robotRot = pose.getRotation().getMeasure();
 
         io.updateInputs(state);
 

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

@@ -22,9 +22,10 @@ public class ShooterConstants {
     public static final double INDEXER_GEAR_RATIO = 1.;
 
     public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0);
-    public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
+    
+    public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(4.0);
     public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
-    public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
+    public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(4.0);
     public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0);
 
 
@@ -59,6 +60,7 @@ public class ShooterConstants {
     //                 .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
     // );
 
+
         public static final CanDevice SHOOTER1_ID   = new CanDevice("SHOOTER 1", 22);
         public static final CanDevice SHOOTER2_ID   = new CanDevice("SHOOTER 2", 23);
         public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24);