Sign in to follow this  

PID steering controller flips from 0° to 178°

This topic is 821 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Recommended Posts

Hey all.

 

I'm working on a virtual robot sim, using a PID controller in software.

 

The problem is the object when traveling at 189° it flips back to 0° which makes it spin back around it's self and then enter some flipping loop.

 

I'm not sure if I need to reset the SetPoint in this case the setpoint is the targets position every iteration.

 

if I dont set the new setpoint the object can miss the target.

 

what I have is 4 waypoints

waypoint[0] = (-400,0,400)

waypoint[0] = (400,0,400)

waypoint[0] = (400,0,-400)

waypoint[0] = (-400,0,-400)

 

if the object is traveling from waypoint 1 to 2 when it gets to waypoint 2 it starts flipping and also from 2 to 3 sometime it spins.

 

I thought I could fix it with a resetting of the target after 50 steps, But that stops it some times but it still there.

 

this is the code for the loop. I could have it all wrong. I'm using PI to -PI for min and max output. I'll go and try using 0 - 360­° and not use radians

I doubt that will work.

.

//-----------------------------------------------------------------------
	//testing out using the pid controller to controll the direction and the speed
	//----------------------------------------------------------------------------
	void Controllerupdate(double timedelta)
	{
		if(HasDest == false)
			return;

		//temp fix not working
		static int scanahead = 0;
		const int updates = 50;

		//defines the pid tunning control
		consKp= 1.4;
		consKi= 0.007;
		consKd= 0.0005;
		PIDSteerController->SetTunings(consKp, consKi, consKd);
		
		RTSComponentMotion.m_dMass = 5.1;//controls how quick the objects turns ???
		RTSComponentMotion.m_dMaxForce = 260.0;//
		RTSComponentMotion.m_dMaxSpeed = 260.0;//controls the speed
		RTSComponentMotion.m_dMaxTurnRate = 1.0;
		RTSComponentMotion.m_dRadius = 260.0;
	
		
		//Update the control and adjust the steering
		if(PIDSteerController->Compute())
		{
			double newHeading;
			double turnRate;

			float  maxTurnRate= 21.9;
			float maxRudder = 30.9;

		   // Calculate rudder position from controller output
		   rudder = -maxRudder * Output;
   
		   // Take account of rudder end stops
		   if(rudder > maxRudder) 
		   {
			  rudder = maxRudder;
		   }
		   else if(rudder < -maxRudder) 
		   {
			  rudder = -maxRudder;
		   }
   
		   // Calculate turn rate from amount of rudder aplied
		   // it is assumed that above a certain amount of rudder, turn rate will be constant
		   // TODO: Rate of change of rate of turn
		   turnRate = 0.2 * rudder;
   
		   // Take account of max turn rate
		   if(turnRate > maxTurnRate) 
		   {
			  turnRate = maxTurnRate;
		   }
		   else if(turnRate < -maxTurnRate) 
		   {
			  turnRate = -maxTurnRate;
		   }
		   float dt = timedelta;
		   // Calculate new heading
		   Input = Input + (turnRate * dt);
			
	}

		//do motion
		double theta = 0.0;//RebuildTheta;
		
		//ok we need to build a direction from the input angle
		RTSComponentMotion.m_vHeading = SphericalCoordinatesToCartesian(Radius,theta ,Input);

		D3DXVec3Normalize(&RTSComponentMotion.m_vHeading,
									&RTSComponentMotion.m_vHeading);

		//create the acceleration in the direction we are heading
		D3DXVECTOR3 acceleration = RTSComponentMotion.m_vHeading *1000.0;

		//update velocity
		RTSComponentMotion.m_vVelocity = (acceleration)  * timedelta;// * timedelta); 

		//make sure vehicle does not exceed maximum velocity
		RTSComponentMotion.m_vVelocity = Truncate(RTSComponentMotion.m_vVelocity, RTSComponentMotion.m_dMaxForce);
			
			//update the position D3DXVECTOR3(0.0, 0.0, 0.0);//
		RTSComponentMotion.m_vPos +=  RTSComponentMotion.m_vVelocity;// * timedelta;

		RTSComponentMotion.m_vPos.y = 0.0;


		//get the direction to travel to target and the distance
		D3DXVECTOR3 targetp =  WayPoints[CurrentWayPoint];
		targetp.y = 1.0f;
		D3DXVECTOR3 p = RTSComponentMotion.m_vPos;
		p.y = 1.0;
		D3DXVECTOR3 distance = targetp - p;
		float t = D3DXVec3Length(&distance);

		D3DXVECTOR3 newtargetnormalized;

		
		//this alligns the objects facing to the heading
		 RotateObjectFacingDirectionToHeading(0);

		//see if we need to change way points
		if(t <= RTSComponentMotion.m_dRadius)
		{
			scanahead = 0;
			PIDSteerController->Reset();
			rudder = 0;
			CurrentWayPoint++;
			if(CurrentWayPoint >= WayPoints.size())
				CurrentWayPoint = 0;
			
			//we need to calulate the new direction to target
			//we need to reset the steering controller for this next destination
			targetp =  WayPoints[CurrentWayPoint];
			targetp.y = 1.0f;
			p = RTSComponentMotion.m_vPos;
			p.y = 1.0;
			distance =targetp - p;

			D3DXVec3Normalize(&newtargetnormalized,
									&distance);


			//set the PIDcontrollers new SetPoint
			//get the angle using spherical coordinates
			CaluclateSphericalCoordinates(Radius, 
											RebuildTheta,//Setpoint,//double &Theta, 
											Setpoint,
											newtargetnormalized.x, newtargetnormalized.y, newtargetnormalized.z,
											distance.x, distance.y, distance.z);

				
			//this is test code to fix feed back loop not working
		/*	D3DXVECTOR3 Facing =  RTSComponentMotion.m_vHeading;//m_UnitsFacingDirection;
		D3DXVECTOR3 Facingnormalized;

		D3DXVec3Normalize(&Facingnormalized,
									&Facing);

		CaluclateSphericalCoordinates(Radius, 
											RebuildTheta,//double &Theta, 
											Input,
											Facingnormalized.x, Facingnormalized.y, Facingnormalized.z,
										Facing.x, Facing.y, Facing.z);
		
		*/

		}
		else
		{

			scanahead++;
			if(scanahead >= updates)
			{
				scanahead = 0;
			//RTSComponentMotion.m_vHeading = SphericalCoordinatesToCartesian(Radius,theta ,Input);

		//D3DXVec3Normalize(&RTSComponentMotion.m_vHeading,
		//							&RTSComponentMotion.m_vHeading);
			targetp =  WayPoints[CurrentWayPoint];//DestPos;
			targetp.y = 1.0f;//so we can stop because the pos has no y value too may fix this later
			p = RTSComponentMotion.m_vPos;
			p.y = 1.0;
			distance = targetp - p;

			//Input = Setpoint;
			//PIDSteerController->Reset();
			//rudder = 0;

			D3DXVec3Normalize(&newtargetnormalized,
									&distance);
		
			//this is the current new heading
			//get the angle using spherical coordinates
			CaluclateSphericalCoordinates(Radius, 
											RebuildTheta,//double &Theta, 
											Setpoint,
											newtargetnormalized.x, newtargetnormalized.y, newtargetnormalized.z,
										distance.x, distance.y, distance.z);

			}//end scan new target position

		}



	}//end Controllerupdate
	/////////////////////////////////////////////////////////////////////////////

