diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..9a9ca7b --- /dev/null +++ b/.gitignore @@ -0,0 +1,187 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json + +# Eclipse generated file for annotation processors +.factorypath diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5e6ede8 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,61 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ], + "java.dependency.enableDependencyCheckup": false +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..d459245 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2026", + "teamNumber": 2890 +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d6f51cf..639b17e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,29 +63,32 @@ public final class Constants { } public static class ShooterConstants { - private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter Velocity", -0.5) + private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter RPM", -1000) .withWidget(BuiltInWidgets.kNumberBar).getEntry(); - public static double SHOOTER_VELOCITY = -0.6; + public static double SHOOTER_RPM = -0.6; public static double SHOOTER_POWER = -0.45; public static void getShooterVelocity() { - SHOOTER_VELOCITY = shooterVelocity.getDouble(-0.5); + SHOOTER_RPM = shooterVelocity.getDouble(-1000); } public static final int CENTER_SHOOTER_MOTOR_ID = 42; public static final int LEFT_SHOOTER_MOTOR_ID = 41; public static final int RIGHT_SHOOTER_MOTOR_ID = 40; public static final int INDEXER_MOTOR_ID = 43; - public static double INDEXER_MOTOR_SPEED = 0.6; + public static double INDEXER_AND_RAMP_MOTOR_RPM; - private static GenericEntry indexerAndRampSpeed = programmingTab.add("Desired Ramp + Indexer Speed", 0.6) + public static double SHOOTER_MOTOR_P = 0.001; + public static double SHOOTER_MOTOR_I = 0; + public static double SHOOTER_MOTOR_D = 0; + private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer Speed", 1000) .withWidget(BuiltInWidgets.kNumberBar).getEntry(); // this method called in robot periodic so values updated in elastic are // constantly read and applied to RAMP_MOTOR_SPEED - public static void getRampMotorSpeed() { - INDEXER_MOTOR_SPEED = indexerAndRampSpeed.getDouble(.6); + public static void getIndexerAndRampMotorRPM() { + INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(1000); } public static final int LEFT_ACTUATOR_PWM_PORT = 1; @@ -97,13 +100,12 @@ public final class Constants { } public static class IntakeConstants { - private static GenericEntry intakeSpeed = programmingTab.add("Desired Intake Speed", -0.4) + private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000) .withWidget(BuiltInWidgets.kNumberBar).getEntry(); - - public static double INTAKE_WHEELS_MOTOR_SPEED; + public static double INTAKE_WHEELS_MOTOR_RPM; public static void getIntakeWheelsSpeed() { - INTAKE_WHEELS_MOTOR_SPEED = intakeSpeed.getDouble(-0.4); + INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000); } public static final int INTAKE_WHEELS_MOTOR_ID = 50; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 115ea0b..0a80085 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -270,37 +270,6 @@ public class RobotContainer { drivebase.setMotorBrake(brake); } - public Command aimAtHopperCommand(DoubleSupplier xSup, DoubleSupplier ySup) { - try (PIDController aimPIDs = new PIDController(0.3, 0, 0.001)) { - aimPIDs.setTolerance(1.0); - - return Commands.run(() -> { - - double xSpeed = xSup.getAsDouble(); - double ySpeed = ySup.getAsDouble(); - - double rot = 0.0; - - if (LimelightHelpers.getTV("limelight")) { - double tx = LimelightHelpers.getTX("limelight"); - rot = aimPIDs.calculate(tx, 0); - rot = MathUtil.clamp(rot, -1.5, 1.5); - } - - drivebase.drive(new Translation2d(xSpeed, ySpeed), rot, false); - }, - drivebase); - } - } - - public SwerveSubsystem getSwerveDriveBase() { - return drivebase; - } - - public CommandXboxController getDriverXbox() { - return driverXbox; - } - public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup( // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE), m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index c6344a1..fb34452 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -1,94 +1,94 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants; - -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.ControlType; - -public class IntakeSubsystem extends SubsystemBase { - - private static SparkFlex intakeMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID, - MotorType.kBrushless); - - private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, - MotorType.kBrushless); - private static SparkClosedLoopController intakeRotatorPIDController; - public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig(); - - public IntakeSubsystem() { - intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P, - Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I, - Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D); - intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, - com.revrobotics.PersistMode.kNoPersistParameters); - intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); - } - - public void startIntakeMotor() { - intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED); - } - - public void reverseIntakeMotor() { - intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED * -1); - } - - public void stopIntakeMotor() { - intakeMotor.set(0); - } - - public Command startIntakeMotorCommand() { - return runOnce(() -> startIntakeMotor()); - } - - public Command reverseIntakeMotorCommand() { - return runOnce(() -> reverseIntakeMotor()); - } - - public Command stopIntakeMotorCommand() { - return runOnce(() -> stopIntakeMotor()); - } - - public void deployIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, - ControlType.kPosition); - } - - public Command deployintakeCommand() { - return runOnce(() -> deployIntake()); - } - - public void retractIntake() { - intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition); - } - - public Command retractIntakeCommand() { - return runOnce(() -> retractIntake()); - } - - public void assistFuelIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE, - ControlType.kPosition); - } - - public Command assistFuelIntakeCommand() { - return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand()) - .andThen(new WaitCommand(2)); - } - - @Override - public void periodic() { - SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition()); - } -} +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; + +public class IntakeSubsystem extends SubsystemBase { + + private static SparkFlex intakeMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID, + MotorType.kBrushless); + + private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID, + MotorType.kBrushless); + private static SparkClosedLoopController intakeRotatorPIDController; + public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig(); + + public IntakeSubsystem() { + intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P, + Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I, + Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D); + intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); + } + + public void startIntakeMotor() { + intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM); + } + + public void reverseIntakeMotor() { + intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM * -1); + } + + public void stopIntakeMotor() { + intakeMotor.set(0); + } + + public Command startIntakeMotorCommand() { + return runOnce(() -> startIntakeMotor()); + } + + public Command reverseIntakeMotorCommand() { + return runOnce(() -> reverseIntakeMotor()); + } + + public Command stopIntakeMotorCommand() { + return runOnce(() -> stopIntakeMotor()); + } + + public void deployIntake() { + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, + ControlType.kPosition); + } + + public Command deployintakeCommand() { + return runOnce(() -> deployIntake()); + } + + public void retractIntake() { + intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition); + } + + public Command retractIntakeCommand() { + return runOnce(() -> retractIntake()); + } + + public void assistFuelIntake() { + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE, + ControlType.kPosition); + } + + public Command assistFuelIntakeCommand() { + return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand()) + .andThen(new WaitCommand(2)); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition()); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index a40fb9e..efcfa7e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,124 +1,168 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.controls.Follower; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkBaseConfig; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.LimelightHelpers; -import edu.wpi.first.wpilibj.AnalogPotentiometer; - -public class ShooterSubsystem extends SubsystemBase { - private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID, - MotorType.kBrushless); - - private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID, - MotorType.kBrushless); - - private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID, - MotorType.kBrushless); - - private static SparkFlex indexerMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID, - MotorType.kBrushless); - - //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT, - // MotorType.kBrushless); - - //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT, - //MotorType.kBrushless); - - //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0); - //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0); - - private static double currentPotentiometerPosition; // might need second value for the right potentiometer - - public void startShooterMotors() { - centerShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER); - leftShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER); - rightShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER); - } - - public double getShooterMotorVelocity() { - return leftShooterMotor.getEncoder().getVelocity(); - } - - public void startIndexerMotor() { - // if (LimelightHelpers.getTX("limelight") < 1.5 && - // LimelightHelpers.getTX("limelight") > -1.5) { - indexerMotor.set(Constants.ShooterConstants.INDEXER_MOTOR_SPEED); - // } else - // indexerMotor.set(0); - } - - /* public Command shootFuelCommand() { - return run(() -> startShooterMotors()) - .until(() -> { - return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY); - }) - .andThen(() -> startIndexerMotor()); - } */ - - - public Command shootFuelCommand() { - return runOnce(() -> startShooterMotors()).andThen(new WaitCommand(2)) - .andThen(() -> startIndexerMotor()); - }; - - - public void stopShooters() { - centerShooterMotor.set(0); - leftShooterMotor.set(0); - rightShooterMotor.set(0); - indexerMotor.set(0); - } - - public Command stopShooterCommand() { - return runOnce(() -> stopShooters()); - } - - public void moveActuator(double desiredPotentiometerPosition) { - if (desiredPotentiometerPosition > currentPotentiometerPosition) { - //TODO: Test for positive or negative power - //leftActuatorMotor.set(0.1); - //rightActuatorMotor.set(0.1); - } else { - //leftActuatorMotor.set(-0.1); - //rightActuatorMotor.set(-0.1); - } - } - - public void stopActuator() { - //leftActuatorMotor.set(0); - //rightActuatorMotor.set(0); - } - - public Command moveActuatorCommand(double desiredPotentiometerPosition) { - return run(() -> moveActuator(desiredPotentiometerPosition)) - .until(() -> currentPotentiometerPosition == currentPotentiometerPosition) - .andThen(() -> stopActuator()); - } - - @Override - public void periodic() { - /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get()); - SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get()); - currentPotentiometerPosition = leftPotentiometer.get(); */ - } -} +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +import java.util.function.BooleanSupplier; + +import com.ctre.phoenix6.controls.Follower; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.LimelightHelpers; +import edu.wpi.first.wpilibj.AnalogPotentiometer; + +public class ShooterSubsystem extends SubsystemBase { + private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID, + MotorType.kBrushless); + + private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID, + MotorType.kBrushless); + + private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID, + MotorType.kBrushless); + + private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID, + MotorType.kBrushless); + + private static SparkClosedLoopController centerShooterMotorPIDController; + public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig(); + + private static SparkClosedLoopController leftShooterMotorPIDController; + public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig(); + + private static SparkClosedLoopController rightShooterMotorPIDController; + public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig(); + + private static SparkClosedLoopController indexerAndRampMotorPIDController; + public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig(); + + public ShooterSubsystem() { + centerShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController(); + + leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController(); + + rightShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController(); + + indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + Constants.ShooterConstants.SHOOTER_MOTOR_I, + Constants.ShooterConstants.SHOOTER_MOTOR_D); + indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, + com.revrobotics.PersistMode.kNoPersistParameters); + indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController(); + } + + + //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT, + // MotorType.kBrushless); + + //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT, + //MotorType.kBrushless); + + //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0); + //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0); + + private static double currentPotentiometerPosition; // might need second value for the right potentiometer + + public void setShooterMotorsRPM() { + centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + } + + public double getShooterMotorRPM() { + return leftShooterMotor.getEncoder().getVelocity(); + } + + public void setIndexerAndRampMotorRPM() { + // if (LimelightHelpers.getTX("limelight") < 1.5 && + // LimelightHelpers.getTX("limelight") > -1.5) { + indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity); + // } else + // indexerMotor.set(0); + } + + /* public Command shootFuelCommand() { + return run(() -> startShooterMotors()) + .until(() -> { + return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY); + }) + .andThen(() -> startIndexerMotor()); + } */ + + + public Command shootFuelCommand() { + return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2)) + .andThen(() -> setIndexerAndRampMotorRPM()); + }; + + + public void stopShooters() { + centerShooterMotor.set(0); + leftShooterMotor.set(0); + rightShooterMotor.set(0); + indexerAndRampMotor.set(0); + } + + public Command stopShooterCommand() { + return runOnce(() -> stopShooters()); + } + + public void moveActuator(double desiredPotentiometerPosition) { + if (desiredPotentiometerPosition > currentPotentiometerPosition) { + //TODO: Test for positive or negative power + //leftActuatorMotor.set(0.1); + //rightActuatorMotor.set(0.1); + } else { + //leftActuatorMotor.set(-0.1); + //rightActuatorMotor.set(-0.1); + } + } + + public void stopActuator() { + //leftActuatorMotor.set(0); + //rightActuatorMotor.set(0); + } + + public Command moveActuatorCommand(double desiredPotentiometerPosition) { + return run(() -> moveActuator(desiredPotentiometerPosition)) + .until(() -> currentPotentiometerPosition == currentPotentiometerPosition) + .andThen(() -> stopActuator()); + } + + @Override + public void periodic() { + /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get()); + SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get()); + currentPotentiometerPosition = leftPotentiometer.get(); */ + } +}