Følge kanten af en sort streg

Ole Caprani
Institut for Datalogi, Aarhus Universitet,
ocaprani@cs.au.dk

Sidst opdateret 10.8.15.

Programmering af kørsel langs kanten af en sort streg

På de fleste robotbaner er der sorte streger, som kan benyttes, når robotten skal navigeres rundt imellem forskellige positioner på banen. Med en farvesensor monteret foran på robotten, som f.eks. vist her:
kan robotten programmeres til at følge kanten af den sorte streg. Farvesensoren registrerer hele tiden, hvor stregen er i forhold til robotten og på basis af farvesensorens registrering programmeres robottens to motorer til at styre hjulene, så robotten holder sig i nærheden af kanten af stregen under kørslen langs stregen.

Under kørslen f.eks. langs den venstre kant af stregen i robottens kørselsretning, ønskes det altså, at robotten programmeres til en styring, så robotten kører ligeud med farvesensoren lige over kanten af stregen:

Når robotten under kørslen har farvesensoren på det hvide til venstre for kanten eller på den sorte streg til højre for kanten, skal robotten altså "rettes op", så den igen kører med farvesensoren over kanten. Det betyder, at robotten skal holde til højre, når farvesensoren registrerer hvid og robotten skal holde til venstre, når farvesensoren registrerer sort:
For at køre ligeud og samtidig holde til højre, skal motor B dreje hurtigere rundt end motor C. For at få robotten til at holde til venstre skal motor C dreje hurtigere end motor B. Men hvor meget hurtigere? Og når farvesensoren er langt væk fra kanten af stregen, skal robotten vel rettes mere op end når farvesensoren er næsten over kanten? Det viser sig, at farvesensorens registrerede lysværdi kan benyttes til at afgøre, hvor meget hurtigere det ene hjul skal dreje end det andet og dermed rette robotten mere eller mindre op afhængig af, hvor tæt farvesensoren er over kanten.

Når farvesensoren er tæt på kanten registreres en lysværdi, der ligger imellem f.eks. 10% (farvesensors røde lysende cirkel helt over sort) og 84% (farvesensor over hvid). Med den røde lysende cirkel halvt over hvid og halvt over sort er farvesensoren lige over kanten og så registreres en lysværdi, som er (84+10)/2 = 47%.

Prøv at bruge portvisning til at undersøge, hvordan farvesensorens lysværdi ændres, når farvesensoren føres fra én position helt over sort til én position helt over hvid. Se eventuelt under EV3 hjælp > Generelt > Hardwareside > Portvisning.
Når farvesensoren er lige over kanten, registreres en værdi, som kaldes offset. I eksemplet er offset altså = 47%. Ved at sammelligne den registrerede lysværdi, kaldet lightValue, med offset, kan det afgøres i hvilken retning robotten skal styres i forhold til kanten:
  • offset = lightValue, farvesensoren er lige over kanten, robotten skal køre ligeud.
  • offset < lightValue, farvesensoren er til venstre for kanten, mere over hvid end sort, robotten skal holde til højre.
  • offset > lightValue, farvesensoren er til højre for kanten, mere over sort end hvid, robotten skal holde til venstre.
Forskellen imellem offset og lightValue, altså offset-lightValue, er et mål for, hvor langt farvesensoren er fra kanten. Denne forskel kan bruges til at afgøre, hvor meget robotten skal holde til højre eller venstre, når robotten skal rettes op. Hvis forskellen er lille, skal robotten blot rettes lidt op, er forskellen stor, skal robotten rettes meget op. Ideen er altså, at forskellen i motorhastighed imellem de to motorer skal være propertional med forskellen offset-lightValue. Det kan f.eks. opnås således:
I den første ligning sættes turn til at være propertional med forskellen offset-lightValue. Propertionalitetsfaktoren kaldes P. Hvis P er positiv, har turn samme fortegn som offset-lightValue. I de to sidste ligninger bruges turn til at justere power til de to motorer i forhold til en konstant power kaldet minPower. Ved at se på de tre tilfælde igen som ovenfor, kan det ses, at ligningerne netop giver den ønskede styring:
  • turn = 0, altså offset = lightValue, ingen forskel i motorhastighed på de to motorer: robotten kører ligeud med minPower på begge motorer.
  • turn < 0, altså offset < lightValue, robotten holder til højre, da power til motor B er abs(turn) højere end minPower og power til motor C er abs(turn) mindre end minPower.
  • turn > 0, altså offset > lightValue, robotten holder til venstre, da power til motor C er turn højere end minPower og power til motor B er turn mindre end minPower.
Denne idé til hvordan motorerne skal styres for at holde robotten på kanten af stregen, kaldes P-regulering eller propertional regulering (Se en mere detaljeret gennemgang i J. Sluka, A PID Controller For Lego Mindstorms Robots).

LeftEdgeDrive

På videoen ses en robot, som følger den inderste kant af en sorte streg rundt i en cirkel:

Robotten har en farvesensor tilsluttet port 3 og to motorer tilsluttet port B og C. Robotten styres af et program, som hedder testLeftEdgeDrive.ev3p. Programmet findes i projektet FollowEdge.ev3. Programmet ser sådan her ud:
I programmet gentages udførelsen af den brugerdefinerede programmeringsblok LeftEdgeDrive ubegrænset i løkken. LeftEdgeDrive benytter P-regulering til at styre de to motorer, så robotten følger kanten af den sorte streg. For at kunne følge kanten ved hjælp af P-regulering skal variablen offset sættes inden løkken, så værdien af offset svarer til den aktuelle lysværdi farvesensoren registrerer over kanten. En måde at finde hvilken værdi offset skal have er at måle lysværdien over hvid og sort og udregne gennemsnit af de to værdier, f.eks. som vist (75+6)/2 % = 40,5 % (rundet til 40 for nemheds skyld):
LeftEdgeDrive har to input parametre, minPower og P. Parametren minPower bestemmer, hvor meget power de to motorer skal have, når robotten kører ligeud over kanten. Parametren P er propertionalitetsfaktoren i P-reguleringen. Jo større P værdi jo skarpere drejer robotten mod kanten, når farvesensoren ikke er over kanten.

Prøv at eksperimenterer med de to input parametre for at finde ud af hvilke værdier, som får robotten til at følge en kant så hurtigt som muligt uden at robotten kører væk fra kanten. Prøv med streger med forskellig krumning.
Åbnes LeftEdgeDrive ser den sådan her ud:
Blokken udregner power til motor B og C ud fra lysværdien fra farvesensoren, værdien af den variable offset samt de to input parametre. Det sker ud fra P-reguleringsligningerne:
Den første ligning udregnes af den første matematik-blok, se EV3 hjælp > Programmering > Data-blokke > Matematik. Lysværdien fra farvesensoren på port 3 registreres af en farvesensor-blok, se EV3 hjælp > Programmering > Sensorblokke > Farve. Værdien af turn fra den første ligning er altså resultatet af udregningen i fra den første matematik-blok. Værdien af turn føres videre til de to andre matematik-blokke, som udregner power til motor B og C ud fra de to næste ligninger.