| /Designs/ROBOTS/3Orbis/SW/cidla/cidla.BAK |
|---|
| 0,0 → 1,158 |
| #include ".\cidla.h" |
| //#include <stdlib.h> |
| #use rs232(baud=9600,parity=N,xmit=PIN_B3,bits=8,restart_wdt) |
| #define IRRX PIN_B0 |
| #define TRESHOLD_MAX 70 // rozhodovaci uroven pro cidla cerna/bila |
| #define TRESHOLD_MIN 50 |
| #define CIHLA 10 // doba, po kterou musi byt detekovana cihla |
| unsigned int8 radius; // co cidla vidi |
| unsigned int8 last_radius; // rozsah |
| unsigned int8 last_cidla; // co cidla videla minule |
| unsigned int8 shure; // citac doby, po kterou musi byt detekovana cihla |
| //tuning |
| /*#define PULZACE 3 // urcuje rychlost pulzovani pomoci PWM |
| //Vystup PWM je na PIN_B3 |
| //////////////////////////////////////////////////////////////////////////////// |
| void pulzovani() // postupne rozsvecuje a zhasina podsvetleni |
| { |
| unsigned int8 i,n; |
| for(n=0;n<=3;n++) |
| { |
| for(i=0;i<255;i++) {set_pwm1_duty(i); Delay_ms(PULZACE);} // rozsvecovani |
| for(i=255;i>0;i--) {set_pwm1_duty(i); Delay_ms(PULZACE);} // zhasinani |
| } |
| } |
| */ |
| //////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| int8 cidla; |
| unsigned int8 a; |
| unsigned int8 n; |
| setup_adc_ports(sAN0|sAN1|sAN2|sAN3|sAN4|sAN5|sAN6|VSS_VDD); |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| setup_timer_1(T1_DISABLED); |
| setup_timer_2(T2_DISABLED,0,1); |
| setup_comparator(NC_NC_NC_NC); |
| setup_vref(FALSE); |
| Delay_ms(500); |
| setup_spi(SPI_SLAVE|SPI_H_TO_L|SPI_SS_DISABLED); |
| // diagnostika |
| printf("\n\r"); |
| Delay_ms(100); |
| printf("***\n\r"); |
| Delay_ms(100); |
| for (n=0; n<=6; n++) |
| { |
| set_adc_channel(n); |
| Delay_ms(100); |
| a=read_adc(); |
| printf("sensor %u - %u\n\r",n,a); |
| } |
| shure=0; |
| while(true) |
| { |
| set_adc_channel(0); |
| cidla=0; |
| Delay_us(10); |
| a=read_adc(); |
| set_adc_channel(1); |
| if(a<TRESHOLD_MAX) //hystereze cidel |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000001); |
| } |
| else cidla |= 0b00000001; |
| } |
| a=read_adc(); |
| set_adc_channel(2); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000010); |
| } |
| else cidla |= 0b00000010; |
| } |
| a=read_adc(); |
| set_adc_channel(3); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00000100); |
| } |
| else cidla |= 0b00000100; |
| } |
| a=read_adc(); |
| set_adc_channel(4); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00001000); |
| } |
| else cidla |= 0b00001000; |
| } |
| a=read_adc(); |
| set_adc_channel(5); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00010000); |
| } |
| else cidla |= 0b00010000; |
| } |
| a=read_adc(); |
| set_adc_channel(6); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |= (last_cidla & 0b00100000); |
| } |
| else cidla |= 0b00100000; |
| } |
| a=read_adc(); |
| if(a<TRESHOLD_MAX) |
| { |
| if(a>TRESHOLD_MIN) |
| { |
| cidla |=(last_cidla & 0b01000000); |
| } |
| else cidla |= 0b01000000; |
| } |
| last_cidla=cidla; |
| if (!input(IRRX)) {if (shure<255) shure++;} else {shure=0;}; |
| if (shure>CIHLA) cidla|=0b10000000; |
| cidla=~cidla; |
| spi_write(cidla); |
| } |
| } |
| /Designs/ROBOTS/3Orbis/SW/main.BAK |
|---|
| 0,0 → 1,288 |
| #include ".\main.h" |
| #define KOLMO1 225 // predni kolecko sroubem dopredu |
| #define KOLMO2 30 // predni kolecko je hlavou sroubu dozadu |
| #define STRED 128 // sredni poloha zataceciho kolecka |
| #define BEAR1 10//10 // 3 stupne zataceni |
| #define BEAR2 25//25 |
| #define BEAR3 45//45 |
| #define R 100 // Rozumna rychlost |
| #define R17 200 // X nasobek rozumne rychlosti |
| #define PRED_CIHLOU 100 // rychlost pri dalkove detekci cihly |
| //#define L1 1 // cara vlevo |
| #define L2 2 // cara vlevo |
| #define L3 3 // cara vlevo |
| #define S 0 // cara mezi sensory |
| //#define R1 -1 // cara vpravo |
| #define R2 -2 // cara vpravo |
| #define R3 -3 // cara vpravo |
| // servo |
| #define SERVO PIN_B5 |
| // kroutitka |
| #define CERVENA 4 // AN4 |
| //#define CERNA 5 // AN5 |
| //#define ZELENA 6 // AN6 |
| #define MODRA 2 // AN2 |
| // IR |
| #define IRTX PIN_B2 |
| #define CIHLA PIN_A3 |
| //motory |
| #define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred |
| #define FL output_low(PIN_A1); output_high(PIN_A0) |
| #define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad |
| #define BL output_low(PIN_A0); output_high(PIN_A1) |
| #define STOPR output_low(PIN_A6);output_low(PIN_A7) |
| #define STOPL output_low(PIN_A0);output_low(PIN_A1) |
| //HID |
| #define LED1 PIN_B1 //oranzova |
| #define LED2 PIN_B2 //zluta |
| #define STROBE PIN_B0 |
| //#define SW1 PIN_A2 // Motory On/off |
| unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru |
| signed int8 line = S; // na ktere strane byla detekovana cara |
| //unsigned int8 dira; // pocita dobu po kterou je ztracena cara |
| unsigned int8 uhel; // urcuje aktualni uhel zataceni |
| unsigned int8 speed; // maximalni povolena rychlost |
| unsigned int8 turn; // rychlost toceni |
| unsigned int8 rovinka; // pocitadlo na zjisteni rovinky |
| signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem |
| signed int16 Rmotor; // a pravem motoru |
| // makro pro PWM pro motory |
| #define GO(motor, direction, power) if(get_timer0()<=power) \ |
| {direction##motor;} else {stop##motor;} |
| //////////////////////////////////////////////////////////////////////////////// |
| #int_TIMER2 |
| TIMER2_isr() // ovladani serva |
| { |
| unsigned int8 n; |
| output_high(SERVO); |
| delay_us(1000); |
| for(n=uhel; n>0; n--) Delay_us(2); |
| output_low(SERVO); |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| int8 IRcheck() // potvrdi detekci cihly |
| { |
| output_high(IRTX); // vypne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| if(bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal |
| { |
| output_low(IRTX); // zapne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| if(!bit_test(sensors,7)) // otestuje, jestli je detekovana cihla |
| { |
| output_high(IRTX); // vypne vysilac IR |
| delay_ms(10); |
| output_low(STROBE); |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); |
| output_low(IRTX); // zapne vysilac IR |
| if(bit_test(sensors,7)) return 1; // vrat 1, kdyz je stale cihla |
| } |
| }; |
| output_low(IRTX); // zapne vysilac IR |
| return 0; // vrat 0, kdyz je detekovano ruseni |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| #include ".\objizdka_centrovani.c" |
| //////////////////////////////////////////////////////////////////////////////// |
| void main() |
| { |
| unsigned int8 n; |
| unsigned int8 i,j; |
| unsigned int8 last_sensors; |
| unsigned int8 RozumnaRychlost; |
| setup_adc_ports(sAN5|sAN2|sAN4|sAN6|VSS_VDD); // AD pro kroutitka |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_16); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
| setup_timer_1(T1_DISABLED|T1_DIV_BY_8); |
| setup_timer_2(T2_DIV_BY_16,140,16); |
| setup_oscillator(OSC_8MHZ|OSC_INTRC); |
| STOPR; STOPL; // zastav motory |
| Lmotor=0;Rmotor=0; |
| uhel = STRED; // nastav zadni kolecko na stred |
| rovinka = 0; |
| enable_interrupts(INT_TIMER2); |
| enable_interrupts(GLOBAL); |
| output_low(IRTX); // zapni IR vysilac |
| delay_ms(2000); // musime pockat na diagnostiku slave CPU |
| //nastaveni rychlosti |
| set_adc_channel(CERVENA); |
| Delay_ms(1); |
| RozumnaRychlost=R+(read_adc()>>2); // rychlost rovne +63; kroutitko dava 0-63 |
| set_adc_channel(MODRA); |
| Delay_ms(1); |
| turn=speed-32+(read_adc()>>2); // rychlost toceni +-32; kroutitko dava 0-63 |
| speed=R17; |
| while(true) |
| { |
| GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory PWM podle promenych Lmotor a Rmotor |
| delay_us(2000); // cekani na SLAVE, nez pripravi data od cidel |
| last_sensors=sensors; |
| output_low(STROBE); // vypni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; // neguj prijata data |
| output_high(STROBE); // zobraz data na posuvnem registru |
| i=0; // havarijni kod |
| for (n=0; n<=6; n++) |
| { |
| if(bit_test(sensors,n)) i++; |
| } |
| if (i>4) // zastavi, kdyz je cerno pod vice nez tremi cidly |
| { |
| BL; BR; |
| delay_ms(300); |
| STOPR; STOPL; |
| While(true); |
| }; |
| /* |
| if (!input(CIHLA)) // dalkova detekce cihly |
| { |
| speed=PRED_CIHLOU; |
| } |
| else |
| { |
| speed=RozumnaRychlost; |
| } |
| */ |
| if (bit_test(sensors,7)) // detekce cihly |
| { |
| BR;BL; |
| Delay_ms(400); |
| STOPR;STOPL; |
| // if (1==IRcheck()) // kontrola, jestli nebylo rusene cidlo |
| { |
| Delay_ms(100); |
| cikcak(); |
| delay_ms(100); |
| objizdka(); // objede cihlu |
| } |
| } |
| if (speed > RozumnaRychlost) speed--; // postupne zpomaleni na Roz. Rychl. |
| if(bit_test(sensors,3)) //...|...// |
| { |
| uhel=STRED; |
| Lmotor=speed; |
| Rmotor=speed; |
| line=S; |
| if (rovinka < 255) rovinka++; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| uhel=STRED - BEAR3; |
| Lmotor=0; |
| Rmotor=turn; |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| uhel=STRED + BEAR3; |
| Rmotor=0; |
| Lmotor=turn; |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| uhel=STRED - BEAR2; |
| Lmotor=speed-70; |
| Rmotor=speed; |
| line=L2; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| uhel=STRED + BEAR2; |
| Rmotor=speed-70; |
| Lmotor=speed; |
| line=R2; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| uhel=STRED - BEAR1; |
| Lmotor=speed-20; |
| Rmotor=speed; |
| line=L2; |
| if (rovinka<255) rovinka++; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| uhel=STRED + BEAR1; |
| Rmotor=speed-20; |
| Lmotor=speed; |
| line=L2; |
| if (rovinka<255) rovinka++; |
| continue; |
| } |
| if ((L3==line) || (R3==line)) // Brzdeni pri vyjeti z trate |
| { |
| if (rovinka>50) |
| { |
| BL; BR; |
| Delay_ms(100); |
| if (rovinka > 250 || speed > 170) delay_ms(50); |
| }; |
| rovinka=0; |
| speed=R17; |
| }; |
| } |
| } |
| /Designs/ROBOTS/3Orbis/SW/objizdka_centrovani.BAK |
|---|
| 0,0 → 1,131 |
| #define DOLEVA 0 |
| #define DOPRAVA 1 |
| void cikcak() |
| { |
| unsigned int8 i=0; |
| uhel=KOLMO1; // aby se dalo tocit na miste |
| Delay_ms(100); |
| if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
| if (line==S) line=L3; |
| if (line==R2) line=R3; |
| Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| while(true) |
| { |
| Delay_ms(3); |
| if (line==L3) |
| { |
| GO(L,B,160);GO(R,F,160); |
| }; |
| if (line==R3) |
| { |
| GO(R,B,160);GO(L,F,160); |
| }; |
| if (line==S) {STOPL;STOPR; i++;} else i=0; |
| if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| if(bit_test(sensors,3)) //...|...// |
| { |
| line=S; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| line=R3; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| line=L3; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| line=R3; |
| continue; |
| } |
| } |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| void objizdka() |
| { |
| BL;FR; |
| Delay_ms(300); |
| FL;BR; |
| Delay_ms(100); |
| STOPL;STOPR; |
| uhel=STRED; |
| FL;FR; |
| Delay_ms(360); // rovne |
| uhel=STRED+55; |
| STOPR;FL; |
| Delay_ms(190); // doprava |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(300); // rovne |
| uhel=STRED+55; |
| FL;STOPR; |
| Delay_ms(190); // doprava |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(60); // rovne |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| BL;BR; |
| Delay_ms(300); |
| uhel=STRED-55; |
| FR;STOPL; // doleva |
| delay_ms(250); |
| line=L3; |
| cikcak(); |
| } |
| /Designs/ROBOTS/3Orbis/SW/objizdka_centrovani_R.BAK |
|---|
| 0,0 → 1,139 |
| #define DOLEVA 0 |
| #define DOPRAVA 1 |
| void cikcak() |
| { |
| unsigned int8 i=0; |
| uhel=KOLMO1; // aby se dalo tocit na miste |
| Delay_ms(100); |
| if (line==L2) line=L3; // poznamenej, kde byla cara pred brzdenim |
| if (line==S) line=L3; |
| if (line==R2) line=R3; |
| Delay_ms(3); // prodleva na cteni senzoru pred prenosem |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| while(true) |
| { |
| Delay_ms(3); |
| if (line==L3) |
| { |
| GO(L,B,160);GO(R,F,160); |
| }; |
| if (line==R3) |
| { |
| GO(R,B,160);GO(L,F,160); |
| }; |
| if (line==S) {STOPL;STOPR; i++;} else i=0; |
| if (i>=100) break; // pokud je dostatecne dlouho cara vprostred, vypadni |
| output_low(STROBE); // zapni zobrazovani na posuvnem registru |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| output_high(STROBE); // vypni zobrazovani na posuvnem registru |
| if(bit_test(sensors,3)) //...|...// |
| { |
| line=S; |
| continue; |
| } |
| if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,6)) //......|// |
| { |
| line=R3; |
| continue; |
| } |
| if(bit_test(sensors,1)) //.|.....// |
| { |
| line=L3; |
| continue; |
| } |
| if(bit_test(sensors,5)) //.....|.// |
| { |
| line=R3; |
| continue; |
| } |
| if (bit_test(sensors,2)) //..|....// |
| { |
| line=L3; |
| continue; |
| } |
| if (bit_test(sensors,4)) //....|..// |
| { |
| line=R3; |
| continue; |
| } |
| } |
| } |
| //////////////////////////////////////////////////////////////////////////////// |
| void objizdka() |
| { |
| FL;BR; // doprava 90 |
| Delay_ms(300); |
| BL;FR; // brzdi z toceni |
| Delay_ms(100); |
| STOPL;STOPR; |
| uhel=STRED; |
| FL;FR; |
| Delay_ms(370); // rovne |
| uhel=STRED-55; |
| STOPL;FR; |
| Delay_ms(190); // doleva |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(300); // rovne |
| uhel=STRED-55; |
| FR;STOPL; |
| Delay_ms(190); // doleva |
| uhel=STRED; |
| FR;FL; |
| Delay_ms(100); // rovne |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| BL;BR; // zabrzdi |
| Delay_ms(250); |
| uhel=KOLMO1; |
| FL;BR; // doprava |
| While((sensors & 0b11111110)!=0) //dokud neni cara |
| { |
| sensors = spi_read(0); // cteni senzoru |
| sensors=~sensors; |
| Delay_ms(3); // cekani na SLAVE nez pripravi data od cidel |
| } |
| STOPL;STOPR; // zabrzdi |
| Delay_ms(300); |
| // delay_ms(250); |
| line=R3; |
| cikcak(); |
| } |