Akshayan SinhaCETECH11
Published © GPL3+

Line Follower Robot - King of Maze

Automated item delivery through shortest available path - Useful in industries for pick and drop cases - no requirement of ROS.

AdvancedFull instructions provided3 hours511
Line Follower Robot - King of Maze

Story

Read more

Custom parts and enclosures

CAD Design

Code

PID Tuned Maze Solver (Dry+Actual)

C/C++
#include <PID_v1.h>
#include <ZumoMotors.h>
#include <ZumoReflectanceSensorArray.h>

// Sensor calibration values (determined experimentally)
const int SENSOR_COUNT = 6;
const int SENSOR_PINS[SENSOR_COUNT] = { 2, 3, 4, 5, 6, 7 };
const int SENSOR_THRESHOLDS[SENSOR_COUNT] = { 800, 800, 800, 800, 800, 800 };
const int SENSOR_OFFSETS[SENSOR_COUNT] = { 400, 400, 400, 400, 400, 400 };

// PID control parameters (tuned experimentally)
double kp = 0.2;
double ki = 0.0;
double kd = 0.0;
double target_speed = 300;
double max_speed = 400;
double min_speed = 150;

// Maze solving parameters
int maze[16][16];
int start_x = 0;
int start_y = 0;
int finish_x = 15;
int finish_y = 15;
int current_x = start_x;
int current_y = start_y;
int next_x = start_x;
int next_y = start_y;
bool is_following_line = true;
bool is_turning_left = false;
bool is_turning_right = false;
bool is_at_finish = false;
bool is_first_run = true;
unsigned long last_turn_time = 0;
unsigned long turn_duration = 500;
unsigned long current_time = 0;
unsigned long start_time = 0;
unsigned long elapsed_time = 0;

// Robot hardware
ZumoReflectanceSensorArray sensors(SENSOR_PINS, SENSOR_COUNT, SENSOR_TIMEOUT);
ZumoMotors motors;

// PID control objects
double input, output, setpoint;
PID pid(&input, &output, &setpoint, kp, ki, kd, DIRECT);

void setup() {
    Serial.begin(9600);
    sensors.init();
    motors.setSpeeds(0, 0);
    start_time = millis();
    setup_maze();
}

void loop() {
    current_time = millis();
    elapsed_time = current_time - start_time;
    update_sensor_readings();
    update_robot_state();
    update_pid_control();
    update_motor_speeds();
    update_line_following();
}

void update_sensor_readings() {
    sensors.readLine(sensor_values);
    for (int i = 0; i < SENSOR_COUNT; i++) {
        sensor_values[i] = max(0, sensor_values[i] - SENSOR_OFFSETS[i]);
    }
}

void update_robot_state() {
    if (is_at_finish) {
        motors.setSpeeds(0, 0);
        return;
    }

    if (is_following_line) {
        if (current_x == finish_x && current_y == finish_y) {
            is_at_finish = true;
            return;
        }
        if (is_first_run && current_x == finish_x) {
            turn_left();
            return;
        }
        if (is_first_run && current_y == finish_y) {
            turn_right();
            return;
        }
        if (is_turning_left) {
            if (current_time - last_turn_time >= turn_duration) {
                is_turning_left = false;
                is_following_line = true
                current_x = next_x;
                current_y = next_y;
            }
        }
        else if (is_turning_right) {
            if (current_time - last_turn_time >= turn_duration) {
                is_turning_right = false;
                is_following_line = true;
                current_x = next_x;
                current_y = next_y;
            }
        }
        else {
            if (is_first_run && maze[current_x][current_y + 1] == 0) {
                turn_left();
            }
            else if (maze[current_x][current_y - 1] == 0) {
                turn_right();
            }
            else if (maze[current_x + 1][current_y] == 0) {
                go_straight();
            }
            else if (maze[current_x][current_y + 1] == 0) {
                turn_left();
            }
            else {
                turn_around();
            }
        }
    }
    else {
        if (is_turning_left || is_turning_right) {
            return;
        }
        if (is_first_run && current_x == finish_x) {
            turn_left();
            return;
        }
        if (is_first_run && current_y == finish_y) {
            turn_right();
            return;
        }
        if (is_turning_left) {
            if (current_time - last_turn_time >= turn_duration) {
                is_turning_left = false;
                is_following_line = true;
                current_x = next_x;
                current_y = next_y;
            }
        }
        else if (is_turning_right) {
            if (current_time - last_turn_time >= turn_duration) {
                is_turning_right = false;
                is_following_line = true;
                current_x = next_x;
                current_y = next_y;
            }
        }
    }
}

