diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7d9731e6..fe538532 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,9 +21,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; -import frc.robot.commands.auto.TestAutos; import frc.robot.subsystems.apriltagvision.AprilTagVision; import frc.robot.subsystems.apriltagvision.AprilTagVisionConstants; import frc.robot.subsystems.apriltagvision.AprilTagVisionIONorthstar; @@ -161,8 +159,6 @@ public RobotContainer() { .beforeStarting(() -> shooter.setCharacterizing(true)) .finallyDo(() -> shooter.setCharacterizing(false))); - autoChooser.addOption("4 Note Davis", TestAutos.fourNoteDavis(drive, intake)); - // Testing autos paths Function> trajectoryCommandFactory = trajectoryFile -> { @@ -170,8 +166,7 @@ public RobotContainer() { return trajectory.map( traj -> Commands.runOnce( - () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose())), - drive) + () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) .andThen(drive.setTrajectoryCommand(traj))); }; final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); @@ -192,12 +187,12 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); + // drive.setDefaultCommand( + // DriveCommands.joystickDrive( + // drive, + // () -> -controller.getLeftY(), + // () -> -controller.getLeftX(), + // () -> -controller.getRightX())); // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); controller .a() diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index f459307e..85dc9a35 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -18,7 +18,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.FieldConstants; import frc.robot.RobotState; import frc.robot.subsystems.drive.Drive; @@ -62,10 +61,12 @@ public static Command joystickDrive( ChassisSpeeds speeds = new ChassisSpeeds( - linearVelocity.getX() * DriveConstants.drivetrainConfig.maxLinearVelocity(), - linearVelocity.getY() * DriveConstants.drivetrainConfig.maxLinearVelocity(), - omega * DriveConstants.drivetrainConfig.maxAngularVelocity()); - drive.setDriveInputSpeeds(speeds); + linearVelocity.getX() * DriveConstants.driveConfig.maxLinearVelocity(), + linearVelocity.getY() * DriveConstants.driveConfig.maxLinearVelocity(), + omega * DriveConstants.driveConfig.maxAngularVelocity()); + drive.setDriveVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, RobotState.getInstance().getEstimatedPose().getRotation())); }); } @@ -81,10 +82,10 @@ public static Command joystickDrive( return shotData.goalHeading(); }; - public static Command toggleCalculateShotWhileMovingRotation(Drive drive) { - return Commands.either( - drive.disableHeadingCommand(), // turn off - drive.setHeadingCommand(shotRotation), // turn on - drive.getMotionPlanner()::isHeadingControlled); - } + // public static Command toggleCalculateShotWhileMovingRotation(Drive drive) { + // return Commands.either( + // drive.disableHeadingCommand(), // turn off + // drive.setHeadingCommand(shotRotation), // turn on + // drive.getAutoMotionPlanner()::isHeadingControlled); + // } } diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index b369f065..20e0667d 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.java @@ -7,19 +7,18 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.FieldConstants; import frc.robot.RobotState; -import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.superstructure.ShotCalculator; import frc.robot.subsystems.superstructure.intake.Intake; import frc.robot.util.AllianceFlipUtil; import java.util.function.Supplier; public class AutoCommands { - public static Command moveWhileShooting(Drive drive) { - return drive - .setHeadingCommand(AutoCommands::calculateShootHeading) - .andThen(Commands.waitSeconds(0.7)) - .andThen(drive.disableHeadingCommand()); - } + // public static Command moveWhileShooting(Drive drive) { + // return drive + // .setHeadingCommand(AutoCommands::calculateShootHeading) + // .andThen(Commands.waitSeconds(0.7)) + // .andThen(drive.disableHeadingCommand()); + // } private static Rotation2d calculateShootHeading() { Twist2d fieldVel = RobotState.getInstance().fieldVelocity(); diff --git a/src/main/java/frc/robot/commands/auto/TestAutos.java b/src/main/java/frc/robot/commands/auto/TestAutos.java index ca9f6273..2bf27691 100644 --- a/src/main/java/frc/robot/commands/auto/TestAutos.java +++ b/src/main/java/frc/robot/commands/auto/TestAutos.java @@ -20,7 +20,7 @@ public class TestAutos { private static final LoggedTunableNumber intakeDistance = new LoggedTunableNumber( - "Auto/intakeDistance", DriveConstants.drivetrainConfig.trackwidthX() + 0.1); + "Auto/intakeDistance", DriveConstants.driveConfig.trackwidthX() + 0.1); // bottom to top private static AutoCommands.CircularRegion spikeIntakeRegion(int i) { @@ -53,20 +53,20 @@ public static Command fourNoteDavis(Drive drive, Intake intake) { new AutoCommands.RectangularRegion(new Translation2d(3.83, 5.99), 0.1, 0.5); var fourthShotRegion = new AutoCommands.RectangularRegion(new Translation2d(4.08, 7.18), 0.1, 0.5); - Command sequenceShots = - Commands.sequence( - AutoCommands.moveWhileShooting(drive), - AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(secondShotRegion)), - AutoCommands.moveWhileShooting(drive), - AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(thirdShotRegion)), - AutoCommands.moveWhileShooting(drive), - AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(fourthShotRegion)), - AutoCommands.moveWhileShooting(drive)); + // Command sequenceShots = + // Commands.sequence( + // AutoCommands.moveWhileShooting(drive), + // AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(secondShotRegion)), + // AutoCommands.moveWhileShooting(drive), + // AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(thirdShotRegion)), + // AutoCommands.moveWhileShooting(drive), + // AutoCommands.waitForRegion(() -> AllianceFlipUtil.apply(fourthShotRegion)), + // AutoCommands.moveWhileShooting(drive)); return Commands.sequence( resetPoseCommand(trajectory.getStartPose()), drive.setTrajectoryCommand(trajectory), - Commands.parallel(sequenceIntake, sequenceShots)); + Commands.parallel(sequenceIntake)); } private static Command resetPoseCommand(Pose2d pose) { diff --git a/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java new file mode 100644 index 00000000..5933f533 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java @@ -0,0 +1,172 @@ +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; +import static frc.robot.util.trajectory.HolonomicDriveController.HolonomicDriveState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.Constants; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.LoggedTunableNumber; +import frc.robot.util.trajectory.HolonomicDriveController; +import frc.robot.util.trajectory.Trajectory; +import java.util.Arrays; +import org.littletonrobotics.junction.Logger; + +public class AutoMotionPlanner { + private static LoggedTunableNumber trajectoryLinearKp = + new LoggedTunableNumber("Trajectory/linearKp", trajectoryConstants.linearKp()); + private static LoggedTunableNumber trajectoryLinearKd = + new LoggedTunableNumber("Trajectory/linearKd", trajectoryConstants.linearKd()); + private static LoggedTunableNumber trajectoryThetaKp = + new LoggedTunableNumber("Trajectory/thetaKp", trajectoryConstants.thetaKp()); + private static LoggedTunableNumber trajectoryThetaKd = + new LoggedTunableNumber("Trajectory/thetaKd", trajectoryConstants.thetaKd()); + private static LoggedTunableNumber trajectoryLinearTolerance = + new LoggedTunableNumber( + "Trajectory/controllerLinearTolerance", trajectoryConstants.linearTolerance()); + private static LoggedTunableNumber trajectoryThetaTolerance = + new LoggedTunableNumber( + "Trajectory/controllerThetaTolerance", trajectoryConstants.thetaTolerance()); + private static LoggedTunableNumber trajectoryGoalLinearTolerance = + new LoggedTunableNumber( + "Trajectory/goalLinearTolerance", trajectoryConstants.goalLinearTolerance()); + private static LoggedTunableNumber trajectoryGoalThetaTolerance = + new LoggedTunableNumber( + "Trajectory/goalThetaTolerance", trajectoryConstants.goalThetaTolerance()); + private static LoggedTunableNumber trajectoryLinearVelocityTolerance = + new LoggedTunableNumber( + "Trajectory/goalLinearVelocityTolerance", trajectoryConstants.linearVelocityTolerance()); + private static LoggedTunableNumber trajectoryAngularVelocityTolerance = + new LoggedTunableNumber( + "Trajectory/goalAngularVelocityTolerance", + trajectoryConstants.angularVelocityTolerance()); + + private Trajectory trajectory = null; + private Double startTime = null; + private Double currentTime = null; + private final HolonomicDriveController controller; + + public AutoMotionPlanner() { + controller = + new HolonomicDriveController( + trajectoryLinearKp.get(), + trajectoryLinearKd.get(), + trajectoryThetaKp.get(), + trajectoryThetaKd.get()); + configTrajectoryTolerances(); + } + + protected void setTrajectory(Trajectory trajectory) { + // Only set if not following trajectory or done with current one + if (this.trajectory == null || isFinished()) { + this.trajectory = trajectory; + controller.setGoalState(trajectory.getEndState()); + controller.resetControllers(); + startTime = null; + currentTime = null; + // Log poses + Logger.recordOutput( + "Trajectory/trajectoryPoses", + Arrays.stream(trajectory.getTrajectoryPoses()) + .map(AllianceFlipUtil::apply) + .toArray(Pose2d[]::new)); + } + } + + /** Output setpoint chassis speeds in */ + protected ChassisSpeeds update(double timestamp, Pose2d robot, Twist2d fieldVelocity) { + System.out.println(timestamp + " " + robot.toString() + " " + fieldVelocity.toString()); + // If disabled reset everything and stop + if (DriverStation.isDisabled()) { + trajectory = null; + // Stop logs + Logger.recordOutput("Trajectory/trajectoryPoses", new Pose2d[] {}); + Logger.recordOutput("Trajectory/setpointPose", new Pose2d()); + Logger.recordOutput("Trajectory/speeds", new double[] {}); + return new ChassisSpeeds(); + } + + // Tunable numbers + if (Constants.tuningMode) { + // Update PID Controllers + LoggedTunableNumber.ifChanged( + hashCode(), + pid -> controller.setPID(pid[0], pid[1], pid[2], pid[3]), + trajectoryLinearKp, + trajectoryLinearKd, + trajectoryThetaKp, + trajectoryThetaKp); + // Tolerances + LoggedTunableNumber.ifChanged( + hashCode(), + this::configTrajectoryTolerances, + trajectoryLinearTolerance, + trajectoryThetaTolerance, + trajectoryGoalLinearTolerance, + trajectoryGoalThetaTolerance, + trajectoryLinearVelocityTolerance, + trajectoryAngularVelocityTolerance); + } + + // Reached end + if (isFinished() || trajectory == null) { + // Stop logs + Logger.recordOutput("Trajectory/trajectoryPoses", new Pose2d[] {}); + Logger.recordOutput("Trajectory/setpointPose", new Pose2d()); + Logger.recordOutput("Trajectory/speeds", new double[] {}); + return new ChassisSpeeds(); + } + + if (startTime == null) { + startTime = timestamp; + } + currentTime = timestamp; + + HolonomicDriveState currentState = + new HolonomicDriveState(robot, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); + // Sample and flip state + HolonomicDriveState setpointState = trajectory.sample(currentTime - startTime); + setpointState = AllianceFlipUtil.apply(setpointState); + // calculate trajectory speeds + ChassisSpeeds speeds = controller.calculate(currentState, setpointState); + + // Log trajectory data + Logger.recordOutput("Trajectory/setpointPose", setpointState.pose()); + Logger.recordOutput( + "Trajectory/translationError", controller.getPoseError().getTranslation().getNorm()); + Logger.recordOutput( + "Trajectory/rotationError", controller.getPoseError().getRotation().getRadians()); + return speeds; + } + + protected boolean isFinished() { + return trajectory != null + && currentTime != null + && startTime != null + && currentTime - startTime >= trajectory.getDuration() + && controller.atGoal(); + } + + private void configTrajectoryTolerances() { + controller.setControllerTolerance( + new Pose2d( + new Translation2d( + trajectoryLinearTolerance.get(), Rotation2d.fromRadians(Math.PI / 4.0)), + Rotation2d.fromRadians(trajectoryThetaTolerance.get()))); + double velocityXTolerance = trajectoryLinearVelocityTolerance.get() / Math.sqrt(2.0); + controller.setGoalTolerance( + new HolonomicDriveState( + new Pose2d( + new Translation2d( + trajectoryGoalLinearTolerance.get(), Rotation2d.fromRadians(Math.PI / 4.0)), + Rotation2d.fromRadians(trajectoryGoalThetaTolerance.get())), + velocityXTolerance, + velocityXTolerance, // Same value + trajectoryAngularVelocityTolerance.get())); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 53920bcf..3b2fa42d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -16,29 +16,32 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.*; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import frc.robot.util.GeomUtil; +import frc.robot.util.swerve.ModuleLimits; +import frc.robot.util.swerve.SwerveSetpoint; +import frc.robot.util.swerve.SwerveSetpointGenerator; import frc.robot.util.trajectory.Trajectory; import java.util.*; import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import java.util.function.Supplier; import java.util.stream.IntStream; -import lombok.Getter; -import lombok.experimental.Delegate; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { - private interface MotionPlannerDelegate { - void setDriveInputSpeeds(ChassisSpeeds speeds); + + private enum ControlMode { + TRAJECTORY_FOLLOWING, + DRIVER_INPUT } public static final Lock odometryLock = new ReentrantLock(); @@ -49,9 +52,21 @@ private interface MotionPlannerDelegate { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; - @Getter - @Delegate(types = MotionPlannerDelegate.class) - private final DriveMotionPlanner motionPlanner; + private ControlMode currentControlMode = ControlMode.DRIVER_INPUT; + private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); + private final AutoMotionPlanner autoMotionPlanner; + + private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; + private SwerveSetpoint currentSetpoint = + new SwerveSetpoint( + new ChassisSpeeds(), + new SwerveModuleState[] { + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() + }); + private SwerveSetpointGenerator setpointGenerator; private boolean characterizing = false; private double characterizationVolts = 0.0; @@ -64,22 +79,29 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) modules[2] = new Module(bl, 2); modules[3] = new Module(br, 3); - motionPlanner = new DriveMotionPlanner(); + setpointGenerator = + SwerveSetpointGenerator.builder() + .kinematics(DriveConstants.kinematics) + .moduleLocations(DriveConstants.moduleTranslations) + .build(); + autoMotionPlanner = new AutoMotionPlanner(); } public void periodic() { // Update & process inputs odometryLock.lock(); - // Read timestamps from odometry thread + + // Read timestamps from odometry thread and fake sim timestamps double[] timestamps = timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); - // fake sim timestamps if (timestamps.length == 0) { timestamps = new double[] {Timer.getFPGATimestamp()}; } timestampQueue.clear(); + // Read inputs from gyro and modules gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); + Arrays.stream(modules).forEach(Module::updateInputs); odometryLock.unlock(); @@ -121,33 +143,65 @@ public void periodic() { : robotRelativeVelocity.omegaRadiansPerSecond; RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); - // Get motion planner output - SwerveModuleState[] setpointStates = - motionPlanner.update( - Timer.getFPGATimestamp(), - RobotState.getInstance().getEstimatedPose(), - RobotState.getInstance().fieldVelocity()); - - SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; - // Run robot - if (!characterizing) { - SwerveDriveKinematics.desaturateWheelSpeeds( - setpointStates, DriveConstants.drivetrainConfig.maxLinearVelocity()); - for (int i = 0; i < modules.length; i++) { - // Optimize setpoints - optimizedSetpointStates[i] = - SwerveModuleState.optimize(setpointStates[i], modules[i].getAngle()); - // Run modules - modules[i].runSetpoint(optimizedSetpointStates[i]); - } - // Log setpoint states - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); - } else { + if (characterizing) { // Run characterization for (Module module : modules) { module.runCharacterization(characterizationVolts); } + + Logger.recordOutput("Drive/SwerveStates/Desired(b4 Poofs)", new double[] {}); + Logger.recordOutput("Drive/SwerveStates/Setpoints", new double[] {}); + Logger.recordOutput("Drive/DesiredSpeeds", new double[] {}); + Logger.recordOutput("Drive/SetpointSpeeds", new double[] {}); + return; + } + + if (currentControlMode != null) { + // Update setpoint + switch (currentControlMode) { + case TRAJECTORY_FOLLOWING -> + desiredSpeeds = + autoMotionPlanner.update( + Timer.getFPGATimestamp(), + RobotState.getInstance().getEstimatedPose(), + RobotState.getInstance().fieldVelocity()); + case DRIVER_INPUT -> { + // set in runVelocity method + } + } + } + + // Run robot at speeds + // account for skew + desiredSpeeds = ChassisSpeeds.discretize(desiredSpeeds, 0.02); + // generate feasible next setpoint + currentSetpoint = + setpointGenerator.generateSetpoint( + currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02); + + // run modules + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; + for (int i = 0; i < modules.length; i++) { + // Optimize setpoints + optimizedSetpointStates[i] = + SwerveModuleState.optimize(currentSetpoint.moduleStates()[i], modules[i].getAngle()); + modules[i].runSetpoint(optimizedSetpointStates[i]); + } + + // Log chassis speeds and swerve states + Logger.recordOutput( + "Drive/SwerveStates/Desired(b4 Poofs)", + DriveConstants.kinematics.toSwerveModuleStates(desiredSpeeds)); + Logger.recordOutput("Drive/SwerveStates/Setpoints", optimizedSetpointStates); + Logger.recordOutput("Drive/DesiredSpeeds", desiredSpeeds); + Logger.recordOutput("Drive/SetpointSpeeds", currentSetpoint.chassisSpeeds()); + } + + /** Runs drive at velocity from drive input */ + public void setDriveVelocity(ChassisSpeeds velocity) { + if (DriverStation.isTeleopEnabled()) { + currentControlMode = ControlMode.DRIVER_INPUT; + desiredSpeeds = velocity; } } @@ -170,26 +224,22 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } - public Command setTrajectoryCommand(Trajectory trajectory) { - return Commands.runOnce(() -> motionPlanner.setTrajectory(trajectory)); - } - - public Command setHeadingCommand(Supplier heading) { - return Commands.runOnce(() -> motionPlanner.setHeadingSupplier(heading)); + public void setBrakeMode(boolean enabled) { + Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } - public Command disableHeadingCommand() { - return Commands.runOnce(motionPlanner::disableHeadingSupplier); + public void setTrajectory(Trajectory trajectory) { + if (currentControlMode != ControlMode.TRAJECTORY_FOLLOWING) + currentControlMode = ControlMode.TRAJECTORY_FOLLOWING; + autoMotionPlanner.setTrajectory(trajectory); } - public Command orientModules(Rotation2d[] orientations) { - return Commands.runOnce(() -> motionPlanner.orientModules(orientations)); + public Command setTrajectoryCommand(Trajectory trajectory) { + return Commands.runOnce(() -> setTrajectory(trajectory)); } - public Command orientModules(Rotation2d orientation) { - Rotation2d[] orientations = new Rotation2d[4]; - Arrays.fill(orientations, orientation); - return orientModules(orientations); + public boolean finishedTrajectory() { + return autoMotionPlanner.isFinished(); } private SwerveDriveWheelPositions getWheelPositions() { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index b46a8810..d7f7b208 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -13,10 +13,10 @@ /** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ public final class DriveConstants { - public static DrivetrainConfig drivetrainConfig = + public static DriveConfig driveConfig = switch (Constants.getRobot()) { case SIMBOT, COMPBOT -> - new DrivetrainConfig( + new DriveConfig( Units.inchesToMeters(25.0), Units.inchesToMeters(25.0), Units.feetToMeters(13.05), @@ -24,7 +24,7 @@ public final class DriveConstants { 8.86, 43.97); default -> - new DrivetrainConfig( + new DriveConfig( Units.inchesToMeters(26.0), Units.inchesToMeters(26.0), Units.feetToMeters(12.16), @@ -35,14 +35,10 @@ public final class DriveConstants { public static final double wheelRadius = Units.inchesToMeters(2.0); public static final Translation2d[] moduleTranslations = new Translation2d[] { - new Translation2d( - drivetrainConfig.trackwidthX() / 2.0, drivetrainConfig.trackwidthY() / 2.0), - new Translation2d( - drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0), - new Translation2d( - -drivetrainConfig.trackwidthX() / 2.0, drivetrainConfig.trackwidthY() / 2.0), - new Translation2d( - -drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0) + new Translation2d(driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), + new Translation2d(driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0), + new Translation2d(-driveConfig.trackwidthX() / 2.0, driveConfig.trackwidthY() / 2.0), + new Translation2d(-driveConfig.trackwidthX() / 2.0, -driveConfig.trackwidthY() / 2.0) }; public static final SwerveDriveKinematics kinematics = @@ -103,8 +99,8 @@ public final class DriveConstants { public static ModuleLimits moduleLimits = new ModuleLimits( - drivetrainConfig.maxLinearVelocity(), - drivetrainConfig.maxLinearVelocity() * 5, + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearVelocity() * 5, Units.degreesToRadians(1080.0)); public static TrajectoryConstants trajectoryConstants = @@ -119,8 +115,8 @@ public final class DriveConstants { Units.degreesToRadians(5.0), Units.inchesToMeters(5.0), Units.degreesToRadians(7.0), - drivetrainConfig.maxLinearVelocity() / 2.0, - drivetrainConfig.maxAngularVelocity() / 2.0); + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); case SIMBOT -> new TrajectoryConstants( 4.0, @@ -131,8 +127,8 @@ public final class DriveConstants { Units.degreesToRadians(5.0), Units.inchesToMeters(5.0), Units.degreesToRadians(7.0), - drivetrainConfig.maxLinearVelocity() / 2.0, - drivetrainConfig.maxAngularVelocity() / 2.0); + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); }; public static HeadingControllerConstants headingControllerConstants = @@ -140,7 +136,7 @@ public final class DriveConstants { default -> new HeadingControllerConstants(3.0, 0.0); }; - public record DrivetrainConfig( + public record DriveConfig( double trackwidthX, double trackwidthY, double maxLinearVelocity, diff --git a/src/main/java/frc/robot/subsystems/drive/DriveMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/DriveMotionPlanner.java deleted file mode 100644 index 3d26081f..00000000 --- a/src/main/java/frc/robot/subsystems/drive/DriveMotionPlanner.java +++ /dev/null @@ -1,308 +0,0 @@ -package frc.robot.subsystems.drive; - -import static frc.robot.subsystems.drive.DriveConstants.*; -import static frc.robot.subsystems.drive.DriveConstants.kinematics; -import static frc.robot.util.trajectory.HolonomicDriveController.HolonomicDriveState; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.Constants; -import frc.robot.util.AllianceFlipUtil; -import frc.robot.util.LoggedTunableNumber; -import frc.robot.util.swerve.ModuleLimits; -import frc.robot.util.swerve.SwerveSetpoint; -import frc.robot.util.swerve.SwerveSetpointGenerator; -import frc.robot.util.trajectory.HolonomicDriveController; -import frc.robot.util.trajectory.Trajectory; -import java.util.Arrays; -import java.util.Optional; -import java.util.function.Supplier; -import lombok.Getter; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class DriveMotionPlanner { - private static LoggedTunableNumber trajectoryLinearKp = - new LoggedTunableNumber("Trajectory/linearKp", trajectoryConstants.linearKp()); - private static LoggedTunableNumber trajectoryLinearKd = - new LoggedTunableNumber("Trajectory/linearKd", trajectoryConstants.linearKd()); - private static LoggedTunableNumber trajectoryThetaKp = - new LoggedTunableNumber("Trajectory/thetaKp", trajectoryConstants.thetaKp()); - private static LoggedTunableNumber trajectoryThetaKd = - new LoggedTunableNumber("Trajectory/thetaKd", trajectoryConstants.thetaKd()); - private static LoggedTunableNumber trajectoryLinearTolerance = - new LoggedTunableNumber( - "Trajectory/controllerLinearTolerance", trajectoryConstants.linearTolerance()); - private static LoggedTunableNumber trajectoryThetaTolerance = - new LoggedTunableNumber( - "Trajectory/controllerThetaTolerance", trajectoryConstants.thetaTolerance()); - private static LoggedTunableNumber trajectoryGoalLinearTolerance = - new LoggedTunableNumber( - "Trajectory/goalLinearTolerance", trajectoryConstants.goalLinearTolerance()); - private static LoggedTunableNumber trajectoryGoalThetaTolerance = - new LoggedTunableNumber( - "Trajectory/goalThetaTolerance", trajectoryConstants.goalThetaTolerance()); - private static LoggedTunableNumber trajectoryLinearVelocityTolerance = - new LoggedTunableNumber( - "Trajectory/goalLinearVelocityTolerance", trajectoryConstants.linearVelocityTolerance()); - private static LoggedTunableNumber trajectoryAngularVelocityTolerance = - new LoggedTunableNumber( - "Trajectory/goalAngularVelocityTolerance", - trajectoryConstants.angularVelocityTolerance()); - private static LoggedTunableNumber headingKp = - new LoggedTunableNumber("Drive/headingKp", headingControllerConstants.Kp()); - private static LoggedTunableNumber headingKd = - new LoggedTunableNumber("Drive/headingKd", headingControllerConstants.Kd()); - - private static final ModuleLimits kinematicLimits = moduleLimits; - - private final SwerveDriveKinematics swerveDriveKinematics; - - private ChassisSpeeds driveInputSpeeds = new ChassisSpeeds(); - @Getter private Optional currentTrajectory = Optional.empty(); - private Optional trajectoryStartTime = Optional.empty(); - private Optional> headingSupplier = Optional.empty(); - private Optional moduleOrientations = Optional.empty(); - - private final HolonomicDriveController trajectoryController; - private final PIDController headingController; - - private final SwerveSetpointGenerator setpointGenerator; - - private SwerveSetpoint previousSetpoint = - new SwerveSetpoint( - new ChassisSpeeds(), - new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }); - - public DriveMotionPlanner() { - swerveDriveKinematics = kinematics; - trajectoryController = - new HolonomicDriveController( - trajectoryLinearKp.get(), - trajectoryLinearKd.get(), - trajectoryThetaKp.get(), - trajectoryThetaKd.get()); - configTrajectoryTolerances(); - headingController = new PIDController(headingKp.get(), 0, headingKd.get()); - headingController.enableContinuousInput(-Math.PI, Math.PI); - setpointGenerator = - SwerveSetpointGenerator.builder() - .kinematics(kinematics) - .moduleLocations(moduleTranslations) - .build(); - } - - protected void setTrajectory(Trajectory trajectory) { - // Only set if there is no current trajectory - if (currentTrajectory.isEmpty()) { - currentTrajectory = Optional.of(trajectory); - trajectoryController.setGoalState(trajectory.getEndState()); - trajectoryController.resetControllers(); - // Log poses - Logger.recordOutput( - "Trajectory/trajectoryPoses", - Arrays.stream(trajectory.getTrajectoryPoses()) - .map(AllianceFlipUtil::apply) - .toArray(Pose2d[]::new)); - } - } - - protected void setHeadingSupplier(Supplier headingSupplier) { - this.headingSupplier = Optional.of(headingSupplier); - headingController.reset(); - } - - protected void disableHeadingSupplier() { - headingSupplier = Optional.empty(); - // Reset theta controller if resuming trajectory - if (currentTrajectory.isPresent()) trajectoryController.resetThetaController(); - } - - protected void setDriveInputSpeeds(ChassisSpeeds driveInputSpeeds) { - this.driveInputSpeeds = driveInputSpeeds; - } - - protected void orientModules(Rotation2d[] moduleOrientations) { - this.moduleOrientations = Optional.of(moduleOrientations); - } - - /** Output setpoint states */ - protected SwerveModuleState[] update(double timestamp, Pose2d robot, Twist2d fieldVelocity) { - // If disabled reset everything and stop - if (DriverStation.isDisabled()) { - currentTrajectory = Optional.empty(); - trajectoryStartTime = Optional.empty(); - headingSupplier = Optional.empty(); - - // Stop logs - Logger.recordOutput("Trajectory/trajectoryPoses", new Pose2d[] {}); - Logger.recordOutput("Trajectory/setpointPose", new Pose2d()); - Logger.recordOutput("Drive/headingControl", new Rotation2d()); - Logger.recordOutput("Drive/headingSetpoint", new Pose2d()); - Logger.recordOutput("Drive/speeds", new double[] {}); - return new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - } - - // Tunable numbers - if (Constants.tuningMode) { - // Update PID Controllers - LoggedTunableNumber.ifChanged( - hashCode(), - pid -> trajectoryController.setPID(pid[0], pid[1], pid[2], pid[3]), - trajectoryLinearKp, - trajectoryLinearKd, - trajectoryThetaKp, - trajectoryThetaKp); - LoggedTunableNumber.ifChanged( - hashCode(), pid -> headingController.setPID(pid[0], 0, pid[1]), headingKp, headingKd); - // Tolerances - LoggedTunableNumber.ifChanged( - hashCode(), - this::configTrajectoryTolerances, - trajectoryLinearTolerance, - trajectoryThetaTolerance, - trajectoryGoalLinearTolerance, - trajectoryGoalThetaTolerance, - trajectoryLinearVelocityTolerance, - trajectoryAngularVelocityTolerance); - } - - // Follow trajectory - ChassisSpeeds speeds = - currentTrajectory - .map( - trajectory -> { - // First update with trajectory - if (trajectoryStartTime.isEmpty()) { - trajectoryStartTime = Optional.of(timestamp); - } - - double sampleTime = timestamp - trajectoryStartTime.get(); - // Reached end - if (sampleTime > trajectory.getDuration() && trajectoryController.atGoal()) { - currentTrajectory = Optional.empty(); - trajectoryStartTime = Optional.empty(); - // Use drive input speeds (none if in auto) - return new ChassisSpeeds(); - } - - HolonomicDriveState currentState = - new HolonomicDriveState( - robot, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); - // Sample and flip state - HolonomicDriveState setpointState = trajectory.sample(sampleTime); - setpointState = AllianceFlipUtil.apply(setpointState); - // calculate trajectory speeds - ChassisSpeeds trajectorySpeeds = - trajectoryController.calculate(currentState, setpointState); - - // Log trajectory data - Logger.recordOutput("Trajectory/setpointPose", setpointState.pose()); - Logger.recordOutput( - "Trajectory/translationError", - trajectoryController.getPoseError().getTranslation().getNorm()); - Logger.recordOutput( - "Trajectory/rotationError", - trajectoryController.getPoseError().getRotation().getRadians()); - return trajectorySpeeds; - }) - .orElse(driveInputSpeeds); // Otherwise use inputted speeds - - // Use heading controller - // Change to robot relative speeds - final ChassisSpeeds robotRelativeSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robot.getRotation()); - headingSupplier.ifPresent( - headingSupplier -> { - Rotation2d setpointHeading = headingSupplier.get(); - robotRelativeSpeeds.omegaRadiansPerSecond = - headingController.calculate( - robot.getRotation().getRadians(), setpointHeading.getRadians()); - Logger.recordOutput("Drive/headingControl", setpointHeading); - Logger.recordOutput( - "Drive/headingError", - setpointHeading.getRadians() - robot.getRotation().getRadians()); - Logger.recordOutput( - "Drive/headingSetpoint", new Pose2d(robot.getTranslation(), setpointHeading)); - }); - - // Calculate output setpoint states - SwerveModuleState[] outputStates = new SwerveModuleState[4]; - if (moduleOrientations.isEmpty()) { - ChassisSpeeds discretizedSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.02); - Logger.recordOutput( - "SwerveStates/BeforePoofs", kinematics.toSwerveModuleStates(discretizedSpeeds)); - SwerveSetpoint output = - setpointGenerator.generateSetpoint( - kinematicLimits, previousSetpoint, discretizedSpeeds, 0.02); - outputStates = output.moduleStates(); - previousSetpoint = output; - Logger.recordOutput( - "Drive/speeds", - new double[] { - output.chassisSpeeds().vxMetersPerSecond, - output.chassisSpeeds().vyMetersPerSecond, - output.chassisSpeeds().omegaRadiansPerSecond - }); - } else { - // If module orientations are present - Rotation2d[] orientations = moduleOrientations.get(); - for (int i = 0; i < 4; i++) { - outputStates[i] = new SwerveModuleState(0.0, orientations[i]); - } - swerveDriveKinematics.resetHeadings(orientations); - moduleOrientations = Optional.empty(); - } - return outputStates; - } - - @AutoLogOutput(key = "Drive/followingTrajectory") - public boolean followingTrajectory() { - return currentTrajectory.isPresent(); - } - - @AutoLogOutput(key = "Drive/headingControlled") - public boolean isHeadingControlled() { - return headingSupplier.isPresent(); - } - - @AutoLogOutput(key = "Drive/modulesOriented") - public boolean modulesOriented() { - return moduleOrientations.isPresent(); - } - - private void configTrajectoryTolerances() { - trajectoryController.setControllerTolerance( - new Pose2d( - new Translation2d( - trajectoryLinearTolerance.get(), Rotation2d.fromRadians(Math.PI / 4.0)), - Rotation2d.fromRadians(trajectoryThetaTolerance.get()))); - double velocityXTolerance = trajectoryLinearVelocityTolerance.get() / Math.sqrt(2.0); - trajectoryController.setGoalTolerance( - new HolonomicDriveState( - new Pose2d( - new Translation2d( - trajectoryGoalLinearTolerance.get(), Rotation2d.fromRadians(Math.PI / 4.0)), - Rotation2d.fromRadians(trajectoryGoalThetaTolerance.get())), - velocityXTolerance, - velocityXTolerance, // Same value - trajectoryAngularVelocityTolerance.get())); - } -} diff --git a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java index c1612fba..0c221f4e 100644 --- a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java +++ b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java @@ -4,16 +4,25 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import lombok.Getter; +import lombok.Setter; public class HolonomicDriveController { private final PIDController linearController; private final PIDController thetaController; private HolonomicDriveState currentState = null; - private HolonomicDriveState goalTolerance = null; - private HolonomicDriveState goalState = null; - private Pose2d poseError; - private Pose2d controllerTolerance = null; + + /** -- SETTER -- Set tolerance for goal state */ + @Setter private HolonomicDriveState goalTolerance = null; + + /** -- SETTER -- Set goal state */ + @Setter private HolonomicDriveState goalState = null; + + @Getter private Pose2d poseError; + + /** -- SETTER -- Set tolerance of controller with Pose2d */ + @Setter private Pose2d controllerTolerance = null; public HolonomicDriveController( double linearKp, double linearKd, double thetaKp, double thetaKd) { @@ -23,21 +32,6 @@ public HolonomicDriveController( this.thetaController.enableContinuousInput(-Math.PI, Math.PI); } - /** Set tolerance of controller with Pose2d */ - public void setControllerTolerance(Pose2d tolerance) { - controllerTolerance = tolerance; - } - - /** Set tolerance for goal state */ - public void setGoalTolerance(HolonomicDriveState tolerance) { - goalTolerance = tolerance; - } - - /** Set goal state */ - public void setGoalState(HolonomicDriveState goalState) { - this.goalState = goalState; - } - /** Reset all controllers */ public void resetControllers() { linearController.reset(); @@ -54,7 +48,7 @@ public void setPID(double linearKp, double linearKd, double thetaKp, double thet thetaController.setPID(thetaKp, 0, thetaKd); } - /** Calculate chassis speeds & update currentState */ + /** Calculate robot relative chassis speeds */ public ChassisSpeeds calculate( HolonomicDriveState currentState, HolonomicDriveState setpointState) { this.currentState = currentState; @@ -81,16 +75,13 @@ public ChassisSpeeds calculate( currentState.pose().getRotation()); } - public Pose2d getPoseError() { - return poseError; - } - /** Within tolerance of current goal */ public boolean atGoal() { + Pose2d goalPoseError = goalState.pose().relativeTo(currentState.pose()); boolean withinPoseTolerance = - (poseError.getTranslation().getNorm() < goalTolerance.pose.getTranslation().getNorm()) - && (poseError.getRotation().getRadians() - < goalTolerance.pose.getRotation().getRadians()); + goalPoseError.getTranslation().getNorm() <= goalTolerance.pose().getTranslation().getNorm() + && Math.abs(goalPoseError.getRotation().getRadians()) + <= goalTolerance.pose().getRotation().getRadians(); boolean withinVelocityTolerance = Math.abs(currentState.velocityX() - goalState.velocityX()) < goalTolerance.velocityX() && Math.abs(currentState.velocityY() - goalState.velocityY())