ShooterSubsystem design feedback

Technical Programming
1

I’m looking for feedback on the following setup. Any thoughts, advice, etc. would be appreciated.

I’ve built a NEOVortexShooterSubsystem along with an abstract AngularSysIdSubsystem that is the parent class of NEOVortexShooterSubsystem.

AngularSysIdSubsystem inherits from SubsystemBase.

The Shooter subsystem uses the TimedRobot addPeriodic method to run the control loop at a different frequency if so desired. The control loop uses the WPILib PIDController, SimpleMotorFeedforward and SlewRateLimiter classes along with some clamping of the voltage to a maxVoltage value. Since this is REV hardware I’ve chosen not to use its closed loop control because I have no way of controlling kS or even kA. Extending this to position mechanisms in the future also is problematic as well. Hence the use of the WPILib control loop classes.

Assume that everything has been configured prior to the constructor call of the Shooter and the units are in the frame of the mechanism itself. I’ve used a static factory class with a private constructor in order to minimize the usage of new keywords

I’ve created a PeriodicTask inner class to Robot.java that implements an interface called Periodically inspired by this post. Thanks SLAB_Mr.Thomas and Oblarg for the advice in this post.

The AngularSysIdSubsystem is just the SysIdRoutine example made into an inherited subsystem to help reduce code repetition.

I’m definitely mixing OOP and Functional method chaining practices/pradigms here especially with the Command factories. I haven’t completely polished this yet. I really like the :: operator over the lambda notation in some cases but have to make the methods exist somewhere to utilize it. I’m sure opinions will abound over which one is best. I’m welcome and appreciate that. Infinite diversity of thought helps one learn more :slight_smile: . I’ve found since teaching CS for a living that nearby parentheses can be problematic for students so I’m still working out how best to approach hence the mixed approach here.

Inspiration was also inspired from this post on best practices for Command Based Frameworks.

One thing to note. The loop is running on its own based off of the setpoint. The 3 shooter commands just set the setpoint target once for fixed speeds and continuously for constantly changing control. The third is just a way to change the internal tolerance.

The sysId commands have to be overridden in the shooter in order to prevent the commands running with the loop. Each sysId command sets the target to 0, waits until the shooter is at the target, disables the control loop, stops the motor, runs the sysId command, and then re-enables the loop. I’m sure a BooleanSupplier parameter for AngularSysIdSubsystem that tells sysId that its ready to run its commands along with a few Runnable parameters that handle stopping the motor and control loop would help reduce the need to override in the child classes and make this easier to extend.

Also I’ve toyed around with the idea of creating a ControlLoop interface with different subclasses to handle the different possible uses here.

Side Note: I know feature creep is always an issue, but could WPILib benefit from integrating PID and the 3 feedforward classes into some form of ClosedLoop class/interface setup that makes the implementation of control loop schemes more ergonomic.

Anyways here is the code. I’ve taken out the packages and import statements.

@FunctionalInterface
public interface Periodically {

    /**
     * @param callback - task to run periodically
     */
    void register(Runnable callback);
}

  public class PeriodicTask implements Periodically {

    private final double periodSeconds;
    private final double offsetSeconds;

    private PeriodicTask(double periodSeconds, double offsetSeconds) {
      this.periodSeconds = periodSeconds;
      this.offsetSeconds = offsetSeconds;
    }

    private PeriodicTask(double periodSeconds) {
      this(periodSeconds, 0.0);
    }

    @Override
    public void register(Runnable callback) {
      addPeriodic(callback, periodSeconds, offsetSeconds);
    }
  }

public abstract class AngularSysIdSubsystem extends SubsystemBase {

    private final SysIdRoutine sysIdRoutine;

