IDk

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

@@ -65,7 +65,7 @@ public class RobotContainer {
     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);
+    public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
     private Boolean operatorManualMode = false;
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
@@ -175,15 +175,15 @@ public class RobotContainer {
 
        
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); 
+        //     .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); 
+        //     .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); 
             
-        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Extended);}, m_robotIntake)); 
+        //     .onTrue(new InstantCommand(() -> {;}, m_robotIntake)); 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
-            .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); 
+        //     .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); 
 
 
 
@@ -196,18 +196,7 @@ public class RobotContainer {
                 m_robotShooter.io.updateGains();
             }));
 
-        // IF the driver is holding the aim button, aim the robot towards the hub
+        // IF the driver is holding the left trigger, intake driving
-        new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
-            .whileTrue(new RunCommand(
-                () -> {
-                m_robotSwerveDrive.driveFacingPosition(
-                    getDeadbandedDriverController().getLeft(),
-                    FieldConstants.BLUE_HUB_POS);
-                }, m_robotSwerveDrive))
-            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
-
-
-        // IF the driver is holding the aim button, aim the robot towards the hub
         new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
             .whileTrue(new RunCommand(
                 () -> {
@@ -216,7 +205,35 @@ public class RobotContainer {
                         false
                     );
                 }, m_robotSwerveDrive))
-            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
+            // .onFalse(new InstantCommand(() -> , m_robotSwerveDrive));
+
+            
+            .onTrue(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Extended);
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Retracted);
+                m_robotSwerveDrive.softStop();
+            }, m_robotSwerveDrive));
+
+        // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
+        new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
+            .whileTrue(new RunCommand(
+                () -> {
+                m_robotSwerveDrive.driveFacingPosition(
+                    getDeadbandedDriverController().getLeft(),
+                    FieldConstants.BLUE_HUB_POS);
+                }, m_robotSwerveDrive)
+            )
+            .onTrue(new InstantCommand(() -> {
+                // When Right trigger is pressed, 
+                m_robotIntake.setMode(IntakeMode.Extended);
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Retracted);
+                m_robotSwerveDrive.softStop();
+            }, m_robotSwerveDrive));
+
 
         // D-PAD fine alignment
         new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)

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

@@ -39,6 +39,10 @@ public class Intake extends SubsystemBase {
         this.mode = mode;
     }
 
+    public IntakeMode getMode() {
+        return mode;
+    }
+
 
     // public enum FieldZone {
     //     // The robot should aim at the hub

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

@@ -78,6 +78,7 @@ public class IntakeReal implements IntakeIO {
         // Assume that the angle is always accurate, because I think we will use a shaft encoder
         // Assume that 0 degrees = forwards. Might need an offset here
 
+        // angle = clampAng(angle, IntakeConstants.ARM_LIMIT_RETRACTED, IntakeConstants.ARM_LIMIT_EXTENDED);
 
         // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
         Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO);

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

