• 13
• 14
• 27
• 9
• 9

# PID steering controller flips from 0° to 178°

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

## 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

//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;

//Update the control and adjust the steering
if(PIDSteerController->Compute())
{
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;
Input = Input + (turnRate * dt);

}

//do motion
double theta = 0.0;//RebuildTheta;

//ok we need to build a direction from the input angle

//create the acceleration in the direction we are heading

//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

//see if we need to change way points
{
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
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 Facingnormalized;

D3DXVec3Normalize(&Facingnormalized,
&Facing);

RebuildTheta,//double &Theta,
Input,
Facingnormalized.x, Facingnormalized.y, Facingnormalized.z,
Facing.x, Facing.y, Facing.z);

*/

}
else
{

{

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
RebuildTheta,//double &Theta,
Setpoint,
newtargetnormalized.x, newtargetnormalized.y, newtargetnormalized.z,
distance.x, distance.y, distance.z);

}//end scan new target position

}

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



##### 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 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 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 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;
}