    public AngularSysIdSubsystem(
            String name,
            Measure<Velocity<Voltage>> rampRate,
            Measure<Voltage> stepVoltage,
            Measure<Time> timeout,
            DoubleConsumer drive,
            DoubleSupplier appliedVoltage,
            DoubleSupplier currentAmps,
            DoubleSupplier angleRadians,
            DoubleSupplier angularVelocityRadPerSec,
            DoubleSupplier angularAccelerationRadPerSecSq) {
        setName(name);
        MutableMeasure<Voltage> sysIdAppliedVoltageMeasure = MutableMeasure.mutable(Volts.of(0.0));
        MutableMeasure<Current> sysIdCurrentMeasure = MutableMeasure.mutable(Amps.of(0.0));
        MutableMeasure<Angle> sysidPositionMeasure = MutableMeasure.mutable(Radians.of(0.0));
        MutableMeasure<Velocity<Angle>> sysidVelocityMeasure = MutableMeasure.mutable(RadiansPerSecond.of(0.0));
        MutableMeasure<Velocity<Velocity<Angle>>> sysidAccelerationMeasure = MutableMeasure
                .mutable(RadiansPerSecond.per(Second).of(0.0));

        Config config = new Config(
                rampRate,
                stepVoltage,
                timeout);

        Mechanism mechanism = new Mechanism(
                (volts) -> drive.accept(volts.in(Volts)),
                log -> {
                    log.motor(name)
                            .voltage(
                                    sysIdAppliedVoltageMeasure
                                            .mut_replace(appliedVoltage.getAsDouble(), Volts))
                            .current(
                                    sysIdCurrentMeasure
                                            .mut_replace(currentAmps.getAsDouble(), Amps))
                            .angularPosition(
                                    sysidPositionMeasure
                                            .mut_replace(angleRadians.getAsDouble(), Radians))
                            .angularVelocity(
                                    sysidVelocityMeasure
                                            .mut_replace(angularVelocityRadPerSec.getAsDouble(), RadiansPerSecond))
                            .angularAcceleration(sysidAccelerationMeasure
                                    .mut_replace(angularAccelerationRadPerSecSq.getAsDouble(),
                                            RadiansPerSecond.per(Second)));
                },
                this);
        this.sysIdRoutine = new SysIdRoutine(config, mechanism);
    }

    public Command sysIdQuasistaticForward() {
        return sysIdRoutine
                .quasistatic(Direction.kForward)
                .withName("shooter.sysIdQuasistatic.forward");
    }

    public Command sysIdQuasistaticReverse() {
        return sysIdRoutine
                .quasistatic(Direction.kReverse)
                .withName("shooter.sysIdQuasistatic.reverse");
    }

    public Command sysIdDynamicForward() {
        return sysIdRoutine
                .dynamic(Direction.kForward)
                .withName("shooter.sysIdDynamic.forward");
    }

    public Command sysIdDynamicReverse() {
        return sysIdRoutine
                .dynamic(Direction.kReverse)
                .withName("shooter.sysIdDynamic.reverse");
    }
}


public class NEOVortexShooter extends AngularSysIdSubsystem {

    private final CANSparkFlex canSparkFlex;
    private final PIDController pidController;
    private final SimpleMotorFeedforward simpleMotorFeedforward;
    private final SlewRateLimiter slewRateLimiter;
    private final double maxVoltage;
    private double targetVelocityRadPerSec;
    private boolean loopEnabled = true;

    public final Trigger atTargetVelocity;
    public final DoubleSupplier velocityRadPerSec;
    public final DoubleSupplier toleranceRadPerSec;

    public static NEOVortexShooter create(
            String name,
            Measure<Velocity<Voltage>> rampRate,
            Measure<Voltage> stepVoltage,
            Measure<Time> timeout,
            CANSparkFlex canSparkFlex,
            PIDController pidController,
            SimpleMotorFeedforward simpleMotorFeedforward,
            SlewRateLimiter slewRateLimiter,
            double maxVoltage,
            PeriodicTask periodicTask) {
        return new NEOVortexShooter(
                name,
                rampRate,
                stepVoltage,
                timeout,
                canSparkFlex,
                pidController,
                simpleMotorFeedforward,
                slewRateLimiter,
                maxVoltage,
                periodicTask);
    }

