Better Furious5

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
+    }
   }
 ]

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": {

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();

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;
+    }
+
 }

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<Double> m_rpm;
+  private Supplier<Double> m_position;
+  /** Creates a new Adjust. */
+  public Adjust(Supplier<Double> rpm, Supplier<Double> 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;
+  }
+}

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<Double> m_position;
-  private final Supplier<Double> m_rpm;
   private final Supplier<Double> m_indexPower;
   /** Creates a new ShootPiece. */
-  public AutonShootContinuous(ShooterSubsystem shooter, Supplier<Double> position, Supplier<Double> rpm, Supplier<Double> indexpower) {
+  public AutonShootContinuous(ShooterSubsystem shooter, Supplier<Double> 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());
   }