Can you explain how the shields are connected on the cables delivering data to the servos?
My suspicion is you may have created a big loop antenna here. Each signal wire needs to
travel alongside a ground-return wire, to avoid creating a loop in the signal path. If the shield
is connected to ground at both ends you may be OK (but will have a ground-loop), but if only
at one end it does nothing to prevent magnetic interference.
I'd definitely use separate power for the servos from any logic boards, that alone could account
for occasional crashes due to spikes/dropouts on the supply. Servos and motors can create
really dirty voltage noise on the rails. Logic devices need stable spike-free power.
Failing that copious decoupling with 1000µF or greater caps on the 5V may be needed.
Once you have separate power you can break the ground-loop and have the data+ground
cables as the only ground connection between servos and Arduino. That would be good.
You've some howlers in the code about handling timeouts:
long next_update = 0;
...
void handle_ease(){
if(millis() > next_update){
next_update = millis() + STEP_TIME;
...
}
}
This will break for several reasons. First next_update is signed, so once it goes negative the
test comparing it to millis() will always trigger. next_update needs to be "unsigned long" just
like the result from millis().
Secondly you will hit the wrap-around issue as there's no subtraction in the test. Wraparound
for millis() is on the order of weeks/months though, so its not likely to be your problem, but it
does need fixing like this:
void handle_ease(){
if(millis() - next_update > STEP_TIME){ // subtraction essential to avoid wrap-around issues
next_update += STEP_TIME; // no need to call millis() again, just schedule next update
[ you might want to rename "next_update" as "prev_update" to match the logic ]
The point of the subtraction is that it returns a value that is always representable as an unsigned long, and always correct, even across wrap-around. This is since computer hardware and thus C integers use arithmetic modulo a power of 2.
