We tried the same thing with the same results last year...

Thats when we learned why the FIRST code always added 2000 calucations then subtracted it from the final result in their mix limit. It prevented sign and overflow issues.

Another project for you to consider is to ramp the motor commands to the new speed when your driver slams the stick from one direction to the other.(or pushed the button). With 26 Ms between pwm updates your motor could be commanded to go full forward at one update and then full backwards at the next up date. Your poor drive train won't last to long doing that.

Good Luck
BC