Package org.firstinspires.ftc.teamcode.fy23.processors

Class AccelLimiter

java.lang.Object
org.firstinspires.ftc.teamcode.fy23.processors.AccelLimiter
public class AccelLimiter extends Object
A suite of tools for controlling acceleration.

AccelLimiter (currently?) only works in one dimension. It can do three things:

  • Limit acceleration on one or more devices
    • When limiting on multiple devices, it maintains the proportions between them so that they behave the way you'd expect.
  • Calculate stopping distance
  • Ramp up and down to a target position
    • This will likely be split out into one or more separate classes in the near future.

There is one major rule to using AccelLimiter: Be consistent with your units! If you use ticks per second when creating an AccelLimiter object, you must continue using ticks per second in all subsequent uses of it.

Creating new AccelLimiter objects

Each thing you want to limit acceleration on should have its own instance of AccelLimiter. (This is because each instance only stores and uses one set of parameters.)

To create a new AccelLimiter object, you need to determine two things. You can use any unit of acceleration you want, as long as you use the same unit everywhere. You can technically use motor power if encoders are not available, but it is not recommended. More common units are meters per second per second (if you really need real-world relatability) or, ideally for FTC, ticks per second per second (t/s²). With t/s², the motor.getVelocity() and motor.setVelocity() methods will already work in the units we need. Anyway, the two things we need to determine are:

  • Maximum Acceleration
  • Maxinum change in velocity each time a request*() method is called
    • With this, if one iteration of the program loop takes longer than normal, the speed won't suddenly increase more than we expect.

Pass these parameters into the constructor:

@Override
 public void init() {
     double maxAccel = 100; // For our purposes, this is in t/s².
     double maxDeltaVEachLoop = 10;
     AccelLimiter exampleAL = new AccelLimiter(maxAccel, maxDeltaVEachLoop);
 // method to be continued...

Limiting Acceleration on a Single Device

There are two methods that you can call in every iteration of your loop to do this:

  • public double requestDeltaVel(double deltaVel, double currentTime)
    • Takes a change in velocity and limits it based on the given maximum acceleration and how much time has passed since it was called last.
  • public double requestVel(double newVel, double currentVel, double currentTime)
    • Does the same thing, but takes and returns the entire velocity rather than only taking the change. (This is shorthand. Under the hood, it figures out the change, limits that, then adds it back to the rest.)

Let's limit the acceleration on a single motor using the AccelLimiter we created earlier. We'll use a DcMotor named "exampleMotor" and the requestVel() method.

Remember that our max. acceleration is 100 t/s². 100 t/s will be our max. drive velocity, so it will take us 1 second to reach full speed.

Also, since our maxDeltaVEachLoop is 10 t/s², if a loop takes longer than 100ms, it won't increase its speed more than this limit on that loop.


     // We need to pass the time into the request*() methods so AccelLimiter knows how much time passes between calls.
     ElapsedTime stopwatch = new ElapsedTime();

     DcMotorEx exampleMotor = hardwareMap.get(DcMotorEx.class, "motor");
 } // end init()

 @Override
 public void loop() {
     // "Requested", as in what the driver wants to do
     // Let's say that our drive velocity runs from -100 to +100 ticks per second.
     double requestedDriveVelocity = (gamepad1.right_trigger - gamepad1.left_trigger) * 100;

     // The user request can jump from 0 to 100.
     // AccelLimiter will only change it as fast as maxAccel allows it to.
     // Here, if a loop takes 50ms, it will only change it by 5 t/s that loop.
     double limitedDriveVelocity = exampleAL.requestVel(requestedDriveVelocity, exampleMotor.getVelocity(), stopwatch.milliseconds());

     // If this is a real motor, you should see it gradually ramp up and down as you press the triggers.
     exampleMotor.setVelocity(limitedDriveVelocity);
 }

To limit acceleration on multiple motors that act independently (like PixelArm - the pivot and elevator motors do not affect each other), create a separate AccelLimiter object for each. If the motors are connected together, like they are on the drivebase, continue to the next section.

Limiting Acceleration on Multiple [Interconnected] Devices

This is done with the following method:


public List<Double> requestDeltaVelOnN(List deltaVelList, double currentTime)

(There is no requestVelOnN() method. You must use change in velocity.)

NOTE: This method is meant for multiple motors that are interconnected (the speed of one affects the rest). If your motors act independently (like PixelArm - the pivot and elevator motors do not affect each other), see the previous section.

This application is not common enough to include an example here. For an example implementation, look at the code of RRMecanumDriveImpl.applyDTS().

Calculating Stopping Distance

public double stoppingDistance(double currentVel, int resolution)

This method uses the maxAccel parameter already set for its AccelLimiter instance to determine how far the device will continue travelling while it's ramping down from the given velocity.

The resolution argument controls how precise the calculation is. A higher resolution yields a more accurate result but takes longer to calculate. 1000 seems to be a happy medium, getting close enough without taking too long.


