From fee598db0a77a00f8c2f38904ab66412c6559ed7 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 8 Oct 2024 20:44:41 -0400 Subject: [PATCH] Start implementing tuning-aimer test mode --- src/main/java/frc/robot/RobotContainer.java | 134 ++++++++++++++++++ .../robot/commands/DriveWithJoysticks.java | 2 +- .../frc/robot/constants/ScoringConstants.java | 4 + 3 files changed, 139 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b4861dc..8501326 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,8 @@ package frc.robot; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -14,6 +16,7 @@ import frc.robot.constants.ModeConstants; import frc.robot.constants.PhoenixDriveConstants; import frc.robot.constants.PhoenixDriveConstants.AlignTarget; +import frc.robot.constants.ScoringConstants; import frc.robot.constants.VisionConstants; import frc.robot.subsystems.drive.PhoenixDrive; import frc.robot.subsystems.drive.PhoenixDrive.SysIdRoutineType; @@ -49,8 +52,11 @@ public class RobotContainer { VisionLocalizer tagVision; + SendableChooser testModeChooser = new SendableChooser(); + public RobotContainer() { configureSubsystems(); + configureModes(); configureBindings(); } @@ -163,6 +169,14 @@ private void initScoring() { } } + private void configureModes() { + testModeChooser.setDefaultOption("Blank", "tuning"); + + testModeChooser.setDefaultOption("Aimer Tunig", "tuning-aimer"); + + SmartDashboard.putData("Test Mode Chooser", testModeChooser); + } + private void configureBindings() { if (drive != null) { drive.registerTelemetry(logger::telemeterize); @@ -315,6 +329,126 @@ public void enabledInit() { scoringSubsystem.setAction(ScoringAction.WAIT); } + public void testInit() { + // Reset bindings + masher = new CommandXboxController(2); + + switch (testModeChooser.getSelected()) { + case "tuning": + break; + case "tuning-aimer": + SmartDashboard.putNumber("Test-Mode/aimer/kP", ScoringConstants.aimerkP); + SmartDashboard.putNumber("Test-Mode/aimer/kI", ScoringConstants.aimerkI); + SmartDashboard.putNumber("Test-Mode/aimer/kD", ScoringConstants.aimerkD); + + SmartDashboard.putNumber( + "Test-Mode/aimer/profileMaxVelocity", ScoringConstants.aimerCruiseVelocity); + SmartDashboard.putNumber( + "Test-Mode/aimer/profileMaxAcceleration", + ScoringConstants.aimerAcceleration); + + SmartDashboard.putNumber("Test-Mode/aimer/setpointPosition", 0.25); + SmartDashboard.putNumber("Test-Mode/aimer/volts", 2.0); + + scoringSubsystem.setAction(ScoringAction.OVERRIDE); + + // TODO: Add Tunables to coppercore! + // masher.a() + // .onTrue(new TuneS(scoringSubsystem, 0)); + + // masher.b() + // .onTrue(new TuneG(scoringSubsystem, 0)); + + masher.y() + .onTrue( + new InstantCommand( + () -> + scoringSubsystem.setPID( + SmartDashboard.getNumber( + "Test-Mode/aimer/kP", + ScoringConstants.aimerkP), + SmartDashboard.getNumber( + "Test-Mode/aimer/kI", + ScoringConstants.aimerkI), + SmartDashboard.getNumber( + "Test-Mode/aimer/kD", + ScoringConstants.aimerkD), + 0))) + .onTrue( + new InstantCommand( + () -> + scoringSubsystem.setMaxProfileProperties( + SmartDashboard.getNumber( + "Test-Mode/aimer/profileMaxVelocity", + ScoringConstants + .aimerCruiseVelocity), + SmartDashboard.getNumber( + "Test-Mode/aimer/profileMaxAcceleration", + ScoringConstants.aimerAcceleration), + 0))) + .onTrue( + new InstantCommand( + () -> + scoringSubsystem.runToPosition( + SmartDashboard.getNumber( + "Test-Mode/aimer/setpointPosition", + 0.25), + 0))) + .onTrue( + new InstantCommand( + () -> + scoringSubsystem.setAction( + ScoringAction.TEMPORARY_SETPOINT))) + .onFalse( + new InstantCommand( + () -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + // TODO: Figure out which of these we need + // masher.povUp() + // .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(1.1, 0))) + // .onTrue( + // new InstantCommand( + // () -> + // scoringSubsystem.setAction( + // ScoringAction.TEMPORARY_SETPOINT))) + // .onFalse( + // new InstantCommand( + // () -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + // masher.povDown() + // .onTrue(new InstantCommand(() -> scoringSubsystem.runToPosition(0.0, 0))) + // .onTrue( + // new InstantCommand( + // () -> + // scoringSubsystem.setAction( + // ScoringAction.TEMPORARY_SETPOINT))) + // .onFalse( + // new InstantCommand( + // () -> scoringSubsystem.setAction(ScoringAction.OVERRIDE))); + + // masher.leftBumper() + // .onTrue( + // new InstantCommand( + // () -> + // scoringSubsystem.setVolts( + // SmartDashboard.getNumber( + // "Test-Mode/aimer/volts", 2.0), + // 0))) + // .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 0))); + + // masher.rightBumper() + // .onTrue( + // new InstantCommand( + // () -> + // scoringSubsystem.setVolts( + // -SmartDashboard.getNumber( + // "Test-Mode/aimer/volts", 2.0), + // 0))) + // .onFalse(new InstantCommand(() -> scoringSubsystem.setVolts(0, 0))); + break; + } + } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } diff --git a/src/main/java/frc/robot/commands/DriveWithJoysticks.java b/src/main/java/frc/robot/commands/DriveWithJoysticks.java index e55fdae..07fbc18 100644 --- a/src/main/java/frc/robot/commands/DriveWithJoysticks.java +++ b/src/main/java/frc/robot/commands/DriveWithJoysticks.java @@ -31,7 +31,7 @@ public class DriveWithJoysticks extends Command { private ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - private Deadband joystickDeadband = new Deadband(); + // private Deadband joystickDeadband = new Deadband(); public DriveWithJoysticks( PhoenixDrive drivetrain, CommandJoystick leftJoystick, CommandJoystick rightJoystick) { diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index e1823a0..5d425c9 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -25,6 +25,10 @@ public class ScoringConstants { public static final int kickerMotorId = 13; + // TODO: REPLACE THIS WHEN THE ACTUAL SHOOTER IO IS MERGED + public static final int shooterLeftMotorId = -1; + public static final int shooterRightMotorId = -1; + public static final double shooterCurrentLimit = 120; public static final double kickerCurrentLimit = 120; public static final double aimerCurrentLimit = 40;