Skip to content

Commit

Permalink
Merge branch 'main' into feature-heading-controller
Browse files Browse the repository at this point in the history
  • Loading branch information
@jwbonner
jwbonner committed Feb 17, 2024
2 parents 37b7601 + b842a67 commit 2f97861
Show file tree
Hide file tree
Showing 7 changed files with 174 additions and 163 deletions.
83 changes: 40 additions & 43 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
import java.util.concurrent.locks.ReentrantLock;
import java.util.function.Supplier;
import java.util.stream.IntStream;
import lombok.Getter;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.Constants;
import org.littletonrobotics.frc2024.RobotState;
Expand All @@ -40,11 +41,10 @@

@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);
private static final LoggedTunableNumber coastWaitTime =
new LoggedTunableNumber("Drive/CoastWaitTimeSeconds", 0.5);
private static final LoggedTunableNumber coastMetersPerSecThreshold =
new LoggedTunableNumber("Drive/CoastMetersPerSecThreshold", 0.05);

public enum DriveMode {
/** Driving with input from driver joysticks. (Default) */
Expand All @@ -66,7 +66,6 @@ public static class OdometryTimestampInputs {
}

public static final Lock odometryLock = new ReentrantLock();
// TODO: DO THIS BETTER!
public static final Queue<Double> timestampQueue = new ArrayBlockingQueue<>(100);

private final OdometryTimestampInputsAutoLogged odometryTimestampInputs =
Expand All @@ -75,15 +74,19 @@ public static class OdometryTimestampInputs {
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
private final Module[] modules = new Module[4];

// Store previous positions and time for filtering odometry data
private SwerveDriveWheelPositions lastPositions = null;
private double lastTime = 0.0;

/** Active drive mode. */
private DriveMode currentDriveMode = DriveMode.TELEOP;

private double characterizationVolts = 0.0;
private double characterizationInput = 0.0;
private boolean modulesOrienting = false;
private final Timer coastTimer = new Timer();
private final Timer lastMovementTimer = new Timer();

@Getter
@AutoLogOutput(key = "Drive/BrakeModeEnabled")
private boolean brakeModeEnabled = true;

private ChassisSpeeds desiredSpeeds = new ChassisSpeeds();
Expand All @@ -97,7 +100,7 @@ public static class OdometryTimestampInputs {
new SwerveModuleState(),
new SwerveModuleState()
});
private SwerveSetpointGenerator setpointGenerator;
private final SwerveSetpointGenerator setpointGenerator;

private final TeleopDriveController teleopDriveController;
private TrajectoryController trajectoryController = null;
Expand All @@ -110,6 +113,8 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br)
modules[1] = new Module(fr, 1);
modules[2] = new Module(bl, 2);
modules[3] = new Module(br, 3);
lastMovementTimer.start();
setBrakeMode(true);

