RPM changes for indexer, ramp, and shooter
This commit is contained in:
Team2890HawkCollective
187
.gitignore
vendored
Normal file
187
.gitignore
vendored
Normal file
@@ -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
|
||||
21
.vscode/launch.json
vendored
Normal file
21
.vscode/launch.json
vendored
Normal file
@@ -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,
|
||||
}
|
||||
]
|
||||
}
|
||||
61
.vscode/settings.json
vendored
Normal file
61
.vscode/settings.json
vendored
Normal file
@@ -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
|
||||
}
|
||||
6
.wpilib/wpilib_preferences.json
Normal file
6
.wpilib/wpilib_preferences.json
Normal file
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"enableCppIntellisense": false,
|
||||
"currentLanguage": "java",
|
||||
"projectYear": "2026",
|
||||
"teamNumber": 2890
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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());
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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(); */
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user