Skip to content

Commit

Permalink
Update usages of old units API
Browse files Browse the repository at this point in the history
  • Loading branch information
SamCarlberg committed Sep 9, 2024
1 parent ab8a5e3 commit 1d70f6d
Show file tree
Hide file tree
Showing 7 changed files with 22 additions and 30 deletions.
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package edu.wpi.first.wpilibj.commandsv3;

import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Time;
import edu.wpi.first.units.measure.Time;
import java.util.Collections;
import java.util.Set;
import java.util.function.Consumer;
Expand Down Expand Up @@ -235,7 +234,7 @@ static CommandBuilder noRequirements(Consumer<Coroutine> impl) {
* values will result in the command running only once before being canceled.
* @return the timed out command.
*/
default Command withTimeout(Measure<Time> timeout) {
default Command withTimeout(Time timeout) {
return ParallelGroup.race(name(), this, new WaitCommand(timeout));
}

Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package edu.wpi.first.wpilibj.commandsv3;

import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Time;
import edu.wpi.first.units.measure.Time;
import java.util.Arrays;
import java.util.Collection;

Expand Down Expand Up @@ -120,7 +119,7 @@ default void awaitAny(Command... commands) {
*
* @param duration the duration of time to wait
*/
default void wait(Measure<Time> duration) {
default void wait(Time duration) {
await(new WaitCommand(duration));
}

Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package edu.wpi.first.wpilibj.commandsv3;

import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Time;
import edu.wpi.first.units.measure.Time;
import java.util.Set;
import java.util.function.Consumer;

Expand Down Expand Up @@ -57,7 +56,7 @@ public Command idle() {
return new IdleCommand(this);
}

public Command idle(Measure<Time> duration) {
public Command idle(Time duration) {
return idle().withTimeout(duration);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,16 +2,15 @@

import static edu.wpi.first.units.Units.Seconds;

import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Time;
import edu.wpi.first.units.measure.Time;
import edu.wpi.first.wpilibj.Timer;
import java.util.Set;

/** A command with no requirements that merely waits for a specified duration of time to elapse. */
public class WaitCommand implements Command {
private final Measure<Time> duration;
private final Time duration;

public WaitCommand(Measure<Time> duration) {
public WaitCommand(Time duration) {
this.duration = duration;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,9 @@
import static edu.wpi.first.units.Units.Seconds;

import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Time;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.units.measure.Time;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand Down Expand Up @@ -54,7 +52,7 @@ public static final class ShooterConstants {
public static final int kFeederMotorPort = 5;

public static final double kShooterFreeRPS = 5300;
public static final Measure<Velocity<Angle>> kShooterTarget = RotationsPerSecond.of(4000);
public static final AngularVelocity kShooterTarget = RotationsPerSecond.of(4000);
public static final double kShooterToleranceRPS = 50;

// These are not real PID gains, and will have to be tuned for your specific robot.
Expand All @@ -81,8 +79,8 @@ public static final class StorageConstants {
}

public static final class AutoConstants {
public static final Measure<Time> kTimeout = Seconds.of(3);
public static final Measure<Distance> kDriveDistance = Meters.of(2);
public static final Time kTimeout = Seconds.of(3);
public static final Distance kDriveDistance = Meters.of(2);
public static final double kDriveSpeed = 0.5;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,7 @@

import static edu.wpi.first.units.Units.Meters;

import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.commandsv3.Command;
Expand Down Expand Up @@ -86,7 +85,7 @@ public Command arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) {
* @param distance The distance to drive forward in meters
* @param speed The fraction of max speed at which to drive
*/
public Command driveDistanceCommand(Measure<Distance> distance, double speed) {
public Command driveDistanceCommand(Distance distance, double speed) {
double distanceMeters = distance.in(Meters);

return run((coroutine) -> {
Expand All @@ -99,6 +98,6 @@ public Command driveDistanceCommand(Measure<Distance> distance, double speed) {
}

m_drive.stopMotor();
}).named("DriveDistance[" + distanceMeters + "m, @" + speed + "]");
}).named("DriveDistance[" + distance.toLongString() + ", @" + speed + "]");
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,11 @@
package edu.wpi.first.wpilibj.examples.coroutines.subsystems;

import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Volts;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.commandsv3.RequireableResource;
import edu.wpi.first.wpilibj.examples.coroutines.Constants.ShooterConstants;
Expand Down Expand Up @@ -46,9 +45,9 @@ public Shooter() {
.named("Idle"));
}

public void ramp(Measure<Velocity<Angle>> setpoint) {
public void ramp(AngularVelocity setpoint) {
m_shooterMotor.set(
m_shooterFeedforward.calculate(setpoint.in(RotationsPerSecond))
m_shooterFeedforward.calculate(setpoint).in(Volts)
+ m_shooterFeedback.calculate(
m_shooterEncoder.getRate(), setpoint.in(RotationsPerSecond)));
}
Expand Down

0 comments on commit 1d70f6d

Please sign in to comment.