Development

src/main/java/frc/robot/Constants.java

@@ -140,6 +140,7 @@ public static final class ShooterConstants {
     public static final double POSITION_kD = 2;
     public static final double POSITION_kS = 0.24;
     public static final double POSITION_kV = 0.12;
+    public static final double POSITION_kG = 0.2; 
     public static final double POSITION_ForwardsLimit = 0.42;
     public static final double POSITION_ReverseLimit = 0;
 

src/main/java/frc/robot/Robot.java

@@ -34,6 +34,8 @@
 public class Robot extends TimedRobot implements Logged {
   private Command m_autonomousCommand;
 
+  private Command m_testCommand;
+
   private RobotContainer m_robotContainer;
 
   @Log.NT
@@ -165,6 +167,9 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    if (m_testCommand != null){
+      m_testCommand.cancel();
+    }
   }
 
   /** This function is called periodically during operator control. */
@@ -176,6 +181,11 @@ public void testInit() {
     getAllianceInfo();
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
+    m_testCommand = m_robotContainer.getTestCommand();
+
+    if(m_testCommand != null){
+      m_testCommand.schedule();
+    }
   }
 
   /** This function is called periodically during test mode. */

src/main/java/frc/robot/RobotContainer.java

@@ -58,22 +58,26 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.commands.Shooter.AmpShot.PositionServos;
 import frc.robot.commands.Shooter.AmpShot.AmpShot;
 
+
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
 import com.fasterxml.jackson.core.sym.Name;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
-import frc.robot.commands.ResetGyro;
+//import frc.robot.commands.ResetGyro;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.AutoWait;
 
+import frc.robot.commands.test.rotateRearLeft;
+
 
 
 
@@ -103,27 +107,27 @@ public class RobotContainer implements Logged {
   private DoublePreference intakePower = new DoublePreference("intake/intakePower", 0.5);
   private DoublePreference intakePosition = new DoublePreference("intake/intakePosition", 100);
   private DoublePreference outtakePower = new DoublePreference("intake/outtakePower", -0.75);
-  private DoublePreference umbrellaPower = new DoublePreference( "umbrella/Power", 0.25);
-  private DoublePreference shooterPower = new DoublePreference("shooter/Power", 0.79);
-  private DoublePreference shooterRPM = new DoublePreference("shooter/RPM", 4000);
+  //private DoublePreference umbrellaPower = new DoublePreference( "umbrella/Power", 0.25);
+  //private DoublePreference shooterPower = new DoublePreference("shooter/Power", 0.79);
+  //private DoublePreference shooterRPM = new DoublePreference("shooter/RPM", 4000);
   private DoublePreference intakeShooterPosition = new DoublePreference("shooter/Position", Constants.ShooterConstants.TARGET_POSITION_DEGREES);
   private DoublePreference indexPower = new DoublePreference("shooter/IndexPower", 0.1);
   private DoublePreference shooterIndexPower = new DoublePreference("shooter/ShootingIndexPower", 0.5);
-  private DoublePreference shooterOuttakePower = new DoublePreference("shooter/OuttakePower", -0.1);  
-  private DoublePreference shooterPosition = new DoublePreference("shooter/shootingPosition", 65); 
+  //private DoublePreference shooterOuttakePower = new DoublePreference("shooter/OuttakePower", -0.1);  
+  //private DoublePreference shooterPosition = new DoublePreference("shooter/shootingPosition", 65); 
   private DoublePreference outdexPower = new DoublePreference("shooter/OutdexPower", -0.2);
   private DoublePreference outtakeFlywheelPower = new DoublePreference("shooter/outtakeFlywheelPower", -0.3);
-  private DoublePreference preSpinDistanceM = new DoublePreference("shooter/preSpinDistanceM", 5.842);
+  //private DoublePreference preSpinDistanceM = new DoublePreference("shooter/preSpinDistanceM", 5.842);
   private DoublePreference shooterHome = new DoublePreference("shooter/homeposition", 0);
 
   private DoublePreference climberUpPower = new DoublePreference("climber/UpPower", ClimberConstants.POWER);
   private DoublePreference climberDownPower = new DoublePreference("climber/DownPower", -ClimberConstants.POWER);
-  private DoublePreference climberUpPosition = new DoublePreference("climber/UpPosition", ClimberConstants.TOP_POSITION);
-  private DoublePreference climberDownPosition = new DoublePreference("climber/DownPosition", ClimberConstants.BOTTOM_POSITION);
+  //private DoublePreference climberUpPosition = new DoublePreference("climber/UpPosition", ClimberConstants.TOP_POSITION);
+  //private DoublePreference climberDownPosition = new DoublePreference("climber/DownPosition", ClimberConstants.BOTTOM_POSITION);
 
   //For tuning
-  private DoublePreference tuningPower = new DoublePreference("shooter/tuning_rpm", 2500);
-  private DoublePreference tuningPosition = new DoublePreference("shooter/tuning_Position", 65);
+  //private DoublePreference tuningPower = new DoublePreference("shooter/tuning_rpm", 2500);
+  //private DoublePreference tuningPosition = new DoublePreference("shooter/tuning_Position", 65);
 
 
   //Low Angle, Mid Angle, High Angle
@@ -160,13 +164,13 @@ public class RobotContainer implements Logged {
   private DoublePreference closeAutoShotRPM = new DoublePreference("shooter/closeAutoShotRPM", 3200);
 
   //High power
-  private DoublePreference shooterHighPower = new DoublePreference("shooter/highPower", 78);
+  //private DoublePreference shooterHighPower = new DoublePreference("shooter/highPower", 78);
 
 
    //preferences for slew rates
-   private DoublePreference m_kDirectionSlewRate = new DoublePreference("Direction Slew Rate", Constants.DriveConstants.kDirectionSlewRate);
-   private DoublePreference m_kMagnitudeSlewRate = new DoublePreference("Magnitude Slew Rate", Constants.DriveConstants.kMagnitudeSlewRate);
-   private DoublePreference m_kRotationalSlewRate = new DoublePreference("Rotational Slew Rate", Constants.DriveConstants.kRotationalSlewRate);
+  // private DoublePreference m_kDirectionSlewRate = new DoublePreference("Direction Slew Rate", Constants.DriveConstants.kDirectionSlewRate);
+  //private DoublePreference m_kMagnitudeSlewRate = new DoublePreference("Magnitude Slew Rate", Constants.DriveConstants.kMagnitudeSlewRate);
+   //private DoublePreference m_kRotationalSlewRate = new DoublePreference("Rotational Slew Rate", Constants.DriveConstants.kRotationalSlewRate);
 
    //preference for auto wait (in seconds)
    private DoublePreference m_autoWait = new DoublePreference("Autonomous Wait Command (Secs)", 0);
@@ -179,32 +183,34 @@ public class RobotContainer implements Logged {
    private DoublePreference m_FourthShotImprovRPM = new DoublePreference("Fourth 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 intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition);
     private final Command extakeCommand = new RunIntake(m_robotIntake, outtakePower, intakePosition);
     private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition);
+
     private final Command intakePieceGroupCommand = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition);
     private final Command outTakePiece = new OuttakePiece(m_robotIntake, m_shooter, outtakePower, intakePosition, outdexPower, intakeShooterPosition, outtakeFlywheelPower);
+
     private final Command stowIntake = new StowIntake(m_robotIntake);
     private final Command parkCommand = new ParkCommand(m_robotDrive);
     private final Command stowShooter = new StowShooter(m_shooter, shooterHome);
-    private final Command raiseShooter = new PositionShooter(m_shooter, intakeShooterPosition);
-    private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower);
-    private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower);
+    //private final Command raiseShooter = new PositionShooter(m_shooter, intakeShooterPosition);
+    //private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower);
+    //private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower);
     private final Command shootSubwoofer = new ShootPiece(m_shooter, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
     private final Command shootPodium = new ShootPiece(m_shooter, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
     private final Command shootWing = new ShootPiece(m_shooter, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
-    private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM);
-    private final Command preSpinShooter = new PrepareShooter(m_shooter, preSpinDistanceM);
+    //private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM);
+    //private final Command preSpinShooter = new PrepareShooter(m_shooter, preSpinDistanceM);
     private final Command ejectPiece = new EjectPiece(m_shooter);
 
     private final Command climberPowerUp = new ClimbPower(m_Climber, climberUpPower);
     private final Command climberPowerDown = new ClimbPower(m_Climber, climberDownPower);
-    private final Command climbMMUp = new ClimbMM(m_Climber, climberUpPosition);
-    private final Command climbMMDown = new ClimbMM(m_Climber, climberDownPosition);
+    //private final Command climbMMUp = new ClimbMM(m_Climber, climberUpPosition);
+    //private final Command climbMMDown = new ClimbMM(m_Climber, climberDownPosition);
 
-    private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15);
+    //private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15);
 
-    private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
+    //private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
     private final Command shootInterpolated = new ShootInterpolated(m_shooter, m_photonCameraWrapper, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean());
     private final Command driveToTarget = new DriveToTarget(m_robotDrive, 
             m_photonCameraWrapper, 
@@ -230,6 +236,24 @@ public class RobotContainer implements Logged {
   //Autonomous Wait Command
   private final Command autoWait = new AutoWait(m_autoWait);
 
+  //test mode commands
+  private final Command driveFrontLeft = m_robotDrive.driveModuleSysIdTestBuilder(2, 2, 0);
+  private final Command driveFrontRight = m_robotDrive.driveModuleSysIdTestBuilder(2, 2, 1);
+  private final Command driveRearLeft = m_robotDrive.driveModuleSysIdTestBuilder(2, 2, 2);
+  private final Command driveRearRight = m_robotDrive.driveModuleSysIdTestBuilder(2, 2, 3);
+  private final Command driveAll = m_robotDrive.driveSysIdTestBuilder(2,2);
+
+  private final Command rotateFrontLeft = m_robotDrive.turnModuleSysIdTestBuilder(2,2,0);
+  private final Command rotateFrontRight = m_robotDrive.turnModuleSysIdTestBuilder(2,2,1);
+  private final Command rotateRearLeft = m_robotDrive.turnModuleSysIdTestBuilder(2,2,2);
+  private final Command rotateRearRight = m_robotDrive.turnModuleSysIdTestBuilder(2,2,3);
+  private final Command rotateAll = m_robotDrive.turnSysIdTestBuilder(2, 2);
+
+  private final Command shooterPositionTest = m_shooter.positionerTestBuilder(2,2);
+
+  private final Command intakeSpeedTest = m_robotIntake.intakeMotorTestBuilder(2, 2);
+
+  private final Command climberSpeedTest = m_Climber.climberTestBuilder(2, 2);
   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);
@@ -242,7 +266,9 @@ public class RobotContainer implements Logged {
 
   private final SendableChooser<Command> autonChooser;
 
-  private final double pathSpeed = 2;
+  private final SendableChooser<Command> testChooser;
+
+  //private final double pathSpeed = 2;
 
   private final Command ampShot = new AmpShot(m_shooter, ampShotOnlyShooterAngle, ampShotOnlyShooterRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean(), ampShotOnlyBroomAngle);
 
@@ -258,7 +284,7 @@ private static class Operator {
 
     private static Supplier<Double> driver_axisLX = () -> MathUtil.applyDeadband(-driver.getRawAxis(0), Constants.OIConstants.kDriveDeadband);
     private static Supplier<Double> driver_axisLY = () -> MathUtil.applyDeadband(-driver.getRawAxis(1), Constants.OIConstants.kDriveDeadband);
-    private static Supplier<Double> driver_axisRY = () -> MathUtil.applyDeadband(-driver.getRawAxis(5), Constants.OIConstants.kDriveDeadband);
+   //private static Supplier<Double> driver_axisRY = () -> MathUtil.applyDeadband(-driver.getRawAxis(5), Constants.OIConstants.kDriveDeadband);
 
     private static JoystickButton driver_x = new JoystickButton(driver, XboxController.Button.kX.value);
     private static JoystickButton driver_a = new JoystickButton(driver, XboxController.Button.kA.value);
@@ -271,9 +297,9 @@ private static class Operator {
     private static AxisButton driver_rightTrigger = new AxisButton(driver, XboxController.Axis.kRightTrigger.value, 0.25);
     private static AxisButton driver_leftTrigger = new AxisButton(driver, XboxController.Axis.kLeftTrigger.value, 0.25);
     private static POVButton driver_dpad_up = new POVButton(driver, 0);
-    private static POVButton driver_dpad_right = new POVButton(driver, 90);
+    //private static POVButton driver_dpad_right = new POVButton(driver, 90);
     private static POVButton driver_dpad_down= new POVButton(driver, 180);
-    private static POVButton driver_dpad_left = new POVButton(driver, 270);
+    //private static POVButton driver_dpad_left = new POVButton(driver, 270);
 
     private static JoystickButton manip_a = new JoystickButton(manipulator, XboxController.Button.kA.value);
     private static JoystickButton manip_b = new JoystickButton(manipulator, XboxController.Button.kB.value);
@@ -330,6 +356,31 @@ public RobotContainer(){
     autonChooser.addOption("FourRingImproved", AutoBuilder.buildAuto("4RingImproved"));
     SmartDashboard.putData("Autonomous", autonChooser);
 
+    testChooser = AutoBuilder.buildAutoChooser();
+
+    testChooser.addOption("driveFrontLeft", driveFrontLeft);
+    testChooser.addOption("driveFrontRight", driveFrontRight);
+    testChooser.addOption("driveRearLeft", driveRearLeft);
+    testChooser.addOption("driveRearRight", driveRearRight);
+    testChooser.addOption("driveAll", driveAll);
+
+    testChooser.addOption("rotateFrontLeft", rotateFrontLeft);
+    testChooser.addOption("rotateFrontRight", rotateFrontRight);
+    testChooser.addOption("rotateRearLeft", rotateRearLeft);
+    testChooser.addOption("rotateRearRight", rotateRearRight);
+    testChooser.addOption("rotateAll", rotateAll);
+
+    testChooser.addOption("shooterPositionTest", shooterPositionTest);
+
+    testChooser.addOption("intakeSpeedTest", intakeSpeedTest);
+
+    testChooser.addOption("climberTest", climberSpeedTest);
+
+    SmartDashboard.putData("Test Mode", testChooser);
+
+
+
+
 
   }
 
@@ -398,6 +449,10 @@ private void configureDefaultCommands(){
   public Command getAutonomousCommand() {
     return autonChooser.getSelected();
   }
+
+  public Command getTestCommand(){
+    return testChooser.getSelected();
+  }
 
 
 }

src/main/java/frc/robot/commands/test/rotateRearLeft.java

@@ -0,0 +1,39 @@
+// 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.test;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drive.DriveSubsystem;
+
+public class rotateRearLeft extends Command {
+  DriveSubsystem m_testDrive;
+  /** Creates a new rotateRearLeft. */
+  public rotateRearLeft(DriveSubsystem testDrive) {
+    m_testDrive = testDrive;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(m_testDrive);
+  }
+
+  // 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() {
+    //m_testDrive.rotateRL360();
+  }
+
+  // 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 m_testDrive.getRLPos() == 360;
+   return false;
+  }
+}

src/main/java/frc/robot/subsystems/Climber.java

@@ -15,7 +15,10 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
 import frc.robot.Constants.ClimberConstants;
 import frc.robot.Constants.ShooterConstants;
@@ -135,4 +138,17 @@ public void periodic() {
     current = m_climberMotor.getStatorCurrent().getValueAsDouble();
     position = m_climberMotor.getPosition().getValueAsDouble();
   }
+
+  public Command climberTestBuilder(double staticTimeout, double dynamicTimeout){
+    return
+      new InstantCommand(() -> this.stop())
+      .andThen(new InstantCommand(() -> this.move(0.5)).withTimeout(staticTimeout))
+      .andThen(new InstantCommand(() -> this.move(-0.5)).withTimeout(staticTimeout))
+      .andThen(new InstantCommand(() -> this.stop()))
+      .andThen(new WaitCommand(2))
+      .andThen(new InstantCommand(() -> this.move(1)).withTimeout(dynamicTimeout))
+      .andThen(new InstantCommand(() -> this.move(-1)).withTimeout(dynamicTimeout))
+      .andThen(new InstantCommand(() -> this.stop()));
+  }
+
 }