double stoppingDistance = exampleAL.stoppingDistance(motor.getVelocity(), 1000)

stoppingDistance now (roughly) equals the distance we will continue to travel while we are ramping down, or the distance we need to have available to stop safely.

One potential application of this is ramping down towards the end of an actuator's range. The stopping distance will tell you at what point you must start slowing down.


Let's consider an example that lets us easily test our example: Our maximum acceleration is 100 t/s², and we're going 100 t/s. The line of our deceleration will cut the square of 100 t/s over 1 second in half, making it a triangle with half the area. In other words, we will tarvel 50 ticks while we're still ramping down, or half the distance we would have traveled at full speed. That is our stopping distance. If we built the example above correctly, stoppingDistance should roughly equal 50.

Also, our logic with the graph is the same logic behind a faster way to calculate the stopping distance for linear acceleration:


public double simpleStoppingDistance(double currentVel)

Currently, AccelLimiter only supports linear acceleration, so you may as well use this method. The other methods (which integrate the area under the line of acceleration), however, may enable new functionality in the future (i.e. other acceleration curves).

Ramping Up/Down to a Target Position

If you're doing this on a single motor, you do not want to use AccelLimiter to do this. Use the SDK's DcMotor.setTargetPosition() method instead.

That said, if you have a fun issue like your motor's encoder counting backwards and the SD won't work for you, this might be a stopgap solution until you can fix the motor. What this is really made for, however, is more complex applications such as moving the entire drivetran (4 motors with different encoder positions and rotation directions) until a target position is reached on one of the dead wheels.

There is no blocking version of this (yet?). It must be done asynchronously.


public void setupRampAlongDistance(double currentPos)

You can call this at any time to set the target position and the highest speed at which it will travel (perhaps we should call it "cruising speed"). This will reset the AccelLimiter object (!) and put it in the correct internal state for later updateRampAlongDistance() calls to work correctly.


public double updateRampALongDistance(double currentPos)

