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.


int SingleLinearAxis::exec()
{
	double disttostop;
	double distremain;
	static double accel;
	double ra;
	double period = getPeriod();
	double velocity;
	double curvel;
	static double finalpos;
	static bool estoptrigger=false;
	static bool stopped=true;
	static double estop_stoppoint=0;
		
	// Store final position, for detection when it changes.
	lastFinalPos = finalpos;
	finalpos = getInput("position");
	
	// Cacl current velocity by getting distance traveled in 1 period
	curvel =( nextpos - despos ) / period;
    despos = nextpos;
    
    // Only update the acceleration after a move is complete.
    // Not doing this will cause deceleration calculation to screw up.
 
    if(stopped == true)
    {
    	accel = getInput("acceleration");	
    	//cout << accel << endl;
    }
      
	// Distance to stop is the current velocity^2/(deceleration*2).
	disttostop = (curvel*curvel)/(accel*2);
    	
	// Distance Remaining
	distremain = finalpos - despos ;
	velocity = getInput("velocity"); 
	
	if(velocity <= 0)
		velocity = 1;
	
	if(velocity > getInput("maxvelocity"))
		velocity = getInput("maxvelocity");
    	
	// Set Point Changed
	if(!flt_equals(lastFinalPos,finalpos))
		stopped = false;
	
	//Figure out what state to be in
	if(fabs(distremain) < disttostop || stopped )
		CurrentState = Decelerating;
	else
		CurrentState = Accelerating;
	
	switch(CurrentState)
	{
	case Accelerating:
        // If need to move in reverse from current location, flip velocity
        if( distremain < 0 )
        	velocity *= -1;
        
        // Required Acceleration to get to the final position in a single period
        ra =( velocity - curvel ) / period; //Should be outputting
           
        // Do we need to limit the acceleration?
        if( fabs( ra ) > accel )
        {
            if( ra > 0 ) // Need to move forward
                velocity = curvel +( accel * period );
            else         // Need to move in reverse
                velocity = curvel -( accel * period );
        }
    
        // Calculate next position to be at
        nextpos = despos + ( velocity * period );
		break;
	case Decelerating:
        velocity = accel*( sqrt( period*period+( 8*( fabs( finalpos-nextpos ) )-( 4*period*curvel ) )/accel )-period )/2;
 
        // Decide what direction to move in.
        if( distremain < 0 )
        	velocity *= -1;
 
        // Move Complete, when calc returns a invalid velocity or the calculated position
        // is less than the minimum movement for the current period.
        if(isnan(velocity) || (fabs(finalpos - despos) < (accel * period * period / 2 ))) 
        {
        	stopped = true;    // Sets the stopped output
        	despos = finalpos; // Set the output to the exact location
            velocity = 0;      // Forces velocity to exactly zero.
        }
        
        // Calculate next position to be at
        nextpos = despos + ( velocity * period );   
        break;

Open in new window

LVL 1
Thorn3Asked:
Who is Participating?
I wear a lot of hats...

"The solutions and answers provided on Experts Exchange have been extremely helpful to me over the last few years. I wear a lot of hats - Developer, Database Administrator, Help Desk, etc., so I know a lot of things but not a lot about one thing. Experts Exchange gives me answers from people who do know a lot about one thing, in a easy to use platform." -Todd S.

ozoCommented:
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?
0

Experts Exchange Solution brought to you by

Your issues matter to us.

Facing a tech roadblock? Get the help and guidance you need from experienced professionals who care. Ask your question anytime, anywhere, with no hassle.

Start your 7-day free trial
Thorn3Author Commented:
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..

Duh.. I did say physics wasn't my strong point right... Thanks for your thoughts anyway..
0
ozoCommented:
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

0
It's more than this solution.Get answers and train to solve all your tech problems - anytime, anywhere.Try it for free Edge Out The Competitionfor your dream job with proven skills and certifications.Get started today Stand Outas the employee with proven skills.Start learning today for free Move Your Career Forwardwith certification training in the latest technologies.Start your trial today
Algorithms

From novice to tech pro — start learning today.