| 0,0 → 1,332 |
|---|
| #include "main.h" |
| |
| #define TXo PIN_C3 // 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 400 // velikost mezery v care |
| #define ODEZVA 1 // za jak dlouho po opusteni cary se ma zacit zatacet |
| #define BRZDENI 100 // doba (v ms) ptrebna k zastaveni jednoho motoru |
| |
| //cidla |
| #define RSENSOR 1 // Senzory na caru |
| #define LSENSOR 0 |
| #define BUMPER PIN_C4 // sensor na cihlu |
| |
| #define DIAG_SERVO PIN_B0 // Propojka pro diagnosticky mod |
| #define DIAG_SENSORS PIN_B1 // Propojka pro diagnosticky mod |
| |
| #DEFINE SOUND_HI PIN_B3 |
| #DEFINE SOUND_LO PIN_B2 |
| |
| char AXstring[40]; // Buffer pro prenos telemetrie |
| |
| int tresholdL; // rozhodovaci uroven pro prave cidlo |
| int tresholdR; // rozhodovaci uroven pro prave cidlo |
| int movement; // smer minuleho pohybu |
| int line; // na ktere strane byla detekovana cara |
| unsigned int16 dira; // pocitadlo pro nalezeni preruseni cary |
| |
| // 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); |
| } |
| } |
| |
| // 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; |
| } |
| set_adc_channel(LSENSOR); |
| Delay_us(10); |
| while (tresholdL < read_adc()) // je tam cara? |
| { |
| 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 kalibrace() |
| { |
| unsigned int16 i; |
| int min; |
| int max; |
| int current; |
| int treshold; |
| |
| FL; BR; Delay_ms(130); |
| chyba1: |
| FR; BL; //kalibrace leveho cidla |
| set_adc_channel(LSENSOR); |
| Delay_us(20); |
| min=max=read_adc(); |
| for (i=1;i<=500;i++) |
| { |
| current=read_adc(); |
| if (max < current) max=current; |
| if (min > current) min=current; |
| Delay_us(500); |
| } |
| FL; BR; |
| for (i=1;i<=500;i++) |
| { |
| current=read_adc(); |
| if (max < current) max=current; |
| if (min > current) min=current; |
| Delay_us(500); |
| } |
| STOPL; STOPR; Delay_ms(200); |
| if((max-min)<50) {Beep(1000,300); GOTO chyba1;} |
| treshold=(max-min)>>1; |
| tresholdL=treshold+min; |
| |
| chyba2: |
| FR; BL; |
| set_adc_channel(RSENSOR); |
| Delay_us(20); |
| min=max=read_adc(); //naplneni min a max nejakou rozumnou hodnotou |
| for (i=1;i<=500 ;i++) |
| { |
| current=read_adc(); |
| if (max < current) max=current; //zmereni minima a maxima |
| if (min > current) min=current; |
| Delay_us(500); |
| } |
| FL; BR; |
| for (i=1;i<=500 ;i++) |
| { |
| current=read_adc(); |
| if (max < current) max=current; //zmereni minima a maxima |
| if (min > current) min=current; |
| Delay_us(500); |
| } |
| STOPL; STOPR; Delay_ms(200); |
| if((max-min)<50) {Beep(1000,300); GOTO chyba2;} |
| treshold=(max-min)>>1; |
| tresholdR=treshold+min; |
| |
| FR; BL; |
| movement=L; |
| set_adc_channel(LSENSOR); |
| Delay_us(20); |
| while (tresholdL < read_adc()) Delay_us(100); |
| FL; BR; Delay_ms(50); |
| STOPL; STOPR; Delay_ms(500); |
| Beep(780,200); |
| } |
| |
| void main() |
| { |
| unsigned int16 rovne; // pocita delku rovne cary |
| |
| STOPL; STOPR; |
| |
| setup_adc_ports(RA0_RA1_RA3_ANALOG); |
| setup_adc(ADC_CLOCK_DIV_2); |
| |
| port_b_pullups(false); |
| |
| diagnostika(); |
| |
| Beep(1000,200); //double beep |
| Delay_ms(50); |
| Beep(1000,200); |
| Delay_ms(1000); // 1s |
| |
| // kalibrace(); |
| tresholdl=tresholdr=80; |
| // FL; FR; |
| movement=S; |
| line=S; |
| dira=0; |
| rovne=0; |
| |
| while(true) |
| { |
| if(!input(BUMPER)) objizdka(); |
| line=0; |
| set_adc_channel(RSENSOR); // podivej se jestli neni cara pod pravym cidlem |
| Delay_us(10); |
| if(tresholdR > read_adc()) |
| { |
| dira=0; |
| line=R; |
| } |
| set_adc_channel(LSENSOR); // kdyz cara nebyla pod pravym cidlem, mozna bude pod levym |
| Delay_us(10); |
| if(tresholdL > read_adc()) |
| { |
| dira=0; |
| line=line | L; |
| } |
| |
| switch(line) |
| { |
| case S: |
| FR;FL; |
| movement=S; |
| continue; |
| case L: |
| STOPL; |
| FR;movement=L; |
| continue; |
| case R: |
| STOPR; |
| FL;movement=R; |
| continue; |
| default: |
| } |
| |
| if (dira==ODEZVA) // kdyz uz chvili jedeme po bile plose |
| { |
| //BR;BL;Delay_us(rovne >>= 5); |
| rovne=0; //kdyz sme museli zatocit, uz neni rovna cara |
| |
| switch (line) // musime zatocit |
| { |
| case L: |
| BL;Delay_ms(BRZDENI);STOPL; |
| FR; |
| movement=L; |
| break; |
| case R: |
| BR;Delay_ms(BRZDENI);STOPR; |
| FL; |
| movement=R; |
| break; |
| } |
| } |
| if (dira==MEZERA) // kdyz zkoncila cara |
| { |
| 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; |
| } |
| dira++; |
| } // while(true) |
| } |