From c5a61e335da3a8ed26b1ebb326c57781ce82e0e6 Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 10:36:27 -0800 Subject: [PATCH 01/17] Added intake in to pathplanner center-4 --- .../pathplanner/autos/4-Center-Stays.auto | 18 ++++++++ src/main/java/frc/robot/RobotContainer.java | 41 ++++++++++++------- 2 files changed, 45 insertions(+), 14 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto index 6e755e0..e67d9cb 100644 --- a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto @@ -49,6 +49,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { @@ -87,6 +93,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { @@ -125,6 +137,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 32b79ef..cf4085b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,13 +4,19 @@ package frc.robot; +import java.nio.file.Path; +import java.util.List; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -87,7 +93,12 @@ public RobotContainer() { // from robot center to // furthest module. new ReplanningConfig(false, false)), - () -> false, m_robotDrive); + () -> + {var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false;}, m_robotDrive); // new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, // ShootSpeed.Shooting), @@ -127,7 +138,7 @@ public RobotContainer() { * DriveConstants.kMaxAngularSpeedRadiansPerSecond * (1 - m_driverController .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) + * IOConstants.kSlowModeScalar) / 2, !m_driverController.getRightBumper()), m_robotDrive)); @@ -191,17 +202,19 @@ public void resetAllSubsystems() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // PathPlannerPath path = PathPlannerPath.fromPathFile("Center4Note"); - - // var alliance = DriverStation.getAlliance(); - // PathPlannerPath autonPath = path; - // if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - // autonPath = autonPath.flipPath(); - // } - // m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); - - // return AutoBuilder.followPath(autonPath); - // return null; - return autoChooser.getSelected(); + + ///List pathGroup = PathPlannerAuto.getPathGroupFromAutoFile(autoChooser.getSelected().getName()); + //PathPlannerAuto path = PathPlannerAuto.getPathGroupFromAutoFile(pathGroup); + + //var alliance = DriverStation.getAlliance(); + //PathPlannerPath pathGroup = path; + //if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + //autonPath = autonPath.flipPath(); + //} + //m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); + + // return new PathPlannerAuto(autoChooser.getSelected().getName()); + return autoChooser.getSelected(); + } } From cc8f4912065acda1ae559eab92025300282ab808 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Sat, 9 Mar 2024 10:45:45 -0800 Subject: [PATCH 02/17] Fixed arm encoder offset --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 20 ++++++++++++------- .../frc/robot/subsystems/IntakeSubsystem.java | 1 + 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 309d83a..8fb314f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -106,7 +106,7 @@ public static final class IntakeConstants { public static final double kIntakeAmpScoringAngle = -93; // 193 - 100 (previous angle) /** Encoder offset in rotations */ - public static final double kArmEncoderOffset = 0.2252; + public static final double kArmEncoderOffset = 0.3415; public static final double kIntakeSpeed = 0.5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cf4085b..26fee18 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -69,7 +69,7 @@ public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), - new ParallelDeadlineGroup(new WaitCommand(2), new NoteOuttakeCommand(m_intakeSubsystem)), + new NoteOuttakeCommand(m_intakeSubsystem), new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off))); NamedCommands.registerCommand("Intake", @@ -93,12 +93,18 @@ public RobotContainer() { // from robot center to // furthest module. new ReplanningConfig(false, false)), - () -> - {var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false;}, m_robotDrive); + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, m_robotDrive); // new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, // ShootSpeed.Shooting), diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index e630d6d..80e1453 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -126,6 +126,7 @@ public void periodic() { SmartDashboard.putNumber("intakespeed", m_intakeSpeed); SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance()); + SmartDashboard.putNumber("Arm Absolute Angle", m_armEncoder.getAbsolutePosition()); SmartDashboard.putBoolean("Have Note?", haveNote); SmartDashboard.putNumber("distance sensor", m_distanceSensorToggle ? m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches) : -1); SmartDashboard.putNumber("pid output", armMotorSpeed); From ba24d02490720572a29789d781682ce220043356 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Sat, 9 Mar 2024 10:49:29 -0800 Subject: [PATCH 03/17] fixed parallel thing --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 26fee18..c6bd158 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -69,7 +69,7 @@ public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), - new NoteOuttakeCommand(m_intakeSubsystem), + new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)), new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off))); NamedCommands.registerCommand("Intake", From b4eedf4e0ca5c062bfd4f9036a7fb63b98ce2e5e Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 10:57:03 -0800 Subject: [PATCH 04/17] Reduced Auton Time --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- .../java/frc/robot/commands/ShooterSetSpeedCommand.java | 8 ++++++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c6bd158..9ad7240 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,9 +68,9 @@ public class RobotContainer { public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 1.5), new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)), - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off))); + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01))); NamedCommands.registerCommand("Intake", new SequentialCommandGroup( @@ -164,9 +164,9 @@ private void configureBindings() { // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), + .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 1.5), new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off)); + .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.3)); new JoystickButton(m_operatorController, Button.kA.value) .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java index 46d32c9..ecb4581 100644 --- a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java @@ -13,15 +13,19 @@ public class ShooterSetSpeedCommand extends Command { ShooterSubsystem m_ShooterSubsystem; double m_shooterSpeed = 0; + + double set_time = 1.5; Timer m_timer = new Timer(); ShootSpeed m_shootSpeed; /** Creates a new ShootCommand. */ - public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, ShootSpeed shootSpeed) { + public ShooterSetSpeedCommand(ShooterSubsystem shooterSubsystem, ShootSpeed shootSpeed, double time) { m_ShooterSubsystem = shooterSubsystem; addRequirements(m_ShooterSubsystem); + set_time = time; + m_shootSpeed = shootSpeed; } @@ -47,6 +51,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return m_timer.get() > 1.5; + return m_timer.get() > set_time; } } From 28f4db5f1df8bc3ee8d1b0f2e1c5b05038f1dd4e Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 11:25:00 -0800 Subject: [PATCH 05/17] chore: added control changes, also modified psi constants --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 59 +++++++++++++-------- 2 files changed, 39 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8fb314f..21af18c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -128,8 +128,8 @@ public static class ClimberConstants { public final static int leftReverseChannel = 2; public final static int rightReverseChannel = 3; - public final static double minPressure = 100.0; - public final static double maxPressure = 110.0; + public final static double minPressure = 95.0; + public final static double maxPressure = 105.0; } public static final class VisionConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ad7240..6a56877 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -146,7 +146,7 @@ public RobotContainer() { .getLeftTriggerAxis() * IOConstants.kSlowModeScalar) / 2, - !m_driverController.getRightBumper()), + !m_driverController.getLeftBumper()), m_robotDrive)); } @@ -168,29 +168,44 @@ private void configureBindings() { new NoteOuttakeCommand(m_intakeSubsystem))) .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.3)); - new JoystickButton(m_operatorController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), - m_climberSubsystem)); - new JoystickButton(m_operatorController, Button.kB.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), - m_climberSubsystem)); - - new Trigger(() -> { - return m_driverController.getLeftTriggerAxis() > 0.5; - }).whileTrue( - new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), - new NoteIntakeCommand(m_intakeSubsystem), - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))) + new JoystickButton(m_driverController, Button.kRightBumper.value) + .onTrue(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp)) .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); - new Trigger(() -> { - return m_driverController.getRightTriggerAxis() > 0.5; - }).whileTrue( - new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp), - new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + // Intake, Driver Controller Right Trigger + new Trigger(() -> { + return m_driverController.getRightTriggerAxis() > 0.5; + }).whileTrue( + new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), + new NoteIntakeCommand(m_intakeSubsystem), + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))) + .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + + // Outtake, Operator Controller Right Trigger + new Trigger(() -> { + return m_operatorController.getRightTriggerAxis() > 0.5; + }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem)); + + new Trigger(() -> { + return m_operatorController.getLeftTriggerAxis() > 0.5; + }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 1.5)) + .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.04)); + + // Climber Up, Operator Controller Right Bumper + A Button + new Trigger(() -> { + return m_operatorController.getAButton() && m_operatorController.getRightBumper(); + }).whileTrue(new InstantCommand(() -> m_climberSubsystem.forward())); + + // Climber Down, Operator Controller Right Bumper + B Button + new Trigger(() -> { + return m_operatorController.getBButton() && m_operatorController.getRightBumper(); + }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); + + // Toggle Distance Sensor, Operatoe Controller Left Bumper + Start Button + new Trigger(() -> { + return m_operatorController.getLeftBumper() && m_operatorController.getStartButton(); + }).onTrue(new InstantCommand(() -> m_intakeSubsystem.toggleDistanceSensor())); } /** From bee6aa6bb1174d653f5218bf196d2832f082788f Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 11:36:04 -0800 Subject: [PATCH 06/17] control-changes port --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a56877..00eceac 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -127,7 +127,7 @@ public RobotContainer() { IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond * (1 - m_driverController - .getLeftTriggerAxis() + .getRightTriggerAxis() * IOConstants.kSlowModeScalar), // * 0.8, MathUtil.applyDeadband( @@ -135,7 +135,7 @@ public RobotContainer() { IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond * (1 - m_driverController - .getLeftTriggerAxis() + .getRightTriggerAxis() * IOConstants.kSlowModeScalar), // * 0.8, MathUtil.applyDeadband( @@ -143,7 +143,7 @@ public RobotContainer() { IOConstants.kControllerDeadband) * DriveConstants.kMaxAngularSpeedRadiansPerSecond * (1 - m_driverController - .getLeftTriggerAxis() + .getRightTriggerAxis() * IOConstants.kSlowModeScalar) / 2, !m_driverController.getLeftBumper()), @@ -174,7 +174,7 @@ private void configureBindings() { // Intake, Driver Controller Right Trigger new Trigger(() -> { - return m_driverController.getRightTriggerAxis() > 0.5; + return m_driverController.getLeftTriggerAxis() > 0.5; }).whileTrue( new SequentialCommandGroup( new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), From 39a247f4b39a2baf768cc2f2ee5fbda9197cfa68 Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 11:43:58 -0800 Subject: [PATCH 07/17] 3 note auton option --- .../autos/3-Center-Stays-copy.auto | 101 ++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto new file mode 100644 index 0000000..fe3a087 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto @@ -0,0 +1,101 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3470318873095533, + "y": 5.5216374597852385 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CenterNote-Center" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftNote-Center" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file From 6d9751b83bb7d552048eee80e079f8135e87667b Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 12:10:19 -0800 Subject: [PATCH 08/17] Auto modified to 1 sec wait --- .../deploy/pathplanner/autos/2-Center-Stays.auto | 2 +- src/main/deploy/pathplanner/autos/2-Right-Leave.auto | 12 ++++++++++++ .../pathplanner/autos/3-Center-Stays-copy.auto | 12 ++++++++++++ 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto index fc696f2..a2e0b01 100644 --- a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto @@ -28,7 +28,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/2-Right-Leave.auto b/src/main/deploy/pathplanner/autos/2-Right-Leave.auto index 9c80858..995cddf 100644 --- a/src/main/deploy/pathplanner/autos/2-Right-Leave.auto +++ b/src/main/deploy/pathplanner/autos/2-Right-Leave.auto @@ -21,6 +21,12 @@ "type": "deadline", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "path", "data": { @@ -42,6 +48,12 @@ "name": "Shoot" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto index fe3a087..2c4da45 100644 --- a/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto @@ -25,6 +25,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.0 + } + }, { "type": "path", "data": { @@ -63,6 +69,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "path", "data": { From d1887c63a02e10eab7144cb38fca84196e9b5496 Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 12:48:07 -0800 Subject: [PATCH 09/17] yes. pneumatics. :> --- src/main/java/frc/robot/subsystems/ClimberSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 4146e7d..1902315 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -13,10 +13,10 @@ import frc.robot.Constants.ClimberConstants; public class ClimberSubsystem extends SubsystemBase { - private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, + private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(2, PneumaticsModuleType.REVPH, ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel); - private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, - ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel); + private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(2, PneumaticsModuleType.REVPH, + ClimberConstants.rightReverseChannel, ClimberConstants.rightForwardChannel); private PneumaticHub m_pHub; private boolean m_compressorEnabled; From b2f93394abd5a5c663c0ef000ef35e5499f97df6 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Sat, 9 Mar 2024 13:00:29 -0800 Subject: [PATCH 10/17] Smart Dashboard output for climber solenoids state --- src/main/java/frc/robot/subsystems/ClimberSubsystem.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 1902315..8da249a 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -37,6 +37,7 @@ public ClimberSubsystem() { public void periodic() { m_leftSolenoid.set(m_state); m_rightSolenoid.set(m_state); + SmartDashboard.putString("pneumatics state", m_state.name()); SmartDashboard.putNumber("pressure", m_pHub.getPressure(0)); } From e31c301bb9ec468215343df72af526bd540f76ae Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 13:05:16 -0800 Subject: [PATCH 11/17] auton to 1 second fix --- src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto | 2 +- src/main/deploy/pathplanner/autos/4-Center-Stays.auto | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto index 2c4da45..68e5624 100644 --- a/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto @@ -28,7 +28,7 @@ { "type": "wait", "data": { - "waitTime": 0.0 + "waitTime": 1.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto index e67d9cb..613aa97 100644 --- a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto @@ -28,7 +28,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.0 } }, { @@ -72,7 +72,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.0 } }, { @@ -116,7 +116,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.0 } }, { From 28c89b4e322e9e847eb2bff8046bcd76c7538671 Mon Sep 17 00:00:00 2001 From: Programming Date: Sat, 9 Mar 2024 16:35:29 -0800 Subject: [PATCH 12/17] chore: sent shotoeriems to constants --- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/RobotContainer.java | 13 +++++++------ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 21af18c..2b6c319 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -120,6 +120,9 @@ public static final class ShooterConstants { public static final double kShooterSpeedTop = 0.8; public static final double kShooterSpeedBottom = 0.9; public static final double kShooterOff = 0; + + public static final double kShooterOffTime = 0.04; + public static final double kShooterOnTime = 1.5; } public static class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 88316d8..92f39d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.IOConstants; +import frc.robot.Constants.ShooterConstants; import frc.robot.commands.IntakeArmPositionCommand; import frc.robot.commands.NoteIntakeCommand; import frc.robot.commands.NoteOuttakeCommand; @@ -63,9 +64,9 @@ public class RobotContainer { public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 1.5), + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)), - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01))); + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime))); NamedCommands.registerCommand("Intake", new SequentialCommandGroup( @@ -159,9 +160,9 @@ private void configureBindings() { // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 1.5), + .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.3)); + .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); new JoystickButton(m_driverController, Button.kRightBumper.value) .onTrue(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp)) @@ -184,8 +185,8 @@ private void configureBindings() { new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; - }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 1.5)) - .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.04)); + }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime)) + .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); // Climber Up, Operator Controller Right Bumper + A Button new Trigger(() -> { From e8b55b6eae0039a5764627c76fc1eb2c526b4bce Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Sun, 10 Mar 2024 09:14:05 -0700 Subject: [PATCH 13/17] this is the message --- .../pathplanner/autos/3-Center-Stays-copy.auto | 12 ------------ src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto index 68e5624..fe3a087 100644 --- a/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays-copy.auto @@ -25,12 +25,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, { "type": "path", "data": { @@ -69,12 +63,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, { "type": "path", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 92f39d1..335f40b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,7 +65,7 @@ public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), - new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)), + new ParallelDeadlineGroup(new WaitCommand(0.45), new NoteOuttakeCommand(m_intakeSubsystem)), new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime))); NamedCommands.registerCommand("Intake", From cce90592fedbba63d19746703fdd19c9db0bfe8c Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Sun, 10 Mar 2024 09:21:33 -0700 Subject: [PATCH 14/17] Added options: not gonna use during competition --- src/main/java/frc/robot/RobotContainer.java | 3 +++ src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 5 +++++ 2 files changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 335f40b..d875266 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -74,6 +74,9 @@ public RobotContainer() { new NoteIntakeCommand(m_intakeSubsystem), new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))); + NamedCommands.registerCommand("Pre-Speed - 30%", + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Halfway, 0.01)); + NamedCommands.registerCommand("Intake in", new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 3629c74..10cd23e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -42,6 +42,10 @@ public void setShootingSpeed(ShootSpeed speed) { m_topSpeed = ShooterConstants.kShooterSpeedTop; m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; break; + case Halfway: + m_topSpeed = ShooterConstants.kShooterSpeedTop/3; + m_bottomSpeed = ShooterConstants.kShooterSpeedBottom/3; + break; case Off: m_topSpeed = 0.0; m_bottomSpeed = 0.0; @@ -65,6 +69,7 @@ public void periodic() { public static enum ShootSpeed { Shooting, + Halfway, Off } } From b9926ebc72de4ab64f828294f52aaec5d64966e3 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Sun, 10 Mar 2024 09:49:22 -0700 Subject: [PATCH 15/17] runable commit --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2b6c319..0ef90eb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -122,7 +122,7 @@ public static final class ShooterConstants { public static final double kShooterOff = 0; public static final double kShooterOffTime = 0.04; - public static final double kShooterOnTime = 1.5; + public static final double kShooterOnTime = 1.9; } public static class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d875266..0431825 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,7 +65,7 @@ public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), - new ParallelDeadlineGroup(new WaitCommand(0.45), new NoteOuttakeCommand(m_intakeSubsystem)), + new ParallelDeadlineGroup(new WaitCommand(0.8), new NoteOuttakeCommand(m_intakeSubsystem)), new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime))); NamedCommands.registerCommand("Intake", From 83e77862468586cea2e1580d0ea6759f0cf5409f Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Sun, 10 Mar 2024 09:57:22 -0700 Subject: [PATCH 16/17] run dis --- src/main/deploy/pathplanner/paths/CenterNote-Center.path | 4 ++-- src/main/deploy/pathplanner/paths/LeftNote-Center.path | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/CenterNote-Center.path b/src/main/deploy/pathplanner/paths/CenterNote-Center.path index 0776d16..b50dfaa 100644 --- a/src/main/deploy/pathplanner/paths/CenterNote-Center.path +++ b/src/main/deploy/pathplanner/paths/CenterNote-Center.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -64,5 +64,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftNote-Center.path b/src/main/deploy/pathplanner/paths/LeftNote-Center.path index 2a7d69e..d87c8f2 100644 --- a/src/main/deploy/pathplanner/paths/LeftNote-Center.path +++ b/src/main/deploy/pathplanner/paths/LeftNote-Center.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 3.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -70,5 +70,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file From 2b105311bf63e1d8baf4196a27be751c98913c38 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Mon, 11 Mar 2024 17:11:33 -0700 Subject: [PATCH 17/17] Constants migration --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0ef90eb..17cf494 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -119,6 +119,7 @@ public static final class ShooterConstants { public static final int kBottomShooterMotorPort = 35; public static final double kShooterSpeedTop = 0.8; public static final double kShooterSpeedBottom = 0.9; + public static final double kPreShooterSpeed = 0.4; public static final double kShooterOff = 0; public static final double kShooterOffTime = 0.04; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 10cd23e..3e0ccb9 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -43,8 +43,8 @@ public void setShootingSpeed(ShootSpeed speed) { m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; break; case Halfway: - m_topSpeed = ShooterConstants.kShooterSpeedTop/3; - m_bottomSpeed = ShooterConstants.kShooterSpeedBottom/3; + m_topSpeed = ShooterConstants.kPreShooterSpeed; + m_bottomSpeed = ShooterConstants.kPreShooterSpeed; break; case Off: m_topSpeed = 0.0;