135 |
kakl |
1 |
#include "laserus.h"
|
|
|
2 |
|
|
|
3 |
#define SERVO PIN_B5 // Vystup pro rizeni serva
|
|
|
4 |
#define LASER PIN_B4 // Vstup pro cteni laseru
|
|
|
5 |
|
|
|
6 |
#define MOT_DIR_L PIN_C0 // Rizeni smeru otaceni motoru
|
|
|
7 |
#define MOT_DIR_R PIN_C3
|
|
|
8 |
|
|
|
9 |
// kroutitka
|
|
|
10 |
#define CERVENA 0 // AN0
|
|
|
11 |
#define CERNA 1 // AN1
|
|
|
12 |
//#define ZELENA 3 // AN3
|
|
|
13 |
//#define MODRA 4 // AN4
|
|
|
14 |
|
|
|
15 |
#byte INTCON = 0x0B // Interrupt configuration register
|
|
|
16 |
#bit GIE = INTCON.7
|
|
|
17 |
#bit PEIE = INTCON.6
|
|
|
18 |
#bit TMR0IE = INTCON.5
|
|
|
19 |
#bit INT0IE = INTCON.4
|
|
|
20 |
#bit RBIE = INTCON.3
|
|
|
21 |
#bit TMR0IF = INTCON.2
|
|
|
22 |
#bit INT0IF = INTCON.1
|
|
|
23 |
#bit RBIF = INTCON.0
|
|
|
24 |
|
|
|
25 |
boolean edge;
|
|
|
26 |
int8 uhel, olduhel;
|
|
|
27 |
int16 uhel16;
|
|
|
28 |
|
|
|
29 |
#int_EXT
|
|
|
30 |
EXT_isr()
|
|
|
31 |
{
|
|
|
32 |
int n, t1, t2, t;
|
|
|
33 |
|
|
|
34 |
set_timer0(0); // Vynulovani casovace pro 2ms
|
|
|
35 |
output_high(SERVO);
|
|
|
36 |
while(get_timer0()<(1000/256)); // Ceka 1ms
|
|
|
37 |
for(n=uhel; n>0; n--) delay_us(3); // Sirka impulzu podle uhlu
|
|
|
38 |
output_low(SERVO);
|
|
|
39 |
while(get_timer0()<(3000/256)); // Ceka do 3ms, nez zacne scanovat
|
|
|
40 |
|
|
|
41 |
set_timer0(0); // Vynulovani casovace pro zjisteni polohy cary
|
|
|
42 |
if(edge) // Zrcatko prejizdelo tam nebo zpet?
|
|
|
43 |
{
|
|
|
44 |
edge=false;
|
|
|
45 |
ext_int_edge(H_TO_L); // Pristi inerrupt bude od opacne hrany
|
|
|
46 |
INT0IF=0; // Povoleni dalsiho preruseni
|
|
|
47 |
enable_interrupts(INT_EXT);
|
|
|
48 |
enable_interrupts(GLOBAL);
|
|
|
49 |
// do
|
|
|
50 |
{
|
|
|
51 |
while(0==input(LASER)); // Ceka na detekci cary
|
|
|
52 |
// output_high(SERVO);
|
|
|
53 |
t1=get_timer0(); // Poznamena cas nabezne hrany
|
|
|
54 |
while(1==input(LASER)); // Ceka az zkonci cara
|
|
|
55 |
// output_low(SERVO);
|
|
|
56 |
t2=get_timer0(); // Poznamena cas sestupne hrany
|
|
|
57 |
t=t2-t1;
|
|
|
58 |
} while((t<3) || (t>20)); // Cara je detekovana, kdyz trva mezi xx ms
|
|
|
59 |
// if ((t>3) && (t<20)) uhel=(160-(read_adc()>>2))-t1;
|
|
|
60 |
// if (abs(olduhel-uhel)<40)
|
|
|
61 |
{
|
|
|
62 |
uhel=(160-(read_adc()>>2))-t1;
|
|
|
63 |
olduhel=uhel;
|
|
|
64 |
};
|
|
|
65 |
// uhel16=uhel;
|
|
|
66 |
// set_pwm1_duty(250-(uhel16<<2)); // Elektronicky diferencial
|
|
|
67 |
// set_pwm2_duty((uhel16));
|
|
|
68 |
set_adc_channel(CERNA);
|
|
|
69 |
}
|
|
|
70 |
else
|
|
|
71 |
{
|
|
|
72 |
edge=true;
|
|
|
73 |
ext_int_edge(L_TO_H); // Pristi inerrupt bude od opacne hrany
|
|
|
74 |
INT0IF=0; // Povoleni dalsiho preruseni
|
|
|
75 |
enable_interrupts(INT_EXT);
|
|
|
76 |
enable_interrupts(GLOBAL);
|
|
|
77 |
// do
|
|
|
78 |
{
|
|
|
79 |
while(0==input(LASER)); // Ceka na detekci cary
|
|
|
80 |
t1=get_timer0(); // Poznamena cas nabezne hrany
|
|
|
81 |
while(1==input(LASER)); // Ceka az zkonci cara
|
|
|
82 |
t2=get_timer0(); // Poznamena cas sestupne hrany
|
|
|
83 |
t=t2-t1;
|
|
|
84 |
} while((t<3) || (t>20)); // Cara je detekovana, kdyz trva mezi xx ms
|
|
|
85 |
if (abs(olduhel-uhel)<40)
|
|
|
86 |
{
|
|
|
87 |
// uhel=((read_adc()>>2)+32)+t1;
|
|
|
88 |
// olduhel=uhel;
|
|
|
89 |
};
|
|
|
90 |
set_adc_channel(CERVENA);
|
|
|
91 |
}
|
|
|
92 |
while(true);
|
|
|
93 |
}
|
|
|
94 |
|
|
|
95 |
|
|
|
96 |
|
|
|
97 |
void main()
|
|
|
98 |
{
|
|
|
99 |
|
|
|
100 |
setup_adc_ports(ALL_ANALOG); // Analogove vstupy pro cteni trimru
|
|
|
101 |
setup_adc(ADC_CLOCK_INTERNAL);
|
|
|
102 |
setup_spi(FALSE);
|
|
|
103 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_256); // Casovac pro cteni laseru
|
|
|
104 |
setup_timer_1(T1_DISABLED);
|
|
|
105 |
setup_timer_2(T2_DIV_BY_4,255,1); // Casovac pro PWM cca 900Hz
|
|
|
106 |
setup_ccp1(CCP_PWM); // Nastaveni PWM pro diferencial RC1, RC2
|
|
|
107 |
setup_ccp2(CCP_PWM);
|
|
|
108 |
|
|
|
109 |
set_pwm1_duty(90); // Zastaveni PWM
|
|
|
110 |
set_pwm2_duty(90);
|
|
|
111 |
|
|
|
112 |
delay_ms(100);
|
|
|
113 |
set_adc_channel(CERVENA);
|
|
|
114 |
Delay_ms(1);
|
|
|
115 |
|
|
|
116 |
ext_int_edge(L_TO_H);
|
|
|
117 |
edge=true;
|
|
|
118 |
uhel=((read_adc()>>2)+32)+30;
|
|
|
119 |
olduhel=uhel;
|
|
|
120 |
|
|
|
121 |
enable_interrupts(INT_EXT);
|
|
|
122 |
enable_interrupts(GLOBAL);
|
|
|
123 |
|
|
|
124 |
delay_ms(1000);
|
|
|
125 |
|
|
|
126 |
output_high(MOT_DIR_L); // Oba motory vpred
|
|
|
127 |
output_high(MOT_DIR_R);
|
|
|
128 |
|
|
|
129 |
while(true);
|
|
|
130 |
}
|