PID Line Following at the Crossroad | SPIKE Prime in C

In my previous article, I tuned a PID-controlled line follower to handle sharp corners on a square course using LEGO SPIKE Prime and C. This time, I challenged the robot to handle a cross-shaped (plus-shaped) line course, which includes intersections requiring decision-making—such as turning.

We used the same robot as in the last article, with left and right motors and dual color sensors.

Supplies

LEGO SPIKE Prime
2 × LEGO Color Sensors
2 × Motors (left and right wheels)
1 PC with Windows 10 or 11 operating system

Go Straight Through the Intersection

First, I tested the original PID line follower program without any changes.

The result: It handled straight crossings without any problem, continuing to follow the line even across the intersection.

This showed that the basic PID control is stable enough to pass through an intersection when no turn is needed.

Detect and Stop at the Intersection

Next, I tried to make the robot turn right at the intersection.

The first step was getting the robot to recognize the intersection and stop.

I modified the program so that when both left and right color sensors detect black, the robot treats it as a crossroads and stops.

if (valL < THRESHOLD && valR < THRESHOLD) {

pup_motor_stop(motorL);

pup_motor_stop(motorR);

dly_tsk(500000);

}

This worked well—the robot consistently stopped at the correct spot.

Turn Right From the Stop

After stopping, I added code for the robot to rotate in place to the right.

Since the color sensors are placed in front of the wheels, I had to adjust the timing and rotation to align the robot properly after the turn.

With a little fine-tuning, the robot was able to turn right smoothly and rejoin the line on the new path.

#define THRESHOLD 30

#define TURN_TIME 570000

if (valL < THRESHOLD && valR < THRESHOLD) {

pup_motor_stop(motorL);　　　　　　

pup_motor_stop(motorR);　　　　　　

dly_tsk(500000);　　　　　　　　　　

pup_motor_set_power(motorL, 50);　　

pup_motor_set_power(motorR, -25);　　

dly_tsk(TURN_TIME);　　　　　　　　　

pup_motor_stop(motorL);　　　　　　　

pup_motor_stop(motorR);　　　　　　　

dly_tsk(200000);　　　　　　　　　　　

}

Demo Video and Code

#include <stdlib.h>

#include <kernel.h>

#include <spike/hub/system.h>

#include <lineFollowerCrossroads.h>

#include "spike/pup/motor.h"

#include "spike/pup/colorsensor.h"

#include <pbio/color.h>

pup_motor_t *motorR, *motorL;

pup_device_t *ColorSensorR, *ColorSensorL;

void Main(intptr_t exinf)

{

// Wait for startup (3s)

dly_tsk(3000000);

// PID control variables

int32_t valL, valR; // Reflection values from left and right sensors

int32_t lspeed, rspeed; // Motor speeds for left and right

int32_t base_speed = 30; // Base speed for motors

float mv; // Manipulated variable

float kp = 0.8f; // Proportional gain

float ki = 0.00005f; // Integral gain

float kd = 0.2f; // Derivative gain

float integral = 0.0f; // Integral term

float prev_error = 0.0f; // Previous error

#define THRESHOLD 30

#define TURN_TIME 570000

// Initialize motors and sensors

motorR = pup_motor_init(PBIO_PORT_ID_A,PUP_DIRECTION_CLOCKWISE);

motorL = pup_motor_init(PBIO_PORT_ID_B,PUP_DIRECTION_COUNTERCLOCKWISE);

ColorSensorR = pup_color_sensor_get_device(PBIO_PORT_ID_C);

ColorSensorL = pup_color_sensor_get_device(PBIO_PORT_ID_D);

while(1) {

// Get sensor values

valL = pup_color_sensor_reflection(ColorSensorL);

valR = pup_color_sensor_reflection(ColorSensorR);

if (valL < THRESHOLD && valR < THRESHOLD) {

pup_motor_stop(motorL);

pup_motor_stop(motorR);

dly_tsk(500000);

pup_motor_set_power(motorL, 50);

pup_motor_set_power(motorR, -25);

dly_tsk(TURN_TIME);

pup_motor_stop(motorL);

pup_motor_stop(motorR);

dly_tsk(200000);

}

// Calculate error (difference between right and left sensor values)

float error = (float)valR - (float)valL;

// PID calculation

integral += error;

float derivative = error - prev_error;

if (integral > 3000) integral = 3000;

if (integral < -3000) integral = -3000;

mv = kp * error + ki * integral + kd * derivative;

prev_error = error;

// Calculate motor speeds based on PID output

lspeed = base_speed - (int32_t)mv;

rspeed = base_speed + (int32_t)mv;

// Limit motor speeds

if (lspeed > 100) lspeed = 100;

if (lspeed < -100) lspeed = -100;

if (rspeed > 100) rspeed = 100;

if (rspeed < -100) rspeed = -100;

// Set motor power

pup_motor_set_power(motorL,lspeed);

pup_motor_set_power(motorR,rspeed);

}

Conclusion

This experiment adds basic intersection handling to our line follower, showing how C programming on SPIKE Prime can be extended step-by-step for more complex tasks—useful for robotics competitions or education alike.

【Free Trial】Start C Language Programming With Afrel's Educational Materials

Are you intrigued by the idea of programming your SPIKE™ Prime with C? Did you know that Afrel is a key information provider for SPIKE™ Prime?

To meet the demand of those who want to "learn more about SPIKE™ Prime " and "try programming in C," Afrel has developed and sells educational materials for learning C language programming with SPIKE™ Prime.

Stepping up from Python to C is an excellent opportunity to further enhance your programming skills. Learning C will give you a deeper understanding of how computers work and enable more advanced control.

Afrel's C language materials are designed for beginners, ensuring a smooth learning experience. Why not take this chance to discover the joy of controlling SPIKE™ Prime with C and expand your programming horizons?

If you're ready to tackle C language programming, click the link below for more details.

Learn more about SPIKE™ Prime C Language Programming Materials here