Lijnvolger-robot - PID-regeling - Android-configuratie

Over dit project

Het doel van dit project is het bouwen van een Line Follower Robot met PID-regeling. We zullen ook een Android-apparaat gebruiken om eenvoudig de belangrijkste regelparameters in te stellen voor een betere en snelle afstemming.

Dit project is het eerste van een meer complex project uit 2 delen, waarbij het mijn bedoeling is om de mogelijkheden van Line Follower Robots te onderzoeken. Op het 2e deel:Maze Solver Robot, met behulp van kunstmatige intelligentie met Arduino, zal de robot doolhoven verkennen en oplossen met behulp van eenvoudige kunstmatige intelligentie-technieken.

Hieronder is een video te zien waarin de robot een lijncircuit volgt:

Stap 1:Stuklijst

De lijst met benodigde materialen is heel eenvoudig en de uiteindelijke robot is erg goedkoop (ongeveer $ 75,00):

Lichaam (kan worden aangepast aan uw behoeften):

• 2 Houten vierkanten (80X80mm)

• 3 bindclips

• 2 houten wielen (diameter:50 mm)

• 1 bal caster

• 9 elastische banden

• 3M Commandolijststrook

• Kunststof verbindingen voor sensorbevestiging

• BreadBoard en bedrading

• 2 sets van 4XNi-metaalhydridebatterijen (5V elke set)

• 2 X SM-S4303R continue rotatie 360 ​​graden plastic servo

• Arduino Nano

• HC-06 Bluetooth-module

• 5 X-lijnsensoren (TCRT5000 4CH infrarood lijnvolgvolgersensormodule + 1 onafhankelijke spoorsensor)

• 1 LED

• 1 knop

Stap 2:De motoren instellen

Voor motoren werden 2 continue servo's (SM-S4303R) gebruikt. Ze worden aan elkaar "gelijmd" zodat een enkel en stevig blok ontstaat, zoals u op de foto kunt zien (gebruik de 3M Command-strip, lijm of dubbelzijdige tape). Die servo's werken op een bepaalde snelheid, gedefinieerd door de pulsbreedte die wordt ontvangen op de gegevensinvoer. Voor deze specifieke servo gaat de pulsbreedte van 1,0 ms (1.000 microseconden) tot 2,0 ms (2.000 microseconden). Andere servo's kunnen met verschillende pulsbreedtes werken.

In details kijken:

• Een puls van 1,5 ms plaatst de servo in de neutrale positie, of "gestopt".

• Een puls van 1,0 ms zal de servo in één richting op volle snelheid sturen (ongeveer 70 RPM)

• Een puls van 2,0 ms op volle snelheid in de tegenovergestelde richting.

• Puls tussen 1,0 en 1,5 ms of 1,5 ms en 2,0 ms, genereert proportionele snelheid.

Zodra je beide servo's fysiek hebt aangesloten, volg je het bovenstaande tekencircuit om ze te sourcen (externe 5V of 6V) en ze te voeden met Arduino's signaal:

• Linker servo:Arduino Pin 5

• Rechter servo:Arduino Pin 3

Als alles eenmaal is aangesloten, is het eerste dat moet worden gedaan, een puls van 1,5 ms verzenden om te controleren of de motoren "gestopt" zijn (niet draaien). Zo niet, dan moeten de servo's tot aan de aanslag worden afgesteld (kijk naar de gele bout, onder de servo).

OPMERKING :Als uw servo deze fysieke aanpassing niet heeft, probeer dan de parameter "1500" microseconde binnen de functie te wijzigen (omhoog of omlaag) totdat u de volledige stop krijgt.

De onderstaande Arduino-code kan het werk doen:

#include  // Servobibliotheek Servo leftServo;Servo rightServo;Void setup(){ leftServo.attach(5); rightServo.attach (3); leftServo.writeMicroseconden (1500); rightServo.writeMicroseconds(1500);}void loop(){} 

Stap 3:Monteer het lichaam en de motoren voor bewegingstest

• Bevestig met de 3M Command-framestrip de 2 servo's op die van het vierkante stuk hout.

• Bevestig het 2e vierkante hout aan het bovenstaande met behulp van de bindclips. Pas de lengte van het platform aan uw behoeften aan.

• Bevestig de Ball Caster met behulp van de bindclip.

