To echo what Austin said: This is a great example of something you can develop and test without a running robot. Write the code to generate a trapezoidal motion profile using J2SE or on a desktop using C++, debug using Excel, Matlab, or Gnu plots, and you will be able to port it to your robot code with ease.
Of course, once you are generating your trapezoidal motion profiles, you will need a controller that makes sure the bot actually follows it! Here are a few ideas on how you could do this:
1. Use a PID loop on your drive motors (in distance mode). Use the speed output of your motion profile to limit the maximum command that PID is allowed to send to your motors.
2. Use a PID loop on your drive motors (in speed mode). As long as your speed loop has an integral term (the I gain is nonzero), this should get you where you are going.
3. Use both (1) and (2) to control both speed and distance; there is also a specialized "PIV" controller used in industrial servos that mixes both speed and distance to command the motors.
4. Just use the speed command you generated to drive the motors directly (open loop), but switch to a PID controller when you get close to the goal to go the last few inches.
5. Make a full-state controller using control theory and simulation; if you go this route you are either insane or 254
