Skip to content

Commit

Permalink
[wpimath] Remove unit suffixes from variable names
Browse files Browse the repository at this point in the history
* Move units into API docs instead because suffixes make user code verbose and hard to read
* Rename trackWidth to trackwidth
* Make ultrasonic classes use meters instead of a mix of m, cm, mm, ft,
  and inches
  • Loading branch information
calcmogul committed Dec 14, 2024
1 parent 945d416 commit 313181f
Show file tree
Hide file tree
Showing 249 changed files with 2,935 additions and 3,601 deletions.
56 changes: 28 additions & 28 deletions .github/workflows/tools.yml
Original file line number Diff line number Diff line change
Expand Up @@ -120,31 +120,31 @@ jobs:
build/allOutputs/
retention-days: 7

PathWeaver:
name: "Build - PathWeaver"
needs: [build-artifacts]
runs-on: ubuntu-24.04
steps:
- uses: actions/checkout@v4
with:
repository: wpilibsuite/PathWeaver
fetch-depth: 0
- name: Patch PathWeaver to use local development
run: sed -i "s/wpilibTools.deps.wpilibVersion.*/wpilibTools.deps.wpilibVersion = \'$YEAR\.424242\.+\'/" dependencies.gradle
- uses: actions/download-artifact@v4
with:
name: MavenArtifacts
- uses: actions/setup-java@v4
with:
java-version: 17
distribution: 'temurin'
- name: Move artifacts
run: mkdir -p ~/releases/maven/development && cp -r edu ~/releases/maven/development
- name: Build with Gradle
run: ./gradlew build
- uses: actions/upload-artifact@v4
with:
name: PathWeaver Build
path: |
build/allOutputs/
retention-days: 7
# PathWeaver:
# name: "Build - PathWeaver"
# needs: [build-artifacts]
# runs-on: ubuntu-24.04
# steps:
# - uses: actions/checkout@v4
# with:
# repository: wpilibsuite/PathWeaver
# fetch-depth: 0
# - name: Patch PathWeaver to use local development
# run: sed -i "s/wpilibTools.deps.wpilibVersion.*/wpilibTools.deps.wpilibVersion = \'$YEAR\.424242\.+\'/" dependencies.gradle
# - uses: actions/download-artifact@v4
# with:
# name: MavenArtifacts
# - uses: actions/setup-java@v4
# with:
# java-version: 17
# distribution: 'temurin'
# - name: Move artifacts
# run: mkdir -p ~/releases/maven/development && cp -r edu ~/releases/maven/development
# - name: Build with Gradle
# run: ./gradlew build
# - uses: actions/upload-artifact@v4
# with:
# name: PathWeaver Build
# path: |
# build/allOutputs/
# retention-days: 7
Original file line number Diff line number Diff line change
Expand Up @@ -1025,10 +1025,10 @@ public interface LoggerFunction {
/**
* Runs main run loop with timeout.
*
* @param timeoutSeconds Timeout in seconds.
* @param timeout Timeout in seconds.
* @return 3 on timeout, 2 on signal, 1 on other.
*/
public static native int runMainRunLoopTimeout(double timeoutSeconds);
public static native int runMainRunLoopTimeout(double timeout);

/** Stops main run loop. */
public static native void stopMainRunLoop();
Expand Down
9 changes: 4 additions & 5 deletions hal/src/main/java/edu/wpi/first/hal/DMAJNI.java
Original file line number Diff line number Diff line change
Expand Up @@ -47,10 +47,10 @@ public class DMAJNI extends JNIWrapper {
* <p>This can only be called if DMA is not started.
*
* @param handle the dma handle
* @param periodSeconds the period to trigger in seconds
* @param period the period to trigger in seconds
* @see "HAL_SetDMATimedTrigger"
*/
public static native void setTimedTrigger(int handle, double periodSeconds);
public static native void setTimedTrigger(int handle, double period);

/**
* Sets DMA transfers to occur at a specific timed interval in FPGA cycles.
Expand Down Expand Up @@ -229,14 +229,13 @@ public static native int setExternalTrigger(
* Reads a DMA sample from the queue.
*
* @param handle the dma handle
* @param timeoutSeconds the time to wait for data to be queued before timing out
* @param timeout the time in seconds to wait for data to be queued before timing out
* @param buffer the sample object to place data into
* @param sampleStore index 0-21 channelOffsets, index 22: capture size, index 23: triggerChannels
* (bitflags), index 24: remaining, index 25: read status
* @return timestamp of the DMA Sample
*/
public static native long readDMA(
int handle, double timeoutSeconds, int[] buffer, int[] sampleStore);
public static native long readDMA(int handle, double timeout, int[] buffer, int[] sampleStore);

/**
* Get the sensor DMA sample.
Expand Down
4 changes: 2 additions & 2 deletions hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@ static class BaseStore {
/** Default constructor. */
public DMAJNISample() {}

public int update(int dmaHandle, double timeoutSeconds) {
m_timeStamp = DMAJNI.readDMA(dmaHandle, timeoutSeconds, m_dataBuffer, m_storage);
public int update(int dmaHandle, double timeout) {
m_timeStamp = DMAJNI.readDMA(dmaHandle, timeout, m_dataBuffer, m_storage);
return m_storage[25];
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,14 +10,14 @@
namespace sysid {

/**
* Calculates the track width given the left distance, right distance, and
* Calculates the trackwidth given the left distance, right distance, and
* accumulated gyro angle.
*
* @param l The distance traveled by the left side of the drivetrain.
* @param r The distance traveled by the right side of the drivetrain.
* @param accum The accumulated gyro angle.
*/
constexpr double CalculateTrackWidth(double l, double r,
constexpr double CalculateTrackwidth(double l, double r,
units::radian_t accum) {
// The below comes from solving ω = (vr − vl) / 2r for 2r.
return (gcem::abs(r) + gcem::abs(l)) / gcem::abs(accum.value());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,9 @@

#include <gtest/gtest.h>

#include "sysid/analysis/TrackWidthAnalysis.h"
#include "sysid/analysis/TrackwidthAnalysis.h"

TEST(TrackWidthAnalysisTest, Calculate) {
double result = sysid::CalculateTrackWidth(-0.5386, 0.5386, 90_deg);
TEST(TrackwidthAnalysisTest, Calculate) {
double result = sysid::CalculateTrackwidth(-0.5386, 0.5386, 90_deg);
EXPECT_NEAR(result, 0.6858, 1E-4);
}
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ public class MecanumControllerCommand extends Command {
private final MecanumDriveKinematics m_kinematics;
private final HolonomicDriveController m_controller;
private final Supplier<Rotation2d> m_desiredRotation;
private final double m_maxWheelVelocityMetersPerSecond;
private final double m_maxWheelVelocity;
private final PIDController m_frontLeftController;
private final PIDController m_rearLeftController;
private final PIDController m_frontRightController;
Expand Down Expand Up @@ -79,7 +79,7 @@ public class MecanumControllerCommand extends Command {
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param desiredRotation The angle that the robot should be facing. This is sampled at each time
* step.
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
* @param maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s.
* @param frontLeftController The front left wheel velocity PID.
* @param rearLeftController The rear left wheel velocity PID.
* @param frontRightController The front right wheel velocity PID.
Expand All @@ -98,7 +98,7 @@ public MecanumControllerCommand(
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocityMetersPerSecond,
double maxWheelVelocity,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
Expand All @@ -120,7 +120,7 @@ public MecanumControllerCommand(
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");

m_maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond;
m_maxWheelVelocity = maxWheelVelocity;

m_frontLeftController =
requireNonNullParam(frontLeftController, "frontLeftController", "MecanumControllerCommand");
Expand Down Expand Up @@ -266,8 +266,7 @@ public MecanumControllerCommand(
xController,
yController,
thetaController,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
maxWheelVelocityMetersPerSecond,
frontLeftController,
rearLeftController,
Expand Down Expand Up @@ -299,7 +298,7 @@ public MecanumControllerCommand(
* @param xController The Trajectory Tracker PID controller for the robot's x position.
* @param yController The Trajectory Tracker PID controller for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
* @param maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s.
* @param frontLeftController The front left wheel velocity PID.
* @param rearLeftController The rear left wheel velocity PID.
* @param frontRightController The front right wheel velocity PID.
Expand All @@ -318,7 +317,7 @@ public MecanumControllerCommand(
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocityMetersPerSecond,
double maxWheelVelocity,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
Expand All @@ -334,7 +333,7 @@ public MecanumControllerCommand(
xController,
yController,
thetaController,
maxWheelVelocityMetersPerSecond,
maxWheelVelocity,
frontLeftController,
rearLeftController,
frontRightController,
Expand Down Expand Up @@ -362,7 +361,7 @@ public MecanumControllerCommand(
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param desiredRotation The angle that the robot should be facing. This is sampled at each time
* step.
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
* @param maxWheelVelocity The maximum velocity of a drivetrain wheel in m/s.
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
* @param requirements The subsystems to require.
*/
Expand All @@ -375,7 +374,7 @@ public MecanumControllerCommand(
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocityMetersPerSecond,
double maxWheelVelocity,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
Expand All @@ -392,7 +391,7 @@ public MecanumControllerCommand(
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");

m_maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond;
m_maxWheelVelocity = maxWheelVelocity;

m_frontLeftController = null;
m_rearLeftController = null;
Expand Down Expand Up @@ -430,7 +429,7 @@ public MecanumControllerCommand(
* @param xController The Trajectory Tracker PID controller for the robot's x position.
* @param yController The Trajectory Tracker PID controller for the robot's y position.
* @param thetaController The Trajectory Tracker PID controller for angle for the robot.
* @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
* @param maxWheelVelocity The maximum velocity of a drivetrain wheel.
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
* @param requirements The subsystems to require.
*/
Expand All @@ -441,7 +440,7 @@ public MecanumControllerCommand(
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocityMetersPerSecond,
double maxWheelVelocity,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
this(
Expand All @@ -451,9 +450,8 @@ public MecanumControllerCommand(
xController,
yController,
thetaController,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
maxWheelVelocityMetersPerSecond,
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
maxWheelVelocity,
outputWheelSpeeds,
requirements);
}
Expand All @@ -462,18 +460,16 @@ public MecanumControllerCommand(
public void initialize() {
var initialState = m_trajectory.sample(0);

var initialXVelocity =
initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos();
var initialYVelocity =
initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
var initialXVelocity = initialState.velocity * initialState.pose.getRotation().getCos();
var initialYVelocity = initialState.velocity * initialState.pose.getRotation().getSin();

MecanumDriveWheelSpeeds prevSpeeds =
m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));

m_prevFrontLeftSpeedSetpoint = prevSpeeds.frontLeftMetersPerSecond;
m_prevRearLeftSpeedSetpoint = prevSpeeds.rearLeftMetersPerSecond;
m_prevFrontRightSpeedSetpoint = prevSpeeds.frontRightMetersPerSecond;
m_prevRearRightSpeedSetpoint = prevSpeeds.rearRightMetersPerSecond;
m_prevFrontLeftSpeedSetpoint = prevSpeeds.frontLeft;
m_prevRearLeftSpeedSetpoint = prevSpeeds.rearLeft;
m_prevFrontRightSpeedSetpoint = prevSpeeds.frontRight;
m_prevRearRightSpeedSetpoint = prevSpeeds.rearRight;

m_timer.restart();
}
Expand All @@ -488,12 +484,12 @@ public void execute() {
m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);

targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
targetWheelSpeeds.desaturate(m_maxWheelVelocity);

double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
double frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
double rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
double frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeft;
double rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeft;
double frontRightSpeedSetpoint = targetWheelSpeeds.frontRight;
double rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;

double frontLeftOutput;
double rearLeftOutput;
Expand All @@ -516,22 +512,22 @@ public void execute() {
frontLeftOutput =
frontLeftFeedforward
+ m_frontLeftController.calculate(
m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
m_currentWheelSpeeds.get().frontLeft, frontLeftSpeedSetpoint);

rearLeftOutput =
rearLeftFeedforward
+ m_rearLeftController.calculate(
m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
m_currentWheelSpeeds.get().rearLeft, rearLeftSpeedSetpoint);

frontRightOutput =
frontRightFeedforward
+ m_frontRightController.calculate(
m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
m_currentWheelSpeeds.get().frontRight, frontRightSpeedSetpoint);

rearRightOutput =
rearRightFeedforward
+ m_rearRightController.calculate(
m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
m_currentWheelSpeeds.get().rearRight, rearRightSpeedSetpoint);

m_outputDriveVoltages.accept(
frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput);
Expand All @@ -553,7 +549,7 @@ public void end(boolean interrupted) {

@Override
public boolean isFinished() {
return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
return m_timer.hasElapsed(m_trajectory.getTotalTime());
}

/** A consumer to represent an operation on the voltages of a mecanum drive. */
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -123,8 +123,7 @@ public SwerveControllerCommand(
xController,
yController,
thetaController,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
outputModuleStates,
requirements);
}
Expand Down Expand Up @@ -162,8 +161,7 @@ public SwerveControllerCommand(
pose,
kinematics,
controller,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
outputModuleStates,
requirements);
}
Expand Down Expand Up @@ -233,6 +231,6 @@ public void end(boolean interrupted) {

@Override
public boolean isFinished() {
return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
return m_timer.hasElapsed(m_trajectory.getTotalTime());
}
}
Loading

0 comments on commit 313181f

Please sign in to comment.