Skip to content

Commit

Permalink
Merge pull request #14 from Mechanical-Advantage/feature-kraken-drive
Browse files Browse the repository at this point in the history 
Set turn motor position to absolute position at start up
Use pre-defined members for control rather than creating new control request each cycle
Use feedforward input from module always
Run drive and turn with amps
Make every control request one shot
Make alerts for drive and turn disconnected
Rename methods in ModuleIO
  • Loading branch information
@jwbonner
jwbonner authored Feb 17, 2024
2 parents 19f6619 + d798a83 commit b842a67
Show file tree
Hide file tree
Showing 8 changed files with 175 additions and 165 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 @@ -20,6 +20,7 @@
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import java.util.stream.IntStream;
import lombok.Getter;
import lombok.experimental.ExtensionMethod;
import org.littletonrobotics.frc2024.Constants;
import org.littletonrobotics.frc2024.RobotState;
Expand All @@ -39,11 +40,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 @@ -65,7 +65,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 @@ -74,15 +73,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 @@ -96,7 +99,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 @@ -109,6 +112,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 @@ -151,7 +156,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 @@ -161,64 +165,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 @@ -246,7 +241,7 @@ public void periodic() {
case CHARACTERIZATION -> {
// run characterization
for (Module module : modules) {
module.runCharacterization(characterizationVolts);
module.runCharacterization(characterizationInput);
}
}
default -> {}
Expand Down Expand Up @@ -344,7 +339,7 @@ public boolean isAutoAimGoalCompleted() {
/** Runs forwards at the commanded voltage. */
public void runCharacterizationVolts(double volts) {
currentDriveMode = DriveMode.CHARACTERIZATION;
characterizationVolts = volts;
characterizationInput = volts;
}

/** Disables the characterization mode. */
Expand All @@ -361,10 +356,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 b842a67

Please sign in to comment.