Bouw een lijnvolgende robot met PID-regeling en Android-integratie

Onderdelen en benodigdheden

Onderdeel	Aantal	Opmerkingen
Arduino Nano R3	1
SparkFun RedBot Sensor - Line Follower	1
TCRT5000 4CH Infrarood Lijnvolger Sensormodule	1
HC-06 Bluetooth-module	1
Android-apparaat	1
4xAA batterijhouder	2
RobotGeek Continuous Rotation Servo	2

Apps en online diensten

Arduino IDE
MIT App Inventor 2

Over dit project

Het doel van dit project is een lijnvolgende robot te bouwen met PID-regeling. We gebruiken ook een Android-apparaat om de belangrijkste regelinstellingen eenvoudig in te stellen voor snellere en betere afstelling.

Dit project is het eerste deel van een twee-deliger, complexer project waarin ik de potentie van lijnvolgende robots wil verkennen. In deel 2: Doolhof-oplossende robot, met kunstmatige intelligentie op Arduino, zal de robot doolhoven verkennen en oplossen met behulp van eenvoudige AI-technieken.

Hieronder een video die de robot een lijnvolgend circuit laat zien:

Stap 1: Stuklijst

De benodigde materialen zijn eenvoudig en de robot is goedkoop (rond $75):

2 Houten vierkanten (80x80mm)
3 Mapclips (binder clips)
2 Houten wielen (diameter: 50mm)
1 Balwieltje (ball caster)
9 Elastieken
3M Command lijststrip
Plastic verbindingsstukken voor sensorbevestiging
Breadboard en bedrading
2 sets van 4x Ni-MH batterijen (5V per set)
2x SM-S4303R Continuous Rotation 360° Plastic Servo
Arduino Nano
HC-06 Bluetooth-module
5x Lijnsensoren (TCRT5000 4CH module + 1 losse sensor)
1 LED
1 Drukknop

Stap 2: Motoren instellen

Bouw een lijnvolgende robot met PID-regeling en Android-integratie — Motoren aan elkaar bevestigen

Voor de motors worden 2 continue servos (SM-S4303R) gebruikt. Ze worden aan elkaar "gelijmd" (met 3M Command strip, lijm of dubbelzijdig tape) tot één vast blok. De servos draaien op een gegeven snelheid, bepaald door de pulslengte op hun data-ingang. Voor deze servo loopt de pulslengte van 1,0ms (1000µs) tot 2,0ms (2000µs). Andere servos kunnen andere bereiken hebben.

In detail:

Een puls van 1,5ms plaatst de servo in neutrale positie ("stilstand").
Een puls van 1,0ms geeft volle snelheid (~70 RPM) in de ene richting.
Een puls van 2,0ms geeft volle snelheid in de andere richting.
Pulsen tussen 1,0-1,5ms of 1,5-2,0ms geven evenredige snelheid.

Zodra beide servos fysiek verbonden zijn, volg het schakelingsschema om ze te voeden (externe 5V of 6V) en het Arduino-signaal te geven:

Linker servo: Arduino Pin 5
Rechter servo: Arduino Pin 3

De eerste test: stuur een 1,5ms puls om te controleren of de motoren stil staan. Zo niet, pas de servos aan (gele bout onder de servo).

LET OP: Als je servo geen fysieke aanpassing heeft, verander de parameter "1500" microseconden in de functie (omhoog of omlaag) tot je volle stilstand bereikt.

De onderstaande Arduino-code doet het werk:

#include <Servo.h>
Servo leftServo;
Servo rightServo;

void setup() {
  leftServo.attach(5);
  rightServo.attach(3);
  leftServo.writeMicroseconds(1500);
  rightServo.writeMicroseconds(1500);
}

void loop() { }

Stap 3: Lichaam en motoren monteren voor bewegings-test

