How to tune a line follower robot PID

[digitalectron]

Hi,
This is my first post in eevblog, I’m working on a Line following robot, I’m using stm32G030K as the main controller, and I have connected 4 reflective sensors for the line position and two motor drivers for my motors, I have tried a lot to tune and write a good code so that my robot would do it’s best to follow the line and have good speed, But if I increase my motor speeds it would not follow the line Here is the main parts of the code that would do the job,

checkout the robot in action
https://www.youtube.com/shorts/eJNDz7MstAs

Code: [Select]

int MyPID_kp =50, MyPID_ki = 0, MyPID_kd = 2;
Calibration();
while(1){
MyPID_SetOutputLimits(-5000, 5000);/*Limt my PID output*/
Input = Calculate_LinePosition(); /* calculate Line Position from IR Sensor*/
Output = MyPID(MyPID_kp, MyPID_ki, MyPID_kd,Input);/* calculate PUD parameter*/
RunMotors(Output); // send PID output to PWM for running Motor*/
}


After reset I would calibrate the sensors like this

Code: [Select]

void Calibration()
{
 
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); // left
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); // right
  Tm0 = HAL_GetTick();
  Tm1 = Tm0;
  unsigned long TmL;
  for (uint8_t i = 0; i < IR; i++)
  {
    SenX[i] = ReadSensors(i);
    MinX[i] = SenX[i];
    MaxX[i] = SenX[i];
  }
  while ((HAL_GetTick() - Tm0) <= 2000)
  {
    for (uint8_t i = 0; i < IR; i++)
    {
      SenX[i] = ReadSensors(i);
      if (SenX[i] < MinX[i])
        MinX[i] = SenX[i];
      if (SenX[i] > MaxX[i])
        MaxX[i] = SenX[i];
    }
    TmL = HAL_GetTick();
    if ((TmL - Tm1) >= 100)
    {
      Tm1 = TmL;
    }
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6, GPIO_PIN_RESET);
    TIM1->CCR3 = 2500; // right
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);
    TIM1->CCR4 = 500; // left
  }
  HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // left
  HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // right
  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6, GPIO_PIN_RESET);
  TIM1->CCR3 = 0; // right
  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET);
  TIM1->CCR4 = 0; // left
}

the MyPID_SetOutputLimits would limit the PID output

Code: [Select]

/*Limit my PID output*/
void MyPID_SetOutputLimits(int Min, int Max)
{
   if(Min > Max) return;
   outMin = Min;
   outMax = Max;
   
   if(Output > outMax) Output = outMax;
   else if(Output < outMin) Output = outMin;
 
   if(ITerm> outMax) ITerm= outMax;
   else if(ITerm< outMin) ITerm= outMin;
}
 

I have used matlab line interpolation for finding polynomials to calculate the line position here is the function code

Code: [Select]

int Calculate_LinePosition(void)
{

  Line1 = LinePosition;

  for (int i = 0; i < IR; i++)
  {
    SenX[i] = ReadSensors(i);
    // SenX[i] = Map_Sensors(SenX[i], MinX[i], MaxX[i], 800, 3100);
    SenX[i] = Map_Sensors(SenX[i], MinX[i], MaxX[i], targetMin, targetMax);
    SenX[i] = Constrain_Sensors(SenX[i], targetMin, targetMax);
  }

  minimum = SenX[activeSensor];
  if (SenX[1] < 3000 || SenX[2] < 3000 || SenX[3] < 3000)
  {
    for (int i = 0; i < IR; i++)
    {
      if (SenX[i] < minimum)
      {
        minimum = SenX[i];
        activeSensor = i;
      }
    }
  }

  // We obtain P1,P2,P3,P4 from basic fitting in matlab
  // SenX[0]=SenX[0]+700;
  if (activeSensor == 0)
  {

    if (SenX[1] > 2900)
    {

      p1 = -0.010884;
      p2 = 35.9846;
    }
    else
    {
      p1 = 0.0092604;
      p2 = 23.3772;
    }
  }

  else if (activeSensor == 1)
  {
    if (SenX[0] < 2000)
    {
      p1 = -0.0069286;
      p2 = 51.6287;
    }
    else
    {
      p1 = 0.0069076;
      p2 = 39.8585;
    }
  }
  else if (activeSensor == 2)
  {
    if (SenX[1] < 3000)
    {
      p1 = -0.0061269;
      p2 = 70.4583;
    }
    else
    {
      p1 = 0.0072529;
      p2 = 61.0744;
    }
  }
  else if (activeSensor == 3)
  {
    if (SenX[2] < 2800)
    {
      p1 = -0.010154;
      p2 = 95.2504;
    }
    else
    {
      p1 = 0.0075137;
      p2 = 78.6127;
    }
  }

  LinePosition = (p1 * SenX[activeSensor] + p2);
  if (SenX[0] > 3000 && SenX[1] > 2800)
  {
    // activeSensor = 0;
  }
  if (10 < LinePosition && LinePosition < 50)
  {
    direction = 0;
  }
  else if (50 < LinePosition && LinePosition < 90)
  {
    direction = 1;
  }
  if (LinePosition > 90)
  {
    LinePosition = 100;
  }

  if (runLineRobot == false)
  {
  }
  else
  {
    // if (SenX[0]>1000 && SenX[0]<2000 && SenX[1]>2000&& SenX[2]>1500){
    // LinePosition=50;
    // MODE_LEFT=true;
  }
  if (SenX[0] > 2000 && activeSensor == 0)
  {
    if (direction == 0)
    {
      LinePosition = 0;
    }
    if (direction == 1)
    {
      LinePosition = 100;
    }
  }
  return LinePosition;
}



