From f2a25ef6ad08755a1f90502cf8cb7a05d221231d Mon Sep 17 00:00:00 2001 From: ohad Date: Sun, 25 Jan 2026 14:22:00 -0500 Subject: [PATCH] Implement SwerveImu for threadedGyro, replace JSON configuration with ThreadedGyro when constructing subsystem in REAL mode --- src/main/java/frc/robot/RobotContainer.java | 158 ++-- .../swervedrive/SwerveSubsystem.java | 883 +++++++++--------- .../robot/utils/logging/input/GyroValues.java | 20 + .../utils/logging/io/gyro/RealGyroIo.java | 4 + .../utils/logging/io/gyro/ThreadedGyro.java | 18 + .../io/gyro/ThreadedGyroSwerveIMU.java | 124 +++ .../swervelib/parser/SwerveParserWithImu.java | 66 ++ 7 files changed, 746 insertions(+), 527 deletions(-) create mode 100644 src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyroSwerveIMU.java create mode 100644 src/main/java/swervelib/parser/SwerveParserWithImu.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dc2a986..213ebae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,25 +4,25 @@ package frc.robot; -import java.io.File; - -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.intake.SpinIntake; -import frc.robot.commands.roller.SpinRoller; -import frc.robot.commands.tilt.TiltDown; -import frc.robot.commands.tilt.TiltUp; +import frc.robot.subsystems.GyroSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.RollerSubsystem; -import frc.robot.subsystems.TiltSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.io.gyro.RealGyroIo; +import frc.robot.utils.logging.io.gyro.ThreadedGyro; +import frc.robot.utils.logging.io.gyro.ThreadedGyroSwerveIMU; import frc.robot.utils.simulation.RobotVisualizer; import swervelib.SwerveInputStream; +import swervelib.imu.SwerveIMU; + +import java.io.File; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -31,78 +31,90 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - //private final RollerSubsystem rollerSubsystem; - //private final TiltSubsystem tiltSubsystem; - private final IntakeSubsystem intakeSubsystem; - private RobotVisualizer robotVisualizer = null; - private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),"YAGSL")); - private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT); - private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT); + // The robot's subsystems and commands are defined here... + //private final RollerSubsystem rollerSubsystem; + //private final TiltSubsystem tiltSubsystem; + private final IntakeSubsystem intakeSubsystem; + private RobotVisualizer robotVisualizer = null; + private SwerveSubsystem drivebase = null; + private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT); + private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT); - // Replace with CommandPS4Controller or CommandJoystick if needed - //new CommandXboxController(OperatorConstants.kDriverControllerPort); + // Replace with CommandPS4Controller or CommandJoystick if needed + //new CommandXboxController(OperatorConstants.kDriverControllerPort); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - // Configure the trigger bindings - switch (Constants.currentMode) { + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the trigger bindings + switch (Constants.currentMode) { case REAL -> { //rollerSubsystem = new RollerSubsystem(RollerSubsystem.createRealIo()); //tiltSubsystem = new TiltSubsystem(TiltSubsystem.createRealIo()); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createRealIo(), new DigitalInput(Constants.INTAKE_DIGITAL_INPUT_CHANNEL)); + // For now, we are not creating an instance of the gyro subsystem - just the threadedIO... + RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo(); + ThreadedGyro threadedGyro = gyroIo.getThreadedGyro(); + SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro); + drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU); } case REPLAY -> { //rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); //tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo()); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo(), new DigitalInput(Constants.INTAKE_DIGITAL_INPUT_CHANNEL)); + // create the drive subsystem with null gyro (use default json) + drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null); } case SIM -> { robotVisualizer = new RobotVisualizer(); - //rollerSubsystem = new RollerSubsystem(RollerSubsystem.createSimIo(robotVisualizer)); - //tiltSubsystem = new TiltSubsystem(TiltSubsystem.createSimIo(robotVisualizer)); + //rollerSubsystem = new RollerSubsystem(RollerSubsystem.createSimIo(robotVisualizer)); + //tiltSubsystem = new TiltSubsystem(TiltSubsystem.createSimIo(robotVisualizer)); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer), new DigitalInput(Constants.INTAKE_DIGITAL_INPUT_CHANNEL)); + // create the drive subsystem with null gyro (use default json) + drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null); } - + default -> { throw new RuntimeException("Did not specify Robot Mode"); } - } - configureBindings(); - putShuffleboardCommands(); - } - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driveJoystick.getY() * -1, - () -> driveJoystick.getX() * -1) - .withControllerRotationAxis(steerJoystick::getX) - .deadband(Constants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) - .allianceRelativeControl(false); + } + configureBindings(); + putShuffleboardCommands(); + } + + /** + * Use this method to define your trigger->command mappings. Triggers can be created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary + * predicate, or via the named factories in {@link + * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link + * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller + * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight + * joysticks}. + */ + private void configureBindings() { + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` + //new Trigger(m_exampleSubsystem::exampleCondition) + // .onTrue(new ExampleCommand(m_exampleSubsystem)); - - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. - */ - private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - //new Trigger(m_exampleSubsystem::exampleCondition) - // .onTrue(new ExampleCommand(m_exampleSubsystem)); + // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, + // cancelling on release. + // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + // TODO: Clean this up a little - create command in method and only create the one actually needed + SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> driveJoystick.getY() * -1, + () -> driveJoystick.getX() * -1) + .withControllerRotationAxis(steerJoystick::getX) + .deadband(Constants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) + .allianceRelativeControl(false); + Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); + drivebase.setDefaultCommand(driveRobotOrientedAngularVelocity); + } - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); - Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); - drivebase.setDefaultCommand(driveRobotOrientedAngularVelocity); - } - public void putShuffleboardCommands() { + public void putShuffleboardCommands() { if (Constants.DEBUG) { /*SmartDashboard.putData( "Spin Roller", @@ -120,17 +132,19 @@ public void putShuffleboardCommands() { "Spin Intake", new SpinIntake(intakeSubsystem)); } - } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return null; - } - public RobotVisualizer getRobotVisualizer() { - return robotVisualizer; - } + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // An example command will be run in autonomous + return null; + } + + public RobotVisualizer getRobotVisualizer() { + return robotVisualizer; + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 1d70326..1ec1ea3 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -4,8 +4,6 @@ package frc.robot.subsystems.swervedrive; -import static edu.wpi.first.units.Units.Meter; - import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,469 +16,444 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; -import java.io.File; -import java.util.Arrays; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; +import frc.robot.utils.logging.io.gyro.ThreadedGyro; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; +import swervelib.imu.SwerveIMU; import swervelib.math.SwerveMath; import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveParser; +import swervelib.parser.SwerveParserWithImu; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; -public class SwerveSubsystem extends SubsystemBase -{ - /** - * Swerve drive object. - */ - private final SwerveDrive swerveDrive; - - /** - * Initialize {@link SwerveDrive} with the directory provided. - * - * @param directory Directory of swerve drive config files. - */ - public SwerveSubsystem(File directory) - { - boolean blueAlliance = false; - Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1), - Meter.of(4)), - Rotation2d.fromDegrees(0)) - : new Pose2d(new Translation2d(Meter.of(16), - Meter.of(4)), - Rotation2d.fromDegrees(180)); - // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being created. - SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; - try - { - swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, startingPose); - // Alternative method if you don't want to supply the conversion factor via JSON files. - // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor); - } catch (Exception e) - { - throw new RuntimeException(e); - } - swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. - swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. - swerveDrive.setAngularVelocityCompensation(true, - true, - 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. - swerveDrive.setModuleEncoderAutoSynchronize(false, - 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. - // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible - } - - /** - * Construct the swerve drive. - * - * @param driveCfg SwerveDriveConfiguration for the swerve. - * @param controllerCfg Swerve Controller. - */ - public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) - { - swerveDrive = new SwerveDrive(driveCfg, - controllerCfg, - Constants.MAX_SPEED, - new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), - Rotation2d.fromDegrees(0))); - } - - @Override - public void periodic() - { - } - - @Override - public void simulationPeriodic() - { - } - - /** - * Command to characterize the robot drive motors using SysId - * - * @return SysId Drive Command - */ - public Command sysIdDriveMotorCommand() - { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine( - new Config(), - this, swerveDrive, 12, true), - 3.0, 5.0, 3.0); - } - - /** - * Command to characterize the robot angle motors using SysId - * - * @return SysId Angle Command - */ - public Command sysIdAngleMotorCommand() - { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setAngleSysIdRoutine( - new Config(), - this, swerveDrive), - 3.0, 5.0, 3.0); - } - - /** - * Returns a Command that centers the modules of the SwerveDrive subsystem. - * - * @return a Command that centers the modules of the SwerveDrive subsystem - */ - public Command centerModulesCommand() - { - return run(() -> Arrays.asList(swerveDrive.getModules()) - .forEach(it -> it.setAngle(0.0))); - } - - /** - * Returns a Command that tells the robot to drive forward until the command ends. - * - * @return a Command that tells the robot to drive forward until the command ends - */ - public Command driveForward() - { - return run(() -> { - swerveDrive.drive(new Translation2d(1, 0), 0, false, false); - }).finallyDo(() -> swerveDrive.drive(new Translation2d(0, 0), 0, false, false)); - } - - - /** - * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. - * - * @param kS the static gain of the feedforward - * @param kV the velocity gain of the feedforward - * @param kA the acceleration gain of the feedforward - */ - public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) - { - swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); - } - - /** - * Command to drive the robot using translative values and heading as angular velocity. - * - * @param translationX Translation in the X direction. Cubed for smoother controls. - * @param translationY Translation in the Y direction. Cubed for smoother controls. - * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. - * @return Drive command. - */ - public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) - { - return run(() -> { - // Make the robot move - swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d( +import java.io.File; +import java.util.Arrays; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import static edu.wpi.first.units.Units.Meter; + +public class SwerveSubsystem extends SubsystemBase { + /** + * Swerve drive object. + */ + private final SwerveDrive swerveDrive; + + /** + * Initialize {@link SwerveDrive} with the directory provided. + * The SwerveIMU (which can be null) is the instance of the SwerveIMU to use. If non-null, + * it will replace whatever is defined in the JSON files. If null, will use the default implementation + * from JSON. + * This setup will allow for use of our special Gyro implementation (see {@link ThreadedGyro}) when running in real + * mode, and to use the swerve-library internal SIM IMU when running in simulation - when the swerveImu passed + * in is NULL. + * + * @param directory Directory of swerve drive config files + * @param swerveIMU The IMU implementation to provide to the SwerveDrive + */ + public SwerveSubsystem(File directory, SwerveIMU swerveIMU) { + boolean blueAlliance = false; + Pose2d startingPose = blueAlliance ? new Pose2d(new Translation2d(Meter.of(1), + Meter.of(4)), + Rotation2d.fromDegrees(0)) + : new Pose2d(new Translation2d(Meter.of(16), + Meter.of(4)), + Rotation2d.fromDegrees(180)); + // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being created. + SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; + try { + SwerveParser parser = new SwerveParserWithImu(directory, swerveIMU); + swerveDrive = parser.createSwerveDrive(Constants.MAX_SPEED, startingPose); + // Alternative method if you don't want to supply the conversion factor via JSON files. + // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor); + } catch (Exception e) { + throw new RuntimeException(e); + } + swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. + swerveDrive.setAngularVelocityCompensation(true, + true, + 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. + swerveDrive.setModuleEncoderAutoSynchronize(false, + 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. + // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible + } + + /** + * Construct the swerve drive. + * + * @param driveCfg SwerveDriveConfiguration for the swerve. + * @param controllerCfg Swerve Controller. + */ + public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { + swerveDrive = new SwerveDrive(driveCfg, + controllerCfg, + Constants.MAX_SPEED, + new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), + Rotation2d.fromDegrees(0))); + } + + @Override + public void periodic() { + } + + @Override + public void simulationPeriodic() { + } + + /** + * Command to characterize the robot drive motors using SysId + * + * @return SysId Drive Command + */ + public Command sysIdDriveMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setDriveSysIdRoutine( + new Config(), + this, swerveDrive, 12, true), + 3.0, 5.0, 3.0); + } + + /** + * Command to characterize the robot angle motors using SysId + * + * @return SysId Angle Command + */ + public Command sysIdAngleMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setAngleSysIdRoutine( + new Config(), + this, swerveDrive), + 3.0, 5.0, 3.0); + } + + /** + * Returns a Command that centers the modules of the SwerveDrive subsystem. + * + * @return a Command that centers the modules of the SwerveDrive subsystem + */ + public Command centerModulesCommand() { + return run(() -> Arrays.asList(swerveDrive.getModules()) + .forEach(it -> it.setAngle(0.0))); + } + + /** + * Returns a Command that tells the robot to drive forward until the command ends. + * + * @return a Command that tells the robot to drive forward until the command ends + */ + public Command driveForward() { + return run(() -> { + swerveDrive.drive(new Translation2d(1, 0), 0, false, false); + }).finallyDo(() -> swerveDrive.drive(new Translation2d(0, 0), 0, false, false)); + } + + + /** + * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. + * + * @param kS the static gain of the feedforward + * @param kV the velocity gain of the feedforward + * @param kA the acceleration gain of the feedforward + */ + public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { + swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); + } + + /** + * Command to drive the robot using translative values and heading as angular velocity. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { + return run(() -> { + // Make the robot move + swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d( translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), 0.8), - Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(), - true, - false); - }); - } - - /** - * Command to drive the robot using translative values and heading as a setpoint. - * - * @param translationX Translation in the X direction. Cubed for smoother controls. - * @param translationY Translation in the Y direction. Cubed for smoother controls. - * @param headingX Heading X to calculate angle of the joystick. - * @param headingY Heading Y to calculate angle of the joystick. - * @return Drive command. - */ - public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, - DoubleSupplier headingY) - { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. - return run(() -> { - - Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(), - translationY.getAsDouble()), 0.8); - - // Make the robot move - driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), - headingX.getAsDouble(), - headingY.getAsDouble(), - swerveDrive.getOdometryHeading().getRadians(), - swerveDrive.getMaximumChassisVelocity())); - }); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and - * calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for - * the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per - * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is - * torwards port (left). In field-relative mode, positive x is away from the alliance wall - * (field North) and positive y is torwards the left wall when looking through the driver station - * glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot - * relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - */ - public void drive(Translation2d translation, double rotation, boolean fieldRelative) - { - swerveDrive.drive(translation, - rotation, - fieldRelative, - false); // Open loop is disabled since it shouldn't be used most of the time. - } - - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public void driveFieldOriented(ChassisSpeeds velocity) - { - swerveDrive.driveFieldOriented(velocity); - } - - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public Command driveFieldOriented(Supplier velocity) - { - return run(() -> { - swerveDrive.driveFieldOriented(velocity.get()); - }); - } - - /** - * Drive according to the chassis robot oriented velocity. - * - * @param velocity Robot oriented {@link ChassisSpeeds} - */ - public void drive(ChassisSpeeds velocity) - { - swerveDrive.drive(velocity); - } - - /** - * Get the swerve drive kinematics object. - * - * @return {@link SwerveDriveKinematics} of the swerve drive. - */ - public SwerveDriveKinematics getKinematics() - { - return swerveDrive.kinematics; - } - - /** - * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this - * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to - * keep working. - * - * @param initialHolonomicPose The pose to set the odometry to - */ - public void resetOdometry(Pose2d initialHolonomicPose) - { - swerveDrive.resetOdometry(initialHolonomicPose); - } - - /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. - * - * @return The robot's pose - */ - public Pose2d getPose() - { - return swerveDrive.getPose(); - } - - /** - * Set chassis speeds with closed-loop velocity control. - * - * @param chassisSpeeds Chassis Speeds to set. - */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) - { - swerveDrive.setChassisSpeeds(chassisSpeeds); - } - - /** - * Post the trajectory to the field. - * - * @param trajectory The trajectory to post. - */ - public void postTrajectory(Trajectory trajectory) - { - swerveDrive.postTrajectory(trajectory); - } - - /** - * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. - */ - public void zeroGyro() - { - swerveDrive.zeroGyro(); - } - - /** - * Checks if the alliance is red, defaults to false if alliance isn't available. - * - * @return true if the red alliance, false if blue. Defaults to false if none is available. - */ - private boolean isRedAlliance() - { - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - } - - /** - * This will zero (calibrate) the robot to assume the current position is facing forward - *

