From b7f43f8dc1b989c16839cdfeef49d257d195d936 Mon Sep 17 00:00:00 2001 From: ProgrammingSR Date: Thu, 7 Mar 2024 19:38:10 -0800 Subject: [PATCH] stuff that works --- simgui.json | 10 ++++++++++ src/main/deploy/pathplanner/autos/2-Center-Stays.auto | 6 ++++++ .../deploy/pathplanner/paths/CenterNote-Center.path | 4 ++-- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 5 ++++- .../frc/robot/commands/ShooterSetSpeedCommand.java | 2 +- .../java/frc/robot/subsystems/ShooterSubsystem.java | 9 ++++----- .../java/frc/robot/subsystems/VisionSubsystem.java | 2 +- 8 files changed, 29 insertions(+), 11 deletions(-) diff --git a/simgui.json b/simgui.json index ee5454e..8c63454 100644 --- a/simgui.json +++ b/simgui.json @@ -24,6 +24,16 @@ } } }, + "NetworkTables": { + "retained": { + "SmartDashboard": { + "Auto Chooser": { + "open": true + }, + "open": true + } + } + }, "NetworkTables Info": { "visible": true } diff --git a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto index dc116ac..fc696f2 100644 --- a/src/main/deploy/pathplanner/autos/2-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto @@ -49,6 +49,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/CenterNote-Center.path b/src/main/deploy/pathplanner/paths/CenterNote-Center.path index 4e41e12..0776d16 100644 --- a/src/main/deploy/pathplanner/paths/CenterNote-Center.path +++ b/src/main/deploy/pathplanner/paths/CenterNote-Center.path @@ -36,8 +36,8 @@ "y": 5.5216374597852385 }, "prevControl": { - "x": 2.3470318873095533, - "y": 5.5216374597852385 + "x": 3.1247868210022136, + "y": 5.541275082550166 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d5e6e54..309d83a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -117,7 +117,7 @@ public static final class IntakeConstants { public static final class ShooterConstants { public static final int kTopShooterMotorPort = 20; public static final int kBottomShooterMotorPort = 35; - public static final double kShooterSpeedTop = 0.75; + public static final double kShooterSpeedTop = 0.8; public static final double kShooterSpeedBottom = 0.9; public static final double kShooterOff = 0; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 32086e4..7715b8a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,6 +75,9 @@ public RobotContainer() { new NoteIntakeCommand(m_intakeSubsystem), new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))); + NamedCommands.registerCommand("Intake in", + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, m_robotDrive::getChassisSpeeds, m_robotDrive::autonDrive, @@ -86,7 +89,7 @@ public RobotContainer() { // meters. Distance // from robot center to // furthest module. - new ReplanningConfig(true, true)), + new ReplanningConfig(false, false)), () -> false, m_robotDrive); // new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java index 05c6280..46d32c9 100644 --- a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java @@ -47,6 +47,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return m_ShooterSubsystem.returnCurrentSpeed() > 0.8; + return m_timer.get() > 1.5; } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 075f230..79df31e 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -21,8 +21,8 @@ public class ShooterSubsystem extends SubsystemBase { private double m_topSpeed = 0; private double m_bottomSpeed = 0; - SlewRateLimiter m_topLimiter = new SlewRateLimiter(0.2); - SlewRateLimiter m_bottomLimiter = new SlewRateLimiter(0.2); +// SlewRateLimiter m_topLimiter = new SlewRateLimiter(0.4); +// SlewRateLimiter m_bottomLimiter = new SlewRateLimiter(0.4); public ShooterSubsystem() { m_bottom.setIdleMode(IdleMode.kCoast); @@ -46,9 +46,9 @@ public void setShootingSpeed(ShootSpeed speed) { switch (speed) { case Shooting: m_topSpeed = ShooterConstants.kShooterSpeedTop; - m_topSpeed = m_topLimiter.calculate(m_topSpeed); + // m_topSpeed = m_topLimiter.calculate(m_topSpeed); m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; - m_bottomSpeed = m_bottomLimiter.calculate(m_topSpeed); + // m_bottomSpeed = m_bottomLimiter.calculate(m_topSpeed); // System.out.println("shoot speed: " + ShooterConstants.kShooterSpeed); break; case Off: @@ -71,7 +71,6 @@ public void periodic() { m_bottom.set(m_bottomSpeed); m_top.set(m_topSpeed); - } public static enum ShootSpeed { diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 3789d47..4575193 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -86,7 +86,7 @@ public void periodic() { if (latestMeasurement != null) { for (Consumer consumer : m_consumerList) { - consumer.accept(latestMeasurement); + // consumer.accept(latestMeasurement); } }