| /roboti/istrobot/merkur/dokumentace/obrazky/PIC16F88/verze/1 tank.c |
|---|
| 0,0 → 1,313 |
| #include "tank.h" |
| #define TXo PIN_B1 // To the transmitter modulator |
| #include "AX25.c" // podprogram pro prenos telemetrie |
| //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) |
| #define STOPL output_low(PIN_B6);output_low(PIN_B7) |
| #define L 0b10 // left |
| #define R 0b01 // right |
| #define S 0b11 // straight |
| #define COUVANI 1600 // couvnuti po zjisteni diry |
| #define MEZERA 5400 // za jak dlouho bude ztracena cara |
| #define PRES_DIRU 1000 // velikost mezery v care |
| #define BRZDENI 5000 // doba ptrebna k zastaveni jednoho motoru |
| //cidla |
| #define RSENSOR C2OUT // Senzory na caru |
| #define LSENSOR C1OUT |
| #define BUMPER PIN_A4 // sensor na cihlu |
| #define DIAG_SERVO PIN_B2 // Propojka pro diagnosticky mod |
| #define DIAG_SENSORS PIN_B3 // Propojka pro diagnosticky mod |
| #DEFINE SOUND_HI PIN_A7 |
| #DEFINE SOUND_LO PIN_A6 |
| char AXstring[40]; // Buffer pro prenos telemetrie |
| #define GO(motor, direction, power) if(get_timer0()<=power) \ |
| {direction##motor;} \ |
| else \ |
| {stop##motor;} |
| int movement; // smer minuleho pohybu |
| int line; // na ktere strane byla detekovana cara |
| unsigned int16 dira; // pocitadlo pro nalezeni preruseni cary |
| int speed,speedL,speedR; |
| // 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); |
| } |
| } |
| #int_TIMER2 |
| void TIMER2_isr() |
| { |
| switch(line) // upravime smer |
| { |
| case S: //obe cidla na care |
| if(speedL<200)speedL++; |
| if(speedR<200)speedR++; |
| break; // vrat se zpet na cteni cidel |
| case L: // cara je pod levym cidlem, trochu zatocime |
| if (speedL>100)speedL -- ; |
| if (speedR<200)speedR ++ ; |
| break; |
| case R: // cara pod pravym cidlem |
| if (speedR>100)speedR -- ; |
| if (speedL<200)speedL ++ ; |
| break; |
| default: |
| } |
| } |
| // Diagnostika pohonu, hejbne vsema motorama ve vsech smerech |
| void diagnostika() |
| { |
| 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)) |
| { |
| int ls, rs; |
| while(!input(BUMPER)){beep(1100,100); Delay_ms(50);} |
| set_adc_channel(RSENSOR); |
| Delay_us(20); |
| rs=read_adc(); |
| set_adc_channel(LSENSOR); |
| Delay_us(20); |
| ls=read_adc(); |
| sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String. |
| SendPacket(&AXstring[0]); |
| delay_ms(1000); |
| }; |
| } |
| void cikcak() |
| { |
| int n; |
| switch(movement) // podivej se na jednu stranu |
| { |
| case L: |
| FL;BR; |
| movement=R; |
| break; |
| case R: |
| FR;BL; |
| movement=L; |
| break; |
| case S: |
| FR;BL; |
| movement=L; |
| break; |
| } |
| while (0==(RSENSOR|LSENSOR)) |
| { |
| if (n==50) // asi bude na druhe strane |
| { |
| STOPR;STOPL; |
| n=0; |
| switch(movement) |
| { |
| case L: |
| FL;BR; |
| movement=R; |
| break; |
| case R: |
| FR;BL; |
| movement=L; |
| break; |
| } |
| } |
| Delay_ms(5); |
| n++; |
| } |
| STOPL;STOPR; // nasli jsme caru |
| line=S; |
| } |
| void objizdka() |
| { |
| BL;BR;Delay_ms(300); |
| STOPR;STOPL; |
| beep(1000,1000); |
| Delay_ms(500); |
| beep(1000,1000); |
| Delay_ms(1000); |
| } |
| void pozordira() |
| { |
| beep(800,500); |
| Delay_ms(50); |
| beep(800,500); |
| switch (movement) //vrat se zpet na caru |
| { |
| case L: |
| STOPL;STOPR; |
| BR;Delay_ms(COUVANI);STOPR; |
| break; |
| case R: |
| STOPL;STOPR; |
| BL;Delay_ms(COUVANI);STOPL; |
| break; |
| case S: |
| BL; BR; Delay_ms(COUVANI); |
| STOPL; STOPR; |
| break; |
| } |
| FR;FL; Delay_ms(PRES_DIRU); // popojedem dopredu mozna tam bude cara |
| STOPL; STOPR; movement=S; |
| cikcak(); // najdi caru |
| dira=0; |
| } |
| void main() |
| { |
| unsigned int16 rovinka; |
| int last; |
| STOPL; STOPR; |
| setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator |
| port_b_pullups(true); |
| setup_adc_ports(NO_ANALOGS|VSS_VDD); |
| setup_adc(ADC_OFF); |
| setup_spi(FALSE); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM |
| setup_timer_1(T1_DISABLED); // Casovac pro regulaci |
| setup_timer_2(T2_DIV_BY_16,50,16); |
| setup_ccp1(CCP_OFF); |
| setup_comparator(A0_VR_A1_VR); |
| setup_vref(VREF_HIGH|15); |
| enable_interrupts(INT_TIMER2); |
| enable_interrupts(GLOBAL); |
| diagnostika(); |
| Beep(1000,200); //double beep |
| Delay_ms(50); |
| Beep(1000,200); |
| Delay_ms(1000); // 1s |
| // FL; FR; |
| movement=S; |
| line=S; |
| dira=0; |
| last=0; |
| rovinka=0; |
| speed=speedL=speedR=200; |
| while(true) |
| { |
| line = RSENSOR; // precteni cidel |
| line |= LSENSOR << 1; // sestav informaci o care |
| if(line!=0) |
| { |
| GO(L, F, speedL); GO(R,F, speedR); |
| } |
| else{STOPR; STOPL;} |
| //sem: |
| /* switch(line) // upravime smer |
| { |
| case S: //obe cidla na care |
| // if (speedL<speedR) speedL=speedR; |
| // else speedR=speedL; |
| GO(L,F,speedL); GO(R,F,speedR) // jedeme rovne |
| // if(rovinka<BRZDENI) rovinka++; //cara je rovne |
| // dira=0; // videli jsme caru, proto neni dira |
| continue; // vrat se zpet na cteni cidel |
| case L: // cara je pod levym cidlem, trochu zatocime |
| GO(L, F, speedL); GO(R,F, speedR); |
| // if(rovinka<BRZDENI) rovinka++; //cara je celkem rovne |
| // dira=0; |
| continue; |
| case R: // cara pod pravym cidlem |
| GO(R, F, speedR); GO(L, F, speedL); |
| // if(rovinka<BRZDENI) rovinka++; |
| // dira=0; |
| continue; |
| default: // cara neni pod zadnym cidlem |
| }*/ |
| /*switch (last) // zatacka |
| { |
| case L: // do leva |
| BL;STOPR; //zabrzdeni leveho motoru |
| for(;rovinka>0;rovinka--) //chvili pockej |
| { |
| line = RSENSOR; // precteni cidel |
| line |= LSENSOR << 1; // sestav informaci o care |
| if(line!=0) goto sem; //kdyz najdes caru, zastav |
| } |
| STOPL; FR; // pokracuj v zataceni |
| while(line==0) |
| { |
| line = RSENSOR; // precteni cidel |
| line |= LSENSOR << 1; // sestav informaci o care |
| } |
| movement=L; |
| rovinka=0; //zataceli jsme, uz neni rovna cara |
| break; |
| case R: |
| BR; STOPL; // zabrzdeni praveho motoru |
| for(;rovinka>0;rovinka--) |
| { |
| line = RSENSOR; // precteni cidel |
| line |= LSENSOR << 1; // sestav informaci o care |
| if(line!=0) goto sem; |
| } |
| STOPR; FL; |
| while(line==0) |
| { |
| line = RSENSOR; // precteni cidel |
| line |= LSENSOR << 1; // sestav informaci o care |
| } |
| movement=R; |
| rovinka=0; //zataceli jsme, uz neni rovna cara |
| break; |
| }*/ |
| } // while(true) |
| } |
| /roboti/istrobot/merkur/dokumentace/obrazky/PIC16F88/verze/2 tank.c |
|---|
| 0,0 → 1,331 |
| #include "tank.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 100 // po jakem case zataceni se detekuje dira |
| #define INC_SPEED 1 // prirustek rychlosti v jednom kroku |
| #define FW_POMALU 100 // trochu mimo caru vnitrni pas |
| #define FW_ZATACKA 200 // rychlost vnejsiho kola pri zataceni |
| #define FW_STREDNE 110 // trochu mimo caru vnejsi pas |
| #define COUVANI 600 // couvnuti zpet na caru, po detekci diry |
| #define PRES_DIRU 300 |
| #define MAX_ROVINKA (255-FW_STREDNE) |
| #define TRESHOLD 6 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd |
| //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() |
| { |
| 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)) |
| { |
| int ls, rs; |
| while(!input(BUMPER)){beep(1100,100); Delay_ms(50);} |
| set_adc_channel(RSENSOR); |
| Delay_us(20); |
| rs=read_adc(); |
| set_adc_channel(LSENSOR); |
| Delay_us(20); |
| ls=read_adc(); |
| sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String. |
| SendPacket(&AXstring[0]); |
| delay_ms(1000); |
| }; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void cikcak() |
| { |
| int n; |
| switch(movement) // podivej se na jednu stranu |
| { |
| case L: |
| FL;BR; |
| movement=R; |
| break; |
| case R: |
| FR;BL; |
| movement=L; |
| break; |
| case S: |
| FR;BL; |
| movement=L; |
| break; |
| } |
| while (0==(RSENSOR|LSENSOR)) |
| { |
| if (n==50) //cara asi bude na druhe strane |
| { |
| STOPR;STOPL; |
| n=0; |
| switch(movement) |
| { |
| case L: |
| FL;BR; |
| movement=R; |
| break; |
| case R: |
| FR;BL; |
| movement=L; |
| break; |
| } |
| } |
| Delay_ms(5); |
| n++; |
| } |
| STOPL;STOPR; // nasli jsme caru |
| line=S; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void objizdka() |
| { |
| BL;BR;Delay_ms(300); |
| STOPR;STOPL; |
| beep(1000,1000); |
| Delay_ms(500); |
| beep(1000,1000); |
| Delay_ms(1000); |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void prejeddiru() |
| { |
| STOPL;STOPR; |
| beep(800,500); |
| Delay_ms(50); |
| beep(800,500); |
| switch (movement) //vrat se zpet na caru |
| { |
| case L: |
| BR;Delay_ms(COUVANI);STOPR; |
| STOPL;STOPR; |
| break; |
| case R: |
| BL;Delay_ms(COUVANI);STOPL; |
| STOPL;STOPR; |
| break; |
| case S: |
| BL; BR; Delay_ms(COUVANI); |
| STOPL; STOPR; |
| break; |
| } |
| FR;FL; Delay_ms(PRES_DIRU); // popojedem dopredu mozna tam bude cara |
| STOPL; STOPR; |
| 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_16,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; |
| switch (sensors) // zatacej podle toho, kde vidis caru |
| { |
| case S: // rovne |
| GO(L, F, speed); GO(R, F, speed); |
| // FL; FR; // pokud se jede dlouho rovne, tak pridej |
| dira=0; |
| continue; |
| case L: // trochu vlevo |
| GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka); |
| line=L; |
| dira=0; |
| continue; |
| case R: // trochu vpravo |
| GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka); |
| line=R; |
| dira=0; |
| 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; |
| } |
| else speed=255; |
| /* if (dira==0) |
| { |
| if (L==line) // kdyz jsou obe cidla mimo caru, zabrzdi vnitrni kolo |
| { |
| BL; |
| for(n=4000;n>0;n--) // Delay |
| { |
| line = RSENSOR; // precteni cidel |
| line |= LSENSOR << 1; // sestav informaci o care |
| if(line!=0) break; |
| } |
| STOPL; |
| } |
| else |
| { |
| BR; |
| for(n=4000;n>0;n--) // Delay |
| { |
| line = RSENSOR; // precteni cidel |
| line |= LSENSOR << 1; // sestav informaci o care |
| if(line!=0) break; |
| } |
| STOPR; |
| } |
| }*/ |
| 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) |
| } |
| /roboti/istrobot/merkur/dokumentace/obrazky/PIC16F88/verze/3 tank.c |
|---|
| 0,0 → 1,350 |
| #include "tank.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 |
| unsigned int8 cihla; // urcuje za jak dlouho muze byt znova detekovana cihla |
| // Konstanty pro dynamiku pohybu |
| #define T_DIRA 85 // 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 200 // rychlost vnejsiho kola pri zataceni |
| #define FW_STREDNE 240 // trochu mimo caru vnejsi pas |
| #define COUVANI 600 // couvnuti zpet na caru, po detekci diry |
| #define PRES_DIRU 150 |
| #define MAX_ROVINKA (255-FW_STREDNE) |
| #define TRESHOLD 10 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd |
| #define BUMPER_TRESHOLD 128 |
| #define CIK_CAK 20000 |
| #define T_CIHLA 100 // 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++; |
| if (cihla>0) cihla--; |
| } |
| // 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() |
| { |
| 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)) |
| { |
| int ls, rs; |
| while(!input(BUMPER)){beep(1100,100); Delay_ms(50);} |
| set_adc_channel(RSENSOR); |
| Delay_us(20); |
| rs=read_adc(); |
| set_adc_channel(LSENSOR); |
| Delay_us(20); |
| ls=read_adc(); |
| sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String. |
| SendPacket(&AXstring[0]); |
| delay_ms(1000); |
| }; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void cikcak() |
| { |
| unsigned int16 n; |
| line = RSENSOR; // cteni senzoru na caru |
| line |= LSENSOR << 1; |
| if (line!=0) return; |
| n=0; |
| switch(movement) // podivej se na druhou stranu nez se jelo |
| { |
| 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; |
| } |
| 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; |
| } |
| } |
| n++; |
| } |
| switch(movement) |
| { |
| case L: |
| FL;BR; |
| movement=R; |
| break; |
| case R: |
| FR;BL; |
| movement=L; |
| break; |
| } |
| Delay_ms(50); |
| STOPL;STOPR; // nasli jsme caru |
| line=S; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void objizdka() // objede cihlu |
| { |
| unsigned int16 n; |
| BL;BR;Delay_ms(200); |
| STOPR;STOPL; |
| beep(900,1000); |
| movement=S; |
| cikcak(); |
| BR; FL; Delay_ms(215); // otoc se 70° do prava |
| FR; FL; Delay_ms(600); // popojed rovne |
| BL; Delay_ms(50); // otoc se 90° do leva |
| STOPL; FR; Delay_ms(550); |
| FR; FL; Delay_ms(100); // popojed rovne na slepo |
| for(n=600;n>0;n--) // popojed rovne ale kontroluj caru |
| { |
| line = RSENSOR; // cteni senzoru na caru |
| line |= LSENSOR << 1; |
| if (line!=0) {Delay_ms(50); break;} |
| Delay_ms(1); |
| } |
| BR; // otoc se 60° do prava |
| for(n=600;n>0;n--) |
| { |
| line = RSENSOR; // cteni senzoru na caru |
| line |= LSENSOR << 1; |
| if (line!=0) break; |
| Delay_ms(1); |
| } |
| STOPR; STOPL; |
| movement=R; |
| cikcak(); |
| cihla=T_DIRA; |
| dira=0; |
| } |
| /////////////////////////////////////////////////////////////////////////////// |
| void prejeddiru() // vyresi diru |
| { |
| unsigned int16 n; |
| STOPL;STOPR; |
| beep(800,500); |
| switch (movement) //vrat se zpet na caru |
| { |
| case L: |
| for (n=COUVANI;n>0;n--) {GO(R,B,speed); Delay_ms(1);} |
| STOPL;STOPR; |
| break; |
| case R: |
| for (n=COUVANI;n>0;n--) {GO(L,B,speed); Delay_ms(1);} |
| STOPL;STOPR; |
| break; |
| case S: |
| goto sem; |
| break; |
| } |
| beep(800,500); |
| FR;FL; Delay_ms(PRES_DIRU); // popojedem dopredu mozna tam bude cara |
| sem: |
| STOPL; STOPR; |
| movement=S; |
| cikcak(); // najdi caru |
| cihla=T_DIRA; |
| 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) && (cihla==0)) 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) |
| } |
| /roboti/istrobot/merkur/dokumentace/obrazky/PIC16F88/verze/vssver.scc |
|---|
| Cannot display: file marked as a binary type. |
| svn:mime-type = application/octet-stream |
| Property changes: |
| Added: svn:mime-type |
| +application/octet-stream |
| \ No newline at end of property |