void update_pid_control() {
    input = get_sensor_error();
    pid.Compute();
    setpoint = constrain(target_speed + output, min_speed, max_speed);
}

void update_motor_speeds() {
    if (is_turning_left) {
        motors.setSpeeds(-max_speed, max_speed);
    }
    else if (is_turning_right) {
        motors.setSpeeds(max_speed, -max_speed);
    }
    else {
        int speed_left = setpoint;
        int speed_right = setpoint;
        motors.setSpeeds(speed_left, speed_right);
    }
}

void update_line_following() {
    if (!is_following_line) {
        return;
    }

    int sensor_count_on_line = 0;
    for (int i = 0; i < SENSOR_COUNT; i++) {
        if (sensor_values[i] < SENSOR_THRESHOLDS[i]) {
            sensor_count_on_line++;
        }
    }

    if (sensor_count_on_line == 0) {
        is_following_line = false;
        turn_around();
        return;
    }

    double sensor_sum = 0;
    double weight_sum = 0;
    for (int i = 0; i < SENSOR_COUNT; i++) {
        if (sensor_values[i] < SENSOR_THRESHOLDS[i]) {
            double weight = pow((double)(SENSOR_COUNT - i - 1), 2.0);
            sensor_sum += (double)sensor_values[i] * weight;
            weight_sum += weight;
        }
    }

    double average_sensor_value = sensor_sum / weight_sum;
    double error = (double)LINE_FOLLOWING_THRESHOLD - average_sensor_value;

    double kP = 0.5;
    double kD = 0.2;
    double kI = 0.2;

    double previous_error = pid.GetLastError();
    double derivative = (error - previous_error) / (double)LOOP_TIME;
    pid.SetTunings(kP, kI, kD);
    pid.SetOutputLimits(-30, 30);
    pid.SetMode(AUTOMATIC);
    pid.SetControllerDirection(DIRECT);

    pid.SetSetpoint(0.0);
    pid.SetInput(error);
    pid.SetOutput(0.0);

    output = pid.Compute();

    update_pid_control();
    update_motor_speeds();
}

void turn_left() {
    is_following_line = false;
    is_turning_left = true;
    last_turn_time = current_time;
    motors.setSpeeds(-max_speed, max_speed);
}

void turn_right() {
    is_following_line = false;
    is_turning_right = true;
    last_turn_time = current_time;
    motors.setSpeeds(max_speed, -max_speed);
}

void turn_around() {
    is_following_line = false;
    is_turning_left = true;
    last_turn_time = current_time;
    motors.setSpeeds(-max_speed, max_speed);
}

void go_straight() {
    is_following_line = true;
    is_turning_left = false;
    is_turning_right = false;
    current_x += dx;
    current_y += dy;
    motors.setSpeeds(max_speed, max_speed);
}

void setup() {
    pinMode(LED_BUILTIN, OUTPUT);
    motors.setSpeeds(0, 0);
    delay(1000);
    initialize_maze();
    initialize_line_sensors();
    initialize_pid_control();
}

void loop() {
    current_time = millis();
    read_line_sensors();
    update_line_following();
    update_maze();
    delay(LOOP_TIME);
}

Credits

Akshayan Sinha

Akshayan Sinha

28 projects • 25 followers
Robotics Software Engineer with a makermind.
CETECH11

CETECH11

41 projects • 10 followers

Comments

Related channels and tags
  • Internet of Things
  • Robotics