Urgent issue with encoder interrupts and PID control

Hello,
We are a group of engineering students very close to a deadline on a robotics projects (roughly 32 hours...)
We are stuck because of what seems to be a timing issue that is causing us to miss encoder ticks when we try to move the robot at any decent speed. We are using 2048 PPR rotary encoders handled via interrupts, and want to use the data to update a PID control system for the motors.
We designed to the system on Matlab to handle via an error on position and speed, for now we would be happy enough if just the position part worked properly. We are using an arduino mega 2560 for our robot.
Originally we had coded the interrupt functions so it got ticks on both A and B of each encoder using 2 digitalReadFast2 for each.
We called the automatic control functions through an isrt running through flexiTimer at different intervals (we had hoped for every 50ms but even 200ms would be fine).
We would miss ticks constantly, and would cover about 33cm for the robot to have thought it covered 30. We think this is because somewhere we are missing ticks, possibly due to interference from the flexitimer interrupt...?
So a few questions :
1) any idea on our problem?
2) is flexitimer interrupt going to interfere with the encoder interrupts?
If we try and call the function every 50ms with milis() that's even worse right since it disables them reactionaries interrupts whenever called? Anyway to do something with interrupt priority to be sure not to miss ticks?
3) is there a difference between digitalReadFast and digitalReadFast2?
We are trying to modify the interrupt functions for the encoders based on this article :
http://www.hessmer.org/blog/2011/01/30/quadrature-encoder-too-fast-for-arduino/
We hope this will maybe make the interrupt routines for the encoders faster and let us miss less ticks.

 

We have no need to disable interrupt, we just wanted to try and scheduel some tasks based on time using millis(), and I'm fairly sure that that function disables interrupts while it reads a time value and then re-enables afterwards. In this test sketch the loop is so small that millis() must be getting called extremely frequently and interfering with the encoder ticks.

We switched to using FlexiTimer, running every 100ms or 50ms and calling the automatic control function, but while this was active it blocks encoder ticks I'm guessing?
We just had the idea that when FlexiTimer interrupts it could just set a flag value to 1 that will cause the automatic functions to be executed a single time in the main loop, which then resets the flag to 0.
The automatic control isn't really time critical, we just want it to be executed at a known frequency so we can calculate integral and derivatives for the PID control, so this would probably work no? The only issue that would mess with this is if in our big main program the execution of the program takes longer then the period between interrupts, but more then 50 or 100ms is unlikely right if we avoid long loops? I have no real sense of how much time an arduino sketch takes to execute.

Here's our code currently.

 

Yes things are working much better now, we can drive forward at a decent speed and the encoder values remain very accurate. We were coding late at night and didn't think that obviously so many calculations with floats and whatnot in an interrupt routine would totally mess with the encoder interrupts.

Thanks for your help!

We're now stuck trying to set up the PID controller to account for the fact that the right motor turns faster then the left for the same voltage and by how much more is seemingly random...

We thought a corrector on the speed of each wheel by looking at tickrate over each 50ms period and forcing this value to even out would help, but so far not so much luck.