Bevestig de 2 servos aan één houten vierkant met de 3M Command strip.
Bevestig het 2e houten vierkant erbovenop met de mapclips. Pas de lengte aan.
Bevestig het balwieltje met een mapclip.
De voeding voor de motors komt van één 5V-batterijset. Deze zit tussen breadboard en frame.
Sluit de batterij aan op het laterale voedingsraster exclusief voor servos.
Plaats de Arduino Nano op het breadboard.
Verbind GND van voedingsraster met Arduino GND.
Sluit servos aan: LINKS → Pin 5; RECHTS → Pin 3.
Sluit LED aan op Arduino Pin 13.
Sluit knop aan op Arduino Pin 9.

Vanwege de tegenovergestelde montage van de servos is het snelheidsbereik:

Rechter servo vooruit: 1500µs (stil) tot 2000µs (vol)
Linker servo vooruit: 1500µs (stil) tot 1000µs (vol)

Een externe LED op pin 13 dient voor signalering en testen (de interne Arduino-LED is moeilijk zichtbaar tussen de kabels).

Een knop op pin 9 is handig voor testen en als startknop. Bijvoorbeeld:

while(digitalRead(buttonPin)) { }
motorTurn(LEFT, 500);
motorTurn(RIGHT, 500);

De 2 regels die de robot laten draaien, worden pas uitgevoerd na indrukken van de knop (buttonPin = 0). Ervoor blijft het programma in de oneindige lus.

De onderstaande code kan als basis dienen voor een complete motortest (vooruit, achteruit, stilstand, linksaf, rechtsaf). Pas de delays aan voor de benodigde draaihoek (soms moeten linker/rechter pulswerwaarden iets verschillen om motorbalans te compenseren).

Stap 4: De Bluetooth-module (optioneel)

De HC-06 Bluetooth-module wordt op het breadboard geplaatst zoals in het schema getoond. De Arduino-bibliotheek SoftSerial wordt gebruikt.

HC-06 pinverbindingen:

Tx Pin naar Arduino pin 10 (Rx)
Rx Pin naar Arduino pin 11 (Tx)
VCC/GND naar Arduino 5V/GND

De robot werkt met of zonder Bluetooth. De code is zo geschreven dat als je de BT niet activeert, de standaardparameters gebruikt worden. Dus geen zorgen als je de HC-06 niet wilt installeren, de code werkt nog steeds. In het laatste deel ga ik dieper in op het gebruik van een Android-app voor het sturen van parameters en handmatige besturing. Ik laat Bluetooth en de app optioneel voor wie de lijnvolger voor competities wil gebruiken.

Stap 5: Lijnsensoren toevoegen

Sluit de kabels aan op de Arduino-pinnen:

Sensor 0 = Pin 12
Sensor 1 = Pin 18 (A4)
Sensor 2 = Pin 17 (A3)
Sensor 3 = Pin 16 (A2)
Sensor 4 = Pin 19 (A5)

Bevestig de 5 sensoren op een plastic staaf zoals op de foto's.
Het is raadzaam de sensoren te labelen voor testdoeleinden. De sensoren zijn genummerd van "0" (meest links) tot "4" (meest rechts).
Voer de kabels onder het frame, vast met elastieken. Let op dat ze niet in de wielen of het wielkruis komen.
Bevestig de tweede 5V-batterijset en sluit deze aan op Arduino Vin.

In mijn geval gebruik ik een module met 4 geïntegreerde sensoren + 1 losse. Allemaal compatibel. Voor simpliciteit toon ik in het schema 5 losse sensoren aangesloten. Het eindresultaat is hetzelfde.

Stap 6: IR-sensorlogica implementeren

De IR-sensor bestaat uit een individuele IR-LED en een IR-fotodiode. Het IR-licht van de LED raakt het oppervlak en wordt teruggereflecteerd naar de fotodiode. De fotodiode genereert een uitgangsspanning evenredig aan de reflectie (hogere waarden voor "lichte oppervlakken", lagere voor "zwarte/donker oppervlakken").

