0,0 → 1,130 |
#include "laserus.h" |
|
#define SERVO PIN_B5 // Vystup pro rizeni serva |
#define LASER PIN_B4 // Vstup pro cteni laseru |
|
#define MOT_DIR_L PIN_C0 // Rizeni smeru otaceni motoru |
#define MOT_DIR_R PIN_C3 |
|
// kroutitka |
#define CERVENA 0 // AN0 |
#define CERNA 1 // AN1 |
//#define ZELENA 3 // AN3 |
//#define MODRA 4 // AN4 |
|
#byte INTCON = 0x0B // Interrupt configuration register |
#bit GIE = INTCON.7 |
#bit PEIE = INTCON.6 |
#bit TMR0IE = INTCON.5 |
#bit INT0IE = INTCON.4 |
#bit RBIE = INTCON.3 |
#bit TMR0IF = INTCON.2 |
#bit INT0IF = INTCON.1 |
#bit RBIF = INTCON.0 |
|
boolean edge; |
int8 uhel, olduhel; |
int16 uhel16; |
|
#int_EXT |
EXT_isr() |
{ |
int n, t1, t2, t; |
|
set_timer0(0); // Vynulovani casovace pro 2ms |
output_high(SERVO); |
while(get_timer0()<(1000/256)); // Ceka 1ms |
for(n=uhel; n>0; n--) delay_us(3); // Sirka impulzu podle uhlu |
output_low(SERVO); |
while(get_timer0()<(3000/256)); // Ceka do 3ms, nez zacne scanovat |
|
set_timer0(0); // Vynulovani casovace pro zjisteni polohy cary |
if(edge) // Zrcatko prejizdelo tam nebo zpet? |
{ |
edge=false; |
ext_int_edge(H_TO_L); // Pristi inerrupt bude od opacne hrany |
INT0IF=0; // Povoleni dalsiho preruseni |
enable_interrupts(INT_EXT); |
enable_interrupts(GLOBAL); |
// do |
{ |
while(0==input(LASER)); // Ceka na detekci cary |
// output_high(SERVO); |
t1=get_timer0(); // Poznamena cas nabezne hrany |
while(1==input(LASER)); // Ceka az zkonci cara |
// output_low(SERVO); |
t2=get_timer0(); // Poznamena cas sestupne hrany |
t=t2-t1; |
} while((t<3) || (t>20)); // Cara je detekovana, kdyz trva mezi xx ms |
// if ((t>3) && (t<20)) uhel=(160-(read_adc()>>2))-t1; |
if (abs(olduhel-uhel)<40) |
{ |
uhel=(160-(read_adc()>>2))-t1; |
olduhel=uhel; |
}; |
// uhel16=uhel; |
// set_pwm1_duty(250-(uhel16<<2)); // Elektronicky diferencial |
// set_pwm2_duty((uhel16)); |
set_adc_channel(CERNA); |
} |
else |
{ |
edge=true; |
ext_int_edge(L_TO_H); // Pristi inerrupt bude od opacne hrany |
INT0IF=0; // Povoleni dalsiho preruseni |
enable_interrupts(INT_EXT); |
enable_interrupts(GLOBAL); |
// do |
{ |
while(0==input(LASER)); // Ceka na detekci cary |
t1=get_timer0(); // Poznamena cas nabezne hrany |
while(1==input(LASER)); // Ceka az zkonci cara |
t2=get_timer0(); // Poznamena cas sestupne hrany |
t=t2-t1; |
} while((t<3) || (t>20)); // Cara je detekovana, kdyz trva mezi xx ms |
if (abs(olduhel-uhel)<40) |
{ |
// uhel=((read_adc()>>2)+32)+t1; |
// olduhel=uhel; |
}; |
set_adc_channel(CERVENA); |
} |
while(true); |
} |
|
|
|
void main() |
{ |
|
setup_adc_ports(ALL_ANALOG); // Analogove vstupy pro cteni trimru |
setup_adc(ADC_CLOCK_INTERNAL); |
setup_spi(FALSE); |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_256); // Casovac pro cteni laseru |
setup_timer_1(T1_DISABLED); |
setup_timer_2(T2_DIV_BY_4,255,1); // Casovac pro PWM cca 900Hz |
setup_ccp1(CCP_PWM); // Nastaveni PWM pro diferencial RC1, RC2 |
setup_ccp2(CCP_PWM); |
|
set_pwm1_duty(90); // Zastaveni PWM |
set_pwm2_duty(90); |
|
delay_ms(100); |
set_adc_channel(CERVENA); |
Delay_ms(1); |
|
ext_int_edge(L_TO_H); |
edge=true; |
uhel=((read_adc()>>2)+32)+30; |
olduhel=uhel; |
|
enable_interrupts(INT_EXT); |
enable_interrupts(GLOBAL); |
|
delay_ms(1000); |
|
output_high(MOT_DIR_L); // Oba motory vpred |
output_high(MOT_DIR_R); |
|
while(true); |
} |