| 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) |
| } |
| |