| 1,22 → 1,290 |
|---|
| //********* Robot Camerus pro IstRobot 2007 ************ |
| //"$Id: camerus.c 229 2007-04-09 11:41:19Z kakl $" |
| //***************************************************** |
| |
| #include ".\camerus.h" |
| |
| #USE FAST_IO (C) // Brana C je ve FAST_IO modu, aby slo rychle cist z kamery |
| |
| // A/D vstupy |
| #define RMAX 4 // AN4/RA5 - leve cidlo na vyjeti z cary |
| #define LMAX 3 // AN3/RA3 - prave cidlo na vyjeti z cary |
| #define CERVENA 2 // AN2/RA2 - cervene kroutitko |
| #define ZELENA 1 // AN1/RA0 - zelene kroutitko |
| #define MODRA 0 // AN0/RA1 - modre kroutitko |
| |
| // I/O |
| #define HREF PIN_C5 // Signal HREF z kamery (v H po celou dobu radku) |
| #define PIX PIN_C6 // Vstup pro body z kamery (za trivstupim hradlem OR (dig. komparator)) |
| #define SERVO PIN_B4 // Vystup na servo (1 az 2ms po cca 20ms (synchronizovano snimkovym kmitoctem)) |
| #define MOT_L PIN_B5 // Smer otaceni leveho motoru; druhy pol je RC2 |
| #define MOT_R PIN_B6 // Smer otaceni praveho motoru; druhy pol je RC1 |
| #define MOT_1 PIN_C1 // PWM vystpy motoru |
| #define MOT_2 PIN_C2 // |
| #define DATA PIN_B2 // K modulu LEDbar data |
| #define CP PIN_B1 // K modulu LEDbar hodiny |
| //#define ODO PIN_C0 // Ze snimace z odometrie z praveho kola na TIMER1 |
| // Jeden impuls je 31,25mm |
| #define IRRX PIN_B0 // Vstup INT, generuje preruseni pri prekazce |
| #define IRTX PIN_B3 // Modulovani vysilaci IR LED na detekci prekazky |
| #define PROXIMITY PIN_C7 // Cidlo kratkeho dosahu na cihlu |
| |
| #define CASMIN 6 // Rozsah radku snimace |
| #define CASMAX 192 |
| #define CASAVR ((CASMAX+CASMIN) / 2) |
| |
| #define OFFSETO 0x9F //0x9F // Vystredeni serva pro objeti prekazky |
| |
| #define THR 90 // Threshold pro UV cidla na caru |
| |
| #byte INTCON = 0x0B // Interrupt configuration register |
| #bit GIE = INTCON.7 |
| #bit PEIE = INTCON.6 |
| #bit TMR0IE = INTCON.5 |
| #bit INT0IE = INTCON.4 |
| #bit RBIE = INTCON.3 |
| #bit TMR0IF = INTCON.2 |
| #bit INT0IF = INTCON.1 |
| #bit RBIF = INTCON.0 |
| |
| enum stavy {start,rozjezd,jizda,cihla,pocihle,cil}; |
| stavy stav; // Kde jsme na trati |
| int8 cas; // Cas hrany bila/cerna v radce |
| int8 stred; // Vystredeni kolecka |
| int16 odocounter; // Zaznamenani aktualniho stavu pocitadla odometrie |
| int8 rr; // Promenna na ulozeni Rozumne rychlost |
| int8 rrold; |
| //int8 pole_h[0x40]; |
| int8 pole_l[0x40]; |
| |
| // Zobrazeni jednoho byte na modulu LEDbar |
| inline void disp(int8 x) |
| { |
| int n; |
| |
| for(n=0;n<=7;n++) |
| { |
| if (x & 1 == 1) output_low(DATA); else output_high(DATA); |
| output_high(CP); |
| x>>=1; |
| output_low(CP); |
| } |
| } |
| |
| // Blikani LEDbarem ve stilu Night Rider |
| void NightRider(int8 x) |
| { |
| int n,i,j; |
| |
| for(j=0;j<x;j++) |
| { |
| i=0x01; |
| for(n=0;n<7;n++) |
| { |
| disp(i); |
| rotate_left(&i, 1); |
| delay_ms(40); |
| } |
| for(n=0;n<7;n++) |
| { |
| disp(i); |
| rotate_right(&i, 1); |
| delay_ms(40); |
| } |
| } |
| disp(i); |
| delay_ms(40); |
| i=0; |
| disp(i); |
| } |
| |
| // Brzdeni motorama stridou 1:1 |
| void brzda() |
| { |
| int8 n,i; |
| |
| set_pwm1_duty(0); // vypni PWM |
| set_pwm2_duty(0); |
| setup_ccp1(CCP_OFF); |
| setup_ccp2(CCP_OFF); |
| for (n=0;n<200;n++) |
| { |
| output_low(MOT_L); |
| output_low(MOT_R); |
| output_high(MOT_1); |
| output_high(MOT_2); |
| delay_us(200); |
| output_high(MOT_L); |
| output_high(MOT_R); |
| output_low(MOT_1); |
| output_low(MOT_2); |
| delay_us(200); |
| } |
| output_low(MOT_L); // smer vpred |
| output_low(MOT_R); |
| setup_ccp1(CCP_PWM); // RC1 // Zapni PWM pro motory |
| setup_ccp2(CCP_PWM); // RC2 |
| } |
| |
| void SetServo(int8 angle) |
| { |
| int8 n; |
| |
| for(n=0; n<14; n++) |
| { |
| output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo |
| delay_us(1000); |
| delay_us(stred); |
| delay_us(stred); |
| delay_us(stred); |
| delay_us(angle); |
| delay_us(angle); |
| output_low(SERVO); |
| delay_ms(18); |
| } |
| } |
| |
| inline void SetServoQ(int8 angle) |
| { |
| output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo |
| delay_us(1000); |
| delay_us(stred); |
| delay_us(stred); |
| delay_us(stred); |
| delay_us(angle); |
| delay_us(angle); |
| output_low(SERVO); |
| } |
| |
| //---------------------------- INT -------------------------------- |
| #int_EXT |
| EXT_isr() // Preruseni od prekazky |
| { |
| set_pwm1_duty(0); // zabrzdi levym kolem, prave vypni |
| set_pwm2_duty(0); |
| output_high(MOT_L); |
| output_low(MOT_R); |
| // Ujistime se, ze prijaty signal je z naseho IR vysilace |
| output_high(IRTX); // Vypni LED na detekci prekazky |
| delay_ms(2); |
| if (!input(IRRX)) // stale nas signal? |
| { |
| output_low(MOT_L); // je odraz -> vpred |
| output_low(MOT_R); |
| return; |
| }; |
| output_low(IRTX); // Zapni LED na detekci prekazky |
| delay_ms(10); |
| if (input(IRRX)) // stale nas signal? |
| { |
| output_low(MOT_L); // neni odraz -> vpred |
| output_low(MOT_R); |
| return; |
| }; |
| |
| //!!! if(stav==cihla) while(true); // Zastav na furt, konec drahy |
| // if(stav==cihla) return; // Po druhe nic neobjizdej |
| // Pozor na rozjezd |
| |
| if(stav==jizda) // Objed cihlu |
| { |
| #include ".\objizdka_L.c" |
| } |
| } |
| |
| |
| //---------------------------------- MAIN -------------------------------------- |
| void main() |
| { |
| setup_adc_ports(NO_ANALOGS); |
| setup_adc(ADC_OFF); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| setup_timer_1(T1_DISABLED); |
| setup_timer_2(T2_DISABLED,0,1); |
| int8 offset; // Promena pro ulozeni offsetu |
| int8 r1; // Rychlost motoru 1 |
| int8 r2; // Rychlost motoru 2 |
| |
| output_high(PIN_C0); |
| setup_adc_ports(ALL_ANALOG); // Zapnuti A/D prevodniku pro cteni kroutitek |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro mereni casu hrany W/B v radce |
| setup_timer_1(T1_EXTERNAL); // Cita pulzy z odometrie z praveho kola |
| setup_timer_2(T2_DIV_BY_16,255,1); // Casovac PWM motoru |
| setup_ccp1(CCP_PWM); // RC1 // PWM pro motory |
| setup_ccp2(CCP_PWM); // RC2 |
| setup_comparator(NC_NC_NC_NC); |
| setup_vref(FALSE); |
| |
| i2c_start(); // Reset |
| i2c_write(0xC0); |
| i2c_write(0x12); |
| i2c_write(0x80 | 0x24); |
| set_tris_c(0b11111001); // Nastaveni vstup/vystup pro branu C, protoze se nedela automaticky |
| |
| set_pwm1_duty(0); // Zastav motory |
| set_pwm2_duty(0); |
| output_low(MOT_L); // Nastav smer vpred |
| output_low(MOT_R); |
| |
| output_low(IRTX); // Zapni LED na detekci prekazky |
| |
| NightRider(1); // Aby se poznalo, ze byl RESET |
| // taky se musi pockat, nez se rozjede kamera, nez se do ni zacnou posilat prikazy |
| |
| |
| /* |
| for(cas=0;cas<=0x3F;cas++) |
| { |
| pole_h[cas]=0x80|cas; |
| pole_l[cas]=cas; |
| } |
| for(cas=0;cas<=0x7F;cas+=2) |
| { |
| write_eeprom(cas,pole_h[cas/2]); |
| write_eeprom(cas+1,pole_l[cas/2]); |
| } |
| */ |
| |
| while(true) |
| { |
| output_high(PIN_B0); |
| delay_ms(200); |
| output_low(PIN_B0); |
| delay_ms(200); |
| } |
| |
| //... Nastaveni sonaru ... |
| i2c_start(); |
| i2c_write(0xE0); |
| i2c_write(0x02); // dosah |
| i2c_write(0x03); // n*43mm |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(0xE0); |
| i2c_write(0x01); // zesileni |
| i2c_write(0x01); // male, pro eliminaci echa z minuleho mereni |
| i2c_stop(); |
| |
| // pro ladeni sonaru |
| /* |
| while(true) |
| { |
| i2c_start(); // Sonar Ping |
| i2c_write(0xE0); |
| i2c_write(0x0); |
| i2c_write(0x51); // 50 mereni v palcich, 51 mereni v cm, 52 v us |
| i2c_stop(); |
| delay_ms(100); |
| i2c_start(); // Odraz ze sonaru |
| i2c_write(0xE0); |
| i2c_write(0x3); |
| i2c_stop(); |
| i2c_start(); |
| i2c_write(0xE1); |
| cas=i2c_read(0); |
| i2c_stop(); |
| disp(cas); |
| } |
| */ |
| |
| //... Nastaveni kamery ... |
| i2c_start(); // Soft RESET kamery |
| i2c_write(0xC0); // Pro single slave musi mit vsechny zapisy adresu C0h |
| i2c_write(0x12); // Adresa registru COMH |
| i2c_write(0x80 | 0x24); // Zapis ridiciho slova |
| i2c_stop(); |
| |
| i2c_start(); // BW |
| i2c_write(0xC0); |
| i2c_write(0x28); |
| 23,44 → 291,222 |
|---|
| i2c_write(0b01000001); |
| i2c_stop(); |
| |
| i2c_start(); // Contrast |
| /* |
| i2c_start(); // Contrast (nema podstatny vliv na obraz) |
| i2c_write(0xC0); |
| i2c_write(0x05); |
| i2c_write(0xFF); |
| i2c_write(0xA0); // 48h |
| i2c_stop(); |
| |
| i2c_start(); // Brightness |
| i2c_start(); // Band Filter (pokud by byl problem se zarivkama 50Hz) |
| i2c_write(0xC0); |
| i2c_write(0x06); |
| i2c_write(0x80); |
| i2c_stop(); |
| |
| /* |
| i2c_start(); // Band Filter |
| i2c_write(0xC0); |
| i2c_write(0x2D); |
| i2c_write(0x04 | 0x03); |
| i2c_stop(); |
| */ |
| |
| i2c_start(); // 4-bit, (CLK/4) |
| i2c_start(); // Fame Rate |
| i2c_write(0xC0); |
| i2c_write(0x3E); |
| i2c_write(0x10 | 0x80); |
| i2c_write(0x2B); |
| i2c_write(0x00); // cca 17ms (puvodni hodnota 5Eh = 20ms) |
| i2c_stop(); |
| */ |
| |
| i2c_start(); // VSTRT |
| i2c_write(0xC0); |
| i2c_write(0x19); |
| i2c_write(45); |
| i2c_write(118); // prostredni radka |
| i2c_stop(); |
| |
| i2c_start(); // VEND |
| i2c_write(0xC0); |
| i2c_write(0x1A); |
| i2c_write(45); |
| i2c_write(118); |
| i2c_stop(); |
| |
| output_low(PIN_C0); |
| NightRider(1); // Musi se dat cas kamere na AGC a AEC |
| |
| while(true); |
| { // Mereni expozice |
| int8 t1,t2; |
| |
| i2c_start(); // Brightness, zacni od uplne tmy |
| i2c_write(0xC0); |
| i2c_write(0x06); |
| i2c_write(0); // 80h default |
| i2c_stop(); |
| delay_ms(50); |
| |
| for(offset=0x04;offset<(255-0x04);offset+=0x04) // Zacni od jasu 10h |
| { |
| i2c_start(); // Brightness |
| i2c_write(0xC0); |
| i2c_write(0x06); |
| i2c_write(offset); // 80h default |
| i2c_stop(); |
| disp(offset); |
| delay_ms(50); |
| |
| t1=0; |
| t2=0; |
| while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky |
| delay_ms(5); |
| while(!input(HREF)); // Cekej nez se zacnou posilat pixely z radky |
| set_timer0(0); // Vynuluj pocitadlo casu |
| if(!input(PIX)) continue; |
| while(input(PIX)); |
| t1=get_timer0(); // Precti cas z citace casu hrany |
| set_timer0(0); // Vynuluj pocitadlo casu |
| while(!input(PIX)); |
| t2=get_timer0(); |
| |
| if((t1>60) && (t1<140) && (t2>5) && (t2<=10)) break; // Vidis, co mas? |
| |
| delay_ms(2); // Preskoc druhou radku z kamery |
| }; |
| delay_ms(1000); // Nech chvili na displayi zmerenou hodnotu |
| } |
| |
| set_adc_channel(CERVENA); // --- Kroutitko pro jas --- |
| delay_ms(1); |
| offset=read_adc(); |
| offset &= 0b11111100; // Dva nejnizsi bity ignoruj |
| // offset += 0x70; // Jas nebude nikdy nizsi |
| disp(offset); |
| i2c_start(); // Brightness |
| i2c_write(0xC0); |
| i2c_write(0x06); |
| i2c_write(offset); // 80h default |
| i2c_stop(); |
| delay_ms(1000); // Nech hodnotu chvili na displayi |
| |
| set_adc_channel(ZELENA); // --- Kroutitko pro vykon motoru --- |
| delay_ms(1); |
| rr=read_adc()>>2; // 0-31 // Pokud by se zvetsil rozsah, tak zkontrolovat jakonasobeni !!! |
| rrold=rr; |
| |
| cas=CASAVR-CASMIN; // Inicializace promenych, aby neslo servo za roh |
| // a aby se to rozjelo jeste dneska |
| stav=start; // Jsme na startu |
| set_timer1(0); // Vynuluj citac odometrie |
| |
| // ........................... Hlavni smycka ................................ |
| while(true) |
| { |
| int8 pom; |
| int8 n; |
| |
| next_snap: |
| |
| pom=0; |
| disable_interrupts(GLOBAL); //----------------------- Critical |
| while(input(HREF)); // Preskoc 1. radku |
| while(!input(HREF)); // Cekej nez se zacnou posilat pixely z 2. radky |
| set_timer0(0); // Vynuluj pocitadlo casu |
| while(input(HREF)) // Po dobu vysilani radky cekej na hranu W/B |
| { |
| // !!!!Dodelat rozpoznani cerne cary napric pro zastaveni ? |
| if(!input(PIX)) // Pokud se X-krat za sebou precetla CERNA |
| if(!input(PIX)) |
| // if(!input(PIX)) |
| { |
| pom=get_timer0(); // Precti cas z citace casu hrany |
| break; |
| }; |
| }; |
| while(input(HREF)); // Pockej na shozeni signalu HREF |
| |
| if((pom<CASMAX) && (pom>CASMIN)) cas=pom; // Orizni konce radku |
| // Na konci obrazovaho radku to blbne. Jednak chyba od apertury |
| // a vubec to nejak na kraji nefunguje. |
| |
| output_high(SERVO); // Odvysilani impuzu 1 az 2ms pro servo |
| delay_us(1000); |
| delay_us(stred); |
| delay_us(stred); |
| delay_us(stred); |
| delay_us(cas); |
| delay_us(cas); |
| output_low(SERVO); |
| |
| // Elektronicky diferencial |
| if(cas<CASAVR) {r1=cas-CASMIN; r2=CASAVR-CASMIN;}; // Normovani vystupni hodnoty radkoveho snimace |
| if(cas==CASAVR) {r1=cas-CASMIN; r2=cas-CASMIN;}; // pro rizeni rychlosti motoru |
| if(cas>CASAVR) {r1=CASAVR-CASMIN; r2=CASMAX-cas;}; // Rozsah 1 az 92 |
| |
| enable_interrupts(GLOBAL); //----------------------- Critical |
| |
| if (r1>(CASAVR-CASMIN-rr)) r1=(r1<<1)+rr-(CASAVR-CASMIN); // Neco jako nasobeni |
| if (r2>(CASAVR-CASMIN-rr)) r2=(r2<<1)+rr-(CASAVR-CASMIN); |
| |
| // r1<<=1; // Rychlost je dvojnasobna |
| // r2<<=1; // Rozsah 2 az 184 |
| |
| /* Nerozumna rychlost po cihle |
| if ((stav==cihla)&&(get_timer1()>(odocounter+5))) // Snizime rychlost po ujeti |
| { |
| rr=rrold; |
| stav=pocihle; |
| }; |
| */ |
| if ((stav==jizda)||(stav==cihla)||(stav==rozjezd)) //||(stav==pocihle)) // Jizda |
| { |
| set_pwm1_duty(r1); |
| set_pwm2_duty(r2); |
| } |
| else |
| { |
| set_pwm1_duty(0); // Zastaveni |
| set_pwm2_duty(0); |
| }; |
| |
| if((stav==rozjezd)&&(get_timer1()>10)) // musi ujet alespon 31cm |
| { |
| ext_int_edge(H_TO_L); // Nastav podminky preruseni od cihly |
| INT0IF=0; // Zruseni predesle udalosti od startera |
| enable_interrupts(INT_EXT); |
| enable_interrupts(GLOBAL); |
| stav=jizda; |
| }; |
| |
| if(stav==start) // Snimkuje, toci servem a ceka na start |
| { |
| set_adc_channel(MODRA); // Kroutitko na vystredeni predniho kolecka |
| Delay_ms(1); |
| stred=read_adc(); |
| if(!input(PROXIMITY)) |
| { |
| disp(0x80); |
| while(input(PROXIMITY)); // Cekej, dokud starter neda ruku pryc |
| set_timer1(0); // Vynuluj citac odometrie |
| set_pwm1_duty(255); // Rychly rozjezd !!! Zkontrolovat na oscyloskopu |
| set_pwm2_duty(255); |
| disp(0x01); |
| while(get_timer1()<=4) // Ujed alespon 12cm |
| { |
| set_adc_channel(LMAX); // Levy UV sensor |
| delay_us(40); |
| if(read_adc()<THR) {cas=CASMIN; break;}; // Prejeli jsme caru vlevo |
| set_adc_channel(RMAX); // Pravy UV sensor |
| delay_us(40); |
| if(read_adc()<THR) {cas=CASMAX; break;}; // Prejeli jsme caru vpravo |
| cas=CASAVR-CASMIN; // Cara je rovne |
| }; |
| stav=rozjezd; |
| }; |
| } |
| |
| pom=0x80; // Zobrazeni pozice cary na displayi |
| for(n=CASMAX/8; n<cas; n+=CASMAX/8) pom>>=1; |
| disp(pom); |
| |
| while(true) // Ve zbytku casu snimku cti krajni UV senzory |
| { |
| set_adc_channel(LMAX); // Levy UV sensor |
| for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |
| if(read_adc()<THR) cas=CASMIN; |
| set_adc_channel(RMAX); // Pravy UV sensor |
| for(n=0;n<20;n++) if(input(HREF)) goto next_snap; |
| if(read_adc()<THR) cas=CASMAX; |
| }; |
| } |
| } |
| |