Shooter intergration

simgui.json

@@ -63,7 +63,8 @@
       },
       "Subscribers": {
         "open": true
-      }
+      },
+      "open": true
     },
     "visible": true
   },

src/main/java/frc/robot/Constants.java

@@ -67,8 +67,8 @@ public static class BotConstants {
     }
 
     public static class ShooterIntakeConstants {
-        public static final double intakeTargetRPM = -100.0;
-        public static final double shootingTargetRPM = 300.0;
+        public static final double intakeTargetRPM = 1000.0;
+        public static final double shootingTargetRPM = -10000.0;
 
         public static class ShooterIntakeSimConstants {
             public static final DCMotor FLYWHEEL_DC_MOTOR = DCMotor.getCIM(2);
@@ -78,6 +78,14 @@ public static class ShooterIntakeSimConstants {
             public static final double FLYWHEEL_KI = 0.01;
             public static final double FLYWHEEL_KD = 0.0;
         }
+
+        public static class ShooterIntakeHardwareConstants {
+            public static final int topIntakeMotorID = 12;
+            public static final int bottomIntakeMotorID = 11;
+            public static final double FLYWHEEL_KP = 0.75;
+            public static final double FLYWHEEL_KI = 0.01;
+            public static final double FLYWHEEL_KD = 0.0;
+        }
     }
 
     public static class VisionConstants { // ALL PLACEHOLDERS

src/main/java/frc/robot/Robot.java

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem.State;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -75,8 +76,23 @@ public void robotPeriodic() {
         // and running subsystem periodic() methods. This must be called from the
         // robot's periodic
         // block in order for anything in the Command-based framework to work.
-
+        robotContainer.intakeSubsystem.periodic();
         CommandScheduler.getInstance().run();
+
+        if (robotContainer.masherController.y().getAsBoolean()) {
+            robotContainer.intakeSubsystem.setTargetState(State.SHOOTING);
+        } else if (robotContainer.masherController.b().getAsBoolean()) {
+            robotContainer.intakeSubsystem.setTargetState(State.INTAKING);
+        } else {
+            robotContainer.intakeSubsystem.setTargetState(State.IDLE);
+        }
+
+        if (robotContainer.masherController.leftBumper().getAsBoolean()){
+            robotContainer.climberMotor.set(robotContainer.climberSpeed);
+        }
+        else{
+            robotContainer.climberMotor.set(0);
+        }
     }
 
     /** This function is called once each time the robot enters Disabled mode. */

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

@@ -10,25 +10,35 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.DriveWithGamepad;
-import frc.robot.subsystems.shooter_intake.ShooterIntakeIOSim;
+import frc.robot.subsystems.shooter_intake.ShooterIntakeIOHardware;
 import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem;
 import frc.robot.subsystems.swerve.SwerveDriveSubsystem;
-import frc.robot.subsystems.swerve.SwerveHardwareIO;
 import frc.robot.subsystems.swerve.SwerveSimIO;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 public class RobotContainer {
 
     private DoubleSupplier leftDistanceSupplier;
     private DoubleSupplier rightDistanceSupplier;
     private Supplier<Rotation2d> gyroSupplier;
     private Supplier<Pose2d> simulatedPoseSupplier;
-    ShooterIntakeSubsystem intakeSubsystem = new ShooterIntakeSubsystem(new ShooterIntakeIOSim());
 
-    private final CommandXboxController driverController =
+    double climberSpeed = 0;
+
+    CANSparkMax climberMotor = new CANSparkMax(9, MotorType.kBrushed);    
+
+
+    ShooterIntakeSubsystem intakeSubsystem =
+            new ShooterIntakeSubsystem(new ShooterIntakeIOHardware());
+
+    final CommandXboxController driverController =
             new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
+    final CommandXboxController masherController = new CommandXboxController(1);
     // Subsystems
     SwerveDriveSubsystem swerveDriveSubsystem;
 
@@ -45,7 +55,7 @@ public void setupSubsystems() {
         switch (Constants.BotConstants.botMode) {
             case REAL:
                 // Real robot, instantiate hardware IO implementations
-                swerveDriveSubsystem = new SwerveDriveSubsystem(new SwerveHardwareIO());
+                // swerveDriveSubsystem = new SwerveDriveSubsystem(new SwerveHardwareIO());
                 break;
 
             case SIM:
@@ -61,13 +71,13 @@ public void setupSubsystems() {
     }
 
     public void setupCommands() {
-        driveWithGamepad =
+        /*driveWithGamepad =
                 new DriveWithGamepad(
                         swerveDriveSubsystem,
                         () -> driverController.getLeftY(),
                         () -> driverController.getLeftX(),
                         () -> -driverController.getRightX());
-        swerveDriveSubsystem.setDefaultCommand(driveWithGamepad);
+        swerveDriveSubsystem.setDefaultCommand(driveWithGamepad);*/
     }
 
     /**

src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOHardware.java

@@ -0,0 +1,81 @@
+package frc.robot.subsystems.shooter_intake;
+
+import static frc.robot.Constants.ShooterIntakeConstants.ShooterIntakeSimConstants;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.Constants.ShooterIntakeConstants.ShooterIntakeHardwareConstants;
+import frc.robot.Constants.ShooterIntakeConstants.ShooterIntakeSimConstants;
+
+public class ShooterIntakeIOHardware implements ShooterIntakeIO {
+
+    private CANSparkMax leftIntake =
+            new CANSparkMax(ShooterIntakeHardwareConstants.topIntakeMotorID, MotorType.kBrushless);
+    private CANSparkMax rightIntake =
+            new CANSparkMax(
+                    ShooterIntakeHardwareConstants.bottomIntakeMotorID, MotorType.kBrushless);
+    PIDController leftController =
+            new PIDController(
+                    ShooterIntakeSimConstants.FLYWHEEL_KP,
+                    ShooterIntakeSimConstants.FLYWHEEL_KI,
+                    ShooterIntakeSimConstants.FLYWHEEL_KD);
+
+    ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs;
+
+    @Override
+    public void periodic() {
+        if (shooterIntakeIOInputs.flywheelPowered) {
+            leftController.setSetpoint(shooterIntakeIOInputs.flywheelTargetSpeed);
+        } else {
+            leftController.setSetpoint(0.0);
+        }
+        double calculatedLeftVoltage =
+                leftController.calculate(leftIntake.getEncoder().getVelocity());
+
+        calculatedLeftVoltage = MathUtil.clamp(calculatedLeftVoltage, -12, 12);
+
+        if (shooterIntakeIOInputs.flywheelPowered) {
+            leftIntake.set(calculatedLeftVoltage / 12);
+            rightIntake.set(calculatedLeftVoltage / 12);
+        } else {
+            leftIntake.set(0.0);
+            rightIntake.set(0.0);
+        }
+        // flywheelSim.setInputVoltage(calculatedVoltage);
+
+        shooterIntakeIOInputs.flywheelMotorVoltage = calculatedLeftVoltage;
+        shooterIntakeIOInputs.flywheelSpeed = getSpeed();
+        // shooterIntakeIOInputs.flywheelSpeed = flywheelSim.getAngularVelocityRPM();
+    }
+
+    @Override
+    public double getSpeed() {
+        // return flywheelSim.getAngularVelocityRPM();
+        return leftIntake.getEncoder().getVelocity();
+    }
+
+    @Override
+    public void setTargetSpeed(double rpm) {
+        shooterIntakeIOInputs.flywheelTargetSpeed = rpm;
+    }
+
+    @Override
+    public void setFlywheelPowered(boolean powered) {
+        shooterIntakeIOInputs.flywheelPowered = powered;
+    }
+
+    @Override
+    public boolean atSpeed() {
+        if (shooterIntakeIOInputs.flywheelTargetSpeed > 0) {
+            return (shooterIntakeIOInputs.flywheelTargetSpeed <= getSpeed());
+        }
+        return (shooterIntakeIOInputs.flywheelTargetSpeed >= getSpeed());
+    }
+
+    @Override
+    public void setInputs(ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs) {
+        this.shooterIntakeIOInputs = shooterIntakeIOInputs;
+    }
+}

src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java

@@ -6,7 +6,7 @@
 
 public class ShooterIntakeSubsystem extends SubsystemBase {
 
-    protected State currentState = State.IDLE;
+    public State currentState = State.IDLE;
     protected State targetState = State.IDLE;
     protected ShooterIntakeIO shooterIntakeIO;
     static ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs =
@@ -18,16 +18,38 @@ public ShooterIntakeSubsystem(ShooterIntakeIO shooterIntakeIO) {
     }
 
     public void periodic() {
-        currentState.periodic(this);
+        if (this.currentState != this.targetState) {
+            if (this.currentState == State.IDLE) {
+                this.currentState = targetState;
+            } else {
+                this.currentState = State.IDLE;
+            }
+        }
+        this.shooterIntakeIO.setFlywheelPowered(this.currentState != State.IDLE);
+        // this.shooterIntakeIO.setFlywheelPowered(true);
+
+        if (this.currentState == State.SHOOTING)
+            this.shooterIntakeIO.setTargetSpeed(Constants.ShooterIntakeConstants.shootingTargetRPM);
+        else if (this.currentState == State.INTAKING)
+            this.shooterIntakeIO.setTargetSpeed(Constants.ShooterIntakeConstants.intakeTargetRPM);
+        else this.shooterIntakeIO.setTargetSpeed(0.0);
+
+        // currentState.periodic(this);
         shooterIntakeIO.periodic();
         Logger.recordOutput("ShooterIntake.CurrentState", currentState);
+        Logger.recordOutput("ShooterIntake.TargetState", targetState);
+
         Logger.processInputs("shooterIntake", shooterIntakeIOInputs);
     }
 
     public void setTargetState(State target) {
         this.targetState = target;
     }
 
+    public State getTargetState() {
+        return this.targetState;
+    }
+
     // spotless:off
     public enum State {
         IDLE {
@@ -127,7 +149,7 @@ protected void handleStateChanges(ShooterIntakeSubsystem subsystem) {
         }
 
         protected void periodic(ShooterIntakeSubsystem subsystem) {
-            this.handleStateChanges(subsystem);
+            //this.handleStateChanges(subsystem);
         }
     }
     // spotless:on

vendordeps/REVLib.json

@@ -0,0 +1,74 @@
+{
+    "fileName": "REVLib.json",
+    "name": "REVLib",
+    "version": "2024.2.4",
+    "frcYear": "2024",
+    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+    "mavenUrls": [
+        "https://maven.revrobotics.com/"
+    ],
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-java",
+            "version": "2024.2.4"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2024.2.4",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-cpp",
+            "version": "2024.2.4",
+            "libName": "REVLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2024.2.4",
+            "libName": "REVLibDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}