# NextControl Proposal

## Requirements

- **Modularity:** The user must not be forced to use
anything they don't want to use.
- **Gradual Complexity:** Users can start with only a few lines of code
and slowly add more, such as motion profiling or state estimation.
- **Best Practices:** It must not only follow best practices but encourage users to do so as well.

Let's also have four example use cases that our final control system must be able to accomplish:

- Moving a turret to a position using a PID
- Moving a flywheel at a constant velocity using a velocity PID and feedforward
- Raising a lift using a trapezoidal motion profile and a PID and feedforward to follow it
- Moving an arm to a position using a PID and feedforward

## Control Systems

According to [GeeksForGeeks](https://www.geeksforgeeks.org/control-system/), a control system is a system or a set of devices that manages, commands and directs the behavior of other devices or systems. A control system has three components: the sensor, the controller, and the actuator.

Additionally, a controller only operates on a *single variable*. For example, a velocity controller could have a velocity feedback controller and a velocity feedforward controller, but nothing else.

Because of this, I have decided to call a control system a `ControlSystem` and not a `Controller` in order to follow best practices and the industry standard.

## Storing States

In the examples on the WPILib docs, the only states stored are position and velocity. However, WPILib still supports using acceleration. It mentions that "acceleration only needs to be accounted for in systems with high inertia." For this reason, I believe that NextControl should store acceleration along with position and velocity in order to work with all systems, but it should be recommended not to use it in most cases.

Another thing we must consider is that not all control systems are controlling motion. Other things that can be controlled are temperature, pressure, and much more, although in FTC motion is the only concern for over 99% of teams. Therefore, I believe that instead of a state being called a `KineticState`, it should be called a `ControlState`. However, calling the values position, velocity, and acceleration is still fine because that will be the far majority of use cases. A `ControlState` would be as follows.

data class @JvmOverloads constructor ControlState(
    val position: Double = 0.0,
    val velocity: Double = 0.0,
    val acceleration: Double = 0.0
)

Additionally, `MotorEx` would have a `currentState` property with a getter that returns the current state. This would require `MotorEx` to cache the previous position and velocity, and possibly the last two as a fail-safe (and use a linear regression).

## The FeedbackElement

The original plan was to have seperate feedback controllers for position and velocity. However, I think it is a better idea to just have one `FeedbackElement` that takes a `ControlState`:

interface FeedbackElement {
    fun calculate(error: ControlState): Double
}

The reason it takes the error directly instead of the current and target positions is because that's what a feedback controller does: brings the error to 0. All it needs to know is the error. Furthermore, it allows for things like angular controllers, which normalize the error then pass only the error to a PID controller. The way homeostasis gets around this is by normalizing the error and then passing zero as the current position and the normalized error as the target. This works, but I feel like it is too much of a "hack."

This `FeedbackElement` also makes it very easy to do things like full state feedback, which in in simple cases is identical to a position P controller and a velocity P controller.

There could also be a `PositionPIDElement` and a `VelocityPIDElement`. However, there's an obvious problem with this: 80% of the code would be the same. So instead, there could be a `PIDElement` that takes a `PIDMode`:

enum class PIDMode {
    POSITION,
    VELOCITY,
    ACCELERATION
}

This is overall good, but there is one problem with it: you can't use one PID on position and another on velocity. This is not a problem though, as you should be using full state feedback for that. Additionally, you should not be using the D of a position PID at the same time as the P of a velocity PID, as they are the same.

When following a motion profile with a PID, where your target velocity is nonzero, the D in a PID will not work. We need to offset it so that it is trying to bring the velocity to our target velocity instead of to 0. However, this is the exact same as the P term in a velocity controller. That's why when using full state feedback, we only use position P and velocity P.

Because of that, I think that it is fine to only have the `PIDElement` support one.

Other `FeedbackElements` could be:

- `AngularFeedbackElement`, which wraps a `FeedbackElement` and is used for dealing with angles that wrap around. Alternatively, it could support any wrapping around, not just for angles. This is how WPILib does it, but personally I don't see any case that it's needed other than angles.
- `BangBangFeedbackElement`, which is pretty self-explanatory.
- `SquidFeedbackElement`, which is also self-explanatory.

## The FeedforwardElement

Feedforward is predicting the required power for a system given a model, or knowledge of how the system works. For example, a basic motor with no external forces other than static friction can be modeled with the equation $$V = K_s \cdot \text{signum}(v) + K_v \cdot v + K_a \cdot a$$ where $V$ is the voltage to apply, $v$ is the desired velocity, and $a$ is the desired acceleration.

Previously, I had considered having `ArmFeedforward` only be the cosine of the angle, but I have since realized that is wrong. Feedforward isn't just supposed to be a power you add, but it's supposed to be a model of how a system behaves. Thus, the equation of an arm feedforward should be $$V = K_g \cdot \cos(\theta) + K_s \cdot \text{signum}(v) + K_v \cdot v + K_a \cdot a$$ Likewise, an elevator feedforward should be $$V = K_g + K_s \cdot \text{signum}(v) + K_v \cdot v + K_a \cdot a$$

A `FeedforwardElement` could be as follows.

interface FeedforwardElement {
    fun calculate(targetState: ControlState): Double
}

## The FilterElement

In all honesty, this was the hardest one for me. I was originally thinking something like this:

interface FilterElement {
    fun filter(sensorMeasurement: ControlState): ControlState
}

However, there are a few flaws with this approach:

- If the user only needs position, it is performing unnecessary calculations by filtering velocity and acceleration as well.
- It doesn't allow the user to use seperate filters for position and velocity.

Another idea I had was to have a filter be this:

interface FilterElement {
    fun filter(sensorMeasurement: Double): Double
}

And then to have a `ControlSystem` take three `FilterElements`; one for position, one for velocity, and one for acceleration. Although I don't see any major problems with this approach, I still feel uneasy about it. I have not decided which approach I like better.

> [!NOTE]
> With either approach, there could be a `ChainFilterElement` that uses multiple filters in a specified order.

## The InterpolatorElement

An `InterpolatorElement` interpolates between target positions. It could look as follows:

interface InterpolatorElement {
    var goal: ControlState
    val currentTarget: ControlState
}

A simple interpolator that doesn't actually interpolate could be as follows:

class ConstantInterpolator(override var goal: ControlState) : InterpolatorElement {
    override val currentTarget: ControlState
        get() = goal
}

Aditionally, there could be more advanced interpolators such as trapezoidal or s-curve motion profiles.

## The ControlSystem

Lastly, the `ControlSystem` is where the four elements come together. Assuming the first approach for a `FilterElement`, it could look something like this:

class ControlSystem(
    private val feedback: FeedbackElement,
    private val feedforward: FeedforwardElement,
    private val filter: FilterElement,
    private val interpolator: InterpolatorElement
) {
    var goal: ControlState by interpolator::goal

    @JvmOverloads
    fun calculate(sensorMeasurement: ControlState = ControlState()): Double {
        val filteredMeasurement = filter.filter(sensorMeasurement)
        
        val error = ControlState(
            goal.position - filteredMeasurement.position,
            goal.velocity - filteredMeasurement.velocity,
            goal.acceleration - filteredMeasurement.acceleration
        ) // probably should make ControlState have an operator function for subtraction

        val feedbackOutput = feedback.calculate(error)
        val feedforwardOutput = feedforward.calculate(goal)

        return feedbackOutput + feedforwardOutput
    }
}

## Examples

Let's show how each of our four initial examples could be done, assuming the required controllers are implemented.

### Turret to Position

val controlSystem = ControlSystem(
    AngularFeedbackElement(
        PIDElement(kP, kI, kD, PIDMode.POSITION)
    ),
    NullFeedforwardElement(),
    RawValueFilterElement(),
    ConstantInterpolatorElement()
)

### Flywheel

val controlSystem = ControlSystem(
    PIDElement(kP, kI, kD, PIDMode.VELOCITY),
    BasicMotorFeedforwardElement(kV, kA, kS),
    RawValueFilterElement(),
    ConstantInterpolatorElement()
)

### Motion Profiled Lift

val controlSystem = ControlSystem(
    FullStateFeedbackElement(kP, kV),
    LiftFeedforwardElement(kG, kV, kA, kS),
    RawValueFilterElement(),
    TrapezoidalMotionProfileElement(maxVel, maxAccel)
)

### Arm to Position

val controlSystem = ControlSystem(
    PIDElement(kP, kI, kD, PIDMode.POSITION),
    ArmFeedforwardElement(kG, 0.0, 0.0, 0.0),
    RawValueFilterElement(),
    ConstantInterpolatorElement()
)

## Making it User-Friendly

The user should not have to pass the elements they don't intend to use. To solve, this, a builder design pattern could be used:

val controlSystem = ControlSystem.builder()
                        .posPid(kP, kI, kD)
                        .lowPassFilter(kA)
                        .build()

Alternatively, a mutating build could be used:

val controlSystem = ControlSystem()
                        .posPid(kP, kI, kD)
                        .lowPassFilter(kA)

Lastly, (in Kotlin only), a DSL could be used:

val controlSystem = controlSystem {
    feedback {
        posPid(kP, kI, kD)
    }
    filter {
        lowPassFilter(kA)
    }
}