- * If red alliance rotate the robot 180 after the drviebase zero command - */ - public void zeroGyroWithAlliance() - { - if (isRedAlliance()) - { - zeroGyro(); - //Set the pose 180 degrees - resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); - } else - { - zeroGyro(); - } - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorBrake(boolean brake) - { - swerveDrive.setMotorIdleMode(brake); - } - - /** - * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the underlying drivebase. - * Note, this is not the raw gyro reading, this may be corrected from calls to resetOdometry(). - * - * @return The yaw angle - */ - public Rotation2d getHeading() - { - return getPose().getRotation(); - } - - /** - * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for - * the angle of the robot. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) - { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), - scaledInputs.getY(), - headingX, - headingY, - getHeading().getRadians(), - Constants.MAX_SPEED); - } - - /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of - * 90deg. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param angle The angle in as a {@link Rotation2d}. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) - { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - - return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), - scaledInputs.getY(), - angle.getRadians(), - getHeading().getRadians(), - Constants.MAX_SPEED); - } - - /** - * Gets the current field-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current field-relative velocity - */ - public ChassisSpeeds getFieldVelocity() - { - return swerveDrive.getFieldVelocity(); - } - - /** - * Gets the current velocity (x, y and omega) of the robot - * - * @return A {@link ChassisSpeeds} object of the current velocity - */ - public ChassisSpeeds getRobotVelocity() - { - return swerveDrive.getRobotVelocity(); - } - - /** - * Get the {@link SwerveController} in the swerve drive. - * - * @return {@link SwerveController} from the {@link SwerveDrive}. - */ - public SwerveController getSwerveController() - { - return swerveDrive.swerveController; - } - - /** - * Get the {@link SwerveDriveConfiguration} object. - * - * @return The {@link SwerveDriveConfiguration} fpr the current drive. - */ - public SwerveDriveConfiguration getSwerveDriveConfiguration() - { - return swerveDrive.swerveDriveConfiguration; - } - - /** - * Lock the swerve drive to prevent it from moving. - */ - public void lock() - { - swerveDrive.lockPose(); - } - - /** - * Gets the current pitch angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - public Rotation2d getPitch() - { - return swerveDrive.getPitch(); - } - - /** - * Gets the swerve drive object. - * - * @return {@link SwerveDrive} - */ - public SwerveDrive getSwerveDrive() - { - return swerveDrive; - } + Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(), + true, + false); + }); + } + + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param headingX Heading X to calculate angle of the joystick. + * @param headingY Heading Y to calculate angle of the joystick. + * @return Drive command. + */ + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, + DoubleSupplier headingY) { + // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. + return run(() -> { + + Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(), + translationY.getAsDouble()), 0.8); + + // Make the robot move + driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), + headingX.getAsDouble(), + headingY.getAsDouble(), + swerveDrive.getOdometryHeading().getRadians(), + swerveDrive.getMaximumChassisVelocity())); + }); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and + * calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for + * the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per + * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is + * torwards port (left). In field-relative mode, positive x is away from the alliance wall + * (field North) and positive y is torwards the left wall when looking through the driver station + * glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot + * relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + */ + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + swerveDrive.drive(translation, + rotation, + fieldRelative, + false); // Open loop is disabled since it shouldn't be used most of the time. + } + + /** + * Drive the robot given a chassis field oriented velocity. + * + * @param velocity Velocity according to the field. + */ + public void driveFieldOriented(ChassisSpeeds velocity) { + swerveDrive.driveFieldOriented(velocity); + } + + /** + * Drive the robot given a chassis field oriented velocity. + * + * @param velocity Velocity according to the field. + */ + public Command driveFieldOriented(Supplier velocity) { + return run(() -> { + swerveDrive.driveFieldOriented(velocity.get()); + }); + } + + /** + * Drive according to the chassis robot oriented velocity. + * + * @param velocity Robot oriented {@link ChassisSpeeds} + */ + public void drive(ChassisSpeeds velocity) { + swerveDrive.drive(velocity); + } + + /** + * Get the swerve drive kinematics object. + * + * @return {@link SwerveDriveKinematics} of the swerve drive. + */ + public SwerveDriveKinematics getKinematics() { + return swerveDrive.kinematics; + } + + /** + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this + * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to + * keep working. + * + * @param initialHolonomicPose The pose to set the odometry to + */ + public void resetOdometry(Pose2d initialHolonomicPose) { + swerveDrive.resetOdometry(initialHolonomicPose); + } + + /** + * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * + * @return The robot's pose + */ + public Pose2d getPose() { + return swerveDrive.getPose(); + } + + /** + * Set chassis speeds with closed-loop velocity control. + * + * @param chassisSpeeds Chassis Speeds to set. + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + swerveDrive.setChassisSpeeds(chassisSpeeds); + } + + /** + * Post the trajectory to the field. + * + * @param trajectory The trajectory to post. + */ + public void postTrajectory(Trajectory trajectory) { + swerveDrive.postTrajectory(trajectory); + } + + /** + * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. + */ + public void zeroGyro() { + swerveDrive.zeroGyro(); + } + + /** + * Checks if the alliance is red, defaults to false if alliance isn't available. + * + * @return true if the red alliance, false if blue. Defaults to false if none is available. + */ + private boolean isRedAlliance() { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + } + + /** + * This will zero (calibrate) the robot to assume the current position is facing forward + *

+ * If red alliance rotate the robot 180 after the drviebase zero command + */ + public void zeroGyroWithAlliance() { + if (isRedAlliance()) { + zeroGyro(); + //Set the pose 180 degrees + resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); + } else { + zeroGyro(); + } + } + + /** + * Sets the drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorBrake(boolean brake) { + swerveDrive.setMotorIdleMode(brake); + } + + /** + * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the underlying drivebase. + * Note, this is not the raw gyro reading, this may be corrected from calls to resetOdometry(). + * + * @return The yaw angle + */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** + * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for + * the angle of the robot. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param headingX X joystick which controls the angle of the robot. + * @param headingY Y joystick which controls the angle of the robot. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) { + Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); + return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), + scaledInputs.getY(), + headingX, + headingY, + getHeading().getRadians(), + Constants.MAX_SPEED); + } + + /** + * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of + * 90deg. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param angle The angle in as a {@link Rotation2d}. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { + Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); + + return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), + scaledInputs.getY(), + angle.getRadians(), + getHeading().getRadians(), + Constants.MAX_SPEED); + } + + /** + * Gets the current field-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current field-relative velocity + */ + public ChassisSpeeds getFieldVelocity() { + return swerveDrive.getFieldVelocity(); + } + + /** + * Gets the current velocity (x, y and omega) of the robot + * + * @return A {@link ChassisSpeeds} object of the current velocity + */ + public ChassisSpeeds getRobotVelocity() { + return swerveDrive.getRobotVelocity(); + } + + /** + * Get the {@link SwerveController} in the swerve drive. + * + * @return {@link SwerveController} from the {@link SwerveDrive}. + */ + public SwerveController getSwerveController() { + return swerveDrive.swerveController; + } + + /** + * Get the {@link SwerveDriveConfiguration} object. + * + * @return The {@link SwerveDriveConfiguration} fpr the current drive. + */ + public SwerveDriveConfiguration getSwerveDriveConfiguration() { + return swerveDrive.swerveDriveConfiguration; + } + + /** + * Lock the swerve drive to prevent it from moving. + */ + public void lock() { + swerveDrive.lockPose(); + } + + /** + * Gets the current pitch angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getPitch() { + return swerveDrive.getPitch(); + } + + /** + * Gets the swerve drive object. + * + * @return {@link SwerveDrive} + */ + public SwerveDrive getSwerveDrive() { + return swerveDrive; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/logging/input/GyroValues.java b/src/main/java/frc/robot/utils/logging/input/GyroValues.java index 5fbd0b5..fe9a92f 100644 --- a/src/main/java/frc/robot/utils/logging/input/GyroValues.java +++ b/src/main/java/frc/robot/utils/logging/input/GyroValues.java @@ -1,5 +1,7 @@ package frc.robot.utils.logging.input; +import edu.wpi.first.math.geometry.Rotation3d; + public class GyroValues { private double anglesInDeg = 0; private double angleOffset = 0; @@ -29,6 +31,8 @@ public class GyroValues { private double robotCentricVelocityX = 0; private double robotCentricVelocityY = 0; private double robotCentricVelocityZ = 0; + private Rotation3d rotation3d = null; + private double rate = 0.0; public double getAnglesInDeg() { return anglesInDeg; @@ -253,4 +257,20 @@ public double getRobotCentricVelocityZ() { public void setRobotCentricVelocityZ(double robotCentricVelocityZ) { this.robotCentricVelocityZ = robotCentricVelocityZ; } + + public Rotation3d getRotation3d() { + return rotation3d; + } + + public void setRotation3d(Rotation3d rotation3d) { + this.rotation3d = rotation3d; + } + + public double getRate() { + return rate; + } + + public void setRate(double rate) { + this.rate = rate; + } } diff --git a/src/main/java/frc/robot/utils/logging/io/gyro/RealGyroIo.java b/src/main/java/frc/robot/utils/logging/io/gyro/RealGyroIo.java index eaae0b9..b469f60 100644 --- a/src/main/java/frc/robot/utils/logging/io/gyro/RealGyroIo.java +++ b/src/main/java/frc/robot/utils/logging/io/gyro/RealGyroIo.java @@ -40,4 +40,8 @@ public GyroValues getGyroValues() { protected void updateInputs(GyroInputs inputs) { inputs.fromHardware(threadedGyro.getGyroValues()); } + + public ThreadedGyro getThreadedGyro() { + return threadedGyro; + } } diff --git a/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyro.java b/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyro.java index a3e9f18..5c798ca 100644 --- a/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyro.java +++ b/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyro.java @@ -47,6 +47,7 @@ public void start() { public void stop() { executor.shutdownNow(); + gyro.close(); } public boolean stopAndWait(long maxTime, TimeUnit timeUnit) { @@ -99,6 +100,10 @@ private void updateGyro() { newValues.setRobotCentricVelocityY(gyro.getRobotCentricVelocityY()); newValues.setRobotCentricVelocityZ(gyro.getRobotCentricVelocityZ()); + newValues.setRotation3d(gyro.getRotation3d()); + + newValues.setRate(gyro.getRate()); + gyroValues.set(newValues); } @@ -118,4 +123,17 @@ public void setAngleAdjustment(double degrees) { public double getAngleOffset() { return Double.longBitsToDouble(gyroOffset.get()); } + + /** + * A method to get the actual Mavx instance underneath the threadedGyro implementation. + * This method should not normally be called, as access to the NavX should be controlled + * by this instance. + * + * Use if you know what you're doing. + * + * @return the NavX instance underneath the threadedGyro + */ + public AHRS getNavxInstance() { + return gyro; + } } diff --git a/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyroSwerveIMU.java b/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyroSwerveIMU.java new file mode 100644 index 0000000..d36a8a9 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/io/gyro/ThreadedGyroSwerveIMU.java @@ -0,0 +1,124 @@ +package frc.robot.utils.logging.io.gyro; + +import com.studica.frc.AHRS; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.measure.MutAngularVelocity; +import frc.robot.utils.logging.input.GyroValues; +import swervelib.imu.SwerveIMU; + +import java.util.Optional; + +import static edu.wpi.first.units.Units.DegreesPerSecond; + +/** + * An implementation of the {@link SwerveIMU} that uses a {@link ThreadedGyro} as its underlying. + * In order to support a background threaded implementation of Gyro updates, this + * class uses the ThreadedGyro to offload the Gyro access, and to read the latest + * values from memory. + * It is modeled after the {@link swervelib.imu.NavXSwerve} implementation. + */ +public class ThreadedGyroSwerveIMU extends SwerveIMU { + /** + * Mutable {@link MutAngularVelocity} for readings. + */ + private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond); + + private ThreadedGyro threadedGyro; + + /** + * Offset for the NavX. + */ + private Rotation3d offset = new Rotation3d(); + + /** + * Inversion state of the {@link AHRS}. + */ + private boolean inverted = false; + + public ThreadedGyroSwerveIMU(ThreadedGyro threadedGyro) { + this.threadedGyro = threadedGyro; + } + + @Override + public void close() { + threadedGyro.stop(); + } + + /** + * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be + * too slow. + */ + @Override + public void factoryDefault() { + // gyro.reset(); // Reported to be slow + offset = threadedGyro.getGyroValues().getRotation3d(); + } + + @Override + public void clearStickyFaults() { + } + + @Override + public void setOffset(Rotation3d offset) { + this.offset = offset; + } + + /** + * Set the gyro to invert its default direction + * + * @param invertIMU invert gyro direction + */ + @Override + public void setInverted(boolean invertIMU) { + inverted = invertIMU; +// setOffset(getRawRotation3d()); + } + + @Override + public Rotation3d getRawRotation3d() { + return inverted ? negate(threadedGyro.getGyroValues().getRotation3d()) : threadedGyro.getGyroValues().getRotation3d(); + } + + /** + * Fetch the {@link Rotation3d} from the IMU. Robot relative. + * + * @return {@link Rotation3d} from the IMU. + */ + @Override + public Rotation3d getRotation3d() { + return getRawRotation3d().rotateBy(offset.unaryMinus()); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns + * empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + @Override + public Optional getAccel() + { + GyroValues gyroValues = threadedGyro.getGyroValues(); + return Optional.of( + new Translation3d( + gyroValues.getWorldLinearAccelX(), + gyroValues.getWorldLinearAccelY(), + gyroValues.getWorldLinearAccelZ()) + .times(9.81)); + } + + @Override + public MutAngularVelocity getYawAngularVelocity() { + return yawVel.mut_setMagnitude(threadedGyro.getGyroValues().getRate()); + } + + @Override + public Object getIMU() { + return threadedGyro.getNavxInstance(); + } + + private Rotation3d negate(Rotation3d rot) { + return new Rotation3d(-rot.getX(), -rot.getY(), -rot.getZ()); + } +} diff --git a/src/main/java/swervelib/parser/SwerveParserWithImu.java b/src/main/java/swervelib/parser/SwerveParserWithImu.java new file mode 100644 index 0000000..7b63165 --- /dev/null +++ b/src/main/java/swervelib/parser/SwerveParserWithImu.java @@ -0,0 +1,66 @@ +package swervelib.parser; + +import edu.wpi.first.math.geometry.Pose2d; +import swervelib.SwerveDrive; +import swervelib.imu.SwerveIMU; +import swervelib.parser.json.ModuleJson; + +import java.io.File; +import java.io.IOException; + +/** + * A subclass of {@link SwerveParser} that allows setting an external IMU. + */ +public class SwerveParserWithImu extends SwerveParser { + private SwerveIMU imu; + + /** + * Construct a swerve parser. Will throw an error if there is a missing file. + * + * @param directory Directory with swerve configurations. + * @throws IOException if a file doesn't exist. + */ + public SwerveParserWithImu(File directory, SwerveIMU imu) throws IOException { + super(directory); + this.imu = imu; + } + + /** + * Create {@link SwerveDrive} from configuration. + * Use the parsed JSON configuration, and, if an IMU has been set externally, use it instead of the + * one configured in the JSON. + * This method is a copy of the one in the superclass with the IMU-related changes. + * + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in + * {@link swervelib.SwerveController} of the robot + * @param initialPose {@link Pose2d} initial pose. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed, Pose2d initialPose) { + SwerveModuleConfiguration[] moduleConfigurations = + new SwerveModuleConfiguration[moduleJsons.length]; + for (int i = 0; i < moduleConfigurations.length; i++) { + ModuleJson module = moduleJsons[i]; + moduleConfigurations[i] = + module.createModuleConfiguration( + pidfPropertiesJson.angle, + pidfPropertiesJson.drive, + physicalPropertiesJson.createPhysicalProperties(), + swerveDriveJson.modules[i]); + } + + SwerveIMU imuToUse = (imu != null) ? imu : swerveDriveJson.imu.createIMU(); + SwerveDriveConfiguration swerveDriveConfiguration = + new SwerveDriveConfiguration( + moduleConfigurations, + imuToUse, + swerveDriveJson.invertedIMU, + physicalPropertiesJson.createPhysicalProperties()); + + return new SwerveDrive( + swerveDriveConfiguration, + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), + maxSpeed, + initialPose); + } +}