Motion Control

I would like to know if anyone has some source code for S curve trajectory programming. The function has a set position, velocity and acceleration and needs to calculate the position on the fly. The function gets called every 1ms in real time and needs to calculate its next position which will accelerate, move at a constant velocity and decelerate to the final position.

I have wrote a function which calculates this but it still has issues when changing the acceleration or the velocities during a move. To solve this I simply don't allow an accel or velocity change until the move is complete. I really don't like to do it this way. One requirement I really need is to have any of the 3 (pos, vel, accel) change and the output to handle this without issue.

I want to redesign it and know its been done before but am unsure as to how to start. I figure it is probably possible to do this all in mathematics and just plug in some values but physics was never my strong point.

Please excuse the dissarray of the code, I have mucked with it too much.

Are you trying to reach a desired position and velocity as soon as possible under the constraint of a limited acceleration and only being able to change the acceleration at discrete intervals?
if so, can we assume that acceleration can change instantaneously?

What I am trying to do is accelerate at a specific acceleration until I reach a desired velocity, travel at that velocity until I need begin decelerating to the final set position.

I was thinking about this again yesterday and realized that allowing the acceleration to change mid move will screw things up.

For instance if start at position 0 and accelerate at 1m/s^2 with a velocity set to 2. The system will accelerate for 2 seconds until it reaches a constant 2m/s it will then start to decelerate at 1m/s^2 for 2 seconds and come to a stop at the final set point...

If I were to change the acceleration midway into the move to say 0.5m/s^2 while it was traveling at the constant 2m/s it would overshoot the end point if it had already traveled too far to decelerate to 0m/s. The only way around this would be to decelerate at 0.5m/s^2 until it came to a full stop, backup on the path and come to a stop at my final position. Not what I would really want..

decelerating at 0.5m/s^2 when you are moving at 2m/s will take 4 seconds to reach 0m/s^2 during which you will travel 4m
decelerating at 1m/s^2 when you are moving at 2m/s will take 2 seconds to reach 0m/s^2 during which you will travel 2m