Team2890/Mothman
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed intake ramp, shooter, and drive PID

Browse Source
This commit is contained in:
Mehooliu
2026-02-21 17:26:10 -05:00
parent a8f351854f
commit 3d7601387a
15 changed files with 382 additions and 292 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
src/main/java/frc/robot/Constants.java
View File

@@ -68,60 +68,66 @@ public final class Constants {
}
public static class ShooterConstants {
private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -5000)
private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
public static double SHOOTER_RPM;
public static void updateShooterRPM() {
SHOOTER_RPM = shooterRPM.getDouble(-5000);
SHOOTER_RPM = shooterRPM.getDouble(-4000);
}
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_AND_RAMP_MOTOR_RPM;
public static double SHOOTER_MOTOR_P = 0.001;
public static double SHOOTER_MOTOR_P = 0.0018;
public static double SHOOTER_MOTOR_I = 0;
public static double SHOOTER_MOTOR_D = 0;
private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer Speed", 2000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
public static double INDEXER_MOTOR_P = 0.0001;
public static double INDEXER_MOTOR_I = 0;
public static double INDEXER_MOTOR_D = 0;
/*private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer RPM", 2000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();*/
public static double INDEXER_AND_RAMP_MOTOR_RPM = 10000;
// this method called in robot periodic so values updated in elastic are
// constantly read and applied to RAMP_MOTOR_SPEED
public static void updateIndexerAndRampMotorRPM() {
/*public static void updateIndexerAndRampMotorRPM() {
INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000);
}
}*/
}
public static class IntakeConstants {
public static double INTAKE_WHEELS_MOTOR_SPEED;
private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry();
public static double INTAKE_WHEELS_MOTOR_RPM;
/* private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000)
.withWidget(BuiltInWidgets.kNumberBar).getEntry(); */
public static double INTAKE_WHEELS_MOTOR_RPM = -7000;
public static void updateIntakeWheelsRPM() {
/*public static void updateIntakeWheelsRPM() {
INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000);
}
}*/
public static final int INTAKE_WHEELS_MOTOR_ID = 50;
public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
public static class IntakeRotatorPID {
public static final double INTAKE_ROTATOR_P = 0.01;
public static final double INTAKE_ROTATOR_P = 0.03;
public static final double INTAKE_ROTATOR_I = 0;
public static final double INTAKE_ROTATOR_D = 0;
}
public static final double INTAKE_MOTOR_P = 0.01;
public static final double INTAKE_MOTOR_I = 0;
public static final double INTAKE_MOTOR_D = 0;
public static final double INTAKE_MOTOR_P = 0.0001;
public static final double INTAKE_MOTOR_I = 0;
public static final double INTAKE_MOTOR_D = 0;
public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = 1.2550222873687744;
public static final double INTAKE_COLLECT_ENCODER_VALUE = 5;
public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5;
public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
}
@@ -135,10 +141,10 @@ public final class Constants {
public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera");
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera");
public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera");
public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera");
public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Rear Left Camera");
public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Rear Right Camera");
public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera");
public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera");
public static final Pose2d HUB_POSE = new Pose2d(4.625, 4.03, new Rotation2d());

11
src/main/java/frc/robot/Robot.java
View File

@@ -4,6 +4,8 @@
package frc.robot;
import org.photonvision.targeting.PhotonPipelineResult;
import com.pathplanner.lib.auto.AutoBuilder;
import edu.wpi.first.networktables.GenericEntry;
@@ -36,6 +38,7 @@ public class Robot extends TimedRobot {
private static NetworkTable table;
private static GenericEntry distanceFromLimelight;
public Robot() {
instance = this;
}
@@ -63,6 +66,8 @@ public class Robot extends TimedRobot {
if (isSimulation()) {
DriverStation.silenceJoystickConnectionWarning(true);
}
m_robotContainer.getSwerveDrive().zeroGyroWithAlliance();
}
/**
@@ -86,10 +91,10 @@ public class Robot extends TimedRobot {
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
// Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
Constants.IntakeConstants.updateIntakeWheelsRPM();
//Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
//Constants.IntakeConstants.updateIntakeWheelsRPM();
Constants.ShooterConstants.updateShooterRPM();
Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
//Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM,
Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive());

444
src/main/java/frc/robot/RobotContainer.java
View File

@@ -51,246 +51,256 @@ import frc.robot.subsystems.ClimberSubsystem;
*/
public class RobotContainer {
// Replace with CommandPS4Controller or CommandJoystick if needed
final CommandXboxController driverXbox = new CommandXboxController(0);
// The robot's subsystems and commands are defined here...
private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
"swerve/neo"));
// Replace with CommandPS4Controller or CommandJoystick if needed
final CommandXboxController driverXbox = new CommandXboxController(0);
final CommandXboxController operatorXbox = new CommandXboxController(1);
// Establish a Sendable Chooser that will be able to be sent to the
// SmartDashboard, allowing selection of desired auto
private final SendableChooser<Command> autoChooser;
// The robot's subsystems and commands are defined here...
private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
"swerve/neo"));
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
// private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
// Establish a Sendable Chooser that will be able to be sent to the
// SmartDashboard, allowing selection of desired auto
private final SendableChooser<Command> autoChooser;
/**
* Converts driver input into a field-relative ChassisSpeeds that is controlled
* by angular velocity.
*/
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> driverXbox.getLeftY() * -1,
() -> driverXbox.getLeftX() * -1)
.withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
// private final TargetingSubsystems m_TargetingSubsystems = new
// TargetingSubsystems();
private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
/**
* Clone's the angular velocity input stream and converts it to a fieldRelative
* input stream.
*/
SwerveInputStream driveDirectAngle = driveAngularVelocity.copy()
.withControllerHeadingAxis(driverXbox::getRightX,
driverXbox::getRightY)
.headingWhile(true);
/**
* Converts driver input into a field-relative ChassisSpeeds that is controlled
* by angular velocity.
*/
SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> driverXbox.getLeftY() * -1,
() -> driverXbox.getLeftX() * -1)
.withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
/**
* Clone's the angular velocity input stream and converts it to a robotRelative
* input stream.
*/
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
.allianceRelativeControl(true);
/**
* Clone's the angular velocity input stream and converts it to a fieldRelative
* input stream.
*/
SwerveInputStream driveDirectAngle = driveAngularVelocity.copy()
.withControllerHeadingAxis(driverXbox::getRightX,
driverXbox::getRightY)
.headingWhile(true);
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> -driverXbox.getLeftY(),
() -> -driverXbox.getLeftX())
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
2))
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
// Derive the heading axis with math!
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
.withControllerHeadingAxis(() -> Math.sin(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2),
() -> Math.cos(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2))
.headingWhile(true)
.translationHeadingOffset(true)
.translationHeadingOffset(Rotation2d.fromDegrees(
0));
/**
* Clone's the angular velocity input stream and converts it to a robotRelative
* input stream.
*/
SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
.allianceRelativeControl(true);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
DriverStation.silenceJoystickConnectionWarning(true);
SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
() -> -driverXbox.getLeftY(),
() -> -driverXbox.getLeftX())
.withControllerRotationAxis(() -> driverXbox.getRawAxis(
2))
.deadband(OperatorConstants.DEADBAND)
.scaleTranslation(0.8)
.allianceRelativeControl(true);
// Derive the heading axis with math!
SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
.withControllerHeadingAxis(() -> Math.sin(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2),
() -> Math.cos(
driverXbox.getRawAxis(
2) *
Math.PI)
*
(Math.PI *
2))
.headingWhile(true)
.translationHeadingOffset(true)
.translationHeadingOffset(Rotation2d.fromDegrees(
0));
// Create the NamedCommands that will be used in PathPlanner
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the trigger bindings
configureBindings();
DriverStation.silenceJoystickConnectionWarning(true);
// Have the autoChooser pull in all PathPlanner autos as options
autoChooser = AutoBuilder.buildAutoChooser();
// Create the NamedCommands that will be used in PathPlanner
NamedCommands.registerCommand("test", Commands.print("I EXIST"));
// Set the default auto (do nothing)
autoChooser.setDefaultOption("Do Nothing", Commands.none());
// Have the autoChooser pull in all PathPlanner autos as options
autoChooser = AutoBuilder.buildAutoChooser();
// Add a simple auto option to have the robot drive forward for 1 second then
// stop
autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
// Set the default auto (do nothing)
autoChooser.setDefaultOption("Do Nothing", Commands.none());
// Put the autoChooser on the SmartDashboard
SmartDashboard.putData("Auto Chooser", autoChooser);
// Add a simple auto option to have the robot drive forward for 1 second then
// stop
autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
// Put the autoChooser on the SmartDashboard
SmartDashboard.putData("Auto Chooser", autoChooser);
}
/**
* Use this method to define your trigger->command mappings. Triggers can be
* created via the
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
* an arbitrary predicate, or via the
* named factories in
* {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
* for
* {@link CommandXboxController
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
* controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
* Flight joysticks}.
*/
private void configureBindings() {
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngle);
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
.driveFieldOriented(driveAngularVelocityKeyboard);
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngleKeyboard);
driverXbox.leftTrigger().whileTrue(m_IntakeSubsystem.startIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand().andThen(m_ShooterSubsystem.reverseIndexerAndRampMotorRPMCommand()))
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand().andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand()));
// command for
// full shooting system including linear actuators
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
// .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand())
.onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand())
.onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
driverXbox.rightStick().onTrue(climbCommand());
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand()
.andThen(m_IntakeSubsystem.deployintakeCommand()));
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
// () -> -driverXbox.getLeftX()));
// driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
operatorXbox.x().whileTrue(m_ShooterSubsystem.testLeftShooterCommand())
.onFalse(m_ShooterSubsystem.stopLeftShooterCommand());
operatorXbox.y().whileTrue(m_ShooterSubsystem.testCenterShooterCommand())
.onFalse(m_ShooterSubsystem.stopCenterShooterCommand());
operatorXbox.b().whileTrue(m_ShooterSubsystem.testRightShooterCommand())
.onFalse(m_ShooterSubsystem.stopRightShooterCommand());
operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
.onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
}
/**
* Use this method to define your trigger->command mappings. Triggers can be
* created via the
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
* an arbitrary predicate, or via the
* named factories in
* {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
* for
* {@link CommandXboxController
* Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
* controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
* Flight joysticks}.
*/
private void configureBindings() {
Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngle);
Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
.driveFieldOriented(driveAngularVelocityKeyboard);
Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
driveDirectAngleKeyboard);
if (Robot.isSimulation()) {
Pose2d target = new Pose2d(new Translation2d(1, 4),
Rotation2d.fromDegrees(90));
// drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
driveDirectAngleKeyboard.driveToPose(() -> target,
new ProfiledPIDController(5,
0,
0,
new Constraints(5, 2)),
new ProfiledPIDController(5,
0,
0,
new Constraints(Units.degreesToRadians(360),
Units.degreesToRadians(180))));
driverXbox.start()
.onTrue(Commands.runOnce(() -> drivebase
.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
driverXbox.button(2)
.whileTrue(Commands.runEnd(
() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
() -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
driverXbox.leftTrigger().whileTrue(m_IntakeSubsystem.startIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand())
.onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
// command for
// full shooting system including linear actuators
driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()
.andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand())
.onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand())
.onFalse(m_ClimberSubsystem.stopClimberCommand());
driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
driverXbox.rightStick().onTrue(climbCommand());
// driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand()
.andThen(m_IntakeSubsystem.deployintakeCommand()));
// driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
// () -> -driverXbox.getLeftX()));
// driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
if (RobotBase.isSimulation()) {
drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
} else {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
}
if (Robot.isSimulation()) {
Pose2d target = new Pose2d(new Translation2d(1, 4),
Rotation2d.fromDegrees(90));
// drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
driveDirectAngleKeyboard.driveToPose(() -> target,
new ProfiledPIDController(5,
0,
0,
new Constraints(5, 2)),
new ProfiledPIDController(5,
0,
0,
new Constraints(Units.degreesToRadians(360),
Units.degreesToRadians(180))));
driverXbox.start()
.onTrue(Commands.runOnce(() -> drivebase
.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
driverXbox.button(2)
.whileTrue(Commands.runEnd(
() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
() -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
// driverXbox.b().whileTrue(
// drivebase.driveToPose(
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
// );
}
if (DriverStation.isTest()) {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
// above!
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
driverXbox.leftBumper().onTrue(Commands.none());
driverXbox.rightBumper().onTrue(Commands.none());
} else {
driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
driverXbox.start().whileTrue(Commands.none());
driverXbox.back().whileTrue(Commands.none());
driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.rightBumper().onTrue(Commands.none());
}
// driverXbox.b().whileTrue(
// drivebase.driveToPose(
// new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
// );
}
if (DriverStation.isTest()) {
drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
// above!
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Pass in the selected auto from the SmartDashboard as our desired autnomous
// commmand
return autoChooser.getSelected();
driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.back().whileTrue(drivebase.centerModulesCommand());
driverXbox.leftBumper().onTrue(Commands.none());
driverXbox.rightBumper().onTrue(Commands.none());
} else {
driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
driverXbox.start().whileTrue(Commands.none());
driverXbox.back().whileTrue(Commands.none());
driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
driverXbox.rightBumper().onTrue(Commands.none());
}
public void setMotorBrake(boolean brake) {
drivebase.setMotorBrake(brake);
}
}
public SwerveSubsystem getSwerveDrive() {
return drivebase;
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// Pass in the selected auto from the SmartDashboard as our desired autnomous
// commmand
return autoChooser.getSelected();
}
public Command climbCommand() {
if (driverXbox.getRightY() > -0.5) {
return m_ClimberSubsystem.lowerRobotCommand();
} else if (driverXbox.getRightX() < 0.5) {
return m_ClimberSubsystem.liftRobotCommand();
} else
return m_ClimberSubsystem.stopClimberCommand();
public void setMotorBrake(boolean brake) {
drivebase.setMotorBrake(brake);
}
}
public SwerveSubsystem getSwerveDrive() {
return drivebase;
}
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
// m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
m_ShooterSubsystem.shootFuelCommand(),
m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
public Command climbCommand() {
if (driverXbox.getRightY() > -0.5) {
return m_ClimberSubsystem.lowerRobotCommand();
} else if (driverXbox.getRightX() < 0.5) {
return m_ClimberSubsystem.liftRobotCommand();
} else
return m_ClimberSubsystem.stopClimberCommand();
}
public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
// m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
m_ShooterSubsystem.shootFuelCommand(),
m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
}

2
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
View File

@@ -50,6 +50,6 @@ public class ClimberSubsystem extends SubsystemBase{
@Override
public void periodic()
{
SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition());
SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getAngle());
}
}

18
src/main/java/frc/robot/subsystems/IntakeSubsystem.java
View File

@@ -31,9 +31,14 @@ public class IntakeSubsystem extends SubsystemBase {
public static SparkFlexConfig intakeWheelsMotorConfig = 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);
intakeRotatorConfig.closedLoop
.p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P)
.i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
.d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
.p(.06, ClosedLoopSlot.kSlot1)
.i(0, ClosedLoopSlot.kSlot1)
.d(0, ClosedLoopSlot.kSlot1);
intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
@@ -43,7 +48,7 @@ public class IntakeSubsystem extends SubsystemBase {
Constants.IntakeConstants.INTAKE_MOTOR_D);
intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
}
public void startIntakeMotor() {
@@ -71,8 +76,7 @@ public class IntakeSubsystem extends SubsystemBase {
}
public void deployIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
ControlType.kPosition);
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0);
}
public Command deployintakeCommand() {
@@ -80,7 +84,7 @@ public class IntakeSubsystem extends SubsystemBase {
}
public void retractIntake() {
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition);
intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1);
}
public Command retractIntakeCommand() {

94
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
View File

@@ -30,8 +30,8 @@ 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 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);
@@ -42,8 +42,8 @@ public class ShooterSubsystem extends SubsystemBase {
private static SparkClosedLoopController centerShooterMotorPIDController;
public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig();
//private static SparkClosedLoopController leftShooterMotorPIDController;
//public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
private static SparkClosedLoopController leftShooterMotorPIDController;
public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
private static SparkClosedLoopController rightShooterMotorPIDController;
public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig();
@@ -59,13 +59,13 @@ public class ShooterSubsystem extends SubsystemBase {
com.revrobotics.PersistMode.kNoPersistParameters);
centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
/*leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P,
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);
@@ -73,9 +73,9 @@ public class ShooterSubsystem extends SubsystemBase {
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);
indexerAndRampMotorConfig.closedLoop.pid(0.0001,
0,
0);
indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
com.revrobotics.PersistMode.kNoPersistParameters);
indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController();
@@ -83,7 +83,7 @@ public class ShooterSubsystem extends SubsystemBase {
//private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
// MotorType.kBrushless);
// MotorType.kBrushless);
//private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
//MotorType.kBrushless);
@@ -95,10 +95,54 @@ public class ShooterSubsystem extends SubsystemBase {
public void setShooterMotorsRPM() {
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
//leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
}
//test individual motor code
public void setLeftShooterMotorRPM() {
leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
}
public Command testLeftShooterCommand() {
return runOnce(() -> setLeftShooterMotorRPM());
}
public void setRightShooterMotorRPM() {
rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
}
public Command testRightShooterCommand() {
return runOnce(() -> setRightShooterMotorRPM());
}
public void setCenterShooterMotorRPM() {
centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity);
}
public Command testCenterShooterCommand() {
return runOnce(() -> setCenterShooterMotorRPM());
}
public void stopLeftShooterMotorRPM() {
leftShooterMotor.set(0);
}
public Command stopLeftShooterCommand() {
return runOnce(() -> stopLeftShooterMotorRPM());
}
public void stopCenterShooterMotorRPM() {
centerShooterMotor.set(0);
}
public Command stopCenterShooterCommand() {
return runOnce(() -> stopCenterShooterMotorRPM());
}
public void stopRightShooterMotorRPM() {
rightShooterMotor.set(0);
}
public Command stopRightShooterCommand() {
return runOnce(() -> stopRightShooterMotorRPM());
}
public double getShooterMotorRPM() {
return rightShooterMotor.getEncoder().getVelocity();
}
@@ -107,11 +151,25 @@ public class ShooterSubsystem extends SubsystemBase {
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity);
}
/* public Command shootFuelCommand() {
return run(() -> setShooterMotorsRPM())
.until(() -> {
return (getShooterMotorRPM() >= Constants.ShooterConstants.SHOOTER_RPM);
})
public void reverseIndexerAndRampMotorRPM() {
indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, ControlType.kVelocity);
}
public Command reverseIndexerAndRampMotorRPMCommand() {
return runOnce(() -> reverseIndexerAndRampMotorRPM());
}
public Command setIndexerAndRampMotorRPMCommand() {
return runOnce(() -> setIndexerAndRampMotorRPM());
}
public Command stopIndexerAndRampMotorCommand() {
return runOnce(()-> indexerAndRampMotor.set(0));
}
/*public Command shootFuelCommand() {
return runOnce(() -> setShooterMotorsRPM())
.until(() -> {return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM * 0.9);})
.andThen(() -> setIndexerAndRampMotorRPM());
} */
@@ -125,7 +183,7 @@ public class ShooterSubsystem extends SubsystemBase {
public void stopShooters() {
centerShooterMotor.set(0);
//leftShooterMotor.set(0);
leftShooterMotor.set(0);
rightShooterMotor.set(0);
indexerAndRampMotor.set(0);
@@ -159,6 +217,8 @@ public class ShooterSubsystem extends SubsystemBase {
@Override
public void periodic() {
SmartDashboard.putNumber("Shooter Velocity", leftShooterMotor.getEncoder().getVelocity());
/* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
currentPotentiometerPosition = leftPotentiometer.get(); */

17
src/main/java/frc/robot/subsystems/TargetingSubsystems.java
View File

@@ -171,16 +171,21 @@ public class TargetingSubsystems extends SubsystemBase {
* }
*/
public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator,
SwerveSubsystem swerveDrive) {
Optional<EstimatedRobotPose> result = poseEstimator.update(camera.getLatestResult());
public static Optional<EstimatedRobotPose> visionEst = Optional.empty();
if (result.isPresent()) {
EstimatedRobotPose estimatedPose = result.get();
public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) {
/* for (var result : camera.getAllUnreadResults()) {
visionEst = poseEstimator.estimateCoprocMultiTagPose(result);
if (visionEst.isPresent()) {
EstimatedRobotPose estimatedPose = visionEst.get();
swerveDrive.getSwerveDrive()
.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
}
}
}*/
}
@Override
public void periodic() {

2
src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View File

@@ -146,7 +146,7 @@ public class SwerveSubsystem extends SubsystemBase {
// When vision is enabled we must manually update odometry in SwerveDrive
if (visionDriveTest) {
swerveDrive.updateOdometry();
vision.updatePoseEstimation(swerveDrive);
//vision.updatePoseEstimation(swerveDrive);
}
}