| 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); |
| } |