Gets shooting working at comp

team401 · Oct 19, 2024 · 20b7e4e · 20b7e4e
1 parent c5d8c38
commit 20b7e4e
Show file tree

Hide file tree

Showing 7 changed files with 143 additions and 5 deletions.
diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json
@@ -2,5 +2,5 @@
     "enableCppIntellisense": false,
     "currentLanguage": "java",
     "projectYear": "2024",
-    "teamNumber": 401
+    "teamNumber": 9999
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -7,11 +7,14 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.DriveWithGamepad;
+import frc.robot.subsystems.shooter_intake.ShooterIntakeIOHardware;
 import frc.robot.subsystems.shooter_intake.ShooterIntakeIOSim;
 import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem;
+import frc.robot.subsystems.shooter_intake.ShooterIntakeSubsystem.State;
 import frc.robot.subsystems.swerve.SwerveDriveSubsystem;
 import frc.robot.subsystems.swerve.SwerveHardwareIO;
 import frc.robot.subsystems.swerve.SwerveSimIO;
@@ -24,16 +27,17 @@ public class RobotContainer {
     private DoubleSupplier rightDistanceSupplier;
     private Supplier<Rotation2d> gyroSupplier;
     private Supplier<Pose2d> simulatedPoseSupplier;
-    ShooterIntakeSubsystem intakeSubsystem = new ShooterIntakeSubsystem(new ShooterIntakeIOSim());
 
     private final CommandXboxController driverController =
             new CommandXboxController(OperatorConstants.kDriverControllerPort);
+    private final CommandXboxController masherController = new CommandXboxController(1);
 
     // Subsystems
     SwerveDriveSubsystem swerveDriveSubsystem;
 
     // Commands
     DriveWithGamepad driveWithGamepad;
+    ShooterIntakeSubsystem intakeSubsystem;
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
@@ -46,16 +50,19 @@ public void setupSubsystems() {
             case REAL:
                 // Real robot, instantiate hardware IO implementations
                 swerveDriveSubsystem = new SwerveDriveSubsystem(new SwerveHardwareIO());
+                intakeSubsystem = new ShooterIntakeSubsystem(new ShooterIntakeIOHardware());
                 break;
 
             case SIM:
                 // Sim robot, instantiate physics sim IO implementations
                 swerveDriveSubsystem = new SwerveDriveSubsystem(new SwerveSimIO());
+                intakeSubsystem = new ShooterIntakeSubsystem(new ShooterIntakeIOSim());
                 break;
 
             default:
                 // Replayed robot, disable IO implementations
                 swerveDriveSubsystem = new SwerveDriveSubsystem(new SwerveSimIO());
+                intakeSubsystem = new ShooterIntakeSubsystem(new ShooterIntakeIOSim());
                 break;
         }
     }
@@ -68,6 +75,15 @@ public void setupCommands() {
                         () -> driverController.getLeftX(),
                         () -> -driverController.getRightX());
         swerveDriveSubsystem.setDefaultCommand(driveWithGamepad);
