There is a lot of code there and I haven't reviewed it in detail, but the logic to derive the PWM value passed to analogWrite() is complex and could easily be producing a value you don't expect.
To start with, I suggest you create a test sketch that does nothing but run the motor(s) at full speed and confirm the hardware is working correctly. Once you've done that, add some debug print statements to show what values you're actually passing to analogWrite() in your real sketch - I suspect you'll find they're wrong, pointing to bugs in your calibration logic.