Share this post


Link to post
Share on other sites

it spin back around it's self and then enter some flipping loop.

The radius to check if it reached the waypoint is too tight or the speed too high. The problem occurs, when your object shoots beyond the waypoint, then turns around and shoots on the other side all the time, never really coming within the radius of the waypoint.

 

To ease it:

1. For intermediate waypoints relax the radius.

2. For the final waypoint use the arrival steering behavior (you get slower when approaching your target to avoid this loop).

 

An other tip:

When checking the radius, it could be useful to ignore the y-component. If you have eg. an high object (center at 1m) and a waypoint at the ground level (0m), then the distance  would be minimal 1m,even when the object sits right on top of the waypoint. When ignoring the y-axis the distance would be 0.

Edited by Ashaman73

Share this post


Link to post
Share on other sites

Hey cool.

It's like boids they also need a arrive. I increased the distance and another thing that affects the steering was the rudder and turn rate I had too small of a value

rundder max is now 10.09 and turnrate to 2.09.

 

But it still suffers from going the wrong way on that same corrner it take the long arc around other then that it works, the arrive may fix that.

 

Any Ideas on how to go about a PID controller for the Slowdown.

 

Thanks again.

 

 

Here is a link to the exe. Get the one named PIDSTEERING

Edited by ankhd

Share this post


Link to post
Share on other sites


the arrive may fix that.

Arrive for intermediate wapyoints isn't the best way. It would slow down everytime you character approaches a waypoint. When relaxing the radius (e.g. 30% of the way length between current and next waypoint) it will automatically smooth the path. Just be careful to avoid running into obstancles. You can either improve the steering behavior by enriching the waypoints with some meta data (e.g. sharp_corner => use arrival)  or by adding additional waypoints to manage special situation.

Share this post


Link to post
Share on other sites

Well I change the code, I never needed to reset the Input after a heading adjustment any thing after the function CaluclateSphericalCoordinates

 

and to take the short arc when turning I added the follwoing after Setpoint adjustment

 

if(Setpoint < 0.0)

{

     if(Input > 0.0)

           Input *= -1;

 }

 

It seem's to work, I even reversed the path and added a extra sign wave path and it followed well.

 

For the speed adjustment I added a new PID controller that has a max speed for the object and a minimum speed, You could set the minimum speed to 0 but for

path following it works smoother with a minimum speed of the objects radius and adjusts  the Kp value to get the desired braking.

Im also using the radius to stope and check for new way point. This allows the object to travel at speed to the next waypoint and slow down so it does not over shoot the target

and enter loop feed back.

 

I have a idea to make a template PID controller this should work for vector(no angles then) may be. I can't see why not this is the Compute that does all the work

in the PID i'm using.

I removed the time as it now runs in a fixed time loop.

.

bool PID::Compute()
{
   if(!inAuto) 
	   return false;

   unsigned long now = millis();
   unsigned long timeChange = (now - lastTime);
  // if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
	  double input = *myInput;
      double error = *mySetpoint - input;
      ITerm+= (ki * error);
      if(ITerm > outMax) 
		  ITerm= outMax;
      else 
		  if(ITerm < outMin) 
			  ITerm= outMin;

      double dInput = (input - lastInput);
 
      /*Compute PID Output*/
      double output = kp * error + ITerm- kd * dInput;
      
	  if(output > outMax)
		  output = outMax;
      else 
		  if(output < outMin)
			  output = outMin;
	  *myOutput = output;
	  
      /*Remember some variables for next time*/
      lastInput = input;
      lastTime = now;
	  return true;
   }
  // else 
	//   return false;
}

Share this post


Link to post
Share on other sites

This topic is 821 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this