+
+        masherController
+                .b()
+                .whileTrue(new InstantCommand(() -> intakeSubsystem.setTargetState(State.INTAKING)))
+                .onFalse(new InstantCommand(() -> intakeSubsystem.setTargetState(State.IDLE)));
+        masherController
+                .y()
+                .whileTrue(new InstantCommand(() -> intakeSubsystem.setTargetState(State.SHOOTING)))
+                .onFalse(new InstantCommand(() -> intakeSubsystem.setTargetState(State.IDLE)));
     }
 
     /**

diff --git a/src/main/java/frc/robot/constants/ShooterIntakeConstants.java b/src/main/java/frc/robot/constants/ShooterIntakeConstants.java
@@ -0,0 +1,9 @@
+package frc.robot.constants;
+
+public class ShooterIntakeConstants {
+
+    public class ShooterIntakeHardwareConstants {
+        public static final int topIntakeMotorID = 12;
+        public static final int bottomIntakeMotorID = 11;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIO.java
@@ -16,6 +16,8 @@ public static class ShooterIntakeIOInputs {
 
     public void setTargetSpeed(double rpm);
 
+    public void setVoltage(double volts);
+
     public void setFlywheelPowered(boolean powered);
 
     public double getSpeed();

diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOHardware.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOHardware.java
@@ -0,0 +1,90 @@
+package frc.robot.subsystems.shooter_intake;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.Constants.ShooterIntakeConstants.ShooterIntakeSimConstants;
+import frc.robot.constants.ShooterIntakeConstants.ShooterIntakeHardwareConstants;
+
+public class ShooterIntakeIOHardware implements ShooterIntakeIO {
+
+    private CANSparkMax leftIntake =
+            new CANSparkMax(ShooterIntakeHardwareConstants.topIntakeMotorID, MotorType.kBrushless);
+    private CANSparkMax rightIntake =
+            new CANSparkMax(
+                    ShooterIntakeHardwareConstants.bottomIntakeMotorID, MotorType.kBrushless);
+    private double targetVoltage = 0.0;
+
+    PIDController leftController =
+            new PIDController(
+                    ShooterIntakeSimConstants.FLYWHEEL_KP,
+                    ShooterIntakeSimConstants.FLYWHEEL_KI,
+                    ShooterIntakeSimConstants.FLYWHEEL_KD);
+
+    ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs;
+
+    @Override
+    public void periodic() {
+        rightIntake.setInverted(true);
+        leftIntake.setIdleMode(CANSparkBase.IdleMode.kBrake);
+
+        // 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);
+        // Override PID and just set to max power
+
+        if (shooterIntakeIOInputs.flywheelPowered) {
+            leftIntake.set(targetVoltage / 12);
+            rightIntake.set(targetVoltage / 12);
+        } else {
+            leftIntake.set(0.0);
+            rightIntake.set(0.0);
+        }
+        // flywheelSim.setInputVoltage(calculatedVoltage);
+
+        shooterIntakeIOInputs.flywheelMotorVoltage = targetVoltage;
+        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 setVoltage(double volts) {
+        targetVoltage = volts;
+    }
+
+    @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;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOSim.java
@@ -1,10 +1,9 @@
 package frc.robot.subsystems.shooter_intake;
 
-import static frc.robot.Constants.ShooterIntakeConstants.ShooterIntakeSimConstants;
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import frc.robot.Constants.ShooterIntakeConstants.ShooterIntakeSimConstants;
 
 public class ShooterIntakeIOSim implements ShooterIntakeIO {
 
@@ -62,4 +61,9 @@ public boolean atSpeed() {
     public void setInputs(ShooterIntakeIOInputsAutoLogged shooterIntakeIOInputs) {
         this.shooterIntakeIOInputs = shooterIntakeIOInputs;
     }
+
+    @Override
+    public void setVoltage(double volts) {
+        // Don't do anything here.
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java
@@ -17,8 +17,25 @@ public ShooterIntakeSubsystem(ShooterIntakeIO shooterIntakeIO) {
         shooterIntakeIO.setInputs(shooterIntakeIOInputs);
     }
 
+    @Override
     public void periodic() {
-        currentState.periodic(this);
+        // currentState.periodic(this);
+
+        switch (this.targetState) {
+            case INTAKING:
+                shooterIntakeIO.setVoltage(12.0);
+                shooterIntakeIO.setFlywheelPowered(true);
+                break;
+            case SHOOTING:
+                shooterIntakeIO.setVoltage(-12.0);
+                shooterIntakeIO.setFlywheelPowered(true);
+                break;
+            default:
+                shooterIntakeIO.setVoltage(0.0);
+                shooterIntakeIO.setFlywheelPowered(false);
+                break;
+        }
+
         shooterIntakeIO.periodic();
         Logger.recordOutput("ShooterIntake.CurrentState", currentState);
         Logger.processInputs("shooterIntake", shooterIntakeIOInputs);