• De voeding voor de motoren komt van een van de 5V-batterijen. Deze set batterijen wordt tussen het breadboard en het frame van de carrosserie geïnstalleerd.

• Sluit de batterij aan die met servo's moet worden gebruikt:links een van het laterale elektriciteitsnet exclusief voor servo's-bron

• Sluit de Arduino Nano aan op het breadboard

• Verbind de GND van Power Grid met Arduino GND.

• Sluit de servo's aan op Arduino:LINKS ==> Pin 5; RECHTS ==> Pin 3

• Sluit de LED aan op Arduino Pin 13

• Verbind de knop met Arduino Pin 9

Merk op dat vanwege de manier waarop de servo's zijn gemonteerd (tegenover elkaar) het snelheidsbereik is:

• Voorwaartse snelheid van de rechter servo gaat van 1.500us (gestopt) naar 2.000us (volle snelheid)

• De voorwaartse snelheid van de linker servo gaat van 1.500us (gestopt) naar 1.000 (volle snelheid)

Een externe LED is toegevoegd aan pin13, voor signalisatie- en testdoeleinden (je kunt de interne Arduino LED gebruiken, in plaats van een externe als je wilt, maar houd er rekening mee dat deze moeilijk te zien is in het midden van de kabels).

Op pin 9 is ook een knop aangesloten. Deze knop is erg handig voor testdoeleinden en voor het starten van de robot.

Bijvoorbeeld:

while(digitalRead(buttonPin)) { }motorTurn (LINKS, 500);motorTurn (RECHTS, 500); 

Merk op dat de 2 regels die de robot opdracht geven om naar LINKS te draaien, 500 ms te wachten en naar RECHTS te draaien, pas zullen plaatsvinden nadat u op de knop hebt gedrukt (buttonPin =0). Daarvoor wordt het programma gestopt in de oneindige lus.

