| 2 |
kaklik |
1 |
#include ".\main.h"
|
|
|
2 |
|
|
|
3 |
#use rtos (timer=2, minor_cycle=2ms)
|
|
|
4 |
|
|
|
5 |
unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru
|
|
|
6 |
unsigned int8 line; // na ktere strane byla detekovana cara
|
|
|
7 |
unsigned int8 dira; // pocita dobu po kterou je ztracena cara
|
|
|
8 |
unsigned int8 uhel; // urcuje aktualni uhel zataceni
|
|
|
9 |
|
|
|
10 |
signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem
|
|
|
11 |
signed int16 Rmotor; // a pravem motoru
|
|
|
12 |
|
|
|
13 |
#define LMAX 190 // maximalni zatoceni doleva
|
|
|
14 |
#define RMAX 60 // maximalni zatoceni doprava
|
|
|
15 |
#define STRED 128 // sredni poloha zadniho kolecka
|
|
|
16 |
|
|
|
17 |
// servo
|
|
|
18 |
#define SERVO PIN_A2
|
|
|
19 |
|
|
|
20 |
//motory
|
|
|
21 |
#define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred
|
|
|
22 |
#define FL output_low(PIN_A1); output_high(PIN_A0)
|
|
|
23 |
#define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad
|
|
|
24 |
#define BL output_low(PIN_A0); output_high(PIN_A1)
|
|
|
25 |
#define STOPR output_low(PIN_A6);output_low(PIN_A7)
|
|
|
26 |
#define STOPL output_low(PIN_A0);output_low(PIN_A1)
|
|
|
27 |
|
|
|
28 |
//HID
|
|
|
29 |
#define LED1 PIN_B1 //oranzova
|
|
|
30 |
#define LED2 PIN_B2 //zluta
|
|
|
31 |
|
|
|
32 |
#define STROBE PIN_B0
|
|
|
33 |
//#define SW1 PIN_A2 // Motory On/off
|
|
|
34 |
|
|
|
35 |
// makro pro PWM
|
|
|
36 |
#define GO(motor, direction, power) if(get_timer0()<=power) \
|
|
|
37 |
{direction##motor;} else {stop##motor;}
|
|
|
38 |
|
|
|
39 |
|
|
|
40 |
////////////////////////////////////////////////////////////////////////////////
|
|
|
41 |
////////////////////////////////////////////////////////////////////////////////
|
|
|
42 |
/*
|
|
|
43 |
void diagnostika()
|
|
|
44 |
{
|
|
|
45 |
if(input(SW1))STOPR;STOPL;While(TRUE);
|
|
|
46 |
// if(LSENSOR==true) output_high(LED2); else output_low(LED2);
|
|
|
47 |
// if(RSENSOR==true) output_high(LED1); else output_low(LED1);
|
|
|
48 |
}
|
|
|
49 |
*/
|
|
|
50 |
////////////////////////////////////////////////////////////////////////////////
|
|
|
51 |
#task (rate=16ms,max=2ms)
|
|
|
52 |
void zatoc() // ridi servo ktere otaci kolem
|
|
|
53 |
{
|
|
|
54 |
unsigned int8 n;
|
|
|
55 |
output_high(SERVO);
|
|
|
56 |
delay_us(1200);
|
|
|
57 |
for(n=uhel; n>0; n--);
|
|
|
58 |
output_low(SERVO);
|
|
|
59 |
}
|
|
|
60 |
|
|
|
61 |
////////////////////////////////////////////////////////////////////////////////
|
|
|
62 |
#task (rate=2ms, max=500us)
|
|
|
63 |
void motory() // umoznuje nezavisle rizeni vykonu na obou motorech
|
|
|
64 |
{
|
|
|
65 |
|
|
|
66 |
if(Rmotor>0) //pravy motor
|
|
|
67 |
{
|
|
|
68 |
GO(R, F, Rmotor);
|
|
|
69 |
}
|
|
|
70 |
|
|
|
71 |
if(Rmotor==0) STOPR;
|
|
|
72 |
|
|
|
73 |
if(Rmotor<0)
|
|
|
74 |
{
|
|
|
75 |
GO(R, B, abs(Rmotor));
|
|
|
76 |
}
|
|
|
77 |
|
|
|
78 |
if(Lmotor>0) // levy motor
|
|
|
79 |
{
|
|
|
80 |
GO(L, F, Lmotor);
|
|
|
81 |
}
|
|
|
82 |
|
|
|
83 |
if(Lmotor==0) STOPL;
|
|
|
84 |
|
|
|
85 |
if(Lmotor<0)
|
|
|
86 |
{
|
|
|
87 |
GO(L, B, abs(Lmotor));
|
|
|
88 |
}
|
|
|
89 |
}
|
|
|
90 |
////////////////////////////////////////////////////////////////////////////////
|
|
|
91 |
#task (rate=4ms, max=500us, queue=0)
|
|
|
92 |
void rizeni()
|
|
|
93 |
{
|
|
|
94 |
rizeni:
|
|
|
95 |
|
|
|
96 |
sensors = spi_read(0); // cteni senzoru na caru
|
|
|
97 |
output_high(STROBE);
|
|
|
98 |
output_low(STROBE);
|
|
|
99 |
|
|
|
100 |
if(bit_test(sensors,0)) //*......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
|
|
|
101 |
{
|
|
|
102 |
uhel=LMAX;
|
|
|
103 |
Lmotor=0;Rmotor=127;
|
|
|
104 |
goto rizeni;
|
|
|
105 |
}
|
|
|
106 |
if(bit_test(sensors,6)) //......*//
|
|
|
107 |
{
|
|
|
108 |
uhel=RMAX;
|
|
|
109 |
Rmotor=0;Lmotor=127;
|
|
|
110 |
goto rizeni;
|
|
|
111 |
}
|
|
|
112 |
if(bit_test(sensors,1)) //.*.....//
|
|
|
113 |
{
|
|
|
114 |
uhel=STRED+30;
|
|
|
115 |
Lmotor=70;Rmotor=127;
|
|
|
116 |
goto rizeni;
|
|
|
117 |
}
|
|
|
118 |
if(bit_test(sensors,5)) //.....*.//
|
|
|
119 |
{
|
|
|
120 |
uhel=STRED-30;
|
|
|
121 |
Rmotor=70;Lmotor=127;
|
|
|
122 |
goto rizeni;
|
|
|
123 |
}
|
|
|
124 |
if(bit_test(sensors,2)) //..*....//
|
|
|
125 |
{
|
|
|
126 |
uhel=STRED+20;
|
|
|
127 |
Lmotor=90;Rmotor=127;
|
|
|
128 |
goto rizeni;
|
|
|
129 |
}
|
|
|
130 |
if(bit_test(sensors,4)) //....*..//
|
|
|
131 |
{
|
|
|
132 |
uhel=STRED-20;
|
|
|
133 |
Rmotor=90;Lmotor=127;
|
|
|
134 |
goto rizeni;
|
|
|
135 |
}
|
|
|
136 |
if(bit_test(sensors,3)) //...*...//
|
|
|
137 |
{
|
|
|
138 |
uhel=STRED;
|
|
|
139 |
Lmotor=100;Rmotor=100;
|
|
|
140 |
goto rizeni;
|
|
|
141 |
}
|
|
|
142 |
|
|
|
143 |
/*STOPL;STOPR;
|
|
|
144 |
if(bit_test(sensors,7))
|
|
|
145 |
{
|
|
|
146 |
|
|
|
147 |
}*/
|
|
|
148 |
}
|
|
|
149 |
////////////////////////////////////////////////////////////////////////////////
|
|
|
150 |
void main()
|
|
|
151 |
{
|
|
|
152 |
setup_adc_ports(NO_ANALOGS);
|
|
|
153 |
setup_adc(ADC_CLOCK_INTERNAL);
|
|
|
154 |
setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
|
|
|
155 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
|
|
|
156 |
setup_timer_1(T1_DISABLED);
|
|
|
157 |
setup_oscillator(OSC_8MHZ|OSC_INTRC);
|
|
|
158 |
// set_pwm1_duty(0); // Spust PWM, ale zatim s trvalou 0 na vystupu
|
|
|
159 |
// setup_ccp1(CCP_PWM);
|
|
|
160 |
// setup_timer_2(T2_DIV_BY_16,200,1); // perioda
|
|
|
161 |
// setup_timer_2(T2_DIV_BY_4,255,10); // Casovac pro regulaci
|
|
|
162 |
|
|
|
163 |
STOPR; STOPL;
|
|
|
164 |
uhel=STRED;
|
|
|
165 |
// diagnostika();
|
|
|
166 |
Lmotor=0;
|
|
|
167 |
Rmotor=0;
|
|
|
168 |
rtos_run();
|
|
|
169 |
|
|
|
170 |
}
|