Well, I finally see got a chance to.

Preventing tipping from rotating is more interesting than translating, because centrifugal force exists (yes, really, it does). If the center of gravity is not the center of rotation (CoR), then there are two factors that need to be limited to prevent tipping: rotational acceleration and rotational velocity.

**Rotational Acceleration:**

I’llLimelight, an integrated vision coprocessor start with acceleration limiting because it’s simpler. The acceleration vector is perpendicular to the radius between the center of rotation and the rotating object (here the center of mass)— in terms of vector math, it’s the cross product of the angular acceleration vector and the position vector of the mass:

This means we can easily find the direction of acceleration, and, from there, I can just re-use my

`calcMaxAccel`

function that I’ve

discussed before. Given the max linear acceleration, we can then just divide by the radius (the distance between the CG and the center of rotation) to get the maximum angular acceleration.

**Rotational Velocity:**

Because of centrifugal force (which exists, despite what some overzealous physics teachers might say), we also need a rotational velocity limit.

##
Centrifugal vs. Centripetal

In order for an object to continue moving in a circle, it needs to be accelerated in some direction. Otherwise, inertia will keep it moving in a straight line.

The acceleration needed is towards the center, or centri- (center) petal (seeking). However, this acceleration is not magically created— if the required acceleration is greater than can be supplied by the system, the mass will no longer move in a circle and instead fly off on a tangent. This force (mass * acceleration) can be supplied by a variety of means depending on the system— for a ball whirling on the end of a string, tension provides centripetal force, and for the case of the robot, the torque due to gravity provides centripetal force.
Now consider the reference frame of the rotating mass— for example, lets use the whirling ball. As already described, the pulls inward on the ball. However, from the ball’s point of view, the ball is not accelerating towards the center. We, as an outside observer, see that it is— otherwise, the ball would fly off in a straight line. However, the ball is not getting closer to the center of rotation, and therefore, from the rotating ball’s reference frame, the force inwards must be balanced by an equal force outwards: centri- (center) fugal (fleeing) force.

Hopefully that makes sense.
In our rotating robot’s reference frame, centrifugal force/acceleration acts outward, along the line between the CG and CoR. Again, we use the same `calcMaxAccel`

function, and the result is the maximum linear acceleration in that direction. To turn this back into rotational velocity, we simply solve the centrifugal/centripetal acceleration formula for omega:

a = \omega^2 r

\omega = \sqrt{\frac a r}

In reality, the total forces from rotational and translational acceleration should probably be combined with rotational velocity, and then the maximum acceleration calculated from the resulting vector, and then somehow resolve out each component, but that seems hard and hopefully doing each part separately is good enough.

**Implementation**

Now that we can calculate the maximum angular acceleration (alpha) and velocity (omega), we need some way to impose those limits. What kind of control system lets you specify maximum acceleration and velocity to reach a desired position? Motion profiling! Specifically, in this case, a trapezoidal profile feeding our heading control loop. Previously, we were just using a PID loop to directly control angular position:

There’s a number of issues with this anyway— wheel slippage is most significant. Additionally, and more to the point here, neither velocity nor acceleration can be controlled directly. By using a `TrapezoidProfile`

to generate setpoints and giving it the max alpha and omega values calculated earlier, we get this:

Much crisper, too.

One final note— if the CG is in the same location as the Cor, the theoretical maximums are both infinity. The simple solution is to just supply a constant maximum and pick whichever value is lower.