Programming Swerve Drive

So we have a robot going to worlds, and we just completely redesigned it. It now has a 4 wheel swerve drive, and is able to score green balls in goals 100% of the time, or close to it. And the best of all, it can score more orange balls then before because the basket is increased significantly downwards, because of the new drive system! I am trying to program the drive system, which is annoying, because in order to make it work the way it needs to, the program needs to be overly complicated. Right now the program works nearly flawlessly, but has a larger delay than that of the old drive system. I know vexnet adds a tad to the delay, but this is double the vexnet lag. Any ideas on shortening my program, so that it isnt delayed? I heard using case statements rather than if statements would make it much faster.

Also: I’m trying to make it so that after i turn, the robot compensates for the lag, by reversing the last used drive motors, for a very small amount of time. But because this changes depending on the direction that the wheels face, it is making my program much longer! Any easy way to make it take reverse that last used motors for a short period of time?

Sorry for typing so much, just kind of want to make sure I explain as much as possible.

mind if you attach your code so we can see whats wrong?
and there are program pro people that would probably help more than me but ill try my best :slight_smile:

Best case is to decompile the code to see what is going on.
Otherwise, you can just experiment to see what helps.
Here are some ideas for experiments:

If/then/else are often compiled slowly compared to case swtich statements.
For very short if/then/else blocks, logic shortcutting can have much less overhead. Also trinary operators can compile very well.
untested pseudo code examples:
original: IF A == 1 then {B = 5} else {B = 6;}
trinary: B= ( A==1 ? 5 : 6);
logic shortcut: B=6; (A==1) && (B= 5);

Another method for your specific problem (motors move at different times),
is to try refactoring your code so that all the motor updates are done at one place in the code, rather than some here and some there:
original: while(1){ blah, set motor1=50, blah, blah, set motor2=50, }
refactor: while(1){ blah, m1=50, blah,blah,m2=50 …
set motor1=m1; setmotor2=m2;}