Motor Steady Check
The motor steady check would output a true signal if the motor was within a certain error tolerance (set by a constant) for the last half second (also changeable by a constant). It would use an incremental counter multiplied by appropriate gains to convert to seconds, but essentially would carry out the following equation:
Counts = (Within_Tolerance*(Not Stable) + Counts)*Within_Tolerance
This way, counts would increment by Within_Tolerance each dt, and since Within_Tolerance only holds zero or one, it would either increment, or become zero, thus multiplying the whole sum by zero and starting over. Also, but multiplying Within_Tolerance by the opposite of Stable, once the motors did become stable the counter could stop thus preventing overflow.