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;

Select allOpen in new window