Raspberry Pi Pico RP2040 – QTR-8RC – PID Line Follower Robot : Code and Project Files

Description

Specification of PID Line Follower Code

• Only the firmware is supplied through email or online download, with no accompanying hardware kit
• Algorithm: Proportional Integral Differential Algorithm (PID)
• Microcontroller Supported: Raspberry Pi Pico RP2040
• Sensor Support: QTR-8RC Sensor
• Special Feature: Auto Sensor Calibration, PWM Based Motor Control for Smooth Operation
• IDE Support: Pico SDK and Visual Studio Code
• Motor Driver Support: Dual Channel Motor Driver Support e.g. L293D, L298, TB6612FNG
• Motor Speed Support: Maximum 300RPM at 4cm Wheel Diameter
• Maximum Robot Speed: 0.3 meter per second [m/s] 
• Tutorial: Step By Step Guide

Code / Software Description

The code for the RP2040 Line Follower Robot is available in RP2040 Pico SDK and Visual Studio Code.

The firmware project has three library files rp2040_qtr.h , rp2040_motor.h and one main.c file. The library file code is written in C. The library files are all well documented. The APIs exposed can be used in Pico SDK. The programming language used in C.

/** @name QTR_CONTROL
    Public Functions to Control QTR Sensor
*/
void QTR_Init(uint8_t *SensorPin, uint8_t EmitterPin);
void QTR_DeInit();
void QTR_Calibrate(uint16_t *CalibratedMinimum, uint16_t *CalibratedMaximum, uint8_t ReadMode);
void QTR_CalibrateSensor(uint8_t ReadMode);
void QTR_ResetCalibration();
void QTR_ReadSensor(uint16_t *SensorValues, uint8_t ReadMode);
void QTR_ReadRaw(uint16_t *SensorValues);
void QTR_ReadCalibrated(uint16_t *SensorValues, uint8_t ReadMode);
QTR_Position QTR_ReadLine(uint16_t *SensorValues, uint8_t ReadMode);
inline static void QTR_EmitterOn();
inline static void QTR_EmitterOff();
/**@}*/

/** @name MOTOR_CONTROL
    Public Functions to Control Motor
*/
/**@{*/
void Motor_Init(void);
void Motor_DeInit(void);
void Motor_SetSpeed(int16_t M1Speed, int16_t M2Speed);
void Motor_SetSpeedM1(int16_t M1Speed);
void Motor_SetSpeedM2(int16_t M2Speed);
/**@}*/

PID Line Follower Robot Code Walkthrough

The first few lines of code declare all the variables needed for the PID algorithm.

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "rp2040_motor.h"
#include "rp2040_qtr.h"

int16_t LFR_MAX_MOTOR_SPEED = 400; /**< Sets the Maximum PWM Duty Cycle for Line Follower Robot 0=0% 1024=100% */

#define LFR_MAX_CALIBRATE_MOTOR_SPEED 300
#define LED_PIN 25

uint16_t LFR_SensorValue[QTR_SENSOR_COUNT]; /**< Array to Save Raw IR Sensor values of QTR-8RC */
QTR_Position Position;
int16_t LFR_Proportional = 0;     /**< Variable to Save the Proportional Output of PID Control Algorithm */
int16_t LFR_LastProportional = 0; /**< Variable to Save the Previous Proportional Output of PID Control Algorithm */
int16_t LFR_Derivative = 0;       /**< Variable to Save the Derivative Output of PID Control Algorithm */
int64_t LFR_Integral = 0;         /**< Variable to Save the Integral Output of PID Control Algorithm */
int16_t LFR_ControlOutput = 0;    /**< Variable to Save the Final Control Output of PID Control Algorithm */
int16_t LFR_Speed = 0;

float Kp = 9.0;
float Ki = 0.0;
float Kd = 250.0;

The function “LFR_Initialize()” initializes all used peripherals of the RP2040 . The QTR_X_PIN macros are defined in the rp2040_qtr.h file.

void LFR_Initialize()
{
  /**< Sets the Pin Mapping for QTR-8RC Sensor ; Change the Individual Pin Macros in avr_qtr.h */
  uint8_t QTR_Pins[] = {QTR_1_PIN, QTR_2_PIN, QTR_3_PIN, QTR_4_PIN, QTR_5_PIN, QTR_6_PIN, QTR_7_PIN, QTR_8_PIN};
  QTR_Init(QTR_Pins, QTR_EMITTER_PIN); /**< Initializes the QTR-8RC Sensor */
  Motor_Init();                        /**< Initializes the Motors */

  gpio_init(LED_PIN);
  gpio_set_dir(LED_PIN, GPIO_OUT);

  sleep_ms(3000); /**< Pause ; Useful to Align the Robot Manually if Outside the Line */

  printf("\r\n\r\nProgram Last Built : %s :: %s\n", __DATE__, __TIME__);
  printf("Using PID values (%f, %f, %f)\n", Kp, Ki, Kd);
}

The function “LFR_Calibrate()” calibrates the QTR-8RC line sensor. The robot rotates in the same place and calibrates the sensor with all possible sensor values sampled during this time.