Onderstaande code kan worden gebruikt als basis voor een complete motortest (Vooruit, Achteruit, Volledige stop, Linksaf, Rechtsaf). Indien nodig moet u de vertragingen voor de vereiste draaihoek aanpassen, afhankelijk van uw motoren (soms moeten de linker- en rechterpulswaarden een beetje anders zijn om een ​​eventueel gebrek aan balans van de motoren te compenseren.

Stap 4:De Bluetooth-module (optioneel)

De Bluetooth-module HC-06 moet op het breadboard worden geïnstalleerd zoals weergegeven in de tekening. De Arduino-bibliotheek SoftSerial zal worden gebruikt.

Onder de HC-06 pin-aansluitingen:

• 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 gebouwd dat als u de BT niet activeert, de standaardparameters degene zijn die door de robot worden gebruikt. Maak je dus geen zorgen als je de HC-06-module liever niet installeert, de code werkt nog steeds prima. In het laatste deel van deze tutorial zal ik onderzoeken hoe je een Android-app kunt gebruiken voor het verzenden van gegevens voor een betere afstemming van de robotparameters en/of het verplaatsen van de robot in de handmatige modus. Ik laat het gebruik van Bluetooth en de app optioneel voor het geval iemand meer wil weten over het gebruik van een Line Follower Robot voor bijvoorbeeld wedstrijden.

Stap 5:De lijnsensoren toevoegen

Sluit de kabels aan op de Arduino-pinnen zoals hieronder:

• Sensor 0 =12

• Sensor 1 =18

• Sensor 2 =17

• Sensor 3 =16

• Sensor 4 =19

• Bevestig de 5 sensoren op een plastic staaf zoals getoond op de foto's

• Het is raadzaam om de sensoren te labelen voor testdoeleinden. De naam van de sensoren gaat van "0" (meer naar links) tot "4" (meer naar rechts)

• Leg de kabels onder het frame door en gebruik de elastische banden om ze vast te maken. Zorg ervoor dat u zich niet verwisselt met de wielen of het zwenkwiel.

• De tweede set 5V-batterijen gerepareerd en aangesloten op Arduino Vin.

In mijn geval gebruik ik een module met 4 sensoren geïntegreerd + 1 extra. Ze zijn allemaal compatibel. Voor de eenvoud heb ik in het diagram 5 zelfstandige sensoren opgenomen die op elkaar zijn aangesloten. De uiteindelijke resultaten zijn hetzelfde in beide configuraties.

Stap 6:De IR-sensorlogica implementeren

De IR-sensor bestaat uit een individuele IR-led en een IR-fotodiode. Het IR-licht dat door de LED wordt uitgestraald, valt op het oppervlak en wordt teruggekaatst naar de IR-fotodiode. De fotodiode genereert dan een uitgangsspanning die evenredig is met het reflectieniveau van het oppervlak (hogere waarden voor "lichte oppervlakken" en lager voor "zwarte/donkere oppervlakken").

Bij de gebruikte sensoren genereert een geïntegreerde schakeling op de module als uitgang een eenvoudig digitaal signaal (HIGH:Dark; LOW:Light). Een potentiometer die op de module is geïnstalleerd (zie foto) zal het juiste lichtniveau aanpassen om als "donker" of "licht" te worden beschouwd. Het werkt zodanig dat wanneer de gereflecteerde lichtkleur zwart/donker is, een HOOG ("1") digitaal niveau wordt gegenereerd aan de uitgang en een LAAG ("0") voor een andere lichtere kleur. Ik gebruikte hier een geïntegreerde module met 4 sensoren en een extra module met een enkele sensor (andere vorm, maar dezelfde logica). De combinatie is een reeks van 5 sensoren waarvan ik vond dat deze goed is voor een mooie en soepele bediening, zoals hieronder wordt uitgelegd.

De reeks van 5 sensoren is zo gemonteerd dat als slechts één sensor gecentreerd is ten opzichte van de zwarte lijn, alleen die specifieke sensor een HOOG zal produceren. Aan de andere kant moet de ruimte tussen de sensoren zo worden berekend dat 2 sensoren tegelijkertijd de volledige breedte van de zwarte lijn kunnen bestrijken, wat ook een HOOG op beide sensoren oplevert (zie de afbeeldingen hierboven).

De mogelijke output van de sensorarray bij het volgen van een lijn 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" worden gegenereerd die helpt om de positie van de robot over de lijn te regelen, zoals hieronder weergegeven.

Laten we aannemen dat de optimale toestand is wanneer de robot gecentreerd is, met de lijn net onder de "middelste sensor" (sensor 2). De uitvoer van de array is:0 0 1 0 0 en in deze situatie is de "fout" "nul". Als de robot naar links begint te rijden (de lijn "lijkt te bewegen" naar rechts"), moet de fout toenemen met een positief signaal. Als de robot naar rechts begint te bewegen (de lijn "lijkt te bewegen" naar links"), moet in de op dezelfde manier moet de fout toenemen, maar nu met een negatief signaal.

De foutvariabele, gerelateerd aan de sensorstatus, is:

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

Kijkend naar de Arduino-code, zal elk van de sensoren worden gedefinieerd met een specifieke naam (denk eraan dat de lijnvolgsensor meer naar links moet worden toegewezen met een 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 van elke sensor op te slaan, wordt een arrayvariabele gemaakt:

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

Elke positie van de array wordt constant bijgewerkt met de output van elk van de sensoren:

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

Met de waarde van elk van de sensoren, moet een logica worden 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 )) fout =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 )) fout =- 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 )) fout =-4; 

Stap 7:Regelrichting (proportionele regeling - P)

Perfect! Op dit punt is onze robot geassembleerd en operationeel. U moet enkele basistests met de motoren uitvoeren, de output van sensoren lezen en ze over een lijn testen. Wat ontbreekt is het echte "brein", de eerste stappen van een "kunstmatige intelligentie". We zullen dit krijgen door een besturingslogica te implementeren die garandeert dat de robot de lijn blijft volgen.

Eenvoudige proportionele besturing:

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

Bijvoorbeeld:

De variabele definiëren:iniMotorSpeed ​​=250 ; betekent dat de LINKER servo pulsen van 1.250us en de RECHTER servo 1.750us zal ontvangen. Met die parameters zal de robot op halve snelheid vooruit bewegen. Onthoud dat de RECHTER servo Voorwaartse snelheid zal variëren met een pulslengte van 1.500us (gestopt) tot 2.000us (volle snelheid) en de LINKER servo van 1.500us (gestopt) tot 1.000us (volle snelheid).

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

