[Foster]

I had similar code for when I was doing MPLabs. I had a fancy one that would let you have multiple threads and time a few things at the same time.

Ahhhh the days of rolling your own components, the sweet smell of 1's and 0's .

Anyway today the RobotC compiler does all of that for you. It also has tasks that you can spawn off to do specific functions and not wait in the main line.

Marks 'case' code is a good example of how we would put state machines together and have them do the heavy lifting of what the robot code needed to do. A little work with Google will get you some good tutorials on state machines.

But it looks like you are on the right track of getting your robot to move.