AI-gestuurde lijnvolgende robot op Arduino Nano

Onderdelen en benodigdheden

• Arduino Nano (1×)
• Seeed Wireless Bluetooth RF Transceiver Master/Slave (1×)
• IR Infrarood Sensor Module Reflectief (1×)
• Sony Continuous Rotation Servo (1×)
• 4,8 V accupakket (1×)

Apps en online diensten

• Arduino IDE

Over dit project

Na het bouwen van een Arduino Pick & Place-robot wilde ik een AI-gebaseerde lijnvolgende robot maken. Een simpele lijnvolger is goed voor beginners; de uitdaging was om kunstmatige intelligentie toe te voegen. Online onderzoek hielp me enorm.

Waarom een AI-gebaseerde lijnvolger?

AI is de technologie die de wereld kan veranderen. Door robots intelligentie te geven om reële problemen op te lossen, maken we een stap naar die toekomst. Toepassingen: productielijnen, zelfrijdende bussen, etc. Mijn prototype is een begin – ik leg uit hoe je het kunt bouwen met eenvoudige code die in de Arduino IDE werkt.

Ondersteun me op Patreon: https://bit.ly/31NfQ6A

Goedkoopste onderdelen in de VS vind je in de videobeschrijving.

Infraroodsensoren

De robot gebruikt 7 infraroodsensoren:

• 5 voor PID-regeling (lijndetectie).
• 1 voor linkerdetectie (ver links).
• 1 voor rechterdetectie (ver rechts).

De 5 PID-sensoren geven digitale uitgang (1 = zwarte lijn gedetecteerd). Mogelijke patronen (van links naar rechts):

10000  11000  01000  01100  00100
00110  00010  00011  00001  00000  11111

De twee buitenste sensoren geven analoge waarden.

Opslag in arrays:

int LFSensor[5] = {1,1,1,1,1};
int farLeftSensor = 0, farRightSensor = 0;

LFSensor[0] = digitalRead(lineFollowSensor0);
LFSensor[1] = digitalRead(lineFollowSensor1);
LFSensor[2] = digitalRead(lineFollowSensor2);
LFSensor[3] = digitalRead(lineFollowSensor3);
LFSensor[4] = digitalRead(lineFollowSensor4);
farRightSensor = analogRead(farRightSensorPin);
farLeftSensor  = analogRead(farLeftSensorPin);

Doolhof Left-Hand Rule

Bekijk deze video voor uitleg van de left-hand rule.

Kortom: plaats linkerhand op muur, loop vooruit, houd hand op muur bij elke kruising. Je bereikt de uitgang (niet noodzakelijk kortst).

In een 2D-doolhof zijn er 8 typen kruisingen (zie afbeelding). De left-hand rule vereenvoudigt de acties tot één optie per kruising:

• Kruis → Links
• T-kruising → Links
• Alleen rechts → Rechts
• Alleen links → Links
• Rechtdoor of links → Links
• Rechtdoor of rechts → Rechtdoor
• Doodlopend → Keer om (180°)
• Einde doolhof → Stop

Om het type kruising te bepalen, moet de robot een extra stapje maken en opnieuw sensoren lezen.

Stroomdiagram:

• Doodlopend: keer om.
• Lijn (continu): extra stap → lijn? dan Kruis → Links; geen lijn? dan T → Links; andere lijn? dan Einde → Stop.
• Rechtsaf: extra stap → lijn? dan Rechtdoor/Rechts → Rechtdoor; geen lijn? dan Alleen rechts → Rechts.
• Linksaf: extra stap → lijn? dan Rechtdoor/Links → Links; geen lijn? dan Alleen links → Links. (Test kan overgeslagen worden want actie is altijd Links.)

Mogelijke problemen

Arduino driver issues? Bekijk deze video voor oplossing.

Code

De code is verdeeld in meerdere tabs:

• RoBot_Maze_Solve_2 (hoofdprogramma)
• Functions
• GeneralFunctions
• sensorFuntions
• RobotDefines.h
• MotorFuntions

RoBot_Maze_Solve_2 (C/C++)