Stel nu dat de robot naar links reed (het is alsof de "LINE gaat naar rechts") en ook de sensor 3 bedekt. ​​De array-uitvoer zal zijn:"0 0 1 1 0 " en de fout =1 . In deze situatie moet je de robot naar rechts draaien. Om dat te doen, moet u de snelheid van de RECHTER-servo verlagen, wat betekent dat de lengte van de puls wordt verkort. Ook moet de snelheid van de LINKER-servo toenemen, wat betekent dat de lengte van de LINKER-servopuls moet afnemen. Om dat te doen, moeten we de motorbesturingsfunctie wijzigen:

rightServo.writeMicroseconds (1500 + iniMotorPower - fout); ==> Positieve fout:verlaag snelheidleftServo.writeMicroseconds (1500 - iniMotorPower - fout); ==> Positieve fout:snelheid verhogen 

De bovenstaande logica is correct, maar het is gemakkelijk te begrijpen dat het optellen of aftrekken van "1" microseconde bij pulslengte niet de vereiste correctie op een realistische tijd zal genereren. Het is intuïtief dat het getal dat moet worden optellen of aftrekken groter moet zijn, bijvoorbeeld 50, 100, enz. Om dat te krijgen, moet de "fout" worden vermenigvuldigd met een constante (laten we het "K" noemen). Zodra de invloed van deze constante evenredig is met de fout, noemen we deze "Proportionele constante:Kp" .

De motorische functie zal zijn:

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

We kunnen hervatten wat er met de motoren zal gebeuren, zoals hieronder weergegeven:

• Sensorarray:0 0 1 0 0 ==> fout =0 ==> Rechter servo puls lengte =1.750us ==> Linker servo puls lengte =1.250us (beide motoren op dezelfde snelheid)

• Sensorarray:0 0 1 1 0 ==> fout =1 ==> Rechter servo puls lengte =1.700us (langzamer) ==> Linker servo puls lengte =1.200us (sneller)

Als de situatie het tegenovergestelde is en de robot naar rechts rijdt, zou de fout "negatief" zijn en zou de snelheid van de servo's moeten veranderen:

• Sensorarray:0 0 1 0 0 ==> fout =0 ==> Rechter servo puls lengte =1.750us ==> Linker servo puls lengte =1.250us (beide motoren op dezelfde snelheid)

• Sensorreeks:0 1 1 0 0 ==> fout =-1 ==> Rechter servo puls lengte =1.800us (sneller) ==> Linker servo puls lengte =1.300us (langzamer)

Op dit punt is het duidelijk dat hoe meer de robot naar één kant wordt gereden, de fout groter zal zijn en sneller naar het midden moet terugkeren. De snelheid waarmee de robot op de fout reageert, is daarmee evenredig. Dit heet "Proportioneel beheer" , dat is de "P"-component van een complexer regelnetwerk, de PDI (Proportional, Derivative, Integral).

Stap 8:PID-regeling (optioneel)

Als je naar dit deel wilt springen, is dat ook OK. Je kunt bij de proportionele besturing blijven die in de laatste stap is uitgelegd, of wat hersens verbranden om een ​​complexer besturingssysteem in je robot te implementeren, het is jouw keuze.

Als je een besluit hebt genomen, laten we gaan!

PID (Proportioneel, Afgeleid en Integraal) is een van de meest voorkomende controleschema's. De meeste industriële regelkringen gebruiken een vleugje PID-regeling. Er zijn veel manieren om een ​​PID-lus af te stemmen, inclusief de handmatige techniek die in dit voorbeeld wordt gebruikt.

Zie PID als een eenvoudige veer. Een veer heeft een oorspronkelijke lengte, die bij verstoring door uitzetting of samentrekking de neiging heeft om in de kortst mogelijke tijd zijn oorspronkelijke lengte terug te krijgen. Evenzo heeft een PID-algoritme in een systeem een ​​instelwaarde van een bepaalde fysieke grootheid die moet worden geregeld, een 'instelpunt genoemd. ’, dat wanneer het om de een of andere reden wordt gewijzigd, het systeem de andere noodzakelijke functies regelt, om zo snel mogelijk terug te keren naar het oorspronkelijke instelpunt. PID-regelaars worden overal gebruikt waar het nodig is om een ​​fysieke hoeveelheid te regelen en deze gelijk te maken aan een gespecificeerde waarde. Voorbeeld, Cruise Controller in auto's, robots, temperatuurregelaars, spanningsregelaars, enz.

Hoe werkt PID?

