From f3ad6c67aab468cf3710a242a2b8b6b57ad71819 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sun, 21 Jan 2024 18:56:54 -0500 Subject: [PATCH 01/12] Native trajopt testing --- Earthfile | 37 +++++++++++++++++++++++++++++++ trajectory_native/mingw-w64.cmake | 16 +++++++++++++ 2 files changed, 53 insertions(+) create mode 100644 Earthfile create mode 100644 trajectory_native/mingw-w64.cmake diff --git a/Earthfile b/Earthfile new file mode 100644 index 00000000..d83446f9 --- /dev/null +++ b/Earthfile @@ -0,0 +1,37 @@ +VERSION 0.7 +FROM debian:bookworm-20240110 +RUN apt-get update -y +WORKDIR /RobotCode2024 + +javabase: + RUN apt-get install -y openjdk-17-jdk-headless git + +ccbase: + RUN apt-get install -y build-essential git cmake clang lld + +ccbase-mingw-w64: + FROM +ccbase + RUN apt-get install -y mingw-w64 + +osxcross: + FROM crazymax/osxcross:13.1-r0-debian + SAVE ARTIFACT /osxcross + +robotcode2024-dependencies: + CACHE /root/.gradle + FROM +javabase + COPY gradle/ gradle/ + COPY settings.gradle settings.gradle + COPY build.gradle build.gradle + COPY .wpilib .wpilib + COPY vendordeps vendordeps + COPY gradlew gradlew + RUN ./gradlew build --no-daemon + SAVE IMAGE --cache-hint + +robotcode2024-build: + CACHE /root/.gradle + FROM +robotcode2024-dependencies + COPY src src + RUN ./gradlew build --no-daemon + SAVE IMAGE --cache-hint \ No newline at end of file diff --git a/trajectory_native/mingw-w64.cmake b/trajectory_native/mingw-w64.cmake new file mode 100644 index 00000000..19fd39c6 --- /dev/null +++ b/trajectory_native/mingw-w64.cmake @@ -0,0 +1,16 @@ +set(CMAKE_SYSTEM_NAME Windows) +set(TOOLCHAIN_PREFIX x86_64-w64-mingw32) + +# cross compilers to use for C, C++ and Fortran +set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc-posix) +set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++-posix) +set(CMAKE_Fortran_COMPILER ${TOOLCHAIN_PREFIX}-gfortran) +set(CMAKE_RC_COMPILER ${TOOLCHAIN_PREFIX}-windres) + +# target environment on the build host system +set(CMAKE_FIND_ROOT_PATH /usr/${TOOLCHAIN_PREFIX}) + +# modify default behavior of FIND_XXX() commands +set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) \ No newline at end of file From b65ca0e3b65431f1f8b0c666e0f53836b6d7ab86 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sun, 21 Jan 2024 18:57:02 -0500 Subject: [PATCH 02/12] Add .gitignore --- trajectory_native/.gitignore | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 trajectory_native/.gitignore diff --git a/trajectory_native/.gitignore b/trajectory_native/.gitignore new file mode 100644 index 00000000..0566c63a --- /dev/null +++ b/trajectory_native/.gitignore @@ -0,0 +1,3 @@ +libs/ +generated/ +build/ \ No newline at end of file From 9b4e35a66d6673e605b26db130d72fa9f502515c Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 2 Feb 2024 18:43:15 -0500 Subject: [PATCH 03/12] =?UTF-8?q?Use=20commands=20=F0=9F=91=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/RobotContainer.java | 342 ++++++++-------- .../frc/robot/commands/DriveCommands.java | 5 +- .../frc/robot/subsystems/drive/Drive.java | 386 +++++++++--------- .../subsystems/drive/DriveConstants.java | 27 +- .../drive/PhoenixOdometryThread.java | 4 +- .../TrajectoryMotionPlanner.java} | 25 +- 6 files changed, 416 insertions(+), 373 deletions(-) rename src/main/java/frc/robot/subsystems/drive/{AutoMotionPlanner.java => planners/TrajectoryMotionPlanner.java} (90%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe538532..32c1c03b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +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.subsystems.apriltagvision.AprilTagVision; import frc.robot.subsystems.apriltagvision.AprilTagVisionConstants; @@ -37,10 +38,12 @@ import frc.robot.util.AllianceFlipUtil; import frc.robot.util.trajectory.ChoreoTrajectoryReader; import frc.robot.util.trajectory.Trajectory; + import java.io.File; import java.util.Objects; import java.util.Optional; import java.util.function.Function; + import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -50,174 +53,193 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Load robot state as field - private final RobotState robotState = RobotState.getInstance(); - - // Subsystems - private Drive drive; - private AprilTagVision aprilTagVision; - private Shooter shooter; - private Intake intake; - - // Controller - private final CommandXboxController controller = new CommandXboxController(0); - - // Dashboard inputs - private final LoggedDashboardChooser autoChooser; - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - switch (Constants.getMode()) { - case REAL -> { - // Real robot, instantiate hardware IO implementations\ - switch (Constants.getRobot()) { - default -> { + // Load robot state + private final RobotState robotState = RobotState.getInstance(); + + // Subsystems + private Drive drive; + private AprilTagVision aprilTagVision; + private Shooter shooter; + private Intake intake; + + // Controller + private final CommandXboxController controller = new CommandXboxController(0); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + switch (Constants.getMode()) { + case REAL -> { + // Real robot, instantiate hardware IO implementations\ + switch (Constants.getRobot()) { + default -> { + drive = + new Drive( + new GyroIOPigeon2(false), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); + aprilTagVision = + new AprilTagVision( + new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); + shooter = new Shooter(new ShooterIOSparkMax()); + intake = new Intake(new IntakeIOSparkMax()); + } + } + } + case SIM -> { + // Sim robot, instantiate physics sim IO implementations + drive = + new Drive( + new GyroIO() { + }, + new ModuleIOSim(DriveConstants.moduleConfigs[0]), + new ModuleIOSim(DriveConstants.moduleConfigs[1]), + new ModuleIOSim(DriveConstants.moduleConfigs[2]), + new ModuleIOSim(DriveConstants.moduleConfigs[3])); + shooter = new Shooter(new ShooterIOSim()); + intake = new Intake(new IntakeIOSim()); + } + default -> { + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); + shooter = new Shooter(new ShooterIO() { + }); + intake = new Intake(new IntakeIO() { + }); + } + } + + if (drive == null) { drive = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); - shooter = new Shooter(new ShooterIOSparkMax()); - intake = new Intake(new IntakeIOSparkMax()); - } + new Drive( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }); } - } - case SIM -> { - // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(DriveConstants.moduleConfigs[0]), - new ModuleIOSim(DriveConstants.moduleConfigs[1]), - new ModuleIOSim(DriveConstants.moduleConfigs[2]), - new ModuleIOSim(DriveConstants.moduleConfigs[3])); - shooter = new Shooter(new ShooterIOSim()); - intake = new Intake(new IntakeIOSim()); - } - default -> { - // Replayed robot, disable IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - shooter = new Shooter(new ShooterIO() {}); - intake = new Intake(new IntakeIO() {}); - } - } - if (drive == null) { - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - } + if (aprilTagVision == null) { + aprilTagVision = new AprilTagVision(); + } - if (aprilTagVision == null) { - aprilTagVision = new AprilTagVision(); - } + if (shooter == null) { + shooter = new Shooter(new ShooterIO() { + }); + } - if (shooter == null) { - shooter = new Shooter(new ShooterIO() {}); - } + if (intake == null) { + intake = new Intake(new IntakeIO() { + }); + } - if (intake == null) { - intake = new Intake(new IntakeIO() {}); + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + // Set up feedforward characterization + autoChooser.addOption( + "Drive FF Characterization", + new FeedForwardCharacterization( + drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) + .finallyDo(drive::endCharacterization)); + autoChooser.addOption( + "Left Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runLeftCharacterizationVolts, + shooter::getLeftCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + autoChooser.addOption( + "Right Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runRightCharacterizationVolts, + shooter::getRightCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + + // Testing autos paths + Function> trajectoryCommandFactory = + trajectoryFile -> { + Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); + return trajectory.map( + traj -> + Commands.runOnce(() -> + robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) + .andThen(drive.followTrajectory(traj))); + }; + final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); + for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { + trajectoryCommandFactory + .apply(trajectoryFile) + .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); + } + + // Configure the button bindings + configureButtonBindings(); } - autoChooser = new LoggedDashboardChooser<>("Auto Choices"); - // Set up feedforward characterization - autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) - .finallyDo(drive::endCharacterization)); - autoChooser.addOption( - "Left Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, - shooter::runLeftCharacterizationVolts, - shooter::getLeftCharacterizationVelocity) - .beforeStarting(() -> shooter.setCharacterizing(true)) - .finallyDo(() -> shooter.setCharacterizing(false))); - autoChooser.addOption( - "Right Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, - shooter::runRightCharacterizationVolts, - shooter::getRightCharacterizationVelocity) - .beforeStarting(() -> shooter.setCharacterizing(true)) - .finallyDo(() -> shooter.setCharacterizing(false))); - - // Testing autos paths - Function> trajectoryCommandFactory = - trajectoryFile -> { - Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); - return trajectory.map( - traj -> - Commands.runOnce( - () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - .andThen(drive.setTrajectoryCommand(traj))); - }; - final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); - for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { - trajectoryCommandFactory - .apply(trajectoryFile) - .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); + // controller + // .a() + // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), + // intake::running)); + // controller + // .x() + // .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), + // intake::running)); + controller + .b() + .onTrue( + Commands.runOnce( + () -> + robotState.resetPose( + new Pose2d( + robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) + .ignoringDisable(true)); + controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); } - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - // drive.setDefaultCommand( - // DriveCommands.joystickDrive( - // drive, - // () -> -controller.getLeftY(), - // () -> -controller.getLeftX(), - // () -> -controller.getRightX())); - // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); - controller - .a() - .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), intake::running)); - controller - .x() - .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), intake::running)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - robotState.resetPose( - new Pose2d( - robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) - .ignoringDisable(true)); - controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.get(); - } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autoChooser.get(); + } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 85dc9a35..5feb7692 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -64,9 +64,8 @@ public static Command joystickDrive( linearVelocity.getX() * DriveConstants.driveConfig.maxLinearVelocity(), linearVelocity.getY() * DriveConstants.driveConfig.maxLinearVelocity(), omega * DriveConstants.driveConfig.maxAngularVelocity()); - drive.setDriveVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - speeds, RobotState.getInstance().getEstimatedPose().getRotation())); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, RobotState.getInstance().getEstimatedPose().getRotation()); + drive.setVelocity(speeds); }); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3b2fa42d..94196e0c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -16,68 +16,74 @@ 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.subsystems.drive.planners.TrajectoryMotionPlanner; 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.stream.IntStream; + import lombok.experimental.ExtensionMethod; +import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { + @AutoLog + public static class OdometryTimeestampInputs { + public double[] timestamps = new double[]{}; + } - private enum ControlMode { - TRAJECTORY_FOLLOWING, - DRIVER_INPUT - } - - public static final Lock odometryLock = new ReentrantLock(); - // TODO: DO THIS BETTER! - public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); + public static final Lock odometryLock = new ReentrantLock(); + // TODO: DO THIS BETTER! + public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Module[] modules = new Module[4]; + private final OdometryTimeestampInputsAutoLogged odometryTimestampInputs = + new OdometryTimeestampInputsAutoLogged(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; 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; - - public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { - System.out.println("[Init] Creating Drive"); - this.gyroIO = gyroIO; - modules[0] = new Module(fl, 0); - modules[1] = new Module(fr, 1); - modules[2] = new Module(bl, 2); - modules[3] = new Module(br, 3); + private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); + private final TrajectoryMotionPlanner trajectoryMotionPlanner; + + 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; + + public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { + System.out.println("[Init] Creating Drive"); + this.gyroIO = gyroIO; + modules[0] = new Module(fl, 0); + modules[1] = new Module(fr, 1); + modules[2] = new Module(bl, 2); + modules[3] = new Module(br, 3); setpointGenerator = SwerveSetpointGenerator.builder() @@ -86,187 +92,173 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) .build(); autoMotionPlanner = new AutoMotionPlanner(); } - - public void periodic() { - // Update & process inputs - odometryLock.lock(); - - // Read timestamps from odometry thread and fake sim timestamps - double[] timestamps = timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); - if (timestamps.length == 0) { - timestamps = new double[] {Timer.getFPGATimestamp()}; + setpointGenerator = + SwerveSetpointGenerator.builder() + .kinematics(DriveConstants.kinematics) + .moduleLocations(DriveConstants.moduleTranslations) + .build(); + trajectoryMotionPlanner = new TrajectoryMotionPlanner(); } - timestampQueue.clear(); - // Read inputs from gyro and modules - gyroIO.updateInputs(gyroInputs); - Logger.processInputs("Drive/Gyro", gyroInputs); + public void periodic() { + // Update & process inputs + odometryLock.lock(); + // Read timestamps from odometry thread and fake sim timestamps + odometryTimestampInputs.timestamps = timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); + if (odometryTimestampInputs.timestamps.length == 0) { + odometryTimestampInputs.timestamps = new double[]{Timer.getFPGATimestamp()}; + } + timestampQueue.clear(); + Logger.processInputs("Drive/OdometryTimestamps", odometryTimestampInputs); + // Read inputs from gyro + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + // Read inputs from modules + Arrays.stream(modules).forEach(Module::updateInputs); + odometryLock.unlock(); + + // Calculate the min odometry position updates across all modules + int minOdometryUpdates = + IntStream.of( + odometryTimestampInputs.timestamps.length, + Arrays.stream(modules) + .mapToInt(module -> module.getModulePositions().length) + .min() + .orElse(0)) + .min() + .orElse(0); + if (gyroInputs.connected) { + minOdometryUpdates = Math.min(gyroInputs.odometryYawPositions.length, minOdometryUpdates); + } + // Pass odometry data to robot state + for (int i = 0; i < minOdometryUpdates; i++) { + int odometryIndex = i; + Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null; + // Get all four swerve module positions at that odometry update + // and store in SwerveDriveWheelPositions object + SwerveDriveWheelPositions wheelPositions = + new SwerveDriveWheelPositions( + Arrays.stream(modules) + .map(module -> module.getModulePositions()[odometryIndex]) + .toArray(SwerveModulePosition[]::new)); + // Add observation to robot state + RobotState.getInstance() + .addOdometryObservation( + new RobotState.OdometryObservation( + wheelPositions, + yaw, + odometryTimestampInputs.timestamps[i])); + } - Arrays.stream(modules).forEach(Module::updateInputs); - odometryLock.unlock(); + // update current velocities use gyro when possible + ChassisSpeeds robotRelativeVelocity = + DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); + robotRelativeVelocity.omegaRadiansPerSecond = + gyroInputs.connected + ? gyroInputs.yawVelocityRadPerSec + : robotRelativeVelocity.omegaRadiansPerSecond; + RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); + + // Run characterization + if (characterizing) { + 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; + } - // Calculate the min odometry position updates across all modules - int minOdometryUpdates = - IntStream.of( - timestamps.length, - Arrays.stream(modules) - .mapToInt(module -> module.getModulePositions().length) - .min() - .orElse(0)) - .min() - .orElse(0); - if (gyroInputs.connected) { - minOdometryUpdates = Math.min(gyroInputs.odometryYawPositions.length, minOdometryUpdates); - } - // Pass odometry data to robot state - for (int i = 0; i < minOdometryUpdates; i++) { - int odometryIndex = i; - Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null; - // Get all four swerve module positions at that odometry update - // and store in SwerveDriveWheelPositions object - SwerveDriveWheelPositions wheelPositions = - new SwerveDriveWheelPositions( - Arrays.stream(modules) - .map(module -> module.getModulePositions()[odometryIndex]) - .toArray(SwerveModulePosition[]::new)); - RobotState.getInstance() - .addOdometryObservation( - new RobotState.OdometryObservation(wheelPositions, yaw, timestamps[i])); + // Run robot at speeds + // 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()); } - // update current velocities use gyro when possible - ChassisSpeeds robotRelativeVelocity = - DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); - robotRelativeVelocity.omegaRadiansPerSecond = - gyroInputs.connected - ? gyroInputs.yawVelocityRadPerSec - : robotRelativeVelocity.omegaRadiansPerSecond; - RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); + /** + * Set drive velocity (robot relative) + */ + public void setVelocity(ChassisSpeeds velocity) { + desiredSpeeds = ChassisSpeeds.discretize(velocity, 0.02); + } - if (characterizing) { - // Run characterization - for (Module module : modules) { - module.runCharacterization(characterizationVolts); - } + /** + * Runs forwards at the commanded voltage. + */ + public void runCharacterizationVolts(double volts) { + characterizing = true; + characterizationVolts = volts; + } - 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; + public void endCharacterization() { + characterizing = false; } - 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 + /** + * Returns the average drive velocity in radians/sec. + */ + public double getCharacterizationVelocity() { + double driveVelocityAverage = 0.0; + for (var module : modules) { + driveVelocityAverage += module.getCharacterizationVelocity(); } - } + return driveVelocityAverage / 4.0; } - // 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]); + public void setBrakeMode(boolean enabled) { + Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } - // 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; + public Command followTrajectory(Trajectory trajectory) { + return Commands.sequence( + Commands.runOnce(() -> trajectoryMotionPlanner.setTrajectory(trajectory)), + Commands.run(() -> setVelocity(trajectoryMotionPlanner.update())) + .until(trajectoryMotionPlanner::isFinished)); } - } - /** Runs forwards at the commanded voltage. */ - public void runCharacterizationVolts(double volts) { - characterizing = true; - characterizationVolts = volts; - } - - public void endCharacterization() { - characterizing = false; - } - - /** Returns the average drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - double driveVelocityAverage = 0.0; - for (var module : modules) { - driveVelocityAverage += module.getCharacterizationVelocity(); + @AutoLogOutput(key = "Drive/GyroYaw") + public Rotation2d getGyroYaw() { + return gyroInputs.connected + ? gyroInputs.yawPosition + : RobotState.getInstance().getEstimatedPose().getRotation(); } - return driveVelocityAverage / 4.0; - } - - public void setBrakeMode(boolean enabled) { - Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); - } - - public void setTrajectory(Trajectory trajectory) { - if (currentControlMode != ControlMode.TRAJECTORY_FOLLOWING) - currentControlMode = ControlMode.TRAJECTORY_FOLLOWING; - autoMotionPlanner.setTrajectory(trajectory); - } - public Command setTrajectoryCommand(Trajectory trajectory) { - return Commands.runOnce(() -> setTrajectory(trajectory)); - } - - public boolean finishedTrajectory() { - return autoMotionPlanner.isFinished(); - } - - private SwerveDriveWheelPositions getWheelPositions() { - return new SwerveDriveWheelPositions( - Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new)); - } - - @AutoLogOutput(key = "Odometry/GyroYaw") - public Rotation2d getGyroYaw() { - return gyroInputs.connected - ? gyroInputs.yawPosition - : RobotState.getInstance().getEstimatedPose().getRotation(); - } - - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); - } + /** + * Returns the module states (turn angles and drive velocities) for all of the modules. + */ + @AutoLogOutput(key = "Drive/SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); + } - public static Rotation2d[] getStraightOrientations() { - return IntStream.range(0, 4).mapToObj(Rotation2d::new).toArray(Rotation2d[]::new); - } + public static Rotation2d[] getStraightOrientations() { + return IntStream.range(0, 4).mapToObj(Rotation2d::new).toArray(Rotation2d[]::new); + } - public static Rotation2d[] getXOrientations() { - return Arrays.stream(DriveConstants.moduleTranslations) - .map(Translation2d::getAngle) - .toArray(Rotation2d[]::new); - } + public static Rotation2d[] getXOrientations() { + return Arrays.stream(DriveConstants.moduleTranslations) + .map(Translation2d::getAngle) + .toArray(Rotation2d[]::new); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index d7f7b208..4ff2c32a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -109,7 +109,7 @@ public final class DriveConstants { new TrajectoryConstants( 6.0, 0.0, - 10.0, + 5.0, 0.0, Units.inchesToMeters(4.0), Units.degreesToRadians(5.0), @@ -131,6 +131,19 @@ public final class DriveConstants { driveConfig.maxAngularVelocity() / 2.0); }; + public static AutoAlignConstants autoAlignConstants = + new AutoAlignConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(2.0), + Units.degreesToRadians(2.0), + driveConfig.maxLinearVelocity() * 0.3, + driveConfig.maxLinearAcceleration() * 0.5, + driveConfig.maxAngularVelocity() * 0.3, + driveConfig.maxAngularAcceleration() * 0.5); + public static HeadingControllerConstants headingControllerConstants = switch (Constants.getRobot()) { default -> new HeadingControllerConstants(3.0, 0.0); @@ -177,6 +190,18 @@ public record TrajectoryConstants( double linearVelocityTolerance, double angularVelocityTolerance) {} + public record AutoAlignConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) {} + public record HeadingControllerConstants(double Kp, double Kd) {} private enum Mk4iReductions { diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 231829d0..1ea7215c 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -17,10 +17,10 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; -import edu.wpi.first.wpilibj.Timer; import java.util.*; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.Logger; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -87,7 +87,7 @@ public void run() { } finally { signalsLock.unlock(); } - double fpgaTimestamp = Timer.getFPGATimestamp(); + double fpgaTimestamp = Logger.getRealTimestamp() / 1e6; // Save new data to queues Drive.odometryLock.lock(); diff --git a/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java similarity index 90% rename from src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java rename to src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java index 5933f533..a8919eff 100644 --- a/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java +++ b/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.drive; +package frc.robot.subsystems.drive.planners; import static frc.robot.subsystems.drive.DriveConstants.*; import static frc.robot.util.trajectory.HolonomicDriveController.HolonomicDriveState; @@ -9,7 +9,9 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; +import frc.robot.RobotState; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.trajectory.HolonomicDriveController; @@ -17,7 +19,7 @@ import java.util.Arrays; import org.littletonrobotics.junction.Logger; -public class AutoMotionPlanner { +public class TrajectoryMotionPlanner { private static LoggedTunableNumber trajectoryLinearKp = new LoggedTunableNumber("Trajectory/linearKp", trajectoryConstants.linearKp()); private static LoggedTunableNumber trajectoryLinearKd = @@ -51,7 +53,7 @@ public class AutoMotionPlanner { private Double currentTime = null; private final HolonomicDriveController controller; - public AutoMotionPlanner() { + public TrajectoryMotionPlanner() { controller = new HolonomicDriveController( trajectoryLinearKp.get(), @@ -61,7 +63,7 @@ public AutoMotionPlanner() { configTrajectoryTolerances(); } - protected void setTrajectory(Trajectory trajectory) { + public void setTrajectory(Trajectory trajectory) { // Only set if not following trajectory or done with current one if (this.trajectory == null || isFinished()) { this.trajectory = trajectory; @@ -79,8 +81,10 @@ protected void setTrajectory(Trajectory trajectory) { } /** Output setpoint chassis speeds in */ - protected ChassisSpeeds update(double timestamp, Pose2d robot, Twist2d fieldVelocity) { - System.out.println(timestamp + " " + robot.toString() + " " + fieldVelocity.toString()); + public ChassisSpeeds update() { + double time = Timer.getFPGATimestamp(); + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); // If disabled reset everything and stop if (DriverStation.isDisabled()) { trajectory = null; @@ -123,12 +127,13 @@ protected ChassisSpeeds update(double timestamp, Pose2d robot, Twist2d fieldVelo } if (startTime == null) { - startTime = timestamp; + startTime = time; } - currentTime = timestamp; + currentTime = time; HolonomicDriveState currentState = - new HolonomicDriveState(robot, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); + new HolonomicDriveState( + currentPose, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); // Sample and flip state HolonomicDriveState setpointState = trajectory.sample(currentTime - startTime); setpointState = AllianceFlipUtil.apply(setpointState); @@ -144,7 +149,7 @@ protected ChassisSpeeds update(double timestamp, Pose2d robot, Twist2d fieldVelo return speeds; } - protected boolean isFinished() { + public boolean isFinished() { return trajectory != null && currentTime != null && startTime != null From 48e8137d158d7b048ba3933ac5494c2d78ec7ea9 Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 2 Feb 2024 18:43:15 -0500 Subject: [PATCH 04/12] Odometry timestamps --- src/main/java/frc/robot/RobotContainer.java | 31 +++--- .../frc/robot/commands/DriveCommands.java | 5 +- .../frc/robot/commands/auto/TestAutos.java | 3 +- .../frc/robot/subsystems/drive/Drive.java | 96 +++++++------------ .../subsystems/drive/DriveConstants.java | 27 +++++- .../drive/PhoenixOdometryThread.java | 4 +- .../drive/SparkMaxOdometryThread.java | 4 +- .../TrajectoryMotionPlanner.java} | 83 ++++++++-------- .../trajectory/HolonomicDriveController.java | 53 +++++++--- 9 files changed, 164 insertions(+), 142 deletions(-) rename src/main/java/frc/robot/subsystems/drive/{AutoMotionPlanner.java => planners/TrajectoryMotionPlanner.java} (77%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe538532..5488e4d2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +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.subsystems.apriltagvision.AprilTagVision; import frc.robot.subsystems.apriltagvision.AprilTagVisionConstants; @@ -50,7 +51,7 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Load robot state as field + // Load robot state private final RobotState robotState = RobotState.getInstance(); // Subsystems @@ -167,7 +168,7 @@ public RobotContainer() { traj -> Commands.runOnce( () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - .andThen(drive.setTrajectoryCommand(traj))); + .andThen(drive.followTrajectory(traj))); }; final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { @@ -187,19 +188,21 @@ 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() - .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), intake::running)); - controller - .x() - .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), intake::running)); + // controller + // .a() + // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), + // intake::running)); + // controller + // .x() + // .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), + // intake::running)); controller .b() .onTrue( diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 85dc9a35..f3ef5847 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -64,9 +64,10 @@ public static Command joystickDrive( linearVelocity.getX() * DriveConstants.driveConfig.maxLinearVelocity(), linearVelocity.getY() * DriveConstants.driveConfig.maxLinearVelocity(), omega * DriveConstants.driveConfig.maxAngularVelocity()); - drive.setDriveVelocity( + speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - speeds, RobotState.getInstance().getEstimatedPose().getRotation())); + speeds, RobotState.getInstance().getEstimatedPose().getRotation()); + drive.setVelocity(speeds); }); } diff --git a/src/main/java/frc/robot/commands/auto/TestAutos.java b/src/main/java/frc/robot/commands/auto/TestAutos.java index 2bf27691..45c22126 100644 --- a/src/main/java/frc/robot/commands/auto/TestAutos.java +++ b/src/main/java/frc/robot/commands/auto/TestAutos.java @@ -65,8 +65,7 @@ public static Command fourNoteDavis(Drive drive, Intake intake) { return Commands.sequence( resetPoseCommand(trajectory.getStartPose()), - drive.setTrajectoryCommand(trajectory), - Commands.parallel(sequenceIntake)); + Commands.parallel(drive.followTrajectory(trajectory), sequenceIntake)); } private static Command resetPoseCommand(Pose2d pose) { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3b2fa42d..16b69a3d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -19,9 +19,9 @@ 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.subsystems.drive.planners.TrajectoryMotionPlanner; import frc.robot.util.GeomUtil; import frc.robot.util.swerve.ModuleLimits; import frc.robot.util.swerve.SwerveSetpoint; @@ -33,28 +33,29 @@ import java.util.concurrent.locks.ReentrantLock; import java.util.stream.IntStream; import lombok.experimental.ExtensionMethod; +import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { - - private enum ControlMode { - TRAJECTORY_FOLLOWING, - DRIVER_INPUT + @AutoLog + public static class OdometryTimeestampInputs { + public double[] timestamps = new double[] {}; } public static final Lock odometryLock = new ReentrantLock(); // TODO: DO THIS BETTER! public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); + private final OdometryTimeestampInputsAutoLogged odometryTimestampInputs = + new OdometryTimeestampInputsAutoLogged(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; - private ControlMode currentControlMode = ControlMode.DRIVER_INPUT; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); - private final AutoMotionPlanner autoMotionPlanner; + private final TrajectoryMotionPlanner trajectoryMotionPlanner; private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; private SwerveSetpoint currentSetpoint = @@ -84,31 +85,31 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) .kinematics(DriveConstants.kinematics) .moduleLocations(DriveConstants.moduleTranslations) .build(); - autoMotionPlanner = new AutoMotionPlanner(); + trajectoryMotionPlanner = new TrajectoryMotionPlanner(); } public void periodic() { // Update & process inputs odometryLock.lock(); - // Read timestamps from odometry thread and fake sim timestamps - double[] timestamps = timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); - if (timestamps.length == 0) { - timestamps = new double[] {Timer.getFPGATimestamp()}; + odometryTimestampInputs.timestamps = + timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); + if (odometryTimestampInputs.timestamps.length == 0) { + odometryTimestampInputs.timestamps = new double[] {Timer.getFPGATimestamp()}; } timestampQueue.clear(); - - // Read inputs from gyro and modules + Logger.processInputs("Drive/OdometryTimestamps", odometryTimestampInputs); + // Read inputs from gyro gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); - + // Read inputs from modules Arrays.stream(modules).forEach(Module::updateInputs); odometryLock.unlock(); // Calculate the min odometry position updates across all modules int minOdometryUpdates = IntStream.of( - timestamps.length, + odometryTimestampInputs.timestamps.length, Arrays.stream(modules) .mapToInt(module -> module.getModulePositions().length) .min() @@ -129,9 +130,11 @@ public void periodic() { Arrays.stream(modules) .map(module -> module.getModulePositions()[odometryIndex]) .toArray(SwerveModulePosition[]::new)); + // Add observation to robot state RobotState.getInstance() .addOdometryObservation( - new RobotState.OdometryObservation(wheelPositions, yaw, timestamps[i])); + new RobotState.OdometryObservation( + wheelPositions, yaw, odometryTimestampInputs.timestamps[i])); } // update current velocities use gyro when possible @@ -143,8 +146,12 @@ public void periodic() { : robotRelativeVelocity.omegaRadiansPerSecond; RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); + if (DriverStation.isDisabled()) { + Arrays.stream(modules).forEach(Module::stop); + } + + // Run characterization if (characterizing) { - // Run characterization for (Module module : modules) { module.runCharacterization(characterizationVolts); } @@ -156,25 +163,8 @@ public void periodic() { 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 + // Generate feasible next setpoint currentSetpoint = setpointGenerator.generateSetpoint( currentModuleLimits, currentSetpoint, desiredSpeeds, 0.02); @@ -197,12 +187,9 @@ public void periodic() { 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; - } + /** Set drive velocity (robot relative) */ + public void setVelocity(ChassisSpeeds velocity) { + desiredSpeeds = ChassisSpeeds.discretize(velocity, 0.02); } /** Runs forwards at the commanded voltage. */ @@ -228,26 +215,13 @@ public void setBrakeMode(boolean enabled) { Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } - public void setTrajectory(Trajectory trajectory) { - if (currentControlMode != ControlMode.TRAJECTORY_FOLLOWING) - currentControlMode = ControlMode.TRAJECTORY_FOLLOWING; - autoMotionPlanner.setTrajectory(trajectory); - } - - public Command setTrajectoryCommand(Trajectory trajectory) { - return Commands.runOnce(() -> setTrajectory(trajectory)); - } - - public boolean finishedTrajectory() { - return autoMotionPlanner.isFinished(); - } - - private SwerveDriveWheelPositions getWheelPositions() { - return new SwerveDriveWheelPositions( - Arrays.stream(modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new)); + public Command followTrajectory(Trajectory trajectory) { + return run(() -> setVelocity(trajectoryMotionPlanner.update())) + .beforeStarting(() -> trajectoryMotionPlanner.setTrajectory(trajectory)) + .until(trajectoryMotionPlanner::isFinished); } - @AutoLogOutput(key = "Odometry/GyroYaw") + @AutoLogOutput(key = "Drive/GyroYaw") public Rotation2d getGyroYaw() { return gyroInputs.connected ? gyroInputs.yawPosition @@ -255,7 +229,7 @@ public Rotation2d getGyroYaw() { } /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") + @AutoLogOutput(key = "Drive/SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index d7f7b208..4ff2c32a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -109,7 +109,7 @@ public final class DriveConstants { new TrajectoryConstants( 6.0, 0.0, - 10.0, + 5.0, 0.0, Units.inchesToMeters(4.0), Units.degreesToRadians(5.0), @@ -131,6 +131,19 @@ public final class DriveConstants { driveConfig.maxAngularVelocity() / 2.0); }; + public static AutoAlignConstants autoAlignConstants = + new AutoAlignConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(2.0), + Units.degreesToRadians(2.0), + driveConfig.maxLinearVelocity() * 0.3, + driveConfig.maxLinearAcceleration() * 0.5, + driveConfig.maxAngularVelocity() * 0.3, + driveConfig.maxAngularAcceleration() * 0.5); + public static HeadingControllerConstants headingControllerConstants = switch (Constants.getRobot()) { default -> new HeadingControllerConstants(3.0, 0.0); @@ -177,6 +190,18 @@ public record TrajectoryConstants( double linearVelocityTolerance, double angularVelocityTolerance) {} + public record AutoAlignConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) {} + public record HeadingControllerConstants(double Kp, double Kd) {} private enum Mk4iReductions { diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 231829d0..325fddf0 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -17,10 +17,10 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; -import edu.wpi.first.wpilibj.Timer; import java.util.*; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.littletonrobotics.junction.Logger; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -87,7 +87,7 @@ public void run() { } finally { signalsLock.unlock(); } - double fpgaTimestamp = Timer.getFPGATimestamp(); + double fpgaTimestamp = Logger.getRealTimestamp() / 1.0e6; // Save new data to queues Drive.odometryLock.lock(); diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index eaaed519..010e7084 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -14,12 +14,12 @@ package frc.robot.subsystems.drive; import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.Timer; import java.util.ArrayList; import java.util.List; import java.util.Queue; import java.util.concurrent.ArrayBlockingQueue; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; /** * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. @@ -60,7 +60,7 @@ public Queue registerSignal(DoubleSupplier signal) { private void periodic() { Drive.odometryLock.lock(); - Drive.timestampQueue.offer(Timer.getFPGATimestamp()); + Drive.timestampQueue.offer(Logger.getRealTimestamp() / 1.0e6); try { for (int i = 0; i < signals.size(); i++) { queues.get(i).offer(signals.get(i).getAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java similarity index 77% rename from src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java rename to src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java index 5933f533..55291c56 100644 --- a/src/main/java/frc/robot/subsystems/drive/AutoMotionPlanner.java +++ b/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.drive; +package frc.robot.subsystems.drive.planners; import static frc.robot.subsystems.drive.DriveConstants.*; import static frc.robot.util.trajectory.HolonomicDriveController.HolonomicDriveState; @@ -9,15 +9,18 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; +import frc.robot.RobotState; 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.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class AutoMotionPlanner { +public class TrajectoryMotionPlanner { private static LoggedTunableNumber trajectoryLinearKp = new LoggedTunableNumber("Trajectory/linearKp", trajectoryConstants.linearKp()); private static LoggedTunableNumber trajectoryLinearKd = @@ -47,11 +50,10 @@ public class AutoMotionPlanner { trajectoryConstants.angularVelocityTolerance()); private Trajectory trajectory = null; - private Double startTime = null; - private Double currentTime = null; + private Timer trajectoryTimer = new Timer(); private final HolonomicDriveController controller; - public AutoMotionPlanner() { + public TrajectoryMotionPlanner() { controller = new HolonomicDriveController( trajectoryLinearKp.get(), @@ -61,33 +63,29 @@ public AutoMotionPlanner() { 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)); - } + public void setTrajectory(Trajectory trajectory) { + this.trajectory = trajectory; + controller.setGoalState(AllianceFlipUtil.apply(trajectory.getEndState())); + controller.resetControllers(); + trajectoryTimer.restart(); + // 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()); + public ChassisSpeeds update() { + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); // 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[] {}); + trajectoryTimer.stop(); + trajectoryTimer.reset(); + controller.resetControllers(); return new ChassisSpeeds(); } @@ -113,42 +111,37 @@ protected ChassisSpeeds update(double timestamp, Pose2d robot, Twist2d fieldVelo trajectoryAngularVelocityTolerance); } + HolonomicDriveState currentState = + new HolonomicDriveState( + currentPose, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); + // Sample and flip state + HolonomicDriveState setpointState = trajectory.sample(trajectoryTimer.get()); + setpointState = AllianceFlipUtil.apply(setpointState); + // calculate trajectory speeds + ChassisSpeeds speeds = controller.calculate(currentState, setpointState); // Reached end - if (isFinished() || trajectory == null) { + if (isFinished()) { + trajectoryTimer.stop(); // 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()); + Logger.recordOutput("Trajectory/speeds", speeds); return speeds; } - protected boolean isFinished() { + @AutoLogOutput(key = "Trajectory/finished") + public boolean isFinished() { return trajectory != null - && currentTime != null - && startTime != null - && currentTime - startTime >= trajectory.getDuration() + && trajectoryTimer.hasElapsed(trajectory.getDuration()) && controller.atGoal(); } diff --git a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java index 0c221f4e..6f8ac8d4 100644 --- a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java +++ b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java @@ -2,7 +2,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import lombok.Getter; import lombok.Setter; @@ -16,20 +16,19 @@ public class HolonomicDriveController { /** -- SETTER -- Set tolerance for goal state */ @Setter private HolonomicDriveState goalTolerance = null; + private Pose2d controllerTolerance = 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) { linearController = new PIDController(linearKp, 0, linearKd); thetaController = new PIDController(thetaKp, 0, thetaKd); - this.thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.enableContinuousInput(-Math.PI, Math.PI); } /** Reset all controllers */ @@ -42,6 +41,11 @@ public void resetThetaController() { thetaController.reset(); } + public void setControllerTolerance(Pose2d controllerTolerance) { + linearController.setTolerance(controllerTolerance.getTranslation().getNorm()); + thetaController.setTolerance(controllerTolerance.getRotation().getRadians()); + } + /** Set PID values */ public void setPID(double linearKp, double linearKd, double thetaKp, double thetaKd) { linearController.setPID(linearKp, 0, linearKd); @@ -52,20 +56,30 @@ public void setPID(double linearKp, double linearKd, double thetaKp, double thet public ChassisSpeeds calculate( HolonomicDriveState currentState, HolonomicDriveState setpointState) { this.currentState = currentState; - Pose2d poseRef = setpointState.pose(); - poseError = poseRef.relativeTo(currentState.pose()); + Pose2d setpointPose = setpointState.pose(); + poseError = setpointPose.relativeTo(currentState.pose()); // Calculate feedback velocities (based on position error). - Translation2d currToStateTranslation = - poseRef.getTranslation().minus(currentState.pose().getTranslation()); double linearFeedback = linearController.calculate( - 0, currentState.pose().getTranslation().getDistance(poseRef.getTranslation())); - double xFeedback = linearFeedback * (currToStateTranslation.getAngle().getCos()); - double yFeedback = linearFeedback * (currToStateTranslation.getAngle().getSin()); + 0, currentState.pose().getTranslation().getDistance(setpointPose.getTranslation())); + Rotation2d currentToStateAngle = + setpointPose.getTranslation().minus(currentState.pose().getTranslation()).getAngle(); + double xFeedback = linearFeedback * currentToStateAngle.getCos(); + double yFeedback = linearFeedback * currentToStateAngle.getSin(); double thetaFeedback = thetaController.calculate( - currentState.pose().getRotation().getRadians(), poseRef.getRotation().getRadians()); + currentState.pose().getRotation().getRadians(), + setpointPose.getRotation().getRadians()); + if (atGoal()) { + if (linearController.atSetpoint()) { + xFeedback = 0; + yFeedback = 0; + } + if (thetaController.atSetpoint()) { + thetaFeedback = 0; + } + } // Return next output. return ChassisSpeeds.fromFieldRelativeSpeeds( @@ -75,6 +89,10 @@ public ChassisSpeeds calculate( currentState.pose().getRotation()); } + public boolean atSetpoint() { + return linearController.atSetpoint() && thetaController.atSetpoint(); + } + /** Within tolerance of current goal */ public boolean atGoal() { Pose2d goalPoseError = goalState.pose().relativeTo(currentState.pose()); @@ -88,6 +106,15 @@ public boolean atGoal() { < goalTolerance.velocityY() && Math.abs(currentState.angularVelocity() - goalState.angularVelocity()) < goalTolerance.angularVelocity(); + System.out.println( + "Pose: " + + withinPoseTolerance + + " Velocity: " + + withinVelocityTolerance + + " Error: " + + goalPoseError.getTranslation().getNorm() + + " " + + goalPoseError.getRotation().getRadians()); return withinPoseTolerance && withinVelocityTolerance; } From f3f990d5bae9a90253e503c29c12bfe95e5c6123 Mon Sep 17 00:00:00 2001 From: suryatho Date: Sat, 3 Feb 2024 12:56:43 -0500 Subject: [PATCH 05/12] =?UTF-8?q?Auto=20Aligning!=20=F0=9F=A4=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/RobotContainer.java | 13 ++ src/main/java/frc/robot/RobotState.java | 1 + .../frc/robot/subsystems/drive/Drive.java | 11 ++ .../subsystems/drive/DriveConstants.java | 2 +- .../planners/AutoAlignMotionPlanner.java | 169 ++++++++++++++++++ .../planners/TrajectoryMotionPlanner.java | 45 +++-- .../trajectory/HolonomicDriveController.java | 9 - 7 files changed, 217 insertions(+), 33 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/planners/AutoAlignMotionPlanner.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5488e4d2..ca49e4f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,7 @@ 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.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; @@ -194,6 +195,18 @@ private void configureButtonBindings() { () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); + controller + .a() + .whileTrue( + drive.autoAlignToPose( + () -> + AllianceFlipUtil.apply( + new Pose2d( + new Translation2d( + FieldConstants.ampCenter.getX(), + FieldConstants.ampCenter.getY() + - DriveConstants.driveConfig.trackwidthY() / 2.0), + new Rotation2d(-Math.PI / 2.0))))); // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); // controller // .a() diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index b28447ef..5dfd195c 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -139,6 +139,7 @@ public void resetPose(Pose2d initialPose) { poseBuffer.clear(); } + @AutoLogOutput(key = "Odometry/FieldVelocity") public Twist2d fieldVelocity() { Translation2d linearFieldVelocity = new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation()); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 16b69a3d..4be4f35b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.drive; +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.kinematics.*; @@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; +import frc.robot.subsystems.drive.planners.AutoAlignMotionPlanner; import frc.robot.subsystems.drive.planners.TrajectoryMotionPlanner; import frc.robot.util.GeomUtil; import frc.robot.util.swerve.ModuleLimits; @@ -31,6 +33,7 @@ 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.experimental.ExtensionMethod; import org.littletonrobotics.junction.AutoLog; @@ -56,6 +59,7 @@ public static class OdometryTimeestampInputs { private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); private final TrajectoryMotionPlanner trajectoryMotionPlanner; + private final AutoAlignMotionPlanner autoAlignMotionPlanner; private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; private SwerveSetpoint currentSetpoint = @@ -86,6 +90,7 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) .moduleLocations(DriveConstants.moduleTranslations) .build(); trajectoryMotionPlanner = new TrajectoryMotionPlanner(); + autoAlignMotionPlanner = new AutoAlignMotionPlanner(); } public void periodic() { @@ -221,6 +226,12 @@ public Command followTrajectory(Trajectory trajectory) { .until(trajectoryMotionPlanner::isFinished); } + public Command autoAlignToPose(Supplier goalPose) { + return run(() -> setVelocity(autoAlignMotionPlanner.update())) + .beforeStarting(() -> autoAlignMotionPlanner.setGoalPose(goalPose.get())) + .until(autoAlignMotionPlanner::atGoal); + } + @AutoLogOutput(key = "Drive/GyroYaw") public Rotation2d getGyroYaw() { return gyroInputs.connected diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 4ff2c32a..35fcbc2d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -139,7 +139,7 @@ public final class DriveConstants { 0.0, Units.inchesToMeters(2.0), Units.degreesToRadians(2.0), - driveConfig.maxLinearVelocity() * 0.3, + driveConfig.maxLinearVelocity() * 0.6, driveConfig.maxLinearAcceleration() * 0.5, driveConfig.maxAngularVelocity() * 0.3, driveConfig.maxAngularAcceleration() * 0.5); diff --git a/src/main/java/frc/robot/subsystems/drive/planners/AutoAlignMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/planners/AutoAlignMotionPlanner.java new file mode 100644 index 00000000..2bb12533 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/planners/AutoAlignMotionPlanner.java @@ -0,0 +1,169 @@ +package frc.robot.subsystems.drive.planners; + +import edu.wpi.first.math.controller.ProfiledPIDController; +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.trajectory.TrapezoidProfile; +import frc.robot.RobotState; +import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class AutoAlignMotionPlanner { + private static LoggedTunableNumber linearKp = + new LoggedTunableNumber("AutoAlign/driveKp", DriveConstants.autoAlignConstants.linearKp()); + private static LoggedTunableNumber linearKd = + new LoggedTunableNumber("AutoAlign/driveKd", DriveConstants.autoAlignConstants.linearKd()); + private static LoggedTunableNumber thetaKp = + new LoggedTunableNumber("AutoAlign/thetaKp", DriveConstants.autoAlignConstants.thetaKp()); + private static LoggedTunableNumber thetaKd = + new LoggedTunableNumber("AutoAlign/thetaKd", DriveConstants.autoAlignConstants.thetaKd()); + private static LoggedTunableNumber linearTolerance = + new LoggedTunableNumber( + "AutoAlign/controllerLinearTolerance", + DriveConstants.autoAlignConstants.linearTolerance()); + private static LoggedTunableNumber thetaTolerance = + new LoggedTunableNumber( + "AutoAlign/controllerThetaTolerance", DriveConstants.autoAlignConstants.thetaTolerance()); + private static LoggedTunableNumber maxLinearVelocity = + new LoggedTunableNumber( + "AutoAlign/maxLinearVelocity", DriveConstants.autoAlignConstants.maxLinearVelocity()); + private static LoggedTunableNumber maxLinearAcceleration = + new LoggedTunableNumber( + "AutoAlign/maxLinearAcceleration", + DriveConstants.autoAlignConstants.maxLinearAcceleration()); + private static LoggedTunableNumber maxAngularVelocity = + new LoggedTunableNumber( + "AutoAlign/maxAngularVelocity", DriveConstants.autoAlignConstants.maxAngularVelocity()); + private static LoggedTunableNumber maxAngularAcceleration = + new LoggedTunableNumber( + "AutoAlign/maxAngularAcceleration", + DriveConstants.autoAlignConstants.maxAngularAcceleration()); + + @Getter(onMethod_ = @AutoLogOutput(key = "AutoAlign/goalPose")) + private Pose2d goalPose = null; + + // Controllers for translation and rotation + private final ProfiledPIDController linearController; + private final ProfiledPIDController thetaController; + + // Store previous velocities for acceleration limiting + private Translation2d prevLinearVelocity; + private double prevAngularVelocity; + + public AutoAlignMotionPlanner() { + linearController = + new ProfiledPIDController( + linearKp.get(), + 0, + linearKd.get(), + new TrapezoidProfile.Constraints(maxLinearVelocity.get(), maxLinearAcceleration.get())); + linearController.setTolerance(linearTolerance.get()); + + thetaController = + new ProfiledPIDController( + thetaKp.get(), + 0, + thetaKd.get(), + new TrapezoidProfile.Constraints( + maxAngularVelocity.get(), maxAngularAcceleration.get())); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.setTolerance(thetaTolerance.get()); + } + + public void setGoalPose(Pose2d goalPose) { + this.goalPose = goalPose; + + // Reset controllers + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); + // Linear controller will control to 0 so distance is the measurement + Rotation2d rotationToGoal = + goalPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); + double velocity = + -new Translation2d(fieldVelocity.dx, fieldVelocity.dy) + .rotateBy(rotationToGoal.unaryMinus()) + .getX(); + linearController.reset( + currentPose.getTranslation().getDistance(goalPose.getTranslation()), velocity); + linearController.setGoal(0.0); + thetaController.reset(currentPose.getRotation().getRadians(), fieldVelocity.dtheta); + + prevLinearVelocity = new Translation2d(fieldVelocity.dx, fieldVelocity.dy); + prevAngularVelocity = fieldVelocity.dtheta; + } + + public ChassisSpeeds update() { + updateControllers(); + Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); + Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); + + // Calculate feedback velocities (based on position error). + double linearVelocityScalar = + linearController.calculate( + currentPose.getTranslation().getDistance(goalPose.getTranslation())) + + linearController.getSetpoint().velocity; + Rotation2d rotationToGoal = + goalPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); + double xVelocity = -linearVelocityScalar * rotationToGoal.getCos(); + double yVelocity = -linearVelocityScalar * rotationToGoal.getSin(); + + // If current angular velocity is fast enough in current direction continue in direction + double angularVelocity = + thetaController.calculate( + currentPose.getRotation().getRadians(), goalPose.getRotation().getRadians()) + + thetaController.getSetpoint().velocity; + + // Limit linear acceleration + // Forward limiting and brake limiting + Translation2d desiredLinearVelocity = new Translation2d(xVelocity, yVelocity); + Translation2d deltaVelocity = desiredLinearVelocity.minus(prevLinearVelocity); + double maxDeltaVelocity = maxLinearAcceleration.get() * 0.02; + if (deltaVelocity.getNorm() > maxDeltaVelocity) { + desiredLinearVelocity = + prevLinearVelocity.plus(deltaVelocity.times(maxDeltaVelocity / deltaVelocity.getNorm())); + } + prevLinearVelocity = new Translation2d(fieldVelocity.dx, fieldVelocity.dy); + + ChassisSpeeds fieldRelativeSpeeds = new ChassisSpeeds(xVelocity, yVelocity, angularVelocity); + Logger.recordOutput("AutoAlign/speeds", fieldRelativeSpeeds); + Logger.recordOutput("AutoAlign/linearError", linearController.getPositionError()); + Logger.recordOutput("AutoAlign/thetaError", thetaController.getPositionError()); + return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, currentPose.getRotation()); + } + + @AutoLogOutput(key = "AutoAlign/atGoal") + public boolean atGoal() { + return linearController.atGoal() && thetaController.atSetpoint(); + } + + private void updateControllers() { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> linearController.setPID(linearKp.get(), 0, linearKd.get()), + linearKp, + linearKd); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> thetaController.setPID(thetaKp.get(), 0, thetaKd.get()), + thetaKp, + thetaKd); + LoggedTunableNumber.ifChanged( + hashCode(), () -> linearController.setTolerance(linearTolerance.get()), linearTolerance); + LoggedTunableNumber.ifChanged( + hashCode(), () -> thetaController.setTolerance(thetaTolerance.get()), thetaTolerance); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> + linearController.setConstraints( + new TrapezoidProfile.Constraints( + maxLinearVelocity.get(), maxLinearAcceleration.get())), + maxLinearVelocity, + maxLinearAcceleration); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java b/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java index 55291c56..93c95313 100644 --- a/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java +++ b/src/main/java/frc/robot/subsystems/drive/planners/TrajectoryMotionPlanner.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants; import frc.robot.RobotState; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.LoggedTunableNumber; @@ -78,6 +77,7 @@ public void setTrajectory(Trajectory trajectory) { /** Output setpoint chassis speeds in */ public ChassisSpeeds update() { + updateControllers(); Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); // If disabled reset everything and stop @@ -89,28 +89,6 @@ public ChassisSpeeds update() { 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); - } - HolonomicDriveState currentState = new HolonomicDriveState( currentPose, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); @@ -138,6 +116,27 @@ public ChassisSpeeds update() { return speeds; } + private void updateControllers() { + // 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); + } + @AutoLogOutput(key = "Trajectory/finished") public boolean isFinished() { return trajectory != null diff --git a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java index 6f8ac8d4..9f7a0ad9 100644 --- a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java +++ b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java @@ -106,15 +106,6 @@ public boolean atGoal() { < goalTolerance.velocityY() && Math.abs(currentState.angularVelocity() - goalState.angularVelocity()) < goalTolerance.angularVelocity(); - System.out.println( - "Pose: " - + withinPoseTolerance - + " Velocity: " - + withinVelocityTolerance - + " Error: " - + goalPoseError.getTranslation().getNorm() - + " " - + goalPoseError.getRotation().getRadians()); return withinPoseTolerance && withinVelocityTolerance; } From 297598f36bd74fce07ee7069af6bbea2256730d4 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 5 Feb 2024 00:00:58 -0500 Subject: [PATCH 06/12] Add ModuleIOKrakenFOC.java back Use onboard controllers Add comments --- src/main/java/frc/robot/RobotContainer.java | 371 ++++++------ .../frc/robot/subsystems/drive/Drive.java | 15 +- .../subsystems/drive/DriveConstants.java | 422 +++++++------- .../frc/robot/subsystems/drive/Module.java | 59 +- .../frc/robot/subsystems/drive/ModuleIO.java | 98 +++- .../subsystems/drive/ModuleIOKrakenFOC.java | 543 +++++++++--------- .../robot/subsystems/drive/ModuleIOSim.java | 43 +- .../subsystems/drive/ModuleIOSparkMax.java | 323 ++++++----- 8 files changed, 1021 insertions(+), 853 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca49e4f8..fecb0825 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,10 +39,12 @@ import frc.robot.util.AllianceFlipUtil; import frc.robot.util.trajectory.ChoreoTrajectoryReader; import frc.robot.util.trajectory.Trajectory; + import java.io.File; import java.util.Objects; import java.util.Optional; import java.util.function.Function; + import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -52,188 +54,209 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Load robot state - private final RobotState robotState = RobotState.getInstance(); - - // Subsystems - private Drive drive; - private AprilTagVision aprilTagVision; - private Shooter shooter; - private Intake intake; - - // Controller - private final CommandXboxController controller = new CommandXboxController(0); - - // Dashboard inputs - private final LoggedDashboardChooser autoChooser; - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - switch (Constants.getMode()) { - case REAL -> { - // Real robot, instantiate hardware IO implementations\ - switch (Constants.getRobot()) { - default -> { + // Load robot state + private final RobotState robotState = RobotState.getInstance(); + + // Subsystems + private Drive drive; + private AprilTagVision aprilTagVision; + private Shooter shooter; + private Intake intake; + + // Controller + private final CommandXboxController controller = new CommandXboxController(0); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + switch (Constants.getMode()) { + case REAL -> { + // Real robot, instantiate hardware IO implementations\ + switch (Constants.getRobot()) { + default -> { + drive = + new Drive( + new GyroIOPigeon2(false), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]), + false); + aprilTagVision = + new AprilTagVision( + new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); + shooter = new Shooter(new ShooterIOSparkMax()); + intake = new Intake(new IntakeIOSparkMax()); + } + } + } + case SIM -> { + // Sim robot, instantiate physics sim IO implementations + drive = + new Drive( + new GyroIO() { + }, + new ModuleIOSim(DriveConstants.moduleConfigs[0]), + new ModuleIOSim(DriveConstants.moduleConfigs[1]), + new ModuleIOSim(DriveConstants.moduleConfigs[2]), + new ModuleIOSim(DriveConstants.moduleConfigs[3]), + false); + shooter = new Shooter(new ShooterIOSim()); + intake = new Intake(new IntakeIOSim()); + } + default -> { + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + false); + shooter = new Shooter(new ShooterIO() { + }); + intake = new Intake(new IntakeIO() { + }); + } + } + + if (drive == null) { drive = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); - shooter = new Shooter(new ShooterIOSparkMax()); - intake = new Intake(new IntakeIOSparkMax()); - } + new Drive( + new GyroIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + false); } - } - case SIM -> { - // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(DriveConstants.moduleConfigs[0]), - new ModuleIOSim(DriveConstants.moduleConfigs[1]), - new ModuleIOSim(DriveConstants.moduleConfigs[2]), - new ModuleIOSim(DriveConstants.moduleConfigs[3])); - shooter = new Shooter(new ShooterIOSim()); - intake = new Intake(new IntakeIOSim()); - } - default -> { - // Replayed robot, disable IO implementations - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - shooter = new Shooter(new ShooterIO() {}); - intake = new Intake(new IntakeIO() {}); - } - } - if (drive == null) { - drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - } + if (aprilTagVision == null) { + aprilTagVision = new AprilTagVision(); + } - if (aprilTagVision == null) { - aprilTagVision = new AprilTagVision(); - } + if (shooter == null) { + shooter = new Shooter(new ShooterIO() { + }); + } - if (shooter == null) { - shooter = new Shooter(new ShooterIO() {}); - } + if (intake == null) { + intake = new Intake(new IntakeIO() { + }); + } - if (intake == null) { - intake = new Intake(new IntakeIO() {}); + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + // Set up feedforward characterization + autoChooser.addOption( + "Drive FF Characterization", + new FeedForwardCharacterization( + drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) + .finallyDo(drive::endCharacterization)); + autoChooser.addOption( + "Left Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runLeftCharacterizationVolts, + shooter::getLeftCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + autoChooser.addOption( + "Right Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runRightCharacterizationVolts, + shooter::getRightCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + + // Testing autos paths + Function> trajectoryCommandFactory = + trajectoryFile -> { + Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); + return trajectory.map( + traj -> + Commands.runOnce( + () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) + .andThen(drive.followTrajectory(traj))); + }; + final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); + for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { + trajectoryCommandFactory + .apply(trajectoryFile) + .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); + } + + // Configure the button bindings + configureButtonBindings(); } - autoChooser = new LoggedDashboardChooser<>("Auto Choices"); - // Set up feedforward characterization - autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) - .finallyDo(drive::endCharacterization)); - autoChooser.addOption( - "Left Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, - shooter::runLeftCharacterizationVolts, - shooter::getLeftCharacterizationVelocity) - .beforeStarting(() -> shooter.setCharacterizing(true)) - .finallyDo(() -> shooter.setCharacterizing(false))); - autoChooser.addOption( - "Right Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, - shooter::runRightCharacterizationVolts, - shooter::getRightCharacterizationVelocity) - .beforeStarting(() -> shooter.setCharacterizing(true)) - .finallyDo(() -> shooter.setCharacterizing(false))); - - // Testing autos paths - Function> trajectoryCommandFactory = - trajectoryFile -> { - Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); - return trajectory.map( - traj -> - Commands.runOnce( - () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - .andThen(drive.followTrajectory(traj))); - }; - final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); - for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { - trajectoryCommandFactory - .apply(trajectoryFile) - .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + controller + .a() + .whileTrue( + drive.autoAlignToPose( + () -> + AllianceFlipUtil.apply( + new Pose2d( + new Translation2d( + FieldConstants.ampCenter.getX(), + FieldConstants.ampCenter.getY() + - DriveConstants.driveConfig.trackwidthY() / 2.0), + new Rotation2d(-Math.PI / 2.0))))); + // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); + // controller + // .a() + // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), + // intake::running)); + // controller + // .x() + // .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), + // intake::running)); + controller + .b() + .onTrue( + Commands.runOnce( + () -> + robotState.resetPose( + new Pose2d( + robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) + .ignoringDisable(true)); + controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); } - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - controller - .a() - .whileTrue( - drive.autoAlignToPose( - () -> - AllianceFlipUtil.apply( - new Pose2d( - new Translation2d( - FieldConstants.ampCenter.getX(), - FieldConstants.ampCenter.getY() - - DriveConstants.driveConfig.trackwidthY() / 2.0), - new Rotation2d(-Math.PI / 2.0))))); - // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); - // controller - // .a() - // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), - // intake::running)); - // controller - // .x() - // .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), - // intake::running)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - robotState.resetPose( - new Pose2d( - robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) - .ignoringDisable(true)); - controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.get(); - } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autoChooser.get(); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 4be4f35b..b2362b92 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -76,13 +76,14 @@ public static class OdometryTimeestampInputs { private boolean characterizing = false; private double characterizationVolts = 0.0; - public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) { + public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br, + boolean useMotorConroller) { System.out.println("[Init] Creating Drive"); this.gyroIO = gyroIO; - modules[0] = new Module(fl, 0); - modules[1] = new Module(fr, 1); - modules[2] = new Module(bl, 2); - modules[3] = new Module(br, 3); + modules[0] = new Module(fl, 0, useMotorConroller); + modules[1] = new Module(fr, 1, useMotorConroller); + modules[2] = new Module(bl, 2, useMotorConroller); + modules[3] = new Module(br, 3, useMotorConroller); setpointGenerator = SwerveSetpointGenerator.builder() @@ -203,6 +204,7 @@ public void runCharacterizationVolts(double volts) { characterizationVolts = volts; } + /** Disables the characterization mode. */ public void endCharacterization() { characterizing = false; } @@ -216,16 +218,19 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } + /** Set brake mode enabled */ public void setBrakeMode(boolean enabled) { Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } + /** Follows a trajectory using the trajectory motion planner. */ public Command followTrajectory(Trajectory trajectory) { return run(() -> setVelocity(trajectoryMotionPlanner.update())) .beforeStarting(() -> trajectoryMotionPlanner.setTrajectory(trajectory)) .until(trajectoryMotionPlanner::isFinished); } + /** Auto aligns to a pose using the auto align motion planner. */ public Command autoAlignToPose(Supplier goalPose) { return run(() -> setVelocity(autoAlignMotionPlanner.update())) .beforeStarting(() -> autoAlignMotionPlanner.setGoalPose(goalPose.get())) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 35fcbc2d..aa9480be 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -11,208 +11,232 @@ import frc.robot.Constants; import frc.robot.util.swerve.ModuleLimits; -/** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ +/** + * All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) + */ public final class DriveConstants { - public static DriveConfig driveConfig = - switch (Constants.getRobot()) { - case SIMBOT, COMPBOT -> - new DriveConfig( - Units.inchesToMeters(25.0), - Units.inchesToMeters(25.0), - Units.feetToMeters(13.05), - Units.feetToMeters(30.02), - 8.86, - 43.97); - default -> - new DriveConfig( - Units.inchesToMeters(26.0), - Units.inchesToMeters(26.0), - Units.feetToMeters(12.16), - Units.feetToMeters(21.32), - 7.93, - 29.89); - }; - public static final double wheelRadius = Units.inchesToMeters(2.0); - public static final Translation2d[] moduleTranslations = - new Translation2d[] { - 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 = - new SwerveDriveKinematics(moduleTranslations); - - public static final double odometryFrequency = - switch (Constants.getRobot()) { - case SIMBOT -> 50.0; - case RAINBOWT -> 100.0; - case COMPBOT -> 250.0; - }; - - public static final Matrix odometryStateStdDevs = - switch (Constants.getRobot()) { - default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); - }; - - public static ModuleConfig[] moduleConfigs = - switch (Constants.getRobot()) { - case COMPBOT, RAINBOWT -> - new ModuleConfig[] { - new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), - new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), - new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), - new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) + // TODO: get effective wheel radius + public static final double wheelRadius = Units.inchesToMeters(2.0); + + // For Kraken + public static class KrakenDriveConstants { + public static final boolean useTorqueCurrentFOC = true; + public static final boolean useMotionMagic = false; + public static final double motionMagicCruiseVelocity = 0.0; + public static final double motionMagicAcceleration = 0.0; + } + + // Drive Constants + public static DriveConfig driveConfig = + switch (Constants.getRobot()) { + case SIMBOT, COMPBOT -> new DriveConfig( + Units.inchesToMeters(25.0), + Units.inchesToMeters(25.0), + Units.feetToMeters(13.05), + Units.feetToMeters(30.02), + 8.86, + 43.97); + default -> new DriveConfig( + Units.inchesToMeters(26.0), + Units.inchesToMeters(26.0), + Units.feetToMeters(12.16), + Units.feetToMeters(21.32), + 7.93, + 29.89); + }; + public static final Translation2d[] moduleTranslations = + new Translation2d[]{ + 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 = + new SwerveDriveKinematics(moduleTranslations); + + // Odometry Constants + public static final double odometryFrequency = + switch (Constants.getRobot()) { + case SIMBOT -> 50.0; + case RAINBOWT -> 100.0; + case COMPBOT -> 250.0; + }; + + public static final Matrix odometryStateStdDevs = + switch (Constants.getRobot()) { + default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); + }; + + // Module Constants + public static ModuleConfig[] moduleConfigs = + switch (Constants.getRobot()) { + case COMPBOT, RAINBOWT -> new ModuleConfig[]{ + new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), + new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), + new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), + new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) + }; + case SIMBOT -> { + ModuleConfig[] configs = new ModuleConfig[4]; + for (int i = 0; i < configs.length; i++) + configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); + yield configs; + } }; - case SIMBOT -> { - ModuleConfig[] configs = new ModuleConfig[4]; - for (int i = 0; i < configs.length; i++) - configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); - yield configs; + + public static ModuleConstants moduleConstants = + switch (Constants.getRobot()) { + case COMPBOT -> new ModuleConstants( + 1.0, + 0.0, + 0.0, + 0.0, + 200.0, + 20.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + case RAINBOWT -> new ModuleConstants( + 0.1, + 0.13, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L2.reduction, + Mk4iReductions.TURN.reduction); + case SIMBOT -> new ModuleConstants( + 0.014, + 0.134, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + }; + + public static ModuleLimits moduleLimits = + new ModuleLimits( + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearVelocity() * 5, + Units.degreesToRadians(1080.0)); + + // Trajectory Following + public static TrajectoryConstants trajectoryConstants = + switch (Constants.getRobot()) { + case COMPBOT, RAINBOWT -> new TrajectoryConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(4.0), + Units.degreesToRadians(5.0), + Units.inchesToMeters(5.0), + Units.degreesToRadians(7.0), + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); + case SIMBOT -> new TrajectoryConstants( + 4.0, + 0.0, + 2.0, + 0.0, + Units.inchesToMeters(4.0), + Units.degreesToRadians(5.0), + Units.inchesToMeters(5.0), + Units.degreesToRadians(7.0), + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); + }; + + // Auto Align + public static AutoAlignConstants autoAlignConstants = + new AutoAlignConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(2.0), + Units.degreesToRadians(2.0), + driveConfig.maxLinearVelocity() * 0.6, + driveConfig.maxLinearAcceleration() * 0.5, + driveConfig.maxAngularVelocity() * 0.3, + driveConfig.maxAngularAcceleration() * 0.5); + + // Swerve Heading Control + public static HeadingControllerConstants headingControllerConstants = + switch (Constants.getRobot()) { + default -> new HeadingControllerConstants(3.0, 0.0); + }; + + public record DriveConfig( + double trackwidthX, + double trackwidthY, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) { + public double driveBaseRadius() { + return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); } - }; - - public static ModuleConstants moduleConstants = - switch (Constants.getRobot()) { - case COMPBOT, RAINBOWT -> - new ModuleConstants( - 0.1, - 0.13, - 0.1, - 0.0, - 10.0, - 0.0, - Mk4iReductions.L2.reduction, - Mk4iReductions.TURN.reduction); - case SIMBOT -> - new ModuleConstants( - 0.014, - 0.134, - 0.1, - 0.0, - 10.0, - 0.0, - Mk4iReductions.L2.reduction, - Mk4iReductions.TURN.reduction); - }; - - public static ModuleLimits moduleLimits = - new ModuleLimits( - driveConfig.maxLinearVelocity(), - driveConfig.maxLinearVelocity() * 5, - Units.degreesToRadians(1080.0)); - - public static TrajectoryConstants trajectoryConstants = - switch (Constants.getRobot()) { - case COMPBOT, RAINBOWT -> - new TrajectoryConstants( - 6.0, - 0.0, - 5.0, - 0.0, - Units.inchesToMeters(4.0), - Units.degreesToRadians(5.0), - Units.inchesToMeters(5.0), - Units.degreesToRadians(7.0), - driveConfig.maxLinearVelocity() / 2.0, - driveConfig.maxAngularVelocity() / 2.0); - case SIMBOT -> - new TrajectoryConstants( - 4.0, - 0.0, - 2.0, - 0.0, - Units.inchesToMeters(4.0), - Units.degreesToRadians(5.0), - Units.inchesToMeters(5.0), - Units.degreesToRadians(7.0), - driveConfig.maxLinearVelocity() / 2.0, - driveConfig.maxAngularVelocity() / 2.0); - }; - - public static AutoAlignConstants autoAlignConstants = - new AutoAlignConstants( - 6.0, - 0.0, - 5.0, - 0.0, - Units.inchesToMeters(2.0), - Units.degreesToRadians(2.0), - driveConfig.maxLinearVelocity() * 0.6, - driveConfig.maxLinearAcceleration() * 0.5, - driveConfig.maxAngularVelocity() * 0.3, - driveConfig.maxAngularAcceleration() * 0.5); - - public static HeadingControllerConstants headingControllerConstants = - switch (Constants.getRobot()) { - default -> new HeadingControllerConstants(3.0, 0.0); - }; - - public record DriveConfig( - double trackwidthX, - double trackwidthY, - double maxLinearVelocity, - double maxLinearAcceleration, - double maxAngularVelocity, - double maxAngularAcceleration) { - public double driveBaseRadius() { - return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); } - } - - public record ModuleConfig( - int driveID, - int turnID, - int absoluteEncoderChannel, - Rotation2d absoluteEncoderOffset, - boolean turnMotorInverted) {} - - public record ModuleConstants( - double ffKs, - double ffKv, - double driveKp, - double driveKd, - double turnKp, - double turnKd, - double driveReduction, - double turnReduction) {} - - public record TrajectoryConstants( - double linearKp, - double linearKd, - double thetaKp, - double thetaKd, - double linearTolerance, - double thetaTolerance, - double goalLinearTolerance, - double goalThetaTolerance, - double linearVelocityTolerance, - double angularVelocityTolerance) {} - - public record AutoAlignConstants( - double linearKp, - double linearKd, - double thetaKp, - double thetaKd, - double linearTolerance, - double thetaTolerance, - double maxLinearVelocity, - double maxLinearAcceleration, - double maxAngularVelocity, - double maxAngularAcceleration) {} - - public record HeadingControllerConstants(double Kp, double Kd) {} - - private enum Mk4iReductions { - L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), - L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), - TURN((150.0 / 7.0)); - - final double reduction; - - Mk4iReductions(double reduction) { - this.reduction = reduction; + + public record ModuleConfig( + int driveID, + int turnID, + int absoluteEncoderChannel, + Rotation2d absoluteEncoderOffset, + boolean turnMotorInverted) { + } + + public record ModuleConstants( + double ffKs, + double ffKv, + double driveKp, + double driveKd, + double turnKp, + double turnKd, + double driveReduction, + double turnReduction) { + } + + public record TrajectoryConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double goalLinearTolerance, + double goalThetaTolerance, + double linearVelocityTolerance, + double angularVelocityTolerance) { + } + + public record AutoAlignConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) { + } + + public record HeadingControllerConstants(double Kp, double Kd) { + } + + private enum Mk4iReductions { + L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), + L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), + TURN((150.0 / 7.0)); + + final double reduction; + + Mk4iReductions(double reduction) { + this.reduction = reduction; + } } - } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index e37282a4..cb21e694 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -9,8 +9,6 @@ import org.littletonrobotics.junction.Logger; public class Module { - private static final LoggedTunableNumber wheelRadius = - new LoggedTunableNumber("Drive/Module/WheelRadius", DriveConstants.wheelRadius); private static final LoggedTunableNumber driveKp = new LoggedTunableNumber("Drive/Module/DriveKp", DriveConstants.moduleConstants.driveKp()); private static final LoggedTunableNumber driveKd = @@ -24,9 +22,10 @@ public class Module { private static final LoggedTunableNumber turnKd = new LoggedTunableNumber("Drive/Module/TurnKd", DriveConstants.moduleConstants.turnKd()); + private final int index; private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; + private final boolean useMotorController; private final PIDController driveController = new PIDController( @@ -34,13 +33,14 @@ public class Module { private final PIDController turnController = new PIDController( DriveConstants.moduleConstants.turnKp(), 0.0, DriveConstants.moduleConstants.turnKd()); - private SimpleMotorFeedforward driveFf = + private SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward( DriveConstants.moduleConstants.ffKs(), DriveConstants.moduleConstants.ffKv(), 0.0); - public Module(ModuleIO io, int index) { + public Module(ModuleIO io, int index, boolean useMotorController) { this.io = io; this.index = index; + this.useMotorController = useMotorController; turnController.enableContinuousInput(-Math.PI, Math.PI); } @@ -50,24 +50,45 @@ public void updateInputs() { io.updateInputs(inputs); Logger.processInputs("Drive/Module" + index, inputs); - driveController.setPID(driveKp.get(), 0, driveKd.get()); + // Update FF and controllers LoggedTunableNumber.ifChanged( - hashCode(), - () -> driveFf = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0), - driveKs, - driveKv); - turnController.setPID(turnKp.get(), 0, driveKd.get()); + hashCode(), + () -> { + driveFF = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0); + io.setDriveFF(driveKs.get(), driveKv.get(), 0); + }, + driveKs, + driveKv); + if (useMotorController) { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> io.setDrivePID(driveKp.get(), 0, driveKd.get()), + driveKp, driveKd); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> io.setTurnPID(turnKp.get(), 0, turnKd.get()), + turnKp, turnKd); + } else { + driveController.setPID(driveKp.get(), 0, driveKd.get()); + turnController.setPID(turnKp.get(), 0, turnKd.get()); + } } public void runSetpoint(SwerveModuleState setpoint) { - io.setDriveVoltage( - driveController.calculate( - inputs.driveVelocityRadPerSec, - setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) - + driveFf.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); - io.setTurnVoltage( - turnController.calculate( - inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); + if (useMotorController) { + io.setDriveVelocitySetpoint(setpoint.speedMetersPerSecond, + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + io.setTurnPositionSetpoint(setpoint.angle.getRadians()); + } else { + io.setDriveVoltage( + driveController.calculate( + inputs.driveVelocityRadPerSec, + setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) + + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + io.setTurnVoltage( + turnController.calculate( + inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); + } } public void runCharacterization(double volts) { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 407cd2c9..40158763 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -17,40 +17,86 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog - class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double driveCurrentAmps = 0.0; + @AutoLog + class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveCurrentAmps = 0.0; - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double turnCurrentAmps = 0.0; + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnCurrentAmps = 0.0; - public double[] odometryDrivePositionsMeters = new double[] {}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; - } + public double[] odometryDrivePositionsMeters = new double[]{}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[]{}; + } - /** Updates the set of loggable inputs. */ - default void updateInputs(ModuleIOInputs inputs) {} + /** + * Updates the set of loggable inputs. + */ + default void updateInputs(ModuleIOInputs inputs) { + } - default void setDriveVoltage(double volts) {} + /** + * Run drive motor at volts + */ + default void setDriveVoltage(double volts) { + } - default void setTurnVoltage(double volts) {} + /** + * Run turn motor at volts + */ + default void setTurnVoltage(double volts) { + } - default void setDriveVelocitySetpoint(double velocityRadPerSec, double ffVolts) {} + /** + * Set drive velocity setpoint + */ + default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + } - default void setTurnPositionSetpoint(double angleRadians, double ffVolts) {} + /** + * Set turn position setpoint + */ + default void setTurnPositionSetpoint(double angleRads) { + } - /** Enable or disable brake mode on the drive motor. */ - default void setDriveBrakeMode(boolean enable) {} + /** + * Configure drive PID + */ + default void setDrivePID(double kP, double kI, double kD) { + } - /** Enable or disable brake mode on the turn motor. */ - default void setTurnBrakeMode(boolean enable) {} + /** + * Configure turn PID + */ + default void setTurnPID(double kP, double kI, double kD) { + } - /** Disable output to all motors */ - default void stop() {} + /** + * Configure drive feedforward for TorqueCurrentFOC + */ + default void setDriveFF(double kS, double kV, double kA) { + } + + /** + * Enable or disable brake mode on the drive motor. + */ + default void setDriveBrakeMode(boolean enable) { + } + + /** + * Enable or disable brake mode on the turn motor. + */ + default void setTurnBrakeMode(boolean enable) { + } + + /** + * Disable output to all motors + */ + default void stop() { + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java index 0368f494..15fbd768 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java @@ -1,279 +1,264 @@ -//// Copyright 2021-2024 FRC 6328 -//// http://github.com/Mechanical-Advantage -//// -//// This program is free software; you can redistribute it and/or -//// modify it under the terms of the GNU General Public License -//// version 3 as published by the Free Software Foundation or -//// available in the root directory of this project. -//// -//// This program is distributed in the hope that it will be useful, -//// but WITHOUT ANY WARRANTY; without even the implied warranty of -//// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -//// GNU General Public License for more details. -// -// package frc.robot.subsystems.drive; -// -// import static frc.robot.subsystems.drive.DriveConstants.*; -// -// import com.ctre.phoenix6.BaseStatusSignal; -// import com.ctre.phoenix6.StatusCode; -// import com.ctre.phoenix6.StatusSignal; -// import com.ctre.phoenix6.configs.MotorOutputConfigs; -// import com.ctre.phoenix6.configs.TalonFXConfiguration; -// import com.ctre.phoenix6.controls.NeutralOut; -// import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -// import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -// import com.ctre.phoenix6.controls.VoltageOut; -// import com.ctre.phoenix6.hardware.TalonFX; -// import com.ctre.phoenix6.signals.InvertedValue; -// import com.ctre.phoenix6.signals.NeutralModeValue; -// import edu.wpi.first.math.geometry.Rotation2d; -// import edu.wpi.first.math.kinematics.SwerveModuleState; -// import edu.wpi.first.math.util.Units; -// import edu.wpi.first.wpilibj.AnalogInput; -// import edu.wpi.first.wpilibj.RobotController; -// import java.util.Queue; -// import java.util.function.Supplier; -// -// public class ModuleIOKrakenFOC implements ModuleIO { -// // Reseed relative encoder to absolute encoder value -// private static final int reseedRelativeEncoderCounts = 100; -// private int currentReseedCount = reseedRelativeEncoderCounts; -// -// private final StatusSignal drivePosition; -// private final StatusSignal driveVelocity; -// private final StatusSignal driveAppliedVolts; -// private final StatusSignal driveCurrent; -// -// private final StatusSignal turnPosition; -// private final Supplier turnAbsolutePosition; -// private final StatusSignal turnVelocity; -// private final StatusSignal turnAppliedVolts; -// private final StatusSignal turnCurrent; -// private final boolean isTurnMotorInverted = true; -// -// // queues -// private final Queue drivePositionQueue; -// private final Queue turnPositionQueue; -// -// private final TalonFX driveTalon; -// private final TalonFX turnTalon; -// private final AnalogInput turnAbsoluteEncoder; -// private final Rotation2d absoluteEncoderOffset; -// -// // Setpoint control -// private final VelocityTorqueCurrentFOC driveVelocityControl = -// new VelocityTorqueCurrentFOC(0, 0, 0, 1, false, false, false); -// private final PositionTorqueCurrentFOC turnPositionControl = -// new PositionTorqueCurrentFOC(0.0, 0.0, 0.0, 1, false, false, false); -// private final VoltageOut driveVoltageControl = new VoltageOut(0.0).withEnableFOC(true); -// private final NeutralOut driveBrake = new NeutralOut(); -// private final NeutralOut turnBrake = new NeutralOut(); -// private Mode currentMode = Mode.SETPOINT; -// private double driveSpeedSetpoint = 0.0; -// private Rotation2d turnAngleSetpoint = new Rotation2d(); -// private double driveVoltage = 0.0; -// -// public ModuleIOKrakenFOC(ModuleConfig config) { -// // init controllers and encoders from config constants -// driveTalon = new TalonFX(config.driveID()); -// turnTalon = new TalonFX(config.turnID()); -// turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); -// absoluteEncoderOffset = config.absoluteEncoderOffset(); -// -// var driveConfig = new TalonFXConfiguration(); -// driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; -// driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; -// driveConfig.Voltage.PeakForwardVoltage = 12.0; -// driveConfig.Voltage.PeakReverseVoltage = -12.0; -// // TUNE PID CONSTANTS -// driveConfig.Slot0.kP = moduleConstants.driveKp(); -// driveConfig.Slot0.kI = 0.0; -// driveConfig.Slot0.kD = moduleConstants.driveKd(); -// driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40; -// driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40; -// -// var turnConfig = new TalonFXConfiguration(); -// turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; -// turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; -// turnConfig.Voltage.PeakForwardVoltage = 12.0; -// turnConfig.Voltage.PeakReverseVoltage = -12.0; -// // TUNE PID CONSTANTS -// turnConfig.Slot0.kP = moduleConstants.turnKp(); -// turnConfig.Slot0.kI = 0.0; -// turnConfig.Slot0.kD = moduleConstants.turnKd(); -// turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 30; -// turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -30; -// turnConfig.ClosedLoopGeneral.ContinuousWrap = true; -// // Map rotation of motor shaft to 1 rotation of turn shaft -// turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); -// -// // Apply configs -// for (int i = 0; i < 4; i++) { -// boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; -// setDriveBrakeMode(true); -// error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); -// setTurnBrakeMode(true); -// if (!error) break; -// } -// -// // Get signals and set update rate -// // 50hz signals -// drivePosition = driveTalon.getPosition(); -// driveVelocity = driveTalon.getVelocity(); -// driveAppliedVolts = driveTalon.getMotorVoltage(); -// driveCurrent = driveTalon.getStatorCurrent(); -// -// turnPosition = turnTalon.getPosition(); -// turnAbsolutePosition = -// () -> -// Rotation2d.fromRadians( -// turnAbsoluteEncoder.getVoltage() -// / RobotController.getVoltage5V() -// * 2.0 -// * Math.PI) -// .minus(absoluteEncoderOffset); -// turnVelocity = turnTalon.getVelocity(); -// turnAppliedVolts = turnTalon.getMotorVoltage(); -// turnCurrent = turnTalon.getStatorCurrent(); -// -// // 250hz signals -// drivePositionQueue = -// PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); -// turnPositionQueue = -// PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); -// -// BaseStatusSignal.setUpdateFrequencyForAll( -// 50.0, -// driveVelocity, -// driveAppliedVolts, -// driveCurrent, -// turnVelocity, -// turnAppliedVolts, -// turnCurrent); -// BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); -// driveTalon.optimizeBusUtilization(); -// turnTalon.optimizeBusUtilization(); -// } -// -// @Override -// public void updateInputs(ModuleIOInputs inputs) { -// BaseStatusSignal.refreshAll( -// drivePosition, -// driveVelocity, -// driveAppliedVolts, -// driveCurrent, -// turnPosition, -// turnVelocity, -// turnAppliedVolts, -// turnCurrent); -// -// inputs.drivePositionRad = -// Units.rotationsToRadians(drivePosition.getValueAsDouble()) -// / moduleConstants.driveReduction(); -// inputs.driveVelocityRadPerSec = -// Units.rotationsToRadians(driveVelocity.getValueAsDouble()) -// / moduleConstants.driveReduction(); -// inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); -// inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; -// -// inputs.turnAbsolutePosition = turnAbsolutePosition.get(); -// inputs.turnPosition = -// Rotation2d.fromRotations(turnPosition.getValueAsDouble() / -// moduleConstants.turnReduction()); -// inputs.turnVelocityRadPerSec = -// Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / -// moduleConstants.turnReduction(); -// inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); -// inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; -// -// inputs.odometryDrivePositionsMeters = -// drivePositionQueue.stream() -// .mapToDouble( -// signal -> -// Units.rotationsToRadians(signal) -// * wheelRadius -// / moduleConstants.driveReduction()) -// .toArray(); -// inputs.odometryTurnPositions = -// turnPositionQueue.stream() -// .map((signal) -> Rotation2d.fromRotations(signal / moduleConstants.turnReduction())) -// .toArray(Rotation2d[]::new); -// drivePositionQueue.clear(); -// turnPositionQueue.clear(); -// } -// -// @Override -// public void periodic() { -// // Only reseed if turn abs encoder is online -// if (++currentReseedCount >= reseedRelativeEncoderCounts -// && turnAbsoluteEncoder.getVoltage() != 0.0) { -// // heading of wheel --> turn motor shaft rotations -// // timeout of 20 ms -// boolean passed = -// turnTalon.setPosition( -// Units.radiansToRotations(turnAbsolutePosition.get().getRadians()) -// * moduleConstants.turnReduction(), -// 0.02) -// == StatusCode.OK; -// // redo next cycle if did not pass -// currentReseedCount = passed ? 0 : currentReseedCount; -// } -// -// // Control motors -// if (currentMode == Mode.NEUTRAL) { -// driveTalon.setControl(driveBrake); -// turnTalon.setControl(turnBrake); -// } else { -// if (currentMode == Mode.SETPOINT) { -// driveTalon.setControl(driveVelocityControl.withVelocity(driveSpeedSetpoint)); -// } else if (currentMode == Mode.CHARACTERIZATION) { -// driveTalon.setControl(driveVoltageControl.withOutput(driveVoltage)); -// } -// turnTalon.setControl(turnPositionControl.withPosition(turnAngleSetpoint.getRotations())); -// } -// } -// -// @Override -// public void runSetpoint(SwerveModuleState state) { -// currentMode = Mode.SETPOINT; -// driveSpeedSetpoint = state.speedMetersPerSecond / wheelRadius; -// turnAngleSetpoint = state.angle; -// } -// -// @Override -// public void runCharacterization(double volts) { -// currentMode = Mode.CHARACTERIZATION; -// driveVoltage = volts; -// } -// -// @Override -// public void setDriveBrakeMode(boolean enable) { -// var config = new MotorOutputConfigs(); -// config.Inverted = InvertedValue.CounterClockwise_Positive; -// config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; -// driveTalon.getConfigurator().apply(config); -// } -// -// @Override -// public void setTurnBrakeMode(boolean enable) { -// var config = new MotorOutputConfigs(); -// config.Inverted = -// isTurnMotorInverted -// ? InvertedValue.Clockwise_Positive -// : InvertedValue.CounterClockwise_Positive; -// config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; -// turnTalon.getConfigurator().apply(config); -// } -// -// @Override -// public void stop() { -// currentMode = Mode.NEUTRAL; -// } -// -// private enum Mode { -// SETPOINT, -// CHARACTERIZATION, -// NEUTRAL; -// } -// } +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.*; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.RobotController; + +import java.util.Queue; +import java.util.function.Supplier; + +public class ModuleIOKrakenFOC implements ModuleIO { + // Hardware + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final AnalogInput turnAbsoluteEncoder; + private final Rotation2d absoluteEncoderOffset; + + // Status Signals + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnPosition; + private final Supplier turnAbsolutePosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private static final int shouldResetCounts = 100; + private int resetCounter = shouldResetCounts; + + private Slot0Configs driveFeedbackConfig = new Slot0Configs(); + private Slot0Configs turnFeedbackConfig = new Slot0Configs(); + + public ModuleIOKrakenFOC(ModuleConfig config) { + // Init controllers and encoders from config constants + driveTalon = new TalonFX(config.driveID(), "canivore"); + turnTalon = new TalonFX(config.turnID(), "canivore"); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + + // Config Motors + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.Voltage.PeakForwardVoltage = 12.0; + driveConfig.Voltage.PeakReverseVoltage = -12.0; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40; + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; + turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; + turnConfig.Voltage.PeakForwardVoltage = 12.0; + turnConfig.Voltage.PeakReverseVoltage = -12.0; + turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 30; + turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -30; + turnConfig.MotorOutput.Inverted = config.turnMotorInverted() ? + InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + + // If in motoControl mode, set reference points in rotations convert from radians + // Affects getPosition() and getVelocity() + driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); + turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + + // Config Motion Magic + if (KrakenDriveConstants.useMotionMagic) { + turnConfig.MotionMagic.MotionMagicCruiseVelocity = KrakenDriveConstants.motionMagicCruiseVelocity; + turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; + } + + // Apply configs + for (int i = 0; i < 4; i++) { + boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; + setDriveBrakeMode(true); + error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); + setTurnBrakeMode(true); + if (!error) break; + } + + // Get signals and set update rate + // 100hz signals + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + turnAbsolutePosition = + () -> + Rotation2d.fromRadians( + turnAbsoluteEncoder.getVoltage() + / RobotController.getVoltage5V() + * 2.0 + * Math.PI) + .minus(absoluteEncoderOffset); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + // 250hz signals + drivePosition = driveTalon.getPosition(); + turnPosition = turnTalon.getPosition(); + BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + + // TODO: what does this do? +// driveTalon.optimizeBusUtilization(); +// turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCounts + && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { + turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); + resetCounter = 0; + } + + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + + inputs.turnAbsolutePosition = turnAbsolutePosition.get(); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble( + signalValue -> Units.rotationsToRadians(signalValue) * wheelRadius) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map(Rotation2d::fromRotations) + .toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts) + .withEnableFOC(true)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts) + .withEnableFOC(true)); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); + } else { + driveTalon.setControl(new VelocityVoltage(velocityRotationsPerSec) + .withFeedForward(ffVolts) + .withEnableFOC(true)); + } + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + double angleRotations = Units.radiansToRotations(angleRads); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); + } else { + turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); + } + } else { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicVoltage(angleRotations) + .withEnableFOC(true)); + } else { + turnTalon.setControl(new PositionVoltage(angleRotations) + .withEnableFOC(true)); + } + } + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveFeedbackConfig.kP = kP; + driveFeedbackConfig.kI = kI; + driveFeedbackConfig.kD = kD; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnFeedbackConfig.kP = kP; + turnFeedbackConfig.kI = kI; + turnFeedbackConfig.kD = kD; + turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); + } + + @Override + public void setDriveFF(double kS, double kV, double kA) { + driveFeedbackConfig.kS = kS; + driveFeedbackConfig.kV = kV; + driveFeedbackConfig.kA = kA; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveTalon.setNeutralMode(enable ? + NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnTalon.setNeutralMode(enable ? + NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void stop() { + driveTalon.setControl(new NeutralOut()); + turnTalon.setControl(new NeutralOut()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 8fc2b247..6f72e9bc 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -16,39 +16,34 @@ import static frc.robot.subsystems.drive.DriveConstants.*; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -/** - * Physics sim implementation of module IO. - * - *