/*------------------------------------------------------------------
  7 sensors Smart Robot - Maze Solver and Line Follower
  met programmeerbare PID-controller via BT
  ==> Basismoving gebaseerd op Nano Mouse Robot (Michael Backus)
  ==> Lijnvolgen gebaseerd op PID-tutorial (samvrit.tk)
  ==> Path Solving Code credit: Patrick McCabe
  Marcelo Jose Rovai - 23 april 2016 - https://mjrobot.org
------------------------------------------------------------------*/
#include <Servo.h>
#include "robotDefines.h"
String command, device;
#include <SoftwareSerial.h>
SoftwareSerial BT1(10, 11); // Rx, Tx

void setup() {
  Serial.begin(9600); BT1.begin(9600);
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(lineFollowSensor0, INPUT);
  pinMode(lineFollowSensor1, INPUT);
  pinMode(lineFollowSensor2, INPUT);
  pinMode(lineFollowSensor3, INPUT);
  pinMode(lineFollowSensor4, INPUT);
  leftServo.attach(5); rightServo.attach(3);
  BT1.print("check the PID constants to be sent to Robot");
  Serial.print("check the PID constants to be sent to Robot");
  BT1.println('\n');
  while (digitalRead(buttonPin) && !mode) {
    checkBTcmd(); manualCmd(); command = "";
  }
  checkPIDvalues();
  mode = STOPPED; status = 0;
}

void loop() {
  ledBlink(1);
  BT1.println("Start First Pass"); Serial.println("Start First Pass");
  readLFSsensors(); mazeSolve();
  ledBlink(2);
  BT1.println("End First Pass"); Serial.println("End First Pass");
  while (digitalRead(buttonPin) && !mode) {
    checkBTcmd(); manualCmd(); command = "";
  }
  BT1.println("Starting 2nd Pass"); Serial.println("Starting 2nd Pass");
  pathIndex = 0; status = 0; mazeOptimization();
  ledBlink(3);
  BT1.println("End 2nd Pass"); Serial.println("End 2nd Pass");
  while (digitalRead(buttonPin) && !mode) {
    checkBTcmd(); manualCmd(); command = "";
  }
  mode = STOPPED; status = 0; pathIndex = 0; pathLength = 0;
}

Functions (C/C++)

void mazeSolve(void) {
  while (!status) {
    readLFSsensors();
    switch (mode) {
      case NO_LINE: motorStop(); goAndTurn(LEFT, 180); recIntersection('B'); break;
      case CONT_LINE:
        runExtraInch(); readLFSsensors();
        if (mode != CONT_LINE) { goAndTurn(LEFT, 90); recIntersection('L'); }
        else mazeEnd();
        break;
      case RIGHT_TURN:
        runExtraInch(); readLFSsensors();
        if (mode == NO_LINE) { goAndTurn(RIGHT, 90); recIntersection('R'); }
        else recIntersection('S');
        break;
      case LEFT_TURN:
        goAndTurn(LEFT, 90); recIntersection('L'); break;
      case FOLLOWING_LINE: followingLine(); break;
    }
  }
}

void recIntersection(char direction) {
  path[pathLength] = direction; pathLength++; simplifyPath();
}

