Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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

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() {