PID Control in C | SPIKE Prime Follows a Square Course

In this project, we use two color sensors—one on each side of the line—and apply PID control (Proportional, Integral, Derivative) in C to build a stable and responsive robot.

In a previous article, I introduced how to build a basic line follower using a PID controller with a color sensor and C language on LEGO SPIKE Prime.

This time, I challenged the robot to follow a square-shaped line course, where sharp corners would test the stability and responsiveness of the control system.

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

Initial Test

First, I used the same PID control program from the previous article.

The robot was able to follow the course without going off track, even at the corners.

However, when turning sharp 90° corners, the body shook significantly, and the motion was far from smooth.

I decided to tweak the controller to make turns smoother.

Attempt 1: Reduce the Derivative Term (kd)

As a first adjustment, I weakened the derivative component to reduce overreaction at the corners.

In sharp corners, the difference between the sensor readings (error) changes suddenly.

The derivative term (D) reacts to these rapid changes, which can cause large spikes in the control output.

This leads to jerky or unstable motion at turns.

float kd = 1.0f;

　　　↓

float kd = 0.2f;

Unfortunately, this didn’t result in any significant improvement—the shaking persisted.

So I tried a different approach.

Attempt 2: Add Integral Windup Prevention

Next, I added a simple integral windup limiter to prevent the integral term from growing too large, which can cause sluggish or erratic behavior, especially after sudden changes like corners.

When the robot continues to detect an error (like in a long curve), the integral term (I) keeps accumulating.

This can cause overshooting or oscillation, especially when turning.

Apply a limit to the integral value (anti-windup) to prevent it from growing too large.

integral += error;

float derivative = error - prev_error;

if (integral > 3000) integral = 3000;

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

This adjustment made a big difference. The robot’s motion became noticeably smoother around the corners while still tracking the line accurately.

Demo Video and Code

#include <stdlib.h>

#include <kernel.h>

#include <spike/hub/system.h>

#include <lineFollowerSquare.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

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

// 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 showed how tuning even a simple PID controller can lead to significant improvements in motion, especially for real-world applications like robot competitions.

【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