Call this on every loop after setting up the ramp using the previous method. It will return the velocity that you should send to the motor each loop. See the basic example below:


 // We want to go to encoder position 1000 at 100 t/s.
 // We'd put this in our init().
 exampleAL.setupRampAlongDistance(1000, 100);

 // Here, we give it our position and it gives us the velocity.
 // We'd put this in our loop().
 double applyVel = exampleAL.updateRampAlongDistance(motor.getCurrentPosition());

 // Here you could do any additional processing on the velocity before sending it to the motor if necessary.

 motor.setVelocity(applyVel);

  • Constructor Summary

    Constructors
    Constructor
    Description
    AccelLimiter(double maxAccel, double maxDeltaVEachLoop)
    Create an AccelLimiter object.

  • Method Summary

    Modifier and Type
    Method
    Description
    double
    getMaxAccel()
    Get the maximum acceleration.
    double
    getMaxDeltaVEachLoop()
    Get the maximum change in velocity each loop.
    double
    getMaxDeltaVThisLoop(double currentTime)
    How much can you change your velocity this loop given the maximum acceleration and how long it's been since last time we calculated this? (Will always return 0 while it initializes the first time it's called since this AccelLimiter was created or reset)
    double
    requestDeltaVel(double reqDeltaV, double currentTime)
    Request the desired change in velocity, and this will return how much velocity you can safely add to your current velocity given the parameters you entered into the constructor.
    List<Double>
    requestDeltaVelOnN(List<Double> deltaVelList, double currentTime)
    Request velocity changes on many actuators, and limit them while maintaining the proportions between them.
    double
    requestVel(double newVel, double currentVel, double currentTime)
    Request the desired final velocity, and this will return the velocity that you can safely go now given the parameters you entered into the constructor.
    void
    reset()
    Call this when you're done with your task and want to use this object for something else.
    void
    setParameters(double maxAccel, double maxDeltaVEachLoop)
    Change the parameters of this AccelLimiter instance.
    void
    setupRampToTarget(double targetPos, double maxVelocity)
    Sets up ramping up to maxVelocity and back down along the specified distance.
    void
    setupRampToTarget(double targetPos, double maxVelocity, com.qualcomm.robotcore.util.ElapsedTime stopwatch)
    Used for dependency injection in UnitTests
    double
    simpleStoppingDistance(double initialVel)
    A much faster stopping distance calculation that can be used for linear acceleration (which is all AccelLimiter supports at the moment anyway).
    double
    stoppingDistance(double initialVel, int resolution)
    How much distance is needed to stop from the given initial velocity at the maximum acceleration set for your AccelLimiter instance? Note that negative velocities still return positive stopping distances.
    double
    updateRampToTarget(double currentPos)
    Run this every loop.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Constructor Details

    • AccelLimiter

      public AccelLimiter(double maxAccel, double maxDeltaVEachLoop)
      Create an AccelLimiter object. (You must do this - the methods are not static.) It will maintain its state until it is reset (using the reset() method).
      Parameters:
      maxAccel - The maximum acceleration (in any unit you want, but we usually use meters per second)
      maxDeltaVEachLoop - The maximum change in velocity each loop (prevents a sudden velocity change / jerk if a loop takes too long)

  • Method Details

    • setParameters

      public void setParameters(double maxAccel, double maxDeltaVEachLoop)
      Change the parameters of this AccelLimiter instance.
      Parameters:
      maxAccel - The maximum acceleration (in any unit you want, but we usually use meters per second)
      maxDeltaVEachLoop - The maximum change in velocity each loop (prevents a sudden velocity change / jerk if a loop takes too long)

    • getMaxAccel

      public double getMaxAccel()
      Get the maximum acceleration. (You set this in the constructor.)

    • getMaxDeltaVEachLoop

      public double getMaxDeltaVEachLoop()
      Get the maximum change in velocity each loop. (You set this in the constructor.)

    • requestVel

      public double requestVel(double newVel, double currentVel, double currentTime)
      Request the desired final velocity, and this will return the velocity that you can safely go now given the parameters you entered into the constructor. (Will always return 0 while it initializes the first time it's called since this AccelLimiter was created or reset)
      Parameters:
      newVel - The velocity you want to go
      currentVel - The velocity you are currently going
      currentTime - The current time, in the same unit as the time component of your velocity (ex. if velocity is in meters per second, then currentTime should be in seconds)

    • requestDeltaVel

      public double requestDeltaVel(double reqDeltaV, double currentTime)
      Request the desired change in velocity, and this will return how much velocity you can safely add to your current velocity given the parameters you entered into the constructor. (Will always return 0 while it initializes the first time it's called since this AccelLimiter was created or reset)
      Parameters:
      reqDeltaV - How much you want to change your velocity
      currentTime - The current time, in the same unit as the time component of your velocity (ex. if velocity is in meters per second, then currentTime should be in seconds)

    • getMaxDeltaVThisLoop

      public double getMaxDeltaVThisLoop(double currentTime)
      How much can you change your velocity this loop given the maximum acceleration and how long it's been since last time we calculated this? (Will always return 0 while it initializes the first time it's called since this AccelLimiter was created or reset)
      Parameters:
      currentTime - The current time, in the same unit as everything else (ex. if the maximum acceleration is in meters per second, then currentTime should be in seconds)

    • requestDeltaVelOnN

      public List<Double> requestDeltaVelOnN(List<Double> deltaVelList, double currentTime)
      Request velocity changes on many actuators, and limit them while maintaining the proportions between them. (Works similarly to DTS.normalize().) (Will always return 0 while it initializes the first time it's called since this AccelLimiter was created or reset) This was created for RRMecanumDrive, but can be used in any similar situation involving multiple interconnected actuators.
      Parameters:
      deltaVelList - A List<Double> of all the changes in velocity you are requesting
      currentTime - The current time, in the same unit as everything else (ex. if the maximum acceleration is in meters per second, then currentTime should be in seconds)

    • reset

      public void reset()
      Call this when you're done with your task and want to use this object for something else.

    • stoppingDistance

      public double stoppingDistance(double initialVel, int resolution)
      How much distance is needed to stop from the given initial velocity at the maximum acceleration set for your AccelLimiter instance? Note that negative velocities still return positive stopping distances. This method sums the area under the curve of the acceleration (the integral). This is not necessary for the linear acceleration that AccelLimiter does exclusively at the moment (see the simpleStoppingDistance method), but it may become useful with new functionality in the future.
      Parameters:
      initialVel - How fast you are going at the moment you start ramping down
      resolution - Higher resolution values make the calculation take longer but yield more accurate results. Use the "stoppingDistancePrinter" Unit Test to determine what resolution you need.

    • simpleStoppingDistance

      public double simpleStoppingDistance(double initialVel)
      A much faster stopping distance calculation that can be used for linear acceleration (which is all AccelLimiter supports at the moment anyway).

    • setupRampToTarget

      public void setupRampToTarget(double targetPos, double maxVelocity)
      Sets up ramping up to maxVelocity and back down along the specified distance. Will also reset this instance!
      Parameters:
      targetPos - The position you want to end at (in whatever units you want, as long as you are consistent)
      maxVelocity - The top speed along that distance

    • setupRampToTarget

      public void setupRampToTarget(double targetPos, double maxVelocity, com.qualcomm.robotcore.util.ElapsedTime stopwatch)
      Used for dependency injection in UnitTests
      Parameters:
      targetPos - The position you want to end at (in whatever units you want, as long as you are consistent)
      maxVelocity - The top speed along that distance
      stopwatch - Pass in a MockElapsedTime to control time in UnitTests.

    • updateRampToTarget

      public double updateRampToTarget(double currentPos)
      Run this every loop. Takes the current position and returns the current velocity along the ramp.
      Parameters:
      currentPos - Where are you currently? Should be in the same distance unit as the maximum acceleration.