void mazeEnd(void) {
  motorStop();
  BT1.print("The End ==> Path: "); for(int i=0;i "); Serial.println(pathLength);
  status = 1; mode = STOPPED;
}

void followingLine(void) { calculatePID(); motorPIDcontrol(); }

void simplifyPath() {
  if (pathLength < 3 || path[pathLength-2] != 'B') return;
  int totalAngle = 0;
  for (int i=1; i<=3; i++) {
    switch (path[pathLength-i]) {
      case 'R': totalAngle += 90; break;
      case 'L': totalAngle += 270; break;
      case 'B': totalAngle += 180; break;
    }
  }
  totalAngle %= 360;
  switch (totalAngle) {
    case 0: path[pathLength-3] = 'S'; break;
    case 90: path[pathLength-3] = 'R'; break;
    case 180: path[pathLength-3] = 'B'; break;
    case 270: path[pathLength-3] = 'L'; break;
  }
  pathLength -= 2;
}

void mazeOptimization(void) {
  while (!status) {
    readLFSsensors();
    switch (mode) {
      case FOLLOWING_LINE: followingLine(); break;
      case CONT_LINE:
      case LEFT_TURN:
      case RIGHT_TURN:
        if (pathIndex >= pathLength) mazeEnd();
        else { mazeTurn(path[pathIndex]); pathIndex++; }
        break;
    }
  }
}

void mazeTurn(char dir) {
  switch (dir) {
    case 'L': goAndTurn(LEFT, 90); break;
    case 'R': goAndTurn(RIGHT, 90); break;
    case 'B': goAndTurn(RIGHT, 800); break;
    case 'S': runExtraInch(); break;
  }
}

GeneralFunctions (C/C++)

void ledBlink(int times) {
  for (int i=0; i 0) {
    Serial.print("Command received from BT ==> "); Serial.println(device);
    command = device; device = ""; BT1.flush();
  }
}

void manualCmd() {
  switch (command[0]) {
    case 'g': mode = FOLLOWING_LINE; break;
    case 's': motorStop(); break;
    case 'f': motorForward(); break;
    case 'r': motorTurn(RIGHT, 30); motorStop(); break;
    case 'l': motorTurn(LEFT, 30); motorStop(); break;
    case 'b': motorBackward(); break;
    case 'p': Kp = command[2]; break;
    case 'i': Ki = command[2]; break;
    case 'd': Kd = command[2]; break;
  }
}

void sendBTdata(int data) {
  digitalWrite(ledPin, HIGH);
  BT1.print("Data from Arduino"); BT1.print(data); BT1.print(" xx"); BT1.println('\n');
  digitalWrite(ledPin, LOW);
}

void calculatePID() {
  P = error; I += error; D = error - previousError;
  PIDvalue = (Kp*P) + (Ki*I) + (Kd*D);
  previousError = error;
}

void checkPIDvalues() {
  BT1.print("PID: "); BT1.print(Kp); BT1.print(" - "); BT1.print(Ki); BT1.print(" - "); BT1.println(Kd);
  Serial.print("PID: "); Serial.print(Kp); Serial.print(" - "); Serial.print(Ki); Serial.print(" - "); Serial.println(Kd);
}

void testLineFollowSensors() {
  int LFS0 = digitalRead(lineFollowSensor0);
  int LFS1 = digitalRead(lineFollowSensor1);
  int LFS2 = digitalRead(lineFollowSensor2);
  int LFS3 = digitalRead(lineFollowSensor3);
  int LFS4 = digitalRead(lineFollowSensor4);
  Serial.print("LFS: L 0 1 2 3 4 R ==> ");
  Serial.print(LFS0); Serial.print(" "); Serial.print(LFS1); Serial.print(" ");
  Serial.print(LFS2); Serial.print(" "); Serial.print(LFS3); Serial.print(" "); Serial.print(LFS4);
  Serial.print(" ==> P: "); Serial.print(P); Serial.print(" I: "); Serial.print(I);
  Serial.print(" D: "); Serial.print(D); Serial.print(" PID: "); Serial.println(PIDvalue);
}

sensorFuntions (C/C++)

void readLFSsensors() {
  LFSensor[0] = digitalRead(lineFollowSensor0);
  LFSensor[1] = digitalRead(lineFollowSensor1);
  LFSensor[2] = digitalRead(lineFollowSensor2);
  LFSensor[3] = digitalRead(lineFollowSensor3);
  LFSensor[4] = digitalRead(lineFollowSensor4);
  farRightSensor = analogRead(farRightSensorPin);
  farLeftSensor  = analogRead(farLeftSensorPin);

  if ((LFSensor[0]==1)&&(LFSensor[1]==1)&&(LFSensor[2]==1)&&(LFSensor[3]==1)&&(LFSensor[4]==1)) { mode = CONT_LINE; error = 0; }
  else if ((LFSensor[0]==0) && (farRightSensor < THRESHOLD)) { mode = RIGHT_TURN; error = 0; }
  else if ((farLeftSensor < THRESHOLD) && (LFSensor[4]==0)) { mode = LEFT_TURN; error = 0; }
  else if ((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) { mode = NO_LINE; error = 0; }
  else if ((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==1)) { mode = FOLLOWING_LINE; error = 4; }
  else if ((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==1)&&(LFSensor[4]==1)) { mode = FOLLOWING_LINE; error = 3; }
  else if ((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==1)&&(LFSensor[4]==0)) { mode = FOLLOWING_LINE; error = 2; }
  else if ((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==1)&&(LFSensor[3]==1)&&(LFSensor[4]==0)) { mode = FOLLOWING_LINE; error = 1; }
  else if ((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==1)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) { mode = FOLLOWING_LINE; error = 0; }
  else if ((LFSensor[0]==0)&&(LFSensor[1]==1)&&(LFSensor[2]==1)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) { mode = FOLLOWING_LINE; error = -1; }
  else if ((LFSensor[0]==0)&&(LFSensor[1]==1)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) { mode = FOLLOWING_LINE; error = -2; }
  else if ((LFSensor[0]==1)&&(LFSensor[1]==1)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) { mode = FOLLOWING_LINE; error = -3; }
  else if ((LFSensor[0]==1)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) { mode = FOLLOWING_LINE; error = -4; }

  Serial.print(farLeftSensor); Serial.print(" <== LEFT RIGH==> "); Serial.print(farRightSensor);
  Serial.print(" mode: "); Serial.print(mode); Serial.print(" error:"); Serial.println(error);
}

void readLateralSensors(void) { }

RobotDefines.h (C/C++)

int mode = 0;
#define STOPPED 0
#define FOLLOWING_LINE 1
#define NO_LINE 2
#define CONT_LINE 3
#define POS_LINE 4
#define RIGHT_TURN 5
#define LEFT_TURN 6

const int power = 250;
const int iniMotorPower = 250;
const int adj = 0;
float adjTurn = 8;
int extraInch = 200;
int adjGoAndTurn = 800;
const int ledPin = 13;
const int buttonPin = 9;

const int lineFollowSensor0 = 12;
const int lineFollowSensor1 = 18;
const int lineFollowSensor2 = 17;
const int lineFollowSensor3 = 16;
const int lineFollowSensor4 = 19;
const int farRightSensorPin = 0; // A0
const int farLeftSensorPin = 1;  // A1
const int THRESHOLD = 150;

int farRightSensor = 0, farLeftSensor = 0;
int LFSensor[5] = {0,0,0,0,0};

float Kp=50, Ki=0, Kd=0;
float error=0, P=0, I=0, D=0, PIDvalue=0;
float previousError=0, previousI=0;

#define RIGHT 1
#define LEFT -1
Servo leftServo, rightServo;

unsigned char dir;
char path[100] = "";
unsigned char pathLength = 0;
int pathIndex = 0;
unsigned int status = 0;

MotorFuntions (C/C++)

void motorStop() {
  leftServo.writeMicroseconds(1500); rightServo.writeMicroseconds(1500); delay(200);
}
void motorForward() {
  leftServo.writeMicroseconds(1500 - (power+adj)); rightServo.writeMicroseconds(1500 + power);
}
void motorBackward() {
  leftServo.writeMicroseconds(1500 + power); rightServo.writeMicroseconds(1500 - power);
}
void motorFwTime(unsigned int time) { motorForward(); delay(time); motorStop(); }
void motorBwTime(unsigned int time) { motorBackward(); delay(time); motorStop(); }

void motorTurn(int direction, int degrees) {
  leftServo.writeMicroseconds(1500 - (iniMotorPower+adj)*direction);
  rightServo.writeMicroseconds(1500 - iniMotorPower*direction);
  delay(round(adjTurn*degrees+20)); motorStop();
}

void motorPIDcontrol() {
  int leftMotorSpeed = 1500 - (iniMotorPower+adj) - PIDvalue;
  int rightMotorSpeed = 1500 + iniMotorPower - PIDvalue;
  constrain(leftMotorSpeed, 1000, 2000);
  constrain(rightMotorSpeed, 1000, 2000);
  leftServo.writeMicroseconds(leftMotorSpeed);
  rightServo.writeMicroseconds(rightMotorSpeed);
}

void runExtraInch(void) { motorPIDcontrol(); delay(extraInch); motorStop(); }
void goAndTurn(int direction, int degrees) { motorPIDcontrol(); delay(adjGoAndTurn); motorTurn(direction, degrees); }

Schema's