From 89de321439ccf84aa71e46cb3ab1026deae88db2 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Sat, 6 Apr 2024 09:59:43 -0400 Subject: [PATCH] Better Furious5 --- networktables.json.bck | 16 ++++++++ .../deploy/pathplanner/autos/Furious5.auto | 18 ++++++++ src/main/java/frc/robot/RobotContainer.java | 19 +++++++-- src/main/java/frc/robot/RobotState.java | 18 ++++++++ src/main/java/frc/robot/commands/Adjust.java | 41 +++++++++++++++++++ .../Shooter/AutonShootContinuous.java | 11 ++--- 6 files changed, 113 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/commands/Adjust.java diff --git a/networktables.json.bck b/networktables.json.bck index 1a5d94a..8621bd0 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -558,5 +558,21 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Run Intake Simple Position", + "type": "double", + "value": 100.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Run Intake Simple Power", + "type": "double", + "value": 0.5, + "properties": { + "persistent": true + } } ] diff --git a/src/main/deploy/pathplanner/autos/Furious5.auto b/src/main/deploy/pathplanner/autos/Furious5.auto index e9a7cef..a117340 100644 --- a/src/main/deploy/pathplanner/autos/Furious5.auto +++ b/src/main/deploy/pathplanner/autos/Furious5.auto @@ -25,18 +25,36 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Adjust2" + } + }, { "type": "path", "data": { "pathName": "SubwooferToCenterRing" } }, + { + "type": "named", + "data": { + "name": "Adjust3" + } + }, { "type": "path", "data": { "pathName": "Center To Podium Improved" } }, + { + "type": "named", + "data": { + "name": "Adjust4" + } + }, { "type": "path", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18db649..f8b4254 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import frc.robot.commands.Adjust; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; @@ -170,12 +171,14 @@ public class RobotContainer implements Logged { private DoublePreference m_autoWait = new DoublePreference("Autonomous Wait Command (Secs)", 0); //Improved Preferences + private DoublePreference m_SecondShotImprovAngle = new DoublePreference("Second Shot Improved Angle", 60); + private DoublePreference m_SecondShotImprovRPM = new DoublePreference("Second Shot Improved RPM", 3200); private DoublePreference m_ThirdShotImprovAngle = new DoublePreference("Third Shot Improved Angle", 60); private DoublePreference m_ThirdShotImprovRPM = new DoublePreference("Third Shot Improved RPM", 3200); - private DoublePreference m_FourthShotImprovAngle = new DoublePreference("Fourth Shot Improved Angle", 60); private DoublePreference m_FourthShotImprovRPM = new DoublePreference("Fourth Shot Improved RPM", 3200); - + private DoublePreference m_FifthShotImprovAngle = new DoublePreference("Fight Shot Improved Angle", 60); + private DoublePreference m_FifthShotImprovRPM = new DoublePreference("Fifth Shot Improved RPM", 3200); private final Command resetGyro = new ResetGyro(m_robotDrive); private final Command intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition); private final Command extakeCommand = new RunIntake(m_robotIntake, outtakePower, intakePosition); @@ -230,8 +233,14 @@ public class RobotContainer implements Logged { private final Command runIntakeSimpleAuto = new RunIntake(m_robotIntake, runIntakeSimplePower, runIntakeSimplePosition).withTimeout(10.5); private final Command runIndexUntilAuto = new RunIndexUntil(m_shooter, indexPower); private final Command runIndexFromAuto = new RunIndexFrom(m_shooter, indexPower).withTimeout(4); - private final Command shooterShootContinuous = new AutonShootContinuous(m_shooter, continuousShootPosition, continuousShootRPM, continuousShootIndexPower).withTimeout(10.5); + private final Command shooterShootContinuous = new AutonShootContinuous(m_shooter, continuousShootIndexPower).withTimeout(10.5); + + //Adjust Commands + private final Command adjustSecondShot = new Adjust(m_SecondShotImprovRPM, m_SecondShotImprovAngle); + private final Command adjustThirdShot = new Adjust(m_ThirdShotImprovRPM, m_ThirdShotImprovAngle); + private final Command adjustFourthShot = new Adjust(m_FourthShotImprovRPM, m_FourthShotImprovAngle); + private final Command adjustFifthShot = new Adjust(m_FifthShotImprovRPM, m_FifthShotImprovAngle); private final ParallelCommandGroup speakerShotGroup = new ParallelCommandGroup(shootInterpolated, driveToTarget); @@ -302,6 +311,10 @@ public RobotContainer(){ NamedCommands.registerCommand("RunIndexFrom", runIndexFromAuto); NamedCommands.registerCommand("ShooterContinuousRun", shooterShootContinuous); NamedCommands.registerCommand("AutonIterpolatedShot", autonShootInterpolated); + NamedCommands.registerCommand("Adjust2", adjustSecondShot); + NamedCommands.registerCommand("Adjust3", adjustThirdShot); + NamedCommands.registerCommand("Adjust4", adjustFourthShot); + NamedCommands.registerCommand("Adjust5", adjustFifthShot); // Configure the button bindings configureButtonBindings(); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index edd1de6..32ea735 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -10,6 +10,8 @@ /** Add your docs here. */ public class RobotState { + private double autoRpm; + private double autoPosition; private Pose2d m_robotPose2d; private Pose2d m_speakerPose2d; private int m_speakerID = 0; @@ -67,4 +69,20 @@ public boolean hasNote() { return m_hasNote; } + public double getAutoRPM(){ + return autoRpm; + } + + public double getAutoPosition(){ + return autoPosition; + } + + public void adjustAutoRPM(double adjust){ + autoRpm = adjust; + } + + public void adjustAutoPosition(double adjust){ + autoPosition = adjust; + } + } diff --git a/src/main/java/frc/robot/commands/Adjust.java b/src/main/java/frc/robot/commands/Adjust.java new file mode 100644 index 0000000..e053a54 --- /dev/null +++ b/src/main/java/frc/robot/commands/Adjust.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; +import frc.robot.RobotState; + +public class Adjust extends Command { + private Supplier m_rpm; + private Supplier m_position; + /** Creates a new Adjust. */ + public Adjust(Supplier rpm, Supplier position) { + m_rpm = rpm; + m_position = position; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + RobotState.getInstance().adjustAutoRPM(m_rpm.get()); + RobotState.getInstance().adjustAutoPosition(m_position.get()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/Shooter/AutonShootContinuous.java b/src/main/java/frc/robot/commands/Shooter/AutonShootContinuous.java index 6dd5ccb..486398b 100644 --- a/src/main/java/frc/robot/commands/Shooter/AutonShootContinuous.java +++ b/src/main/java/frc/robot/commands/Shooter/AutonShootContinuous.java @@ -7,20 +7,17 @@ import edu.wpi.first.wpilibj.shuffleboard.SuppliedValueWidget; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.RobotState; import java.util.function.Supplier; public class AutonShootContinuous extends Command { private final ShooterSubsystem m_shooter; - private final Supplier m_position; - private final Supplier m_rpm; private final Supplier m_indexPower; /** Creates a new ShootPiece. */ - public AutonShootContinuous(ShooterSubsystem shooter, Supplier position, Supplier rpm, Supplier indexpower) { + public AutonShootContinuous(ShooterSubsystem shooter, Supplier indexpower) { m_shooter = shooter; - m_position = position; - m_rpm = rpm; m_indexPower = indexpower; // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_shooter); @@ -33,8 +30,8 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.spinRPM(m_rpm.get()); - m_shooter.setAngleDegrees(m_position.get()); + m_shooter.spinRPM(RobotState.getInstance().getAutoRPM()); + m_shooter.setAngleDegrees(RobotState.getInstance().getAutoPosition()); m_shooter.runIndex(m_indexPower.get()); }