setpointGenerator =
SwerveSetpointGenerator.builder()
Expand Down Expand Up @@ -152,7 +157,6 @@ public void periodic() {
}
// Pass odometry data to robot state
for (int i = 0; i < minOdometryUpdates; i++) {
boolean includeMeasurement = true;
int odometryIndex = i;
Rotation2d yaw = gyroInputs.connected ? gyroInputs.odometryYawPositions[i] : null;
// Get all four swerve module positions at that odometry update
Expand All @@ -162,64 +166,55 @@ public void periodic() {
Arrays.stream(modules)
.map(module -> module.getModulePositions()[odometryIndex])
.toArray(SwerveModulePosition[]::new));
// Filtering
// Filtering based on delta wheel positions
boolean includeMeasurement = true;
if (lastPositions != null) {
double dt = Timer.getFPGATimestamp() - lastTime;
for (int j = 0; j < 4; j++) {
double dt = odometryTimestampInputs.timestamps[i] - lastTime;
for (int j = 0; j < modules.length; j++) {
double velocity =
(wheelPositions.positions[j].distanceMeters
- lastPositions.positions[j].distanceMeters)
/ dt;
double omega =
wheelPositions.positions[j].angle.minus(lastPositions.positions[j].angle).getRadians()
/ dt;

if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 100.0
|| Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 100.0) {
// Check if delta is too large
if (Math.abs(omega) > currentModuleLimits.maxSteeringVelocity() * 5.0
|| Math.abs(velocity) > currentModuleLimits.maxDriveVelocity() * 5.0) {
includeMeasurement = false;
break;
}
}
}
// If delta isn't too large we can include the measurement.
if (includeMeasurement) {
lastPositions = wheelPositions;
RobotState.getInstance()
.addOdometryObservation(
new RobotState.OdometryObservation(
wheelPositions, yaw, odometryTimestampInputs.timestamps[i]));
lastTime = odometryTimestampInputs.timestamps[i];
}
}
lastTime = Timer.getFPGATimestamp();

// update current velocities use gyro when possible
// Update current velocities use gyro when possible
ChassisSpeeds robotRelativeVelocity = getSpeeds();
robotRelativeVelocity.omegaRadiansPerSecond =
gyroInputs.connected
? gyroInputs.yawVelocityRadPerSec
: 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()
&& brakeModeEnabled) {
setBrakeMode(false);
coastTimer.stop();
coastTimer.reset();
} else if (coastTimer.hasElapsed(coastDisableTime.get()) && brakeModeEnabled) {
setBrakeMode(false);
coastTimer.stop();
coastTimer.reset();
} else {
coastTimer.start();
}
return;
} else {
// Brake mode
setBrakeMode(true);
// Update brake mode
// Reset movement timer if moved
if (Arrays.stream(modules)
.anyMatch(module -> module.getVelocityMetersPerSec() > coastMetersPerSecThreshold.get())) {
lastMovementTimer.reset();
}
if (DriverStation.isEnabled()) {
setBrakeMode(true); // Always in brake mode during teleop
} else if (lastMovementTimer.hasElapsed(coastWaitTime.get())) {
setBrakeMode(false);
}

// Run drive based on current mode
Expand Down Expand Up @@ -247,7 +242,7 @@ public void periodic() {
case CHARACTERIZATION -> {
// run characterization
for (Module module : modules) {
module.runCharacterization(characterizationVolts);
module.runCharacterization(characterizationInput);
}
}
default -> {}
Expand Down Expand Up @@ -345,7 +340,7 @@ public boolean atHeadingGoal() {
/** Runs forwards at the commanded voltage. */
public void runCharacterizationVolts(double volts) {
currentDriveMode = DriveMode.CHARACTERIZATION;
characterizationVolts = volts;
characterizationInput = volts;
}

/** Disables the characterization mode. */
Expand All @@ -362,10 +357,12 @@ public double getCharacterizationVelocity() {
return driveVelocityAverage / 4.0;
}

/** Set brake mode enabled */
/** Set brake mode to {@code enabled} doesn't change brake mode if already set */
public void setBrakeMode(boolean enabled) {
if (brakeModeEnabled != enabled) {
Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled));
}
brakeModeEnabled = enabled;
Arrays.stream(modules).forEach(module -> module.setBrakeMode(enabled));
}

public Command orientModules(Rotation2d orientation) {
Expand Down
9 changes: 0 additions & 9 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/drive/DriveConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,15 +20,6 @@

/** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */
public final class DriveConstants {
// For Kraken
public static class KrakenDriveConstants {
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;
}

// Drive Constants
public static DriveConfig driveConfig =
switch (Constants.getRobot()) {
case SIMBOT, COMPBOT ->
Expand Down
56 changes: 42 additions & 14 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Module.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import lombok.Getter;
import org.littletonrobotics.frc2024.util.Alert;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
import org.littletonrobotics.junction.Logger;

Expand All @@ -31,62 +32,83 @@ public class Module {
new LoggedTunableNumber("Drive/Module/TurnkP", moduleConstants.turnkP());
private static final LoggedTunableNumber turnkD =
new LoggedTunableNumber("Drive/Module/TurnkD", moduleConstants.turnkD());
private static final String[] moduleNames = new String[] {"FL", "FR", "BL", "BR"};

private final int index;
private final ModuleIO io;
private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
private SimpleMotorFeedforward driveFF =
private SimpleMotorFeedforward ff =
new SimpleMotorFeedforward(moduleConstants.ffkS(), moduleConstants.ffkV(), 0.0);

@Getter private SwerveModuleState setpointState = new SwerveModuleState();

// Alerts
private final Alert driveMotorDisconnected;
private final Alert turnMotorDisconnected;

public Module(ModuleIO io, int index) {
this.io = io;
this.index = index;

driveMotorDisconnected =
new Alert(
"Drive", moduleNames[index] + " drive motor disconnected!", Alert.AlertType.WARNING);
turnMotorDisconnected =
new Alert(
"Drive", moduleNames[index] + " turn motor disconnected!", Alert.AlertType.WARNING);
}

/** Called while blocking odometry thread */
public void updateInputs() {
io.updateInputs(inputs);
Logger.processInputs("Drive/Module" + index, inputs);
Logger.processInputs("Drive/" + moduleNames[index] + "_Module", inputs);

// Update FF and controllers
// Update ff and controllers
LoggedTunableNumber.ifChanged(
hashCode(),
() -> {
driveFF = new SimpleMotorFeedforward(drivekS.get(), drivekV.get(), 0);
io.setDriveFF(drivekS.get(), drivekV.get(), 0);
},
() -> ff = new SimpleMotorFeedforward(drivekS.get(), drivekV.get(), 0),
drivekS,
drivekV);
LoggedTunableNumber.ifChanged(
hashCode(), () -> io.setDrivePID(drivekP.get(), 0, drivekD.get()), drivekP, drivekD);
LoggedTunableNumber.ifChanged(
hashCode(), () -> io.setTurnPID(turnkP.get(), 0, turnkD.get()), turnkP, turnkD);

// Display alerts
driveMotorDisconnected.set(!inputs.driveMotorConnected);
turnMotorDisconnected.set(!inputs.turnMotorConnected);
}

/** Runs to {@link SwerveModuleState} */
public void runSetpoint(SwerveModuleState setpoint) {
setpointState = setpoint;
io.setDriveVelocitySetpoint(
io.runDriveVelocitySetpoint(
setpoint.speedMetersPerSecond / driveConfig.wheelRadius(),
driveFF.calculate(setpoint.speedMetersPerSecond / driveConfig.wheelRadius()));
io.setTurnPositionSetpoint(setpoint.angle.getRadians());
ff.calculate(setpoint.speedMetersPerSecond / driveConfig.wheelRadius()));
io.runTurnPositionSetpoint(setpoint.angle.getRadians());
}

public void runCharacterization(double volts) {
io.setTurnPositionSetpoint(0.0);
io.setDriveVoltage(volts);
/** Runs characterization volts or amps depending on using voltage or current control. */
public void runCharacterization(double input) {
io.runTurnPositionSetpoint(0.0);
if (inputs.hasCurrentControl) {
io.runDriveCurrent(input);
} else {
io.runDriveVolts(input);
}
}

/** Sets brake mode to {@code enabled}. */
public void setBrakeMode(boolean enabled) {
io.setDriveBrakeMode(enabled);
io.setTurnBrakeMode(enabled);
}

/** Stops motors. */
public void stop() {
io.stop();
}

/** Get all latest {@link SwerveModulePosition}'s from last cycle. */
public SwerveModulePosition[] getModulePositions() {
int minOdometryPositions =
Math.min(inputs.odometryDrivePositionsMeters.length, inputs.odometryTurnPositions.length);
Expand All @@ -99,26 +121,32 @@ public SwerveModulePosition[] getModulePositions() {
return positions;
}

/** Get turn angle of module as {@link Rotation2d}. */
public Rotation2d getAngle() {
return inputs.turnAbsolutePosition;
}

/** Get position of wheel in meters. */
public double getPositionMeters() {
return inputs.drivePositionRad * driveConfig.wheelRadius();
}

/** Get velocity of wheel in m/s. */
public double getVelocityMetersPerSec() {
return inputs.driveVelocityRadPerSec * driveConfig.wheelRadius();
}

/** Get current {@link SwerveModulePosition} of module. */
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(getPositionMeters(), getAngle());
}

/** Get current {@link SwerveModuleState} of module. */
public SwerveModuleState getState() {
return new SwerveModuleState(getVelocityMetersPerSec(), getAngle());
}

/** Get velocity of drive wheel for characterization */
public double getCharacterizationVelocity() {
return inputs.driveVelocityRadPerSec;
}
Expand Down
Loading

0 comments on commit 2f97861

Please sign in to comment.