apply

PositionAndVelocityControl.apply() → None | float | int

It computes the pwm to apply to the powertrain’s motor in order to stick to position and velocity set by the trajectory through the two nested position_PID and velocity_PID.

Returns

float or int or None

PWM value to apply to the powertrain’s motor in order to stick to the S curve trajectory.

Notes

It computes a velocity reference through the position_PID as:

\[\dot{\theta}_{ref} = PID_{pos}(\theta - \theta_{ref})\]

where:

• \(\dot{\theta}_{ref}\) is the velocity reference,

• \(\theta\) is the current angular position, tracked by the encoder,

• \(\theta_{ref}\) is the reference angular position, get by the S curve trajectory.

Then, it computes a PWM through the velocity_PID as:

\[PWM_{ref} = PID_{vel}(\dot{\theta} - \dot{\theta}_{ref})\]

where:

• \(PWM_{ref}\) is the reference PWM,

• \(\dot{\theta}\) is the current angular speed, tracked by the tachometer.