diff --git a/simgui.json b/simgui.json index 185b996..656564d 100644 --- a/simgui.json +++ b/simgui.json @@ -63,7 +63,8 @@ }, "Subscribers": { "open": true - } + }, + "open": true }, "visible": true }, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 55bb5ce..afa0f84 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8d388ba..c513149 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f2b5ae..e9b4f55 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,13 +10,15 @@ 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 { @@ -24,11 +26,19 @@ public class RobotContainer { private DoubleSupplier rightDistanceSupplier; private Supplier gyroSupplier; private Supplier 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);*/ } /** diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOHardware.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeIOHardware.java new file mode 100644 index 0000000..c192f49 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java index 67836a9..5de6694 100644 --- a/src/main/java/frc/robot/subsystems/shooter_intake/ShooterIntakeSubsystem.java +++ b/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,9 +18,27 @@ 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); } @@ -28,6 +46,10 @@ 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 diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..f85acd4 --- /dev/null +++ b/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" + ] + } + ] +} \ No newline at end of file