Bij de gebruikte sensoren genereert een geïntegreerde schakeling op de module een eenvoudig digitaal signaal (HOOG: Donker; LAAG: Licht). Een potentiometer op de module stelt het correcte lichtniveau in dat als "donker" of "licht" beschouwd wordt. Wanneer het gereflecteerde licht zwart/donker is, wordt een HOOG ("1") digitaal niveau gegenereerd, en een LAAG ("0") voor een lichtere kleur. Ik gebruik een geïntegreerde module met 4 sensoren en een extra module met een enkele sensor (andere vorm, dezelfde logica). De combinatie is een array van 5 sensoren die ik goed vindt voor vloeiende regeling.

De array van 5 sensoren is zo gemonteerd dat als er maar één sensor gecentreerd is ten opzichte van de zwarte lijn, alleen die specifieke sensor HOOG wordt. De afstand tussen sensoren is zo berekend dat 2 sensoren de volledige breedte van de zwarte lijn tegelijk kunnen dekken, wat ook HOOG op beide sensoren geeft.

De mogelijke sensorarray-uitgangen tijdens lijnvolgen zijn:

0 0 0 0 1
0 0 0 1 1
0 0 0 1 0
0 0 1 1 0
0 0 1 0 0
0 1 1 0 0
0 1 0 0 0
1 1 0 0 0
1 0 0 0 0

Met 5 sensoren kan een "foutvariabele" gegenereerd worden die helpt de positie van de robot op de lijn te regelen.

Stel dat de optimale conditie is wanneer de robot gecentreerd is, met de lijn precies onder de "middelste sensor" (Sensor 2). De array-uitgang is: 0 0 1 0 0 en de "fout" is "nul". Als de robot begint naar links af te dwalen (de lijkt "naar rechts te bewegen"), moet de fout met een positief teken toenemen. Als de robot naar rechts afdwaalt (lijkt "naar links te bewegen"), neemt de fout toe met een negatief teken.

De foutvariabele, gerelateerd aan de sensorstatus:

0 0 1 0 0 ==> Fout = 0
0 0 0 0 1 ==> Fout = 4
0 0 0 1 1 ==> Fout = 3
0 0 0 1 0 ==> Fout = 2
0 0 1 1 0 ==> Fout = 1
0 1 1 0 0 ==> Fout = -1
0 1 0 0 0 ==> Fout = -2
1 1 0 0 0 ==> Fout = -3
1 0 0 0 0 ==> Fout = -4

In de Arduino-code krijgt elke sensor een specifieke naam (de meest linkse sensor krijgt label "0"):

const int lineFollowSensor0 = 12;
const int lineFollowSensor1 = 18;
const int lineFollowSensor2 = 17;
const int lineFollowSensor3 = 16;
const int lineFollowSensor4 = 19;

Om de waarden op te slaan wordt een array aangemaakt:

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

Elke array-positie wordt constant bijgewerkt met de sensoruitgang:

LFSensor[0] = digitalRead(lineFollowSensor0);
LFSensor[1] = digitalRead(lineFollowSensor1);
LFSensor[2] = digitalRead(lineFollowSensor2);
LFSensor[3] = digitalRead(lineFollowSensor3);
LFSensor[4] = digitalRead(lineFollowSensor4);

Met de sensorwaarden wordt de logica geïmplementeerd om de foutvariabele te genereren:

