Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions src/main/java/frc/robot/generic/subsystems/drive/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
}, this);
}, this).ignoringDisable(true);

This function needs to ignore disabling

}
}
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/generic/subsystems/drive/Module.java
Original file line number Diff line number Diff line change
Expand Up @@ -133,4 +133,8 @@ public double getWheelRadiusCharacterizationPosition() {
public double getFFCharacterizationVelocity() {
return inputs.driveVelocityRadPerSec;
}
// Gets moduleio instance
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// Gets moduleio instance
/// Gets moduleio instance

In general, It would be better to use 3 slashes in comments above functions because that makes the comment interpreted as a javadoc, meaning it will appear when you hover over the function

public ModuleIO getModuleIO(){
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It is not good practice to allow direct access to the IO interface from elsewhere. Instead, could you make a function that calls the function in the io interface?

return io;
}
}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/generic/util/SwerveBuilder.java
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The .andThen should be below .ignoringDisable because the .ignoringDisable needs to apply to the runOnce command, I don't think it does anything after you already make the command group (.andThen)

.ignoringDisable(true));
return drive;
}
Expand Down