Het systeem berekent de 'fout ’, of ‘afwijking ’ van de fysieke grootheid vanaf het instelpunt, door de huidige waarde van die fysieke grootheid te meten met behulp van een sensor(en). Om terug te keren naar het instelpunt, deze 'fout ' moet worden geminimaliseerd en moet idealiter gelijk zijn aan nul. Dit proces moet ook zo snel mogelijk gebeuren. Idealiter zou er geen vertraging moeten zijn in de reactie van het systeem op de verandering in het instelpunt.

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

PID implementeren

i) Foutterm (e):

Dit is gelijk aan het verschil tussen het setpoint en de huidige waarde van de te regelen hoeveelheid. Fout =set_point – huidige_waarde (in ons geval is de foutvariabele get van de positie van Robot over de lijn

ii) Proportionele termijn (P):

Deze term is evenredig met de fout.

P =fout

Deze waarde is verantwoordelijk voor de grootte van de verandering die nodig is in de fysieke hoeveelheid om het instelpunt te bereiken. De verhoudingsterm is wat de stijgtijd van de regellus bepaalt of hoe snel deze het instelpunt zal bereiken.

iii) Integrale term (I):

Deze term is de som van alle voorgaande foutwaarden.

I =I + fout

Deze waarde is verantwoordelijk voor de reactiesnelheid van het systeem op de wijziging van het instelpunt. De integrale term wordt gebruikt om de door de proportionele term vereiste stationaire fout te elimineren. Meestal gebruiken kleine robots de integrale term niet omdat we ons geen zorgen maken over steady-state-fouten en het kan de "loop-afstemming bemoeilijken ".

iv) Differentiële of afgeleide term (D):

Deze term is het verschil tussen de momentane fout vanaf het instelpunt en de fout vanaf het vorige moment.

D =error - previousError

Deze waarde is verantwoordelijk voor het vertragen van de veranderingssnelheid van de fysieke hoeveelheid wanneer deze dicht bij het instelpunt komt. De afgeleide term wordt gebruikt om de overshoot te verminderen of hoeveel het systeem moet "overcorrigeren ".

Vergelijking:

PID-waarde =(Kp*P) + (Ki*I) + (Kd*D)

Waar:

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

Een manier om de lus af te stemmen kan zijn:Try-error voorlopige methode :

Stel de Kd variabele in op 0 en stem eerst de Kp-term alleen af. Kp van 25 is in ons geval hier een goede plek om te beginnen. Bij de laatste stap gebruikten we een Kp van 50 die heel goed werkt met mijn robot. Zodra de robot redelijk reageert, stemt u het afgeleide deel van de regellus af (Kd ). Stel eerst de Kp- en Kd-waarde elk in op de 1/2 van de Kp-waarde. Als de robot bijvoorbeeld redelijk reageert met een Kp =50, stel dan Kp =25 en Kd =25 in om te starten. Verhoog de Kd (afgeleide) versterking om de overshoot te verminderen, verlaag deze als de robot onstabiel wordt.

• Als de robot te langzaam reageert, verhoogt u de waarde.

• Als de robot snel lijkt te reageren en onstabiel wordt, verlaag dan de waarde.

Een ander onderdeel van de lus waarmee u rekening moet houden, is de werkelijke Sample/Loop Rate . Het versnellen of vertragen van deze parameter kan een aanzienlijk verschil maken in de prestaties van de robot. Dit wordt ingesteld door de vertraging statements die je in je code hebt staan. Het is een voorlopige try-error-methode om het optimale resultaat te krijgen.

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

ongeldig berekenenPID(){ P =fout; ik =ik + fout; D =fout-vorigeError; PID-waarde =(Kp*P) + (Ki*I) + (Kd*D); previousError =fout;} 

De eenvoudige Kp-constante die in de laatste stap werd gebruikt, wordt vervangen voor deze meer complete PIDvalue :

void motorPIDcontrol(){int leftMotorSpeed ​​=1500 - iniMotorPower - PID-waarde; int rightMotorSpeed ​​=1500 + iniMotorPower - PID-waarde; leftServo.writeMicroseconden(linksMotorSpeed); rightServo.writeMicroseconds(rightMotorSpeed);} 

Maar merk op dat als je Kd en Ki =0 . hebt , PID-waarde wordt alleen Kp*error gebruikt in de laatste stap.

Stap 9:De definitieve code