@@ -2,23 +2,38 @@ package frc4388.robot.subsystems.shooter;
 
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
+import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.intake.Intake;
+import frc4388.robot.subsystems.intake.Intake.IntakeMode;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 public class Shooter extends SubsystemBase {
     public ShooterIO io;
     ShooterStateAutoLogged state = new ShooterStateAutoLogged();
 
+    SwerveDrive m_SwerveDrive;
+    Intake m_Intake;
+    LED m_robotLED;
+    
+
     // Supplier<Pose2d> m_swervePoseSupplier;
 
 
     public Shooter(
-        ShooterIO io
+        ShooterIO io,
-        // Supplier<Pose2d> swervePoseSupplier
+        SwerveDrive swerveDrive,
+        Intake intake,
+        LED robotLED
     ) {
         this.io = io;
+        this.m_SwerveDrive = swerveDrive;
+        this.m_Intake = intake;
+        this.m_robotLED = robotLED;
         // this.m_swervePoseSupplier = swervePoseSupplier;
     }
 
@@ -32,57 +47,67 @@ public class Shooter extends SubsystemBase {
 
     
     public enum ShooterMode {
-        //Shooter is at speed it fires balls
+        // Shooter is actively shooting
-        Active,
+        Shooting,
-        //Shooter is at a resting velocity
+        // Shooter is going to fire soon
-        Resting,
+        Ready,
-        //Shooter is inactive (Off)
+        // Not ready to shoot
-        Inactive,
+        NotReady,
     }
 
-    private ShooterMode mode = ShooterMode.Inactive;
+    private ShooterMode mode = ShooterMode.NotReady;
 
-    public void setMode(ShooterMode mode) {
+    // public void setMode(ShooterMode mode) {
-        this.mode = mode;
+    //     this.mode = mode;
+    // }
+
+    public void setShooterReady() {
+        if(this.mode == ShooterMode.NotReady) {
+            this.mode = ShooterMode.Ready;
+        }
     }
     
-    // Calculate what should be done based off of the position of the robot
+    public void setShooterNotReady() {
-    // TODO: Implement field zones
+        this.mode = ShooterMode.NotReady;
-    public FieldZone getTarget(Pose2d position) {
-        return FieldZone.InShootZone;
     }
 
+    @AutoLogOutput
+    public ShooterMode getMode() {
+        return mode;
+    }
+
+
 
     @Override
     public void periodic() {
-        
-        
-
         // FaultReporter.register(this); // TODO Implement fault reporter
 
-
         Logger.processInputs("Shooter", state);
-
-        // Pose2d pose = m_swervePoseSupplier.get();
-        // Angle robotRot = pose.getRotation().getMeasure();
-
         io.updateInputs(state);
 
+        if(this.mode != ShooterMode.NotReady) {
+            double badRobotSpeed = Math.sqrt(Math.pow(m_SwerveDrive.chassisSpeeds.vxMetersPerSecond,2) + Math.pow(m_SwerveDrive.chassisSpeeds.vyMetersPerSecond,2));
+            double badAngSpeed = Math.abs(m_SwerveDrive.chassisSpeeds.omegaRadiansPerSecond);
+            double badShooterVelocity = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
+            boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended;
+
+
+
+            // TODO: get if the robot is within the correct distance of the hub
+        }
+
         switch (mode) {
-            case Active:
+            case Shooting:
                 io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
-                // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY);
+                io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get()));
-                io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get()));
                 break;
-            case Resting:
+            case Ready:
+                io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get()));
+                io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get()));
+                break;
+            case NotReady:
                 io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get()));
-                // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
+                io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get()));
-                io.setIndexerVelocity(state, RotationsPerSecond.of(0));
-                break;
-            case Inactive:
-                io.setShooterVelocity(state, RotationsPerSecond.of(0));
-                // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY);
-                io.setIndexerVelocity(state, RotationsPerSecond.of(0));
                 break;
         }
 

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

@@ -33,8 +33,21 @@ public class    ShooterConstants {
     public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15);
     // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
     
-    public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Indexer Active Velocity", 10);
+    public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10);
-    // public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0);
+    public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10);
+
+    // Tolerances
+    public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist", 1);
+    public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist", 1);
+
+    public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang  tolerance", 1);
+
+    public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Shoot speed tolerance", 1);
+    public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance", 1);
+
+    public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1);
+
+    
 
     public static Slot0Configs SHOOTER_PID = new Slot0Configs()
         .withKV(0.0)

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

@@ -44,51 +44,6 @@ public class ShooterReal implements ShooterIO {
         m_indexerVelocity.Slot = 0;
     }
     
-    private Angle clampAng(Angle x, Angle min, Angle max){
-        if(x.gt(max)) {
-            return max;
-        }else if(x.lt(min)) {
-            return min;
-        }else{
-            return x;
-        }
-    }
-
-
-
-   
-
-    // // TODO: Test
-    // @Override
-    // public void setShooterAngle(ShooterState state, Angle angle) {
-    //     state.shooterTargetAngle = angle;
-    //     // Assume that the angle is always accurate, because I think we will use a shaft encoder
-    //     // Assume that 0 degrees = forwards. Might need an offset here
-
-    //     Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT);
-    //     // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
-    //     double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO;
-    //     PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
-    //     m_angleMotor.setControl(posRequest);
-    // }
-
-
-    // TODO: Test
-    // @Override
-    // public void setShooterPitch(ShooterState state, Angle angle) {
-    //     state.shooterTargetPitch = angle;
-    //     // TODO: Test
-    //     // This assumes that the 0 is paralell to the ground. Might need an offset here
-
-
-    //     Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER);
-    //     // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT
-    //     double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO;
-    //     PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle);
-    //     m_pitchMotor.setControl(posRequest);
-    // }
-    
-
     @Override
     public void setShooterVelocity(ShooterState state, AngularVelocity target) {
         state.motor1TargetVelocity = target;