Rev 143 | Blame | Last modification | View Log | Download
#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);
}