Motion Profiling Implementation

Continuing the discussion from Motion Profiling:

Hi, I’m the new lead programmer for my FRC team and I’ve been working on research on motion profiling. I’ve understood most of the theory but what I don’t understand is the point of adding on a acceleration constant when implementing your trajectory points(such as the way Austin Schuh did in his post in an old thread linked above). Can someone explain to me why this is necessary? Also, what timer should I use when implementing my trajectory points?

Hey Derek!

So, in an ideal world, our kP and kV constants (proportional feedback and feedforward, respectively) would cause the motor to react more or less instantly to changes in the velocity setpoint. However, when we go to accelerate, since the open loop feedforward (kV) will only cause the motor to approach the target velocity eventually (assuming no outside forces - extra resistance, collisions, etc) and since the proportional term relies on a significant amount of error between the setpoint and the actual velocity (there won’t be enough error to cause the kP term to accelerate the motor a meaningful amount until some time has passed, resulting in longer term error). As such, if we were to only use PF (proportional+feedforward) control (with or without the derivative term), the velocity of the motor would lag the reference (setpoint), possibly with enough error to introduce significant path tracking error over longer trajectories. By explicitly offsetting the motor voltage enough to cause the motor’s acceleration to match the acceleration at the current point in the trajectory, which is what the kA feedforward term does, we can avoid the sort of tracking lag we would otherwise see.

2 Likes

Ah okay that makes sense. Is there any way that I can calc kA on my own or do i have to tune it like PID constants?

Another question I have is what’s the best way to set the motor speeds. I’ve been using PercentOutput on TalonSRX’s with my kv just being 1/Max Velocity, but is there a better way to implement trajectory points into motor controllers?

It boils down to the physics of your system. PID loops need to see error to generate an output. This, by its very definition, means that you will start following a trajectory by not following it and waiting until enough error accumulates to get started. FF lets you take the known physics of your system into account.

Your basic motor model says that a current into your motor results in a torque proportional to the current. So, input -> acceleration. If we want to be able to actually track a trajectory, we need a trajectory which has bounded acceleration. Instantaneously changing velocity would not have bounded acceleration, so it would not be able to be followed by a motor without some error.

For Kv and Ka, you can tune them empirically or with the physics. https://www.youtube.com/watch?v=RLrZzSpHP4E walks you through how to model an elevator.

As for implementing a motion profile, you will need to update the output very frequently to get precise results. We run everything at 200 hz on 971. If you are using CAN, I’d recommend using the built in profiling on the talon during the season, and to try implementing everything yourself during the offseason to learn more.

We do use CAN. While the built in motion profiling control mode is nice, I’d prefer to create my own implementation as it is currently the off season. When we apply velocity, it would be output = velocity * kv. Applying acceleration to that output would just be output = velocity * kv + accel * ka. Would accel just be the next point from my trajectory planner - current point?