void LFR_Calibrate()
{
  Motor_SetSpeed(90, -90);         /**< Rotates the Robot 90, -90 300RPM */
  for (uint8_t i = 0; i < 40; i++) /**< Calibrate the QTR-8RC Sensor */
  {
    QTR_CalibrateSensor(QTR_EMITTERS_ON);
    delay(20);
  }
  Motor_SetSpeed(0, 0);            /**< Stops the Robot */
  delay(500);
  Motor_SetSpeed(-90, 90);         /**< Rotates the Robot */
  for (uint8_t i = 0; i < 80; i++) /**< Calibrate the QTR-8RC Sensor */
  {
    QTR_CalibrateSensor(QTR_EMITTERS_ON);
    delay(20);
  }
  Motor_SetSpeed(0, 0);            /**< Stops the Robot */
  delay(500);
  Motor_SetSpeed(90, -90);         /**< Rotates the Robot */
  for (uint8_t i = 0; i < 40; i++) /**< Calibrate the QTR-8RC Sensor */
  {
    QTR_CalibrateSensor(QTR_EMITTERS_ON);
    delay(20);
  }
  Motor_SetSpeed(0, 0);         /**< Stops the Robot */
  delay(2000);                  /**< Pause ; Useful to Realign the Robot Manually if Outside the Line */
}

The infinite while loop below is responsible for the PID algorithm. It is good to note that PID coefficients depend heavily on physical parameters like the size of the robot, weight, proportions, battery voltage, friction, etc. It is always advised to tune your robot after any hardware change.

int main()
{
  stdio_init_all();
  LFR_Initialize();
  printf("LFR_Initialize, Line Follower Robot!\n");

  // If Cablibrate is used, please change the code in rp2040_qtr.h function : QTR_ReadLine : Line 408 to use calibrated values
  // LFR_Calibrate();
  // printf("Line Follower Calibrated!\n");

  while (1)
  {

    Position = QTR_ReadLine(LFR_SensorValue, QTR_EMITTERS_ON); /**< Reads the QTR-8RC Line Sensor to Get the Line Position */

    if (Position.LFR_Position == ON_LINE)
    {
      gpio_put(LED_PIN, true);

      LFR_Proportional = Position.Sensor_Position - QTR_LINE_MID_VALUE; /**< Computes the Proportional Output of PID Control Algorithm */
      LFR_Derivative = LFR_Proportional - LFR_LastProportional;         /**< Computes the Derivative Output of PID Control Algorithm */
      LFR_Integral += LFR_Proportional;                                 /**< Computes the Integral Output of PID Control Algorithm */
      LFR_LastProportional = LFR_Proportional;                          /**< Saves the Old Proportional Output of PID Control Algorithm */

      LFR_ControlOutput = LFR_Proportional * Kp + LFR_Integral * Ki + LFR_Derivative * Kd; /**< Computes the Final Control Output of PID Control Algorithm 800RPM*/

      if (LFR_ControlOutput > LFR_MAX_MOTOR_SPEED)
      {
        LFR_ControlOutput = LFR_MAX_MOTOR_SPEED; /**< Keeps The Motor Speed in Limit */
      }
      if (LFR_ControlOutput < -LFR_MAX_MOTOR_SPEED)
      {
        LFR_ControlOutput = -LFR_MAX_MOTOR_SPEED; /**< Keeps The Motor Speed in Limit */
      }

      if (LFR_ControlOutput < 0)
      {
        Motor_SetSpeed(LFR_MAX_MOTOR_SPEED + LFR_ControlOutput, LFR_MAX_MOTOR_SPEED); /**< Drives the Motor According to the Control Output */
      }
      else
      {
        Motor_SetSpeed(LFR_MAX_MOTOR_SPEED, LFR_MAX_MOTOR_SPEED - LFR_ControlOutput); /**< Drives the Motor According to the Control Output */
      }
    }
    else if (Position.LFR_Position == ON_WHITE)
    {
      gpio_put(LED_PIN, false);
      Motor_SetSpeed(0, 0);
      LFR_Integral = 0;
      LFR_LastProportional = 0;
      LFR_Derivative = 0;
      printf("ON_WHITE Place LFR at beginning of line in 4 seconds\n");
      sleep_ms(4000);
    }
    else if (Position.LFR_Position == ON_BLACK)
    {
      gpio_put(LED_PIN, false);
      Motor_SetSpeed(0, 0);
      LFR_Integral = 0;
      LFR_LastProportional = 0;
      LFR_Derivative = 0;
      printf("ON_BLACK Place LFR at beginning of line in 4 seconds\n");
      sleep_ms(4000);
    }
  }

  return 0;
}

The speed of the line follower robot is controlled by the macro LFR_MAX_MOTOR_SPEED . It is advised to recalibrate the robot at a higher speed for optimum performance.

#define LFR_MAX_MOTOR_SPEED 400 /**< Sets the Maximum PWM Duty Cycle for Line Follower Robot 0=0% 1024=100% */
#define LFR_MAX_CALIBRATE_MOTOR_SPEED 300

Programming The Raspberry Pi Pico RP2040 Line Follower Robot

The project folder has all the required files. If you use Pico Visual Studio Code IDE then open the project folder and flash the Raspberry Pi Pico RP2040 either using the drag and drop method or by OpenGDB. I personally recommend using another Pico as a debugger and flash the binary using it.