    private NEOVortexShooter(
            String name,
            Measure<Velocity<Voltage>> rampRate,
            Measure<Voltage> stepVoltage,
            Measure<Time> timeout,
            CANSparkFlex canSparkFlex,
            PIDController pidController,
            SimpleMotorFeedforward simpleMotorFeedforward,
            SlewRateLimiter slewRateLimiter,
            double maxVoltage,
            PeriodicTask periodicTask) {
        super(
                name,
                rampRate,
                stepVoltage,
                timeout,
                canSparkFlex::setVoltage,
                () -> canSparkFlex.getAppliedOutput() * canSparkFlex.getBusVoltage(),
                canSparkFlex::getOutputCurrent,
                canSparkFlex.getEncoder()::getPosition,
                canSparkFlex.getEncoder()::getVelocity,
                () -> 0.0);
        this.canSparkFlex = canSparkFlex;
        this.pidController = pidController;
        this.simpleMotorFeedforward = simpleMotorFeedforward;
        this.slewRateLimiter = slewRateLimiter;
        this.maxVoltage = maxVoltage;
        targetVelocityRadPerSec = 0.0;
        atTargetVelocity = new Trigger(pidController::atSetpoint);
        velocityRadPerSec = canSparkFlex.getEncoder()::getVelocity;
        toleranceRadPerSec = pidController::getPositionTolerance;
        periodicTask.register(this::controlLoop);

    }

    private void controlLoop() {
        if (loopEnabled) {
            double velocitySetpoint = slewRateLimiter.calculate(targetVelocityRadPerSec);
            double feedbackVoltage = pidController
                    .calculate(
                            canSparkFlex.getEncoder().getVelocity(),
                            velocitySetpoint);
            double feedforwardVoltage = simpleMotorFeedforward
                    .calculate(velocitySetpoint);
            double voltage = feedbackVoltage + feedforwardVoltage;
            voltage = MathUtil.clamp(voltage, -maxVoltage, maxVoltage);
            canSparkFlex.setVoltage(voltage);
        }
    }

    private void enableLoop() {
        loopEnabled = true;
    }

    private void disableLoop() {
        loopEnabled = false;
        canSparkFlex.stopMotor();
        pidController.reset();
        slewRateLimiter.reset(0.0);
    }

    public Command setTargetVelocity(DoubleSupplier targetVelocityRadPerSec) {
        return run(() -> this.targetVelocityRadPerSec = targetVelocityRadPerSec.getAsDouble())
                .withName("shooter.setTargetVelocity");
    }

    public Command setTargetVelocity(double targetVelocityRadPerSec) {
        String format = String.format("%.3f", targetVelocityRadPerSec);
        String name = "shooter.setTargetVelocity." + format + ".RadPerSec";
        return runOnce(() -> this.targetVelocityRadPerSec = targetVelocityRadPerSec)
                .withName(name);
    }

    public Command setTolerance(double toleranceRadPerSec) {
        String toleranceString = String.format("%.4f", toleranceRadPerSec);
        String name = "shooter.setTolerance." + toleranceString + ".RadPerSec";
        return runOnce(() -> pidController.setTolerance(toleranceRadPerSec))
                .withName(name);
    }

    @Override
    public Command sysIdQuasistaticForward() {
        return runOnce(() -> targetVelocityRadPerSec = 0.0)
                .andThen(Commands.waitUntil(atTargetVelocity))
                .andThen(this::disableLoop, this)
                .andThen(super.sysIdQuasistaticForward())
                .finallyDo(this::enableLoop);
    }

    @Override
    public Command sysIdQuasistaticReverse() {
        return runOnce(() -> targetVelocityRadPerSec = 0.0)
                .andThen(Commands.waitUntil(atTargetVelocity))
                .andThen(this::disableLoop, this)
                .andThen(super.sysIdQuasistaticReverse())
                .finallyDo(this::enableLoop);
    }

    @Override
    public Command sysIdDynamicForward() {
        return runOnce(() -> targetVelocityRadPerSec = 0.0)
                .andThen(Commands.waitUntil(atTargetVelocity))
                .andThen(this::disableLoop, this)
                .andThen(super.sysIdDynamicForward())
                .finallyDo(this::enableLoop);
    }

