diff --git a/src/main/java/frc/robot/generic/subsystems/drive/Drive.java b/src/main/java/frc/robot/generic/subsystems/drive/Drive.java index c61932d..87d5fc6 100644 --- a/src/main/java/frc/robot/generic/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/generic/subsystems/drive/Drive.java @@ -40,6 +40,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; @@ -78,11 +79,14 @@ public Drive( ModuleIO blModuleIO, ModuleIO brModuleIO) { this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); modules[1] = new Module(frModuleIO, 1); modules[2] = new Module(blModuleIO, 2); modules[3] = new Module(brModuleIO, 3); + resetModulesToAbsolute(); + // Usage reporting for swerve template HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); @@ -320,4 +324,14 @@ public double getMaxLinearSpeedMetersPerSec() { public double getMaxAngularSpeedRadPerSec() { return maxSpeedMetersPerSec / driveBaseRadius; } + // Reset ModuleIO to absolute position based on module array + // Made this a method for implementation into the SwerveBuilder class (that's also why it returns command not void) + public Command resetModulesToAbsolute(){ + // Added getter as the code was a bit more readable than just manually reseting each instance + return Commands.runOnce(() ->{ + for(Module module : modules) { + module.getModuleIO().resetToAbsolute(); + } + }, this); + } } diff --git a/src/main/java/frc/robot/generic/subsystems/drive/Module.java b/src/main/java/frc/robot/generic/subsystems/drive/Module.java index ac0ab15..519af99 100644 --- a/src/main/java/frc/robot/generic/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/generic/subsystems/drive/Module.java @@ -133,4 +133,8 @@ public double getWheelRadiusCharacterizationPosition() { public double getFFCharacterizationVelocity() { return inputs.driveVelocityRadPerSec; } + // Gets moduleio instance + public ModuleIO getModuleIO(){ + return io; + } } diff --git a/src/main/java/frc/robot/generic/util/SwerveBuilder.java b/src/main/java/frc/robot/generic/util/SwerveBuilder.java index 33d7466..3d1d9a4 100644 --- a/src/main/java/frc/robot/generic/util/SwerveBuilder.java +++ b/src/main/java/frc/robot/generic/util/SwerveBuilder.java @@ -75,6 +75,7 @@ public static Drive buildDefaultDrive(CommandXboxController driveController) { drive.setPose( new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) + .andThen(drive.resetModulesToAbsolute()) // Resets modules when button pressed .ignoringDisable(true)); return drive; }