Uses two flywheel sims for the drive and turn motors, with the absolute position initialized - * to a random value. The flywheel sims are not physically accurate, but provide a decent - * approximation for the behavior of the module. - */ public class ModuleIOSim implements ModuleIO { - private static final double LOOP_PERIOD_SECS = 0.02; private final DCMotorSim driveSim = new DCMotorSim(DCMotor.getKrakenX60(1), moduleConstants.driveReduction(), 0.025); private final DCMotorSim turnSim = new DCMotorSim(DCMotor.getKrakenX60(1), moduleConstants.turnReduction(), 0.004); + private final PIDController driveFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); + private final PIDController turnFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); + private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; private final Rotation2d turnAbsoluteInitPosition; - private Rotation2d turnAbsolutePosition; public ModuleIOSim(ModuleConfig config) { turnAbsoluteInitPosition = config.absoluteEncoderOffset(); - turnAbsolutePosition = turnAbsoluteInitPosition; + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(ModuleIOInputs inputs) { - driveSim.update(LOOP_PERIOD_SECS); - turnSim.update(LOOP_PERIOD_SECS); + driveSim.update(0.02); + turnSim.update(0.02); inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); @@ -57,7 +52,6 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); - turnAbsolutePosition = inputs.turnAbsolutePosition; inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; @@ -79,6 +73,29 @@ public void setTurnVoltage(double volts) { turnSim.setInputVoltage(turnAppliedVolts); } + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + setDriveVoltage( + driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) + + ffVolts); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + setTurnVoltage( + turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveFeedback.setPID(kP, kI, kD); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnFeedback.setPID(kP, kI, kD); + } + @Override public void stop() { driveSim.setInputVoltage(0.0); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 38112b06..d4a29f16 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -15,153 +15,200 @@ import static frc.robot.subsystems.drive.DriveConstants.*; +import com.revrobotics.*; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; + import java.util.Queue; import java.util.function.Supplier; public class ModuleIOSparkMax implements ModuleIO { - private final CANSparkMax driveMotor; - private final CANSparkMax turnMotor; - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - - // Queues - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final Rotation2d absoluteEncoderOffset; - private final Supplier absoluteEncoderValue; - - public ModuleIOSparkMax(ModuleConfig config) { - // Init motor & encoder objects - driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); - turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); - driveEncoder = driveMotor.getEncoder(); - turnRelativeEncoder = turnMotor.getEncoder(); - - driveMotor.restoreFactoryDefaults(); - turnMotor.restoreFactoryDefaults(); - driveMotor.setCANTimeout(250); - turnMotor.setCANTimeout(250); - - for (int i = 0; i < 4; i++) { - turnMotor.setInverted(config.turnMotorInverted()); - driveMotor.setSmartCurrentLimit(40); - turnMotor.setSmartCurrentLimit(30); - driveMotor.enableVoltageCompensation(12.0); - turnMotor.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - - if (driveMotor.burnFlash().equals(REVLibError.kOk) - && turnMotor.burnFlash().equals(REVLibError.kOk)) break; + // Hardware + private final CANSparkMax driveMotor; + private final CANSparkMax turnMotor; + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + + // Controllers + private final SparkPIDController driveController; + private final SparkPIDController turnController; + + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final Rotation2d absoluteEncoderOffset; + private final Supplier absoluteEncoderValue; + private static final int shouldResetCount = 100; + private int resetCounter = shouldResetCount; + + public ModuleIOSparkMax(ModuleConfig config) { + // Init motor & encoder objects + driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); + turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + driveEncoder = driveMotor.getEncoder(); + turnRelativeEncoder = turnMotor.getEncoder(); + + driveController = driveMotor.getPIDController(); + turnController = turnMotor.getPIDController(); + turnController.setPositionPIDWrappingEnabled(true); + final double maxInput = Math.PI * moduleConstants.driveReduction(); + turnController.setPositionPIDWrappingMinInput(-maxInput); + turnController.setPositionPIDWrappingMaxInput(maxInput); + + driveMotor.restoreFactoryDefaults(); + turnMotor.restoreFactoryDefaults(); + driveMotor.setCANTimeout(250); + turnMotor.setCANTimeout(250); + + for (int i = 0; i < 4; i++) { + turnMotor.setInverted(config.turnMotorInverted()); + driveMotor.setSmartCurrentLimit(40); + turnMotor.setSmartCurrentLimit(30); + driveMotor.enableVoltageCompensation(12.0); + turnMotor.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + + if (driveMotor.burnFlash().equals(REVLibError.kOk) + && turnMotor.burnFlash().equals(REVLibError.kOk)) break; + } + driveMotor.setCANTimeout(0); + turnMotor.setCANTimeout(0); + + absoluteEncoderValue = + () -> Rotation2d.fromRotations( + turnAbsoluteEncoder.getVoltage() + / RobotController.getVoltage5V()) + .minus(absoluteEncoderOffset); + + drivePositionQueue = + SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); + turnPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal(() -> absoluteEncoderValue.get().getRadians()); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCount + && Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) <= 0.1) { + for (int i = 0; i < 4; i++) { + turnRelativeEncoder.setPosition( + absoluteEncoderValue.get().getRotations() + * moduleConstants.turnReduction()); + } + resetCounter = 0; + } + + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + driveEncoder.getVelocity() / moduleConstants.driveReduction()); + inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); + inputs.driveCurrentAmps = driveMotor.getOutputCurrent(); + + inputs.turnAbsolutePosition = absoluteEncoderValue.get(); + inputs.turnPosition = + Rotation2d.fromRadians( + Units.rotationsToRadians( + turnRelativeEncoder.getPosition() / moduleConstants.turnReduction())); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + turnRelativeEncoder.getVelocity() / moduleConstants.turnReduction()); + inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); + inputs.turnCurrentAmps = turnMotor.getOutputCurrent(); + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble( + motorPositionRevs -> + Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) + * wheelRadius) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveMotor.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnMotor.setVoltage(volts); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + driveController.setReference( + Units.radiansToRotations(velocityRadsPerSec) * moduleConstants.driveReduction(), + CANSparkBase.ControlType.kVelocity, + 0, + ffVolts, + SparkPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + turnController.setReference( + Units.radiansToRotations(angleRads) * moduleConstants.turnReduction(), + CANSparkBase.ControlType.kPosition); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + for (int i = 0; i < 4; i++) { + driveController.setP(kP); + driveController.setI(kI); + driveController.setD(kD); + } + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + for (int i = 0; i < 4; i++) { + turnController.setP(kP); + turnController.setI(kI); + turnController.setD(kD); + } + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void stop() { + driveMotor.stopMotor(); + turnMotor.stopMotor(); } - driveMotor.setCANTimeout(0); - turnMotor.setCANTimeout(0); - - absoluteEncoderValue = - () -> - Rotation2d.fromRadians( - turnAbsoluteEncoder.getVoltage() - / RobotController.getVoltage5V() - * 2.0 - * Math.PI) - .minus(absoluteEncoderOffset); - - drivePositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal(() -> absoluteEncoderValue.get().getRadians()); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - driveEncoder.getVelocity() / moduleConstants.driveReduction()); - inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); - inputs.driveCurrentAmps = driveMotor.getOutputCurrent(); - - inputs.turnAbsolutePosition = absoluteEncoderValue.get(); - inputs.turnPosition = - Rotation2d.fromRadians( - Units.rotationsToRadians( - turnRelativeEncoder.getPosition() / moduleConstants.turnReduction())); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - turnRelativeEncoder.getVelocity() / moduleConstants.turnReduction()); - inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); - inputs.turnCurrentAmps = turnMotor.getOutputCurrent(); - - inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream() - .mapToDouble( - motorPositionRevs -> - Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) - * wheelRadius) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveMotor.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnMotor.setVoltage(volts); - } - - @Override - public void setDriveVelocitySetpoint(double velocityRadPerSec, double ffVolts) { - // TODO - } - - @Override - public void setTurnPositionSetpoint(double angleRadians, double ffVolts) { - // TODO - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void stop() { - driveMotor.stopMotor(); - turnMotor.stopMotor(); - } } From d25a9556278387811b51b3fb0b4ad7883903a1fc Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 5 Feb 2024 00:32:25 -0500 Subject: [PATCH 07/12] Coast Mode Drive --- .../frc/robot/subsystems/drive/Drive.java | 41 ++++++++++++++++--- 1 file changed, 35 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b2362b92..aba81fde 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -25,6 +25,7 @@ import frc.robot.subsystems.drive.planners.AutoAlignMotionPlanner; import frc.robot.subsystems.drive.planners.TrajectoryMotionPlanner; import frc.robot.util.GeomUtil; +import frc.robot.util.LoggedTunableNumber; import frc.robot.util.swerve.ModuleLimits; import frc.robot.util.swerve.SwerveSetpoint; import frc.robot.util.swerve.SwerveSetpointGenerator; @@ -42,6 +43,11 @@ @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { + private static final LoggedTunableNumber coastSpeedLimit = new LoggedTunableNumber( + "Drive/CoastSpeedLimit", DriveConstants.driveConfig.maxLinearVelocity() * 0.6); + private static final LoggedTunableNumber coastDisableTime = new LoggedTunableNumber( + "Drive/CoastDisableTimeSeconds", 0.5); + @AutoLog public static class OdometryTimeestampInputs { public double[] timestamps = new double[] {}; @@ -57,10 +63,9 @@ public static class OdometryTimeestampInputs { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; + private boolean characterizing = false; + private double characterizationVolts = 0.0; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); - private final TrajectoryMotionPlanner trajectoryMotionPlanner; - private final AutoAlignMotionPlanner autoAlignMotionPlanner; - private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; private SwerveSetpoint currentSetpoint = new SwerveSetpoint( @@ -73,8 +78,11 @@ public static class OdometryTimeestampInputs { }); private SwerveSetpointGenerator setpointGenerator; - private boolean characterizing = false; - private double characterizationVolts = 0.0; + private final TrajectoryMotionPlanner trajectoryMotionPlanner; + private final AutoAlignMotionPlanner autoAlignMotionPlanner; + + private final Timer coastTimer = new Timer(); + private boolean shouldCoast = false; public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br, boolean useMotorConroller) { @@ -152,16 +160,37 @@ public void periodic() { : robotRelativeVelocity.omegaRadiansPerSecond; RobotState.getInstance().addVelocityData(robotRelativeVelocity.toTwist2d()); + // Disabled, stop modules and coast if (DriverStation.isDisabled()) { Arrays.stream(modules).forEach(Module::stop); + if (Math.hypot(robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) <= coastSpeedLimit.get()) { + setBrakeMode(false); + coastTimer.stop(); + coastTimer.reset(); + } else if (coastTimer.hasElapsed(coastDisableTime.get())) { + setBrakeMode(false); + coastTimer.stop(); + coastTimer.reset(); + } else { + coastTimer.start(); + } + // Clear logs + 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; } + // Set brake mode we are enabled + setBrakeMode(true); + // Run characterization if (characterizing) { for (Module module : modules) { module.runCharacterization(characterizationVolts); } - + // Clear logs Logger.recordOutput("Drive/SwerveStates/Desired(b4 Poofs)", new double[] {}); Logger.recordOutput("Drive/SwerveStates/Setpoints", new double[] {}); Logger.recordOutput("Drive/DesiredSpeeds", new double[] {}); From 5c2d1d932f9e91e3152b9ea4284adbedde0d1fa7 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 5 Feb 2024 01:41:06 -0500 Subject: [PATCH 08/12] Orient Modules command --- .../frc/robot/subsystems/drive/Drive.java | 33 ++++++++++++++++--- .../frc/robot/subsystems/drive/Module.java | 5 +++ 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index aba81fde..83b3c7da 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -63,6 +63,7 @@ public static class OdometryTimeestampInputs { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; + private boolean modulesOrienting = false; private boolean characterizing = false; private double characterizationVolts = 0.0; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); @@ -182,9 +183,6 @@ public void periodic() { return; } - // Set brake mode we are enabled - setBrakeMode(true); - // Run characterization if (characterizing) { for (Module module : modules) { @@ -198,7 +196,17 @@ public void periodic() { return; } - // Run robot at speeds + // Skip if orienting modules + if (modulesOrienting) { + // Clear logs + 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; + } + + // Run robot at desiredSpeeds // Generate feasible next setpoint currentSetpoint = setpointGenerator.generateSetpoint( @@ -225,6 +233,7 @@ public void periodic() { /** Set drive velocity (robot relative) */ public void setVelocity(ChassisSpeeds velocity) { desiredSpeeds = ChassisSpeeds.discretize(velocity, 0.02); + setBrakeMode(true); } /** Runs forwards at the commanded voltage. */ @@ -252,6 +261,22 @@ public void setBrakeMode(boolean enabled) { Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } + public Command orientModules(Rotation2d orientation) { + return orientModules(new Rotation2d[] {orientation, orientation, orientation, orientation}); + } + + public Command orientModules(Rotation2d[] orientations) { + return run(() -> { + for (int i = 0; i < orientations.length; i++) { + modules[i].runSetpoint(new SwerveModuleState(0.0, orientations[i])); + }}) + .beforeStarting(() -> modulesOrienting = true) + .until(() -> Arrays.stream(modules).allMatch(module -> + Math.abs( + module.getAngle().getDegrees() - module.getSetpointState().angle.getDegrees()) <= 2.0)) + .andThen(() -> modulesOrienting = false); + } + /** Follows a trajectory using the trajectory motion planner. */ public Command followTrajectory(Trajectory trajectory) { return run(() -> setVelocity(trajectoryMotionPlanner.update())) diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index cb21e694..87ddb0b8 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.util.LoggedTunableNumber; +import lombok.Getter; import org.littletonrobotics.junction.Logger; public class Module { @@ -37,6 +38,9 @@ public class Module { new SimpleMotorFeedforward( DriveConstants.moduleConstants.ffKs(), DriveConstants.moduleConstants.ffKv(), 0.0); + @Getter + private SwerveModuleState setpointState = new SwerveModuleState(); + public Module(ModuleIO io, int index, boolean useMotorController) { this.io = io; this.index = index; @@ -75,6 +79,7 @@ public void updateInputs() { } public void runSetpoint(SwerveModuleState setpoint) { + setpointState = setpoint; if (useMotorController) { io.setDriveVelocitySetpoint(setpoint.speedMetersPerSecond, driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); From 214278b439d92b9fe09997c06001c2e48166c86a Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Mon, 5 Feb 2024 06:06:37 -0500 Subject: [PATCH 09/12] Add vts implementation --- .../.devcontainer/devcontainer.json | 12 + trajectory_native/CMakeLists.txt | 26 +++ trajectory_native/Earthfile | 77 +++++++ trajectory_native/mingw-w64.cmake | 16 -- .../proto/VehicleTrajectoryService.proto | 80 +++++++ trajectory_native/src/trajectory_service.cpp | 214 ++++++++++++++++++ 6 files changed, 409 insertions(+), 16 deletions(-) create mode 100644 trajectory_native/.devcontainer/devcontainer.json create mode 100644 trajectory_native/CMakeLists.txt create mode 100644 trajectory_native/Earthfile delete mode 100644 trajectory_native/mingw-w64.cmake create mode 100644 trajectory_native/proto/VehicleTrajectoryService.proto create mode 100644 trajectory_native/src/trajectory_service.cpp diff --git a/trajectory_native/.devcontainer/devcontainer.json b/trajectory_native/.devcontainer/devcontainer.json new file mode 100644 index 00000000..c48c1214 --- /dev/null +++ b/trajectory_native/.devcontainer/devcontainer.json @@ -0,0 +1,12 @@ +{ + "image": "littletonrobotics/vts-dev:latest", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cmake-tools", + "ms-vscode.cpptools-extension-pack", + "zxh404.vscode-proto3" + ] + } + } +} \ No newline at end of file diff --git a/trajectory_native/CMakeLists.txt b/trajectory_native/CMakeLists.txt new file mode 100644 index 00000000..9d4af1d7 --- /dev/null +++ b/trajectory_native/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.24) +project(trajectory_native) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--whole-archive --allow-multiple-definition") + +find_package(protobuf CONFIG REQUIRED) +find_package(gRPC CONFIG REQUIRED) +find_package(Threads) +find_package(fmt REQUIRED) +find_package(nlohmann_json REQUIRED) + +add_library(trajectory_native_proto proto/VehicleTrajectoryService.proto) +target_link_libraries(trajectory_native_proto PUBLIC protobuf::libprotobuf gRPC::grpc gRPC::grpc++ gRPC::grpc++_reflection) +target_include_directories(trajectory_native_proto PUBLIC ${CMAKE_CURRENT_BINARY_DIR}) + +get_target_property(grpc_cpp_plugin_location gRPC::grpc_cpp_plugin LOCATION) +protobuf_generate(TARGET trajectory_native_proto LANGUAGE cpp) +protobuf_generate(TARGET trajectory_native_proto LANGUAGE grpc GENERATE_EXTENSIONS .grpc.pb.h .grpc.pb.cc PLUGIN "protoc-gen-grpc=${grpc_cpp_plugin_location}") + +find_library(TRAJOPT_LIBRARY NAMES TrajoptLib) +message(${TRAJOPT_LIBRARY}) + + +add_executable(trajectory_native src/trajectory_service.cpp) +target_link_libraries(trajectory_native PRIVATE ${TRAJOPT_LIBRARY} trajectory_native_proto fmt::fmt nlohmann_json::nlohmann_json) diff --git a/trajectory_native/Earthfile b/trajectory_native/Earthfile new file mode 100644 index 00000000..d8aadddc --- /dev/null +++ b/trajectory_native/Earthfile @@ -0,0 +1,77 @@ +VERSION 0.7 +FROM debian:bookworm-20240110 +WORKDIR /RobotCode2024/trajectory_native + +apt-deps: + ENV DEBIAN_FRONTEND=noninteractive + RUN apt update && apt install -y wget build-essential cmake autoconf libtool pkg-config git libblas-dev liblapack-dev clang lld gfortran + SAVE IMAGE --cache-hint + +grpc: + FROM +apt-deps + RUN git clone --recurse-submodules -b v1.60.0 --depth 1 --shallow-submodules https://github.com/grpc/grpc + RUN mkdir grpc/build + WORKDIR grpc/build + ENV CC=clang + ENV CXX=clang++ + RUN cmake -DgRPC_INSTALL=ON -DgRPC_BUILD_TESTS=OFF .. + RUN make -j4 + RUN make DESTDIR=$(pwd)/installroot_top install + RUN mkdir installroot + RUN mv installroot_top/usr/local/* installroot/ + SAVE ARTIFACT installroot + +ipopt: + FROM +apt-deps + RUN wget https://github.com/coin-or/Ipopt/archive/refs/tags/releases/3.14.14.tar.gz + RUN tar -zvxf 3.14.14.tar.gz + RUN mkdir Ipopt-releases-3.14.14/build + WORKDIR Ipopt-releases-3.14.14/build + ENV CC=clang + ENV CXX=clang++ + RUN ../configure + RUN make -j4 + RUN make DESTDIR=$(pwd)/installroot_top install + RUN mkdir installroot + RUN mv installroot_top/usr/local/* installroot/ + SAVE ARTIFACT installroot + +trajoptlib: + FROM +apt-deps + BUILD +ipopt + COPY +ipopt/installroot /usr/local/ + GIT CLONE https://github.com/camearle20/TrajoptLib TrajoptLib + RUN mkdir TrajoptLib/build + WORKDIR TrajoptLib/build + ENV CC=clang + ENV CXX=clang++ + RUN cmake -DOPTIMIZER_BACKEND=casadi -DBUILD_TESTING=OFF .. + RUN make -j4 + RUN make DESTDIR=$(pwd)/installroot_top install + RUN mkdir installroot + RUN mv installroot_top/usr/local/* installroot/ + SAVE ARTIFACT installroot + +dev-image: + FROM +apt-deps + BUILD +grpc + BUILD +trajoptlib + COPY +grpc/installroot /usr/local/ + COPY +trajoptlib/installroot /usr/local/ + ENV CC=clang + ENV CXX=clang++ + SAVE IMAGE littletonrobotics/vts-dev + +vts: + FROM +dev-image + COPY src src + COPY proto proto + COPY CMakeLists.txt CMakeLists.txt + RUN mkdir build + WORKDIR build + RUN cmake .. + RUN make -j4 + EXPOSE 56328 + ENV GRPC_VERBOSITY=info + ENTRYPOINT ["./trajectory_native"] + SAVE IMAGE littletonrobotics/vts \ No newline at end of file diff --git a/trajectory_native/mingw-w64.cmake b/trajectory_native/mingw-w64.cmake deleted file mode 100644 index 19fd39c6..00000000 --- a/trajectory_native/mingw-w64.cmake +++ /dev/null @@ -1,16 +0,0 @@ -set(CMAKE_SYSTEM_NAME Windows) -set(TOOLCHAIN_PREFIX x86_64-w64-mingw32) - -# cross compilers to use for C, C++ and Fortran -set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc-posix) -set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++-posix) -set(CMAKE_Fortran_COMPILER ${TOOLCHAIN_PREFIX}-gfortran) -set(CMAKE_RC_COMPILER ${TOOLCHAIN_PREFIX}-windres) - -# target environment on the build host system -set(CMAKE_FIND_ROOT_PATH /usr/${TOOLCHAIN_PREFIX}) - -# modify default behavior of FIND_XXX() commands -set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) -set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) -set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) \ No newline at end of file diff --git a/trajectory_native/proto/VehicleTrajectoryService.proto b/trajectory_native/proto/VehicleTrajectoryService.proto new file mode 100644 index 00000000..f876bf6f --- /dev/null +++ b/trajectory_native/proto/VehicleTrajectoryService.proto @@ -0,0 +1,80 @@ +syntax = "proto3"; + +package org.littletonrobotics.vehicletrajectoryservice; + +message VehicleModel { + double mass = 1; + double moi = 2; + double vehicle_length = 3; + double vehicle_width = 4; + double wheel_radius = 5; + double max_wheel_omega = 6; + double max_wheel_torque = 7; +} + +message VehicleState { + double x = 1; + double y = 2; + double theta = 3; + double vx = 4; + double vy = 5; + double omega = 6; +} + +message TimestampedVehicleState { + double time = 1; + VehicleState state = 2; +} + +message VehicleVelocityConstraint { + double vx = 1; + double vy = 2; + double omega = 3; +} + +message ZeroVelocityConstraint { +} + +message Waypoint { + reserved 5 to 9; + double x = 1; + double y = 2; + optional double heading_constraint = 3; + optional uint32 samples = 4; + oneof velocity_constraint { + ZeroVelocityConstraint zero_velocity = 10; + VehicleVelocityConstraint vehicle_velocity = 11; + } +} + +message PathSegment { + repeated Waypoint waypoints = 1; + optional double max_velocity = 2; + optional double max_omega = 3; + bool straight_line = 4; +} + +message Trajectory { + repeated TimestampedVehicleState states = 1; + string hash_code = 2; +} + +message TrajectoryGenerationError { + string reason = 1; +} + +message PathRequest { + repeated PathSegment segments = 1; + VehicleModel model = 2; +} + +message TrajectoryResponse { + oneof response { + Trajectory trajectory = 1; + TrajectoryGenerationError error = 2; + } +} + +service VehicleTrajectoryService { + rpc GenerateTrajectory(PathRequest) returns (TrajectoryResponse) {} +} \ No newline at end of file diff --git a/trajectory_native/src/trajectory_service.cpp b/trajectory_native/src/trajectory_service.cpp new file mode 100644 index 00000000..065e6bcc --- /dev/null +++ b/trajectory_native/src/trajectory_service.cpp @@ -0,0 +1,214 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vts = org::littletonrobotics::vehicletrajectoryservice; + +trajopt::SwerveDrivetrain create_drivetrain(const vts::VehicleModel &model) { + trajopt::SwerveDrivetrain drivetrain{ + .mass = model.mass(), + .moi = model.moi(), + .modules = {{.x = model.vehicle_length(), + .y = model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = model.vehicle_length(), + .y = -model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = -model.vehicle_length(), + .y = model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}, + {.x = -model.vehicle_length(), + .y = -model.vehicle_width(), + .wheelRadius = model.wheel_radius(), + .wheelMaxAngularVelocity = model.max_wheel_omega(), + .wheelMaxTorque = model.max_wheel_torque()}}}; + + return drivetrain; +} + +int guess_control_interval_count(const vts::Waypoint &waypoint, const vts::Waypoint &prev_waypoint, + const vts::VehicleModel &model) { + double dx = waypoint.x() - prev_waypoint.x(); + double dy = waypoint.y() - prev_waypoint.y(); + double dtheta = waypoint.has_heading_constraint() && prev_waypoint.has_heading_constraint() ? + waypoint.heading_constraint() - prev_waypoint.heading_constraint() : 0; + static const double heading_weight = 0.5; + double distance = hypot(dx, dy); + double max_force = model.max_wheel_torque() / model.wheel_radius(); + double max_accel = (max_force * 4) / model.mass(); + double max_vel = model.max_wheel_omega() * model.wheel_radius(); + double distance_at_cruise = distance - (max_vel * max_vel) / max_accel; + double total_time = distance_at_cruise < 0 ? + 2 * (sqrt(distance * max_accel) / max_accel) : + distance / max_vel + max_vel / max_accel; + total_time += heading_weight * abs(dtheta); + return ceil(total_time / 0.1); +} + +void convert_sample(vts::TimestampedVehicleState *state_out, const trajopt::HolonomicTrajectorySample &sample_in) { + state_out->set_time(sample_in.timestamp); + vts::VehicleState *vehicle_state = state_out->mutable_state(); + vehicle_state->set_x(sample_in.x); + vehicle_state->set_y(sample_in.y); + vehicle_state->set_theta(sample_in.heading); + vehicle_state->set_vx(sample_in.velocityX); + vehicle_state->set_vy(sample_in.velocityY); + vehicle_state->set_omega(sample_in.angularVelocity); +} + +void convert_trajectory(vts::Trajectory *trajectory_out, const trajopt::HolonomicTrajectory &trajectory_in) { + for (const trajopt::HolonomicTrajectorySample &sample: trajectory_in.samples) { + convert_sample(trajectory_out->add_states(), sample); + } +} + +class VehicleTrajectoryService final + : public vts::VehicleTrajectoryService::Service { +public: + ::grpc::Status GenerateTrajectory(::grpc::ServerContext *context, + const vts::PathRequest *request, + vts::TrajectoryResponse *response) override { + trajopt::SwervePathBuilder builder; + builder.SetDrivetrain(create_drivetrain(request->model())); + std::vector control_intervals; + + int segment_start_offset = 0; + for (int segment_idx = 0; segment_idx < request->segments_size(); segment_idx++) { + fmt::print("Starting segment {}\n", segment_idx); + // Add segment waypoints + const vts::PathSegment &segment = request->segments(segment_idx); + const int last_waypoint_idx = segment.waypoints_size() - 1; + const vts::Waypoint *prev_waypoint; + + for (int waypoint_idx = 0; waypoint_idx < segment.waypoints_size(); waypoint_idx++) { + const vts::Waypoint &waypoint = segment.waypoints(segment_start_offset + waypoint_idx); + + if (waypoint.has_heading_constraint()) { + fmt::print("Adding pose waypoint {} ({}, {}, {})\n", segment_start_offset + waypoint_idx, + waypoint.x(), waypoint.y(), waypoint.heading_constraint()); + builder.PoseWpt(segment_start_offset + waypoint_idx, waypoint.x(), waypoint.y(), + waypoint.heading_constraint()); + } else { + fmt::print("Adding translation waypoint {} ({}, {})\n", segment_start_offset + waypoint_idx, + waypoint.x(), waypoint.y()); + builder.TranslationWpt(segment_start_offset + waypoint_idx, waypoint.x(), waypoint.y()); + } + + switch (waypoint.velocity_constraint_case()) { + case vts::Waypoint::VELOCITY_CONSTRAINT_NOT_SET: + // If this is the first or last waypoint, add an implicit stop if no other constraint is added + if (waypoint_idx == 0 || waypoint_idx == last_waypoint_idx) { + fmt::print("Adding implicit zero velocity constraint to waypoint {}\n", + segment_start_offset + waypoint_idx); + builder.WptZeroVelocity(segment_start_offset + waypoint_idx); + } + break; + case vts::Waypoint::kZeroVelocity: + fmt::print("Adding zero velocity constraint to waypoint {}\n", + segment_start_offset + waypoint_idx); + builder.WptZeroVelocity(segment_start_offset + waypoint_idx); + break; + case vts::Waypoint::kVehicleVelocity: + fmt::print("Adding vehicle velocity constraint ({}, {}, {}) to waypoint {}\n", + waypoint.vehicle_velocity().vx(), waypoint.vehicle_velocity().vy(), + waypoint.vehicle_velocity().omega(), segment_start_offset + waypoint_idx); + builder.WptConstraint(segment_start_offset + waypoint_idx, + trajopt::HolonomicVelocityConstraint{ + trajopt::RectangularSet2d{ + {waypoint.vehicle_velocity().vx(), + waypoint.vehicle_velocity().vx()}, + {waypoint.vehicle_velocity().vy(), + waypoint.vehicle_velocity().vy()}, + }, + trajopt::CoordinateSystem::kField + }); + break; + } + + if (segment_idx > 0 || waypoint_idx > 0) { + unsigned int sample_count = waypoint.has_samples() ? waypoint.samples() + : guess_control_interval_count(waypoint, + *prev_waypoint, + request->model()); + fmt::print("Adding sample count {} between waypoints {}-{}\n", sample_count, waypoint_idx - 1, + waypoint_idx); + control_intervals.push_back(sample_count); + } + + prev_waypoint = &waypoint; + } + + // Segment constraints + if (segment.has_max_velocity()) { + fmt::print("Adding max velocity {} to segment {} (waypoints {}-{})\n", segment.max_velocity(), + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtVelocityMagnitude(segment_start_offset, segment_start_offset + last_waypoint_idx, + segment.max_velocity()); + } + + if (segment.has_max_omega()) { + fmt::print("Adding max omega {} to segment {} (waypoints {}-{})\n", segment.max_omega(), + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtConstraint(segment_start_offset, segment_start_offset + last_waypoint_idx, + trajopt::AngularVelocityConstraint{segment.max_omega()}); + } + + if (segment.straight_line()) { + double x1 = segment.waypoints(segment_start_offset).x(); + double x2 = segment.waypoints(segment_start_offset + last_waypoint_idx).x(); + double y1 = segment.waypoints(segment_start_offset).y(); + double y2 = segment.waypoints(segment_start_offset + last_waypoint_idx).y(); + double angle = atan2(y2 - y1, x2 - x1); + fmt::print("Adding straight line constraint with angle {} to segment {} (waypoints {}-{})\n", angle, + segment_idx, segment_start_offset, segment_start_offset + last_waypoint_idx); + builder.SgmtVelocityDirection(segment_start_offset, segment_start_offset + last_waypoint_idx, + angle); + } + + segment_start_offset = last_waypoint_idx; + } + + builder.ControlIntervalCounts(std::move(control_intervals)); + + try { + fmt::print("Generating trajectory\n"); + trajopt::HolonomicTrajectory trajectory{trajopt::OptimalTrajectoryGenerator::Generate(builder)}; + fmt::print("Generation finished\n"); + convert_trajectory(response->mutable_trajectory(), trajectory); + std::size_t hash = std::hash{}(request->SerializeAsString()); + response->mutable_trajectory()->set_hash_code(std::to_string(hash)); + } catch (std::exception &e) { + fmt::print("Generation failed: {}\n", std::string(e.what())); + response->mutable_error()->set_reason(std::string(e.what())); + } + + return grpc::Status::OK; + } +}; + +int main([[maybe_unused]] int argc, [[maybe_unused]] char **argv) { + VehicleTrajectoryService service; + + grpc::reflection::InitProtoReflectionServerBuilderPlugin(); + ::grpc::ServerBuilder builder; + builder.AddListeningPort("0.0.0.0:56328", grpc::InsecureServerCredentials()); + + builder.RegisterService(&service); + + std::unique_ptr server(builder.BuildAndStart()); + server->Wait(); + + return 0; +} \ No newline at end of file From 4adc213413d24662f6be19d517e51192279c6558 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 5 Feb 2024 13:50:22 -0500 Subject: [PATCH 10/12] Kraken? --- src/main/java/frc/robot/RobotContainer.java | 375 +++++++------- src/main/java/frc/robot/RobotState.java | 3 +- .../frc/robot/subsystems/drive/Drive.java | 82 +-- .../subsystems/drive/DriveConstants.java | 447 ++++++++--------- .../frc/robot/subsystems/drive/Module.java | 42 +- .../frc/robot/subsystems/drive/ModuleIO.java | 133 ++--- .../subsystems/drive/ModuleIOKrakenFOC.java | 470 +++++++++--------- .../robot/subsystems/drive/ModuleIOSim.java | 15 +- .../subsystems/drive/ModuleIOSparkMax.java | 358 +++++++------ 9 files changed, 938 insertions(+), 987 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fecb0825..cf5239cc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,12 +39,10 @@ import frc.robot.util.AllianceFlipUtil; import frc.robot.util.trajectory.ChoreoTrajectoryReader; import frc.robot.util.trajectory.Trajectory; - import java.io.File; import java.util.Objects; import java.util.Optional; import java.util.function.Function; - import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -54,209 +52,192 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // Load robot state - private final RobotState robotState = RobotState.getInstance(); - - // Subsystems - private Drive drive; - private AprilTagVision aprilTagVision; - private Shooter shooter; - private Intake intake; - - // Controller - private final CommandXboxController controller = new CommandXboxController(0); - - // Dashboard inputs - private final LoggedDashboardChooser autoChooser; - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - switch (Constants.getMode()) { - case REAL -> { - // Real robot, instantiate hardware IO implementations\ - switch (Constants.getRobot()) { - default -> { - drive = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), - new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]), - false); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); - shooter = new Shooter(new ShooterIOSparkMax()); - intake = new Intake(new IntakeIOSparkMax()); - } - } - } - case SIM -> { - // Sim robot, instantiate physics sim IO implementations - drive = - new Drive( - new GyroIO() { - }, - new ModuleIOSim(DriveConstants.moduleConfigs[0]), - new ModuleIOSim(DriveConstants.moduleConfigs[1]), - new ModuleIOSim(DriveConstants.moduleConfigs[2]), - new ModuleIOSim(DriveConstants.moduleConfigs[3]), - false); - shooter = new Shooter(new ShooterIOSim()); - intake = new Intake(new IntakeIOSim()); - } - default -> { - // Replayed robot, disable IO implementations - drive = - new Drive( - new GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - false); - shooter = new Shooter(new ShooterIO() { - }); - intake = new Intake(new IntakeIO() { - }); - } - } - - if (drive == null) { + // Load robot state + private final RobotState robotState = RobotState.getInstance(); + + // Subsystems + private Drive drive; + private AprilTagVision aprilTagVision; + private Shooter shooter; + private Intake intake; + + // Controller + private final CommandXboxController controller = new CommandXboxController(0); + + // Dashboard inputs + private final LoggedDashboardChooser autoChooser; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + switch (Constants.getMode()) { + case REAL -> { + // Real robot, instantiate hardware IO implementations\ + switch (Constants.getRobot()) { + default -> { drive = - new Drive( - new GyroIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - new ModuleIO() { - }, - false); - } - - if (aprilTagVision == null) { - aprilTagVision = new AprilTagVision(); - } - - if (shooter == null) { - shooter = new Shooter(new ShooterIO() { - }); - } - - if (intake == null) { - intake = new Intake(new IntakeIO() { - }); + new Drive( + new GyroIOPigeon2(false), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]), + false); + aprilTagVision = + new AprilTagVision( + new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); + shooter = new Shooter(new ShooterIOSparkMax()); + intake = new Intake(new IntakeIOSparkMax()); + } } + } + case SIM -> { + // Sim robot, instantiate physics sim IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(DriveConstants.moduleConfigs[0]), + new ModuleIOSim(DriveConstants.moduleConfigs[1]), + new ModuleIOSim(DriveConstants.moduleConfigs[2]), + new ModuleIOSim(DriveConstants.moduleConfigs[3]), + false); + shooter = new Shooter(new ShooterIOSim()); + intake = new Intake(new IntakeIOSim()); + } + default -> { + // Replayed robot, disable IO implementations + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + false); + shooter = new Shooter(new ShooterIO() {}); + intake = new Intake(new IntakeIO() {}); + } + } - autoChooser = new LoggedDashboardChooser<>("Auto Choices"); - // Set up feedforward characterization - autoChooser.addOption( - "Drive FF Characterization", - new FeedForwardCharacterization( - drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) - .finallyDo(drive::endCharacterization)); - autoChooser.addOption( - "Left Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, - shooter::runLeftCharacterizationVolts, - shooter::getLeftCharacterizationVelocity) - .beforeStarting(() -> shooter.setCharacterizing(true)) - .finallyDo(() -> shooter.setCharacterizing(false))); - autoChooser.addOption( - "Right Flywheels FF Characterization", - new FeedForwardCharacterization( - shooter, - shooter::runRightCharacterizationVolts, - shooter::getRightCharacterizationVelocity) - .beforeStarting(() -> shooter.setCharacterizing(true)) - .finallyDo(() -> shooter.setCharacterizing(false))); + if (drive == null) { + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + false); + } - // Testing autos paths - Function> trajectoryCommandFactory = - trajectoryFile -> { - Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); - return trajectory.map( - traj -> - Commands.runOnce( - () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - .andThen(drive.followTrajectory(traj))); - }; - final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); - for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { - trajectoryCommandFactory - .apply(trajectoryFile) - .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); - } + if (aprilTagVision == null) { + aprilTagVision = new AprilTagVision(); + } - // Configure the button bindings - configureButtonBindings(); + if (shooter == null) { + shooter = new Shooter(new ShooterIO() {}); } - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( - drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); - controller - .a() - .whileTrue( - drive.autoAlignToPose( - () -> - AllianceFlipUtil.apply( - new Pose2d( - new Translation2d( - FieldConstants.ampCenter.getX(), - FieldConstants.ampCenter.getY() - - DriveConstants.driveConfig.trackwidthY() / 2.0), - new Rotation2d(-Math.PI / 2.0))))); - // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); - // controller - // .a() - // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), - // intake::running)); - // controller - // .x() - // .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), - // intake::running)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - robotState.resetPose( - new Pose2d( - robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) - .ignoringDisable(true)); - controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); + if (intake == null) { + intake = new Intake(new IntakeIO() {}); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.get(); + autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + // Set up feedforward characterization + autoChooser.addOption( + "Drive FF Characterization", + new FeedForwardCharacterization( + drive, drive::runCharacterizationVolts, drive::getCharacterizationVelocity) + .finallyDo(drive::endCharacterization)); + autoChooser.addOption( + "Left Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runLeftCharacterizationVolts, + shooter::getLeftCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + autoChooser.addOption( + "Right Flywheels FF Characterization", + new FeedForwardCharacterization( + shooter, + shooter::runRightCharacterizationVolts, + shooter::getRightCharacterizationVelocity) + .beforeStarting(() -> shooter.setCharacterizing(true)) + .finallyDo(() -> shooter.setCharacterizing(false))); + + // Testing autos paths + Function> trajectoryCommandFactory = + trajectoryFile -> { + Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); + return trajectory.map( + traj -> + Commands.runOnce( + () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) + .andThen(drive.followTrajectory(traj))); + }; + final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); + for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { + trajectoryCommandFactory + .apply(trajectoryFile) + .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); } + + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + drive.setDefaultCommand( + DriveCommands.joystickDrive( + drive, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); + controller + .a() + .whileTrue( + drive.autoAlignToPose( + () -> + AllianceFlipUtil.apply( + new Pose2d( + new Translation2d( + FieldConstants.ampCenter.getX(), + FieldConstants.ampCenter.getY() + - DriveConstants.driveConfig.trackwidthY() / 2.0), + new Rotation2d(-Math.PI / 2.0))))); + // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); + // controller + // .a() + // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), + // intake::running)); + // controller + // .x() + // .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), + // intake::running)); + controller + .b() + .onTrue( + Commands.runOnce( + () -> + robotState.resetPose( + new Pose2d( + robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) + .ignoringDisable(true)); + controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autoChooser.get(); + } } diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 5dfd195c..1aa3a850 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -56,7 +56,6 @@ private RobotState() { /** Add odometry observation */ public void addOdometryObservation(OdometryObservation observation) { - Pose2d lastOdometryPose = odometryPose; Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions()); lastWheelPositions = observation.wheelPositions(); // Check gyro connected @@ -72,7 +71,7 @@ public void addOdometryObservation(OdometryObservation observation) { // Add pose to buffer at timestamp poseBuffer.addSample(observation.timestamp(), odometryPose); // Calculate diff from last odometry pose and add onto pose estimate - estimatedPose = estimatedPose.exp(lastOdometryPose.log(odometryPose)); + estimatedPose = estimatedPose.exp(twist); } public void addVisionObservation(VisionObservation observation) { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 83b3c7da..6d1e3f3b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -43,13 +43,14 @@ @ExtensionMethod({GeomUtil.class}) public class Drive extends SubsystemBase { - private static final LoggedTunableNumber coastSpeedLimit = new LoggedTunableNumber( + private static final LoggedTunableNumber coastSpeedLimit = + new LoggedTunableNumber( "Drive/CoastSpeedLimit", DriveConstants.driveConfig.maxLinearVelocity() * 0.6); - private static final LoggedTunableNumber coastDisableTime = new LoggedTunableNumber( - "Drive/CoastDisableTimeSeconds", 0.5); + private static final LoggedTunableNumber coastDisableTime = + new LoggedTunableNumber("Drive/CoastDisableTimeSeconds", 0.5); @AutoLog - public static class OdometryTimeestampInputs { + public static class OdometryTimestampInputs { public double[] timestamps = new double[] {}; } @@ -57,14 +58,16 @@ public static class OdometryTimeestampInputs { // TODO: DO THIS BETTER! public static final Queue timestampQueue = new ArrayBlockingQueue<>(100); - private final OdometryTimeestampInputsAutoLogged odometryTimestampInputs = - new OdometryTimeestampInputsAutoLogged(); + private final OdometryTimestampInputsAutoLogged odometryTimestampInputs = + new OdometryTimestampInputsAutoLogged(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; private boolean modulesOrienting = false; private boolean characterizing = false; + private final Timer coastTimer = new Timer(); + private boolean brakeModeEnabled = true; private double characterizationVolts = 0.0; private ChassisSpeeds desiredSpeeds = new ChassisSpeeds(); private ModuleLimits currentModuleLimits = DriveConstants.moduleLimits; @@ -82,11 +85,13 @@ public static class OdometryTimeestampInputs { private final TrajectoryMotionPlanner trajectoryMotionPlanner; private final AutoAlignMotionPlanner autoAlignMotionPlanner; - private final Timer coastTimer = new Timer(); - private boolean shouldCoast = false; - - public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br, - boolean useMotorConroller) { + public Drive( + GyroIO gyroIO, + ModuleIO fl, + ModuleIO fr, + ModuleIO bl, + ModuleIO br, + boolean useMotorConroller) { System.out.println("[Init] Creating Drive"); this.gyroIO = gyroIO; modules[0] = new Module(fl, 0, useMotorConroller); @@ -153,8 +158,7 @@ public void periodic() { } // update current velocities use gyro when possible - ChassisSpeeds robotRelativeVelocity = - DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); + ChassisSpeeds robotRelativeVelocity = getSpeeds(); robotRelativeVelocity.omegaRadiansPerSecond = gyroInputs.connected ? gyroInputs.yawVelocityRadPerSec @@ -164,22 +168,20 @@ public void periodic() { // Disabled, stop modules and coast if (DriverStation.isDisabled()) { Arrays.stream(modules).forEach(Module::stop); - if (Math.hypot(robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) <= coastSpeedLimit.get()) { + if (Math.hypot( + robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) + <= coastSpeedLimit.get() + && brakeModeEnabled) { setBrakeMode(false); coastTimer.stop(); coastTimer.reset(); - } else if (coastTimer.hasElapsed(coastDisableTime.get())) { + } else if (coastTimer.hasElapsed(coastDisableTime.get()) && brakeModeEnabled) { setBrakeMode(false); coastTimer.stop(); coastTimer.reset(); } else { coastTimer.start(); } - // Clear logs - 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; } @@ -188,21 +190,11 @@ public void periodic() { for (Module module : modules) { module.runCharacterization(characterizationVolts); } - // Clear logs - 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; } // Skip if orienting modules if (modulesOrienting) { - // Clear logs - 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; } @@ -258,6 +250,7 @@ public double getCharacterizationVelocity() { /** Set brake mode enabled */ public void setBrakeMode(boolean enabled) { + brakeModeEnabled = enabled; Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled)); } @@ -267,14 +260,21 @@ public Command orientModules(Rotation2d orientation) { public Command orientModules(Rotation2d[] orientations) { return run(() -> { - for (int i = 0; i < orientations.length; i++) { - modules[i].runSetpoint(new SwerveModuleState(0.0, orientations[i])); - }}) - .beforeStarting(() -> modulesOrienting = true) - .until(() -> Arrays.stream(modules).allMatch(module -> - Math.abs( - module.getAngle().getDegrees() - module.getSetpointState().angle.getDegrees()) <= 2.0)) - .andThen(() -> modulesOrienting = false); + for (int i = 0; i < orientations.length; i++) { + modules[i].runSetpoint(new SwerveModuleState(0.0, orientations[i])); + } + }) + .until( + () -> + Arrays.stream(modules) + .allMatch( + module -> + Math.abs( + module.getAngle().getDegrees() + - module.getSetpointState().angle.getDegrees()) + <= 2.0)) + .beforeStarting(() -> modulesOrienting = true) + .finallyDo(() -> modulesOrienting = false); } /** Follows a trajectory using the trajectory motion planner. */ @@ -304,6 +304,12 @@ private SwerveModuleState[] getModuleStates() { return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); } + /** Returns the measured speeds of the robot in the robot's frame of reference. */ + @AutoLogOutput(key = "Drive/MeasuredSpeeds") + private ChassisSpeeds getSpeeds() { + return DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); + } + public static Rotation2d[] getStraightOrientations() { return IntStream.range(0, 4).mapToObj(Rotation2d::new).toArray(Rotation2d[]::new); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index aa9480be..f0d858ff 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -11,232 +11,233 @@ import frc.robot.Constants; import frc.robot.util.swerve.ModuleLimits; -/** - * All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) - */ +/** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ public final class DriveConstants { - // TODO: get effective wheel radius - public static final double wheelRadius = Units.inchesToMeters(2.0); - - // For Kraken - public static class KrakenDriveConstants { - public static final boolean useTorqueCurrentFOC = true; - public static final boolean useMotionMagic = false; - public static final double motionMagicCruiseVelocity = 0.0; - public static final double motionMagicAcceleration = 0.0; - } - - // Drive Constants - public static DriveConfig driveConfig = - switch (Constants.getRobot()) { - case SIMBOT, COMPBOT -> new DriveConfig( - Units.inchesToMeters(25.0), - Units.inchesToMeters(25.0), - Units.feetToMeters(13.05), - Units.feetToMeters(30.02), - 8.86, - 43.97); - default -> new DriveConfig( - Units.inchesToMeters(26.0), - Units.inchesToMeters(26.0), - Units.feetToMeters(12.16), - Units.feetToMeters(21.32), - 7.93, - 29.89); - }; - public static final Translation2d[] moduleTranslations = - new Translation2d[]{ - 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 = - new SwerveDriveKinematics(moduleTranslations); - - // Odometry Constants - public static final double odometryFrequency = - switch (Constants.getRobot()) { - case SIMBOT -> 50.0; - case RAINBOWT -> 100.0; - case COMPBOT -> 250.0; - }; - - public static final Matrix odometryStateStdDevs = - switch (Constants.getRobot()) { - default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); - }; - - // Module Constants - public static ModuleConfig[] moduleConfigs = - switch (Constants.getRobot()) { - case COMPBOT, RAINBOWT -> new ModuleConfig[]{ - new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), - new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), - new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), - new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) - }; - case SIMBOT -> { - ModuleConfig[] configs = new ModuleConfig[4]; - for (int i = 0; i < configs.length; i++) - configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); - yield configs; - } + // TODO: get effective wheel radius + public static final double wheelRadius = Units.inchesToMeters(2.0); + + // For Kraken + public static class KrakenDriveConstants { + public static final boolean useTorqueCurrentFOC = true; + public static final boolean useMotionMagic = false; + public static final double motionMagicCruiseVelocity = 0.0; + public static final double motionMagicAcceleration = 0.0; + } + + // Drive Constants + public static DriveConfig driveConfig = + switch (Constants.getRobot()) { + case SIMBOT, COMPBOT -> + new DriveConfig( + Units.inchesToMeters(25.0), + Units.inchesToMeters(25.0), + Units.feetToMeters(13.05), + Units.feetToMeters(30.02), + 8.86, + 43.97); + default -> + new DriveConfig( + Units.inchesToMeters(26.0), + Units.inchesToMeters(26.0), + Units.feetToMeters(12.16), + Units.feetToMeters(21.32), + 7.93, + 29.89); + }; + public static final Translation2d[] moduleTranslations = + new Translation2d[] { + 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 = + new SwerveDriveKinematics(moduleTranslations); + + // Odometry Constants + public static final double odometryFrequency = + switch (Constants.getRobot()) { + case SIMBOT -> 50.0; + case RAINBOWT -> 100.0; + case COMPBOT -> 250.0; + }; + + public static final Matrix odometryStateStdDevs = + switch (Constants.getRobot()) { + default -> new Matrix<>(VecBuilder.fill(0.003, 0.003, 0.0002)); + }; + + // Module Constants + public static ModuleConfig[] moduleConfigs = + switch (Constants.getRobot()) { + case COMPBOT, RAINBOWT -> + new ModuleConfig[] { + new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), + new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), + new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), + new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) }; - - public static ModuleConstants moduleConstants = - switch (Constants.getRobot()) { - case COMPBOT -> new ModuleConstants( - 1.0, - 0.0, - 0.0, - 0.0, - 200.0, - 20.0, - Mk4iReductions.L3.reduction, - Mk4iReductions.TURN.reduction); - case RAINBOWT -> new ModuleConstants( - 0.1, - 0.13, - 0.1, - 0.0, - 10.0, - 0.0, - Mk4iReductions.L2.reduction, - Mk4iReductions.TURN.reduction); - case SIMBOT -> new ModuleConstants( - 0.014, - 0.134, - 0.1, - 0.0, - 10.0, - 0.0, - Mk4iReductions.L3.reduction, - Mk4iReductions.TURN.reduction); - }; - - public static ModuleLimits moduleLimits = - new ModuleLimits( - driveConfig.maxLinearVelocity(), - driveConfig.maxLinearVelocity() * 5, - Units.degreesToRadians(1080.0)); - - // Trajectory Following - public static TrajectoryConstants trajectoryConstants = - switch (Constants.getRobot()) { - case COMPBOT, RAINBOWT -> new TrajectoryConstants( - 6.0, - 0.0, - 5.0, - 0.0, - Units.inchesToMeters(4.0), - Units.degreesToRadians(5.0), - Units.inchesToMeters(5.0), - Units.degreesToRadians(7.0), - driveConfig.maxLinearVelocity() / 2.0, - driveConfig.maxAngularVelocity() / 2.0); - case SIMBOT -> new TrajectoryConstants( - 4.0, - 0.0, - 2.0, - 0.0, - Units.inchesToMeters(4.0), - Units.degreesToRadians(5.0), - Units.inchesToMeters(5.0), - Units.degreesToRadians(7.0), - driveConfig.maxLinearVelocity() / 2.0, - driveConfig.maxAngularVelocity() / 2.0); - }; - - // Auto Align - public static AutoAlignConstants autoAlignConstants = - new AutoAlignConstants( - 6.0, - 0.0, - 5.0, - 0.0, - Units.inchesToMeters(2.0), - Units.degreesToRadians(2.0), - driveConfig.maxLinearVelocity() * 0.6, - driveConfig.maxLinearAcceleration() * 0.5, - driveConfig.maxAngularVelocity() * 0.3, - driveConfig.maxAngularAcceleration() * 0.5); - - // Swerve Heading Control - public static HeadingControllerConstants headingControllerConstants = - switch (Constants.getRobot()) { - default -> new HeadingControllerConstants(3.0, 0.0); - }; - - public record DriveConfig( - double trackwidthX, - double trackwidthY, - double maxLinearVelocity, - double maxLinearAcceleration, - double maxAngularVelocity, - double maxAngularAcceleration) { - public double driveBaseRadius() { - return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); + case SIMBOT -> { + ModuleConfig[] configs = new ModuleConfig[4]; + for (int i = 0; i < configs.length; i++) + configs[i] = new ModuleConfig(0, 0, 0, new Rotation2d(0), false); + yield configs; } + }; + + public static ModuleConstants moduleConstants = + switch (Constants.getRobot()) { + case COMPBOT -> + new ModuleConstants( + 2.0, + 0.0, + 200.0, + 0.0, + 200.0, + 20.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + case RAINBOWT -> + new ModuleConstants( + 0.1, + 0.13, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L2.reduction, + Mk4iReductions.TURN.reduction); + case SIMBOT -> + new ModuleConstants( + 0.014, + 0.134, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L3.reduction, + Mk4iReductions.TURN.reduction); + }; + + public static ModuleLimits moduleLimits = + new ModuleLimits( + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearVelocity() * 5, + Units.degreesToRadians(1080.0)); + + // Trajectory Following + public static TrajectoryConstants trajectoryConstants = + switch (Constants.getRobot()) { + case COMPBOT, RAINBOWT -> + new TrajectoryConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(4.0), + Units.degreesToRadians(5.0), + Units.inchesToMeters(5.0), + Units.degreesToRadians(7.0), + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); + case SIMBOT -> + new TrajectoryConstants( + 4.0, + 0.0, + 2.0, + 0.0, + Units.inchesToMeters(4.0), + Units.degreesToRadians(5.0), + Units.inchesToMeters(5.0), + Units.degreesToRadians(7.0), + driveConfig.maxLinearVelocity() / 2.0, + driveConfig.maxAngularVelocity() / 2.0); + }; + + // Auto Align + public static AutoAlignConstants autoAlignConstants = + new AutoAlignConstants( + 6.0, + 0.0, + 5.0, + 0.0, + Units.inchesToMeters(2.0), + Units.degreesToRadians(2.0), + driveConfig.maxLinearVelocity(), + driveConfig.maxLinearAcceleration() * 0.5, + driveConfig.maxAngularVelocity() * 0.3, + driveConfig.maxAngularAcceleration() * 0.5); + + // Swerve Heading Control + public static HeadingControllerConstants headingControllerConstants = + switch (Constants.getRobot()) { + default -> new HeadingControllerConstants(3.0, 0.0); + }; + + public record DriveConfig( + double trackwidthX, + double trackwidthY, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) { + public double driveBaseRadius() { + return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); } - - public record ModuleConfig( - int driveID, - int turnID, - int absoluteEncoderChannel, - Rotation2d absoluteEncoderOffset, - boolean turnMotorInverted) { - } - - public record ModuleConstants( - double ffKs, - double ffKv, - double driveKp, - double driveKd, - double turnKp, - double turnKd, - double driveReduction, - double turnReduction) { - } - - public record TrajectoryConstants( - double linearKp, - double linearKd, - double thetaKp, - double thetaKd, - double linearTolerance, - double thetaTolerance, - double goalLinearTolerance, - double goalThetaTolerance, - double linearVelocityTolerance, - double angularVelocityTolerance) { - } - - public record AutoAlignConstants( - double linearKp, - double linearKd, - double thetaKp, - double thetaKd, - double linearTolerance, - double thetaTolerance, - double maxLinearVelocity, - double maxLinearAcceleration, - double maxAngularVelocity, - double maxAngularAcceleration) { - } - - public record HeadingControllerConstants(double Kp, double Kd) { - } - - private enum Mk4iReductions { - L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), - L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), - TURN((150.0 / 7.0)); - - final double reduction; - - Mk4iReductions(double reduction) { - this.reduction = reduction; - } + } + + public record ModuleConfig( + int driveID, + int turnID, + int absoluteEncoderChannel, + Rotation2d absoluteEncoderOffset, + boolean turnMotorInverted) {} + + public record ModuleConstants( + double ffKs, + double ffKv, + double driveKp, + double driveKd, + double turnKp, + double turnKd, + double driveReduction, + double turnReduction) {} + + public record TrajectoryConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double goalLinearTolerance, + double goalThetaTolerance, + double linearVelocityTolerance, + double angularVelocityTolerance) {} + + public record AutoAlignConstants( + double linearKp, + double linearKd, + double thetaKp, + double thetaKd, + double linearTolerance, + double thetaTolerance, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) {} + + public record HeadingControllerConstants(double Kp, double Kd) {} + + private enum Mk4iReductions { + L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), + L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), + TURN((150.0 / 7.0)); + + final double reduction; + + Mk4iReductions(double reduction) { + this.reduction = reduction; } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 87ddb0b8..fbfde029 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -38,8 +38,7 @@ public class Module { new SimpleMotorFeedforward( DriveConstants.moduleConstants.ffKs(), DriveConstants.moduleConstants.ffKv(), 0.0); - @Getter - private SwerveModuleState setpointState = new SwerveModuleState(); + @Getter private SwerveModuleState setpointState = new SwerveModuleState(); public Module(ModuleIO io, int index, boolean useMotorController) { this.io = io; @@ -56,22 +55,18 @@ public void updateInputs() { // Update FF and controllers LoggedTunableNumber.ifChanged( - hashCode(), - () -> { - driveFF = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0); - io.setDriveFF(driveKs.get(), driveKv.get(), 0); - }, - driveKs, - driveKv); + hashCode(), + () -> { + driveFF = new SimpleMotorFeedforward(driveKs.get(), driveKv.get(), 0); + io.setDriveFF(driveKs.get(), driveKv.get(), 0); + }, + driveKs, + driveKv); if (useMotorController) { LoggedTunableNumber.ifChanged( - hashCode(), - () -> io.setDrivePID(driveKp.get(), 0, driveKd.get()), - driveKp, driveKd); + hashCode(), () -> io.setDrivePID(driveKp.get(), 0, driveKd.get()), driveKp, driveKd); LoggedTunableNumber.ifChanged( - hashCode(), - () -> io.setTurnPID(turnKp.get(), 0, turnKd.get()), - turnKp, turnKd); + hashCode(), () -> io.setTurnPID(turnKp.get(), 0, turnKd.get()), turnKp, turnKd); } else { driveController.setPID(driveKp.get(), 0, driveKd.get()); turnController.setPID(turnKp.get(), 0, turnKd.get()); @@ -81,18 +76,19 @@ public void updateInputs() { public void runSetpoint(SwerveModuleState setpoint) { setpointState = setpoint; if (useMotorController) { - io.setDriveVelocitySetpoint(setpoint.speedMetersPerSecond, - driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + io.setDriveVelocitySetpoint( + setpoint.speedMetersPerSecond / DriveConstants.wheelRadius, + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); io.setTurnPositionSetpoint(setpoint.angle.getRadians()); } else { io.setDriveVoltage( - driveController.calculate( - inputs.driveVelocityRadPerSec, - setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) - + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); + driveController.calculate( + inputs.driveVelocityRadPerSec, + setpoint.speedMetersPerSecond / DriveConstants.wheelRadius) + + driveFF.calculate(setpoint.speedMetersPerSecond / DriveConstants.wheelRadius)); io.setTurnVoltage( - turnController.calculate( - inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); + turnController.calculate( + inputs.turnAbsolutePosition.getRadians(), setpoint.angle.getRadians())); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 40158763..d60f0e8c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -17,86 +17,55 @@ import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { - @AutoLog - class ModuleIOInputs { - public double drivePositionRad = 0.0; - public double driveVelocityRadPerSec = 0.0; - public double driveAppliedVolts = 0.0; - public double driveCurrentAmps = 0.0; - - public Rotation2d turnAbsolutePosition = new Rotation2d(); - public Rotation2d turnPosition = new Rotation2d(); - public double turnVelocityRadPerSec = 0.0; - public double turnAppliedVolts = 0.0; - public double turnCurrentAmps = 0.0; - - public double[] odometryDrivePositionsMeters = new double[]{}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[]{}; - } - - /** - * Updates the set of loggable inputs. - */ - default void updateInputs(ModuleIOInputs inputs) { - } - - /** - * Run drive motor at volts - */ - default void setDriveVoltage(double volts) { - } - - /** - * Run turn motor at volts - */ - default void setTurnVoltage(double volts) { - } - - /** - * Set drive velocity setpoint - */ - default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - } - - /** - * Set turn position setpoint - */ - default void setTurnPositionSetpoint(double angleRads) { - } - - /** - * Configure drive PID - */ - default void setDrivePID(double kP, double kI, double kD) { - } - - /** - * Configure turn PID - */ - default void setTurnPID(double kP, double kI, double kD) { - } - - /** - * Configure drive feedforward for TorqueCurrentFOC - */ - default void setDriveFF(double kS, double kV, double kA) { - } - - /** - * Enable or disable brake mode on the drive motor. - */ - default void setDriveBrakeMode(boolean enable) { - } - - /** - * Enable or disable brake mode on the turn motor. - */ - default void setTurnBrakeMode(boolean enable) { - } - - /** - * Disable output to all motors - */ - default void stop() { - } + @AutoLog + class ModuleIOInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double driveSupplyCurrentAmps = 0.0; + public double driveTorqueCurrentAmps = 0.0; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnSupplyCurrentAmps = 0.0; + public double turnTorqueCurrentAmps = 0.0; + + public double[] odometryDrivePositionsMeters = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + } + + /** Updates the set of loggable inputs. */ + default void updateInputs(ModuleIOInputs inputs) {} + + /** Run drive motor at volts */ + default void setDriveVoltage(double volts) {} + + /** Run turn motor at volts */ + default void setTurnVoltage(double volts) {} + + /** Set drive velocity setpoint */ + default void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) {} + + /** Set turn position setpoint */ + default void setTurnPositionSetpoint(double angleRads) {} + + /** Configure drive PID */ + default void setDrivePID(double kP, double kI, double kD) {} + + /** Configure turn PID */ + default void setTurnPID(double kP, double kI, double kD) {} + + /** Configure drive feedforward for TorqueCurrentFOC */ + default void setDriveFF(double kS, double kV, double kA) {} + + /** Enable or disable brake mode on the drive motor. */ + default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + default void setTurnBrakeMode(boolean enable) {} + + /** Disable output to all motors */ + default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java index 15fbd768..ee06881d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java @@ -15,250 +15,252 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; - import java.util.Queue; import java.util.function.Supplier; public class ModuleIOKrakenFOC implements ModuleIO { - // Hardware - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final AnalogInput turnAbsoluteEncoder; - private final Rotation2d absoluteEncoderOffset; - - // Status Signals - private final StatusSignal drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final StatusSignal turnPosition; - private final Supplier turnAbsolutePosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Queues - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private static final int shouldResetCounts = 100; - private int resetCounter = shouldResetCounts; - - private Slot0Configs driveFeedbackConfig = new Slot0Configs(); - private Slot0Configs turnFeedbackConfig = new Slot0Configs(); - - public ModuleIOKrakenFOC(ModuleConfig config) { - // Init controllers and encoders from config constants - driveTalon = new TalonFX(config.driveID(), "canivore"); - turnTalon = new TalonFX(config.turnID(), "canivore"); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); - - // Config Motors - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - driveConfig.Voltage.PeakForwardVoltage = 12.0; - driveConfig.Voltage.PeakReverseVoltage = -12.0; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40; - - var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; - turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; - turnConfig.Voltage.PeakForwardVoltage = 12.0; - turnConfig.Voltage.PeakReverseVoltage = -12.0; - turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 30; - turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -30; - turnConfig.MotorOutput.Inverted = config.turnMotorInverted() ? - InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - - // If in motoControl mode, set reference points in rotations convert from radians - // Affects getPosition() and getVelocity() - driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); - turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); - turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - - // Config Motion Magic - if (KrakenDriveConstants.useMotionMagic) { - turnConfig.MotionMagic.MotionMagicCruiseVelocity = KrakenDriveConstants.motionMagicCruiseVelocity; - turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; - } - - // Apply configs - for (int i = 0; i < 4; i++) { - boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; - setDriveBrakeMode(true); - error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); - setTurnBrakeMode(true); - if (!error) break; - } - - // Get signals and set update rate - // 100hz signals - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); - turnAbsolutePosition = - () -> - Rotation2d.fromRadians( - turnAbsoluteEncoder.getVoltage() - / RobotController.getVoltage5V() - * 2.0 - * Math.PI) - .minus(absoluteEncoderOffset); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - // 250hz signals - drivePosition = driveTalon.getPosition(); - turnPosition = turnTalon.getPosition(); - BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); - - // TODO: what does this do? -// driveTalon.optimizeBusUtilization(); -// turnTalon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - // Reset position of encoder to absolute position every shouldResetCount cycles - // Make sure turnMotor is not moving too fast - if (++resetCounter >= shouldResetCounts - && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { - turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); - resetCounter = 0; - } - - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - - inputs.turnAbsolutePosition = turnAbsolutePosition.get(); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - - inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream() - .mapToDouble( - signalValue -> Units.rotationsToRadians(signalValue) * wheelRadius) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map(Rotation2d::fromRotations) - .toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts) - .withEnableFOC(true)); - } - - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts) - .withEnableFOC(true)); - } - - @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); - if (KrakenDriveConstants.useTorqueCurrentFOC) { - driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); - } else { - driveTalon.setControl(new VelocityVoltage(velocityRotationsPerSec) - .withFeedForward(ffVolts) - .withEnableFOC(true)); - } - } - - @Override - public void setTurnPositionSetpoint(double angleRads) { - double angleRotations = Units.radiansToRotations(angleRads); - if (KrakenDriveConstants.useTorqueCurrentFOC) { - if (KrakenDriveConstants.useMotionMagic) { - turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); - } else { - turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); - } - } else { - if (KrakenDriveConstants.useMotionMagic) { - turnTalon.setControl(new MotionMagicVoltage(angleRotations) - .withEnableFOC(true)); - } else { - turnTalon.setControl(new PositionVoltage(angleRotations) - .withEnableFOC(true)); - } - } - } - - @Override - public void setDrivePID(double kP, double kI, double kD) { - driveFeedbackConfig.kP = kP; - driveFeedbackConfig.kI = kI; - driveFeedbackConfig.kD = kD; - driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); - } - - @Override - public void setTurnPID(double kP, double kI, double kD) { - turnFeedbackConfig.kP = kP; - turnFeedbackConfig.kI = kI; - turnFeedbackConfig.kD = kD; - turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); + // Hardware + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final AnalogInput turnAbsoluteEncoder; + private final Rotation2d absoluteEncoderOffset; + + // Status Signals + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrent; + private final StatusSignal driveTorqueCurrent; + + private final StatusSignal turnPosition; + private final Supplier turnAbsolutePosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnSupplyCurrent; + private final StatusSignal turnTorqueCurrent; + + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private static final int shouldResetCounts = 100; + private int resetCounter = shouldResetCounts; + + private Slot0Configs driveFeedbackConfig = new Slot0Configs(); + private Slot0Configs turnFeedbackConfig = new Slot0Configs(); + + public ModuleIOKrakenFOC(ModuleConfig config) { + // Init controllers and encoders from config constants + driveTalon = new TalonFX(config.driveID(), "canivore"); + turnTalon = new TalonFX(config.turnID(), "canivore"); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + + // Config Motors + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveConfig.Voltage.PeakForwardVoltage = 12.0; + driveConfig.Voltage.PeakReverseVoltage = -12.0; + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0; + turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + turnConfig.Voltage.PeakForwardVoltage = 12.0; + turnConfig.Voltage.PeakReverseVoltage = -12.0; + turnConfig.MotorOutput.Inverted = + config.turnMotorInverted() + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + + // If in motoControl mode, set reference points in rotations convert from radians + // Affects getPosition() and getVelocity() + driveConfig.Feedback.SensorToMechanismRatio = moduleConstants.driveReduction(); + turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + + // Config Motion Magic + if (KrakenDriveConstants.useMotionMagic) { + turnConfig.MotionMagic.MotionMagicCruiseVelocity = + KrakenDriveConstants.motionMagicCruiseVelocity; + turnConfig.MotionMagic.MotionMagicAcceleration = KrakenDriveConstants.motionMagicAcceleration; } - @Override - public void setDriveFF(double kS, double kV, double kA) { - driveFeedbackConfig.kS = kS; - driveFeedbackConfig.kV = kV; - driveFeedbackConfig.kA = kA; - driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + // Apply configs + for (int i = 0; i < 4; i++) { + boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; + setDriveBrakeMode(true); + error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); + setTurnBrakeMode(true); + if (!error) break; } - @Override - public void setDriveBrakeMode(boolean enable) { - driveTalon.setNeutralMode(enable ? - NeutralModeValue.Brake : NeutralModeValue.Coast); + // Get signals and set update rate + // 100hz signals + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveSupplyCurrent = driveTalon.getSupplyCurrent(); + driveTorqueCurrent = driveTalon.getTorqueCurrent(); + turnAbsolutePosition = + () -> + Rotation2d.fromRadians( + turnAbsoluteEncoder.getVoltage() + / RobotController.getVoltage5V() + * 2.0 + * Math.PI) + .minus(absoluteEncoderOffset); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnSupplyCurrent = turnTalon.getSupplyCurrent(); + turnTorqueCurrent = turnTalon.getTorqueCurrent(); + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + + // 250hz signals + drivePosition = driveTalon.getPosition(); + turnPosition = turnTalon.getPosition(); + BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + + // TODO: what does this do? + // driveTalon.optimizeBusUtilization(); + // turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCounts + && Units.rotationsToRadians(turnVelocity.getValueAsDouble()) <= 0.1) { + turnTalon.setPosition(turnAbsolutePosition.get().getRotations()); + resetCounter = 0; } - @Override - public void setTurnBrakeMode(boolean enable) { - turnTalon.setNeutralMode(enable ? - NeutralModeValue.Brake : NeutralModeValue.Coast); + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveSupplyCurrent, + driveTorqueCurrent, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnSupplyCurrent, + turnTorqueCurrent); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveSupplyCurrentAmps = driveSupplyCurrent.getValueAsDouble(); + inputs.driveTorqueCurrentAmps = driveTorqueCurrent.getValueAsDouble(); + + inputs.turnAbsolutePosition = turnAbsolutePosition.get(); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnSupplyCurrentAmps = turnSupplyCurrent.getValueAsDouble(); + inputs.turnTorqueCurrentAmps = turnTorqueCurrent.getValueAsDouble(); + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble(signalValue -> Units.rotationsToRadians(signalValue) * wheelRadius) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void setTurnVoltage(double volts) { + turnTalon.setControl(new VoltageOut(volts).withEnableFOC(true)); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + double velocityRotationsPerSec = Units.radiansToRotations(velocityRadsPerSec); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + driveTalon.setControl(new VelocityTorqueCurrentFOC(velocityRotationsPerSec)); + } else { + driveTalon.setControl( + new VelocityVoltage(velocityRotationsPerSec) + .withFeedForward(ffVolts) + .withEnableFOC(true)); } - - @Override - public void stop() { - driveTalon.setControl(new NeutralOut()); - turnTalon.setControl(new NeutralOut()); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + double angleRotations = Units.radiansToRotations(angleRads); + if (KrakenDriveConstants.useTorqueCurrentFOC) { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicTorqueCurrentFOC(angleRotations)); + } else { + turnTalon.setControl(new PositionTorqueCurrentFOC(angleRotations)); + } + } else { + if (KrakenDriveConstants.useMotionMagic) { + turnTalon.setControl(new MotionMagicVoltage(angleRotations).withEnableFOC(true)); + } else { + turnTalon.setControl(new PositionVoltage(angleRotations).withEnableFOC(true)); + } } + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + driveFeedbackConfig.kP = kP; + driveFeedbackConfig.kI = kI; + driveFeedbackConfig.kD = kD; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + turnFeedbackConfig.kP = kP; + turnFeedbackConfig.kI = kI; + turnFeedbackConfig.kD = kD; + turnTalon.getConfigurator().apply(turnFeedbackConfig, 0.01); + } + + @Override + public void setDriveFF(double kS, double kV, double kA) { + driveFeedbackConfig.kS = kS; + driveFeedbackConfig.kV = kV; + driveFeedbackConfig.kA = kA; + driveTalon.getConfigurator().apply(driveFeedbackConfig, 0.01); + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast); + } + + @Override + public void stop() { + driveTalon.setControl(new NeutralOut()); + turnTalon.setControl(new NeutralOut()); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index 6f72e9bc..e0ca78cd 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -24,9 +24,9 @@ public class ModuleIOSim implements ModuleIO { private final DCMotorSim driveSim = - new DCMotorSim(DCMotor.getKrakenX60(1), moduleConstants.driveReduction(), 0.025); + new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025); private final DCMotorSim turnSim = - new DCMotorSim(DCMotor.getKrakenX60(1), moduleConstants.turnReduction(), 0.004); + new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.turnReduction(), 0.004); private final PIDController driveFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); private final PIDController turnFeedback = new PIDController(0.0, 0.0, 0.0, 0.02); @@ -48,14 +48,14 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.drivePositionRad = driveSim.getAngularPositionRad(); inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec(); inputs.driveAppliedVolts = driveAppliedVolts; - inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); + inputs.driveSupplyCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps()); inputs.turnAbsolutePosition = new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition); inputs.turnPosition = Rotation2d.fromRadians(turnSim.getAngularPositionRad()); inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec(); inputs.turnAppliedVolts = turnAppliedVolts; - inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); + inputs.turnSupplyCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); inputs.odometryDrivePositionsMeters = new double[] {driveSim.getAngularPositionRad() * wheelRadius}; @@ -76,14 +76,13 @@ public void setTurnVoltage(double volts) { @Override public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { setDriveVoltage( - driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) - + ffVolts); + driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec(), velocityRadsPerSec) + + ffVolts); } @Override public void setTurnPositionSetpoint(double angleRads) { - setTurnVoltage( - turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); + setTurnVoltage(turnFeedback.calculate(turnSim.getAngularPositionRad(), angleRads)); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index d4a29f16..4f688368 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -22,193 +22,191 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; - import java.util.Queue; import java.util.function.Supplier; public class ModuleIOSparkMax implements ModuleIO { - // Hardware - private final CANSparkMax driveMotor; - private final CANSparkMax turnMotor; - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - - // Controllers - private final SparkPIDController driveController; - private final SparkPIDController turnController; - - // Queues - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final Rotation2d absoluteEncoderOffset; - private final Supplier absoluteEncoderValue; - private static final int shouldResetCount = 100; - private int resetCounter = shouldResetCount; - - public ModuleIOSparkMax(ModuleConfig config) { - // Init motor & encoder objects - driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); - turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); - driveEncoder = driveMotor.getEncoder(); - turnRelativeEncoder = turnMotor.getEncoder(); - - driveController = driveMotor.getPIDController(); - turnController = turnMotor.getPIDController(); - turnController.setPositionPIDWrappingEnabled(true); - final double maxInput = Math.PI * moduleConstants.driveReduction(); - turnController.setPositionPIDWrappingMinInput(-maxInput); - turnController.setPositionPIDWrappingMaxInput(maxInput); - - driveMotor.restoreFactoryDefaults(); - turnMotor.restoreFactoryDefaults(); - driveMotor.setCANTimeout(250); - turnMotor.setCANTimeout(250); - - for (int i = 0; i < 4; i++) { - turnMotor.setInverted(config.turnMotorInverted()); - driveMotor.setSmartCurrentLimit(40); - turnMotor.setSmartCurrentLimit(30); - driveMotor.enableVoltageCompensation(12.0); - turnMotor.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - - if (driveMotor.burnFlash().equals(REVLibError.kOk) - && turnMotor.burnFlash().equals(REVLibError.kOk)) break; - } - driveMotor.setCANTimeout(0); - turnMotor.setCANTimeout(0); - - absoluteEncoderValue = - () -> Rotation2d.fromRotations( - turnAbsoluteEncoder.getVoltage() - / RobotController.getVoltage5V()) - .minus(absoluteEncoderOffset); - - drivePositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal(() -> absoluteEncoderValue.get().getRadians()); + // Hardware + private final CANSparkMax driveMotor; + private final CANSparkMax turnMotor; + private final RelativeEncoder driveEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final AnalogInput turnAbsoluteEncoder; + + // Controllers + private final SparkPIDController driveController; + private final SparkPIDController turnController; + + // Queues + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final Rotation2d absoluteEncoderOffset; + private final Supplier absoluteEncoderValue; + private static final int shouldResetCount = 100; + private int resetCounter = shouldResetCount; + + public ModuleIOSparkMax(ModuleConfig config) { + // Init motor & encoder objects + driveMotor = new CANSparkMax(config.driveID(), CANSparkMax.MotorType.kBrushless); + turnMotor = new CANSparkMax(config.turnID(), CANSparkMax.MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + driveEncoder = driveMotor.getEncoder(); + turnRelativeEncoder = turnMotor.getEncoder(); + + driveController = driveMotor.getPIDController(); + turnController = turnMotor.getPIDController(); + turnController.setPositionPIDWrappingEnabled(true); + final double maxInput = Math.PI * moduleConstants.driveReduction(); + turnController.setPositionPIDWrappingMinInput(-maxInput); + turnController.setPositionPIDWrappingMaxInput(maxInput); + + driveMotor.restoreFactoryDefaults(); + turnMotor.restoreFactoryDefaults(); + driveMotor.setCANTimeout(250); + turnMotor.setCANTimeout(250); + + for (int i = 0; i < 4; i++) { + turnMotor.setInverted(config.turnMotorInverted()); + driveMotor.setSmartCurrentLimit(40); + turnMotor.setSmartCurrentLimit(30); + driveMotor.enableVoltageCompensation(12.0); + turnMotor.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + + driveMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + turnMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + + if (driveMotor.burnFlash().equals(REVLibError.kOk) + && turnMotor.burnFlash().equals(REVLibError.kOk)) break; } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - // Reset position of encoder to absolute position every shouldResetCount cycles - // Make sure turnMotor is not moving too fast - if (++resetCounter >= shouldResetCount - && Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) <= 0.1) { - for (int i = 0; i < 4; i++) { - turnRelativeEncoder.setPosition( - absoluteEncoderValue.get().getRotations() - * moduleConstants.turnReduction()); - } - resetCounter = 0; - } - - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - driveEncoder.getVelocity() / moduleConstants.driveReduction()); - inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); - inputs.driveCurrentAmps = driveMotor.getOutputCurrent(); - - inputs.turnAbsolutePosition = absoluteEncoderValue.get(); - inputs.turnPosition = - Rotation2d.fromRadians( - Units.rotationsToRadians( - turnRelativeEncoder.getPosition() / moduleConstants.turnReduction())); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - turnRelativeEncoder.getVelocity() / moduleConstants.turnReduction()); - inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); - inputs.turnCurrentAmps = turnMotor.getOutputCurrent(); - - inputs.odometryDrivePositionsMeters = - drivePositionQueue.stream() - .mapToDouble( - motorPositionRevs -> - Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) - * wheelRadius) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); + driveMotor.setCANTimeout(0); + turnMotor.setCANTimeout(0); + + absoluteEncoderValue = + () -> + Rotation2d.fromRotations( + turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) + .minus(absoluteEncoderOffset); + + drivePositionQueue = + SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); + turnPositionQueue = + SparkMaxOdometryThread.getInstance() + .registerSignal(() -> absoluteEncoderValue.get().getRadians()); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Reset position of encoder to absolute position every shouldResetCount cycles + // Make sure turnMotor is not moving too fast + if (++resetCounter >= shouldResetCount + && Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) <= 0.1) { + for (int i = 0; i < 4; i++) { + turnRelativeEncoder.setPosition( + absoluteEncoderValue.get().getRotations() * moduleConstants.turnReduction()); + } + resetCounter = 0; } - @Override - public void setDriveVoltage(double volts) { - driveMotor.setVoltage(volts); + inputs.drivePositionRad = + Units.rotationsToRadians(driveEncoder.getPosition() / moduleConstants.driveReduction()); + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + driveEncoder.getVelocity() / moduleConstants.driveReduction()); + inputs.driveAppliedVolts = driveMotor.getAppliedOutput() * driveMotor.getBusVoltage(); + inputs.driveSupplyCurrentAmps = driveMotor.getOutputCurrent(); + + inputs.turnAbsolutePosition = absoluteEncoderValue.get(); + inputs.turnPosition = + Rotation2d.fromRadians( + Units.rotationsToRadians( + turnRelativeEncoder.getPosition() / moduleConstants.turnReduction())); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond( + turnRelativeEncoder.getVelocity() / moduleConstants.turnReduction()); + inputs.turnAppliedVolts = turnMotor.getAppliedOutput() * turnMotor.getBusVoltage(); + inputs.turnSupplyCurrentAmps = turnMotor.getOutputCurrent(); + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble( + motorPositionRevs -> + Units.rotationsToRadians(motorPositionRevs / moduleConstants.driveReduction()) + * wheelRadius) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + } + + @Override + public void setDriveVoltage(double volts) { + driveMotor.setVoltage(volts); + } + + @Override + public void setTurnVoltage(double volts) { + turnMotor.setVoltage(volts); + } + + @Override + public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { + driveController.setReference( + Units.radiansToRotations(velocityRadsPerSec) * moduleConstants.driveReduction(), + CANSparkBase.ControlType.kVelocity, + 0, + ffVolts, + SparkPIDController.ArbFFUnits.kVoltage); + } + + @Override + public void setTurnPositionSetpoint(double angleRads) { + turnController.setReference( + Units.radiansToRotations(angleRads) * moduleConstants.turnReduction(), + CANSparkBase.ControlType.kPosition); + } + + @Override + public void setDrivePID(double kP, double kI, double kD) { + for (int i = 0; i < 4; i++) { + driveController.setP(kP); + driveController.setI(kI); + driveController.setD(kD); } - - @Override - public void setTurnVoltage(double volts) { - turnMotor.setVoltage(volts); - } - - @Override - public void setDriveVelocitySetpoint(double velocityRadsPerSec, double ffVolts) { - driveController.setReference( - Units.radiansToRotations(velocityRadsPerSec) * moduleConstants.driveReduction(), - CANSparkBase.ControlType.kVelocity, - 0, - ffVolts, - SparkPIDController.ArbFFUnits.kVoltage); - } - - @Override - public void setTurnPositionSetpoint(double angleRads) { - turnController.setReference( - Units.radiansToRotations(angleRads) * moduleConstants.turnReduction(), - CANSparkBase.ControlType.kPosition); - } - - @Override - public void setDrivePID(double kP, double kI, double kD) { - for (int i = 0; i < 4; i++) { - driveController.setP(kP); - driveController.setI(kI); - driveController.setD(kD); - } - } - - @Override - public void setTurnPID(double kP, double kI, double kD) { - for (int i = 0; i < 4; i++) { - turnController.setP(kP); - turnController.setI(kI); - turnController.setD(kD); - } - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void stop() { - driveMotor.stopMotor(); - turnMotor.stopMotor(); + } + + @Override + public void setTurnPID(double kP, double kI, double kD) { + for (int i = 0; i < 4; i++) { + turnController.setP(kP); + turnController.setI(kI); + turnController.setD(kD); } + } + + @Override + public void setDriveBrakeMode(boolean enable) { + driveMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + turnMotor.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + @Override + public void stop() { + driveMotor.stopMotor(); + turnMotor.stopMotor(); + } } From b14bff357fb755ac095e11e50d229829b04ac1ce Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 5 Feb 2024 18:36:51 -0500 Subject: [PATCH 11/12] drive --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 37 +++---------------- .../frc/robot/commands/DriveCommands.java | 2 +- .../frc/robot/commands/auto/AutoCommands.java | 2 +- .../subsystems/drive/DriveConstants.java | 2 +- .../shooting}/ShotCalculator.java | 2 +- 6 files changed, 11 insertions(+), 36 deletions(-) rename src/main/java/frc/robot/{subsystems/superstructure => util/shooting}/ShotCalculator.java (98%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cb6ea60c..4f9f2f58 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.RAINBOWT; public static final boolean tuningMode = true; private static boolean invalidRobotAlertSent = false; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cf5239cc..83af2715 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ 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.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; @@ -25,17 +24,13 @@ import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; import frc.robot.subsystems.apriltagvision.AprilTagVision; -import frc.robot.subsystems.apriltagvision.AprilTagVisionConstants; -import frc.robot.subsystems.apriltagvision.AprilTagVisionIONorthstar; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.superstructure.intake.Intake; import frc.robot.subsystems.superstructure.intake.IntakeIO; import frc.robot.subsystems.superstructure.intake.IntakeIOSim; -import frc.robot.subsystems.superstructure.intake.IntakeIOSparkMax; import frc.robot.subsystems.superstructure.shooter.Shooter; import frc.robot.subsystems.superstructure.shooter.ShooterIO; import frc.robot.subsystems.superstructure.shooter.ShooterIOSim; -import frc.robot.subsystems.superstructure.shooter.ShooterIOSparkMax; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.trajectory.ChoreoTrajectoryReader; import frc.robot.util.trajectory.Trajectory; @@ -82,11 +77,12 @@ public RobotContainer() { new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]), false); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); - shooter = new Shooter(new ShooterIOSparkMax()); - intake = new Intake(new IntakeIOSparkMax()); + // aprilTagVision = + // new AprilTagVision( + // new + // AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); + // shooter = new Shooter(new ShooterIOSparkMax()); + // intake = new Intake(new IntakeIOSparkMax()); } } } @@ -199,27 +195,6 @@ private void configureButtonBindings() { () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); - controller - .a() - .whileTrue( - drive.autoAlignToPose( - () -> - AllianceFlipUtil.apply( - new Pose2d( - new Translation2d( - FieldConstants.ampCenter.getX(), - FieldConstants.ampCenter.getY() - - DriveConstants.driveConfig.trackwidthY() / 2.0), - new Rotation2d(-Math.PI / 2.0))))); - // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); - // controller - // .a() - // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), - // intake::running)); - // controller - // .x() - // .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), - // intake::running)); controller .b() .onTrue( diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index f3ef5847..9910e455 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -22,8 +22,8 @@ import frc.robot.RobotState; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; -import frc.robot.subsystems.superstructure.ShotCalculator; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.shooting.ShotCalculator; import java.util.function.DoubleSupplier; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index 20e0667d..3d01170d 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.java @@ -7,9 +7,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.FieldConstants; import frc.robot.RobotState; -import frc.robot.subsystems.superstructure.ShotCalculator; import frc.robot.subsystems.superstructure.intake.Intake; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.shooting.ShotCalculator; import java.util.function.Supplier; public class AutoCommands { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index f0d858ff..68e19b41 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -18,7 +18,7 @@ public final class DriveConstants { // For Kraken public static class KrakenDriveConstants { - public static final boolean useTorqueCurrentFOC = true; + public static final boolean useTorqueCurrentFOC = false; public static final boolean useMotionMagic = false; public static final double motionMagicCruiseVelocity = 0.0; public static final double motionMagicAcceleration = 0.0; diff --git a/src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java b/src/main/java/frc/robot/util/shooting/ShotCalculator.java similarity index 98% rename from src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java rename to src/main/java/frc/robot/util/shooting/ShotCalculator.java index bb0ae8ac..1f674413 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java +++ b/src/main/java/frc/robot/util/shooting/ShotCalculator.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.util.shooting; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; From bc71b671b022bfc590a0984020ca96ed417948b5 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 5 Feb 2024 18:36:51 -0500 Subject: [PATCH 12/12] Add autoAlignCommand --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 48 +++++++------------ .../frc/robot/commands/DriveCommands.java | 2 +- .../frc/robot/commands/auto/AutoCommands.java | 2 +- .../AprilTagVisionConstants.java | 2 +- .../subsystems/drive/DriveConstants.java | 2 +- .../shooting}/ShotCalculator.java | 2 +- 7 files changed, 23 insertions(+), 37 deletions(-) rename src/main/java/frc/robot/{subsystems/superstructure => util/shooting}/ShotCalculator.java (98%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cb6ea60c..4f9f2f58 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.SIMBOT; + private static RobotType robotType = RobotType.RAINBOWT; public static final boolean tuningMode = true; private static boolean invalidRobotAlertSent = false; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cf5239cc..7fa531c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ 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.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; @@ -25,17 +24,13 @@ import frc.robot.commands.DriveCommands; import frc.robot.commands.FeedForwardCharacterization; import frc.robot.subsystems.apriltagvision.AprilTagVision; -import frc.robot.subsystems.apriltagvision.AprilTagVisionConstants; -import frc.robot.subsystems.apriltagvision.AprilTagVisionIONorthstar; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.superstructure.intake.Intake; import frc.robot.subsystems.superstructure.intake.IntakeIO; import frc.robot.subsystems.superstructure.intake.IntakeIOSim; -import frc.robot.subsystems.superstructure.intake.IntakeIOSparkMax; import frc.robot.subsystems.superstructure.shooter.Shooter; import frc.robot.subsystems.superstructure.shooter.ShooterIO; import frc.robot.subsystems.superstructure.shooter.ShooterIOSim; -import frc.robot.subsystems.superstructure.shooter.ShooterIOSparkMax; import frc.robot.util.AllianceFlipUtil; import frc.robot.util.trajectory.ChoreoTrajectoryReader; import frc.robot.util.trajectory.Trajectory; @@ -82,11 +77,12 @@ public RobotContainer() { new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), new ModuleIOSparkMax(DriveConstants.moduleConfigs[3]), false); - aprilTagVision = - new AprilTagVision( - new AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); - shooter = new Shooter(new ShooterIOSparkMax()); - intake = new Intake(new IntakeIOSparkMax()); + // aprilTagVision = + // new AprilTagVision( + // new + // AprilTagVisionIONorthstar(AprilTagVisionConstants.cameraNames[0])); + // shooter = new Shooter(new ShooterIOSparkMax()); + // intake = new Intake(new IntakeIOSparkMax()); } } } @@ -199,27 +195,6 @@ private void configureButtonBindings() { () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); - controller - .a() - .whileTrue( - drive.autoAlignToPose( - () -> - AllianceFlipUtil.apply( - new Pose2d( - new Translation2d( - FieldConstants.ampCenter.getX(), - FieldConstants.ampCenter.getY() - - DriveConstants.driveConfig.trackwidthY() / 2.0), - new Rotation2d(-Math.PI / 2.0))))); - // controller.a().onTrue(DriveCommands.toggleCalculateShotWhileMovingRotation(drive)); - // controller - // .a() - // .onTrue(Commands.either(intake.stopCommand(), intake.intakeCommand(), - // intake::running)); - // controller - // .x() - // .onTrue(Commands.either(intake.stopCommand(), intake.ejectCommand(), - // intake::running)); controller .b() .onTrue( @@ -230,6 +205,17 @@ private void configureButtonBindings() { robotState.getEstimatedPose().getTranslation(), new Rotation2d()))) .ignoringDisable(true)); controller.y().onTrue(Commands.runOnce(() -> robotState.resetPose(new Pose2d()))); + + controller + .a() + .whileTrue( + drive.autoAlignToPose( + () -> + AllianceFlipUtil.apply( + new Pose2d( + FieldConstants.ampCenter.getX(), + FieldConstants.ampCenter.getY() - 0.6, + new Rotation2d(Math.PI / 2.0))))); } /** diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index f3ef5847..9910e455 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -22,8 +22,8 @@ import frc.robot.RobotState; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; -import frc.robot.subsystems.superstructure.ShotCalculator; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.shooting.ShotCalculator; import java.util.function.DoubleSupplier; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index 20e0667d..3d01170d 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.java @@ -7,9 +7,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.FieldConstants; import frc.robot.RobotState; -import frc.robot.subsystems.superstructure.ShotCalculator; import frc.robot.subsystems.superstructure.intake.Intake; import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.shooting.ShotCalculator; import java.util.function.Supplier; public class AutoCommands { diff --git a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java index ac37c823..5163fd5e 100644 --- a/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java +++ b/src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionConstants.java @@ -22,7 +22,7 @@ public class AprilTagVisionConstants { Units.inchesToMeters(9.974), Units.inchesToMeters(6.839), new Rotation3d(0.0, Units.degreesToRadians(-28.125), 0.0)) - .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0))) // offset? + .rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0))) }; default -> new Pose3d[] {}; }; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index f0d858ff..68e19b41 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -18,7 +18,7 @@ public final class DriveConstants { // For Kraken public static class KrakenDriveConstants { - public static final boolean useTorqueCurrentFOC = true; + public static final boolean useTorqueCurrentFOC = false; public static final boolean useMotionMagic = false; public static final double motionMagicCruiseVelocity = 0.0; public static final double motionMagicAcceleration = 0.0; diff --git a/src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java b/src/main/java/frc/robot/util/shooting/ShotCalculator.java similarity index 98% rename from src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java rename to src/main/java/frc/robot/util/shooting/ShotCalculator.java index bb0ae8ac..1f674413 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/ShotCalculator.java +++ b/src/main/java/frc/robot/util/shooting/ShotCalculator.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.superstructure; +package frc.robot.util.shooting; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d;