Bij deze stap kan de robot een constante lus volgen en zal dit doen zonder te stoppen. Het loopprogramma zou zijn:

ongeldige lus (){ readLFSsensors(); // lees sensoren, opslagwaarden bij Sensor Array en bereken "error" calculatorPID(); motorPIDcontrol();} 

Maar voor een meer complete en echte operatie, is het belangrijk om op zijn minst een paar basis "commando's toe te voegen. " done "met de regel ". Laten we bijvoorbeeld een nieuwe variabele introduceren:"mode ". We zullen 3 toestanden voor deze variabele definiëren:

Modus:

• #define GESTOPT 0

• #define FOLLOWING_LINE 1

• #define NO_LINE 2

Als alle sensoren een zwarte lijn vinden, is de output van de sensorarray:1 1 1 1 1. In deze toestand kunnen we de modus definiëren als "GESTOPT" en moet de robot een "volledige stop uitvoeren ".

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

Een andere veelvoorkomende situatie met Follower Line Robots is waar het "geen lijn" vindt ", of de sensorarray-uitvoer is:0 0 0 0 0. In dit geval kunnen we het programmeren om 180o terug te draaien of in kleine hoeken te draaien totdat een lijn is gevonden en de normale lijnvolgconditie wordt hervat.

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

De volledige loop () zou zijn:

void loop() { readLFSsensors(); schakelaar (modus) { geval GESTOPT:motorStop(); pauze; geval NO_LINE:motorStop(); motorTurn (LINKS, 180); pauze; geval VOLGENDE_LINE:berekenPID(); motorPIDcontrol(); pauze; }} 

De echte definitieve code zal wat extra logica bevatten en ook enkele variabelen die moeten worden geïnitialiseerd, enz. Tijdens de bovenstaande uitleg heb ik ze voor de eenvoud weggelaten, maar alles zou duidelijk moeten zijn als we naar de uiteindelijke code kijken.

Hieronder vindt u de definitieve Arduino-code:

Step 10:Tuning the PID control using the Android App

In the previous code, you can find at "robotDefines.h " tab the following definitions for the constants to be used with the PID control:

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

As explained at previous step, the best way to define the correct constant to be used with a PID controller is using the "Try-error" methodology. The bad side of that is that you must re-compile the program each time that you must change it. One way to speed-up the process is to use the Android App to send the constants at the "Set-Up Phase" .

I developed an Android App exclusively for that. In short there are the traditional manual commands:

• FW, BW, Left, Right and Stop where the app will send respectively to the BT module:'f', 'b', 'l', 'r' and 's'.

Also 3 sliders were included, one for each PID constants:

• Kd:"d/XXX" where "XXX" it is a number from 0 to 100.

• Kp:"p/XXX"

• Ki:"i/XXX"

An extra button was included that will work exactly as the button connected on Arduino Pin9. You can use one or the other, it does not matter.

Below you can find the .aia file that can be modified at MIT AppInventor and the .apk file to be installed directly in your Android device.

Step 11:Changing the Code for PID remote tuning

During the Setup, we will introduce a loop where you can send the PID parameters to the Robot before you put him over the line:

 while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command =""; } checkPIDvalues(); mode =STOPPED; 

The manual command function will be:

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

In the video, you can see some tests using the Android App. Below the final code including the PID setup via Android:

Step 12:Conclusion

This is the first part of a more complex project, exploring the potentiality of a line follower robot. In the next part, I will develop a Maze solve robot, based on this this project here. Hopefully I can contribute for others to learn more about electronics, robot, Arduino, etc.

The update files for this project can be found at GITHUB. For more tutorials, please visit my Blog:MJRoBot.org

Greetings from the south of the world!

Thanks

Marcelo

#include  // Servo library Servo leftServo;Servo rightServo;Void setup(){ leftServo.attach(5); rightServo.attach(3); leftServo.writeMicroseconds(1500); rightServo.writeMicroseconds(1500);}void loop(){}

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

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

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

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;

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

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

void loop (){ readLFSsensors(); // read sensors, storage values at Sensor Array and calculate "error" calculatePID(); motorPIDcontrol();}

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

 while (digitalRead(buttonPin) &&!mode) { checkBTcmd(); // verify if a comand is received from BT remote control manualCmd (); command =""; } checkPIDvalues(); mode =STOPPED;

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