if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==1)) error = 4;
else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==1)&&(LFSensor[4]==1)) error = 3;
else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==1)&&(LFSensor[4]==0)) error = 2;
else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==1)&&(LFSensor[3]==1)&&(LFSensor[4]==0)) error = 1;
else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==1)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = 0;
else if((LFSensor[0]==0)&&(LFSensor[1]==1)&&(LFSensor[2]==1)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = -1;
else if((LFSensor[0]==0)&&(LFSensor[1]==1)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = -2;
else if((LFSensor[0]==1)&&(LFSensor[1]==1)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = -3;
else if((LFSensor[0]==1)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = -4;

Stap 7: Richting regelen (Proportionele regeling - P)

Perfect! Op dit punt is de robot gemonteerd en operationeel. Je moet basistests doen met motoren, sensoruitgangen lezen en testen over een lijn. Wat ontbreekt is de echte "hersenen", de eerste stappen van "kunstmatige intelligentie". We implementeren een regellogica die garandeert dat de robot de lijn blijft volgen.

Eenvoudige Proportionele Regeling:

Stel dat de robot over een lijn rijdt en de sensorarray-uitgang is: "0 0 1 0 0". De bijbehorende fout is "0". In deze situatie moeten beide motoren vooruit lopen met constante snelheid.

Voorbeeld:

Definieer variabele: iniMotorSpeed = 250; betekent dat de LINKER servo pulsen van 1250µs ontvangt en de RECHTER servo 1750µs. Met deze parameters rijdt de robot vooruit met halve snelheid. Onthoud: RECHTER servo vooruit snelheid loopt van 1500µs (stil) tot 2000µs (vol) en LINKER servo van 1500µs (stil) tot 1000µs (vol).

rightServo.writeMicroseconds(1500 + iniMotorPower);
leftServo.writeMicroseconds(1500 - iniMotorPower);

Stel nu dat de robot naar links afdwaalt (lijkt "lijn gaat naar rechts") en sensor 3 ook bedekt. De array-uitgang is: "0 0 1 1 0" en fout = 1. In deze situatie moet de robot naar rechts bochten. Daarvoor moet de snelheid van de RECHTER servo verminderd worden (kortere puls) en de snelheid van de LINKER servo verhoogd worden (kortere puls voor linker servo). De motorregelfunctie wordt:

rightServo.writeMicroseconds(1500 + iniMotorPower - error); // Positieve fout: verminder snelheid
leftServo.writeMicroseconds(1500 - iniMotorPower - error);  // Positieve fout: verhoog snelheid

De bovenstaande logica klopt, maar 1 microseconde toevoegen/aftrekken aan de pulslengte geeft geen realistische correctie. Het getal moet groter zijn, bv. 50, 100. Daarom vermenigvuldigen we de "fout" met een constante (noemen we "K"). Omdat de invloed evenredig is met de fout, noemen we het "Proportionele constante: Kp".

int Kp = 50;
rightServo.writeMicroseconds(1500 + iniMotorPower - Kp*error);
leftServo.writeMicroseconds(1500 - iniMotorPower - Kp*error);

Samenvatting motorreacties:

Sensorarray: 0 0 1 0 0 ==> fout = 0 ==> Rechter servo puls = 1750µs ==> Linker servo puls = 1250µs (beide motoren dezelfde snelheid)
Sensorarray: 0 0 1 1 0 ==> fout = 1 ==> Rechter servo puls = 1700µs (langzamer) ==> Linker servo puls = 1200µs (sneller)

Bij het tegenovergestelde (robot dwaalt naar rechts), is de fout "negatief" en veranderen de snelheden:

Sensorarray: 0 0 1 0 0 ==> fout = 0 ==> Rechter servo puls = 1750µs ==> Linker servo puls = 1250µs
Sensorarray: 0 1 1 0 0 ==> fout = -1 ==> Rechter servo puls = 1800µs (sneller) ==> Linker servo puls = 1300µs (langzamer)

Hoewel de robot verder afdwaalt, wordt de fout groter en moet sneller gecorrigeerd worden. De reactiesnelheid is evenredig aan de fout. Dit heet "Proportionele Regeling", de "P"-component van een complexer regelsysteem, de PID (Proportioneel, Differentieel, Integrerend).

Stap 8: PID-regeling (optioneel)

Als je hier wil overslaan, is dat ook OK. Je kunt bij de proportionele regeling van de vorige stap blijven, of je hersenen branden voor een complexer systeem, het is jouw keuze.

PID (Proportioneel, Differentieel, Integrerend) is een van de meest voorkomende regelschema's. De meeste industriële regellussen gebruiken een variant van PID. Er zijn veel manieren om een PID-lus af te stellen, inclusief de handmatige techniek die hier gebruikt wordt.

Denk aan PID als een eenvoudige veer. Een veer heeft een oorspronkelijke lengte, die wanneer verstoord door uitrekken of samendrukken, probeert zo snel mogelijk de oorspronkelijke lengte terug te krijgen. Op dezelfde manier heeft een PID-algoritme in een systeem een ingestelde waarde van een fysieke hoeveelheid die moet worden geregeld, een 'setpoint', die wanneer gewijzigd, het systeem de andere benodigde kenmerken regelt om zo snel mogelijk terug te keren naar de oorspronkelijke setpoint. PID-regelaars worden overal gebruikt waar een fysieke hoeveelheid gecontroleerd moet worden en gelijkgesteld moet worden aan een gespecificeerde waarde. Voorbeelden: Tempomaat in auto's, robots, temperatuurregelaars, spanningregelaars, etc.

Hoe werkt PID?

Het systeem berekent de 'fout' of 'afwijking' van de fysieke hoeveelheid ten opzichte van de setpoint, door de huidige waarde te meten met sensor(en). Om terug te keren naar de setpoint moet deze 'fout' geminimaliseerd worden, ideaal nul. Dit moet zo snel mogelijk gebeuren. Ideaal is nul vertraging in de systeemreactie op verandering in de setpoint.

Meer informatie is te vinden in veel boeken en websites, waaronder hier.

PID implementeren

i) Fouttermijn (e):

Dit is het verschil tussen de setpoint en de huidige waarde van de geregelde hoeveelheid. Fout = set_point – current_value (in ons geval de foutvariabele van de robotpositie over de lijn).

ii) Proportionele term (P):

Deze term is evenredig aan de fout.

P = fout

Deze waarde is verantwoordelijk voor de grootte van de verandering die nodig is in de fysieke hoeveelheid om de setpoint te bereiken. De proportionele term bepaalt de opstijgtijd van de regellus of hoe snel de setpoint bereikt wordt.

iii) Integrerende term (I):

Deze term is de som van alle eerdere foutwaarden.

I = I + fout

Deze waarde is verantwoordelijk voor de reactiesnelheid van het systeem op afwijking van de setpoint. De integrerende term elimineert de stationaire fout die de proportionele term vereist. Meestal gebruiken kleine robots de integrerende term niet, omdat we ons geen zorgen maken over stationaire fout en het de "lus-afstelling" kan bemoeilijken.

iv) Differentiele of Afgeleide term (D):

