| 0,0 → 1,271 |
|---|
| // Program pro MiniSumo na R-Day 2006 |
| #include "cholerik.h" |
| |
| // Konstanty |
| #define TRESHOLD 0x254 // rozhodovaci uroven pro okraj areny |
| //#define DEBUG1 1 // Diagnostika pohonu |
| |
| //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) |
| |
| //cidla |
| #define L 2 // Senzory na okraj areny |
| #define R 3 |
| #define SIDE_R !input(PIN_A7) // Sensory na soupere |
| #define SIDE_L !input(PIN_A4) |
| #define FRONT !input(PIN_A6) |
| #define BACK !input(PIN_B3) |
| #define GRAVITY !input(PIN_A1) |
| |
| #DEFINE SOUND_HI PIN_B1 // komplementarni vystupy pro piezo pipak |
| #DEFINE SOUND_LO PIN_B2 |
| |
| // makro pro PWM |
| #define GO(motor, direction, power) if(get_timer0()<=power) \ |
| {direction##motor;} else {stop##motor;} |
| |
| unsigned int8 majak=0; |
| unsigned int8 sl=0; |
| unsigned int8 sr=0; |
| unsigned int8 b=0; |
| unsigned int8 f=0; |
| unsigned int8 g=0; |
| int1 arena_l; |
| int1 arena_r; |
| int1 diag=FALSE; |
| |
| #int_TIMER0 |
| TIMER0_isr() |
| { |
| int1 stav; |
| |
| stav = ((majak & 0b1) == 0b1); |
| if (((SIDE_R && stav) || (!SIDE_R && !stav))) {if (sr<255) sr++;} else {sr=0;}; |
| if (((SIDE_L && stav) || (!SIDE_L && !stav))) {if (sl<255) sl++;} else {sl=0;}; |
| if (((BACK && stav) || (!BACK && !stav))) {if (b<255) b++;} else {b=0;}; |
| if (((FRONT && stav) || (!FRONT && !stav))) {if (f<255) f++;} else {f=0;}; |
| majak++; |
| stav = ((majak & 0b1) == 0b1); |
| if (stav) |
| { |
| if (read_adc(ADC_READ_ONLY) > 250) arena_l=TRUE; else arena_l=FALSE; |
| set_adc_channel(R); // prepnuti kanalu ADC, je treba min 10us na ustaleni |
| delay_us(10); |
| read_adc(ADC_START_ONLY); |
| set_pwm1_duty(27); // 1:1 |
| } |
| else |
| { |
| if (read_adc(ADC_READ_ONLY) > 250) arena_r=TRUE; else arena_r=FALSE; |
| set_adc_channel(L); // prepnuti kanalu ADC, je treba min 10us na ustaleni |
| delay_us(10); |
| read_adc(ADC_START_ONLY); |
| set_pwm1_duty(55); // 1:0 |
| }; |
| |
| if (GRAVITY) {if (g<255) g++;} else g=0; |
| if (g>3 && !diag) {FL; FR; while(TRUE);}; // kdyz nas preklopi, nedej se |
| } |
| |
| // Primitivni Pipani |
| void beep(unsigned int16 period, unsigned int16 length) |
| { |
| unsigned int16 nn; |
| |
| disable_interrupts(GLOBAL); |
| 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); |
| } |
| enable_interrupts(GLOBAL); |
| } |
| |
| /******************************************************************************/ |
| inline void diagnostika() |
| { |
| unsigned int16 n; |
| |
| #ifdef DEBUG1 |
| while (true) // Diagnostika podvozku |
| { |
| 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); |
| }; |
| #endif |
| |
| if (GRAVITY) { |
| diag=TRUE; |
| enable_interrupts(INT_TIMER0); |
| enable_interrupts(GLOBAL); |
| while (true) // Diagnostika cidel |
| { |
| if (g>100) beep(800,100); |
| Delay_ms(50); |
| if (arena_l) beep(1000,200); |
| Delay_ms(50); |
| if (arena_r) beep(2000,300); |
| Delay_ms(50); |
| if (sr>10) beep(3000,400); |
| Delay_ms(50); |
| if (f>10) beep(4000,500); |
| Delay_ms(50); |
| if (sl>10) beep(5000,500); |
| Delay_ms(50); |
| if (b>10) beep(6000,600); |
| Delay_ms(50); |
| }}; |
| } |
| |
| void main() |
| { |
| unsigned int16 n; // for FOR |
| |
| STOPL; STOPR; // zastavi motory |
| |
| setup_oscillator(OSC_8MHZ|OSC_INTRC); // CPU clock 8MHz |
| setup_adc_ports(sAN2|sAN3|VSS_VDD); // prevodniky na cidla na okraj areny |
| setup_adc(ADC_CLOCK_INTERNAL); |
| setup_spi(FALSE); |
| setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4); // Casovac pro SW PWM a cteni cidel |
| setup_timer_1(T1_DISABLED); |
| setup_timer_2(T2_DIV_BY_1,54,1); // Casovac pro PWM pro IR sensory cca 36kHz |
| setup_ccp1(CCP_PWM); // HW PWM ON |
| set_pwm1_duty(27); // 1:1 |
| setup_comparator(NC_NC_NC_NC); |
| setup_vref(FALSE); |
| |
| set_adc_channel(R); |
| |
| Beep(1000,200); //double beep |
| Delay_ms(50); |
| Beep(1000,200); |
| diagnostika(); |
| |
| enable_interrupts(INT_TIMER0); |
| enable_interrupts(GLOBAL); |
| /*---------------------------------------------------------------------------*/ |
| for (n=1;n<=3;n++) // 5s do zacatku souboje |
| { |
| Delay_ms(990); |
| Beep(1000,200); |
| } |
| Delay_ms(300); |
| |
| while(true) // hlavni smycka |
| { |
| LOOP: |
| |
| GO(L, F, 150); GO(R, F, 150); |
| |
| if (arena_r) |
| { |
| BL; BR; |
| delay_ms(100); |
| STOPL; BR; |
| for(n=0; n<5000; n++) |
| { |
| if (!arena_r || arena_l) {BL; BR;}; |
| }; |
| FL; BR; |
| delay_ms(100); |
| STOPL; STOPR; |
| } |
| |
| if (arena_l) |
| { |
| BL; BR; |
| delay_ms(100); |
| BL; STOPR; |
| for(n=0; n<5000; n++) |
| { |
| if (!arena_l || arena_r) {BL; BR;}; |
| }; |
| BL; FR; |
| delay_ms(100); |
| STOPL; STOPR; |
| } |
| |
| if (sr>10) // Nepritel vpravo |
| { |
| FL; FR; // popojed rovne |
| for(n=0; n<5000; n++) |
| { |
| if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;}; |
| }; |
| FL; BR; // otoc se na nej |
| for(n=0; n<10000; n++) |
| { |
| if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;}; |
| if (f>5) |
| { |
| FL; FR; // vytlac ho |
| }; |
| if (sl>5) {BL; FR;}; |
| if (sr>5) {FL; BR;}; |
| }; |
| } |
| |
| if (sl>10) // Nepritel vlevo |
| { |
| FL; FR; // popojed rovne |
| for(n=0; n<5000; n++) |
| { |
| if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;}; |
| }; |
| BL; FR; // otoc se na nej |
| for(n=0; n<10000; n++) |
| { |
| if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;}; |
| if (f>5) |
| { |
| FL; FR; // vytlac ho |
| }; |
| if (sl>5) {BL; FR;}; |
| if (sr>5) {FL; BR;}; |
| }; |
| } |
| |
| if (f>10) // Nepritel vpredu |
| { |
| BL; FR; |
| delay_ms(110); |
| FL; BR; |
| delay_ms(50); |
| STOPL; STOPR; |
| } |
| |
| if (b>10) // Nepritel vzadu |
| { |
| BL; FR; |
| delay_ms(110); |
| FL; BR; |
| delay_ms(50); |
| STOPL; STOPR; |
| } |
| |
| } // while(true) |
| } |