the actual PID function

Code: [Select]

int MyPID(int Kp, int Ki, int Kd,int input){
long P = 0, I = 0, D = 0, PIDv = 0, pErr = 0;
MyPID_error=Setpoint - input;
P = MyPID_error;
    I = I + MyPID_error;
    D = MyPID_error - pErr;


PIDv = 3 * ((Kp * MyPID_error) + (Ki * (I + MyPID_error)) + (Kd * (MyPID_error - pErr)));

    pErr = MyPID_error;
return PIDv;

}


The function that would run the motors

Code: [Select]

void RunMotors(int16_t OutPutPID)
{

  if (AllowMotorsRun)
  {
    PIDValue = mapOutPutPID(OutPutPID, -5000, 5000, -1500, 1500);

    if (OutPutPID > 0)
    {                                          // left
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // right
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // left
      MotorLeft = 0 - PIDValue;                // the power level for the A motor
      MotorRight = 0 - PIDValue;               // the power level for the C motor
      SpeedLeft = BaseSpeed - PIDValue;
      SpeedRight = BaseSpeed + PIDValue;
    }
    else if (OutPutPID < 0)

    {                                          // Right
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // right
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // left
      MotorLeft = PIDValue;                    // the power level for the A motor
      MotorRight = PIDValue;                   // the power level for the C motor
      SpeedLeft = BaseSpeed - PIDValue;
      SpeedRight = BaseSpeed + PIDValue;
    }
    else
    {

      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // left
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // right
      SpeedLeft = BaseSpeed;
      SpeedRight = BaseSpeed;
    }

    if (SpeedLeft < SpeedMin)
      SpeedLeft = SpeedMin;
    if (SpeedLeft > SpeedMax)
      SpeedLeft = SpeedMax;
    if (SpeedRight < SpeedMin)
      SpeedRight = SpeedMin;
    if (SpeedRight > SpeedMax)
      SpeedRight = SpeedMax;

    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); // rifht
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); // left

    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6, GPIO_PIN_RESET); // right
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET); // left

    BATTERY_VOLTAGE = BAT_GetVoltag(adcData[IR]);
    // RPM=(BATTERY_VOLTAGE-350)*0.975+150;
    // TIM1->CCR4 = SpeedLeft-(BATTERY_VOLTAGE-350)*0.6;  // Left
    //    TIM1->CCR3 = SpeedRight-(BATTERY_VOLTAGE-350)*0.6;  // Left

    TIM1->CCR4 = SpeedLeft;  // Left
    TIM1->CCR3 = SpeedRight; // Right
  }
}

I'm out of Ideas and I do not know what I have done wrong, do you have any Idea what I should do?

[Terry Bites]

Determine the open loop characteristic of the system and go from there.
ie stimulate the controller with a short pulse and scope the motor response.
D for undershoot, I for overshoot.
Do you need all the PID terms? Often D is not required.
I sets the damping and that may be enough.

[Smokey]

I could see how having a physical system without an absolute position feedback like that would make tuning the gains hard.  For a current loop for instance, you can hook a current sensor up and watch the step response on a scope while you tweak the gains.  How do you do that for a robot in the middle of the room trying to follow a line?  I guess you could use an external camera pointed at the robot and some computer vision and actually quantify how much hunting/oscillation happens as it's trying to follow the line around a corner or something.

[digitalectron]

Determine the open loop characteristic of the system and go from there.

How can I do that for this robot? do you suggest any work around?

How do you do that for a robot in the middle of the room trying to follow a line?  I guess you could use an external camera pointed at the robot and some computer vision and actually quantify how much hunting/oscillation happens as it's trying to follow the line around a corner or something.

I have seen many kids in robot races do this, I doubt that even one of them use Computer vision, I know it can be used, But I have lack of knowledge there, do you suggest simpler ways?

[MarkF]

I've always struggled tuning PID controllers.
Here's a little reading that might give you some insight:

  PID controllers (phenomenological introduction)
  PID controllers (analytical introduction)
  PID control of a 1D helicopter

[digitalectron]

MarkF Thanks for the links, I need some tome to study them, But if someone here  that has the experience would help, I would appreciate it a lot.

[Andy Chee]

Arghh. I’m encountering similar issues PID tuning a homemade hexcopter drone!

Anyway I found this video a somewhat helpful in understanding the intuitive behaviour of a system’s response to changing PID values:

https://youtube.com/watch?v=qKy98Cbcltw

But what is immediately helpful is having some direct physical adjustment of parameters (in the case of the video, knobs!).

On a line follower robot (or drone in my case) knobs are somewhat impractical. So you need a remote control of some sort. Or you could have a PC/Mac application with on screen tuning sliders/knobs.