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