Hello,
I wrote the following RobotC program and I have imbedded a PID controller. It works just fine with my robot. However, the PID gains must be tuned based on the robot physical configuration. Robot Speed must also be taken as main factor that influence the PID tuning.
My next step is to make it faster and better but in order to do that I have to use a better way to achieve a very very very good PID setup...
I wrote a C# program to help with PID tuning. I can use it to analyze how the PID controller behave based on the input that is the Error that I need to fix...
I would like to thanks Tim Wescott who wrote "PID Without a PhD"
he gaves me some hints on what a PID controller is...
http://www.embedded.com/2000/0010/0010feat3.htm
You can find my robot movie on google VIDEO.
Look for "LEGO based Robot using a PID controller to follow a line"
I just published today (Sunday) theh I expect to have it available Monday...
and NOW the RobotC program............................................ Enjoy!
//********************************************************
//********************************************************
//*!!Sensor, S1, LightLeft, sensorLightActive, , !!*//
//*!!Sensor, S3, LightRight, sensorLightActive, , !!*//
//*!!Motor, motorB, motorB, tmotorNxtEncoderClosedLoop, !!*//
//*!!Motor, motorC, motorC, tmotorNxtEncoderClosedLoop, !!*//
//*!! !!*//
//*!!Start automatically generated configuration code. !!*//
const tSensors LightLeft = (tSensors) S1; //sensorLightActive //*!!!!*//
const tSensors LightRight = (tSensors) S3; //sensorLightActive //*!!!!*//
const tMotor motorB = (tMotor) motorB; //tmotorNxtEncoderClosedLoop //*!!!!*//
const tMotor motorC = (tMotor) motorC; //tmotorNxtEncoderClosedLoop //*!!!!*//
//*!!CLICK to edit 'wizard' created sensor & motor configuration. !!*//
//* ********************************************************** *
//* * PID Global variables and array
//* ********************************************************** *
//* * dState Last position input
//* * iState Integrator state
//* * iMax, iMin Maximum and minimum allowable integrator state
//* * iGain Integral gain
//* * pGain Proportional gain
//* * dGain Derivative gain
float dState[2];
float iState[2];
float iMax[2];
float iMin[2];
float pGain[2];
float iGain[2];
float dGain[2];
//* ********************************************************** *
//* * Global variable used for Light sensor
//* ********************************************************** *
int nLeft;
int nRight;
float fError= 0.0;
float fsError= 0.0;
//* ********************************************************** *
//* * Light Sensor data acquiring (front left and right)
//* ********************************************************** *
task tLightSensors()
{
nSchedulePriority = kDefaultTaskPriority;
SensorMode[LightLeft] = modePercentage;
SensorMode[LightRight] = modePercentage;
while(true)
{
wait1Msec(3);
nLeft = SensorValue(LightLeft);
nRight = SensorValue(LightRight);
fError = (nRight - nLeft);
if (fError > fsError)
fsError = fError;
}
return;
}
//* ********************************************************** *
//* * PID controller
//* ********************************************************** *
float UpdatePID(int x, float error,float position)
{
float pTerm = 0.0, dTerm = 0.0, iTerm = 0.0, result = 0.0;
pTerm = pGain[x] * error;
// calculate the proportional term
// calculate the integral state with appropriate limiting
iState[x] += error;
if (iState[x] > iMax[x])
iState[x] = iMax[x];
else
if (iState[x] < iMin[x])
iState[x] = iMin[x];
iTerm = iGain[x] * iState[x]; // calculate the integral term
dTerm = dGain[x] * (position - dState[x]);
dState[x] = position;
result = pTerm + iTerm - dTerm;
return (result);
}
//* ********************************************************** *
//* * Arbiter nPower is the PID CMD
//* ********************************************************** *
void Arbiter(int nPower)
{
int outputL = 0;
int outputR = 0;
//* * Servo motors initial setup.
bFloatDuringInactiveMotorPWM = false;
nMaxRegulatedSpeed = 2160;
nPidUpdateInterval = 1;
nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;
nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg;
motor[motorB] = nPower;
motor[motorC] = nPower;
//* ********************************************************** *
//* * PID save area.
//* ********************************************************** *
dState[0] = 0.0;
iState[0] = 0.0;
//* ********************************************************** *
//* * Tune the PID using gains parameters
//* ********************************************************** *
pGain[0] = 2.95;
iGain[0] = 0.0;
dGain[0] = 0.026;
//* ********************************************************** *
//* * Percent of the input cmd (motor power)
//* ********************************************************** *
iMin[0] = (nPower / 100.0) * 25.0;
iMax[0] = (nPower / 100.0) * 75.0;
//* ********************************************************** *
//* *
//* ********************************************************** *
while (true)
{
//* ********************************************************** *
//* * call the PID controller (Left motor-plant)
//* ********************************************************** *
if (fError < 0.0)
{
outputL = nPower - (int) UpdatePID(0,abs(fError),abs(fError));
if (outputL > nPower) outputL = nPower;
if (outputL < 0) outputL = 0;
motor[motorC] = outputL;
wait1Msec(1.0);
continue;
}
//* ********************************************************** *
//* * call the PID controller (Right motor-plant)
//* ********************************************************** *
if (fError > 0.0)
{
outputR = nPower - (int) UpdatePID(0,abs(fError),abs(fError));
if (outputR > nPower) outputR = nPower;
if (outputR < 0) outputR = 0;
motor[motorB] = outputR;
wait1Msec(1.0);
continue;
}
}
motor[motorB] = 0;
motor[motorC] = 0;
while(nMotorRunState[motorB] != runStateIdle);
return;
}
//* ********************************************************** *
//* * Main program
//* ********************************************************** *
task main()
{
nSchedulePriority = kDefaultTaskPriority;
eraseDisplay();
//* ********************************************************** *
//* * Start Light sensors
//* ********************************************************** *
StartTask(tLightSensors);
wait1Msec(1000);
//* ********************************************************** *
//* * Call the Arbiter
//* ********************************************************** *
Arbiter(40);
//* ********************************************************** *
//* * Stop all tasks
//* * Exit
//* ********************************************************** *
StopAllTasks();
return;
}