Deze term is het verschil tussen de ogenblikkelijke fout van de setpoint en de fout van het vorige ogenblik.

D = fout - vorigeFout

Deze waarde is verantwoordelijk voor het vertragen van de veranderingssnelheid van de fysieke hoeveelheid wanneer deze zich de setpoint nader. De differentiele term vermindert de overshoot of hoeveel het systeem "overcorrigeert".

Vergelijking:

PIDwaarde = (Kp*P) + (Ki*I) + (Kd*D)

Waarbij:

Kp is de constante om de grootte van de verandering te variëren die nodig is om de setpoint te bereiken.
Ki is de constante om de snelheid waarmee de verandering in de fysieke hoeveelheid moet worden aangebracht om de setpoint te bereiken.
Kd is de constante om de stabiliteit van het systeem te variëren.

Een benadering om de lus af te stellen is de Probeer-fout methode:

Zet Kd op 0 en stem Kp alleen af. Kp van 25 is een goed startpunt. In de vorige stap gebruikten we Kp=50 wat goed werkte. Zodra de robot redelijk reageert, stem de differentiele term af (Kd). Zet Kp en Kd elk op de helft van de Kp-waarde. Bijvoorbeeld, als de robot redelijk reageert met Kp=50, zet dan Kp=25 en Kd=25 om te beginnen. Verhoog Kd om overshoot te verminderen, verlaag als de robot onstabiel wordt.
Als de robot te traag reageer, verhoog de waarde.
Als de robot snel reageert maar onstabiel wordt, verlaag de waarde.

Een ander onderdeel is de daadwerkelijke Sample/Loop-frequentie. Het versnellen of vertragen van deze parameter kan een significant verschil maken in de prestaties. Dit wordt ingesteld door de delay-statements in je code. Het is een probeer-fout methode om het optimale resultaat te krijgen.

Op basis van de bovenstaande benadering is de onderstaande functie geïmplementeerd:

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

