Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).

This PR also replaces the speed verbiage in MotorController with duty
cycle.

Fixes #8423.
This commit is contained in:
Tyler Veness
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpilibjExamples/src/main/java/org/wpilib/examples/addressableled/Robot.java
View File

@@ -24,10 +24,11 @@ public class Robot extends TimedRobot {
// Our LED strip has a density of 120 LEDs per meter
private static final Distance kLedSpacing = Meters.of(1 / 120.0);
// Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a speed
// Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a
// velocity
// of 1 meter per second.
private final LEDPattern m_scrollingRainbow =
m_rainbow.scrollAtAbsoluteSpeed(MetersPerSecond.of(1), kLedSpacing);
m_rainbow.scrollAtAbsoluteVelocity(MetersPerSecond.of(1), kLedSpacing);
/** Called once at the beginning of the robot program. */
public Robot() {

2
wpilibjExamples/src/main/java/org/wpilib/examples/arcadedrive/Robot.java
View File

@@ -18,7 +18,7 @@ public class Robot extends TimedRobot {
private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
private final Joystick m_stick = new Joystick(0);
/** Called once at the beginning of the robot program. */

2
wpilibjExamples/src/main/java/org/wpilib/examples/arcadedrivegamepad/Robot.java
View File

@@ -18,7 +18,7 @@ public class Robot extends TimedRobot {
private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
private final Gamepad m_driverController = new Gamepad(0);
/** Called once at the beginning of the robot program. */

4
wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/subsystems/Arm.java
View File

@@ -86,7 +86,7 @@ public class Arm implements AutoCloseable {
public void simulationPeriodic() {
// In this method, we update our simulation of what our arm is doing
// First, we set our "inputs" (voltages)
m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
m_armSim.setInput(m_motor.getDutyCycle() * RobotController.getBatteryVoltage());
// Next, we update it. The standard loop time is 20ms.
m_armSim.update(0.020);
@@ -120,7 +120,7 @@ public class Arm implements AutoCloseable {
}
public void stop() {
m_motor.set(0.0);
m_motor.setDutyCycle(0.0);
}
@Override

32
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdrivebot/Drivetrain.java
View File

@@ -9,15 +9,15 @@ import org.wpilib.hardware.motor.PWMSparkMax;
import org.wpilib.hardware.rotation.Encoder;
import org.wpilib.math.controller.PIDController;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
import org.wpilib.math.kinematics.DifferentialDriveOdometry;
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
import org.wpilib.math.kinematics.DifferentialDriveWheelVelocities;
/** Represents a differential drive style drivetrain. */
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
public static final double kMaxVelocity = 3.0; // meters per second
public static final double kMaxAngularVelocity = 2 * Math.PI; // one rotation per second
private static final double kTrackwidth = 0.381 * 2; // meters
private static final double kWheelRadius = 0.0508; // meters
@@ -74,17 +74,18 @@ public class Drivetrain {
}
/**
* Sets the desired wheel speeds.
* Sets the desired wheel velocities.
*
* @param speeds The desired wheel speeds.
* @param velocities The desired wheel velocities.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.left);
final double rightFeedforward = m_feedforward.calculate(speeds.right);
public void setVelocities(DifferentialDriveWheelVelocities velocities) {
final double leftFeedforward = m_feedforward.calculate(velocities.left);
final double rightFeedforward = m_feedforward.calculate(velocities.right);
final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), velocities.left);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right);
m_rightPIDController.calculate(m_rightEncoder.getRate(), velocities.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}
@@ -92,12 +93,13 @@ public class Drivetrain {
/**
* Drives the robot with the given linear velocity and angular velocity.
*
* @param xSpeed Linear velocity in m/s.
* @param xVelocity Linear velocity in m/s.
* @param rot Angular velocity in rad/s.
*/
public void drive(double xSpeed, double rot) {
var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
setSpeeds(wheelSpeeds);
public void drive(double xVelocity, double rot) {
var wheelVelocities =
m_kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0.0, rot));
setVelocities(wheelVelocities);
}
/** Updates the field-relative position. */

12
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdrivebot/Robot.java
View File

@@ -13,7 +13,7 @@ public class Robot extends TimedRobot {
private final Drivetrain m_drive = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
@Override
@@ -24,16 +24,18 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
final var xVelocity =
-m_velocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_drive.drive(xSpeed, rot);
m_drive.drive(xVelocity, rot);
}
}

36
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -16,9 +16,9 @@ import org.wpilib.math.geometry.Quaternion;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
import org.wpilib.math.kinematics.DifferentialDriveWheelVelocities;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.DCMotor;
@@ -39,8 +39,8 @@ import org.wpilib.vision.apriltag.AprilTagFields;
/** Represents a differential drive style drivetrain. */
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
public static final double kMaxVelocity = 3.0; // meters per second
public static final double kMaxAngularVelocity = 2 * Math.PI; // one rotation per second
private static final double kTrackwidth = 0.381 * 2; // meters
private static final double kWheelRadius = 0.0508; // meters
@@ -132,17 +132,18 @@ public class Drivetrain {
}
/**
* Sets the desired wheel speeds.
* Sets the desired wheel velocities.
*
* @param speeds The desired wheel speeds.
* @param velocities The desired wheel velocities.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.left);
final double rightFeedforward = m_feedforward.calculate(speeds.right);
public void setVelocities(DifferentialDriveWheelVelocities velocities) {
final double leftFeedforward = m_feedforward.calculate(velocities.left);
final double rightFeedforward = m_feedforward.calculate(velocities.right);
final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.left);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), velocities.left);
final double rightOutput =
m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.right);
m_rightPIDController.calculate(m_rightEncoder.getRate(), velocities.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
}
@@ -150,12 +151,13 @@ public class Drivetrain {
/**
* Drives the robot with the given linear velocity and angular velocity.
*
* @param xSpeed Linear velocity in m/s.
* @param xVelocity Linear velocity in m/s.
* @param rot Angular velocity in rad/s.
*/
public void drive(double xSpeed, double rot) {
var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
setSpeeds(wheelSpeeds);
public void drive(double xVelocity, double rot) {
var wheelVelocities =
m_kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0.0, rot));
setVelocities(wheelVelocities);
}
/**
@@ -246,8 +248,8 @@ public class Drivetrain {
// simulation, and write the simulated positions and velocities to our
// simulated encoder and gyro.
m_drivetrainSimulator.setInputs(
m_leftLeader.get() * RobotController.getInputVoltage(),
m_rightLeader.get() * RobotController.getInputVoltage());
m_leftLeader.getDutyCycle() * RobotController.getInputVoltage(),
m_rightLeader.getDutyCycle() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition());

12
wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Robot.java
View File

@@ -19,7 +19,7 @@ public class Robot extends TimedRobot {
private final Drivetrain m_drive = new Drivetrain(m_doubleArrayTopic);
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
@Override
@@ -40,16 +40,18 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// Get the x speed. We are inverting this because gamepad return
// Get the x velocity. We are inverting this because gamepad return
// negative values when we push forward.
final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
final var xVelocity =
-m_velocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_drive.drive(xSpeed, rot);
m_drive.drive(xVelocity, rot);
}
}

2
wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/Constants.java
View File

@@ -30,7 +30,7 @@ public final class Constants {
public static final double kp = 1;
public static final double kMaxSpeed = 3; // m/s
public static final double kMaxVelocity = 3; // m/s
public static final double kMaxAcceleration = 3; // m/s²
}

6
wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/ExampleSmartMotorController.java
View File

@@ -72,11 +72,11 @@ public class ExampleSmartMotorController {
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
public void set(double speed) {
m_value = speed;
public void setDutyCycle(double dutyCycle) {
m_value = dutyCycle;
}
public double get() {
public double getDutyCycle() {
return m_value;
}

9
wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/RobotContainer.java
View File

@@ -22,8 +22,9 @@ public class RobotContainer {
private final DriveSubsystem m_robotDrive = new DriveSubsystem();
// Retained command references
private final Command m_driveFullSpeed = Commands.runOnce(() -> m_robotDrive.setMaxOutput(1));
private final Command m_driveHalfSpeed = Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5));
private final Command m_driveFullVelocity = Commands.runOnce(() -> m_robotDrive.setMaxOutput(1));
private final Command m_driveHalfVelocity =
Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5));
// The driver's controller
CommandGamepad m_driverController = new CommandGamepad(OIConstants.kDriverControllerPort);
@@ -52,8 +53,8 @@ public class RobotContainer {
* org.wpilib.command2.button.JoystickButton}.
*/
private void configureButtonBindings() {
// Drive at half speed when the bumper is held
m_driverController.rightBumper().onTrue(m_driveHalfSpeed).onFalse(m_driveFullSpeed);
// Drive at half velocity when the bumper is held
m_driverController.rightBumper().onTrue(m_driveHalfVelocity).onFalse(m_driveFullVelocity);
// Drive forward by 3 meters when the 'South Face' button is pressed, with a timeout of 10
// seconds

4
wpilibjExamples/src/main/java/org/wpilib/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
View File

@@ -37,13 +37,13 @@ public class DriveSubsystem extends SubsystemBase {
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
new DifferentialDrive(m_leftLeader::setDutyCycle, m_rightLeader::setDutyCycle);
// The trapezoid profile
private final TrapezoidProfile m_profile =
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
DriveConstants.kMaxSpeed, DriveConstants.kMaxAcceleration));
DriveConstants.kMaxVelocity, DriveConstants.kMaxAcceleration));
// The timer
private final Timer m_timer = new Timer();

4
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialprofile/ExampleSmartMotorController.java
View File

@@ -70,9 +70,9 @@ public class ExampleSmartMotorController {
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
public void set(double speed) {}
public void setDutyCycle(double dutyCycle) {}
public double get() {
public double getDutyCycle() {
return 0;
}

4
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/subsystems/Elevator.java
View File

@@ -85,7 +85,7 @@ public class Elevator implements AutoCloseable {
public void simulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
m_elevatorSim.setInput(m_motorSim.getDutyCycle() * RobotController.getBatteryVoltage());
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.update(0.020);
@@ -118,7 +118,7 @@ public class Elevator implements AutoCloseable {
/** Stop the control loop and motor output. */
public void stop() {
m_motor.set(0.0);
m_motor.setDutyCycle(0.0);
}
/** Reset Exponential profile to begin from current position on enable. */

4
wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/subsystems/Elevator.java
View File

@@ -78,7 +78,7 @@ public class Elevator implements AutoCloseable {
public void simulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
m_elevatorSim.setInput(m_motorSim.getDutyCycle() * RobotController.getBatteryVoltage());
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.update(0.020);
@@ -107,7 +107,7 @@ public class Elevator implements AutoCloseable {
/** Stop the control loop and motor output. */
public void stop() {
m_controller.setGoal(0.0);
m_motor.set(0.0);
m_motor.setDutyCycle(0.0);
}
/** Update telemetry, including the mechanism visualization. */

4
wpilibjExamples/src/main/java/org/wpilib/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
View File

@@ -70,9 +70,9 @@ public class ExampleSmartMotorController {
/** Resets the encoder to zero distance. */
public void resetEncoder() {}
public void set(double speed) {}
public void setDutyCycle(double dutyCycle) {}
public double get() {
public double getDutyCycle() {
return 0;
}

4
wpilibjExamples/src/main/java/org/wpilib/examples/eventloop/Robot.java
View File

@@ -42,7 +42,7 @@ public class Robot extends TimedRobot {
// and there is not a ball at the kicker
.and(isBallAtKicker.negate())
// activate the intake
.ifHigh(() -> m_intake.set(0.5));
.ifHigh(() -> m_intake.setDutyCycle(0.5));
// if the thumb button is not held
intakeButton
@@ -73,7 +73,7 @@ public class Robot extends TimedRobot {
.debounce(0.2);
// if we're at the target velocity, kick the ball into the shooter wheel
atTargetVelocity.ifHigh(() -> m_kicker.set(0.7));
atTargetVelocity.ifHigh(() -> m_kicker.setDutyCycle(0.7));
// when we stop being at the target velocity, it means the ball was shot
atTargetVelocity

5
wpilibjExamples/src/main/java/org/wpilib/examples/flywheelbangbangcontroller/Robot.java
View File

@@ -70,7 +70,7 @@ public class Robot extends TimedRobot {
SmartDashboard.putData(m_bangBangController);
}
/** Controls flywheel to a set speed (RPM) controlled by a joystick. */
/** Controls flywheel to a set velocity (RPM) controlled by a joystick. */
@Override
public void teleopPeriodic() {
// Scale setpoint value between 0 and maxSetpointValue
@@ -94,7 +94,8 @@ public class Robot extends TimedRobot {
public void simulationPeriodic() {
// To update our simulation, we set motor voltage inputs, update the
// simulation, and write the simulated velocities to our simulated encoder
m_flywheelSim.setInputVoltage(m_flywheelMotor.get() * RobotController.getInputVoltage());
m_flywheelSim.setInputVoltage(
m_flywheelMotor.getDutyCycle() * RobotController.getInputVoltage());
m_flywheelSim.update(0.02);
m_encoderSim.setRate(m_flywheelSim.getAngularVelocity());
}

4
wpilibjExamples/src/main/java/org/wpilib/examples/gettingstarted/Robot.java
View File

@@ -20,7 +20,7 @@ public class Robot extends TimedRobot {
private final PWMSparkMax m_leftDrive = new PWMSparkMax(0);
private final PWMSparkMax m_rightDrive = new PWMSparkMax(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
new DifferentialDrive(m_leftDrive::setDutyCycle, m_rightDrive::setDutyCycle);
private final Gamepad m_controller = new Gamepad(0);
private final Timer m_timer = new Timer();
@@ -46,7 +46,7 @@ public class Robot extends TimedRobot {
public void autonomousPeriodic() {
// Drive for 2 seconds
if (m_timer.get() < 2.0) {
// Drive forwards half speed, make sure to turn input squaring off
// Drive forwards half velocity, make sure to turn input squaring off
m_robotDrive.arcadeDrive(0.5, 0.0, false);
} else {
m_robotDrive.stopMotor(); // stop robot

6
wpilibjExamples/src/main/java/org/wpilib/examples/gyro/Robot.java
View File

@@ -29,7 +29,7 @@ public class Robot extends TimedRobot {
private final PWMSparkMax m_leftDrive = new PWMSparkMax(kLeftMotorPort);
private final PWMSparkMax m_rightDrive = new PWMSparkMax(kRightMotorPort);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
new DifferentialDrive(m_leftDrive::setDutyCycle, m_rightDrive::setDutyCycle);
private final OnboardIMU m_imu = new OnboardIMU(kIMUMountOrientation);
private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -45,8 +45,8 @@ public class Robot extends TimedRobot {
}
/**
* The motor speed is set from the joystick while the DifferentialDrive turning value is assigned
* from the error between the setpoint and the gyro angle.
* The motor velocity is set from the joystick while the DifferentialDrive turning value is
* assigned from the error between the setpoint and the gyro angle.
*/
@Override
public void teleopPeriodic() {

7
wpilibjExamples/src/main/java/org/wpilib/examples/gyromecanum/Robot.java
View File

@@ -40,7 +40,12 @@ public class Robot extends TimedRobot {
frontRight.setInverted(true);
rearRight.setInverted(true);
m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
m_robotDrive =
new MecanumDrive(
frontLeft::setDutyCycle,
rearLeft::setDutyCycle,
frontRight::setDutyCycle,
rearRight::setDutyCycle);
SendableRegistry.addChild(m_robotDrive, frontLeft);
SendableRegistry.addChild(m_robotDrive, rearLeft);

2
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/Constants.java
View File

@@ -39,7 +39,7 @@ public final class Constants {
public static final class AutoConstants {
public static final double kAutoDriveDistanceInches = 60;
public static final double kAutoBackupDistanceInches = 20;
public static final double kAutoDriveSpeed = 0.5;
public static final double kAutoDriveVelocity = 0.5;
}
public static final class OIConstants {

2
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/RobotContainer.java
View File

@@ -79,7 +79,7 @@ public class RobotContainer {
m_driverController.eastFace().onTrue(m_hatchSubsystem.grabHatchCommand());
// Release the hatch when the west face button is pressed.
m_driverController.westFace().onTrue(m_hatchSubsystem.releaseHatchCommand());
// While holding right bumper, drive at half speed
// While holding right bumper, drive at half velocity
m_driverController
.rightBumper()
.onTrue(Commands.runOnce(() -> m_robotDrive.setMaxOutput(0.5)))

6
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/commands/Autos.java
View File

@@ -19,7 +19,7 @@ public final class Autos {
// Reset encoders on command start
drive::resetEncoders,
// Drive forward while the command is executing
() -> drive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
() -> drive.arcadeDrive(AutoConstants.kAutoDriveVelocity, 0),
// Stop driving at the end of the command
interrupt -> drive.arcadeDrive(0, 0),
// End the command when the robot's driven distance exceeds the desired value
@@ -36,7 +36,7 @@ public final class Autos {
// Reset encoders on command start
driveSubsystem::resetEncoders,
// Drive forward while the command is executing
() -> driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
() -> driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveVelocity, 0),
// Stop driving at the end of the command
interrupt -> driveSubsystem.arcadeDrive(0, 0),
// End the command when the robot's driven distance exceeds the desired value
@@ -54,7 +54,7 @@ public final class Autos {
// Reset encoders on command start
driveSubsystem::resetEncoders,
// Drive backward while the command is executing
() -> driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0),
() -> driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveVelocity, 0),
// Stop driving at the end of the command
interrupt -> driveSubsystem.arcadeDrive(0, 0),
// End the command when the robot's driven distance exceeds the desired value

2
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbotinlined/subsystems/DriveSubsystem.java
View File

@@ -23,7 +23,7 @@ public class DriveSubsystem extends SubsystemBase {
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
new DifferentialDrive(m_leftLeader::setDutyCycle, m_rightLeader::setDutyCycle);
// The left-side drive encoder
private final Encoder m_leftEncoder =

2
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/Constants.java
View File

@@ -39,7 +39,7 @@ public final class Constants {
public static final class AutoConstants {
public static final double kAutoDriveDistanceInches = 60;
public static final double kAutoBackupDistanceInches = 20;
public static final double kAutoDriveSpeed = 0.5;
public static final double kAutoDriveVelocity = 0.5;
}
public static final class OIConstants {

8
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/RobotContainer.java
View File

@@ -15,7 +15,7 @@ import org.wpilib.examples.hatchbottraditional.commands.ComplexAuto;
import org.wpilib.examples.hatchbottraditional.commands.DefaultDrive;
import org.wpilib.examples.hatchbottraditional.commands.DriveDistance;
import org.wpilib.examples.hatchbottraditional.commands.GrabHatch;
import org.wpilib.examples.hatchbottraditional.commands.HalveDriveSpeed;
import org.wpilib.examples.hatchbottraditional.commands.HalveDriveVelocity;
import org.wpilib.examples.hatchbottraditional.commands.ReleaseHatch;
import org.wpilib.examples.hatchbottraditional.subsystems.DriveSubsystem;
import org.wpilib.examples.hatchbottraditional.subsystems.HatchSubsystem;
@@ -38,7 +38,7 @@ public class RobotContainer {
// A simple auto routine that drives forward a specified distance, and then stops.
private final Command m_simpleAuto =
new DriveDistance(
AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, m_robotDrive);
AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveVelocity, m_robotDrive);
// A complex auto routine that drives forward, drops a hatch, and then drives backward.
private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
@@ -88,9 +88,9 @@ public class RobotContainer {
// Release the hatch when the 'East Face' button is pressed.
new JoystickButton(m_driverController, Button.kEastFace.value)
.onTrue(new ReleaseHatch(m_hatchSubsystem));
// While holding the bumper button, drive at half speed
// While holding the bumper button, drive at half velocity
new JoystickButton(m_driverController, Button.kRightBumper.value)
.whileTrue(new HalveDriveSpeed(m_robotDrive));
.whileTrue(new HalveDriveVelocity(m_robotDrive));
}
/**

4
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/ComplexAuto.java
View File

@@ -21,13 +21,13 @@ public class ComplexAuto extends SequentialCommandGroup {
addCommands(
// Drive forward the specified distance
new DriveDistance(
AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, drive),
AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveVelocity, drive),
// Release the hatch
new ReleaseHatch(hatch),
// Drive backward the specified distance
new DriveDistance(
AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveSpeed, drive));
AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveVelocity, drive));
}
}

12
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/DriveDistance.java
View File

@@ -10,18 +10,18 @@ import org.wpilib.examples.hatchbottraditional.subsystems.DriveSubsystem;
public class DriveDistance extends Command {
private final DriveSubsystem m_drive;
private final double m_distance;
private final double m_speed;
private final double m_velocity;
/**
* Creates a new DriveDistance.
*
* @param inches The number of inches the robot will drive
* @param speed The speed at which the robot will drive
* @param velocity The velocity at which the robot will drive
* @param drive The drive subsystem on which this command will run
*/
public DriveDistance(double inches, double speed, DriveSubsystem drive) {
public DriveDistance(double inches, double velocity, DriveSubsystem drive) {
m_distance = inches;
m_speed = speed;
m_velocity = velocity;
m_drive = drive;
addRequirements(m_drive);
}
@@ -29,12 +29,12 @@ public class DriveDistance extends Command {
@Override
public void initialize() {
m_drive.resetEncoders();
m_drive.arcadeDrive(m_speed, 0);
m_drive.arcadeDrive(m_velocity, 0);
}
@Override
public void execute() {
m_drive.arcadeDrive(m_speed, 0);
m_drive.arcadeDrive(m_velocity, 0);
}
@Override

4
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/HalveDriveSpeed.java → wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/commands/HalveDriveVelocity.java
View File

@@ -7,10 +7,10 @@ package org.wpilib.examples.hatchbottraditional.commands;
import org.wpilib.command2.Command;
import org.wpilib.examples.hatchbottraditional.subsystems.DriveSubsystem;
public class HalveDriveSpeed extends Command {
public class HalveDriveVelocity extends Command {
private final DriveSubsystem m_drive;
public HalveDriveSpeed(DriveSubsystem drive) {
public HalveDriveVelocity(DriveSubsystem drive) {
m_drive = drive;
}

2
wpilibjExamples/src/main/java/org/wpilib/examples/hatchbottraditional/subsystems/DriveSubsystem.java
View File

@@ -23,7 +23,7 @@ public class DriveSubsystem extends SubsystemBase {
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
new DifferentialDrive(m_leftLeader::setDutyCycle, m_rightLeader::setDutyCycle);
// The left-side drive encoder
private final Encoder m_leftEncoder =

83
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumbot/Drivetrain.java
View File

@@ -10,16 +10,16 @@ import org.wpilib.hardware.rotation.Encoder;
import org.wpilib.math.controller.PIDController;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.MecanumDriveKinematics;
import org.wpilib.math.kinematics.MecanumDriveOdometry;
import org.wpilib.math.kinematics.MecanumDriveWheelPositions;
import org.wpilib.math.kinematics.MecanumDriveWheelSpeeds;
import org.wpilib.math.kinematics.MecanumDriveWheelVelocities;
/** Represents a mecanum drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
public static final double kMaxVelocity = 3.0; // 3 meters per second
public static final double kMaxAngularVelocity = Math.PI; // 1/2 rotation per second
private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
@@ -48,7 +48,7 @@ public class Drivetrain {
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(m_kinematics, m_imu.getRotation2d(), getCurrentDistances());
new MecanumDriveOdometry(m_kinematics, m_imu.getRotation2d(), getCurrentWheelDistances());
// Gains are for example purposes only - must be determined for your own robot!
private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
@@ -64,24 +64,11 @@ public class Drivetrain {
}
/**
* Returns the current state of the drivetrain.
* Returns the current wheel distances measured by the drivetrain.
*
* @return The current state of the drivetrain.
* @return The current wheel distances measured by the drivetrain.
*/
public MecanumDriveWheelSpeeds getCurrentState() {
return new MecanumDriveWheelSpeeds(
m_frontLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_backLeftEncoder.getRate(),
m_backRightEncoder.getRate());
}
/**
* Returns the current distances measured by the drivetrain.
*
* @return The current distances measured by the drivetrain.
*/
public MecanumDriveWheelPositions getCurrentDistances() {
public MecanumDriveWheelPositions getCurrentWheelDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
@@ -90,24 +77,37 @@ public class Drivetrain {
}
/**
* Set the desired speeds for each wheel.
* Returns the current wheel velocities of the drivetrain.
*
* @param speeds The desired wheel speeds.
* @return The current wheel velocities of the drivetrain.
*/
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
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);
public MecanumDriveWheelVelocities getCurrentWheelVelocities() {
return new MecanumDriveWheelVelocities(
m_frontLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_backLeftEncoder.getRate(),
m_backRightEncoder.getRate());
}
/**
* Set the desired velocities for each wheel.
*
* @param velocities The desired wheel velocities.
*/
public void setVelocities(MecanumDriveWheelVelocities velocities) {
final double frontLeftFeedforward = m_feedforward.calculate(velocities.frontLeft);
final double frontRightFeedforward = m_feedforward.calculate(velocities.frontRight);
final double backLeftFeedforward = m_feedforward.calculate(velocities.rearLeft);
final double backRightFeedforward = m_feedforward.calculate(velocities.rearRight);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), speeds.frontLeft);
m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), velocities.frontLeft);
final double frontRightOutput =
m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), speeds.frontRight);
m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), velocities.frontRight);
final double backLeftOutput =
m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), speeds.rearLeft);
m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), velocities.rearLeft);
final double backRightOutput =
m_backRightPIDController.calculate(m_backRightEncoder.getRate(), speeds.rearRight);
m_backRightPIDController.calculate(m_backRightEncoder.getRate(), velocities.rearRight);
m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
@@ -118,22 +118,25 @@ public class Drivetrain {
/**
* Method to drive the robot using joystick info.
*
* @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways).
* @param xVelocity Velocity of the robot in the x direction (forward).
* @param yVelocity Velocity of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
* @param fieldRelative Whether the provided x and y velocities are relative to the field.
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
double xVelocity, double yVelocity, double rot, boolean fieldRelative, double period) {
var chassisVelocities = new ChassisVelocities(xVelocity, yVelocity, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_imu.getRotation2d());
chassisVelocities = chassisVelocities.toRobotRelative(m_imu.getRotation2d());
}
setSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(kMaxSpeed));
setVelocities(
m_kinematics
.toWheelVelocities(chassisVelocities.discretize(period))
.desaturate(kMaxVelocity));
}
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_odometry.update(m_imu.getRotation2d(), getCurrentDistances());
m_odometry.update(m_imu.getRotation2d(), getCurrentWheelDistances());
}
}

19
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumbot/Robot.java
View File

@@ -13,8 +13,8 @@ public class Robot extends TimedRobot {
private final Drivetrain m_mecanum = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
@Override
@@ -29,21 +29,24 @@ public class Robot extends TimedRobot {
}
private void driveWithJoystick(boolean fieldRelative) {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
final var xVelocity =
-m_xVelocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the y speed or sideways/strafe speed. We are inverting this because
// Get the y velocity or sideways/strafe velocity. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
final var yVelocity =
-m_yVelocityLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
m_mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
}
}

7
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdrive/Robot.java
View File

@@ -34,7 +34,12 @@ public class Robot extends TimedRobot {
frontRight.setInverted(true);
rearRight.setInverted(true);
m_robotDrive = new MecanumDrive(frontLeft::set, rearLeft::set, frontRight::set, rearRight::set);
m_robotDrive =
new MecanumDrive(
frontLeft::setDutyCycle,
rearLeft::setDutyCycle,
frontRight::setDutyCycle,
rearRight::setDutyCycle);
m_stick = new Joystick(kJoystickChannel);

85
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -12,18 +12,18 @@ import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.estimator.MecanumDrivePoseEstimator;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.MecanumDriveKinematics;
import org.wpilib.math.kinematics.MecanumDriveWheelPositions;
import org.wpilib.math.kinematics.MecanumDriveWheelSpeeds;
import org.wpilib.math.kinematics.MecanumDriveWheelVelocities;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.util.Units;
import org.wpilib.system.Timer;
/** Represents a mecanum drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
public static final double kMaxVelocity = 3.0; // 3 meters per second
public static final double kMaxAngularVelocity = Math.PI; // 1/2 rotation per second
private final PWMSparkMax m_frontLeftMotor = new PWMSparkMax(1);
private final PWMSparkMax m_frontRightMotor = new PWMSparkMax(2);
@@ -57,7 +57,7 @@ public class Drivetrain {
new MecanumDrivePoseEstimator(
m_kinematics,
m_imu.getRotation2d(),
getCurrentDistances(),
getCurrentWheelDistances(),
Pose2d.kZero,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
@@ -76,24 +76,11 @@ public class Drivetrain {
}
/**
* Returns the current state of the drivetrain.
* Returns the current wheel distances measured by the drivetrain.
*
* @return The current state of the drivetrain.
* @return The current wheel distances measured by the drivetrain.
*/
public MecanumDriveWheelSpeeds getCurrentState() {
return new MecanumDriveWheelSpeeds(
m_frontLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_backLeftEncoder.getRate(),
m_backRightEncoder.getRate());
}
/**
* Returns the current distances measured by the drivetrain.
*
* @return The current distances measured by the drivetrain.
*/
public MecanumDriveWheelPositions getCurrentDistances() {
public MecanumDriveWheelPositions getCurrentWheelDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftEncoder.getDistance(),
m_frontRightEncoder.getDistance(),
@@ -102,24 +89,37 @@ public class Drivetrain {
}
/**
* Set the desired speeds for each wheel.
* Returns the current wheel velocities of the drivetrain.
*
* @param speeds The desired wheel speeds.
* @return The current wheel velocities of the drivetrain.
*/
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
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);
public MecanumDriveWheelVelocities getCurrentWheelVelocities() {
return new MecanumDriveWheelVelocities(
m_frontLeftEncoder.getRate(),
m_frontRightEncoder.getRate(),
m_backLeftEncoder.getRate(),
m_backRightEncoder.getRate());
}
/**
* Set the desired velocities for each wheel.
*
* @param velocities The desired wheel velocities.
*/
public void setVelocities(MecanumDriveWheelVelocities velocities) {
final double frontLeftFeedforward = m_feedforward.calculate(velocities.frontLeft);
final double frontRightFeedforward = m_feedforward.calculate(velocities.frontRight);
final double backLeftFeedforward = m_feedforward.calculate(velocities.rearLeft);
final double backRightFeedforward = m_feedforward.calculate(velocities.rearRight);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), speeds.frontLeft);
m_frontLeftPIDController.calculate(m_frontLeftEncoder.getRate(), velocities.frontLeft);
final double frontRightOutput =
m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), speeds.frontRight);
m_frontRightPIDController.calculate(m_frontRightEncoder.getRate(), velocities.frontRight);
final double backLeftOutput =
m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), speeds.rearLeft);
m_backLeftPIDController.calculate(m_backLeftEncoder.getRate(), velocities.rearLeft);
final double backRightOutput =
m_backRightPIDController.calculate(m_backRightEncoder.getRate(), speeds.rearRight);
m_backRightPIDController.calculate(m_backRightEncoder.getRate(), velocities.rearRight);
m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
@@ -130,24 +130,27 @@ public class Drivetrain {
/**
* Method to drive the robot using joystick info.
*
* @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways).
* @param xVelocity Velocity of the robot in the x direction (forward).
* @param yVelocity Velocity of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
* @param fieldRelative Whether the provided x and y velocities are relative to the field.
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
double xVelocity, double yVelocity, double rot, boolean fieldRelative, double period) {
var chassisVelocities = new ChassisVelocities(xVelocity, yVelocity, rot);
if (fieldRelative) {
chassisSpeeds =
chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
chassisVelocities =
chassisVelocities.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
}
setSpeeds(m_kinematics.toWheelSpeeds(chassisSpeeds.discretize(period)).desaturate(kMaxSpeed));
setVelocities(
m_kinematics
.toWheelVelocities(chassisVelocities.discretize(period))
.desaturate(kMaxVelocity));
}
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_poseEstimator.update(m_imu.getRotation2d(), getCurrentDistances());
m_poseEstimator.update(m_imu.getRotation2d(), getCurrentWheelDistances());
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.

19
wpilibjExamples/src/main/java/org/wpilib/examples/mecanumdriveposeestimator/Robot.java
View File

@@ -13,8 +13,8 @@ public class Robot extends TimedRobot {
private final Drivetrain m_mecanum = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
@Override
@@ -29,21 +29,24 @@ public class Robot extends TimedRobot {
}
private void driveWithJoystick(boolean fieldRelative) {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
final var xVelocity =
-m_xVelocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the y speed or sideways/strafe speed. We are inverting this because
// Get the y velocity or sideways/strafe velocity. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
final var yVelocity =
-m_yVelocityLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
m_mecanum.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
}
}

4
wpilibjExamples/src/main/java/org/wpilib/examples/mechanism2d/Robot.java
View File

@@ -65,7 +65,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
m_elevatorMotor.set(m_joystick.getRawAxis(0));
m_wristMotor.set(m_joystick.getRawAxis(1));
m_elevatorMotor.setDutyCycle(m_joystick.getRawAxis(0));
m_wristMotor.setDutyCycle(m_joystick.getRawAxis(1));
}
}

2
wpilibjExamples/src/main/java/org/wpilib/examples/motorcontrol/Robot.java
View File

@@ -52,6 +52,6 @@ public class Robot extends TimedRobot {
/** The teleop periodic function is called every control packet in teleop. */
@Override
public void teleopPeriodic() {
m_motor.set(m_joystick.getY());
m_motor.setDutyCycle(m_joystick.getY());
}
}

4
wpilibjExamples/src/main/java/org/wpilib/examples/potentiometerpid/Robot.java
View File

@@ -25,7 +25,7 @@ public class Robot extends TimedRobot {
// Bottom, middle, and top elevator setpoints in meters
static final double[] kSetpoints = {0.2, 0.8, 1.4};
// proportional, integral, and derivative speed constants
// proportional, integral, and derivative velocity constants
// DANGER: when tuning PID constants, high/inappropriate values for kP, kI,
// and kD may cause dangerous, uncontrollable, or undesired behavior!
private static final double kP = 0.7;
@@ -57,7 +57,7 @@ public class Robot extends TimedRobot {
double pidOut = m_pidController.calculate(position);
// Apply PID output
m_elevatorMotor.set(pidOut);
m_elevatorMotor.setDutyCycle(pidOut);
// when the button is pressed once, the selected elevator setpoint is incremented
if (m_joystick.getTriggerPressed()) {

4
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/Constants.java
View File

@@ -74,7 +74,7 @@ public final class Constants {
// Should have value 12V at free speed
public static final double kV = 12.0 / kShooterFreeRPS; // V/(rot/s)
public static final double kFeederSpeed = 0.5;
public static final double kFeederVelocity = 0.5;
}
public static final class IntakeConstants {
@@ -90,7 +90,7 @@ public final class Constants {
public static final class AutoConstants {
public static final double kTimeout = 3;
public static final double kDriveDistance = 2; // m
public static final double kDriveSpeed = 0.5;
public static final double kDriveVelocity = 0.5;
}
public static final class OIConstants {

4
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/RapidReactCommandBot.java
View File

@@ -84,9 +84,9 @@ public class RapidReactCommandBot {
* <p>Scheduled during {@link Robot#autonomousInit()}.
*/
public Command getAutonomousCommand() {
// Drive forward for 2 meters at half speed with a 3 second timeout
// Drive forward for 2 meters at half velocity with a 3 second timeout
return m_drive
.driveDistanceCommand(AutoConstants.kDriveDistance, AutoConstants.kDriveSpeed)
.driveDistanceCommand(AutoConstants.kDriveDistance, AutoConstants.kDriveVelocity)
.withTimeout(AutoConstants.kTimeout);
}
}

12
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Drive.java
View File

@@ -33,7 +33,7 @@ public class Drive extends SubsystemBase {
// The robot's drive
@NotLogged // Would duplicate motor data, there's no point sending it twice
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
new DifferentialDrive(m_leftLeader::setDutyCycle, m_rightLeader::setDutyCycle);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -100,20 +100,20 @@ public class Drive extends SubsystemBase {
}
/**
* Returns a command that drives the robot forward a specified distance at a specified speed.
* Returns a command that drives the robot forward a specified distance at a specified velocity.
*
* @param distance The distance to drive forward in meters
* @param speed The fraction of max speed at which to drive
* @param velocity The fraction of max velocity at which to drive
*/
public Command driveDistanceCommand(double distance, double speed) {
public Command driveDistanceCommand(double distance, double velocity) {
return runOnce(
() -> {
// Reset encoders at the start of the command
m_leftEncoder.reset();
m_rightEncoder.reset();
})
// Drive forward at specified speed
.andThen(run(() -> m_drive.arcadeDrive(speed, 0)))
// Drive forward at specified velocity
.andThen(run(() -> m_drive.arcadeDrive(velocity, 0)))
// End command when we've traveled the specified distance
.until(
() -> Math.max(m_leftEncoder.getDistance(), m_rightEncoder.getDistance()) >= distance)

2
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Intake.java
View File

@@ -29,7 +29,7 @@ public class Intake extends SubsystemBase {
/** Returns a command that deploys the intake, and then runs the intake motor indefinitely. */
public Command intakeCommand() {
return runOnce(() -> m_pistons.set(DoubleSolenoid.Value.kForward))
.andThen(run(() -> m_motor.set(1.0)))
.andThen(run(() -> m_motor.setDutyCycle(1.0)))
.withName("Intake");
}

4
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Shooter.java
View File

@@ -56,14 +56,14 @@ public class Shooter extends SubsystemBase {
// Run the shooter flywheel at the desired setpoint using feedforward and feedback
run(
() -> {
m_shooterMotor.set(
m_shooterMotor.setDutyCycle(
m_shooterFeedforward.calculate(setpointRotationsPerSecond)
+ m_shooterFeedback.calculate(
m_shooterEncoder.getRate(), setpointRotationsPerSecond));
}),
// Wait until the shooter has reached the setpoint, and then run the feeder
waitUntil(m_shooterFeedback::atSetpoint).andThen(() -> m_feederMotor.set(1)))
waitUntil(m_shooterFeedback::atSetpoint).andThen(() -> m_feederMotor.setDutyCycle(1)))
.withName("Shoot");
}
}

2
wpilibjExamples/src/main/java/org/wpilib/examples/rapidreactcommandbot/subsystems/Storage.java
View File

@@ -34,6 +34,6 @@ public class Storage extends SubsystemBase {
/** Returns a command that runs the storage motor indefinitely. */
public Command runCommand() {
return run(() -> m_motor.set(1)).withName("run");
return run(() -> m_motor.setDutyCycle(1)).withName("run");
}
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/ArcadeDrive.java
View File

@@ -10,23 +10,23 @@ import org.wpilib.examples.romireference.subsystems.Drivetrain;
public class ArcadeDrive extends Command {
private final Drivetrain m_drivetrain;
private final Supplier<Double> m_xaxisSpeedSupplier;
private final Supplier<Double> m_xaxisVelocitySupplier;
private final Supplier<Double> m_zaxisRotateSupplier;
/**
* Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier
* lambdas. This command does not terminate.
* Creates a new ArcadeDrive. This command will drive your robot according to the velocity
* supplier lambdas. This command does not terminate.
*
* @param drivetrain The drivetrain subsystem on which this command will run
* @param xaxisSpeedSupplier Lambda supplier of forward/backward speed
* @param zaxisRotateSupplier Lambda supplier of rotational speed
* @param xaxisVelocitySupplier Lambda supplier of forward/backward velocity
* @param zaxisRotateSupplier Lambda supplier of rotational velocity
*/
public ArcadeDrive(
Drivetrain drivetrain,
Supplier<Double> xaxisSpeedSupplier,
Supplier<Double> xaxisVelocitySupplier,
Supplier<Double> zaxisRotateSupplier) {
m_drivetrain = drivetrain;
m_xaxisSpeedSupplier = xaxisSpeedSupplier;
m_xaxisVelocitySupplier = xaxisVelocitySupplier;
m_zaxisRotateSupplier = zaxisRotateSupplier;
addRequirements(drivetrain);
}
@@ -38,7 +38,7 @@ public class ArcadeDrive extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drivetrain.arcadeDrive(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get());
m_drivetrain.arcadeDrive(m_xaxisVelocitySupplier.get(), m_zaxisRotateSupplier.get());
}
// Called once the command ends or is interrupted.

12
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/DriveDistance.java
View File

@@ -10,19 +10,19 @@ import org.wpilib.examples.romireference.subsystems.Drivetrain;
public class DriveDistance extends Command {
private final Drivetrain m_drive;
private final double m_distance;
private final double m_speed;
private final double m_velocity;
/**
* Creates a new DriveDistance. This command will drive your your robot for a desired distance at
* a desired speed.
* a desired velocity.
*
* @param speed The speed at which the robot will drive
* @param velocity The velocity at which the robot will drive
* @param inches The number of inches the robot will drive
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveDistance(double speed, double inches, Drivetrain drive) {
public DriveDistance(double velocity, double inches, Drivetrain drive) {
m_distance = inches;
m_speed = speed;
m_velocity = velocity;
m_drive = drive;
addRequirements(drive);
}
@@ -37,7 +37,7 @@ public class DriveDistance extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_speed, 0);
m_drive.arcadeDrive(m_velocity, 0);
}
// Called once the command ends or is interrupted.

12
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/DriveTime.java
View File

@@ -9,19 +9,19 @@ import org.wpilib.examples.romireference.subsystems.Drivetrain;
public class DriveTime extends Command {
private final double m_duration;
private final double m_speed;
private final double m_velocity;
private final Drivetrain m_drive;
private long m_startTime;
/**
* Creates a new DriveTime. This command will drive your robot for a desired speed and time.
* Creates a new DriveTime. This command will drive your robot for a desired velocity and time.
*
* @param speed The speed which the robot will drive. Negative is in reverse.
* @param velocity The velocity which the robot will drive. Negative is in reverse.
* @param time How much time to drive in seconds
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveTime(double speed, double time, Drivetrain drive) {
m_speed = speed;
public DriveTime(double velocity, double time, Drivetrain drive) {
m_velocity = velocity;
m_duration = time * 1000;
m_drive = drive;
addRequirements(drive);
@@ -37,7 +37,7 @@ public class DriveTime extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_speed, 0);
m_drive.arcadeDrive(m_velocity, 0);
}
// Called once the command ends or is interrupted.

12
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/TurnDegrees.java
View File

@@ -10,19 +10,19 @@ import org.wpilib.examples.romireference.subsystems.Drivetrain;
public class TurnDegrees extends Command {
private final Drivetrain m_drive;
private final double m_degrees;
private final double m_speed;
private final double m_velocity;
/**
* Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
* degrees) and rotational speed.
* degrees) and rotational velocity.
*
* @param speed The speed which the robot will drive. Negative is in reverse.
* @param velocity The velocity which the robot will drive. Negative is in reverse.
* @param degrees Degrees to turn. Leverages encoders to compare distance.
* @param drive The drive subsystem on which this command will run
*/
public TurnDegrees(double speed, double degrees, Drivetrain drive) {
public TurnDegrees(double velocity, double degrees, Drivetrain drive) {
m_degrees = degrees;
m_speed = speed;
m_velocity = velocity;
m_drive = drive;
addRequirements(drive);
}
@@ -38,7 +38,7 @@ public class TurnDegrees extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_speed);
m_drive.arcadeDrive(0, m_velocity);
}
// Called once the command ends or is interrupted.

12
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/commands/TurnTime.java
View File

@@ -9,23 +9,23 @@ import org.wpilib.examples.romireference.subsystems.Drivetrain;
/*
* Creates a new TurnTime command. This command will turn your robot for a
* desired rotational speed and time.
* desired rotational velocity and time.
*/
public class TurnTime extends Command {
private final double m_duration;
private final double m_rotationalSpeed;
private final double m_rotationalVelocity;
private final Drivetrain m_drive;
private long m_startTime;
/**
* Creates a new TurnTime.
*
* @param speed The speed which the robot will turn. Negative is in reverse.
* @param velocity The velocity which the robot will turn. Negative is in reverse.
* @param time How much time to turn in seconds
* @param drive The drive subsystem on which this command will run
*/
public TurnTime(double speed, double time, Drivetrain drive) {
m_rotationalSpeed = speed;
public TurnTime(double velocity, double time, Drivetrain drive) {
m_rotationalVelocity = velocity;
m_duration = time * 1000;
m_drive = drive;
addRequirements(drive);
@@ -41,7 +41,7 @@ public class TurnTime extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_rotationalSpeed);
m_drive.arcadeDrive(0, m_rotationalVelocity);
}
// Called once the command ends or is interrupted.

6
wpilibjExamples/src/main/java/org/wpilib/examples/romireference/subsystems/Drivetrain.java
View File

@@ -27,7 +27,7 @@ public class Drivetrain extends SubsystemBase {
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
// Set up the RomiGyro
private final RomiGyro m_gyro = new RomiGyro();
@@ -48,8 +48,8 @@ public class Drivetrain extends SubsystemBase {
resetEncoders();
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {

30
wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Drivetrain.java
View File

@@ -10,10 +10,10 @@ import org.wpilib.hardware.rotation.Encoder;
import org.wpilib.math.controller.PIDController;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
import org.wpilib.math.kinematics.DifferentialDriveOdometry;
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
import org.wpilib.math.kinematics.DifferentialDriveWheelVelocities;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.DCMotor;
import org.wpilib.math.system.LinearSystem;
@@ -26,9 +26,9 @@ import org.wpilib.system.RobotController;
public class Drivetrain {
// 3 meters per second.
public static final double kMaxSpeed = 3.0;
public static final double kMaxVelocity = 3.0;
// 1/2 rotation per second.
public static final double kMaxAngularSpeed = Math.PI;
public static final double kMaxAngularVelocity = Math.PI;
private static final double kTrackwidth = 0.381 * 2;
private static final double kWheelRadius = 0.0508;
@@ -90,12 +90,12 @@ public class Drivetrain {
SmartDashboard.putData("Field", m_fieldSim);
}
/** Sets speeds to the drivetrain motors. */
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
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);
/** Sets velocities to the drivetrain motors. */
public void setVelocities(DifferentialDriveWheelVelocities velocities) {
final double leftFeedforward = m_feedforward.calculate(velocities.left);
final double rightFeedforward = m_feedforward.calculate(velocities.right);
double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(), velocities.left);
double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(), velocities.right);
m_leftLeader.setVoltage(leftOutput + leftFeedforward);
m_rightLeader.setVoltage(rightOutput + rightFeedforward);
@@ -104,11 +104,11 @@ public class Drivetrain {
/**
* Controls the robot using arcade drive.
*
* @param xSpeed the speed for the x axis
* @param xVelocity the velocity for the x axis
* @param rot the rotation
*/
public void drive(double xSpeed, double rot) {
setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)));
public void drive(double xVelocity, double rot) {
setVelocities(m_kinematics.toWheelVelocities(new ChassisVelocities(xVelocity, 0, rot)));
}
/** Update robot odometry. */
@@ -136,8 +136,8 @@ public class Drivetrain {
// simulated encoder and gyro. We negate the right side so that positive
// voltages make the right side move forward.
m_drivetrainSimulator.setInputs(
m_leftLeader.get() * RobotController.getInputVoltage(),
m_rightLeader.get() * RobotController.getInputVoltage());
m_leftLeader.getDutyCycle() * RobotController.getInputVoltage(),
m_rightLeader.getDutyCycle() * RobotController.getInputVoltage());
m_drivetrainSimulator.update(0.02);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPosition());

17
wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Robot.java
View File

@@ -11,7 +11,7 @@ import org.wpilib.math.controller.LTVUnicycleController;
import org.wpilib.math.filter.SlewRateLimiter;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.trajectory.Trajectory;
import org.wpilib.math.trajectory.TrajectoryConfig;
import org.wpilib.math.trajectory.TrajectoryGenerator;
@@ -22,7 +22,7 @@ public class Robot extends TimedRobot {
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_velocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
private final Drivetrain m_drive = new Drivetrain();
@@ -55,22 +55,23 @@ public class Robot extends TimedRobot {
public void autonomousPeriodic() {
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.vx, speeds.omega);
ChassisVelocities velocities = m_feedback.calculate(m_drive.getPose(), reference);
m_drive.drive(velocities.vx, velocities.omega);
}
@Override
public void teleopPeriodic() {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
double xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
double xVelocity =
-m_velocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Xbox controllers return positive values when you pull to
// the right by default.
double rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
m_drive.drive(xSpeed, rot);
double rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_drive.drive(xVelocity, rot);
}
@Override

2
wpilibjExamples/src/main/java/org/wpilib/examples/statespacearm/Robot.java
View File

@@ -44,7 +44,7 @@ public class Robot extends TimedRobot {
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
Units.degreesToRadians(45),
Units.degreesToRadians(90))); // Max arm speed and acceleration.
Units.degreesToRadians(90))); // Max arm velocity and acceleration.
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
// The plant holds a state-space model of our arm. This system has the following properties:

2
wpilibjExamples/src/main/java/org/wpilib/examples/statespaceelevator/Robot.java
View File

@@ -46,7 +46,7 @@ public class Robot extends TimedRobot {
new TrapezoidProfile(
new TrapezoidProfile.Constraints(
Units.feetToMeters(3.0),
Units.feetToMeters(6.0))); // Max elevator speed and acceleration.
Units.feetToMeters(6.0))); // Max elevator velocity and acceleration.
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
/* The plant holds a state-space model of our elevator. This system has the following properties:

2
wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheel/Robot.java
View File

@@ -93,7 +93,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// Sets the target speed of our flywheel. This is similar to setting the setpoint of a
// Sets the target velocity of our flywheel. This is similar to setting the setpoint of a
// PID controller.
if (m_joystick.getTriggerPressed()) {
// We just pressed the trigger, so let's set our next reference

2
wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheelsysid/Robot.java
View File

@@ -89,7 +89,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
// Sets the target speed of our flywheel. This is similar to setting the setpoint of a
// Sets the target velocity of our flywheel. This is similar to setting the setpoint of a
// PID controller.
if (m_joystick.getTriggerPressed()) {
// We just pressed the trigger, so let's set our next reference

32
wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Drivetrain.java
View File

@@ -6,15 +6,15 @@ package org.wpilib.examples.swervebot;
import org.wpilib.hardware.imu.OnboardIMU;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveDriveOdometry;
import org.wpilib.math.kinematics.SwerveModulePosition;
/** Represents a swerve drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
public static final double kMaxVelocity = 3.0; // 3 meters per second
public static final double kMaxAngularVelocity = Math.PI; // 1/2 rotation per second
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
@@ -50,26 +50,26 @@ public class Drivetrain {
/**
* Method to drive the robot using joystick info.
*
* @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways).
* @param xVelocity Velocity of the robot in the x direction (forward).
* @param yVelocity Velocity of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
* @param fieldRelative Whether the provided x and y velocities are relative to the field.
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
double xVelocity, double yVelocity, double rot, boolean fieldRelative, double period) {
var chassisVelocities = new ChassisVelocities(xVelocity, yVelocity, rot);
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.toRobotRelative(m_imu.getRotation2d());
chassisVelocities = chassisVelocities.toRobotRelative(m_imu.getRotation2d());
}
chassisSpeeds = chassisSpeeds.discretize(period);
chassisVelocities = chassisVelocities.discretize(period);
var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);
var states = m_kinematics.toWheelVelocities(chassisVelocities);
SwerveDriveKinematics.desaturateWheelVelocities(states, kMaxVelocity);
m_frontLeft.setDesiredState(states[0]);
m_frontRight.setDesiredState(states[1]);
m_backLeft.setDesiredState(states[2]);
m_backRight.setDesiredState(states[3]);
m_frontLeft.setDesiredVelocity(states[0]);
m_frontRight.setDesiredVelocity(states[1]);
m_backLeft.setDesiredVelocity(states[2]);
m_backRight.setDesiredVelocity(states[3]);
}
/** Updates the field relative position of the robot. */

24
wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Robot.java
View File

@@ -14,8 +14,8 @@ public class Robot extends TimedRobot {
private final Drivetrain m_swerve = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
@Override
@@ -30,18 +30,18 @@ public class Robot extends TimedRobot {
}
private void driveWithJoystick(boolean fieldRelative) {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xSpeed =
-m_xspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
* Drivetrain.kMaxSpeed;
final var xVelocity =
-m_xVelocityLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
* Drivetrain.kMaxVelocity;
// Get the y speed or sideways/strafe speed. We are inverting this because
// Get the y velocity or sideways/strafe velocity. We are inverting this because
// we want a positive value when we pull to the left. Xbox controllers
// return positive values when you pull to the right by default.
final var ySpeed =
-m_yspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
* Drivetrain.kMaxSpeed;
final var yVelocity =
-m_yVelocityLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
* Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
@@ -49,8 +49,8 @@ public class Robot extends TimedRobot {
// the right by default.
final var rot =
-m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
* Drivetrain.kMaxAngularSpeed;
* Drivetrain.kMaxAngularVelocity;
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
m_swerve.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
}
}

45
wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/SwerveModule.java
View File

@@ -11,14 +11,14 @@ import org.wpilib.math.controller.ProfiledPIDController;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.kinematics.SwerveModuleVelocity;
import org.wpilib.math.trajectory.TrapezoidProfile;
public class SwerveModule {
private static final double kWheelRadius = 0.0508;
private static final int kEncoderResolution = 4096;
private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularVelocity;
private static final double kModuleMaxAngularAcceleration =
2 * Math.PI; // radians per second squared
@@ -82,16 +82,6 @@ public class SwerveModule {
m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
}
/**
* Returns the current state of the module.
*
* @return The current state of the module.
*/
public SwerveModuleState getState() {
return new SwerveModuleState(
m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
}
/**
* Returns the current position of the module.
*
@@ -103,32 +93,43 @@ public class SwerveModule {
}
/**
* Sets the desired state for the module.
* Returns the current velocity of the module.
*
* @param desiredState Desired state with speed and angle.
* @return The current velocity of the module.
*/
public void setDesiredState(SwerveModuleState desiredState) {
public SwerveModuleVelocity getVelocity() {
return new SwerveModuleVelocity(
m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
}
/**
* Sets the desired velocity for the module.
*
* @param desiredVelocity Desired velocity.
*/
public void setDesiredVelocity(SwerveModuleVelocity desiredVelocity) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference state to avoid spinning further than 90 degrees
desiredState.optimize(encoderRotation);
// Optimize the desired velocity to avoid spinning further than 90 degrees
desiredVelocity.optimize(encoderRotation);
// Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
// Scale velocity by cosine of angle error. This scales down movement perpendicular to the
// desired
// direction of travel that can occur when modules change directions. This results in smoother
// driving.
desiredState.cosineScale(encoderRotation);
desiredVelocity.cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredVelocity.velocity);
final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed);
final double driveFeedforward = m_driveFeedforward.calculate(desiredVelocity.velocity);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), desiredState.angle.getRadians());
m_turningEncoder.getDistance(), desiredVelocity.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);

34
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -8,7 +8,7 @@ import org.wpilib.hardware.imu.OnboardIMU;
import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.linalg.VecBuilder;
@@ -17,8 +17,8 @@ import org.wpilib.system.Timer;
/** Represents a swerve drive style drivetrain. */
public class Drivetrain {
public static final double kMaxSpeed = 3.0; // 3 meters per second
public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
public static final double kMaxVelocity = 3.0; // 3 meters per second
public static final double kMaxAngularVelocity = Math.PI; // 1/2 rotation per second
private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
@@ -59,28 +59,28 @@ public class Drivetrain {
/**
* Method to drive the robot using joystick info.
*
* @param xSpeed Speed of the robot in the x direction (forward).
* @param ySpeed Speed of the robot in the y direction (sideways).
* @param xVelocity Velocity of the robot in the x direction (forward).
* @param yVelocity Velocity of the robot in the y direction (sideways).
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
* @param fieldRelative Whether the provided x and y velocities are relative to the field.
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double period) {
var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
double xVelocity, double yVelocity, double rot, boolean fieldRelative, double period) {
var chassisVelocities = new ChassisVelocities(xVelocity, yVelocity, rot);
if (fieldRelative) {
chassisSpeeds =
chassisSpeeds.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
chassisVelocities =
chassisVelocities.toRobotRelative(m_poseEstimator.getEstimatedPosition().getRotation());
}
chassisSpeeds = chassisSpeeds.discretize(period);
chassisVelocities = chassisVelocities.discretize(period);
var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);
var states = m_kinematics.toWheelVelocities(chassisVelocities);
SwerveDriveKinematics.desaturateWheelVelocities(states, kMaxVelocity);
m_frontLeft.setDesiredState(states[0]);
m_frontRight.setDesiredState(states[1]);
m_backLeft.setDesiredState(states[2]);
m_backRight.setDesiredState(states[3]);
m_frontLeft.setDesiredVelocity(states[0]);
m_frontRight.setDesiredVelocity(states[1]);
m_backLeft.setDesiredVelocity(states[2]);
m_backRight.setDesiredVelocity(states[3]);
}
/** Updates the field relative position of the robot. */

19
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Robot.java
View File

@@ -13,8 +13,8 @@ public class Robot extends TimedRobot {
private final Drivetrain m_swerve = new Drivetrain();
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_xVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_yVelocityLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
@Override
@@ -29,21 +29,24 @@ public class Robot extends TimedRobot {
}
private void driveWithJoystick(boolean fieldRelative) {
// Get the x speed. We are inverting this because gamepads return
// Get the x velocity. We are inverting this because gamepads return
// negative values when we push forward.
final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
final var xVelocity =
-m_xVelocityLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxVelocity;
// Get the y speed or sideways/strafe speed. We are inverting this because
// Get the y velocity or sideways/strafe velocity. We are inverting this because
// we want a positive value when we pull to the left. Gamepads
// return positive values when you pull to the right by default.
final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
final var yVelocity =
-m_yVelocityLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxVelocity;
// Get the rate of angular rotation. We are inverting this because we want a
// positive value when we pull to the left (remember, CCW is positive in
// mathematics). Gamepads return positive values when you pull to
// the right by default.
final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
final var rot =
-m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularVelocity;
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
m_swerve.drive(xVelocity, yVelocity, rot, fieldRelative, getPeriod());
}
}

27
wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -11,14 +11,14 @@ import org.wpilib.math.controller.ProfiledPIDController;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.kinematics.SwerveModuleVelocity;
import org.wpilib.math.trajectory.TrapezoidProfile;
public class SwerveModule {
private static final double kWheelRadius = 0.0508;
private static final int kEncoderResolution = 4096;
private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularVelocity;
private static final double kModuleMaxAngularAcceleration =
2 * Math.PI; // radians per second squared
@@ -87,8 +87,8 @@ public class SwerveModule {
*
* @return The current state of the module.
*/
public SwerveModuleState getState() {
return new SwerveModuleState(
public SwerveModuleVelocity getState() {
return new SwerveModuleVelocity(
m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.getDistance()));
}
@@ -105,29 +105,30 @@ public class SwerveModule {
/**
* Sets the desired state for the module.
*
* @param desiredState Desired state with speed and angle.
* @param desiredVelocity Desired velocity.
*/
public void setDesiredState(SwerveModuleState desiredState) {
public void setDesiredVelocity(SwerveModuleVelocity desiredVelocity) {
var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
// Optimize the reference state to avoid spinning further than 90 degrees
desiredState.optimize(encoderRotation);
// Optimize the reference velocity to avoid spinning further than 90 degrees
desiredVelocity.optimize(encoderRotation);
// Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
// Scale velocity by cosine of angle error. This scales down movement perpendicular to the
// desired
// direction of travel that can occur when modules change directions. This results in smoother
// driving.
desiredState.cosineScale(encoderRotation);
desiredVelocity.cosineScale(encoderRotation);
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredVelocity.velocity);
final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed);
final double driveFeedforward = m_driveFeedforward.calculate(desiredVelocity.velocity);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(
m_turningEncoder.getDistance(), desiredState.angle.getRadians());
m_turningEncoder.getDistance(), desiredVelocity.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);

4
wpilibjExamples/src/main/java/org/wpilib/examples/sysidroutine/Constants.java
View File

@@ -58,7 +58,7 @@ public final class Constants {
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;
public static final double kFeederVelocity = 0.5;
}
public static final class IntakeConstants {
@@ -74,7 +74,7 @@ public final class Constants {
public static final class AutoConstants {
public static final double kTimeout = 3;
public static final double kDriveDistance = 2; // m
public static final double kDriveSpeed = 0.5;
public static final double kDriveVelocity = 0.5;
}
public static final class OIConstants {

8
wpilibjExamples/src/main/java/org/wpilib/examples/sysidroutine/subsystems/Drive.java
View File

@@ -27,7 +27,7 @@ public class Drive extends SubsystemBase {
// The robot's drive
private final DifferentialDrive m_drive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
// The left-side drive encoder
private final Encoder m_leftEncoder =
@@ -60,13 +60,15 @@ public class Drive extends SubsystemBase {
// Record a frame for the left motors. Since these share an encoder, we consider
// the entire group to be one motor.
log.motor("drive-left")
.voltage(Volts.of(m_leftMotor.get() * RobotController.getBatteryVoltage()))
.voltage(
Volts.of(m_leftMotor.getDutyCycle() * RobotController.getBatteryVoltage()))
.linearPosition(Meters.of(m_leftEncoder.getDistance()))
.linearVelocity(MetersPerSecond.of(m_leftEncoder.getRate()));
// Record a frame for the right motors. Since these share an encoder, we consider
// the entire group to be one motor.
log.motor("drive-right")
.voltage(Volts.of(m_rightMotor.get() * RobotController.getBatteryVoltage()))
.voltage(
Volts.of(m_rightMotor.getDutyCycle() * RobotController.getBatteryVoltage()))
.linearPosition(Meters.of(m_rightEncoder.getDistance()))
.linearVelocity(MetersPerSecond.of(m_rightEncoder.getRate()));
},

16
wpilibjExamples/src/main/java/org/wpilib/examples/sysidroutine/subsystems/Shooter.java
View File

@@ -46,7 +46,9 @@ public class Shooter extends SubsystemBase {
log -> {
// Record a frame for the shooter motor.
log.motor("shooter-wheel")
.voltage(Volts.of(m_shooterMotor.get() * RobotController.getBatteryVoltage()))
.voltage(
Volts.of(
m_shooterMotor.getDutyCycle() * RobotController.getBatteryVoltage()))
.angularPosition(Rotations.of(m_shooterEncoder.getDistance()))
.angularVelocity(RotationsPerSecond.of(m_shooterEncoder.getRate()));
},
@@ -70,15 +72,15 @@ public class Shooter extends SubsystemBase {
/**
* Returns a command that runs the shooter at a specifc velocity.
*
* @param shooterSpeed The commanded shooter wheel speed in rotations per second
* @param shooterVelocity The commanded shooter wheel velocity in rotations per second
*/
public Command runShooter(DoubleSupplier shooterSpeed) {
// Run shooter wheel at the desired speed using a PID controller and feedforward.
public Command runShooter(DoubleSupplier shooterVelocity) {
// Run shooter wheel at the desired velocity using a PID controller and feedforward.
return run(() -> {
m_shooterMotor.setVoltage(
m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble())
+ m_shooterFeedforward.calculate(shooterSpeed.getAsDouble()));
m_feederMotor.set(ShooterConstants.kFeederSpeed);
m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterVelocity.getAsDouble())
+ m_shooterFeedforward.calculate(shooterVelocity.getAsDouble()));
m_feederMotor.setDutyCycle(ShooterConstants.kFeederVelocity);
})
.finallyDo(
() -> {

2
wpilibjExamples/src/main/java/org/wpilib/examples/tankdrive/Robot.java
View File

@@ -29,7 +29,7 @@ public class Robot extends TimedRobot {
// gearbox is constructed, you might have to invert the left side instead.
m_rightMotor.setInverted(true);
m_robotDrive = new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
m_robotDrive = new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
m_leftStick = new Joystick(0);
m_rightStick = new Joystick(1);

2
wpilibjExamples/src/main/java/org/wpilib/examples/tankdrivegamepad/Robot.java
View File

@@ -18,7 +18,7 @@ public class Robot extends TimedRobot {
private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
private final Gamepad m_driverController = new Gamepad(0);
/** Called once at the beginning of the robot program. */

2
wpilibjExamples/src/main/java/org/wpilib/examples/unittest/Constants.java
View File

@@ -20,6 +20,6 @@ public final class Constants {
public static final int kPistonFwdChannel = 0;
public static final int kPistonRevChannel = 1;
public static final double kIntakeSpeed = 0.5;
public static final double kIntakeVelocity = 0.5;
}
}

2
wpilibjExamples/src/main/java/org/wpilib/examples/unittest/Robot.java
View File

@@ -23,7 +23,7 @@ public class Robot extends TimedRobot {
public void teleopPeriodic() {
// Activate the intake while the trigger is held
if (m_joystick.getTrigger()) {
m_intake.activate(IntakeConstants.kIntakeSpeed);
m_intake.activate(IntakeConstants.kIntakeVelocity);
} else {
m_intake.activate(0);
}

8
wpilibjExamples/src/main/java/org/wpilib/examples/unittest/subsystems/Intake.java
View File

@@ -29,14 +29,14 @@ public class Intake implements AutoCloseable {
public void retract() {
m_piston.set(DoubleSolenoid.Value.kReverse);
m_motor.set(0); // turn off the motor
m_motor.setDutyCycle(0); // turn off the motor
}
public void activate(double speed) {
public void activate(double velocity) {
if (isDeployed()) {
m_motor.set(speed);
m_motor.setDutyCycle(velocity);
} else { // if piston isn't open, do nothing
m_motor.set(0);
m_motor.setDutyCycle(0);
}
}

16
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/ArcadeDrive.java
View File

@@ -10,23 +10,23 @@ import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
public class ArcadeDrive extends Command {
private final Drivetrain m_drivetrain;
private final Supplier<Double> m_xaxisSpeedSupplier;
private final Supplier<Double> m_xaxisVelocitySupplier;
private final Supplier<Double> m_zaxisRotateSupplier;
/**
* Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier
* lambdas. This command does not terminate.
* Creates a new ArcadeDrive. This command will drive your robot according to the velocity
* supplier lambdas. This command does not terminate.
*
* @param drivetrain The drivetrain subsystem on which this command will run
* @param xaxisSpeedSupplier Lambda supplier of forward/backward speed
* @param zaxisRotateSupplier Lambda supplier of rotational speed
* @param xaxisVelocitySupplier Lambda supplier of forward/backward velocity
* @param zaxisRotateSupplier Lambda supplier of rotational velocity
*/
public ArcadeDrive(
Drivetrain drivetrain,
Supplier<Double> xaxisSpeedSupplier,
Supplier<Double> xaxisVelocitySupplier,
Supplier<Double> zaxisRotateSupplier) {
m_drivetrain = drivetrain;
m_xaxisSpeedSupplier = xaxisSpeedSupplier;
m_xaxisVelocitySupplier = xaxisVelocitySupplier;
m_zaxisRotateSupplier = zaxisRotateSupplier;
addRequirements(drivetrain);
}
@@ -38,7 +38,7 @@ public class ArcadeDrive extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drivetrain.arcadeDrive(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get());
m_drivetrain.arcadeDrive(m_xaxisVelocitySupplier.get(), m_zaxisRotateSupplier.get());
}
// Called once the command ends or is interrupted.

12
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/DriveDistance.java
View File

@@ -10,19 +10,19 @@ import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
public class DriveDistance extends Command {
private final Drivetrain m_drive;
private final double m_distance;
private final double m_speed;
private final double m_velocity;
/**
* Creates a new DriveDistance. This command will drive your your robot for a desired distance at
* a desired speed.
* a desired velocity.
*
* @param speed The speed at which the robot will drive
* @param velocity The velocity at which the robot will drive
* @param inches The number of inches the robot will drive
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveDistance(double speed, double inches, Drivetrain drive) {
public DriveDistance(double velocity, double inches, Drivetrain drive) {
m_distance = inches;
m_speed = speed;
m_velocity = velocity;
m_drive = drive;
addRequirements(drive);
}
@@ -37,7 +37,7 @@ public class DriveDistance extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_speed, 0);
m_drive.arcadeDrive(m_velocity, 0);
}
// Called once the command ends or is interrupted.

12
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/DriveTime.java
View File

@@ -9,19 +9,19 @@ import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
public class DriveTime extends Command {
private final double m_duration;
private final double m_speed;
private final double m_velocity;
private final Drivetrain m_drive;
private long m_startTime;
/**
* Creates a new DriveTime. This command will drive your robot for a desired speed and time.
* Creates a new DriveTime. This command will drive your robot for a desired velocity and time.
*
* @param speed The speed which the robot will drive. Negative is in reverse.
* @param velocity The velocity which the robot will drive. Negative is in reverse.
* @param time How much time to drive in seconds
* @param drive The drivetrain subsystem on which this command will run
*/
public DriveTime(double speed, double time, Drivetrain drive) {
m_speed = speed;
public DriveTime(double velocity, double time, Drivetrain drive) {
m_velocity = velocity;
m_duration = time * 1000;
m_drive = drive;
addRequirements(drive);
@@ -37,7 +37,7 @@ public class DriveTime extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(m_speed, 0);
m_drive.arcadeDrive(m_velocity, 0);
}
// Called once the command ends or is interrupted.

12
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/TurnDegrees.java
View File

@@ -10,19 +10,19 @@ import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
public class TurnDegrees extends Command {
private final Drivetrain m_drive;
private final double m_degrees;
private final double m_speed;
private final double m_velocity;
/**
* Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
* degrees) and rotational speed.
* degrees) and rotational velocity.
*
* @param speed The speed which the robot will drive. Negative is in reverse.
* @param velocity The velocity which the robot will drive. Negative is in reverse.
* @param degrees Degrees to turn. Leverages encoders to compare distance.
* @param drive The drive subsystem on which this command will run
*/
public TurnDegrees(double speed, double degrees, Drivetrain drive) {
public TurnDegrees(double velocity, double degrees, Drivetrain drive) {
m_degrees = degrees;
m_speed = speed;
m_velocity = velocity;
m_drive = drive;
addRequirements(drive);
}
@@ -38,7 +38,7 @@ public class TurnDegrees extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_speed);
m_drive.arcadeDrive(0, m_velocity);
}
// Called once the command ends or is interrupted.

12
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/commands/TurnTime.java
View File

@@ -9,23 +9,23 @@ import org.wpilib.examples.xrpreference.subsystems.Drivetrain;
/*
* Creates a new TurnTime command. This command will turn your robot for a
* desired rotational speed and time.
* desired rotational velocity and time.
*/
public class TurnTime extends Command {
private final double m_duration;
private final double m_rotationalSpeed;
private final double m_rotationalVelocity;
private final Drivetrain m_drive;
private long m_startTime;
/**
* Creates a new TurnTime.
*
* @param speed The speed which the robot will turn. Negative is in reverse.
* @param velocity The velocity which the robot will turn. Negative is in reverse.
* @param time How much time to turn in seconds
* @param drive The drive subsystem on which this command will run
*/
public TurnTime(double speed, double time, Drivetrain drive) {
m_rotationalSpeed = speed;
public TurnTime(double velocity, double time, Drivetrain drive) {
m_rotationalVelocity = velocity;
m_duration = time * 1000;
m_drive = drive;
addRequirements(drive);
@@ -41,7 +41,7 @@ public class TurnTime extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_drive.arcadeDrive(0, m_rotationalSpeed);
m_drive.arcadeDrive(0, m_rotationalVelocity);
}
// Called once the command ends or is interrupted.

6
wpilibjExamples/src/main/java/org/wpilib/examples/xrpreference/subsystems/Drivetrain.java
View File

@@ -30,7 +30,7 @@ public class Drivetrain extends SubsystemBase {
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
// Set up the XRPGyro
private final XRPGyro m_gyro = new XRPGyro();
@@ -51,8 +51,8 @@ public class Drivetrain extends SubsystemBase {
resetEncoders();
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {

2
wpilibjExamples/src/main/java/org/wpilib/examples/xrptimed/Robot.java
View File

@@ -20,7 +20,7 @@ public class Robot extends TimedRobot {
private final XRPMotor m_leftDrive = new XRPMotor(0);
private final XRPMotor m_rightDrive = new XRPMotor(1);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftDrive::set, m_rightDrive::set);
new DifferentialDrive(m_leftDrive::setDutyCycle, m_rightDrive::setDutyCycle);
// Assumes a gamepad plugged into channel 0
private final Joystick m_controller = new Joystick(0);
private final Timer m_timer = new Timer();

5
wpilibjExamples/src/main/java/org/wpilib/snippets/encoderdrive/Robot.java
View File

@@ -21,7 +21,8 @@ public class Robot extends TimedRobot {
Spark m_leftFollower = new Spark(1);
Spark m_rightLeader = new Spark(2);
Spark m_rightFollower = new Spark(3);
DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader::set, m_rightLeader::set);
DifferentialDrive m_drive =
new DifferentialDrive(m_leftLeader::setDutyCycle, m_rightLeader::setDutyCycle);
/** Called once at the beginning of the robot program. */
public Robot() {
@@ -36,7 +37,7 @@ public class Robot extends TimedRobot {
m_rightLeader.addFollower(m_rightFollower);
}
/** Drives forward at half speed until the robot has moved 5 feet, then stops. */
/** Drives forward at half velocity until the robot has moved 5 feet, then stops. */
@Override
public void autonomousPeriodic() {
if (m_encoder.getDistance() < 5.0) {

6
wpilibjExamples/src/main/java/org/wpilib/snippets/encoderhoming/Robot.java
View File

@@ -20,15 +20,15 @@ public class Robot extends TimedRobot {
DigitalInput m_limit = new DigitalInput(2);
/**
* Runs the motor backwards at half speed until the limit switch is pressed then turn off the
* Runs the motor backwards at half velocity until the limit switch is pressed then turn off the
* motor and reset the encoder.
*/
@Override
public void autonomousPeriodic() {
if (!m_limit.get()) {
m_spark.set(-0.5);
m_spark.setDutyCycle(-0.5);
} else {
m_spark.set(0.0);
m_spark.setDutyCycle(0.0);
m_encoder.reset();
}
}

22
wpilibjExamples/src/main/java/org/wpilib/snippets/limitswitch/Robot.java
View File

@@ -21,30 +21,30 @@ public class Robot extends TimedRobot {
@Override
public void teleopPeriodic() {
setMotorSpeed(m_joystick.getRawAxis(2));
setMotorVelocity(m_joystick.getRawAxis(2));
}
/**
* Sets the motor speed based on joystick input while respecting limit switches.
* Sets the motor velocity based on joystick input while respecting limit switches.
*
* @param speed the desired speed of the motor, positive for up and negative for down
* @param velocity the desired velocity of the motor, positive for up and negative for down
*/
public void setMotorSpeed(double speed) {
if (speed > 0) {
public void setMotorVelocity(double velocity) {
if (velocity > 0) {
if (m_toplimitSwitch.get()) {
// We are going up and top limit is tripped so stop
m_motor.set(0);
m_motor.setDutyCycle(0);
} else {
// We are going up but top limit is not tripped so go at commanded speed
m_motor.set(speed);
// We are going up but top limit is not tripped so go at commanded velocity
m_motor.setDutyCycle(velocity);
}
} else {
if (m_bottomlimitSwitch.get()) {
// We are going down and bottom limit is tripped so stop
m_motor.set(0);
m_motor.setDutyCycle(0);
} else {
// We are going down but bottom limit is not tripped so go at commanded speed
m_motor.set(speed);
// We are going down but bottom limit is not tripped so go at commanded velocity
m_motor.setDutyCycle(velocity);
}
}
}

6
wpilibjExamples/src/main/java/org/wpilib/snippets/profiledpidfeedforward/Robot.java
View File

@@ -22,7 +22,7 @@ public class Robot extends TimedRobot {
private final Encoder m_encoder = new Encoder(0, 1);
private final PWMSparkMax m_motor = new PWMSparkMax(0);
double m_lastSpeed;
double m_lastVelocity;
/** Called once at the beginning of the robot program. */
public Robot() {
@@ -37,8 +37,8 @@ public class Robot extends TimedRobot {
public void goToPosition(double goalPosition) {
double pidVal = m_controller.calculate(m_encoder.getDistance(), goalPosition);
m_motor.setVoltage(
pidVal + m_feedforward.calculate(m_lastSpeed, m_controller.getSetpoint().velocity));
m_lastSpeed = m_controller.getSetpoint().velocity;
pidVal + m_feedforward.calculate(m_lastVelocity, m_controller.getSetpoint().velocity));
m_lastVelocity = m_controller.getSetpoint().velocity;
}
@Override

6
wpilibjExamples/src/main/java/org/wpilib/templates/romicommandv2/subsystems/RomiDrivetrain.java
View File

@@ -25,7 +25,7 @@ public class RomiDrivetrain extends SubsystemBase {
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
/** Creates a new RomiDrivetrain. */
public RomiDrivetrain() {
@@ -38,8 +38,8 @@ public class RomiDrivetrain extends SubsystemBase {
m_rightMotor.setInverted(true);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {

6
wpilibjExamples/src/main/java/org/wpilib/templates/romieducational/RomiDrivetrain.java
View File

@@ -24,7 +24,7 @@ public class RomiDrivetrain {
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
/** Creates a new RomiDrivetrain. */
public RomiDrivetrain() {
@@ -40,8 +40,8 @@ public class RomiDrivetrain {
m_diffDrive.setSafetyEnabled(false);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {

6
wpilibjExamples/src/main/java/org/wpilib/templates/romitimed/RomiDrivetrain.java
View File

@@ -24,7 +24,7 @@ public class RomiDrivetrain {
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
/** Creates a new RomiDrivetrain. */
public RomiDrivetrain() {
@@ -37,8 +37,8 @@ public class RomiDrivetrain {
m_rightMotor.setInverted(true);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {

6
wpilibjExamples/src/main/java/org/wpilib/templates/xrpcommandv2/subsystems/XRPDrivetrain.java
View File

@@ -28,7 +28,7 @@ public class XRPDrivetrain extends SubsystemBase {
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
@@ -41,8 +41,8 @@ public class XRPDrivetrain extends SubsystemBase {
m_rightMotor.setInverted(true);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {

6
wpilibjExamples/src/main/java/org/wpilib/templates/xrpeducational/XRPDrivetrain.java
View File

@@ -27,7 +27,7 @@ public class XRPDrivetrain {
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
@@ -40,8 +40,8 @@ public class XRPDrivetrain {
m_rightMotor.setInverted(true);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {

6
wpilibjExamples/src/main/java/org/wpilib/templates/xrptimed/XRPDrivetrain.java
View File

@@ -27,7 +27,7 @@ public class XRPDrivetrain {
// Set up the differential drive controller
private final DifferentialDrive m_diffDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
new DifferentialDrive(m_leftMotor::setDutyCycle, m_rightMotor::setDutyCycle);
/** Creates a new XRPDrivetrain. */
public XRPDrivetrain() {
@@ -40,8 +40,8 @@ public class XRPDrivetrain {
m_rightMotor.setInverted(true);
}
public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
public void arcadeDrive(double xaxisVelocity, double zaxisRotate) {
m_diffDrive.arcadeDrive(xaxisVelocity, zaxisRotate);
}
public void resetEncoders() {