0,0 → 1,272 |
// Program pro MiniSumo na R-Day 2006 |
//"$Id$" |
|
#include "cholerik.h" |
|
#define THRESHOLD 0x8 // rozhodovaci uroven pro okraj areny 0-15 (od 0.25 do 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) |
|
//cidla |
#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) |
// PIN_A3 Prave cidlo na okraj areny (C1OUT) |
// PIN_A2 Leve cidlo na okraj areny (C2OUT) |
|
#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=FALSE; |
int1 arena_r=FALSE; |
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 (!C1OUT) arena_r=TRUE; else arena_r=FALSE; |
if (!C2OUT) arena_l=TRUE; else arena_l=FALSE; |
|
if (stav) |
{ |
set_pwm1_duty(27); // 1:1 |
} |
else |
{ |
set_pwm1_duty(55); // 1:0 |
}; |
setup_comparator(NC_NC_NC_NC); // inicializace komparatoru |
if (GRAVITY) {if (g<255) g++;} else g=0; |
setup_comparator(A3_VR_A2_VR); // inicializace komparatoru |
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); |
} |
|
/**** DIAG ********************************************************************/ |
inline void diagnostika() |
{ |
unsigned int16 n; |
|
setup_comparator(NC_NC_NC_NC); // inicializace komparatoru |
if (GRAVITY) |
{ |
setup_comparator(A3_VR_A2_VR); // inicializace komparatoru |
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(10);beep(1000,200);}; |
Delay_ms(50); |
if (arena_r) {beep(2000,300); delay_ms(10);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); |
if((g>100) && arena_l && arena_r) break; // Preklopen na zada a bily papir na obou cidlech na okraj areny |
}; |
|
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); |
}; |
}; |
} |
|
/**** MAIN ********************************************************************/ |
void main() |
{ |
unsigned int16 n; // for FOR |
|
STOPL; STOPR; // zastavi motory |
|
setup_oscillator(OSC_8MHZ|OSC_INTRC); // CPU clock 8MHz |
setup_adc_ports(NO_ANALOGS); // komparatory vypnuty |
setup_adc(ADC_OFF); |
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(A3_VR_A2_VR); // inicializace komparatoru |
setup_comparator(NC_NC_NC_NC); // inicializace komparatoru |
setup_vref(VREF_HIGH|THRESHOLD); // 16 kroku od 0.25 do 0.75 Vdd |
|
Beep(1000,200); //double beep |
Delay_ms(50); |
Beep(1000,200); |
diagnostika(); |
|
enable_interrupts(INT_TIMER0); |
enable_interrupts(GLOBAL); |
|
for (n=1;n<=5;n++) // 5s do zacatku souboje |
{ |
Delay_ms(720); |
Beep(1000,200); |
} |
|
/*----- Main Loop ------------------------------------------------------------*/ |
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) |
} |
Property changes: |
Added: svn:keywords |
+Id Rev |
\ No newline at end of property |