De eenvoudige Kp-constante van de vorige stap wordt vervangen door deze complete PIDvalue:

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

Let op: als Kd en Ki = 0, is PIDvalue slechts Kp*error zoals in de vorige stap.

Stap 9: De finale code

Op deze stap kan de robot een constante lus volgen zonder te stoppen. Het lusprogramma zou zijn:

void loop() {
  readLFSensors(); // lees sensoren, sla waarden op in array en bereken "fout"
  calculatePID();
  motorPIDcontrol();
}

Voor een completere en realistische werking is het belangrijk om minimaal een paar basis "commando's" toe te voegen die "met de lijn" gedaan worden. Laten we een nieuwe variabele introduceren: "mode". We definiëren 3 toestanden:

#define STOPPED 0
#define FOLLOWING_LINE 1
#define NO_LINE 2

Als alle sensoren een zwarte lijn detecteren, is de array-uitgang: 1 1 1 1 1. In deze conditie definiëren we mode als "STOPPED" en moet de robot een "volle stop" uitvoeren.

if((LFSensor[0]==1)&&(LFSensor[1]==1)&&(LFSensor[2]==1)&&(LFSensor[3]==1)&&(LFSensor[4]==1)) {
  mode = STOPPED;
}

Een andere veelvoorkomende situatie is "geen lijn", array-uitgang: 0 0 0 0 0. Dan programmeren we een 180° kehrt of kleine draaihoeken tot een lijn gevonden wordt en normaal lijnvolgen hervat wordt.

else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) {
  mode = NO_LINE;
}

De complete loop() wordt:

void loop() {
  readLFSensors();
  switch (mode) {
    case STOPPED:
      motorStop();
      break;
    case NO_LINE:
      motorStop();
      motorTurn(LEFT, 180);
      break;
    case FOLLOWING_LINE:
      calculatePID();
      motorPIDcontrol();
      break;
  }
}

De echte finale code bevat extra logica en initialisatie van variabelen. Voor simpliciteit zijn deze weggelaten, maar alles moet duidelijk zijn bij de finale code.

Hieronder de finale Arduino-code (bestanden beschikbaar op GitHub):

Code snippet #1
Code snippet #2
Code snippet #3
Code snippet #5
Code snippet #6
Code snippet #10
Code snippet #11
Code snippet #12
Code snippet #15
Code snippet #17
Code snippet #18

Code snippet #1

#include <Servo.h>
Servo leftServo;
Servo rightServo;
void setup() {
  leftServo.attach(5);
  rightServo.attach(3);
  leftServo.writeMicroseconds(1500);
  rightServo.writeMicroseconds(1500);
}
void loop() { }

Code snippet #2

while(digitalRead(buttonPin)) { }
motorTurn(LEFT, 500);
motorTurn(RIGHT, 500);

Code snippet #3

const int lineFollowSensor0 = 12;
const int lineFollowSensor1 = 18;
const int lineFollowSensor2 = 17;
const int lineFollowSensor3 = 16;
const int lineFollowSensor4 = 19;

Code snippet #5

LFSensor[0] = digitalRead(lineFollowSensor0);
LFSensor[1] = digitalRead(lineFollowSensor1);
LFSensor[2] = digitalRead(lineFollowSensor2);
LFSensor[3] = digitalRead(lineFollowSensor3);
LFSensor[4] = digitalRead(lineFollowSensor4);

Code snippet #6

