Linjeföljare Digitala projekt Fredrik Boberg (820714-3978) Axel Gustafson (830706-5576) e02fb@efd.lth.se e02agu@efd.lth.se
Abstract In this report we describe the construction of a line follower. The follower is built in Lego and uses four motors for the forward movement. The steering is controlled by a step engine that adjusted the steering angel depending on the poison of the car. To know were the car is regarding to the line we use two ir-diodes as a backlight, and two photodiodes that should detect how much of the ir-light that is reflected. The control of this advanced equipment is handle by an Atmel AVR mega16 processor. To make the processor work we have written a c-program that calculates the right steering parameters depending on the witch of the photodiodes that received the most light.
Innehållsförteckning Innehållsförteckning 3 Inledning 4 Kravspecifikation 4 Funktionsbeskrivning 4 Hårdvaran 5 Mjukvaran 5 Genomförande 6 Resultat 7 Slutsats 7 Referenser 7 Bilaga 1 8 Bilaga 2 9
Inledning Vem har inte drömt om en bil som kör av sig själv och låter föraren ta en välbehövlig paus? Vårat projekt har tagit första steget mot att förverkliga denna dröm. Det som vi i gruppen har utvecklat är en liten legobil som skall följa en svart linje i golvet. Med hjälp av optiska sensorer och en Atmel AVR mega16 processor sköts kontrolleringen av bilen. Kanske någon gång i framtiden får vi se en bil i fullstorlek som bygger på samma huvudprincip. Kravspecifikation Vår egenhändigt byggda bil skall klara av att följa en upp märkt bana under normal rumsbelysning. Banan är konstruerad av en enkel linje eltejp. Tejpen är utplacerad så att bilen måste klarar både raksträckor och kurvor. Funktionsbeskrivning För att bestämma var bilen befinner sig utifrån linjen användes två ir-dioder som belyser underlaget. För att veta hur mycket av ir-ljuset som reflekterats mot underlaget användes två fotodioder. Dessa fotodioder är orienterade på var sin sida om linjen, se bild 1. Beroende på om fotodioden befinner sig rakt ovanför tejplinjen eller strax utanför kommer där att ligga olika mycket spänning över den. En mörkare färg dvs. eltejpen ger upphov till mindre reflektioner, vilket i sin tur ger en mindre spänning över den berörda fotodioden. Genom att veta vilken spänning som ligger
över respektive fotodiod så kan bilens position förhållande till linjen bestämmas. Spänning över respektive fotodiod läses sedan in i processor med hjälp av de inbyggda A/D omvandlarna. Med denna information bestäms sedan styrparametrarna till en stegmotor som reglerar styrning av framhjulen. Framdrivning av bilen sköts av fyra stycken elmotorer som är direkt kopplade till bilens bakaxel via en diffrential. Regleringen av dessa motorer sköts av processorn samt en H-brygga. För att förtydliga vad H-brygga egentligen är för något så kan den liknas med ett avancerat relä. Själva Bryggan är direkt kopplad till batteriet och triggas sedan med hjälp av processorn. När denna triggning sker kommer spänning över bryggan skickas vidare till elmotorerna. Genom att använda denna typ av komponent erhålles en ställbar reglering av framdrivningen. Hastigheten på bilen styrs därför med avseende på vilken frekvens som processor triggar H-bryggan (pulsstyrning). Hårdvaran Nedan i figur 2 följer ett blockschema som visar kommunikationen mellan huvud beståndsdelarna i konstruktionen. I bilaga 1 finns ett komplett kopplingsschema att beskåda. Figur 2 blockschema
Mjukvaran Programmet är uppbyggt av tre tidsstyrda avbrott och ett händelsestyrt avbrott samt ett huvudprogram. Huvudprogrammets uppgift är att initiera alla avbrott samt sätta värden på all de variabler som används i programmet. De tre tidsstyrda avbrotten sköter styrning av stegmotorn, pulsning av motorerna för framåt drift samt ett avbrott för inläsning från dioderna. Det händelse styrda avbrottet sker då AD omvandlingen av de analoga signalerarn från dioderna är färdig. I detta avbrott sker även alla beräkningar av styrreferenser samt hastighet av motorerna. Det som var mest krävande i programmeringen var att komma på det bästa sättet att styra bilen. Vi valde en positions styrning av vinkeln på framhjulen, dvs programmet håller reda på i vilken vinkel som hjulen står och regler utifrån det. Detta ger en jämnare och bättre reglering av bilen. Styr referensen beräknas via en medelvärdes PI kontroller. Medelvärdet på de senaste 6 värdena från dioderna används för att ge proportionaldelen (P-delen). Summan av de 10 senaste P-delarna ger integral delen. P och I delen adderas sedan för att ge styrreferensen. Medelvärdet används för att eliminera störningar som kan förekomma i diodinläsningen. För att göra bilen extra snabb har vi även implementerat en variabel hastighet av bilen. Då bilen är på en raksträcka så accelererar bilen och då det svänger så kör bilen långsammare. I bilaga 2 finns hela C-koden. Genomförande Det först delen av detta projekt bestod i att skapa en kravspecifikation. Efter att tagit fram denna så föreföll det naturligt att börja skissa på en möjlig konstruktion som kunde uppfylla de krav som satts upp. I detta arbetet ingick bland annat att konstruerar ett komplett kopplingsschema samt en legobil som kunde bära med sig nödvändig elektronik.. Till en början var det mycket komponent letande samt utprövning av storheter på olika motstånd. Att hitta rätt motstånd till foto och lysdioderna krävde mycket mätande med multimetern samt en del beräkningar. I bilaga 1 finns ett komplett kopplings schema där alla storheter är utmärkta. Spänningen över respektive fotodiod minskade med ca 20mV då ett mörkt föremål placerades framför. Positioneringen av dioderna krävde även det en del finjustering för att anpassas mot linjen. Avståndet mellan diod paren valdes till ca 3cm. Styrning av de fyra motorerna som sköter framdrivning krävde en del eftertanke. Att driva dem direkt från processorns utgångar var aldrig ett alternativ, beroende på att den ger ut för låg effekt. Efter att ha konsulterat med våran handledare så var valet enkelt nämligen en H-brygga var det som behövdes. Det finna med denna komponent
var att den var kapabel till att lämna den effekt som motorena erfordrade samt att den kunde puls köras. Frekvensen på den puls som processorn mattade H-bryggan med genererades med hjälp av internt avbrott i processorn. Efter en del laborerande med H-bryggan upptäckte vi att den inte klarade alltför hög frekvens på styrsignalen. Detta kom vi fram till måste ha berott på att transistorerna inuti bryggan hade en viss inbyggd fördröjning. Om vi istället ser till själva styrningen av bilen så vålade även det problem. Stegmotorn som sköte rattutslaget var tvungen att matas med en fyra bitars sekvens som skulle uppdateras med en viss frekvens. Frekvensen som bitarna skrivs till motorn är direkt proportionerlig till hur snabbt som motorn snurrar. En alt för snabb frekvens resulterade endast i att motorn stod helt stilla. Om istället frekvens var för låg rörde sig motorn mycket långsamt. En annan frågeställning som vi hade var om processorns utgångar skulle klara av att driva stegmotorn. Drivning gick fint men den var relativt kraftlös. Motorn var ganska svag eftersom den var avsedd att drivas med 12 volt men vart batteri kunde bara leverera knappt 6 volt. Detta problem löste vi med hjälp av att applicera en låg utväxling. Sist men inte minst så var styrprogrammet som styrde processorn den del av projektet som var mest tidskrävande. Själva programmet skrevs i C och dess huvuduppgift var att göra alla beräkningar och avläsningar.
Resultat I det stora hela är vi mycket nöjda med resultatet. Självklart finns det saker som kunde gjorts bättre som tex. reglersystemet och den mekaniska konstruktionen vilket skulle medför att bilens hastighet kunde ökats. Trots detta så uppfyller vi kravspecifikation vilket var vårat ursprungliga mål. Att bilen inte färdas med en avsevärd hastighet är inget som vi tagit hänsyn till, huvudfunktionen var således det som prioriterades. Tillförlitligheten på konstruktionen är relativt bra. Underlaget är dock en stor felkälla till att bilen kan tappa spåret. Slutsats Det har varit en mycket rolig och givande kurs att läsa. För första gången i vårat akademiska liv har ansvaret helt och hållet lagts på oss själva vilket har varit väldigt lärorikt. Förutom att ta ansvar har vi även lärt oss lite om programmering i C, kretsdesign samt hur en processor kan tänkas fungera. En annan intressant aspekt av projektet var att det tog mycket längre tid än vad vi först trott att färdigställa linjeföljaren. Det har således varit många sena nätter och tidiga morgnar för att lösa alla nyuppkomna problem med konstruktionen. Referenser 1 www.atmel.com 2 Utdelade datablad samt kursens hemsida http://www.it.lth.se/digp/information.asp 3 Instruktionsboken till processorn
Bilaga 1
Bilaga 2 #include <avr/io.h> #include <inttypes.h> #include <avr/wdt.h> #include <avr/signal.h> #include <avr/interrupt.h> int8_t i; int8_t pulls; int16_t time_low; int16_t time_high; int16_t varv; int16_t max_varv; int16_t varv_ref; int16_t varv_ref_temp; int16_t lock; int16_t lock_2; int16_t t; double percent_high; double full_speed; double low_speed; int16_t result[6]; int16_t r; int16_t ti; int16_t temp; int8_t length_vektor; int16_t diff; int16_t brake_point; int16_t p; int16_t e; int16_t t_pi; int16_t ki; int32_t varv_ref_i; int16_t varv_ref_pi; double kpi; int32_t temp_pi; int16_t temp_pi_v[8]; int16_t y; int main(void){ y=0; t_pi=0; ki=8; varv_ref_i=0; varv_ref_pi=0; temp_pi=0; kpi=0.1;
lock_2= 1; e=0; p=0; lock=0; brake_point=12; //Vid vilket fel som vi skall bromsa bak motorerna diff = 31; //Flyttar nollan i den inlästa vektorn temp=0; //Används för att beräkna varv_ref r=1; //stegvariabel t=0; //stegvariabel ti=0; //stegvariabel varv_ref=0; //Vilken vilket håll som vi skall svänga åt varv_ref_temp=0; t=0; //stegvariabel low_speed = 0.5; //låga hastigheten full_speed = 0.9; //Höga hastigheten percent_high = full_speed; //Hur stor del av pulserna som skall vara höga time_high=1023*percent_high; //Hur lång en hög puls är time_low =1023-time_high; //Hur lång en hög puls är varv=0; i = 0; pulls = 0; max_varv = 37; //hur många varav stegmotorn har snurrat //stegvariabel //Bestämmer om det skall komma en hög eller låg pulls //Max varvutslag DDRB = 0xff; DDRD = 0xff; //Port B för utsignaler //Port D för utsignaler ADCSR = _BV(ADPS0) _BV(ADPS1) _BV(ADPS2); //AD omvandling TCCR1B = _BV(CS10); // dividerat på 8 TCCR1A = _BV(WGM10) _BV(WGM11); TIMSK = _BV(TOIE1) _BV(OCIE1A) _BV(OCIE1B); //sätter på overflow och två compare avbrott OCR1A = time_low/2; //Starvärden på pulslängder OCR1B = 100; //bestämmer när inläsning sker TCNT1 = 0x03ff; sei(); i=0; while(1){ INTERRUPT (SIG_OVERFLOW1){
if (t==11){ //Svänger vänster if (varv < varv_ref_pi - 5){ if (i==0){ PORTB = 0x05; if (i==1){ PORTB = 0x09; if (i==2){ PORTB = 0x0A; if (i==3){ PORTB = 0x06; i=i+1; if (i==4){ i=0; varv = varv + 1; if (varv > varv_ref_pi + 5){ //Svänger höger t=t+1; if (t==12){ t=0; if (i==0){ PORTB = 0x06; if (i==1){ PORTB = 0x0A; if (i==2){ PORTB = 0x09; if (i==3){ PORTB = 0x05; i=i+1; if (i==4){ i=0; varv = varv - 1;
INTERRUPT (SIG_OUTPUT_COMPARE1A){ if(p == 40){ if(1){ //Pullskörning av bakmotorerna if (pulls==0){ PORTD = 0x30; OCR1A = time_low/2; if (pulls==1){ PORTD = 0x20; pulls=pulls + 1; //motor på //motor av if(pulls==2){ pulls=0; p=p+1; if(p==41){ p=0; INTERRUPT (SIG_OUTPUT_COMPARE1B){ if (e == 20){ if (1){ (REFS0); e=e+1; if(e==21){ e=0; //Inläsning från dioderna ADMUX = _BV(MUX3) _BV(MUX0) _BV(REFS1) _BV //ADC0 och ADC1 ADCSRA = _BV(ADEN) _BV(ADSC) _BV(ADIE); //ADC Enable INTERRUPT (SIG_ADC){ //Inläsning klar result[ti] = ADCL; result[ti] = (int)adch<<8; if(result[ti]>980){ result[ti]=512;
if(result[ti]<40){ result[ti]=512; temp = temp + result[ti]; ti = ti + 1; if (ti == 6){ ti=0; temp = ((temp)/6); varv_ref = temp/16-diff; temp=0; temp_pi_v[y] = varv_ref; //flyttar nollan while (t_pi < ki){ // Integral temp_pi = temp_pi + temp_pi_v[t_pi]; t_pi=t_pi+1; t_pi = 0; varv_ref_i = temp_pi; temp_pi = 0; y=y+1; if (y == ki){ y=0; varv_ref_pi = varv_ref_i*kpi + varv_ref; if (varv_ref_pi > max_varv){ // begränsar utslaget varv_ref_pi = max_varv; if (varv_ref_pi < -max_varv){ // begränsar utslaget varv_ref_pi = -max_varv; if (varv_ref_pi > brake_point){ //Bestämmer om vi skall brommsa eller inte percent_high = low_speed; else if (varv_ref_pi < -brake_point){ percent_high = low_speed; else{ percent_high = full_speed; time_high=1023*percent_high; time_low =1023-time_high; //beräknar pullslängder //beräknar pullslängder