Skip to content

Commit

Permalink
Start implementing tuning-aimer test mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@aidnem
aidnem committed Oct 9, 2024
1 parent a535ad8 commit fee598d
Show file tree
Hide file tree
Showing 3 changed files with 139 additions and 1 deletion.
134 changes: 134 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -49,8 +52,11 @@ public class RobotContainer {

VisionLocalizer tagVision;

SendableChooser<String> testModeChooser = new SendableChooser<String>();

public RobotContainer() {
configureSubsystems();
configureModes();
configureBindings();
}

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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");
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/DriveWithJoysticks.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/constants/ScoringConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit fee598d

Please sign in to comment.