diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index e5b44ea7..07a734a5 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.team2412.robot.commands.intake.AllInCommand; import frc.team2412.robot.commands.intake.AllReverseCommand; import frc.team2412.robot.commands.intake.AllStopCommand; @@ -72,6 +73,10 @@ public Controls(Subsystems s) { codriveIntakeReverseButton = codriveController.povLeft(); codriveIntakeRejectButton = codriveController.povDown(); + if (Robot.isSysIdMode() && LAUNCHER_ENABLED) { + bindSysIdControls(); + return; + } if (DRIVEBASE_ENABLED) { bindDrivebaseControls(); } @@ -175,6 +180,28 @@ private void bindLauncherControls() { driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(4000))); } + private void bindSysIdControls() { + // only one routine can be run in one robot log + // switch these between arm and flywheel in code when tuning + driveController + .leftBumper() + .whileTrue(s.launcherSubsystem.armSysIdQuasistatic(Direction.kForward)); + driveController + .rightBumper() + .whileTrue(s.launcherSubsystem.armSysIdQuasistatic(Direction.kReverse)); + driveController + .leftTrigger() + .whileTrue(s.launcherSubsystem.armSysIdDynamic(Direction.kForward)); + driveController + .rightTrigger() + .whileTrue(s.launcherSubsystem.armSysIdDynamic(Direction.kReverse)); + // switch these between angle and drive tests in code when tuning + driveController.x().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kForward)); + driveController.y().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kReverse)); + driveController.a().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kForward)); + driveController.b().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kReverse)); + } + public void vibrateDriveController(double vibration) { if (!DriverStation.isAutonomous()) { driveController.getHID().setRumble(RumbleType.kBothRumble, vibration); diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index c4f87026..c0df2eda 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -1,5 +1,6 @@ package frc.team2412.robot; +import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -40,6 +41,8 @@ public static Robot getInstance() { } private static final boolean debugMode = true; + // Really dangerous to keep this enabled as it disables all other controls, use with caution + private static final boolean sysIdMode = false; private final RobotType robotType; private final PowerDistribution PDP; @@ -148,6 +151,7 @@ public void autonomousInit() { @Override public void teleopInit() { Shuffleboard.startRecording(); + SignalLogger.start(); } @Override @@ -158,6 +162,7 @@ public void autonomousExit() { @Override public void teleopExit() { CommandScheduler.getInstance().cancelAll(); + SignalLogger.stop(); } @Override @@ -192,4 +197,8 @@ public boolean isCompetition() { public static boolean isDebugMode() { return debugMode; } + + public static boolean isSysIdMode() { + return sysIdMode; + } } diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 9705eee8..466f8342 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -11,6 +11,8 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; @@ -21,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.team2412.robot.Robot; import frc.team2412.robot.Robot.RobotType; import frc.team2412.robot.Subsystems.SubsystemConstants; @@ -30,6 +33,7 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; import swervelib.SwerveDrive; +import swervelib.SwerveModule; import swervelib.math.SwerveMath; import swervelib.parser.SwerveParser; import swervelib.telemetry.SwerveDriveTelemetry; @@ -319,4 +323,47 @@ private void initShuffleboard() { public SwerveDrive getSwerveDrive() { return swerveDrive; } + + private SysIdRoutine getDriveSysIdRoutine() { + return new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + for (SwerveModule module : swerveDrive.getModules()) { + module.getDriveMotor().setVoltage(volts.magnitude()); + module.setAngle(0); + } + }, + null, + this)); + } + + private SysIdRoutine getAngleSysIdRoutine() { + return new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + for (SwerveModule module : swerveDrive.getModules()) { + module.getAngleMotor().setVoltage(volts.magnitude()); + } + }, + null, + this)); + } + + public Command driveSysIdQuasistatic(SysIdRoutine.Direction direction) { + return getDriveSysIdRoutine().quasistatic(direction); + } + + public Command driveSysIdDynamic(SysIdRoutine.Direction direction) { + return getDriveSysIdRoutine().dynamic(direction); + } + + public Command angleSysIdQuasistatic(SysIdRoutine.Direction direction) { + return getAngleSysIdRoutine().quasistatic(direction); + } + + public Command angleSysIdDynamic(SysIdRoutine.Direction direction) { + return getAngleSysIdRoutine().dynamic(direction); + } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 000ec748..753ae2d0 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -10,11 +10,19 @@ import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.units.BaseUnits; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.team2412.robot.Hardware; import frc.team2412.robot.Robot; import frc.team2412.robot.util.SparkPIDWidget; @@ -52,10 +60,18 @@ public class LauncherSubsystem extends SubsystemBase { private final RelativeEncoder launcherBottomEncoder; private final SparkAbsoluteEncoder launcherAngleEncoder; private final SparkPIDController launcherAngleOnePIDController; - // private final SparkPIDController launcherAngleTwoPIDController; + + // arm FF values: + // Ks: 0.40434 + // Kv: 0.096771 + // Ka: 0.0056403 private final SparkPIDController launcherTopPIDController; private final SparkPIDController launcherBottomPIDController; + private final SimpleMotorFeedforward launcherTopFeedforward = + new SimpleMotorFeedforward(0, 0.11335, 0.048325); + private final SimpleMotorFeedforward launcherBottomFeedforward = + new SimpleMotorFeedforward(0, 0.11238, 0.048209); private double rpmSetpoint; private double angleSetpoint; @@ -129,26 +145,18 @@ public void configMotors() { launcherAngleTwoMotor.follow(launcherAngleOneMotor, true); // PID - launcherAngleOnePIDController.setP(5.0); + launcherAngleOnePIDController.setP(5.421); launcherAngleOnePIDController.setI(0); - launcherAngleOnePIDController.setD(0); - launcherAngleOnePIDController.setFF(0); + launcherAngleOnePIDController.setD(0.066248); launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); - // launcherAngleTwoPIDController.setP(0.1); - // launcherAngleTwoPIDController.setI(0); - // launcherAngleTwoPIDController.setD(0); - // launcherAngleTwoPIDController.setFF(0); - - launcherTopPIDController.setP(0.002); + launcherTopPIDController.setP(7.7633E-05); launcherTopPIDController.setI(0); - launcherTopPIDController.setD(0.001); - launcherTopPIDController.setFF(0); + launcherTopPIDController.setD(0); - launcherBottomPIDController.setP(0.002); + launcherBottomPIDController.setP(0.00011722); launcherBottomPIDController.setI(0); - launcherBottomPIDController.setD(0.001); - launcherBottomPIDController.setFF(0); + launcherBottomPIDController.setD(0); launcherAngleOneMotor.getEncoder().setPosition(launcherAngleEncoder.getPosition()); launcherAngleOneMotor.getEncoder().setPositionConversionFactor(PIVOT_GEARING_RATIO); @@ -169,18 +177,20 @@ public void stopLauncher() { // uses the value from the entry public void launch() { rpmSetpoint = setLauncherSpeedEntry.getDouble(SPEAKER_SHOOT_SPEED_RPM); - launcherTopPIDController.setReference(rpmSetpoint, ControlType.kVelocity); - launcherBottomPIDController.setReference(rpmSetpoint, ControlType.kVelocity); + launch(rpmSetpoint); } // used for presets public void launch(double speed) { rpmSetpoint = speed; - launcherTopPIDController.setReference(rpmSetpoint, ControlType.kVelocity); - launcherBottomPIDController.setReference(rpmSetpoint, ControlType.kVelocity); + launcherTopPIDController.setReference( + rpmSetpoint, ControlType.kVelocity, 0, launcherTopFeedforward.calculate(speed)); + launcherBottomPIDController.setReference( + rpmSetpoint, ControlType.kVelocity, 0, launcherBottomFeedforward.calculate(speed)); } public void ampLaunch(double speed) { - launcherTopPIDController.setReference(-speed, ControlType.kVelocity); + launcherTopPIDController.setReference( + -speed, ControlType.kVelocity, 0, launcherTopFeedforward.calculate(-speed)); launcherBottomMotor.disable(); } @@ -292,4 +302,88 @@ public void periodic() { launcherAngleSpeedEntry.setDouble(getAngleSpeed()); launcherIsAtSpeed.setBoolean(isAtSpeed()); } + + private SysIdRoutine getArmSysIdRoutine() { + return new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + launcherAngleOneMotor.setVoltage(volts.in(BaseUnits.Voltage)); + launcherAngleTwoMotor.setVoltage(volts.in(BaseUnits.Voltage)); + }, + (SysIdRoutineLog log) -> { + log.motor("angle-one") + .voltage( + BaseUnits.Voltage.of( + launcherAngleOneMotor.getAppliedOutput() + * RobotController.getBatteryVoltage())) + .angularPosition( + edu.wpi.first.units.Units.Rotations.of( + launcherAngleOneMotor.getEncoder().getPosition())) + .angularVelocity( + edu.wpi.first.units.Units.Rotations.per(edu.wpi.first.units.Units.Minute) + .of(launcherAngleOneMotor.getEncoder().getVelocity())); + log.motor("angle-two") + .voltage( + BaseUnits.Voltage.of( + launcherAngleTwoMotor.getAppliedOutput() + * RobotController.getBatteryVoltage())) + .angularPosition( + edu.wpi.first.units.Units.Rotations.of( + launcherAngleTwoMotor.getEncoder().getPosition())) + .angularVelocity( + edu.wpi.first.units.Units.Rotations.per(edu.wpi.first.units.Units.Minute) + .of(launcherAngleTwoMotor.getEncoder().getVelocity())); + }, + this)); + } + + private SysIdRoutine getFlywheelSysIdRoutine() { + return new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + launcherTopMotor.setVoltage(volts.in(BaseUnits.Voltage)); + launcherBottomMotor.setVoltage(volts.in(BaseUnits.Voltage)); + }, + (SysIdRoutineLog log) -> { + log.motor("top-flywheel") + .voltage( + BaseUnits.Voltage.of( + launcherTopMotor.getAppliedOutput() + * RobotController.getBatteryVoltage())) + .angularPosition( + edu.wpi.first.units.Units.Rotations.of(launcherTopEncoder.getPosition())) + .angularVelocity( + edu.wpi.first.units.Units.Rotations.per(edu.wpi.first.units.Units.Minute) + .of(launcherTopEncoder.getVelocity())); + log.motor("bottom-flywheel") + .voltage( + BaseUnits.Voltage.of( + launcherBottomMotor.getAppliedOutput() + * RobotController.getBatteryVoltage())) + .angularPosition( + edu.wpi.first.units.Units.Rotations.of(launcherBottomEncoder.getPosition())) + .angularVelocity( + edu.wpi.first.units.Units.Rotations.per(edu.wpi.first.units.Units.Minute) + .of(launcherBottomEncoder.getVelocity())); + }, + this)); + } + + public Command armSysIdQuasistatic(SysIdRoutine.Direction direction) { + return getArmSysIdRoutine().quasistatic(direction); + } + + public Command armSysIdDynamic(SysIdRoutine.Direction direction) { + return getArmSysIdRoutine().dynamic(direction); + } + + public Command flywheelSysIdQuasistatic(SysIdRoutine.Direction direction) { + return getFlywheelSysIdRoutine().quasistatic(direction); + } + + public Command flywheelSysIdDynamic(SysIdRoutine.Direction direction) { + return getFlywheelSysIdRoutine().dynamic(direction); + } } diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java index 7c7852d7..8067393b 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/AutoLogic.java @@ -81,7 +81,7 @@ public void registerCommands() { "MidSpeakerCenterLineN5N4N3", AutoPaths.midSpeakerCenterLineN3N2N1); NamedCommands.registerCommand( "LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4N3); - NamedCommands.registerCommand("LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4); + NamedCommands.registerCommand("LowSpeakerCenterLineN5N4", AutoPaths.lowSpeakerCenterLineN5N4); NamedCommands.registerCommand( "TopSpeakerCenterLineN1N2AutoLine1", AutoPaths.TopSpeakerCenterLineN1N2AutoLine1); NamedCommands.registerCommand( diff --git a/src/main/java/frc/team2412/robot/util/AutoPaths.java b/src/main/java/frc/team2412/robot/util/AutoPaths.java index 039c5fb4..e1dc7da2 100644 --- a/src/main/java/frc/team2412/robot/util/AutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/AutoPaths.java @@ -11,115 +11,152 @@ import frc.team2412.robot.subsystems.LauncherSubsystem; public class AutoPaths { + private static Command registerAuto( + String autoName, Command command, String... primaryPathNames) { + PathPlannerAutos.registerAuto(autoName, primaryPathNames); + return command.withName(autoName); + } // Test Auto - public static SequentialCommandGroup testAuto = - new SequentialCommandGroup( - AutoLogic.getAutoCommand("TestPath"), - new ConditionalCommand( - AutoLogic.getAutoCommand("TestPathTrue"), - AutoLogic.getAutoCommand("TestPathFalse"), - () -> false)); + public static Command testAuto = + registerAuto( + "AutoLogicTest", + new SequentialCommandGroup( + AutoLogic.getAutoCommand("TestPath"), + new ConditionalCommand( + AutoLogic.getAutoCommand("TestPathTrue"), + AutoLogic.getAutoCommand("TestPathFalse"), + () -> false)), + "TestPath", + "TestPathTrue"); // Complex Autos public static Command TopSpeakerCenterLineN1N2AutoLine1 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("TopSpeakerQCenterLineN1"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN1LCenterLineN1"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN1QCenterLineN2")), - Commands.sequence( - getAutoCommand("QCenterLineN1QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN2LAutoLineN1")), - Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")), - () -> true)), - () -> true), - VisionLaunchCommand()); + registerAuto( + "TopSpeakerCenterLineN1N2AutoLine1", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("TopSpeakerQCenterLineN1"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN1LCenterLineN1"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN1QCenterLineN2")), + Commands.sequence( + getAutoCommand("QCenterLineN1QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN2LAutoLineN1")), + Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")), + () -> true)), + () -> true), + VisionLaunchCommand()), + "TopSpeakerQCenterLineN1", + "QCenterLineN1LCenterLineN1", + "LCenterLineN1QCenterLineN2"); public static Command TopSpeakerCenterLineN1N2N3 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("TopSpeakerQCenterLineN1"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN1LCenterLineN1"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN1QCenterLineN2")), - Commands.sequence( - getAutoCommand("QCenterLineN1QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN2LCenterLineN3")), - Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")), - () -> true)), - () -> true), - VisionLaunchCommand()); + registerAuto( + "TopSpeakerCenterLineN1N2N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("TopSpeakerQCenterLineN1"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN1LCenterLineN1"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN1QCenterLineN2")), + Commands.sequence( + getAutoCommand("QCenterLineN1QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN2LCenterLineN3")), + Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")), + () -> true)), + () -> true), + VisionLaunchCommand()), + "TopSpeakerQCenterLineN1", + "QCenterLineN1LCenterLineN1", + "LCenterLineN1QCenterLineN2"); public static Command midSpeakerCenterLineN3N2N1 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("MidSpeakerQCenterLineN3"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN3LCenterLineN3"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN3QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("N3QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("N3LCenterLineN2LCenterLineN1")), - getAutoCommand("QCenterLineN2LCenterLineN1"), - () -> true)), - Commands.sequence( - getAutoCommand("QCenterLineN3QCenterLineN2"), - getAutoCommand("QCenterLineN2LCenterLineN2")), - () -> true), - VisionLaunchCommand()); + registerAuto( + "MidSpeakerCenterLineN5N4N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("MidSpeakerQCenterLineN3"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN3LCenterLineN3"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN3QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("N3QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("N3LCenterLineN2LCenterLineN1")), + getAutoCommand("QCenterLineN2LCenterLineN1"), + () -> true)), + Commands.sequence( + getAutoCommand("QCenterLineN3QCenterLineN2"), + getAutoCommand("QCenterLineN2LCenterLineN2")), + () -> true), + VisionLaunchCommand()), + "MidSpeakerQCenterLineN3", + "QCenterLineN3LCenterLineN3", + "LCenterLineN3QCenterLineN2", + "N3QCenterLineN2LCenterLineN2", + "N3LCenterLineN2LCenterLineN1"); public static Command lowSpeakerCenterLineN5N4 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("LowSpeakerQCenterLineN5"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN5LCenterLineN5"), - Commands.waitSeconds(0.5), - getAutoCommand("LCenterLineN5LCenterLineN4")), - Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), - () -> false)); + registerAuto( + "LowSpeakerCenterLineN5N4", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("LowSpeakerQCenterLineN5"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN5LCenterLineN5"), + Commands.waitSeconds(0.5), + getAutoCommand("LCenterLineN5LCenterLineN4")), + Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), + () -> false)), + "LowSpeakerQCenterLineN5", + "QCenterLineN5LCenterLineN5", + "LCenterLineN5LCenterLineN4"); public static Command lowSpeakerCenterLineN5N4N3 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("LowSpeakerQCenterLineN5"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN5QCenterLineN4"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN4LCenterLineN4"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN4LCenterLineN3")), - Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")), - () -> true)), - Commands.sequence( - getAutoCommand("QCenterLineN5LCenterLineN4"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN4LCenterLineN3")), - () -> false), - VisionLaunchCommand()); + registerAuto( + "LowSpeakerCenterLineN5N4N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("LowSpeakerQCenterLineN5"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN5QCenterLineN4"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN4LCenterLineN4"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN4LCenterLineN3")), + Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")), + () -> true)), + Commands.sequence( + getAutoCommand("QCenterLineN5LCenterLineN4"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN4LCenterLineN3")), + () -> false), + VisionLaunchCommand()), + "LowSpeakerQCenterLineN5", + "QCenterLineN5QCenterLineN4", + "QCenterLineN4LCenterLineN4", + "LCenterLineN4LCenterLineN3"); // new command getters diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 9496cfda..451dba90 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -5,7 +5,9 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -39,7 +41,19 @@ public static void configureShuffleboardTab( .subscribe(""); var autonomousField = new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); - addPeriodic.accept(() -> autonomousField.update(activeAutoSub.get()), UPDATE_RATE); + var watchdog = + new Watchdog(0.001, () -> DriverStation.reportWarning("auto field loop overrun", false)); + addPeriodic.accept( + () -> { + watchdog.reset(); + autonomousField.update(activeAutoSub.get()); + watchdog.addEpoch("AutonomousField.update()"); + watchdog.disable(); + if (watchdog.isExpired()) { + watchdog.printEpochs(); + } + }, + UPDATE_RATE); tab.add("Selected auto", autonomousField.getField()) .withPosition(columnIndex, rowIndex) .withSize(2, 2); @@ -49,6 +63,7 @@ public static void configureShuffleboardTab( private final Field2d field = new Field2d(); // Keeping track of the current trajectory + private PathPlannerAutos.Auto auto; private List trajectories; private int trajectoryIndex = 0; @@ -92,12 +107,16 @@ public Pose2d getUpdatedPose(String autoName) { double fpgaTime = Timer.getFPGATimestamp(); if (lastName.isEmpty() || !lastName.get().equals(autoName)) { lastName = Optional.of(autoName); - trajectories = PathPlannerAutos.getAutoTrajectories(autoName); + auto = PathPlannerAutos.getAuto(autoName); + trajectories = auto.trajectories; trajectoryIndex = 0; lastFPGATime = fpgaTime; lastTrajectoryTimeOffset = 0; } if (trajectories.isEmpty()) { + if (auto.startingPose != null) { + return auto.startingPose; + } return new Pose2d(); } lastTrajectoryTimeOffset += (fpgaTime - lastFPGATime) * speed; @@ -115,11 +134,16 @@ public Pose2d getUpdatedPose(String autoName) { } /** - * Updates the {@link Field2d} robot pose. + * Updates the {@link Field2d} robot pose. If the robot is enabled, does nothing and the + * trajectory will restart when the robot is disabled. * * @param autoName The name of the selected PathPlanner autonomous routine. */ public void update(String autoName) { + if (DriverStation.isEnabled()) { + lastName = Optional.empty(); + return; + } field.setRobotPose(getUpdatedPose(autoName)); } } diff --git a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java index c9c77f96..f5d924bc 100644 --- a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java +++ b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java @@ -4,6 +4,7 @@ import com.fasterxml.jackson.databind.ObjectMapper; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.PathPlannerTrajectory; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; @@ -17,6 +18,37 @@ import java.util.function.Function; public class PathPlannerAutos { + public static final class Auto { + public final Pose2d startingPose; + public final List trajectories; + + public Auto() { + this.startingPose = null; + this.trajectories = List.of(); + } + + public Auto(Pose2d startingPose, List trajectories) { + this.startingPose = startingPose; + this.trajectories = trajectories; + } + } + + // Registering named commands + + private static final Map> namedCommandPathsCache = new HashMap<>(); + + public static void registerAuto(String autoName, List paths) { + namedCommandPathsCache.put(autoName, paths); + } + + public static void registerAuto(String autoName, String... pathNames) { + List paths = new ArrayList<>(pathNames.length); + for (String pathName : pathNames) { + paths.add(getPathPlannerPath(pathName)); + } + registerAuto(autoName, paths); + } + // Individual paths private static PathPlannerPath loadPathPlannerPath(String name) { @@ -47,7 +79,8 @@ private static void addPathsFromCommand( case "wait": break; case "named": - // TODO Handle conditional paths (after those are added) + String commandName = commandJson.get("data").get("name").asText(); + paths.addAll(namedCommandPathsCache.getOrDefault(commandName, List.of())); break; case "path": String pathName = commandJson.get("data").get("pathName").asText(); @@ -69,10 +102,6 @@ private static List getPaths(JsonNode autoJson) { return paths; } - private static Rotation2d getStartingRotation(JsonNode autoJson) { - return Rotation2d.fromDegrees(autoJson.get("startingPose").get("rotation").asDouble()); - } - private static ChassisSpeeds speedsFromState(PathPlannerTrajectory.State state) { return new ChassisSpeeds( state.heading.getCos() * state.velocityMps, @@ -80,23 +109,14 @@ private static ChassisSpeeds speedsFromState(PathPlannerTrajectory.State state) state.headingAngularVelocityRps); } - private static List loadAutoTrajectories(String autoName) { - File autoFile = - new File(Filesystem.getDeployDirectory(), "pathplanner/autos/" + autoName + ".auto"); - if (!autoFile.exists()) { - DriverStation.reportWarning( - "Attempted to load non-existent auto \"" + autoName + "\"", false); + private static List trajectoriesFromPaths( + List paths, Rotation2d startingRotation) { + if (paths.isEmpty()) { return List.of(); } - JsonNode autoJson; - try { - autoJson = new ObjectMapper().readTree(autoFile); - } catch (IOException e) { - DriverStation.reportWarning("Could not load auto \"" + autoName + "\"", e.getStackTrace()); - return List.of(); + if (startingRotation == null) { + startingRotation = paths.get(0).getPreviewStartingHolonomicPose().getRotation(); } - List paths = getPaths(autoJson); - Rotation2d startingRotation = getStartingRotation(autoJson); ChassisSpeeds startingSpeeds = new ChassisSpeeds(); List trajectories = new ArrayList<>(paths.size()); for (var path : paths) { @@ -108,11 +128,44 @@ private static List loadAutoTrajectories(String autoName) return List.copyOf(trajectories); } - private static final Map> autoTrajectoriesCache = - new HashMap<>(); + private static Pose2d getStartingPose(JsonNode autoJson) { + JsonNode startingPose = autoJson.get("startingPose"); + if (startingPose.isNull()) { + return null; + } + JsonNode translation = startingPose.get("position"); + return new Pose2d( + translation.get("x").asDouble(), + translation.get("y").asDouble(), + Rotation2d.fromDegrees(startingPose.get("rotation").asDouble())); + } + + private static Auto loadAuto(String autoName) { + File autoFile = + new File(Filesystem.getDeployDirectory(), "pathplanner/autos/" + autoName + ".auto"); + if (!autoFile.exists()) { + DriverStation.reportWarning( + "Attempted to load non-existent auto \"" + autoName + "\"", false); + return new Auto(); + } + JsonNode autoJson; + try { + autoJson = new ObjectMapper().readTree(autoFile); + } catch (IOException e) { + DriverStation.reportWarning("Could not load auto \"" + autoName + "\"", e.getStackTrace()); + return new Auto(); + } + Pose2d startingPose = getStartingPose(autoJson); + Rotation2d startingRotation = startingPose == null ? null : startingPose.getRotation(); + List trajectories = + trajectoriesFromPaths(getPaths(autoJson), startingRotation); + return new Auto(startingPose, trajectories); + } + + private static final Map autosCache = new HashMap<>(); - public static List getAutoTrajectories(String autoName) { - return autoTrajectoriesCache.computeIfAbsent(autoName, PathPlannerAutos::loadAutoTrajectories); + public static Auto getAuto(String autoName) { + return autosCache.computeIfAbsent(autoName, PathPlannerAutos::loadAuto); } // Pre-loading everything @@ -157,10 +210,6 @@ public static void loadAllChoreoTrajectories() { } public static void loadAllAutoTrajectories() { - loadAll( - "pathplanner/autos", - ".auto", - autoTrajectoriesCache, - PathPlannerAutos::loadAutoTrajectories); + loadAll("pathplanner/autos", ".auto", autosCache, PathPlannerAutos::loadAuto); } }