Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove unit suffixes from variable names (#7529)

Browse Source 
* 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
This commit is contained in:
Tyler Veness
2025-02-10 07:23:04 -08:00
committed by GitHub
parent 764ada9b66
commit ac1705ae2b
250 changed files with 2953 additions and 3584 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
.github/workflows/tools.yml vendored
View File

@@ -43,31 +43,31 @@ jobs:
development
retention-days: 1
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

4
cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java
View File

@@ -1025,10 +1025,10 @@ public class CameraServerJNI {
/**
* 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();

4
cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
View File

@@ -2085,9 +2085,9 @@ Java_edu_wpi_first_cscore_CameraServerJNI_runMainRunLoop
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_cscore_CameraServerJNI_runMainRunLoopTimeout
(JNIEnv*, jclass, jdouble timeoutSeconds)
(JNIEnv*, jclass, jdouble timeout)
{
return cs::RunMainRunLoopTimeout(timeoutSeconds);
return cs::RunMainRunLoopTimeout(timeout);
}
/*

11
cscore/src/main/native/include/cscore_runloop.h
View File

@@ -5,7 +5,16 @@
#pragma once
namespace cs {
void RunMainRunLoop();
int RunMainRunLoopTimeout(double timeoutSeconds);
/**
* Runs main run loop with timeout.
*
* @param timeout Timeout in seconds.
*/
int RunMainRunLoopTimeout(double timeout);
void StopMainRunLoop();
} // namespace cs

7
cscore/src/main/native/linux/RunLoopHelpers.cpp
View File

@@ -12,16 +12,16 @@ static wpi::Event& GetInstance() {
}
namespace cs {
void RunMainRunLoop() {
wpi::Event& event = GetInstance();
wpi::WaitForObject(event.GetHandle());
}
int RunMainRunLoopTimeout(double timeoutSeconds) {
int RunMainRunLoopTimeout(double timeout) {
wpi::Event& event = GetInstance();
bool timedOut = false;
bool signaled =
wpi::WaitForObject(event.GetHandle(), timeoutSeconds, &timedOut);
bool signaled = wpi::WaitForObject(event.GetHandle(), timeout, &timedOut);
if (timedOut) {
return 3;
}
@@ -35,4 +35,5 @@ void StopMainRunLoop() {
wpi::Event& event = GetInstance();
event.Set();
}
} // namespace cs

6
cscore/src/main/native/objcpp/RunLoopHelpers.mm
View File

@@ -8,6 +8,7 @@
#import <Foundation/Foundation.h>
namespace cs {
void RunMainRunLoop() {
if (CFRunLoopGetMain() != CFRunLoopGetCurrent()) {
NSLog(@"This method can only be called from the main thread");
@@ -16,15 +17,16 @@ void RunMainRunLoop() {
CFRunLoopRun();
}
int RunMainRunLoopTimeout(double timeoutSeconds) {
int RunMainRunLoopTimeout(double timeout) {
if (CFRunLoopGetMain() != CFRunLoopGetCurrent()) {
NSLog(@"This method can only be called from the main thread");
return -1;
}
return CFRunLoopRunInMode(kCFRunLoopDefaultMode, timeoutSeconds, false);
return CFRunLoopRunInMode(kCFRunLoopDefaultMode, timeout, false);
}
void StopMainRunLoop() {
CFRunLoopStop(CFRunLoopGetMain());
}
}

7
cscore/src/main/native/windows/RunLoopHelpers.cpp
View File

@@ -12,16 +12,16 @@ static wpi::Event& GetInstance() {
}
namespace cs {
void RunMainRunLoop() {
wpi::Event& event = GetInstance();
wpi::WaitForObject(event.GetHandle());
}
int RunMainRunLoopTimeout(double timeoutSeconds) {
int RunMainRunLoopTimeout(double timeout) {
wpi::Event& event = GetInstance();
bool timedOut = false;
bool signaled =
wpi::WaitForObject(event.GetHandle(), timeoutSeconds, &timedOut);
bool signaled = wpi::WaitForObject(event.GetHandle(), timeout, &timedOut);
if (timedOut) {
return 3;
}
@@ -35,4 +35,5 @@ void StopMainRunLoop() {
wpi::Event& event = GetInstance();
event.Set();
}
} // namespace cs

18
hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
View File

@@ -57,18 +57,14 @@ public class AddressableLEDJNI extends JNIWrapper {
* those.
*
* @param handle the Addressable LED handle
* @param highTime0NanoSeconds high time for 0 bit (default 400ns)
* @param lowTime0NanoSeconds low time for 0 bit (default 900ns)
* @param highTime1NanoSeconds high time for 1 bit (default 900ns)
* @param lowTime1NanoSeconds low time for 1 bit (default 600ns)
* @param highTime0 high time for 0 bit in nanoseconds (default 400 ns)
* @param lowTime0 low time for 0 bit in nanoseconds (default 900 ns)
* @param highTime1 high time for 1 bit in nanoseconds (default 900 ns)
* @param lowTime1 low time for 1 bit in nanoseconds (default 600 ns)
* @see "HAL_SetAddressableLEDBitTiming"
*/
public static native void setBitTiming(
int handle,
int highTime0NanoSeconds,
int lowTime0NanoSeconds,
int highTime1NanoSeconds,
int lowTime1NanoSeconds);
int handle, int highTime0, int lowTime0, int highTime1, int lowTime1);
/**
* Sets the sync time.
@@ -76,10 +72,10 @@ public class AddressableLEDJNI extends JNIWrapper {
* <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B and WS2815.
*
* @param handle the Addressable LED handle
* @param syncTimeMicroSeconds the sync time (default 280us)
* @param syncTime the sync time in microseconds (default 280 μs)
* @see "HAL_SetAddressableLEDSyncTime"
*/
public static native void setSyncTime(int handle, int syncTimeMicroSeconds);
public static native void setSyncTime(int handle, int syncTime);
/**
* Starts the output.

8
hal/src/main/java/edu/wpi/first/hal/DIOJNI.java
View File

@@ -89,10 +89,10 @@ public class DIOJNI extends JNIWrapper {
* going at any time.
*
* @param dioPortHandle the digital port handle
* @param pulseLengthSeconds the active length of the pulse (in seconds)
* @param pulseLength the active length of the pulse in seconds
* @see "HAL_Pulse"
*/
public static native void pulse(int dioPortHandle, double pulseLengthSeconds);
public static native void pulse(int dioPortHandle, double pulseLength);
/**
* Generates a single digital pulse on multiple channels.
@@ -101,10 +101,10 @@ public class DIOJNI extends JNIWrapper {
* any time.
*
* @param channelMask the channel mask
* @param pulseLengthSeconds the active length of the pulse (in seconds)
* @param pulseLength the active length of the pulse in seconds
* @see "HAL_PulseMultiple"
*/
public static native void pulseMultiple(long channelMask, double pulseLengthSeconds);
public static native void pulseMultiple(long channelMask, double pulseLength);
/**
* Checks a DIO line to see if it is currently generating a pulse.

19
hal/src/main/native/include/hal/AddressableLED.h
View File

@@ -81,17 +81,15 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
* needs to be set for those.
*
* @param[in] handle the Addressable LED handle
* @param[in] highTime0NanoSeconds high time for 0 bit (default 400ns)
* @param[in] lowTime0NanoSeconds low time for 0 bit (default 900ns)
* @param[in] highTime1NanoSeconds high time for 1 bit (default 900ns)
* @param[in] lowTime1NanoSeconds low time for 1 bit (default 600ns)
* @param[in] highTime0 high time for 0 bit in nanoseconds (default 400 ns)
* @param[in] lowTime0 low time for 0 bit in nanoseconds (default 900 ns)
* @param[in] highTime1 high time for 1 bit in nanoseconds (default 900 ns)
* @param[in] lowTime1 low time for 1 bit in nanoseconds (default 600 ns)
* @param[out] status the error code, or 0 for success
*/
void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
int32_t highTime0NanoSeconds,
int32_t lowTime0NanoSeconds,
int32_t highTime1NanoSeconds,
int32_t lowTime1NanoSeconds,
int32_t highTime0, int32_t lowTime0,
int32_t highTime1, int32_t lowTime1,
int32_t* status);
/**
@@ -101,12 +99,11 @@ void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
* WS2812B and WS2815.
*
* @param[in] handle the Addressable LED handle
* @param[in] syncTimeMicroSeconds the sync time (default 280us)
* @param[in] syncTime the sync time in microseconds (default 280 μs)
* @param[out] status the error code, or 0 for success
*/
void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
int32_t syncTimeMicroSeconds,
int32_t* status);
int32_t syncTime, int32_t* status);
/**
* Starts the output.

10
hal/src/main/native/include/hal/DIO.h
View File

@@ -158,10 +158,10 @@ HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status);
* single pulse going at any time.
*
* @param[in] dioPortHandle the digital port handle
* @param[in] pulseLengthSeconds the active length of the pulse (in seconds)
* @param[in] pulseLength the active length of the pulse in seconds
* @param[out] status Error status variable. 0 on success.
*/
void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds,
void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,
int32_t* status);
/**
@@ -171,10 +171,10 @@ void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds,
* single pulse going at any time.
*
* @param[in] channelMask the channel mask
* @param[in] pulseLengthSeconds the active length of the pulse (in seconds)
* @param[out] status Error status variable. 0 on success.
* @param[in] pulseLength the active length of the pulse in seconds
* @param[out] status Error status variable. 0 on success.
*/
void HAL_PulseMultiple(uint32_t channelMask, double pulseLengthSeconds,
void HAL_PulseMultiple(uint32_t channelMask, double pulseLength,
int32_t* status);
/**

9
hal/src/main/native/sim/AddressableLED.cpp
View File

@@ -146,15 +146,12 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
}
void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
int32_t highTime0NanoSeconds,
int32_t lowTime0NanoSeconds,
int32_t highTime1NanoSeconds,
int32_t lowTime1NanoSeconds,
int32_t highTime0, int32_t lowTime0,
int32_t highTime1, int32_t lowTime1,
int32_t* status) {}
void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
int32_t syncTimeMicroSeconds,
int32_t* status) {}
int32_t syncTime, int32_t* status) {}
void HAL_StartAddressableLEDOutput(HAL_AddressableLEDHandle handle,
int32_t* status) {

5
hal/src/main/native/sim/DutyCycle.cpp
View File

@@ -107,9 +107,8 @@ int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
return 0;
}
double periodSeconds = 1.0 / SimDutyCycleData[dutyCycle->index].frequency;
double periodNanoSeconds = periodSeconds * 1e9;
return periodNanoSeconds * SimDutyCycleData[dutyCycle->index].output;
double period = 1e9 / SimDutyCycleData[dutyCycle->index].frequency; // ns
return period * SimDutyCycleData[dutyCycle->index].output;
}
int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,

9
hal/src/main/native/systemcore/AddressableLED.cpp
View File

@@ -54,18 +54,15 @@ void HAL_WriteAddressableLEDData(HAL_AddressableLEDHandle handle,
}
void HAL_SetAddressableLEDBitTiming(HAL_AddressableLEDHandle handle,
int32_t highTime0NanoSeconds,
int32_t lowTime0NanoSeconds,
int32_t highTime1NanoSeconds,
int32_t lowTime1NanoSeconds,
int32_t highTime0, int32_t lowTime0,
int32_t highTime1, int32_t lowTime1,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_SetAddressableLEDSyncTime(HAL_AddressableLEDHandle handle,
int32_t syncTimeMicroSeconds,
int32_t* status) {
int32_t syncTime, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}

4
hal/src/main/native/systemcore/DIO.cpp
View File

@@ -177,13 +177,13 @@ HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status) {
}
}
void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLengthSeconds,
void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_PulseMultiple(uint32_t channelMask, double pulseLengthSeconds,
void HAL_PulseMultiple(uint32_t channelMask, double pulseLength,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;

4
sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h → sysid/src/main/native/include/sysid/analysis/TrackwidthAnalysis.h
View File

@@ -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());

6
sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp → sysid/src/test/native/cpp/analysis/TrackwidthAnalysisTest.cpp
View File

@@ -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);
}

6
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
View File

@@ -121,12 +121,12 @@ public final class Commands {
/**
* Constructs a command that does nothing, finishing after a specified duration.
*
* @param seconds after how long the command finishes
* @param time after how long the command finishes in seconds
* @return the command
* @see WaitCommand
*/
public static Command waitSeconds(double seconds) {
return new WaitCommand(seconds);
public static Command wait(double time) {
return new WaitCommand(time);
}
/**

66
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View File

@@ -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;
@@ -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.
@@ -98,7 +98,7 @@ public class MecanumControllerCommand extends Command {
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocityMetersPerSecond,
double maxWheelVelocity,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
@@ -120,7 +120,7 @@ public class MecanumControllerCommand extends Command {
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
m_maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond;
m_maxWheelVelocity = maxWheelVelocity;
m_frontLeftController =
requireNonNullParam(frontLeftController, "frontLeftController", "MecanumControllerCommand");
@@ -266,8 +266,7 @@ public class MecanumControllerCommand extends Command {
xController,
yController,
thetaController,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
maxWheelVelocityMetersPerSecond,
frontLeftController,
rearLeftController,
@@ -299,7 +298,7 @@ public class MecanumControllerCommand extends Command {
* @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.
@@ -318,7 +317,7 @@ public class MecanumControllerCommand extends Command {
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocityMetersPerSecond,
double maxWheelVelocity,
PIDController frontLeftController,
PIDController rearLeftController,
PIDController frontRightController,
@@ -334,7 +333,7 @@ public class MecanumControllerCommand extends Command {
xController,
yController,
thetaController,
maxWheelVelocityMetersPerSecond,
maxWheelVelocity,
frontLeftController,
rearLeftController,
frontRightController,
@@ -362,7 +361,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 outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
* @param requirements The subsystems to require.
*/
@@ -375,7 +374,7 @@ public class MecanumControllerCommand extends Command {
PIDController yController,
ProfiledPIDController thetaController,
Supplier<Rotation2d> desiredRotation,
double maxWheelVelocityMetersPerSecond,
double maxWheelVelocity,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
@@ -392,7 +391,7 @@ public class MecanumControllerCommand extends Command {
m_desiredRotation =
requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
m_maxWheelVelocityMetersPerSecond = maxWheelVelocityMetersPerSecond;
m_maxWheelVelocity = maxWheelVelocity;
m_frontLeftController = null;
m_rearLeftController = null;
@@ -430,7 +429,7 @@ public class MecanumControllerCommand extends Command {
* @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.
*/
@@ -441,7 +440,7 @@ public class MecanumControllerCommand extends Command {
PIDController xController,
PIDController yController,
ProfiledPIDController thetaController,
double maxWheelVelocityMetersPerSecond,
double maxWheelVelocity,
Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
Subsystem... requirements) {
this(
@@ -451,9 +450,8 @@ public class MecanumControllerCommand extends Command {
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);
}
@@ -462,18 +460,16 @@ public class MecanumControllerCommand extends Command {
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();
}
@@ -488,12 +484,12 @@ public class MecanumControllerCommand extends Command {
m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
targetWheelSpeeds = targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
targetWheelSpeeds = 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;
@@ -516,22 +512,22 @@ public class MecanumControllerCommand extends Command {
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);
@@ -553,7 +549,7 @@ public class MecanumControllerCommand extends Command {
@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. */

8
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
View File

@@ -123,8 +123,7 @@ public class SwerveControllerCommand extends Command {
xController,
yController,
thetaController,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
outputModuleStates,
requirements);
}
@@ -162,8 +161,7 @@ public class SwerveControllerCommand extends Command {
pose,
kinematics,
controller,
() ->
trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
() -> trajectory.getStates().get(trajectory.getStates().size() - 1).pose.getRotation(),
outputModuleStates,
requirements);
}
@@ -233,6 +231,6 @@ public class SwerveControllerCommand extends Command {
@Override
public boolean isFinished() {
return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
return m_timer.hasElapsed(m_trajectory.getTotalTime());
}
}

30
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
View File

@@ -60,24 +60,24 @@ class MecanumControllerCommandTest {
private static final double kAngularTolerance = 1 / 12.0;
private static final double kWheelBase = 0.5;
private static final double kTrackWidth = 0.5;
private static final double kTrackwidth = 0.5;
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(
m_kinematics, Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero);
public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond;
this.m_rearLeftSpeed = wheelSpeeds.rearLeftMetersPerSecond;
this.m_frontRightSpeed = wheelSpeeds.frontRightMetersPerSecond;
this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond;
this.m_frontLeftSpeed = wheelSpeeds.frontLeft;
this.m_rearLeftSpeed = wheelSpeeds.rearLeft;
this.m_frontRightSpeed = wheelSpeeds.frontRight;
this.m_rearRightSpeed = wheelSpeeds.rearRight;
}
public MecanumDriveWheelPositions getCurrentWheelDistances() {
@@ -87,7 +87,7 @@ class MecanumControllerCommandTest {
public Pose2d getRobotPose() {
m_odometry.update(m_angle, getCurrentWheelDistances());
return m_odometry.getPoseMeters();
return m_odometry.getPose();
}
@Test
@@ -101,7 +101,7 @@ class MecanumControllerCommandTest {
var config = new TrajectoryConfig(8.8, 0.1);
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
final var endState = trajectory.sample(trajectory.getTotalTime());
final var command =
new MecanumControllerCommand(
@@ -120,7 +120,7 @@ class MecanumControllerCommandTest {
command.initialize();
while (!command.isFinished()) {
command.execute();
m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
m_angle = trajectory.sample(m_timer.get()).pose.getRotation();
m_frontLeftDistance += m_frontLeftSpeed * 0.005;
m_rearLeftDistance += m_rearLeftSpeed * 0.005;
m_frontRightDistance += m_frontRightSpeed * 0.005;
@@ -131,11 +131,11 @@ class MecanumControllerCommandTest {
command.end(true);
assertAll(
() -> assertEquals(endState.poseMeters.getX(), getRobotPose().getX(), kxTolerance),
() -> assertEquals(endState.poseMeters.getY(), getRobotPose().getY(), kyTolerance),
() -> assertEquals(endState.pose.getX(), getRobotPose().getX(), kxTolerance),
() -> assertEquals(endState.pose.getY(), getRobotPose().getY(), kyTolerance),
() ->
assertEquals(
endState.poseMeters.getRotation().getRadians(),
endState.pose.getRotation().getRadians(),
getRobotPose().getRotation().getRadians(),
kAngularTolerance));
}

24
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
View File

@@ -67,14 +67,14 @@ class SwerveControllerCommandTest {
private static final double kAngularTolerance = 1 / 12.0;
private static final double kWheelBase = 0.5;
private static final double kTrackWidth = 0.5;
private static final double kTrackwidth = 0.5;
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
private final SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(m_kinematics, Rotation2d.kZero, m_modulePositions, Pose2d.kZero);
@@ -86,7 +86,7 @@ class SwerveControllerCommandTest {
public Pose2d getRobotPose() {
m_odometry.update(m_angle, m_modulePositions);
return m_odometry.getPoseMeters();
return m_odometry.getPose();
}
@Test
@@ -100,7 +100,7 @@ class SwerveControllerCommandTest {
var config = new TrajectoryConfig(8.8, 0.1);
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
final var endState = trajectory.sample(trajectory.getTotalTime());
final var command =
new SwerveControllerCommand(
@@ -118,10 +118,10 @@ class SwerveControllerCommandTest {
command.initialize();
while (!command.isFinished()) {
command.execute();
m_angle = trajectory.sample(m_timer.get()).poseMeters.getRotation();
m_angle = trajectory.sample(m_timer.get()).pose.getRotation();
for (int i = 0; i < m_modulePositions.length; i++) {
m_modulePositions[i].distanceMeters += m_moduleStates[i].speedMetersPerSecond * 0.005;
m_modulePositions[i].distance += m_moduleStates[i].speed * 0.005;
m_modulePositions[i].angle = m_moduleStates[i].angle;
}
@@ -131,11 +131,11 @@ class SwerveControllerCommandTest {
command.end(true);
assertAll(
() -> assertEquals(endState.poseMeters.getX(), getRobotPose().getX(), kxTolerance),
() -> assertEquals(endState.poseMeters.getY(), getRobotPose().getY(), kyTolerance),
() -> assertEquals(endState.pose.getX(), getRobotPose().getX(), kxTolerance),
() -> assertEquals(endState.pose.getY(), getRobotPose().getY(), kyTolerance),
() ->
assertEquals(
endState.poseMeters.getRotation().getRadians(),
endState.pose.getRotation().getRadians(),
getRobotPose().getRotation().getRadians(),
kAngularTolerance));
}

10
wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
View File

@@ -51,13 +51,13 @@ class MecanumControllerCommandTest : public ::testing::Test {
static constexpr units::radian_t kAngularTolerance{1 / 12.0};
static constexpr units::meter_t kWheelBase{0.5};
static constexpr units::meter_t kTrackWidth{0.5};
static constexpr units::meter_t kTrackwidth{0.5};
frc::MecanumDriveKinematics m_kinematics{
frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
getCurrentWheelDistances(),

10
wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
View File

@@ -51,13 +51,13 @@ class SwerveControllerCommandTest : public ::testing::Test {
static constexpr units::radian_t kAngularTolerance{1 / 12.0};
static constexpr units::meter_t kWheelBase{0.5};
static constexpr units::meter_t kTrackWidth{0.5};
static constexpr units::meter_t kTrackwidth{0.5};
frc::SwerveDriveKinematics<4> m_kinematics{
frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad, m_modulePositions,
frc::Pose2d{0_m, 0_m, 0_rad}};

29
wpilibc/src/main/native/cpp/SharpIR.cpp
View File

@@ -7,6 +7,7 @@
#include <algorithm>
#include <hal/UsageReporting.h>
#include <units/length.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
@@ -15,29 +16,30 @@
using namespace frc;
SharpIR SharpIR::GP2Y0A02YK0F(int channel) {
return SharpIR(channel, 62.28, -1.092, 20, 150.0);
return SharpIR(channel, 62.28, -1.092, 20_cm, 150_cm);
}
SharpIR SharpIR::GP2Y0A21YK0F(int channel) {
return SharpIR(channel, 26.449, -1.226, 10.0, 80.0);
return SharpIR(channel, 26.449, -1.226, 10_cm, 80_cm);
}
SharpIR SharpIR::GP2Y0A41SK0F(int channel) {
return SharpIR(channel, 12.354, -1.07, 4.0, 30.0);
return SharpIR(channel, 12.354, -1.07, 4_cm, 30_cm);
}
SharpIR SharpIR::GP2Y0A51SK0F(int channel) {
return SharpIR(channel, 5.2819, -1.161, 2.0, 15.0);
return SharpIR(channel, 5.2819, -1.161, 2_cm, 15_cm);
}
SharpIR::SharpIR(int channel, double a, double b, double minCM, double maxCM)
: m_sensor(channel), m_A(a), m_B(b), m_minCM(minCM), m_maxCM(maxCM) {
SharpIR::SharpIR(int channel, double a, double b, units::meter_t min,
units::meter_t max)
: m_sensor(channel), m_A(a), m_B(b), m_min(min), m_max(max) {
HAL_ReportUsage("IO", channel, "SharpIR");
wpi::SendableRegistry::Add(this, "SharpIR", channel);
m_simDevice = hal::SimDevice("SharpIR", m_sensor.GetChannel());
if (m_simDevice) {
m_simRange = m_simDevice.CreateDouble("Range (cm)", false, 0.0);
m_simRange = m_simDevice.CreateDouble("Range (m)", false, 0.0);
m_sensor.SetSimDevice(m_simDevice);
}
}
@@ -46,19 +48,16 @@ int SharpIR::GetChannel() const {
return m_sensor.GetChannel();
}
units::centimeter_t SharpIR::GetRange() const {
double distance;
units::meter_t SharpIR::GetRange() const {
if (m_simRange) {
distance = m_simRange.Get();
return std::clamp(units::meter_t{m_simRange.Get()}, m_min, m_max);
} else {
// Don't allow zero/negative values
auto v = std::max(m_sensor.GetVoltage(), 0.00001);
distance = m_A * std::pow(v, m_B);
}
// Always constrain output
return units::centimeter_t{std::max(std::min(distance, m_maxCM), m_minCM)};
return std::clamp(units::meter_t{m_A * std::pow(v, m_B) * 1e-2}, m_min,
m_max);
}
}
void SharpIR::InitSendable(wpi::SendableBuilder& builder) {

10
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -18,11 +18,11 @@ using namespace frc;
using namespace frc::sim;
DifferentialDrivetrainSim::DifferentialDrivetrainSim(
LinearSystem<2, 2, 2> plant, units::meter_t trackWidth, DCMotor driveMotor,
LinearSystem<2, 2, 2> plant, units::meter_t trackwidth, DCMotor driveMotor,
double gearRatio, units::meter_t wheelRadius,
const std::array<double, 7>& measurementStdDevs)
: m_plant(std::move(plant)),
m_rb(trackWidth / 2.0),
m_rb(trackwidth / 2.0),
m_wheelRadius(wheelRadius),
m_motor(driveMotor),
m_originalGearing(gearRatio),
@@ -36,11 +36,11 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
DifferentialDrivetrainSim::DifferentialDrivetrainSim(
frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
units::kilogram_t mass, units::meter_t wheelRadius,
units::meter_t trackWidth, const std::array<double, 7>& measurementStdDevs)
units::meter_t trackwidth, const std::array<double, 7>& measurementStdDevs)
: DifferentialDrivetrainSim(
frc::LinearSystemId::DrivetrainVelocitySystem(
driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
driveMotor, mass, wheelRadius, trackwidth / 2.0, J, gearing),
trackwidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
Eigen::Vector2d DifferentialDrivetrainSim::ClampInput(
const Eigen::Vector2d& u) {

6
wpilibc/src/main/native/cpp/simulation/SharpIRSim.cpp
View File

@@ -16,9 +16,9 @@ SharpIRSim::SharpIRSim(const SharpIR& sharpIR)
SharpIRSim::SharpIRSim(int channel) {
frc::sim::SimDeviceSim deviceSim{"SharpIR", channel};
m_simRange = deviceSim.GetDouble("Range (cm)");
m_simRange = deviceSim.GetDouble("Range (m)");
}
void SharpIRSim::SetRange(units::centimeter_t rng) {
m_simRange.Set(rng.value());
void SharpIRSim::SetRange(units::meter_t range) {
m_simRange.Set(range.value());
}

8
wpilibc/src/main/native/include/frc/AddressableLED.h
View File

@@ -133,10 +133,10 @@ class AddressableLED {
* <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing
* needs to be set for those.
*
* @param highTime0 high time for 0 bit (default 400ns)
* @param lowTime0 low time for 0 bit (default 900ns)
* @param highTime1 high time for 1 bit (default 900ns)
* @param lowTime1 low time for 1 bit (default 600ns)
* @param highTime0 high time for 0 bit (default 400 ns)
* @param lowTime0 low time for 0 bit (default 900 ns)
* @param highTime1 high time for 1 bit (default 900 ns)
* @param lowTime1 low time for 1 bit (default 600 ns)
*/
void SetBitTiming(units::nanosecond_t highTime0, units::nanosecond_t lowTime0,
units::nanosecond_t highTime1,

13
wpilibc/src/main/native/include/frc/SharpIR.h
View File

@@ -63,10 +63,11 @@ class SharpIR : public wpi::Sendable, public wpi::SendableHelper<SharpIR> {
* @param channel Analog input channel the sensor is connected to
* @param a Constant A
* @param b Constant B
* @param minCM Minimum distance to report in centimeters
* @param maxCM Maximum distance to report in centimeters
* @param min Minimum distance to report
* @param max Maximum distance to report
*/
SharpIR(int channel, double a, double b, double minCM, double maxCM);
SharpIR(int channel, double a, double b, units::meter_t min,
units::meter_t max);
/**
* Get the analog input channel number.
@@ -80,7 +81,7 @@ class SharpIR : public wpi::Sendable, public wpi::SendableHelper<SharpIR> {
*
* @return range of the target returned by the sensor
*/
units::centimeter_t GetRange() const;
units::meter_t GetRange() const;
void InitSendable(wpi::SendableBuilder& builder) override;
@@ -92,8 +93,8 @@ class SharpIR : public wpi::Sendable, public wpi::SendableHelper<SharpIR> {
double m_A;
double m_B;
double m_minCM;
double m_maxCM;
units::meter_t m_min;
units::meter_t m_max;
};
} // namespace frc

8
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
View File

@@ -25,7 +25,7 @@ class DifferentialDrivetrainSim {
* system can be created with
* LinearSystemId::DrivetrainVelocitySystem() or
* LinearSystemId::IdentifyDrivetrainSystem().
* @param trackWidth The robot's track width.
* @param trackwidth The robot's trackwidth.
* @param driveMotor A DCMotor representing the left side of the drivetrain.
* @param gearingRatio The gearingRatio ratio of the left side, as output over
* input. This must be the same ratio as the ratio used to
@@ -41,7 +41,7 @@ class DifferentialDrivetrainSim {
* starting point.
*/
DifferentialDrivetrainSim(
LinearSystem<2, 2, 2> plant, units::meter_t trackWidth,
LinearSystem<2, 2, 2> plant, units::meter_t trackwidth,
DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius,
const std::array<double, 7>& measurementStdDevs = {});
@@ -56,7 +56,7 @@ class DifferentialDrivetrainSim {
* center.
* @param mass The mass of the drivebase.
* @param wheelRadius The radius of the wheels on the drivetrain.
* @param trackWidth The robot's track width, or distance between left and
* @param trackwidth The robot's trackwidth, or distance between left and
* right wheels.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity,
@@ -70,7 +70,7 @@ class DifferentialDrivetrainSim {
DifferentialDrivetrainSim(
frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
units::kilogram_t mass, units::meter_t wheelRadius,
units::meter_t trackWidth,
units::meter_t trackwidth,
const std::array<double, 7>& measurementStdDevs = {});
/**

4
wpilibc/src/main/native/include/frc/simulation/SharpIRSim.h
View File

@@ -31,9 +31,9 @@ class SharpIRSim {
/**
* Set the range returned by the distance sensor.
*
* @param rng range of the target returned by the sensor
* @param range range of the target returned by the sensor
*/
void SetRange(units::centimeter_t rng);
void SetRange(units::meter_t range);
private:
hal::SimDouble m_simRange;

10
wpilibc/src/test/native/cpp/SharpIRTest.cpp
View File

@@ -13,11 +13,11 @@ TEST(SharpIRTest, SimDevices) {
SharpIR s = SharpIR::GP2Y0A02YK0F(1);
SharpIRSim sim(s);
EXPECT_EQ(20, s.GetRange().value());
EXPECT_EQ(0.2, s.GetRange().value());
sim.SetRange(units::centimeter_t{30});
EXPECT_EQ(30, s.GetRange().value());
sim.SetRange(30_cm);
EXPECT_EQ(0.3, s.GetRange().value());
sim.SetRange(units::centimeter_t{300});
EXPECT_EQ(150, s.GetRange().value());
sim.SetRange(300_cm);
EXPECT_EQ(1.5, s.GetRange().value());
}

4
wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
View File

@@ -56,7 +56,7 @@ class Drivetrain {
void UpdateOdometry();
private:
static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
static constexpr units::meter_t kTrackwidth = 0.381_m * 2;
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
@@ -73,7 +73,7 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
frc::DifferentialDriveKinematics m_kinematics{kTrackwidth};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};

6
wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
View File

@@ -111,7 +111,7 @@ class Drivetrain {
nt::DoubleArrayEntry& cameraToObjectEntry);
private:
static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
static constexpr units::meter_t kTrackwidth = 0.381_m * 2;
static constexpr units::meter_t kWheelRadius = 0.0508_m;
static constexpr int kEncoderResolution = 4096;
@@ -146,7 +146,7 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
frc::DifferentialDriveKinematics m_kinematics{kTrackwidth};
// Gains are for example purposes only - must be determined for your own
// robot!
@@ -172,5 +172,5 @@ class Drivetrain {
frc::LinearSystemId::IdentifyDrivetrainSystem(
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
m_drivetrainSystem, kTrackwidth, frc::DCMotor::CIM(2), 8, 2_in};
};

8
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp
View File

@@ -7,10 +7,10 @@
namespace DriveConstants {
const frc::MecanumDriveKinematics kDriveKinematics{
frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
} // namespace DriveConstants

6
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
View File

@@ -43,17 +43,17 @@ inline constexpr bool kRearLeftEncoderReversed = true;
inline constexpr bool kFrontRightEncoderReversed = false;
inline constexpr bool kRearRightEncoderReversed = true;
inline constexpr auto kTrackWidth =
inline constexpr auto kTrackwidth =
0.5_m; // Distance between centers of right and left wheels on robot
inline constexpr auto kWheelBase =
0.7_m; // Distance between centers of front and back wheels on robot
extern const frc::MecanumDriveKinematics kDriveKinematics;
inline constexpr int kEncoderCPR = 1024;
inline constexpr double kWheelDiameterMeters = 0.15;
inline constexpr auto kWheelDiameter = 0.15_m;
inline constexpr double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * std::numbers::pi) /
(kWheelDiameter.value() * std::numbers::pi) /
static_cast<double>(kEncoderCPR);
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!

6
wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
View File

@@ -72,7 +72,7 @@ class Drivetrain {
void Periodic();
private:
static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
static constexpr units::meter_t kTrackwidth = 0.381_m * 2;
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
@@ -89,7 +89,7 @@ class Drivetrain {
frc::AnalogGyro m_gyro{0};
frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
frc::DifferentialDriveKinematics m_kinematics{kTrackwidth};
frc::DifferentialDriveOdometry m_odometry{
m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
@@ -106,5 +106,5 @@ class Drivetrain {
frc::LinearSystemId::IdentifyDrivetrainSystem(
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
m_drivetrainSystem, kTrackwidth, frc::DCMotor::CIM(2), 8, 2_in};
};

4
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
View File

@@ -80,10 +80,10 @@ inline constexpr double kPRearRightVel = 0.5;
namespace ModuleConstants {
inline constexpr int kEncoderCPR = 1024;
inline constexpr double kWheelDiameterMeters = 0.15;
inline constexpr auto kWheelDiameter = 0.15_m;
inline constexpr double kDriveEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * std::numbers::pi) /
(kWheelDiameter.value() * std::numbers::pi) /
static_cast<double>(kEncoderCPR);
inline constexpr double kTurningEncoderDistancePerPulse =

10
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -88,16 +88,16 @@ class DriveSubsystem : public frc2::SubsystemBase {
*/
void ResetOdometry(frc::Pose2d pose);
units::meter_t kTrackWidth =
units::meter_t kTrackwidth =
0.5_m; // Distance between centers of right and left wheels on robot
units::meter_t kWheelBase =
0.7_m; // Distance between centers of front and back wheels on robot
frc::SwerveDriveKinematics<4> kDriveKinematics{
frc::Translation2d{kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{kWheelBase / 2, -kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
private:
// Components (e.g. motor controllers and sensors) should generally be

27
wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
View File

@@ -73,22 +73,13 @@ public class AddressableLED implements AutoCloseable {
* <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing needs to be set for
* those.
*
* @param highTime0NanoSeconds high time for 0 bit (default 400ns)
* @param lowTime0NanoSeconds low time for 0 bit (default 900ns)
* @param highTime1NanoSeconds high time for 1 bit (default 900ns)
* @param lowTime1NanoSeconds low time for 1 bit (default 600ns)
* @param highTime0 high time for 0 bit in nanoseconds (default 400 ns)
* @param lowTime0 low time for 0 bit in nanoseconds (default 900 ns)
* @param highTime1 high time for 1 bit in nanoseconds (default 900 ns)
* @param lowTime1 low time for 1 bit in nanoseconds (default 600 ns)
*/
public void setBitTiming(
int highTime0NanoSeconds,
int lowTime0NanoSeconds,
int highTime1NanoSeconds,
int lowTime1NanoSeconds) {
AddressableLEDJNI.setBitTiming(
m_handle,
highTime0NanoSeconds,
lowTime0NanoSeconds,
highTime1NanoSeconds,
lowTime1NanoSeconds);
public void setBitTiming(int highTime0, int lowTime0, int highTime1, int lowTime1) {
AddressableLEDJNI.setBitTiming(m_handle, highTime0, lowTime0, highTime1, lowTime1);
}
/**
@@ -96,10 +87,10 @@ public class AddressableLED implements AutoCloseable {
*
* <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B and WS2815.
*
* @param syncTimeMicroSeconds the sync time (default 280us)
* @param syncTime the sync time in microseconds (default 280 μs)
*/
public void setSyncTime(int syncTimeMicroSeconds) {
AddressableLEDJNI.setSyncTime(m_handle, syncTimeMicroSeconds);
public void setSyncTime(int syncTime) {
AddressableLEDJNI.setSyncTime(m_handle, syncTime);
}
/**

6
wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
View File

@@ -83,10 +83,10 @@ public class DigitalOutput implements AutoCloseable, Sendable {
* <p>Send a single pulse on the digital output line where the pulse duration is specified in
* seconds. Maximum of 65535 microseconds.
*
* @param pulseLengthSeconds The pulse length in seconds
* @param pulseLength The pulse length in seconds
*/
public void pulse(final double pulseLengthSeconds) {
DIOJNI.pulse(m_handle, pulseLengthSeconds);
public void pulse(final double pulseLength) {
DIOJNI.pulse(m_handle, pulseLength);
}
/**

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
View File

@@ -1148,19 +1148,19 @@ public final class DriverStation {
/**
* Wait for a DS connection.
*
* @param timeoutSeconds timeout in seconds. 0 for infinite.
* @param timeout timeout in seconds. 0 for infinite.
* @return true if connected, false if timeout
*/
public static boolean waitForDsConnection(double timeoutSeconds) {
public static boolean waitForDsConnection(double timeout) {
int event = WPIUtilJNI.createEvent(true, false);
DriverStationJNI.provideNewDataEventHandle(event);
boolean result;
try {
if (timeoutSeconds == 0) {
if (timeout == 0) {
WPIUtilJNI.waitForObject(event);
result = true;
} else {
result = !WPIUtilJNI.waitForObjectTimeout(event, timeoutSeconds);
result = !WPIUtilJNI.waitForObjectTimeout(event, timeout);
}
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();

30
wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
View File

@@ -31,11 +31,11 @@ public class Notifier implements AutoCloseable {
// The time, in seconds, at which the callback should be called. Has the same
// zero as RobotController.getFPGATime().
private double m_expirationTimeSeconds;
private double m_expirationTime;
// If periodic, stores the callback period; if single, stores the time until
// the callback call.
private double m_periodSeconds;
// If periodic, stores the callback period in seconds; if single, stores the time until
// the callback call in seconds.
private double m_period;
// True if the callback is periodic
private boolean m_periodic;
@@ -75,7 +75,7 @@ public class Notifier implements AutoCloseable {
/** Update the alarm hardware to reflect the next alarm. */
private void updateAlarm() {
updateAlarm((long) (m_expirationTimeSeconds * 1e6));
updateAlarm((long) (m_expirationTime * 1e6));
}
/**
@@ -109,7 +109,7 @@ public class Notifier implements AutoCloseable {
try {
threadHandler = m_callback;
if (m_periodic) {
m_expirationTimeSeconds += m_periodSeconds;
m_expirationTime += m_period;
updateAlarm();
} else {
// Need to update the alarm to cause it to wait again
@@ -172,14 +172,14 @@ public class Notifier implements AutoCloseable {
/**
* Run the callback once after the given delay.
*
* @param delaySeconds Time in seconds to wait before the callback is called.
* @param delay Time in seconds to wait before the callback is called.
*/
public void startSingle(double delaySeconds) {
public void startSingle(double delay) {
m_processLock.lock();
try {
m_periodic = false;
m_periodSeconds = delaySeconds;
m_expirationTimeSeconds = RobotController.getFPGATime() * 1e-6 + delaySeconds;
m_period = delay;
m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay;
updateAlarm();
} finally {
m_processLock.unlock();
@@ -192,15 +192,15 @@ public class Notifier implements AutoCloseable {
* <p>The user-provided callback should be written so that it completes before the next time it's
* scheduled to run.
*
* @param periodSeconds Period in seconds after which to to call the callback starting one period
* after the call to this method.
* @param period Period in seconds after which to to call the callback starting one period after
* the call to this method.
*/
public void startPeriodic(double periodSeconds) {
public void startPeriodic(double period) {
m_processLock.lock();
try {
m_periodic = true;
m_periodSeconds = periodSeconds;
m_expirationTimeSeconds = RobotController.getFPGATime() * 1e-6 + periodSeconds;
m_period = period;
m_expirationTime = RobotController.getFPGATime() * 1e-6 + period;
updateAlarm();
} finally {
m_processLock.unlock();

60
wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java
View File

@@ -8,6 +8,7 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
@@ -28,47 +29,47 @@ public class SharpIR implements Sendable, AutoCloseable {
private final double m_A;
private final double m_B;
private final double m_minCM;
private final double m_maxCM;
private final double m_min; // m
private final double m_max; // m
/**
* Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20cm to 150cm.
* Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20 cm to 150 cm.
*
* @param channel Analog input channel the sensor is connected to
* @return sensor object
*/
public static SharpIR GP2Y0A02YK0F(int channel) {
return new SharpIR(channel, 62.28, -1.092, 20, 150.0);
return new SharpIR(channel, 62.28, -1.092, 0.2, 1.5);
}
/**
* Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10cm to 80cm.
* Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10 cm to 80 cm.
*
* @param channel Analog input channel the sensor is connected to
* @return sensor object
*/
public static SharpIR GP2Y0A21YK0F(int channel) {
return new SharpIR(channel, 26.449, -1.226, 10.0, 80.0);
return new SharpIR(channel, 26.449, -1.226, 0.1, 0.8);
}
/**
* Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4cm to 30cm.
* Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4 cm to 30 cm.
*
* @param channel Analog input channel the sensor is connected to
* @return sensor object
*/
public static SharpIR GP2Y0A41SK0F(int channel) {
return new SharpIR(channel, 12.354, -1.07, 4.0, 30.0);
return new SharpIR(channel, 12.354, -1.07, 0.04, 0.3);
}
/**
* Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2cm to 15cm.
* Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2 cm to 15 cm.
*
* @param channel Analog input channel the sensor is connected to
* @return sensor object
*/
public static SharpIR GP2Y0A51SK0F(int channel) {
return new SharpIR(channel, 5.2819, -1.161, 2.0, 15.0);
return new SharpIR(channel, 5.2819, -1.161, 0.02, 0.15);
}
/**
@@ -78,24 +79,24 @@ public class SharpIR implements Sendable, AutoCloseable {
* @param channel AnalogInput channel
* @param a Constant A
* @param b Constant B
* @param minCM Minimum distance to report in centimeters
* @param maxCM Maximum distance to report in centimeters
* @param min Minimum distance to report in meters
* @param max Maximum distance to report in meters
*/
@SuppressWarnings("this-escape")
public SharpIR(int channel, double a, double b, double minCM, double maxCM) {
public SharpIR(int channel, double a, double b, double min, double max) {
m_sensor = new AnalogInput(channel);
m_A = a;
m_B = b;
m_minCM = minCM;
m_maxCM = maxCM;
m_min = min;
m_max = max;
HAL.reportUsage("IO", channel, "SharpIR");
SendableRegistry.add(this, "SharpIR", channel);
m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel());
if (m_simDevice != null) {
m_simRange = m_simDevice.createDouble("Range (cm)", Direction.kInput, 0.0);
m_simRange = m_simDevice.createDouble("Range (m)", Direction.kInput, 0.0);
m_sensor.setSimDevice(m_simDevice);
}
}
@@ -123,37 +124,24 @@ public class SharpIR implements Sendable, AutoCloseable {
}
/**
* Get the range in inches from the distance sensor.
* Get the range in meters from the distance sensor.
*
* @return range in inches of the target returned by the sensor
* @return range in meters of the target returned by the sensor
*/
public double getRangeInches() {
return getRangeCM() / 2.54;
}
/**
* Get the range in centimeters from the distance sensor.
*
* @return range in centimeters of the target returned by the sensor
*/
public double getRangeCM() {
double distance;
public double getRange() {
if (m_simRange != null) {
distance = m_simRange.get();
return MathUtil.clamp(m_simRange.get(), m_min, m_max);
} else {
// Don't allow zero/negative values
var v = Math.max(m_sensor.getVoltage(), 0.00001);
distance = m_A * Math.pow(v, m_B);
}
// Always constrain output
return Math.max(Math.min(distance, m_maxCM), m_minCM);
return MathUtil.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max);
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Ultrasonic");
builder.addDoubleProperty("Value", this::getRangeInches, null);
builder.addDoubleProperty("Value", this::getRange, null);
}
}

6
wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
View File

@@ -125,11 +125,11 @@ public class Solenoid implements Sendable, AutoCloseable {
* <p>On the PH, the timing can be controlled in 0.001 second increments, with a maximum of 65.534
* seconds.
*
* @param durationSeconds The duration of the pulse in seconds.
* @param duration The duration of the pulse in seconds.
* @see #startPulse()
*/
public void setPulseDuration(double durationSeconds) {
long durationMS = (long) (durationSeconds * 1000);
public void setPulseDuration(double duration) {
long durationMS = (long) (duration * 1000);
m_module.setOneShotDuration(m_channel, (int) durationMS);
}

31
wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
View File

@@ -30,18 +30,18 @@ public class TimedRobot extends IterativeRobotBase {
* Construct a callback container.
*
* @param func The callback to run.
* @param startTimeUs The common starting point for all callback scheduling in microseconds.
* @param periodUs The period at which to run the callback in microseconds.
* @param offsetUs The offset from the common starting time in microseconds.
* @param startTime The common starting point for all callback scheduling in microseconds.
* @param period The period at which to run the callback in microseconds.
* @param offset The offset from the common starting time in microseconds.
*/
Callback(Runnable func, long startTimeUs, long periodUs, long offsetUs) {
Callback(Runnable func, long startTime, long period, long offset) {
this.func = func;
this.period = periodUs;
this.period = period;
this.expirationTime =
startTimeUs
+ offsetUs
startTime
+ offset
+ this.period
+ (RobotController.getFPGATime() - startTimeUs) / this.period * this.period;
+ (RobotController.getFPGATime() - startTime) / this.period * this.period;
}
@Override
@@ -177,10 +177,10 @@ public class TimedRobot extends IterativeRobotBase {
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param periodSeconds The period at which to run the callback in seconds.
* @param period The period at which to run the callback in seconds.
*/
public final void addPeriodic(Runnable callback, double periodSeconds) {
m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (periodSeconds * 1e6), 0));
public final void addPeriodic(Runnable callback, double period) {
m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0));
}
/**
@@ -190,14 +190,13 @@ public class TimedRobot extends IterativeRobotBase {
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param periodSeconds The period at which to run the callback in seconds.
* @param offsetSeconds The offset from the common starting time in seconds. This is useful for
* @param period The period at which to run the callback in seconds.
* @param offset The offset from the common starting time in seconds. This is useful for
* scheduling a callback in a different timeslot relative to TimedRobot.
*/
public final void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
public final void addPeriodic(Runnable callback, double period, double offset) {
m_callbacks.add(
new Callback(
callback, m_startTimeUs, (long) (periodSeconds * 1e6), (long) (offsetSeconds * 1e6)));
new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6)));
}
/**

48
wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
View File

@@ -19,13 +19,13 @@ import java.util.concurrent.locks.ReentrantLock;
*/
public class Watchdog implements Closeable, Comparable<Watchdog> {
// Used for timeout print rate-limiting
private static final long kMinPrintPeriodMicroS = (long) 1e6;
private static final long kMinPrintPeriod = (long) 1e6; // μs
private double m_startTimeSeconds;
private double m_timeoutSeconds;
private double m_expirationTimeSeconds;
private double m_startTime;
private double m_timeout;
private double m_expirationTime;
private final Runnable m_callback;
private double m_lastTimeoutPrintSeconds;
private double m_lastTimeoutPrint; // s
boolean m_isExpired;
@@ -46,11 +46,11 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
/**
* Watchdog constructor.
*
* @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution.
* @param timeout The watchdog's timeout in seconds with microsecond resolution.
* @param callback This function is called when the timeout expires.
*/
public Watchdog(double timeoutSeconds, Runnable callback) {
m_timeoutSeconds = timeoutSeconds;
public Watchdog(double timeout, Runnable callback) {
m_timeout = timeout;
m_callback = callback;
m_tracer = new Tracer();
}
@@ -63,19 +63,19 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
@Override
public boolean equals(Object obj) {
return obj instanceof Watchdog watchdog
&& Double.compare(m_expirationTimeSeconds, watchdog.m_expirationTimeSeconds) == 0;
&& Double.compare(m_expirationTime, watchdog.m_expirationTime) == 0;
}
@Override
public int hashCode() {
return Double.hashCode(m_expirationTimeSeconds);
return Double.hashCode(m_expirationTime);
}
@Override
public int compareTo(Watchdog rhs) {
// Elements with sooner expiration times are sorted as lesser. The head of
// Java's PriorityQueue is the least element.
return Double.compare(m_expirationTimeSeconds, rhs.m_expirationTimeSeconds);
return Double.compare(m_expirationTime, rhs.m_expirationTime);
}
/**
@@ -84,25 +84,25 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
* @return The time in seconds since the watchdog was last fed.
*/
public double getTime() {
return Timer.getFPGATimestamp() - m_startTimeSeconds;
return Timer.getFPGATimestamp() - m_startTime;
}
/**
* Sets the watchdog's timeout.
*
* @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution.
* @param timeout The watchdog's timeout in seconds with microsecond resolution.
*/
public void setTimeout(double timeoutSeconds) {
m_startTimeSeconds = Timer.getFPGATimestamp();
public void setTimeout(double timeout) {
m_startTime = Timer.getFPGATimestamp();
m_tracer.clearEpochs();
m_queueMutex.lock();
try {
m_timeoutSeconds = timeoutSeconds;
m_timeout = timeout;
m_isExpired = false;
m_watchdogs.remove(this);
m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds;
m_expirationTime = m_startTime + m_timeout;
m_watchdogs.add(this);
updateAlarm();
} finally {
@@ -118,7 +118,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
public double getTimeout() {
m_queueMutex.lock();
try {
return m_timeoutSeconds;
return m_timeout;
} finally {
m_queueMutex.unlock();
}
@@ -168,7 +168,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
/** Enables the watchdog timer. */
public void enable() {
m_startTimeSeconds = Timer.getFPGATimestamp();
m_startTime = Timer.getFPGATimestamp();
m_tracer.clearEpochs();
m_queueMutex.lock();
@@ -176,7 +176,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
m_isExpired = false;
m_watchdogs.remove(this);
m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds;
m_expirationTime = m_startTime + m_timeout;
m_watchdogs.add(this);
updateAlarm();
} finally {
@@ -212,7 +212,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
NotifierJNI.cancelNotifierAlarm(m_notifier);
} else {
NotifierJNI.updateNotifierAlarm(
m_notifier, (long) (m_watchdogs.peek().m_expirationTimeSeconds * 1e6));
m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6));
}
}
@@ -241,11 +241,11 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
Watchdog watchdog = m_watchdogs.poll();
double now = curTime * 1e-6;
if (now - watchdog.m_lastTimeoutPrintSeconds > kMinPrintPeriodMicroS) {
watchdog.m_lastTimeoutPrintSeconds = now;
if (now - watchdog.m_lastTimeoutPrint > kMinPrintPeriod) {
watchdog.m_lastTimeoutPrint = now;
if (!watchdog.m_suppressTimeoutMessage) {
DriverStation.reportWarning(
String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeoutSeconds), false);
String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout), false);
}
}

113
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java
View File

@@ -4,19 +4,11 @@
package edu.wpi.first.wpilibj.simulation;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularAcceleration;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.RobotController;
/** Represents a simulated DC motor mechanism. */
@@ -27,8 +19,8 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
// The gearing from the motors to the output.
private final double m_gearing;
// The moment of inertia for the DC motor mechanism.
private final double m_jKgMetersSquared;
// The moment of inertia for the DC motor mechanism in kg-m².
private final double m_j;
/**
* Creates a simulated DC motor mechanism.
@@ -64,36 +56,36 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
//
// B = GKₜ/(RJ)
// J = GKₜ/(RB)
m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(1, 1) / plant.getB(1, 0);
m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(1, 0));
m_gearing = -gearbox.Kv * plant.getA(1, 1) / plant.getB(1, 0);
m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(1, 0));
}
/**
* Sets the state of the DC motor.
*
* @param angularPositionRad The new position in radians.
* @param angularVelocityRadPerSec The new velocity in radians per second.
* @param angularPosition The new position in radians.
* @param angularVelocity The new velocity in radians per second.
*/
public void setState(double angularPositionRad, double angularVelocityRadPerSec) {
setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec));
public void setState(double angularPosition, double angularVelocity) {
setState(VecBuilder.fill(angularPosition, angularVelocity));
}
/**
* Sets the DC motor's angular position.
*
* @param angularPositionRad The new position in radians.
* @param angularPosition The new position in radians.
*/
public void setAngle(double angularPositionRad) {
setState(angularPositionRad, getAngularVelocityRadPerSec());
public void setAngle(double angularPosition) {
setState(angularPosition, getAngularVelocity());
}
/**
* Sets the DC motor's angular velocity.
*
* @param angularVelocityRadPerSec The new velocity in radians per second.
* @param angularVelocity The new velocity in radians per second.
*/
public void setAngularVelocity(double angularVelocityRadPerSec) {
setState(getAngularPositionRad(), angularVelocityRadPerSec);
public void setAngularVelocity(double angularVelocity) {
setState(getAngularPosition(), angularVelocity);
}
/**
@@ -108,10 +100,10 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
/**
* Returns the moment of inertia of the DC motor.
*
* @return The DC motor's moment of inertia.
* @return The DC motor's moment of inertia in kg-m².
*/
public double getJKgMetersSquared() {
return m_jKgMetersSquared;
public double getJ() {
return m_j;
}
/**
@@ -126,91 +118,46 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
/**
* Returns the DC motor's position.
*
* @return The DC motor's position.
* @return The DC motor's position in meters.
*/
public double getAngularPositionRad() {
public double getAngularPosition() {
return getOutput(0);
}
/**
* Returns the DC motor's position in rotations.
*
* @return The DC motor's position in rotations.
*/
public double getAngularPositionRotations() {
return Units.radiansToRotations(getAngularPositionRad());
}
/**
* Returns the DC motor's position.
*
* @return The DC motor's position
*/
public Angle getAngularPosition() {
return Radians.of(getAngularPositionRad());
}
/**
* Returns the DC motor's velocity.
*
* @return The DC motor's velocity.
*/
public double getAngularVelocityRadPerSec() {
public double getAngularVelocity() {
return getOutput(1);
}
/**
* Returns the DC motor's velocity in RPM.
*
* @return The DC motor's velocity in RPM.
*/
public double getAngularVelocityRPM() {
return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
}
/**
* Returns the DC motor's velocity.
*
* @return The DC motor's velocity
*/
public AngularVelocity getAngularVelocity() {
return RadiansPerSecond.of(getAngularVelocityRadPerSec());
}
/**
* Returns the DC motor's acceleration in Radians Per Second Squared.
*
* @return The DC motor's acceleration in Radians Per Second Squared.
*/
public double getAngularAccelerationRadPerSecSq() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(1, 0);
}
/**
* Returns the DC motor's acceleration.
*
* @return The DC motor's acceleration.
* @return The DC motor's acceleration in rad/s².
*/
public AngularAcceleration getAngularAcceleration() {
return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq());
public double getAngularAcceleration() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(1, 0);
}
/**
* Returns the DC motor's torque in Newton-Meters.
* Returns the DC motor's torque in.
*
* @return The DC motor's torque in Newton-Meters.
* @return The DC motor's torque in Newton-meters.
*/
public double getTorqueNewtonMeters() {
return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared;
public double getTorque() {
return getAngularAcceleration() * m_j;
}
/**
* Returns the DC motor's current draw.
*
* @return The DC motor's current draw.
* @return The DC motor's current draw in amps.
*/
public double getCurrentDrawAmps() {
public double getCurrentDraw() {
// I = V / R - omega / (Kv * R)
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// 2x faster than the output

92
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -42,7 +42,7 @@ public class DifferentialDrivetrainSim {
private final double m_originalGearing;
private final Matrix<N7, N1> m_measurementStdDevs;
private double m_currentGearing;
private final double m_wheelRadiusMeters;
private final double m_wheelRadius;
private Matrix<N2, N1> m_u;
private Matrix<N7, N1> m_x;
@@ -57,10 +57,10 @@ public class DifferentialDrivetrainSim {
* @param driveMotor A {@link DCMotor} representing the left side of the drivetrain.
* @param gearing The gearing ratio between motor and wheel, as output over input. This must be
* the same ratio as the ratio used to identify or create the drivetrainPlant.
* @param jKgMetersSquared The moment of inertia of the drivetrain about its center.
* @param massKg The mass of the drivebase.
* @param wheelRadiusMeters The radius of the wheels on the drivetrain.
* @param trackWidthMeters The robot's track width, or distance between left and right wheels.
* @param j The moment of inertia of the drivetrain about its center in kg-m².
* @param mass The mass of the drivebase in kg.
* @param wheelRadius The radius of the wheels on the drivetrain in meters.
* @param trackwidth The robot's trackwidth, or distance between left and right wheels, in meters.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
@@ -70,23 +70,18 @@ public class DifferentialDrivetrainSim {
public DifferentialDrivetrainSim(
DCMotor driveMotor,
double gearing,
double jKgMetersSquared,
double massKg,
double wheelRadiusMeters,
double trackWidthMeters,
double j,
double mass,
double wheelRadius,
double trackwidth,
Matrix<N7, N1> measurementStdDevs) {
this(
LinearSystemId.createDrivetrainVelocitySystem(
driveMotor,
massKg,
wheelRadiusMeters,
trackWidthMeters / 2.0,
jKgMetersSquared,
gearing),
driveMotor, mass, wheelRadius, trackwidth / 2.0, j, gearing),
driveMotor,
gearing,
trackWidthMeters,
wheelRadiusMeters,
trackwidth,
wheelRadius,
measurementStdDevs);
}
@@ -102,9 +97,9 @@ public class DifferentialDrivetrainSim {
* @param driveMotor A {@link DCMotor} representing the drivetrain.
* @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same
* ratio as the ratio used to identify or create the drivetrainPlant.
* @param trackWidthMeters The distance between the two sides of the drivetrain. Can be found with
* SysId.
* @param wheelRadiusMeters The radius of the wheels on the drivetrain, in meters.
* @param trackwidth The distance between the two sides of the drivetrain in meters. Can be found
* with SysId.
* @param wheelRadius The radius of the wheels on the drivetrain, in meters.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
@@ -115,15 +110,15 @@ public class DifferentialDrivetrainSim {
LinearSystem<N2, N2, N2> plant,
DCMotor driveMotor,
double gearing,
double trackWidthMeters,
double wheelRadiusMeters,
double trackwidth,
double wheelRadius,
Matrix<N7, N1> measurementStdDevs) {
this.m_plant = plant;
this.m_rb = trackWidthMeters / 2.0;
this.m_rb = trackwidth / 2.0;
this.m_motor = driveMotor;
this.m_originalGearing = gearing;
this.m_measurementStdDevs = measurementStdDevs;
m_wheelRadiusMeters = wheelRadiusMeters;
m_wheelRadius = wheelRadius;
m_currentGearing = m_originalGearing;
m_x = new Matrix<>(Nat.N7(), Nat.N1());
@@ -145,10 +140,10 @@ public class DifferentialDrivetrainSim {
/**
* Update the drivetrain states with the current time difference.
*
* @param dtSeconds the time difference
* @param dt the time difference in seconds
*/
public void update(double dtSeconds) {
m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dtSeconds);
public void update(double dt) {
m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dt);
m_y = m_x;
if (m_measurementStdDevs != null) {
m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
@@ -195,38 +190,38 @@ public class DifferentialDrivetrainSim {
}
/**
* Get the right encoder position in meters.
* Get the right encoder position.
*
* @return The encoder position.
* @return The encoder position in meters.
*/
public double getRightPositionMeters() {
public double getRightPosition() {
return getOutput(State.kRightPosition);
}
/**
* Get the right encoder velocity in meters per second.
* Get the right encoder velocity.
*
* @return The encoder velocity.
* @return The encoder velocity in meters per second.
*/
public double getRightVelocityMetersPerSecond() {
public double getRightVelocity() {
return getOutput(State.kRightVelocity);
}
/**
* Get the left encoder position in meters.
* Get the left encoder position.
*
* @return The encoder position.
* @return The encoder position in meters.
*/
public double getLeftPositionMeters() {
public double getLeftPosition() {
return getOutput(State.kLeftPosition);
}
/**
* Get the left encoder velocity in meters per second.
* Get the left encoder velocity.
*
* @return The encoder velocity.
* @return The encoder velocity in meters per second.
*/
public double getLeftVelocityMetersPerSecond() {
public double getLeftVelocity() {
return getOutput(State.kLeftVelocity);
}
@@ -235,9 +230,9 @@ public class DifferentialDrivetrainSim {
*
* @return the drivetrain's left side current draw, in amps
*/
public double getLeftCurrentDrawAmps() {
public double getLeftCurrentDraw() {
return m_motor.getCurrent(
getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(0, 0))
getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadius, m_u.get(0, 0))
* Math.signum(m_u.get(0, 0));
}
@@ -246,9 +241,9 @@ public class DifferentialDrivetrainSim {
*
* @return the drivetrain's right side current draw, in amps
*/
public double getRightCurrentDrawAmps() {
public double getRightCurrentDraw() {
return m_motor.getCurrent(
getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(1, 0))
getState(State.kRightVelocity) * m_currentGearing / m_wheelRadius, m_u.get(1, 0))
* Math.signum(m_u.get(1, 0));
}
@@ -257,8 +252,8 @@ public class DifferentialDrivetrainSim {
*
* @return the current draw, in amps
*/
public double getCurrentDrawAmps() {
return getLeftCurrentDrawAmps() + getRightCurrentDrawAmps();
public double getCurrentDraw() {
return getLeftCurrentDraw() + getRightCurrentDraw();
}
/**
@@ -472,8 +467,7 @@ public class DifferentialDrivetrainSim {
* @param motor The motors installed in the bot.
* @param gearing The gearing reduction used.
* @param wheelSize The wheel size.
* @param jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using
* SysId.
* @param j The moment of inertia of the drivebase in kg-m². This can be calculated using SysId.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
@@ -485,12 +479,12 @@ public class DifferentialDrivetrainSim {
KitbotMotor motor,
KitbotGearing gearing,
KitbotWheelSize wheelSize,
double jKgMetersSquared,
double j,
Matrix<N7, N1> measurementStdDevs) {
return new DifferentialDrivetrainSim(
motor.value,
gearing.value,
jKgMetersSquared,
j,
Units.lbsToKilograms(60),
wheelSize.value / 2.0,
Units.inchesToMeters(26),

113
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -36,10 +36,10 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
* double, double)}.
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param minHeight The min allowable height of the elevator in meters.
* @param maxHeight The max allowable height of the elevator in meters.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param startingHeight The starting height of the elevator in meters.
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for position.
*/
@@ -47,18 +47,18 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
public ElevatorSim(
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
double minHeight,
double maxHeight,
boolean simulateGravity,
double startingHeightMeters,
double startingHeight,
double... measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_minHeight = minHeight;
m_maxHeight = maxHeight;
m_simulateGravity = simulateGravity;
setState(startingHeightMeters, 0);
setState(startingHeight, 0);
}
/**
@@ -67,10 +67,10 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @param kV The velocity gain.
* @param kA The acceleration gain.
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param minHeight The min allowable height of the elevator in meters.
* @param maxHeight The max allowable height of the elevator in meters.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param startingHeight The starting height of the elevator in meters.
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for position.
*/
@@ -78,18 +78,18 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
double kV,
double kA,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
double minHeight,
double maxHeight,
boolean simulateGravity,
double startingHeightMeters,
double startingHeight,
double... measurementStdDevs) {
this(
LinearSystemId.identifyPositionSystem(kV, kA),
gearbox,
minHeightMeters,
maxHeightMeters,
minHeight,
maxHeight,
simulateGravity,
startingHeightMeters,
startingHeight,
measurementStdDevs);
}
@@ -98,32 +98,32 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
*
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
* @param carriageMassKg The mass of the elevator carriage.
* @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param carriageMass The mass of the elevator carriage in kg.
* @param drumRadius The radius of the drum that the elevator spool is wrapped around in meters.
* @param minHeight The min allowable height of the elevator in meters.
* @param maxHeight The max allowable height of the elevator in meters.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param startingHeight The starting height of the elevator in meters.
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for position.
*/
public ElevatorSim(
DCMotor gearbox,
double gearing,
double carriageMassKg,
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
double carriageMass,
double drumRadius,
double minHeight,
double maxHeight,
boolean simulateGravity,
double startingHeightMeters,
double startingHeight,
double... measurementStdDevs) {
this(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
LinearSystemId.createElevatorSystem(gearbox, carriageMass, drumRadius, gearing),
gearbox,
minHeightMeters,
maxHeightMeters,
minHeight,
maxHeight,
simulateGravity,
startingHeightMeters,
startingHeight,
measurementStdDevs);
}
@@ -131,33 +131,31 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* Sets the elevator's state. The new position will be limited between the minimum and maximum
* allowed heights.
*
* @param positionMeters The new position in meters.
* @param velocityMetersPerSecond New velocity in meters per second.
* @param position The new position in meters.
* @param velocity New velocity in meters per second.
*/
public final void setState(double positionMeters, double velocityMetersPerSecond) {
setState(
VecBuilder.fill(
MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond));
public final void setState(double position, double velocity) {
setState(VecBuilder.fill(MathUtil.clamp(position, m_minHeight, m_maxHeight), velocity));
}
/**
* Returns whether the elevator would hit the lower limit.
*
* @param elevatorHeightMeters The elevator height.
* @param elevatorHeight The elevator height in meters.
* @return Whether the elevator would hit the lower limit.
*/
public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
return elevatorHeightMeters <= this.m_minHeight;
public boolean wouldHitLowerLimit(double elevatorHeight) {
return elevatorHeight <= this.m_minHeight;
}
/**
* Returns whether the elevator would hit the upper limit.
*
* @param elevatorHeightMeters The elevator height.
* @param elevatorHeight The elevator height in meters.
* @return Whether the elevator would hit the upper limit.
*/
public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
return elevatorHeightMeters >= this.m_maxHeight;
public boolean wouldHitUpperLimit(double elevatorHeight) {
return elevatorHeight >= this.m_maxHeight;
}
/**
@@ -166,7 +164,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @return Whether the elevator has hit the lower limit.
*/
public boolean hasHitLowerLimit() {
return wouldHitLowerLimit(getPositionMeters());
return wouldHitLowerLimit(getPosition());
}
/**
@@ -175,43 +173,42 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @return Whether the elevator has hit the upper limit.
*/
public boolean hasHitUpperLimit() {
return wouldHitUpperLimit(getPositionMeters());
return wouldHitUpperLimit(getPosition());
}
/**
* Returns the position of the elevator.
*
* @return The position of the elevator.
* @return The position of the elevator in meters.
*/
public double getPositionMeters() {
public double getPosition() {
return getOutput(0);
}
/**
* Returns the velocity of the elevator.
*
* @return The velocity of the elevator.
* @return The velocity of the elevator in meters per second.
*/
public double getVelocityMetersPerSecond() {
public double getVelocity() {
return getOutput(1);
}
/**
* Returns the elevator current draw.
*
* @return The elevator current draw.
* @return The elevator current draw in amps.
*/
public double getCurrentDrawAmps() {
public double getCurrentDraw() {
// I = V / R - omega / (Kv * R)
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
// spinning 10x faster than the output
// v = r w, so w = v/r
double kA = 1 / m_plant.getB().get(1, 0);
double kV = -m_plant.getA().get(1, 1) * kA;
double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt;
double motorVelocity = m_x.get(1, 0) * kV * m_gearbox.Kv;
var appliedVoltage = m_u.get(0, 0);
return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
* Math.signum(appliedVoltage);
return m_gearbox.getCurrent(motorVelocity, appliedVoltage) * Math.signum(appliedVoltage);
}
/**
@@ -229,10 +226,10 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
*
* @param currentXhat The current state estimate.
* @param u The system inputs (voltage).
* @param dtSeconds The time difference between controller updates.
* @param dt The time difference between controller updates in seconds.
*/
@Override
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dt) {
// Calculate updated x-hat from Runge-Kutta.
var updatedXhat =
NumericalIntegration.rkdp(
@@ -245,7 +242,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
},
currentXhat,
u,
dtSeconds);
dt);
// We check for collisions after updating x-hat.
if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {

79
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
View File

@@ -4,17 +4,11 @@
package edu.wpi.first.wpilibj.simulation;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.AngularAcceleration;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.RobotController;
/** Represents a simulated flywheel mechanism. */
@@ -26,7 +20,7 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
private final double m_gearing;
// The moment of inertia for the flywheel mechanism.
private final double m_jKgMetersSquared;
private final double m_j;
/**
* Creates a simulated flywheel mechanism.
@@ -60,17 +54,17 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
//
// B = GKₜ/(RJ)
// J = GKₜ/(RB)
m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(0, 0) / plant.getB(0, 0);
m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(0, 0));
m_gearing = -gearbox.Kv * plant.getA(0, 0) / plant.getB(0, 0);
m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(0, 0));
}
/**
* Sets the flywheel's angular velocity.
*
* @param velocityRadPerSec The new velocity in radians per second.
* @param velocity The new velocity in radians per second.
*/
public void setAngularVelocity(double velocityRadPerSec) {
setState(VecBuilder.fill(velocityRadPerSec));
public void setAngularVelocity(double velocity) {
setState(VecBuilder.fill(velocity));
}
/**
@@ -83,12 +77,12 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
}
/**
* Returns the moment of inertia in kilograms meters squared.
* Returns the moment of inertia.
*
* @return The flywheel's moment of inertia.
* @return The flywheel's moment of inertia in kg-m².
*/
public double getJKgMetersSquared() {
return m_jKgMetersSquared;
public double getJ() {
return m_j;
}
/**
@@ -100,67 +94,40 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
return m_gearbox;
}
/**
* Returns the flywheel's velocity in Radians Per Second.
*
* @return The flywheel's velocity in Radians Per Second.
*/
public double getAngularVelocityRadPerSec() {
return getOutput(0);
}
/**
* Returns the flywheel's velocity in RPM.
*
* @return The flywheel's velocity in RPM.
*/
public double getAngularVelocityRPM() {
return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
}
/**
* Returns the flywheel's velocity.
*
* @return The flywheel's velocity
* @return The flywheel's velocity in rad/s.
*/
public AngularVelocity getAngularVelocity() {
return RadiansPerSecond.of(getAngularVelocityRadPerSec());
}
/**
* Returns the flywheel's acceleration in Radians Per Second Squared.
*
* @return The flywheel's acceleration in Radians Per Second Squared.
*/
public double getAngularAccelerationRadPerSecSq() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(0, 0);
public double getAngularVelocity() {
return getOutput(0);
}
/**
* Returns the flywheel's acceleration.
*
* @return The flywheel's acceleration.
* @return The flywheel's acceleration in rad/s².
*/
public AngularAcceleration getAngularAcceleration() {
return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq());
public double getAngularAcceleration() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(0, 0);
}
/**
* Returns the flywheel's torque in Newton-Meters.
* Returns the flywheel's torque.
*
* @return The flywheel's torque in Newton-Meters.
* @return The flywheel's torque in Newton-meters.
*/
public double getTorqueNewtonMeters() {
return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared;
public double getTorque() {
return getAngularAcceleration() * m_j;
}
/**
* Returns the flywheel's current draw.
*
* @return The flywheel's current draw.
* @return The flywheel's current draw in amps.
*/
public double getCurrentDrawAmps() {
public double getCurrentDraw() {
// I = V / R - omega / (Kv * R)
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// 2x faster than the flywheel

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
View File

@@ -72,11 +72,11 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
/**
* Updates the simulation.
*
* @param dtSeconds The time between updates.
* @param dt The time between updates in seconds.
*/
public void update(double dtSeconds) {
public void update(double dt) {
// Update X. By default, this is the linear system dynamics X = Ax + Bu
m_x = updateX(m_x, m_u, dtSeconds);
m_x = updateX(m_x, m_u, dt);
// y = cx + du
m_y = m_plant.calculateY(m_x, m_u);
@@ -171,12 +171,12 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
*
* @param currentXhat The current state estimate.
* @param u The system inputs (usually voltage).
* @param dtSeconds The time difference between controller updates.
* @param dt The time difference between controller updates in seconds.
* @return The new state.
*/
protected Matrix<States, N1> updateX(
Matrix<States, N1> currentXhat, Matrix<Inputs, N1> u, double dtSeconds) {
return m_plant.calculateX(currentXhat, u, dtSeconds);
Matrix<States, N1> currentXhat, Matrix<Inputs, N1> u, double dt) {
return m_plant.calculateX(currentXhat, u, dt);
}
/**

20
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.SharpIR;
/** Simulation class for Sharp IR sensors. */
@@ -28,24 +27,15 @@ public class SharpIRSim {
*/
public SharpIRSim(int channel) {
SimDeviceSim simDevice = new SimDeviceSim("SharpIR", channel);
m_simRange = simDevice.getDouble("Range (cm)");
m_simRange = simDevice.getDouble("Range (m)");
}
/**
* Set the range in inches returned by the distance sensor.
* Set the range in meters returned by the distance sensor.
*
* @param inches range in inches of the target returned by the sensor
* @param range range in meters of the target returned by the sensor
*/
public void setRangeInches(double inches) {
m_simRange.set(Units.inchesToMeters(inches) * 100.0);
}
/**
* Set the range in centimeters returned by the distance sensor.
*
* @param cm range in centimeters of the target returned by the sensor
*/
public void setRangeCm(double cm) {
m_simRange.set(cm);
public void setRange(double range) {
m_simRange.set(range);
}
}

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
View File

@@ -65,18 +65,18 @@ public final class SimHooks {
/**
* Advance the simulator time and wait for all notifiers to run.
*
* @param deltaSeconds the amount to advance (in seconds)
* @param delta the amount to advance in seconds
*/
public static void stepTiming(double deltaSeconds) {
SimulatorJNI.stepTiming((long) (deltaSeconds * 1e6));
public static void stepTiming(double delta) {
SimulatorJNI.stepTiming((long) (delta * 1e6));
}
/**
* Advance the simulator time and return immediately.
*
* @param deltaSeconds the amount to advance (in seconds)
* @param delta the amount to advance in seconds
*/
public static void stepTimingAsync(double deltaSeconds) {
SimulatorJNI.stepTimingAsync((long) (deltaSeconds * 1e6));
public static void stepTimingAsync(double delta) {
SimulatorJNI.stepTimingAsync((long) (delta * 1e6));
}
}

54
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -24,7 +24,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
private final double m_gearing;
// The length of the arm.
private final double m_armLenMeters;
private final double m_armLength;
// The minimum angle that the arm is capable of.
private final double m_minAngle;
@@ -43,7 +43,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
* double, double)}.
* @param gearbox The type of and number of motors in the arm gearbox.
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
* @param armLengthMeters The length of the arm.
* @param armLength The length of the arm in meters.
* @param minAngleRads The minimum angle that the arm is capable of.
* @param maxAngleRads The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
@@ -56,7 +56,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double gearing,
double armLengthMeters,
double armLength,
double minAngleRads,
double maxAngleRads,
boolean simulateGravity,
@@ -65,7 +65,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_armLenMeters = armLengthMeters;
m_armLength = armLength;
m_minAngle = minAngleRads;
m_maxAngle = maxAngleRads;
m_simulateGravity = simulateGravity;
@@ -78,8 +78,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
*
* @param gearbox The type of and number of motors in the arm gearbox.
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
* @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software.
* @param armLengthMeters The length of the arm.
* @param j The moment of inertia of the arm in kg-m²; can be calculated from CAD software.
* @param armLength The length of the arm in meters.
* @param minAngleRads The minimum angle that the arm is capable of.
* @param maxAngleRads The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
@@ -90,18 +90,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
public SingleJointedArmSim(
DCMotor gearbox,
double gearing,
double jKgMetersSquared,
double armLengthMeters,
double j,
double armLength,
double minAngleRads,
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
double... measurementStdDevs) {
this(
LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
LinearSystemId.createSingleJointedArmSystem(gearbox, j, gearing),
gearbox,
gearing,
armLengthMeters,
armLength,
minAngleRads,
maxAngleRads,
simulateGravity,
@@ -147,7 +147,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
* @return Whether the arm has hit the lower limit.
*/
public boolean hasHitLowerLimit() {
return wouldHitLowerLimit(getAngleRads());
return wouldHitLowerLimit(getAngle());
}
/**
@@ -156,33 +156,33 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
* @return Whether the arm has hit the upper limit.
*/
public boolean hasHitUpperLimit() {
return wouldHitUpperLimit(getAngleRads());
return wouldHitUpperLimit(getAngle());
}
/**
* Returns the current arm angle.
*
* @return The current arm angle.
* @return The current arm angle in radians.
*/
public double getAngleRads() {
public double getAngle() {
return getOutput(0);
}
/**
* Returns the current arm velocity.
*
* @return The current arm velocity.
* @return The current arm velocity in radians per second.
*/
public double getVelocityRadPerSec() {
public double getVelocity() {
return getOutput(1);
}
/**
* Returns the arm current draw.
*
* @return The arm current draw.
* @return The arm current draw in amps.
*/
public double getCurrentDrawAmps() {
public double getCurrentDraw() {
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
// spinning 10x faster than the output
var motorVelocity = m_x.get(1, 0) * m_gearing;
@@ -202,12 +202,12 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
/**
* Calculates a rough estimate of the moment of inertia of an arm given its length and mass.
*
* @param lengthMeters The length of the arm.
* @param massKg The mass of the arm.
* @return The calculated moment of inertia.
* @param length The length of the arm in m.
* @param mass The mass of the arm in kg.
* @return The calculated moment of inertia in kg-m².
*/
public static double estimateMOI(double lengthMeters, double massKg) {
return 1.0 / 3.0 * massKg * lengthMeters * lengthMeters;
public static double estimateMOI(double length, double mass) {
return 1.0 / 3.0 * mass * length * length;
}
/**
@@ -215,10 +215,10 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
*
* @param currentXhat The current state estimate.
* @param u The system inputs (voltage).
* @param dtSeconds The time difference between controller updates.
* @param dt The time difference between controller updates in seconds.
*/
@Override
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dt) {
// The torque on the arm is given by τ = F⋅r, where F is the force applied by
// gravity and r the distance from pivot to center of mass. Recall from
// dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is
@@ -248,14 +248,14 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
(Matrix<N2, N1> x, Matrix<N1, N1> _u) -> {
Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
if (m_simulateGravity) {
double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLenMeters;
double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLength;
xdot = xdot.plus(VecBuilder.fill(0, alphaGrav));
}
return xdot;
},
currentXhat,
u,
dtSeconds);
dt);
// We check for collision after updating xhat
if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
View File

@@ -58,12 +58,12 @@ public class Field2d implements NTSendable, AutoCloseable {
/**
* Set the robot pose from x, y, and rotation.
*
* @param xMeters X location, in meters
* @param yMeters Y location, in meters
* @param x X location, in meters
* @param y Y location, in meters
* @param rotation rotation
*/
public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
m_objects.get(0).setPose(xMeters, yMeters, rotation);
public synchronized void setRobotPose(double x, double y, Rotation2d rotation) {
m_objects.get(0).setPose(x, y, rotation);
}
/**

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
View File

@@ -43,12 +43,12 @@ public class FieldObject2d implements AutoCloseable {
/**
* Set the pose from x, y, and rotation.
*
* @param xMeters X location, in meters
* @param yMeters Y location, in meters
* @param x X location, in meters
* @param y Y location, in meters
* @param rotation rotation
*/
public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) {
setPose(new Pose2d(xMeters, yMeters, rotation));
public synchronized void setPose(double x, double y, Rotation2d rotation) {
setPose(new Pose2d(x, y, rotation));
}
/**
@@ -94,7 +94,7 @@ public class FieldObject2d implements AutoCloseable {
public synchronized void setTrajectory(Trajectory trajectory) {
m_poses.clear();
for (Trajectory.State state : trajectory.getStates()) {
m_poses.add(state.poseMeters);
m_poses.add(state.pose);
}
updateEntry();
}

10
wpilibj/src/test/java/edu/wpi/first/wpilibj/SharpIRTest.java
View File

@@ -15,13 +15,13 @@ class SharpIRTest {
try (SharpIR s = SharpIR.GP2Y0A02YK0F(1)) {
SharpIRSim sim = new SharpIRSim(s);
assertEquals(20, s.getRangeCM());
assertEquals(0.2, s.getRange());
sim.setRangeCm(30);
assertEquals(30, s.getRangeCM());
sim.setRange(0.3);
assertEquals(0.3, s.getRange());
sim.setRangeCm(300);
assertEquals(150, s.getRangeCM());
sim.setRange(3);
assertEquals(1.5, s.getRange());
}
}
}

16
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java
View File

@@ -36,23 +36,23 @@ class DCMotorSimTest {
// ------ SimulationPeriodic() happens after user code -------
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
sim.update(0.020);
encoderSim.setRate(sim.getAngularVelocityRadPerSec());
encoderSim.setRate(sim.getAngularVelocity());
}
assertEquals(gearbox.KvRadPerSecPerVolt * 12, encoder.getRate(), 0.1);
assertEquals(gearbox.Kv * 12, encoder.getRate(), 0.1);
for (int i = 0; i < 100; i++) {
motor.setVoltage(0);
// ------ SimulationPeriodic() happens after user code -------
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
sim.update(0.020);
encoderSim.setRate(sim.getAngularVelocityRadPerSec());
encoderSim.setRate(sim.getAngularVelocity());
}
assertEquals(0, encoder.getRate(), 0.1);
@@ -78,11 +78,11 @@ class DCMotorSimTest {
// ------ SimulationPeriodic() happens after user code -------
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDraw()));
sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage());
sim.update(0.020);
encoderSim.setDistance(sim.getAngularPositionRad());
encoderSim.setRate(sim.getAngularVelocityRadPerSec());
encoderSim.setDistance(sim.getAngularPosition());
encoderSim.setRate(sim.getAngularVelocity());
}
assertEquals(750, encoder.getDistance(), 1.0);

18
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
View File

@@ -42,7 +42,7 @@ class DifferentialDrivetrainSimTest {
plant,
motor,
1,
kinematics.trackWidthMeters,
kinematics.trackwidth,
Units.inchesToMeters(2),
VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
@@ -61,15 +61,13 @@ class DifferentialDrivetrainSimTest {
new TrajectoryConfig(1, 1)
.addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) {
for (double t = 0; t < traj.getTotalTime(); t += 0.020) {
var state = traj.sample(t);
var feedbackOut = feedback.calculate(sim.getPose(), state);
var wheelSpeeds = kinematics.toWheelSpeeds(feedbackOut);
var voltages =
feedforward.calculate(
VecBuilder.fill(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond));
var voltages = feedforward.calculate(VecBuilder.fill(wheelSpeeds.left, wheelSpeeds.right));
// Sim periodic code
sim.setInputs(voltages.get(0, 0), voltages.get(1, 0));
@@ -104,25 +102,25 @@ class DifferentialDrivetrainSimTest {
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
var sim =
new DifferentialDrivetrainSim(
plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2), null);
plant, motor, 1, kinematics.trackwidth, Units.inchesToMeters(2), null);
sim.setInputs(-12, -12);
for (int i = 0; i < 10; i++) {
sim.update(0.020);
}
assertTrue(sim.getCurrentDrawAmps() > 0);
assertTrue(sim.getCurrentDraw() > 0);
sim.setInputs(12, 12);
for (int i = 0; i < 20; i++) {
sim.update(0.020);
}
assertTrue(sim.getCurrentDrawAmps() > 0);
assertTrue(sim.getCurrentDraw() > 0);
sim.setInputs(-12, 12);
for (int i = 0; i < 30; i++) {
sim.update(0.020);
}
assertTrue(sim.getCurrentDrawAmps() > 0);
assertTrue(sim.getCurrentDraw() > 0);
}
@Test
@@ -138,7 +136,7 @@ class DifferentialDrivetrainSimTest {
plant,
motor,
5,
kinematics.trackWidthMeters,
kinematics.trackwidth,
Units.inchesToMeters(2),
VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));

8
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
View File

@@ -59,7 +59,7 @@ class ElevatorSimTest {
encoderSim.setDistance(y.get(0, 0));
}
assertEquals(controller.getSetpoint(), sim.getPositionMeters(), 0.2);
assertEquals(controller.getSetpoint(), sim.getPosition(), 0.2);
}
}
@@ -81,14 +81,14 @@ class ElevatorSimTest {
for (int i = 0; i < 100; i++) {
sim.setInput(VecBuilder.fill(0));
sim.update(0.020);
var height = sim.getPositionMeters();
var height = sim.getPosition();
assertTrue(height >= -0.05);
}
for (int i = 0; i < 100; i++) {
sim.setInput(VecBuilder.fill(12.0));
sim.update(0.020);
var height = sim.getPositionMeters();
var height = sim.getPosition();
assertTrue(height <= 1.05);
}
}
@@ -110,7 +110,7 @@ class ElevatorSimTest {
DCMotor.getVex775Pro(4), 4, Units.inchesToMeters(0.5), 100);
assertEquals(
system.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0),
sim.getPositionMeters(),
sim.getPosition(),
0.01);
}
}

2
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java
View File

@@ -34,6 +34,6 @@ class SingleJointedArmSimTest {
}
// the arm should swing down
assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1);
assertEquals(-Math.PI / 2.0, m_sim.getAngle(), 0.1);
}
}

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java
View File

@@ -65,7 +65,7 @@ public class Arm implements AutoCloseable {
new MechanismLigament2d(
"Arm",
30,
Units.radiansToDegrees(m_armSim.getAngleRads()),
Units.radiansToDegrees(m_armSim.getAngle()),
6,
new Color8Bit(Color.kYellow)));
@@ -92,13 +92,13 @@ public class Arm implements AutoCloseable {
m_armSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_armSim.getAngleRads());
m_encoderSim.setDistance(m_armSim.getAngle());
// SimBattery estimates loaded battery voltages
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDraw()));
// Update the Mechanism Arm angle based on the simulated arm angle
m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngle()));
}
/** Load setpoint and kP from preferences. */

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -19,7 +19,7 @@ public class Drivetrain {
public static final double kMaxSpeed = 3.0; // meters per second
public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
private static final double kTrackWidth = 0.381 * 2; // meters
private static final double kTrackwidth = 0.381 * 2; // meters
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
@@ -37,7 +37,7 @@ public class Drivetrain {
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(kTrackWidth);
new DifferentialDriveKinematics(kTrackwidth);
private final DifferentialDriveOdometry m_odometry;
@@ -79,13 +79,12 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftFeedforward = m_feedforward.calculate(speeds.left);
final double rightFeedforward = m_feedforward.calculate(speeds.right);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}

23
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -42,7 +42,7 @@ public class Drivetrain {
public static final double kMaxSpeed = 3.0; // meters per second
public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
private static final double kTrackWidth = 0.381 * 2; // meters
private static final double kTrackwidth = 0.381 * 2; // meters
private static final double kWheelRadius = 0.0508; // meters
private static final int kEncoderResolution = 4096;
@@ -60,7 +60,7 @@ public class Drivetrain {
private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(kTrackWidth);
new DifferentialDriveKinematics(kTrackwidth);
private final Pose3d m_objectInField;
@@ -96,7 +96,7 @@ public class Drivetrain {
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
private final DifferentialDrivetrainSim m_drivetrainSimulator =
new DifferentialDrivetrainSim(
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null);
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
/**
* Constructs a differential drive object. Sets the encoder distance per pulse and resets the
@@ -137,13 +137,12 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftFeedforward = m_feedforward.calculate(speeds.left);
final double rightFeedforward = m_feedforward.calculate(speeds.right);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}
@@ -251,10 +250,10 @@ public class Drivetrain {
m_rightLeader.get() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocity());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPosition());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocity());
// m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
}

10
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
View File

@@ -24,14 +24,14 @@ public final class Constants {
// These characterization values MUST be determined either experimentally or theoretically
// for *your* robot's drive.
// The SysId tool provides a convenient method for obtaining these values for your robot.
public static final double ksVolts = 1;
public static final double kvVoltSecondsPerMeter = 0.8;
public static final double kaVoltSecondsSquaredPerMeter = 0.15;
public static final double ks = 1; // V
public static final double kv = 0.8; // V/(m/s)
public static final double ka = 0.15; // V/(m/s²)
public static final double kp = 1;
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxSpeed = 3; // m/s
public static final double kMaxAcceleration = 3; // m/s²
}
public static final class OIConstants {

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
View File

@@ -33,10 +33,7 @@ public class DriveSubsystem extends SubsystemBase {
// The feedforward controller.
private final SimpleMotorFeedforward m_feedforward =
new SimpleMotorFeedforward(
DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter);
new SimpleMotorFeedforward(DriveConstants.ks, DriveConstants.kv, DriveConstants.ka);
// The robot's drive
private final DifferentialDrive m_drive =
@@ -46,8 +43,7 @@ public class DriveSubsystem extends SubsystemBase {
private final TrapezoidProfile m_profile =
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
DriveConstants.kMaxSpeedMetersPerSecond,
DriveConstants.kMaxAccelerationMetersPerSecondSquared));
DriveConstants.kMaxSpeed, DriveConstants.kMaxAcceleration));
// The timer
private final Timer m_timer = new Timer();

8
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Constants.java
View File

@@ -26,11 +26,11 @@ public class Constants {
public static final double kElevatorDrumRadius = Units.inchesToMeters(1.0);
public static final double kCarriageMass = Units.lbsToKilograms(12); // kg
public static final double kSetpointMeters = Units.inchesToMeters(42.875);
public static final double kLowerkSetpointMeters = Units.inchesToMeters(15);
public static final double kSetpoint = Units.inchesToMeters(42.875);
public static final double kLowerkSetpoint = Units.inchesToMeters(15);
// Encoder is reset to measure 0 at the bottom, so minimum height is 0.
public static final double kMinElevatorHeightMeters = 0.0;
public static final double kMaxElevatorHeightMeters = Units.inchesToMeters(50);
public static final double kMinElevatorHeight = 0.0; // m
public static final double kMaxElevatorHeight = Units.inchesToMeters(50);
// distance per pulse = (distance per revolution) / (pulses per revolution)
// = (Pi * D) / ppr

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/Robot.java
View File

@@ -38,10 +38,10 @@ public class Robot extends TimedRobot {
public void teleopPeriodic() {
if (m_joystick.getTrigger()) {
// Here, we set the constant setpoint of 10 meters.
m_elevator.reachGoal(Constants.kSetpointMeters);
m_elevator.reachGoal(Constants.kSetpoint);
} else {
// Otherwise, we update the setpoint to 1 meter.
m_elevator.reachGoal(Constants.kLowerkSetpointMeters);
m_elevator.reachGoal(Constants.kLowerkSetpoint);
}
}

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java
View File

@@ -55,8 +55,8 @@ public class Elevator implements AutoCloseable {
Constants.kElevatorGearing,
Constants.kCarriageMass,
Constants.kElevatorDrumRadius,
Constants.kMinElevatorHeightMeters,
Constants.kMaxElevatorHeightMeters,
Constants.kMinElevatorHeight,
Constants.kMaxElevatorHeight,
true,
0,
0.005,
@@ -70,8 +70,7 @@ public class Elevator implements AutoCloseable {
private final MechanismRoot2d m_mech2dRoot =
m_mech2d.getRoot("Elevator Root", Units.inchesToMeters(5), Units.inchesToMeters(0.5));
private final MechanismLigament2d m_elevatorMech2d =
m_mech2dRoot.append(
new MechanismLigament2d("Elevator", m_elevatorSim.getPositionMeters(), 90));
m_mech2dRoot.append(new MechanismLigament2d("Elevator", m_elevatorSim.getPosition(), 90));
/** Subsystem constructor. */
public Elevator() {
@@ -92,10 +91,10 @@ public class Elevator implements AutoCloseable {
m_elevatorSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
m_encoderSim.setDistance(m_elevatorSim.getPosition());
// SimBattery estimates loaded battery voltages
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDraw()));
}
/**

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java
View File

@@ -25,10 +25,10 @@ public class Constants {
public static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
public static final double kCarriageMass = 4.0; // kg
public static final double kSetpointMeters = 0.75;
public static final double kSetpoint = 0.75; // m
// Encoder is reset to measure 0 at the bottom, so minimum height is 0.
public static final double kMinElevatorHeightMeters = 0.0;
public static final double kMaxElevatorHeightMeters = 1.25;
public static final double kMinElevatorHeight = 0.0; // m
public static final double kMaxElevatorHeight = 1.25; // m
// distance per pulse = (distance per revolution) / (pulses per revolution)
// = (Pi * D) / ppr

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
View File

@@ -31,7 +31,7 @@ public class Robot extends TimedRobot {
public void teleopPeriodic() {
if (m_joystick.getTrigger()) {
// Here, we set the constant setpoint of 0.75 meters.
m_elevator.reachGoal(Constants.kSetpointMeters);
m_elevator.reachGoal(Constants.kSetpoint);
} else {
// Otherwise, we update the setpoint to 0.
m_elevator.reachGoal(0.0);

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java
View File

@@ -50,8 +50,8 @@ public class Elevator implements AutoCloseable {
Constants.kElevatorGearing,
Constants.kCarriageMass,
Constants.kElevatorDrumRadius,
Constants.kMinElevatorHeightMeters,
Constants.kMaxElevatorHeightMeters,
Constants.kMinElevatorHeight,
Constants.kMaxElevatorHeight,
true,
0,
0.01,
@@ -63,8 +63,7 @@ public class Elevator implements AutoCloseable {
private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
private final MechanismLigament2d m_elevatorMech2d =
m_mech2dRoot.append(
new MechanismLigament2d("Elevator", m_elevatorSim.getPositionMeters(), 90));
m_mech2dRoot.append(new MechanismLigament2d("Elevator", m_elevatorSim.getPosition(), 90));
/** Subsystem constructor. */
public Elevator() {
@@ -85,10 +84,10 @@ public class Elevator implements AutoCloseable {
m_elevatorSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
m_encoderSim.setDistance(m_elevatorSim.getPosition());
// SimBattery estimates loaded battery voltages
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDraw()));
}
/**

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
View File

@@ -14,8 +14,8 @@ import edu.wpi.first.wpilibj.event.EventLoop;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
public class Robot extends TimedRobot {
public static final int SHOT_VELOCITY = 200; // rpm
public static final int TOLERANCE = 8; // rpm
public static final double SHOT_VELOCITY = 200; // rpm
public static final double TOLERANCE = 8; // rpm
private final PWMSparkMax m_shooter = new PWMSparkMax(0);
private final Encoder m_shooterEncoder = new Encoder(0, 1);
@@ -34,7 +34,7 @@ public class Robot extends TimedRobot {
m_controller.setTolerance(TOLERANCE);
BooleanEvent isBallAtKicker =
new BooleanEvent(m_loop, () -> false); // m_kickerSensor.getRangeMM() < KICKER_THRESHOLD);
new BooleanEvent(m_loop, () -> false); // m_kickerSensor.getRange() < KICKER_THRESHOLD);
BooleanEvent intakeButton = new BooleanEvent(m_loop, () -> m_joystick.getRawButton(2));
// if the thumb button is held

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java
View File

@@ -96,6 +96,6 @@ public class Robot extends TimedRobot {
// simulation, and write the simulated velocities to our simulated encoder
m_flywheelSim.setInputVoltage(m_flywheelMotor.get() * RobotController.getInputVoltage());
m_flywheelSim.update(0.02);
m_encoderSim.setRate(m_flywheelSim.getAngularVelocityRadPerSec());
m_encoderSim.setRate(m_flywheelSim.getAngularVelocity());
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -95,23 +95,19 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeft);
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRight);
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeft);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRight);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(
m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond);
m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), speeds.frontLeft);
final double frontRightOutput =
m_frontRightPIDController.calculate(
m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond);
m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), speeds.frontRight);
final double backLeftOutput =
m_backLeftPIDController.calculate(
m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond);
m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), speeds.rearLeft);
final double backRightOutput =
m_backRightPIDController.calculate(
m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond);
m_backRightPIDController.calculate(m_backRightEncoder.getRate(), speeds.rearRight);
m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
@@ -128,13 +124,12 @@ public class Drivetrain {
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
}
setSpeeds(
m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed));
setSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(kMaxSpeed));
}
/** Updates the field relative position of the robot. */

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
View File

@@ -34,23 +34,23 @@ public final class Constants {
public static final boolean kFrontRightEncoderReversed = false;
public static final boolean kRearRightEncoderReversed = true;
public static final double kTrackWidth = 0.5;
public static final double kTrackwidth = 0.5;
// Distance between centers of right and left wheels on robot
public static final double kWheelBase = 0.7;
// Distance between centers of front and back wheels on robot
public static final MecanumDriveKinematics kDriveKinematics =
new MecanumDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterMeters = 0.15;
public static final double kWheelDiameter = 0.15; // m
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / kEncoderCPR;
(kWheelDiameter * Math.PI) / kEncoderCPR;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These characterization values MUST be determined either experimentally or theoretically
@@ -71,10 +71,10 @@ public final class Constants {
}
public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kMaxSpeed = 3; // m/s
public static final double kMaxAcceleration = 3; // m/s²
public static final double kMaxAngularSpeed = Math.PI; // rad/s
public static final double kMaxAngularAcceleration = Math.PI; // rad/s²
public static final double kPXController = 0.5;
public static final double kPYController = 0.5;
@@ -82,7 +82,6 @@ public final class Constants {
// Constraint for the motion profilied robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
new TrapezoidProfile.Constraints(kMaxAngularSpeed, kMaxAngularAcceleration);
}
}

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
View File

@@ -80,9 +80,7 @@ public class RobotContainer {
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config =
new TrajectoryConfig(
AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
new TrajectoryConfig(AutoConstants.kMaxSpeed, AutoConstants.kMaxAcceleration)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
@@ -111,7 +109,7 @@ public class RobotContainer {
AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints),
// Needed for normalizing wheel speeds
AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxSpeed,
// Velocity PID's
new PIDController(DriveConstants.kPFrontLeftVel, 0, 0),

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -94,7 +94,7 @@ public class DriveSubsystem extends SubsystemBase {
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
return m_odometry.getPose();
}
/**

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -107,23 +107,19 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeft);
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRight);
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeft);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRight);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(
m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond);
m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), speeds.frontLeft);
final double frontRightOutput =
m_frontRightPIDController.calculate(
m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond);
m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), speeds.frontRight);
final double backLeftOutput =
m_backLeftPIDController.calculate(
m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond);
m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), speeds.rearLeft);
final double backRightOutput =
m_backRightPIDController.calculate(
m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond);
m_backRightPIDController.calculate(m_backRightEncoder.getRate(), speeds.rearRight);
m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
@@ -140,14 +136,13 @@ public class Drivetrain {
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds =
chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
}
setSpeeds(
m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(periodSeconds)).desaturate(kMaxSpeed));
setSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(kMaxSpeed));
}
/** Updates the field relative position of the robot. */

14
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
View File

@@ -20,10 +20,10 @@ public class Robot extends TimedRobot {
static final int kJoystickChannel = 3;
// The elevator can move 1.5 meters from top to bottom
static final double kFullHeightMeters = 1.5;
static final double kFullHeight = 1.5;
// Bottom, middle, and top elevator setpoints
static final double[] kSetpointsMeters = {0.2, 0.8, 1.4};
// Bottom, middle, and top elevator setpoints in meters
static final double[] kSetpoints = {0.2, 0.8, 1.4};
// proportional, integral, and derivative speed constants
// DANGER: when tuning PID constants, high/inappropriate values for kP, kI,
@@ -35,7 +35,7 @@ public class Robot extends TimedRobot {
private final PIDController m_pidController = new PIDController(kP, kI, kD);
// Scaling is handled internally
private final AnalogPotentiometer m_potentiometer =
new AnalogPotentiometer(kPotChannel, kFullHeightMeters);
new AnalogPotentiometer(kPotChannel, kFullHeight);
private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(kMotorChannel);
private final Joystick m_joystick = new Joystick(kJoystickChannel);
@@ -45,7 +45,7 @@ public class Robot extends TimedRobot {
public void teleopInit() {
// Move to the bottom setpoint when teleop starts
m_index = 0;
m_pidController.setSetpoint(kSetpointsMeters[m_index]);
m_pidController.setSetpoint(kSetpoints[m_index]);
}
@Override
@@ -62,9 +62,9 @@ public class Robot extends TimedRobot {
// when the button is pressed once, the selected elevator setpoint is incremented
if (m_joystick.getTriggerPressed()) {
// index of the elevator setpoint wraps around.
m_index = (m_index + 1) % kSetpointsMeters.length;
m_index = (m_index + 1) % kSetpoints.length;
System.out.println("m_index = " + m_index);
m_pidController.setSetpoint(kSetpointsMeters[m_index]);
m_pidController.setSetpoint(kSetpoints[m_index]);
}
}

18
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/Constants.java
View File

@@ -27,10 +27,10 @@ public final class Constants {
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterMeters = Units.inchesToMeters(6);
public static final double kWheelDiameter = Units.inchesToMeters(6);
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / kEncoderCPR;
(kWheelDiameter * Math.PI) / kEncoderCPR;
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
// These values MUST be determined either experimentally or theoretically for *your* robot's
@@ -46,9 +46,9 @@ public final class Constants {
public static final double kMaxTurnRateDegPerS = 100;
public static final double kMaxTurnAccelerationDegPerSSquared = 300;
public static final double ksVolts = 1;
public static final double kvVoltSecondsPerDegree = 0.8;
public static final double kaVoltSecondsSquaredPerDegree = 0.15;
public static final double ks = 1; // V
public static final double kv = 0.8; // V/(deg/s)
public static final double ka = 0.15; // V/(deg/s²)
}
public static final class ShooterConstants {
@@ -70,9 +70,9 @@ public final class Constants {
// On a real robot the feedforward constants should be empirically determined; these are
// reasonable guesses.
public static final double kSVolts = 0.05;
public static final double kS = 0.05; // V
// Should have value 12V at free speed
public static final double kVVoltSecondsPerRotation = 12.0 / kShooterFreeRPS;
public static final double kV = 12.0 / kShooterFreeRPS; // V/(rot/s)
public static final double kFeederSpeed = 0.5;
}
@@ -88,8 +88,8 @@ public final class Constants {
}
public static final class AutoConstants {
public static final double kTimeoutSeconds = 3;
public static final double kDriveDistanceMeters = 2;
public static final double kTimeout = 3;
public static final double kDriveDistance = 2; // m
public static final double kDriveSpeed = 0.5;
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/RapidReactCommandBot.java
View File

@@ -87,7 +87,7 @@ public class RapidReactCommandBot {
public Command getAutonomousCommand() {
// Drive forward for 2 meters at half speed with a 3 second timeout
return m_drive
.driveDistanceCommand(AutoConstants.kDriveDistanceMeters, AutoConstants.kDriveSpeed)
.withTimeout(AutoConstants.kTimeoutSeconds);
.driveDistanceCommand(AutoConstants.kDriveDistance, AutoConstants.kDriveSpeed)
.withTimeout(AutoConstants.kTimeout);
}
}

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
View File

@@ -59,10 +59,7 @@ public class Drive extends SubsystemBase {
DriveConstants.kMaxTurnRateDegPerS,
DriveConstants.kMaxTurnAccelerationDegPerSSquared));
private final SimpleMotorFeedforward m_feedforward =
new SimpleMotorFeedforward(
DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerDegree,
DriveConstants.kaVoltSecondsSquaredPerDegree);
new SimpleMotorFeedforward(DriveConstants.ks, DriveConstants.kv, DriveConstants.ka);
/** Creates a new Drive subsystem. */
public Drive() {
@@ -105,10 +102,10 @@ public class Drive extends SubsystemBase {
/**
* Returns a command that drives the robot forward a specified distance at a specified speed.
*
* @param distanceMeters The distance to drive forward in meters
* @param distance The distance to drive forward in meters
* @param speed The fraction of max speed at which to drive
*/
public Command driveDistanceCommand(double distanceMeters, double speed) {
public Command driveDistanceCommand(double distance, double speed) {
return runOnce(
() -> {
// Reset encoders at the start of the command
@@ -119,9 +116,7 @@ public class Drive extends SubsystemBase {
.andThen(run(() -> m_drive.arcadeDrive(speed, 0)))
// End command when we've traveled the specified distance
.until(
() ->
Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance())
>= distanceMeters)
() -> Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance()) >= distance)
// Stop the drive when the command ends
.finallyDo(interrupted -> m_drive.stopMotor());
}

3
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
View File

@@ -26,8 +26,7 @@ public class Shooter extends SubsystemBase {
ShooterConstants.kEncoderPorts[1],
ShooterConstants.kEncoderReversed);
private final SimpleMotorFeedforward m_shooterFeedforward =
new SimpleMotorFeedforward(
ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
new SimpleMotorFeedforward(ShooterConstants.kS, ShooterConstants.kV);
private final PIDController m_shooterFeedback = new PIDController(ShooterConstants.kP, 0.0, 0.0);
/** The shooter subsystem for the robot. */

28
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
View File

@@ -30,7 +30,7 @@ public class Drivetrain {
// 1/2 rotation per second.
public static final double kMaxAngularSpeed = Math.PI;
private static final double kTrackWidth = 0.381 * 2;
private static final double kTrackwidth = 0.381 * 2;
private static final double kWheelRadius = 0.0508;
private static final int kEncoderResolution = -4096;
@@ -48,7 +48,7 @@ public class Drivetrain {
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(kTrackWidth);
new DifferentialDriveKinematics(kTrackwidth);
private final DifferentialDriveOdometry m_odometry =
new DifferentialDriveOdometry(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
@@ -65,7 +65,7 @@ public class Drivetrain {
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
private final DifferentialDrivetrainSim m_drivetrainSimulator =
new DifferentialDrivetrainSim(
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null);
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
/** Subsystem constructor. */
public Drivetrain() {
@@ -92,12 +92,10 @@ public class Drivetrain {
/** Sets speeds to the drivetrain motors. */
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
final double leftFeedforward = m_feedforward.calculate(speeds.left);
final double rightFeedforward = m_feedforward.calculate(speeds.right);
double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left);
double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
@@ -128,7 +126,7 @@ public class Drivetrain {
/** Check the current robot pose. */
public Pose2d getPose() {
return m_odometry.getPoseMeters();
return m_odometry.getPose();
}
/** Update our simulation. This should be run every robot loop in simulation. */
@@ -142,16 +140,16 @@ public class Drivetrain {
m_rightLeader.get() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocity());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPosition());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocity());
// m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
}
/** Update odometry - this should be run every robot loop. */
public void periodic() {
updateOdometry();
m_fieldSim.setRobotPose(m_odometry.getPoseMeters());
m_fieldSim.setRobotPose(m_odometry.getPose());
}
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java
View File

@@ -56,7 +56,7 @@ public class Robot extends TimedRobot {
double elapsed = m_timer.get();
Trajectory.State reference = m_trajectory.sample(elapsed);
ChassisSpeeds speeds = m_feedback.calculate(m_drive.getPose(), reference);
m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
m_drive.drive(speeds.vx, speeds.omega);
}
@Override

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
View File

@@ -56,12 +56,12 @@ public class Drivetrain {
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_gyro.getRotation2d());
}
chassisSpeeds = chassisSpeeds.discretize(periodSeconds);
chassisSpeeds = chassisSpeeds.discretize(period);
var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
View File

@@ -121,9 +121,9 @@ public class SwerveModule {
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond);
final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =

37
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
View File

@@ -52,16 +52,16 @@ public final class Constants {
// If you call DriveSubsystem.drive() with a different period make sure to update this.
public static final double kDrivePeriod = TimedRobot.kDefaultPeriod;
public static final double kTrackWidth = 0.5;
public static final double kTrackwidth = 0.5;
// Distance between centers of right and left wheels on robot
public static final double kWheelBase = 0.7;
// Distance between front and back wheels on robot
public static final SwerveDriveKinematics kDriveKinematics =
new SwerveDriveKinematics(
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
public static final boolean kGyroReversed = false;
@@ -69,22 +69,22 @@ public final class Constants {
// These characterization values MUST be determined either experimentally or theoretically
// for *your* robot's drive.
// The SysId tool provides a convenient method for obtaining these values for your robot.
public static final double ksVolts = 1;
public static final double kvVoltSecondsPerMeter = 0.8;
public static final double kaVoltSecondsSquaredPerMeter = 0.15;
public static final double ks = 1; // V
public static final double kv = 0.8; // V/(m/s)
public static final double ka = 0.15; // V/(m/s²)
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxSpeed = 3; // m/s
}
public static final class ModuleConstants {
public static final double kMaxModuleAngularSpeedRadiansPerSecond = 2 * Math.PI;
public static final double kMaxModuleAngularAccelerationRadiansPerSecondSquared = 2 * Math.PI;
public static final double kMaxModuleAngularSpeed = 2 * Math.PI; // rad/s
public static final double kMaxModuleAngularAcceleration = 2 * Math.PI; // rad/s²
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterMeters = 0.15;
public static final double kWheelDiameter = 0.15; // m
public static final double kDriveEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / kEncoderCPR;
(kWheelDiameter * Math.PI) / kEncoderCPR;
public static final double kTurningEncoderDistancePerPulse =
// Assumes the encoders are on a 1:1 reduction with the module shaft.
@@ -100,10 +100,10 @@ public final class Constants {
}
public static final class AutoConstants {
public static final double kMaxSpeedMetersPerSecond = 3;
public static final double kMaxAccelerationMetersPerSecondSquared = 3;
public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
public static final double kMaxSpeed = 3; // m/s
public static final double kMaxAcceleration = 3; // m/s²
public static final double kMaxAngularSpeed = Math.PI; // rad/s
public static final double kMaxAngularAcceleration = Math.PI; // rad/s²
public static final double kPXController = 1;
public static final double kPYController = 1;
@@ -111,7 +111,6 @@ public final class Constants {
// Constraint for the motion profiled robot angle controller
public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
new TrapezoidProfile.Constraints(
kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
new TrapezoidProfile.Constraints(kMaxAngularSpeed, kMaxAngularAcceleration);
}
}

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
View File

@@ -54,10 +54,9 @@ public class RobotContainer {
// Multiply by max speed to map the joystick unitless inputs to actual units.
// This will map the [-1, 1] to [max speed backwards, max speed forwards],
// converting them to actual units.
m_driverController.getLeftY() * DriveConstants.kMaxSpeedMetersPerSecond,
m_driverController.getLeftX() * DriveConstants.kMaxSpeedMetersPerSecond,
m_driverController.getRightX()
* ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
m_driverController.getLeftY() * DriveConstants.kMaxSpeed,
m_driverController.getLeftX() * DriveConstants.kMaxSpeed,
m_driverController.getRightX() * ModuleConstants.kMaxModuleAngularSpeed,
false),
m_robotDrive));
}
@@ -78,9 +77,7 @@ public class RobotContainer {
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config =
new TrajectoryConfig(
AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared)
new TrajectoryConfig(AutoConstants.kMaxSpeed, AutoConstants.kMaxAcceleration)
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -89,7 +89,7 @@ public class DriveSubsystem extends SubsystemBase {
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
return m_odometry.getPose();
}
/**
@@ -125,7 +125,7 @@ public class DriveSubsystem extends SubsystemBase {
chassisSpeeds = chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
var states = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeedMetersPerSecond);
SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeed);
m_frontLeft.setDesiredState(states[0]);
m_frontRight.setDesiredState(states[1]);
@@ -139,8 +139,7 @@ public class DriveSubsystem extends SubsystemBase {
* @param desiredStates The desired SwerveModule states.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(
desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kMaxSpeed);
m_frontLeft.setDesiredState(desiredStates[0]);
m_frontRight.setDesiredState(desiredStates[1]);
m_rearLeft.setDesiredState(desiredStates[2]);

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
View File

@@ -31,8 +31,8 @@ public class SwerveModule {
0,
0,
new TrapezoidProfile.Constraints(
ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared));
ModuleConstants.kMaxModuleAngularSpeed,
ModuleConstants.kMaxModuleAngularAcceleration));
/**
* Constructs a SwerveModule.
@@ -117,7 +117,7 @@ public class SwerveModule {
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -65,14 +65,14 @@ public class Drivetrain {
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
if (fieldRelative) {
chassisSpeeds =
chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
}
chassisSpeeds = chassisSpeeds.discretize(periodSeconds);
chassisSpeeds = chassisSpeeds.discretize(period);
var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -120,9 +120,9 @@ public class SwerveModule {
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speedMetersPerSecond);
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speedMetersPerSecond);
final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =

17
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/Constants.java
View File

@@ -27,10 +27,10 @@ public final class Constants {
public static final boolean kRightEncoderReversed = true;
public static final int kEncoderCPR = 1024;
public static final double kWheelDiameterMeters = Units.inchesToMeters(6);
public static final double kWheelDiameter = Units.inchesToMeters(6);
public static final double kEncoderDistancePerPulse =
// Assumes the encoders are directly mounted on the wheel shafts
(kWheelDiameterMeters * Math.PI) / kEncoderCPR;
(kWheelDiameter * Math.PI) / kEncoderCPR;
}
public static final class ShooterConstants {
@@ -53,11 +53,10 @@ public final class Constants {
// On a real robot the feedforward constants should be empirically determined; these are
// reasonable guesses.
public static final double kSVolts = 0.05;
public static final double kVVoltSecondsPerRotation =
// Should have value 12V at free speed...
12.0 / kShooterFreeRPS;
public static final double kAVoltSecondsSquaredPerRotation = 0;
public static final double kS = 0.05; // V
// Should have value 12V at free speed
public static final double kV = 12.0 / kShooterFreeRPS; // V/(rot/s)
public static final double kA = 0; // V/(rot/s²)
public static final double kFeederSpeed = 0.5;
}
@@ -73,8 +72,8 @@ public final class Constants {
}
public static final class AutoConstants {
public static final double kTimeoutSeconds = 3;
public static final double kDriveDistanceMeters = 2;
public static final double kTimeout = 3;
public static final double kDriveDistance = 2; // m
public static final double kDriveSpeed = 0.5;
}

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysidroutine/subsystems/Shooter.java
View File

@@ -74,10 +74,7 @@ public class Shooter extends SubsystemBase {
// Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to
// those calculated by SysId
private final SimpleMotorFeedforward m_shooterFeedforward =
new SimpleMotorFeedforward(
ShooterConstants.kSVolts,
ShooterConstants.kVVoltSecondsPerRotation,
ShooterConstants.kAVoltSecondsSquaredPerRotation);
new SimpleMotorFeedforward(ShooterConstants.kS, ShooterConstants.kV, ShooterConstants.kA);
/** Creates a new Shooter subsystem. */
public Shooter() {

8
wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java
View File

@@ -88,12 +88,12 @@ class ElevatorSimulationTest {
// advance 75 timesteps
SimHooks.stepTiming(1.5);
assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05);
// advance 25 timesteps to see setpoint is held.
SimHooks.stepTiming(0.5);
assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05);
}
{
@@ -115,12 +115,12 @@ class ElevatorSimulationTest {
// advance 75 timesteps
SimHooks.stepTiming(1.5);
assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05);
// advance 25 timesteps to see setpoint is held.
SimHooks.stepTiming(0.5);
assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
assertEquals(Constants.kSetpoint, m_encoderSim.getDistance(), 0.05);
}
{

12
wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java
View File

@@ -53,7 +53,7 @@ class PotentiometerPIDTest {
kCarriageMassKg,
kElevatorDrumRadius,
0.0,
Robot.kFullHeightMeters,
Robot.kFullHeight,
true,
0);
m_analogSim = new AnalogInputSim(Robot.kPotChannel);
@@ -74,7 +74,7 @@ class PotentiometerPIDTest {
*/
m_analogSim.setVoltage(
RobotController.getVoltage5V()
* (m_elevatorSim.getPositionMeters() / Robot.kFullHeightMeters));
* (m_elevatorSim.getPosition() / Robot.kFullHeight));
});
m_thread.start();
@@ -113,7 +113,7 @@ class PotentiometerPIDTest {
// advance 50 timesteps
SimHooks.stepTiming(1);
assertEquals(Robot.kSetpointsMeters[0], m_elevatorSim.getPositionMeters(), 0.1);
assertEquals(Robot.kSetpoints[0], m_elevatorSim.getPosition(), 0.1);
}
// second setpoint
@@ -125,7 +125,7 @@ class PotentiometerPIDTest {
// advance 50 timesteps
SimHooks.stepTiming(1);
assertEquals(Robot.kSetpointsMeters[1], m_elevatorSim.getPositionMeters(), 0.1);
assertEquals(Robot.kSetpoints[1], m_elevatorSim.getPosition(), 0.1);
}
// we need to unpress the button
@@ -146,7 +146,7 @@ class PotentiometerPIDTest {
// advance 50 timesteps
SimHooks.stepTiming(1);
assertEquals(Robot.kSetpointsMeters[2], m_elevatorSim.getPositionMeters(), 0.1);
assertEquals(Robot.kSetpoints[2], m_elevatorSim.getPosition(), 0.1);
}
// we need to unpress the button
@@ -167,7 +167,7 @@ class PotentiometerPIDTest {
// advance 60 timesteps
SimHooks.stepTiming(1.2);
assertEquals(Robot.kSetpointsMeters[0], m_elevatorSim.getPositionMeters(), 0.1);
assertEquals(Robot.kSetpoints[0], m_elevatorSim.getPosition(), 0.1);
}
}
}

177
wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java generated
View File

@@ -19,80 +19,80 @@ import us.hebi.quickbuf.RepeatedByte;
import us.hebi.quickbuf.RepeatedMessage;
public final class Kinematics {
private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3021,
private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3020,
"ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" +
"aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" +
"BW9tZWdhIkYKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh8KC3RyYWNrX3dpZHRo" +
"GAEgASgBUgp0cmFja1dpZHRoIlAKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVXaGVlbFNwZWVkcxIS" +
"CgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCJTCidQcm90b2J1ZkRpZmZlcmVu" +
"dGlhbERyaXZlV2hlZWxQb3NpdGlvbnMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" +
"cmlnaHQipAIKHlByb3RvYnVmTWVjYW51bURyaXZlS2luZW1hdGljcxI/Cgpmcm9udF9sZWZ0GAEgASgL" +
"MiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJZnJvbnRMZWZ0EkEKC2Zyb250X3JpZ2h0" +
"GAIgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFy" +
"X2xlZnQYAyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/Cgpy" +
"ZWFyX3JpZ2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0" +
"IqABCiJQcm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFS" +
"CWZyb250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyAB" +
"KAFSCHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNh" +
"bnVtRHJpdmVXaGVlbFNwZWVkcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRf" +
"cmlnaHQYAiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFy" +
"X3JpZ2h0GAQgASgBUglyZWFyUmlnaHQiWwodUHJvdG9idWZTd2VydmVEcml2ZUtpbmVtYXRpY3MSOgoH" +
"bW9kdWxlcxgBIAMoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSB21vZHVsZXMibwoc" +
"UHJvdG9idWZTd2VydmVNb2R1bGVQb3NpdGlvbhIaCghkaXN0YW5jZRgBIAEoAVIIZGlzdGFuY2USMwoF" +
"YW5nbGUYAiABKAsyHS53cGkucHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZSJmChlQcm90b2J1",
"ZlN3ZXJ2ZU1vZHVsZVN0YXRlEhQKBXNwZWVkGAEgASgBUgVzcGVlZBIzCgVhbmdsZRgCIAEoCzIdLndw" +
"aS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90" +
"b0qZDQoGEgQAAD0BCggKAQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAES" +
"AwYAMQoKCgIEABIECAAMAQoKCgMEAAESAwgIHQoLCgQEAAIAEgMJAhAKDAoFBAACAAUSAwkCCAoMCgUE" +
"AAIAARIDCQkLCgwKBQQAAgADEgMJDg8KCwoEBAACARIDCgIQCgwKBQQAAgEFEgMKAggKDAoFBAACAQES" +
"AwoJCwoMCgUEAAIBAxIDCg4PCgsKBAQAAgISAwsCEwoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCQ4K" +
"DAoFBAACAgMSAwsREgoKCgIEARIEDgAQAQoKCgMEAQESAw4IKwoLCgQEAQIAEgMPAhkKDAoFBAECAAUS" +
"Aw8CCAoMCgUEAQIAARIDDwkUCgwKBQQBAgADEgMPFxgKCgoCBAISBBIAFQEKCgoDBAIBEgMSCCwKCwoE" +
"BAICABIDEwISCgwKBQQCAgAFEgMTAggKDAoFBAICAAESAxMJDQoMCgUEAgIAAxIDExARCgsKBAQCAgES" +
"AxQCEwoMCgUEAgIBBRIDFAIICgwKBQQCAgEBEgMUCQ4KDAoFBAICAQMSAxQREgoKCgIEAxIEFwAaAQoK" +
"CgMEAwESAxcILwoLCgQEAwIAEgMYAhIKDAoFBAMCAAUSAxgCCAoMCgUEAwIAARIDGAkNCgwKBQQDAgAD" +
"EgMYEBEKCwoEBAMCARIDGQITCgwKBQQDAgEFEgMZAggKDAoFBAMCAQESAxkJDgoMCgUEAwIBAxIDGRES" +
"CgoKAgQEEgQcACEBCgoKAwQEARIDHAgmCgsKBAQEAgASAx0CJwoMCgUEBAIABhIDHQIXCgwKBQQEAgAB" +
"EgMdGCIKDAoFBAQCAAMSAx0lJgoLCgQEBAIBEgMeAigKDAoFBAQCAQYSAx4CFwoMCgUEBAIBARIDHhgj" +
"CgwKBQQEAgEDEgMeJicKCwoEBAQCAhIDHwImCgwKBQQEAgIGEgMfAhcKDAoFBAQCAgESAx8YIQoMCgUE" +
"BAICAxIDHyQlCgsKBAQEAgMSAyACJwoMCgUEBAIDBhIDIAIXCgwKBQQEAgMBEgMgGCIKDAoFBAQCAwMS" +
"AyAlJgoKCgIEBRIEIwAoAQoKCgMEBQESAyMIKgoLCgQEBQIAEgMkAhgKDAoFBAUCAAUSAyQCCAoMCgUE" +
"BQIAARIDJAkTCgwKBQQFAgADEgMkFhcKCwoEBAUCARIDJQIZCgwKBQQFAgEFEgMlAggKDAoFBAUCAQES" +
"AyUJFAoMCgUEBQIBAxIDJRcYCgsKBAQFAgISAyYCFwoMCgUEBQICBRIDJgIICgwKBQQFAgIBEgMmCRIK" +
"DAoFBAUCAgMSAyYVFgoLCgQEBQIDEgMnAhgKDAoFBAUCAwUSAycCCAoMCgUEBQIDARIDJwkTCgwKBQQF",
"AgMDEgMnFhcKCgoCBAYSBCoALwEKCgoDBAYBEgMqCCcKCwoEBAYCABIDKwIYCgwKBQQGAgAFEgMrAggK" +
"DAoFBAYCAAESAysJEwoMCgUEBgIAAxIDKxYXCgsKBAQGAgESAywCGQoMCgUEBgIBBRIDLAIICgwKBQQG" +
"AgEBEgMsCRQKDAoFBAYCAQMSAywXGAoLCgQEBgICEgMtAhcKDAoFBAYCAgUSAy0CCAoMCgUEBgICARID" +
"LQkSCgwKBQQGAgIDEgMtFRYKCwoEBAYCAxIDLgIYCgwKBQQGAgMFEgMuAggKDAoFBAYCAwESAy4JEwoM" +
"CgUEBgIDAxIDLhYXCgoKAgQHEgQxADMBCgoKAwQHARIDMQglCgsKBAQHAgASAzICLQoMCgUEBwIABBID" +
"MgIKCgwKBQQHAgAGEgMyCyAKDAoFBAcCAAESAzIhKAoMCgUEBwIAAxIDMissCgoKAgQIEgQ1ADgBCgoK" +
"AwQIARIDNQgkCgsKBAQIAgASAzYCFgoMCgUECAIABRIDNgIICgwKBQQIAgABEgM2CREKDAoFBAgCAAMS" +
"AzYUFQoLCgQECAIBEgM3Ah8KDAoFBAgCAQYSAzcCFAoMCgUECAIBARIDNxUaCgwKBQQIAgEDEgM3HR4K" +
"CgoCBAkSBDoAPQEKCgoDBAkBEgM6CCEKCwoEBAkCABIDOwITCgwKBQQJAgAFEgM7AggKDAoFBAkCAAES" +
"AzsJDgoMCgUECQIAAxIDOxESCgsKBAQJAgESAzwCHwoMCgUECQIBBhIDPAIUCgwKBQQJAgEBEgM8FRoK" +
"DAoFBAkCAQMSAzwdHmIGcHJvdG8z");
"BW9tZWdhIkUKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh4KCnRyYWNrd2lkdGgY" +
"ASABKAFSCnRyYWNrd2lkdGgiUAokUHJvdG9idWZEaWZmZXJlbnRpYWxEcml2ZVdoZWVsU3BlZWRzEhIK" +
"BGxlZnQYASABKAFSBGxlZnQSFAoFcmlnaHQYAiABKAFSBXJpZ2h0IlMKJ1Byb3RvYnVmRGlmZmVyZW50" +
"aWFsRHJpdmVXaGVlbFBvc2l0aW9ucxISCgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVy" +
"aWdodCKkAgoeUHJvdG9idWZNZWNhbnVtRHJpdmVLaW5lbWF0aWNzEj8KCmZyb250X2xlZnQYASABKAsy" +
"IC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglmcm9udExlZnQSQQoLZnJvbnRfcmlnaHQY" +
"AiABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUgpmcm9udFJpZ2h0Ej0KCXJlYXJf" +
"bGVmdBgDIAEoCzIgLndwaS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSCHJlYXJMZWZ0Ej8KCnJl" +
"YXJfcmlnaHQYBCABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglyZWFyUmlnaHQi" +
"oAEKIlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxQb3NpdGlvbnMSHQoKZnJvbnRfbGVmdBgBIAEoAVIJ" +
"ZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVmdBgDIAEo" +
"AVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0Ip0BCh9Qcm90b2J1Zk1lY2Fu" +
"dW1Ecml2ZVdoZWVsU3BlZWRzEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9udF9y" +
"aWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJlYXJf" +
"cmlnaHQYBCABKAFSCXJlYXJSaWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURyaXZlS2luZW1hdGljcxI6Cgdt" +
"b2R1bGVzGAEgAygLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIHbW9kdWxlcyJvChxQ" +
"cm90b2J1ZlN3ZXJ2ZU1vZHVsZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEgASgBUghkaXN0YW5jZRIzCgVh" +
"bmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRSBWFuZ2xlImYKGVByb3RvYnVm",
"U3dlcnZlTW9kdWxlU3RhdGUSFAoFc3BlZWQYASABKAFSBXNwZWVkEjMKBWFuZ2xlGAIgASgLMh0ud3Bp" +
"LnByb3RvLlByb3RvYnVmUm90YXRpb24yZFIFYW5nbGVCGgoYZWR1LndwaS5maXJzdC5tYXRoLnByb3Rv" +
"SpkNCgYSBAAAPQEKCAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAaCggKAQgSAwYAMQoJCgIIARID" +
"BgAxCgoKAgQAEgQIAAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoMCgUEAAIABRIDCQIICgwKBQQA" +
"AgABEgMJCQsKDAoFBAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAACAQUSAwoCCAoMCgUEAAIBARID" +
"CgkLCgwKBQQAAgEDEgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgMLAggKDAoFBAACAgESAwsJDgoM" +
"CgUEAAICAxIDCxESCgoKAgQBEgQOABABCgoKAwQBARIDDggrCgsKBAQBAgASAw8CGAoMCgUEAQIABRID" +
"DwIICgwKBQQBAgABEgMPCRMKDAoFBAECAAMSAw8WFwoKCgIEAhIEEgAVAQoKCgMEAgESAxIILAoLCgQE" +
"AgIAEgMTAhIKDAoFBAICAAUSAxMCCAoMCgUEAgIAARIDEwkNCgwKBQQCAgADEgMTEBEKCwoEBAICARID" +
"FAITCgwKBQQCAgEFEgMUAggKDAoFBAICAQESAxQJDgoMCgUEAgIBAxIDFBESCgoKAgQDEgQXABoBCgoK" +
"AwQDARIDFwgvCgsKBAQDAgASAxgCEgoMCgUEAwIABRIDGAIICgwKBQQDAgABEgMYCQ0KDAoFBAMCAAMS" +
"AxgQEQoLCgQEAwIBEgMZAhMKDAoFBAMCAQUSAxkCCAoMCgUEAwIBARIDGQkOCgwKBQQDAgEDEgMZERIK" +
"CgoCBAQSBBwAIQEKCgoDBAQBEgMcCCYKCwoEBAQCABIDHQInCgwKBQQEAgAGEgMdAhcKDAoFBAQCAAES" +
"Ax0YIgoMCgUEBAIAAxIDHSUmCgsKBAQEAgESAx4CKAoMCgUEBAIBBhIDHgIXCgwKBQQEAgEBEgMeGCMK" +
"DAoFBAQCAQMSAx4mJwoLCgQEBAICEgMfAiYKDAoFBAQCAgYSAx8CFwoMCgUEBAICARIDHxghCgwKBQQE" +
"AgIDEgMfJCUKCwoEBAQCAxIDIAInCgwKBQQEAgMGEgMgAhcKDAoFBAQCAwESAyAYIgoMCgUEBAIDAxID" +
"ICUmCgoKAgQFEgQjACgBCgoKAwQFARIDIwgqCgsKBAQFAgASAyQCGAoMCgUEBQIABRIDJAIICgwKBQQF" +
"AgABEgMkCRMKDAoFBAUCAAMSAyQWFwoLCgQEBQIBEgMlAhkKDAoFBAUCAQUSAyUCCAoMCgUEBQIBARID" +
"JQkUCgwKBQQFAgEDEgMlFxgKCwoEBAUCAhIDJgIXCgwKBQQFAgIFEgMmAggKDAoFBAUCAgESAyYJEgoM" +
"CgUEBQICAxIDJhUWCgsKBAQFAgMSAycCGAoMCgUEBQIDBRIDJwIICgwKBQQFAgMBEgMnCRMKDAoFBAUC",
"AwMSAycWFwoKCgIEBhIEKgAvAQoKCgMEBgESAyoIJwoLCgQEBgIAEgMrAhgKDAoFBAYCAAUSAysCCAoM" +
"CgUEBgIAARIDKwkTCgwKBQQGAgADEgMrFhcKCwoEBAYCARIDLAIZCgwKBQQGAgEFEgMsAggKDAoFBAYC" +
"AQESAywJFAoMCgUEBgIBAxIDLBcYCgsKBAQGAgISAy0CFwoMCgUEBgICBRIDLQIICgwKBQQGAgIBEgMt" +
"CRIKDAoFBAYCAgMSAy0VFgoLCgQEBgIDEgMuAhgKDAoFBAYCAwUSAy4CCAoMCgUEBgIDARIDLgkTCgwK" +
"BQQGAgMDEgMuFhcKCgoCBAcSBDEAMwEKCgoDBAcBEgMxCCUKCwoEBAcCABIDMgItCgwKBQQHAgAEEgMy" +
"AgoKDAoFBAcCAAYSAzILIAoMCgUEBwIAARIDMiEoCgwKBQQHAgADEgMyKywKCgoCBAgSBDUAOAEKCgoD" +
"BAgBEgM1CCQKCwoEBAgCABIDNgIWCgwKBQQIAgAFEgM2AggKDAoFBAgCAAESAzYJEQoMCgUECAIAAxID" +
"NhQVCgsKBAQIAgESAzcCHwoMCgUECAIBBhIDNwIUCgwKBQQIAgEBEgM3FRoKDAoFBAgCAQMSAzcdHgoK" +
"CgIECRIEOgA9AQoKCgMECQESAzoIIQoLCgQECQIAEgM7AhMKDAoFBAkCAAUSAzsCCAoMCgUECQIAARID" +
"OwkOCgwKBQQJAgADEgM7ERIKCwoEBAkCARIDPAIfCgwKBQQJAgEGEgM8AhQKDAoFBAkCAQESAzwVGgoM" +
"CgUECQIBAxIDPB0eYgZwcm90bzM=");
static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor());
static final Descriptors.Descriptor wpi_proto_ProtobufChassisSpeeds_descriptor = descriptor.internalContainedType(49, 77, "ProtobufChassisSpeeds", "wpi.proto.ProtobufChassisSpeeds");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(128, 70, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveKinematics_descriptor = descriptor.internalContainedType(128, 69, "ProtobufDifferentialDriveKinematics", "wpi.proto.ProtobufDifferentialDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(200, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(199, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(282, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions");
static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(281, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(368, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(367, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(663, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(662, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(826, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds");
static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(825, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(985, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(984, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1078, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1077, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1191, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState");
static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1190, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState");
/**
* @return this proto file's descriptor.
@@ -520,9 +520,9 @@ public final class Kinematics {
private static final long serialVersionUID = 0L;
/**
* <code>optional double track_width = 1;</code>
* <code>optional double trackwidth = 1;</code>
*/
private double trackWidth;
private double trackwidth;
private ProtobufDifferentialDriveKinematics() {
}
@@ -535,39 +535,39 @@ public final class Kinematics {
}
/**
* <code>optional double track_width = 1;</code>
* @return whether the trackWidth field is set
* <code>optional double trackwidth = 1;</code>
* @return whether the trackwidth field is set
*/
public boolean hasTrackWidth() {
public boolean hasTrackwidth() {
return (bitField0_ & 0x00000001) != 0;
}
/**
* <code>optional double track_width = 1;</code>
* <code>optional double trackwidth = 1;</code>
* @return this
*/
public ProtobufDifferentialDriveKinematics clearTrackWidth() {
public ProtobufDifferentialDriveKinematics clearTrackwidth() {
bitField0_ &= ~0x00000001;
trackWidth = 0D;
trackwidth = 0D;
return this;
}
/**
* <code>optional double track_width = 1;</code>
* @return the trackWidth
* <code>optional double trackwidth = 1;</code>
* @return the trackwidth
*/
public double getTrackWidth() {
return trackWidth;
public double getTrackwidth() {
return trackwidth;
}
/**
* <code>optional double track_width = 1;</code>
* @param value the trackWidth to set
* <code>optional double trackwidth = 1;</code>
* @param value the trackwidth to set
* @return this
*/
public ProtobufDifferentialDriveKinematics setTrackWidth(final double value) {
public ProtobufDifferentialDriveKinematics setTrackwidth(final double value) {
bitField0_ |= 0x00000001;
trackWidth = value;
trackwidth = value;
return this;
}
@@ -577,7 +577,7 @@ public final class Kinematics {
cachedSize = other.cachedSize;
if ((bitField0_ | other.bitField0_) != 0) {
bitField0_ = other.bitField0_;
trackWidth = other.trackWidth;
trackwidth = other.trackwidth;
}
return this;
}
@@ -589,8 +589,8 @@ public final class Kinematics {
return this;
}
cachedSize = -1;
if (other.hasTrackWidth()) {
setTrackWidth(other.trackWidth);
if (other.hasTrackwidth()) {
setTrackwidth(other.trackwidth);
}
return this;
}
@@ -602,7 +602,7 @@ public final class Kinematics {
}
cachedSize = -1;
bitField0_ = 0;
trackWidth = 0D;
trackwidth = 0D;
return this;
}
@@ -626,14 +626,14 @@ public final class Kinematics {
}
ProtobufDifferentialDriveKinematics other = (ProtobufDifferentialDriveKinematics) o;
return bitField0_ == other.bitField0_
&& (!hasTrackWidth() || ProtoUtil.isEqual(trackWidth, other.trackWidth));
&& (!hasTrackwidth() || ProtoUtil.isEqual(trackwidth, other.trackwidth));
}
@Override
public void writeTo(final ProtoSink output) throws IOException {
if ((bitField0_ & 0x00000001) != 0) {
output.writeRawByte((byte) 9);
output.writeDoubleNoTag(trackWidth);
output.writeDoubleNoTag(trackwidth);
}
}
@@ -655,8 +655,8 @@ public final class Kinematics {
while (true) {
switch (tag) {
case 9: {
// trackWidth
trackWidth = input.readDouble();
// trackwidth
trackwidth = input.readDouble();
bitField0_ |= 0x00000001;
tag = input.readTag();
if (tag != 0) {
@@ -681,7 +681,7 @@ public final class Kinematics {
public void writeTo(final JsonSink output) throws IOException {
output.beginObject();
if ((bitField0_ & 0x00000001) != 0) {
output.writeDouble(FieldNames.trackWidth, trackWidth);
output.writeDouble(FieldNames.trackwidth, trackwidth);
}
output.endObject();
}
@@ -694,11 +694,10 @@ public final class Kinematics {
}
while (!input.isAtEnd()) {
switch (input.readFieldHash()) {
case 1152213819:
case 1600986578: {
if (input.isAtField(FieldNames.trackWidth)) {
case 1181766491: {
if (input.isAtField(FieldNames.trackwidth)) {
if (!input.trySkipNullValue()) {
trackWidth = input.readDouble();
trackwidth = input.readDouble();
bitField0_ |= 0x00000001;
}
} else {
@@ -768,7 +767,7 @@ public final class Kinematics {
* Contains name constants used for serializing JSON
*/
static class FieldNames {
static final FieldName trackWidth = FieldName.forField("trackWidth", "track_width");
static final FieldName trackwidth = FieldName.forField("trackwidth");
}
}

Some files were not shown because too many files have changed in this diff Show More