    @Override
    public Command sysIdDynamicReverse() {
        return runOnce(() -> targetVelocityRadPerSec = 0.0)
                .andThen(Commands.waitUntil(atTargetVelocity))
                .andThen(this::disableLoop, this)
                .andThen(super.sysIdDynamicReverse())
                .finallyDo(this::enableLoop);
    }

}

EDIT: Changed name to NEOVortexShooter
EDIT: Fixed sysId commands to wait until atTargetVelocity is true instead of the until decorators that were there before.
Thanks SLAB_Mr.Thomas

1 Like
2

This code has a nice separation of concerns between different levels.

I got stuck on one specific part:

because I started wondering if there could be a better way to tie a MutableMeasure to a DoubleSupplier. I threw together an untested gist for MutableMeasureSupplier, but the resulting code is only slightly simpler:

MutableMeasureSupplier<Voltage> sysIdAppliedVoltageMeasure = 
    new MutableMeasureSupplier<Units.Volts>(appliedVoltage);
...
.voltage(sysIdAppliedVoltageMeasure.get())
1 Like
3

I’d like to see that too. WPILIB now has at least six different feed forward classes[1], nine feedback controllers[2], five limiters and other filters[3], five or six derived commands[4], and two or three derived subsystems[5]. There’s considerable variety in how they are created and invoked. If I were doing it over from scratch, I think I’d be tempted to build it by writing factories that construct suppliers and then chaining them together.

  1. ArmFeedforward, ControlAffinePlantInversionFeedforward, DifferentialDriveFeedforward, ElevatorFeedforward, LinearPlantInversionFeedforward, SimpleMotorFeedforward ↩︎

  2. BangBangController, HolonomicDriveController, ImplicitModelFollower, LinearQuadraticRegulator, LTVDifferentialDriveController, LTVUnicycleController, PIDController, ProfiledPIDController, RamseteController ↩︎

  3. DifferentialDriveAccelerationLimiter, Debouncer, LinearFilter, MedianFilter, SlewRateLimiter ↩︎

  4. MecanumControllerCommand, PIDCommand, ProfiledPIDCommand, RamseteCommand, SwerveControllerCommand, TrapezoidProfileCommand ↩︎

  5. PIDSubsystem, ProfiledPIDSubsystem, TrapezoidProfileSubsystem ↩︎

4

Or through interfaces and abstract classes.

5

Disagree; no pre-plumbed solution of this can really improve the situation here. Doing the plumbing for the user forces user code into a very rigid framework, and typically results in lots of code being thrown out once the requirements no longer match the assumptions of the pre-plumbed implementation.

If you doubt this, frame out what you think an API that does this should look like, and mock up several different consumers of it. You’ll hit substantial problems with no clear solution almost immediately.

Combining a feedback and feedforward controller in user code involves instantiating two classes, calling a function on each of them, and adding the results together before passing the sum to a motor. This is not an onerous amount of code, and any config format rich enough to replace it with a single class will likely be both longer and less readable than the code it replaces.

1 Like
6

Clarification: I didn’t state that it could. I was wondering if it could. Partly my fault. I missed a question mark at the end :melting_face:

1 Like
7

Ah. Unfortunately I think the answer is “no,” for the reasons above. We’ve tried pre-plumbing combined controllers in WPILib before, and it never turned out as an improvement (which is why we don’t have it now).

8

I understand your point. I think it could benefit from more of a method chaining pattern/static factory system similar to the Command/Functional paradigm used when creating/composing command. Avoiding the new keyword etc.

Having the Feedforward classes implement a common interface probably wouldn’t hurt either or use static factories or both in some cases. Just spitballing here.

I think the feedforward classes need to be sendable like the PID controller.

9

What’s in this common interface? In general the inputs depend on the type of feedforward, so there’s not really much shared structure.

Even if the input structure were uniform, what could you include other than the calculate method? If it’s just one method, a functional interface will do and all you’re gaining is a mild improvement in naming.

1 Like
10

Probably not then. Maybe the method chaining setup though. Reduce usage of the new keyword

1 Like