Fórum témák

» Több friss téma
Cikkek » WILL-I THE END (robotika és AVR kezdőknek)
WILL-I THE END (robotika és AVR kezdőknek)
Szerző: Fizikus, idő: Máj 17, 2010, Olvasva: 23609
Lapozás: OK   7 / 8

WILL-I V3.0 programja

A vezérlőprogram szinte ugyanaz mint a második verzióé (LINK).

A különbség csak annyi, hogy a karokon lévő mikrokapcsolók jeleit külső megszakításként használva, a robot ha előre menet, vagy kanyarodás közben a karjaival akadályba ütközik, akkor  azonnal megáll, hátratolat, majd elfordul az akadállyal ellentétes irányba.

A fenti algoritmus folyamatábrán ábrázolva így néz ki:

(Ahhoz hogy az alábbi kód működjön, kell az előző cikkben leírt motor.h kód is! (LINK))

  1. /* WILL-I V2.0 vezerlo program
  2. Nyomogomb: PD4-es labra kotve
  3. Bal mikrokapcsolo: PD2-es labra kotve (INT0)
  4. Jobb mikrokapcsolo: PD3-es labra kotve (INT1)
  5. Sharp IR szenzor: PC0-as (ADC0) labra kotve
  6. */
  7. //---------------------------------------------------------------------
  8.  
  9. #define F_CPU 1000000UL     // 1 MHz-es CPU orajel megadasa
  10. #define jobbra 16         // Szervo jobb szelso pozicio
  11. #define kozepre 23         // Szervo kozepso (Neutral) pozicio
  12. #define balra 31         // Szervo bal szelso pozicio
  13. #include <util/delay.h>     // idozito, keslelteto rutinokat tart. fajl
  14. #include <avr/io.h>         //AVR konstansokat, beallitasokat tart. fájl
  15. #include <avr/interrupt.h>    // Megszakitasokat kezelo rutinokat tart. fajl
  16. #include <util/dcmotor.h>     // motorvezerlo utasitasokat tart. fajl
  17.  
  18. volatile int SharpIR = 0;
  19. volatile int SharpIRjobb = 0;
  20. volatile int SharpIRbal = 0;
  21. volatile int Balkapcs = 0;
  22. volatile int Jobbkapcs = 0;
  23.  
  24. void Konfig10bitADC()        // ADC konfiguralas (beallitas)
  25. {
  26.     ADMUX |= (1<<REFS0);    // Vcc mint referencia
  27.     ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0);    // ADC engedelyezese, ADC eloosztas = 8 (125 KHz)
  28. }
  29.  
  30. unsigned int Beolvas10bitADC(unsigned char csatorna)
  31. {
  32.     ADMUX = (ADMUX & 0b11110000) | csatorna;
  33.     ADCSRA |= (1<<ADSC);        // elso ADC konverzio elinditasa
  34.     while (ADCSRA & (1<<ADSC));    // varas az atalakitasra
  35.     ADCSRA |= (1<<ADSC);         // masodik ADC konverzió elindítás
  36.     while (ADCSRA & (1<<ADSC));    // varas az atalakitasra
  37.     return (ADCL | (ADCH<<8));    // ADC ertek kiolvasasa
  38. }
  39.  
  40. void KonfigSzervo()        // Szervo konfiguralas (beallitas)
  41. {
  42.     DDRB = _BV(DDB3);     // PORTB 3. lab kimenet (Szervo PWM)
  43.  
  44.     TCCR2 = _BV(WGM20)     // Timer2 8bites gyors PWM mod
  45.           | _BV(WGM21)     // Timer2 8bites gyors PWM mod
  46.           | _BV(COM21)     // nem-invertalt PWM
  47.           | _BV(CS22);     // Timer2 eloosztas: 1/64 (61 Hz-es PWM impulzus frekvencia)
  48.     OCR2 = 23;     // 1.5ms-os kezdeti PWM impulzus (Szervo kozepso (Neutral) pozicio)
  49. }
  50.  
  51. void Szervo(unsigned char pozicio)        // Szervo pozicionalo utasitas
  52. {
  53.     OCR2 = pozicio;
  54. }
  55.  
  56. ISR (INT0_vect)    // INT0 megszakitas kiszolgalo rutin utasitasai (Bal mikrokapcsolo)
  57. {
  58.     GICR &= ~(1<<INT0);    //  INT0 letiltasa (nyomogomb perges megakadalyozasa erdekeben)
  59.     motor_stop(mind);     // Robot STOP!
  60.     Balkapcs  = 1;        //
  61. }
  62.  
  63. ISR (INT1_vect)     // INT1 megszakitas kiszolgalo rutin utasitasai (Jobb mikrokapcsolo)
  64. {
  65.     GICR &= ~(1<<INT1);    //  INT1 letiltasa (nyomogomb perges megakadalyozasa erdekeben)
  66.     motor_stop(mind);         //Robot STOP!
  67.     Jobbkapcs  = 1;        //
  68. }
  69.  
  70. int main (void)
  71. {
  72.     // portok beallitasa
  73.     PORTD |= (1<<PD4);    // PD4-es lab bemenet, pull-up bekapcs (nyomogomb)
  74.     DDRC &= ~(1<<PC0);    // PC0-as lab bemenet (Sharp IR szenzor)
  75.     PORTC = 0x00;        // PORTC osszes laban a felhuzoellenallasok kikapcsolva
  76.     PORTD &= ~(1<<PD2);    // PD2 bemenet
  77.     PORTD |= (1<<PD2);    // PD2-hoz tartozo felhuzoellenallas be
  78.     PORTD &= ~(1<<PD3);    // PD3 bemenet
  79.     PORTD |= (1<<PD3);    // PD3-hoz tartozo felhuzoellenallas be
  80.  
  81.     // az INT0 kulso interrupt beallitasa
  82.     GICR |= (1<<INT0);        // INT0 engedelyezese (PD2-es lab)
  83.     MCUCR |= (1<<ISC01);     // a lab lefuto elre adjon megszakítást
  84.  
  85.     // az INT1 kulso interrupt beallitasa
  86.     GICR |= (1<<INT1);    // INT0 engedelyezese (PD2-es lab)
  87.     MCUCR |= (1<<ISC11);     // a lab lefuto elre adjon megszakítást
  88.  
  89.     sei();         // megszakítások bekapcsolasa
  90.  
  91.     KonfigSzervo();    // Szervo beallitas lefuttatasa
  92.    
  93.     Konfig10bitADC();    // ADC beallitas lefuttatasa
  94.  
  95.     while (PIND & (1<<PD4)); // varakozo cilkus amig PD4 erteke nem 0 (amig a gomb nincs lenyomva)
  96.  
  97.     while (1)
  98.     {
  99.  
  100.     // Ha INT0 megszakitas ki van kapcsolva, akkor egy kis varakozas utan bekapcsoljuk
  101.             if((GICR & (1<<INT0)) == 0)
  102.             {
  103.             // varakozas nehany szaz ms-ig, amig a nyomogomb befejezi a pergest
  104.                 _delay_ms(250);
  105.     // minden tovabbi varakozo megszakitas torlese, majd a megszakitasok engedelyezese
  106.                 GIFR |= (1<<INTF0);        // INT0-hoz tartozo jelzobit torlese
  107.                 GICR |= (1<<INT0);        // INT0 megszakítás engedelyezese
  108.             }
  109.        
  110.     // Ha INT1 megszakitas ki van kapcsolva, akkor egy kis varakozas utan bekapcsoljuk
  111.             if((GICR & (1<<INT1)) == 0)
  112.             {
  113.             // varakozas nehany szaz ms-ig, amig a nyomogomb befejezi a pergest
  114.                 _delay_ms(250);
  115.     // minden tovabbi varakozo megszakitas torlese, majd a megszakitasok engedelyezese
  116.                 GIFR |= (1<<INTF1);        // INT1-hoz tartozo jelzobit torlese
  117.                 GICR |= (1<<INT1);        // INT1 megszakitas engedelyezese
  118.             }       
  119.  
  120.             if ( Balkapcs == 1 )   // Ha Bal oldali utkozes tortent, akkor
  121.             {
  122.                 Balkapcs  = 0;        // Bal oldali utkozest jelzo valtozo torlese
  123.                 hatra(100);        //  hatramenet
  124.                 _delay_ms(1000);      //  1.0  s kesleltetes
  125.                 fordul_jobb(100);    //  fordulas jobbra
  126.                 _delay_ms(500);    //  0.5  s kesleltetes
  127.             }
  128.  
  129.             if ( Jobbkapcs == 1 )   //  Ha Jobb oldali utkozes tortent, akkor
  130.             {
  131.                 Jobbkapcs  = 0;    //  Jobb oldali utkozest jelzo valtozo torlese
  132.                 hatra(100);        //  hatramenet
  133.                 _delay_ms(1000);      // 1.0  s kesleltetes
  134.                 fordul_bal(100);    //  fordulas balra
  135.                 _delay_ms(500);    //  0.5  s kesleltetes
  136.             }
  137.  
  138.         elore(100); // teljes gozzel elore!
  139.         SharpIR = Beolvas10bitADC(0);
  140.         if ( SharpIR > 200 ) 
  141.         { 
  142.             motor_stop(mind);
  143.             Szervo(balra);
  144.             _delay_ms(500);     // 0.5 s kesleltetes
  145.             SharpIRbal = Beolvas10bitADC(0);
  146.             _delay_ms(500);     // 0.5 s kesleltetes
  147.             Szervo(jobbra);
  148.             _delay_ms(500);     // 0.5 s kesleltetes
  149.             SharpIRjobb = Beolvas10bitADC(0);
  150.             _delay_ms(500);     // 0.5 s kesleltetes
  151.             Szervo(kozepre);
  152.             if ( SharpIRbal > SharpIRjobb)
  153.                 {
  154.                     fordul_jobb(100); // fordulas balra 0.5 s
  155.                     _delay_ms(500);
  156.                 }
  157.                 else
  158.                 {
  159.                     fordul_bal(100); //fordulas jobbra 0.5 s
  160.                     _delay_ms(500);
  161.                 }
  162.         }
  163.         else
  164.         {
  165.             _delay_ms(100);     // 0.1 s kesleltetes
  166.         }     
  167.     }
  168. }

 

Következő: »»   7 / 8
Értékeléshez bejelentkezés szükséges!
Bejelentkezés

Belépés

Hirdetés
Frissek
2014. Okt, 02. Csü
16:32:09
Jelenleg 421 fő olvassa az oldalt
Online tagok:
Lapoda.hu     XDT.hu     HEStore.hu