| 0,0 → 1,332 |
|---|
| //////////////////////////////////////////////////////////////////////////////// |
| // // |
| // Program pro robota Merkur // |
| // // |
| //////////////////////////////////////////////////////////////////////////////// |
| #include "merkur.h" |
| |
| //#define DEBUG |
| |
| #define TXo PIN_A3 // To the transmitter modulator |
| #include "AX25.c" // podprogram pro prenos telemetrie |
| |
| unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
| unsigned int8 line; // na ktere strane byla detekovana cara |
| unsigned int8 speed; // rychlost zataceni |
| unsigned int8 rovinka; // pocitadlo pro zjisteni rovneho useku |
| unsigned int8 last; // kde byla cara, kdyz byly minule cidla mimo |
| unsigned int8 movement; // obsahuje aktualni smer zataceni |
| unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
| |
| // Konstanty pro dynamiku pohybu |
| #define T_DIRA 120 // po jakem case zataceni se detekuje dira |
| #define INC_SPEED 1 // prirustek rychlosti v jednom kroku |
| #define FW_POMALU 230 // trochu mimo caru vnitrni pas |
| #define FW_ZATACKA 240 // rychlost vnejsiho kola pri zataceni |
| #define FW_STREDNE 240 // trochu mimo caru vnejsi pas |
| #define COUVANI 750 // couvnuti zpet na caru, po detekci diry |
| #define PRES_DIRU 300 // predpokladana velikost diry |
| #define MAX_ROVINKA (255-FW_STREDNE) // maximalni rychlost na rovince |
| #define TRESHOLD 6 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd |
| #define BUMPER_TRESHOLD 128 // rozhodovaci uroven cidla na cihlu |
| #define CIK_CAK 30000 // maximalni rozkmit pri hledani cary |
| #define T_CIHLA 50 // perioda detekce cihly |
| |
| //motory //Napred vypnout potom zapnout! |
| #define FR output_low(PIN_B5); output_high(PIN_B4) // Vpred |
| #define FL output_low(PIN_B7); output_high(PIN_B6) |
| #define BR output_low(PIN_B4); output_high(PIN_B5) // Vzad |
| #define BL output_low(PIN_B6); output_high(PIN_B7) |
| #define STOPR output_low(PIN_B4);output_low(PIN_B5) // Zastav |
| #define STOPL output_low(PIN_B6);output_low(PIN_B7) |
| |
| #define L 0b10 // left |
| #define R 0b01 // right |
| #define S 0b11 // straight |
| |
| //cidla |
| #define RSENSOR C2OUT // Senzory na caru |
| #define LSENSOR C1OUT |
| #define BUMPER PIN_A4 // Senzor na cihlu |
| |
| #define DIAG_SERVO PIN_B3 // Propojka pro diagnosticky mod |
| #define DIAG_SENSORS PIN_B2 // Propojka pro diagnosticky mod |
| |
| #DEFINE SOUND_HI PIN_A6 // komplementarni vystupy pro piezo pipak |
| #DEFINE SOUND_LO PIN_A7 |
| |
| char AXstring[40]; // Buffer pro prenos telemetrie |
| |
| // makro pro PWM |
| #define GO(motor, direction, power) if(get_timer0()<=power) \ |
| {direction##motor;} else {stop##motor;} |
| |
| #int_TIMER2 |
| void TIMER2_isr() |
| { |
| if (speed<255) speed+=INC_SPEED; |
| if (rovinka<MAX_ROVINKA) rovinka++; |
| if (dira<=T_DIRA) dira++; |
| } |
| // Primitivni Pipani |
| void beep(unsigned int16 period, unsigned int16 length) |
| { |
| unsigned int16 nn; |
| |
| for(nn=length; nn>0; nn--) |
| { |
| output_high(SOUND_HI);output_low(SOUND_LO); |
| delay_us(period); |
| output_high(SOUND_LO);output_low(SOUND_HI); |
| delay_us(period); |
| } |
| } |
| /******************************************************************************/ |
| void diagnostika() // vyzkousi funkci pohonu a cidel |
| { |
| unsigned int16 n; |
| |
| while (input(DIAG_SERVO)) // Propojka, ktera spousti diagnostiku |
| { |
| for (n=500; n<800; n+=100) |
| { |
| beep(n,n); //beep UP |
| }; |
| Delay_ms(1000); |
| //zastav vse |
| STOPL; STOPR; |
| //pravy pas |
| FR; Delay_ms(1000); STOPR; Delay_ms(1000); |
| BR; Delay_ms(1000); STOPR; Delay_ms(1000); |
| Beep(880,100); Delay_ms(1000); |
| //levy pas |
| FL; Delay_ms(1000); STOPL; Delay_ms(1000); |
| BL; Delay_ms(1000); STOPL; Delay_ms(1000); |
| Beep(880,100); Delay_ms(1000); |
| //oba pasy |
| FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000); |
| BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000); |
| }; |
| while (input(DIAG_SENSORS)) // spusteni diagnostiky cidel |
| { |
| if (RSENSOR) beep(900,500); |
| if (LSENSOR) beep(800,500); |
| if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(1000,500); |
| }; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void cikcak() // najde ztracenou caru |
| { |
| unsigned int16 n; |
| sem1: |
| n=CIK_CAK; |
| while (0==RSENSOR||LSENSOR) // zkontroluj caru |
| { |
| if (n==CIK_CAK) // zmen smer zataceni |
| { |
| n=0; |
| switch(movement) |
| { |
| case L: |
| FL;BR; |
| movement=R; |
| break; |
| case R: |
| FR;BL; |
| movement=L; |
| break; |
| case S: |
| FL;BR; |
| movement=R; |
| n=CIK_CAK/2; |
| break; |
| } |
| } |
| n++; |
| } |
| STOPL;STOPR; |
| line = RSENSOR; // cteni senzoru na caru |
| line |= LSENSOR << 1; |
| if (line==0) goto sem1; |
| // nasli jsme caru |
| line=S; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void objizdka() // objede cihlu |
| { |
| unsigned int16 n; |
| |
| BL;BR;Delay_ms(150); |
| STOPR;STOPL; |
| beep(900,1000); |
| // movement=S; |
| // cikcak(); |
| |
| BR; FL; Delay_ms(270); // otoc se 70° do prava |
| |
| FR; FL; Delay_ms(500); // popojed rovne |
| |
| BL; Delay_ms(30); // otoc se 90° do leva |
| STOPL; FR; Delay_ms(500); |
| |
| FR; FL; Delay_ms(100); // popojed rovne na slepo |
| for(n=40000;n>0;n--) // popojed rovne ale kontroluj caru |
| { |
| line = RSENSOR; // cteni senzoru na caru |
| line |= LSENSOR << 1; |
| if (line!=0) {Delay_ms(150); break;} |
| // Delay_ms(1); |
| } |
| |
| BR; FL; // otoc se 60° do prava |
| for(n=40000;n>0;n--) |
| { |
| line = RSENSOR; // cteni senzoru na caru |
| line |= LSENSOR << 1; |
| if (line!=0) break; |
| // Delay_ms(1); |
| } |
| STOPR; STOPL; |
| |
| movement=L; //R; |
| cikcak(); |
| dira=0; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void prejeddiru() // vyresi diru |
| { |
| unsigned int16 n; |
| unsigned int8 speed_dira; |
| |
| STOPL;STOPR; |
| speed_dira=speed; |
| beep(1000,500); |
| switch (movement) //vrat se zpet na caru |
| { |
| case L: |
| for (n=COUVANI;n>0;n--) {GO(R,B,speed_dira); Delay_ms(1);} |
| STOPL;STOPR; |
| break; |
| case R: |
| for (n=COUVANI;n>0;n--) {GO(L,B,speed_dira); Delay_ms(1);} |
| STOPL;STOPR; |
| break; |
| case S: |
| goto sem; |
| break; |
| } |
| beep(1000,500); |
| |
| FR;FL; //popojed rovne |
| for(n=PRES_DIRU;n>0;n--) |
| { |
| line = RSENSOR; // cteni senzoru na caru |
| line |= LSENSOR << 1; |
| if (line!=0) break; |
| Delay_ms(1); |
| } |
| sem: |
| STOPL; STOPR; |
| movement=S; |
| cikcak(); // najdi caru |
| dira=0; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| unsigned int16 n; // pro FOR |
| |
| STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi |
| |
| setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator |
| |
| port_b_pullups(TRUE); // pullups pro piano na diagnostiku |
| setup_spi(FALSE); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
| |
| setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci |
| // preruseni kazdych 10ms |
| setup_adc_ports(sAN2|VSS_VDD); // nastaveni A/D prevodniku pro naraznik |
| setup_adc(ADC_CLOCK_INTERNAL); |
| set_adc_channel(2); |
| setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik |
| setup_ccp1(CCP_COMPARE_RESET_TIMER); |
| CCP_1=(2^10)-1; // prevod kazdou 1ms |
| |
| setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary |
| setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd |
| |
| Beep(1000,200); //double beep |
| Delay_ms(50); |
| Beep(1000,200); |
| Delay_ms(1000); // 1s |
| |
| // povoleni rizeni rychlosti zataceni pres preruseni |
| enable_interrupts(INT_TIMER2); |
| enable_interrupts(GLOBAL); |
| |
| /*---------------------------------------------------------------------------*/ |
| sensors=S; |
| line=S; |
| last=S; |
| movement=S; |
| speed=FW_POMALU; |
| |
| diagnostika(); |
| //cikcak(); // toc se, abys nasel caru |
| Delay_ms(500); |
| Beep(1000,200); |
| Delay_ms(500); |
| |
| while(true) // hlavni smycka (jizda podle cary) |
| { |
| sensors = RSENSOR; // cteni senzoru na caru |
| sensors |= LSENSOR << 1; |
| |
| if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) && \ |
| (dira<=T_CIHLA)) objizdka(); |
| |
| switch (sensors) // zatacej podle toho, kde vidis caru |
| { |
| case S: // rovne |
| FL; FR; // pokud se jede dlouho rovne, tak pridej |
| dira=0; |
| movement=S; |
| continue; |
| case L: // trochu vlevo |
| GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka); |
| line=L; |
| dira=0; |
| movement=L; |
| continue; |
| case R: // trochu vpravo |
| GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka); |
| line=R; |
| dira=0; |
| movement=R; |
| continue; |
| default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal |
| } |
| rovinka=0; |
| if (dira>=T_DIRA) prejeddiru(); |
| if (last!=line) // pokud si prejel caru z jedne strany na druhou |
| // stranu, tak zabrzdi |
| { |
| last=line; |
| speed=FW_ZATACKA; |
| } |
| if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru |
| { |
| STOPL; |
| GO(R, F, speed); |
| movement=L; |
| } |
| else |
| { |
| STOPR; |
| GO(L, F, speed); |
| movement=R; |
| } |
| } // while(true) |
| } |
| |