if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==1)) error = 4;
else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==1)&&(LFSensor[4]==1)) error = 3;
else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==1)&&(LFSensor[4]==0)) error = 2;
else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==1)&&(LFSensor[3]==1)&&(LFSensor[4]==0)) error = 1;
else if((LFSensor[0]==0)&&(LFSensor[1]==0)&&(LFSensor[2]==1)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = 0;
else if((LFSensor[0]==0)&&(LFSensor[1]==1)&&(LFSensor[2]==1)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = -1;
else if((LFSensor[0]==0)&&(LFSensor[1]==1)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = -2;
else if((LFSensor[0]==1)&&(LFSensor[1]==1)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = -3;
else if((LFSensor[0]==1)&&(LFSensor[1]==0)&&(LFSensor[2]==0)&&(LFSensor[3]==0)&&(LFSensor[4]==0)) error = -4;

Code snippet #10

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

Code snippet #11

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

Code snippet #12

void loop() {
  readLFSensors();
  calculatePID();
  motorPIDcontrol();
}

Code snippet #15

void loop() {
  readLFSensors();
  switch (mode) {
    case STOPPED:
      motorStop();
      break;
    case NO_LINE:
      motorStop();
      motorTurn(LEFT, 180);
      break;
    case FOLLOWING_LINE:
      calculatePID();
      motorPIDcontrol();
      break;
  }
}

Code snippet #17

while (digitalRead(buttonPin) && !mode) {
  checkBTcmd(); // controleer of commando ontvangen via BT
  manualCmd();
  command = "";
}
checkPIDvalues();
mode = STOPPED;

Code snippet #18

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;
  }
}

GitHub: https://github.com/Mjrovai/MJRoBot-Line-Follower

Stap 10: PID-regeling afstellen met de Android-app

In de vorige code vind je in het tabblad "robotDefines.h" de definitie van de constanten voor PID-regeling:

float Kp=50;
float Ki=0;
float Kd=0;

Zoals uitgelegd, is de beste manier om de juiste constanten te vinden de "Probeer-fout" methode. Het nadeel is dat je de code telkens moet hercompileren. Om het te versnellen, gebruik je de Android-app om de constanten in de "Set-Up Fase" te versturen.

Ik heb een Android-app ontwikkeld daarvoor. Kortom, er zijn traditionele handmatige commando's:

FW, BW, Links, Rechts, Stop – de app stuurt respectievelijk 'f', 'b', 'l', 'r', 's' naar de BT-module.

Daarnaast 3 schuifregelaars voor de PID-constanten:

Kd: "d/XXX" waar "XXX" een getal is van 0 tot 100.
Kp: "p/XXX"
Ki: "i/XXX"

Een extra knop werkt precies als de knop op Arduino Pin 9. Je kunt de een of de ander gebruiken.

Onderstaand vind je het .aia bestand (te bewerken in MIT App Inventor) en het .apk bestand voor directe installatie op je Android-apparaat.

FTH62BVIN699T4B.aia
F60H7R0IN699T4I.apk

Stap 11: Code aanpassen voor PID remote afstelling

Tijdens de setup voegen we een lus toe waarin je de PID-parameters kunt versturen voordat je de robot op de lijn plaatst:

while (digitalRead(buttonPin) && !mode) {
  checkBTcmd();
  manualCmd();
  command = "";
}
checkPIDvalues();
mode = STOPPED;

De handmatige commando-functie:

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;
  }
}

In de video zie je tests met de Android-app. Hieronder de finale code inclusief PID-setup via Android:

FGAEB9BIN7QQQAW.ino
FBMONSNIN7QQQCD.ino
F8B3CDHIN7QQQCL.h
FNOMRUNIN7QQQCP.ino
FA3K57ZIN7QQQCR.ino

Stap 12: Conclusie

Dit is het eerste deel van een complexer project, waarin de potentie van een lijnvolgende robot wordt verkend. In het volgende deel ontwikkel ik een doolhof-oplossende robot, gebaseerd op dit project. Hoopelijk kan ik anderen helpen meer te leren over elektronica, robotica, Arduino, etc.

De updatebestanden voor dit project zijn te vinden op GitHub. Voor meer tutorials, bezoek mijn blog: MJRoBot.org

Groetjes uit het zuiden van de wereld!

Marcelo

Schema's

oIDhLcHQ30lDgVBXZvb8.fzz (Fritzing-bestand)

Volledig geautomatiseerde 3D-geprinte brushless Nerf-blaster aangestuurd door Arduino Geavanceerde GPS Data Logger: Realtime Vlucht Tracking, Piekhoogte & Snelheidsmonitoring

Productieproces

3d printen

Automatisering Besturingssysteem

Industriële technologie