Subversion Repositories svnkaklik

Rev

Go to most recent revision | Details | Last modification | View Log

Rev Author Line No. Line
203 kakl 1 
// Program pro MiniSumo na R-Day 2006
2 
#include "cholerik.h"
3 
 
4 
// Konstanty
5 
#define TRESHOLD     0x254        // rozhodovaci uroven pro okraj areny
6 
//#define DEBUG1 1   // Diagnostika pohonu
7 
 
8 
//motory            //Napred vypnout potom zapnout!
9 
#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred
10 
#define FL         output_low(PIN_B7); output_high(PIN_B6)
11 
#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad
12 
#define BL         output_low(PIN_B6); output_high(PIN_B7)
13 
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)    // Zastav
14 
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)
15 
 
16 
//cidla
17 
#define L         2           // Senzory na okraj areny
18 
#define R         3
19 
#define SIDE_R    !input(PIN_A7)     // Sensory na soupere
20 
#define SIDE_L    !input(PIN_A4)
21 
#define FRONT     !input(PIN_A6)
22 
#define BACK      !input(PIN_B3)
23 
#define GRAVITY   !input(PIN_A1)
24 
 
25 
#DEFINE SOUND_HI   PIN_B1     // komplementarni vystupy pro piezo pipak
26 
#DEFINE SOUND_LO   PIN_B2
27 
 
28 
// makro pro PWM
29 
#define GO(motor, direction, power) if(get_timer0()<=power) \
30 
{direction##motor;} else {stop##motor;}
31 
 
32 
unsigned int8 majak=0;
33 
unsigned int8 sl=0;
34 
unsigned int8 sr=0;
35 
unsigned int8 b=0;
36 
unsigned int8 f=0;
37 
unsigned int8 g=0;
38 
int1  arena_l;
39 
int1  arena_r;
40 
int1 diag=FALSE;
41 
 
42 
#int_TIMER0
43 
TIMER0_isr()
44 
{
45 
   int1 stav;
46 
 
47 
   stav = ((majak & 0b1) == 0b1);
48 
   if (((SIDE_R && stav) || (!SIDE_R && !stav))) {if (sr<255) sr++;} else {sr=0;};
49 
   if (((SIDE_L && stav) || (!SIDE_L && !stav))) {if (sl<255) sl++;} else {sl=0;};
50 
   if (((BACK && stav) || (!BACK && !stav))) {if (b<255) b++;} else {b=0;};
51 
   if (((FRONT && stav) || (!FRONT && !stav))) {if (f<255) f++;} else {f=0;};
52 
   majak++;
53 
   stav = ((majak & 0b1) == 0b1);
54 
   if (stav)
55 
   {
56 
      if (read_adc(ADC_READ_ONLY) > 250) arena_l=TRUE; else arena_l=FALSE;
57 
      set_adc_channel(R); // prepnuti kanalu ADC, je treba min 10us na ustaleni
58 
      delay_us(10);
59 
      read_adc(ADC_START_ONLY);
60 
      set_pwm1_duty(27);      // 1:1
61 
   }
62 
   else
63 
   {
64 
      if (read_adc(ADC_READ_ONLY) > 250) arena_r=TRUE; else arena_r=FALSE;
65 
      set_adc_channel(L); // prepnuti kanalu ADC, je treba min 10us na ustaleni
66 
      delay_us(10);
67 
      read_adc(ADC_START_ONLY);
68 
      set_pwm1_duty(55);      // 1:0
69 
   };
70 
 
71 
   if (GRAVITY) {if (g<255) g++;} else g=0;
72 
   if (g>3 && !diag) {FL; FR; while(TRUE);}; // kdyz nas preklopi, nedej se
73 
}
74 
 
75 
// Primitivni Pipani
76 
void beep(unsigned int16 period, unsigned int16 length)
77 
{
78 
   unsigned int16 nn;
79 
 
80 
   disable_interrupts(GLOBAL);
81 
   for(nn=length; nn>0; nn--)
82 
   {
83 
     output_high(SOUND_HI);output_low(SOUND_LO);
84 
     delay_us(period);
85 
     output_high(SOUND_LO);output_low(SOUND_HI);
86 
     delay_us(period);
87 
   }
88 
   enable_interrupts(GLOBAL);
89 
}
90 
 
91 
/******************************************************************************/
92 
inline void diagnostika()
93 
{
94 
   unsigned int16 n;
95 
 
96 
#ifdef DEBUG1
97 
   while (true)   // Diagnostika podvozku
98 
   {
99 
      for (n=500; n<800; n+=100)
100 
      {
101 
         beep(n,n); //beep UP
102 
      };
103 
      Delay_ms(1000);
104 
      //zastav vse
105 
      STOPL; STOPR;
106 
      //pravy pas
107 
      FR; Delay_ms(1000); STOPR; Delay_ms(1000);
108 
      BR; Delay_ms(1000); STOPR; Delay_ms(1000);
109 
      Beep(880,100); Delay_ms(1000);
110 
      //levy pas
111 
      FL; Delay_ms(1000); STOPL; Delay_ms(1000);
112 
      BL; Delay_ms(1000); STOPL; Delay_ms(1000);
113 
      Beep(880,100); Delay_ms(1000);
114 
      //oba pasy
115 
      FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
116 
      BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
117 
   };
118 
#endif
119 
 
120 
   if (GRAVITY) {
121 
   diag=TRUE;
122 
   enable_interrupts(INT_TIMER0);
123 
   enable_interrupts(GLOBAL);
124 
   while (true)         // Diagnostika cidel
125 
   {
126 
      if (g>100) beep(800,100);
127 
      Delay_ms(50);
128 
      if (arena_l) beep(1000,200);
129 
      Delay_ms(50);
130 
      if (arena_r) beep(2000,300);
131 
      Delay_ms(50);
132 
      if (sr>10) beep(3000,400);
133 
      Delay_ms(50);
134 
      if (f>10) beep(4000,500);
135 
      Delay_ms(50);
136 
      if (sl>10) beep(5000,500);
137 
      Delay_ms(50);
138 
      if (b>10) beep(6000,600);
139 
      Delay_ms(50);
140 
   }};
141 
}
142 
 
143 
void main()
144 
{
145 
   unsigned int16 n; // for FOR
146 
 
147 
   STOPL; STOPR;     // zastavi motory
148 
 
149 
   setup_oscillator(OSC_8MHZ|OSC_INTRC);     // CPU clock 8MHz
150 
   setup_adc_ports(sAN2|sAN3|VSS_VDD);   // prevodniky na cidla na okraj areny
151 
   setup_adc(ADC_CLOCK_INTERNAL);
152 
   setup_spi(FALSE);
153 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4);  // Casovac pro SW PWM a cteni cidel
154 
   setup_timer_1(T1_DISABLED);
155 
   setup_timer_2(T2_DIV_BY_1,54,1); // Casovac pro PWM pro IR sensory cca 36kHz
156 
   setup_ccp1(CCP_PWM);    // HW PWM ON
157 
   set_pwm1_duty(27);      // 1:1
158 
   setup_comparator(NC_NC_NC_NC);
159 
   setup_vref(FALSE);
160 
 
161 
   set_adc_channel(R);
162 
 
163 
   Beep(1000,200);     //double beep
164 
   Delay_ms(50);
165 
   Beep(1000,200);
166 
   diagnostika();
167 
 
168 
   enable_interrupts(INT_TIMER0);
169 
   enable_interrupts(GLOBAL);
170 
/*---------------------------------------------------------------------------*/
171 
   for (n=1;n<=3;n++)   // 5s do zacatku souboje
172 
   {
173 
      Delay_ms(990);
174 
      Beep(1000,200);
175 
   }
176 
   Delay_ms(300);
177 
 
178 
   while(true)       // hlavni smycka
179 
   {
180 
LOOP:
181 
 
182 
      GO(L, F, 150); GO(R, F, 150);
183 
 
184 
      if (arena_r)
185 
      {
186 
         BL; BR;
187 
         delay_ms(100);
188 
         STOPL; BR;
189 
         for(n=0; n<5000; n++)
190 
         {
191 
            if (!arena_r || arena_l) {BL; BR;};
192 
         };
193 
         FL; BR;
194 
         delay_ms(100);
195 
         STOPL; STOPR;
196 
      }
197 
 
198 
      if (arena_l)
199 
      {
200 
         BL; BR;
201 
         delay_ms(100);
202 
         BL; STOPR;
203 
         for(n=0; n<5000; n++)
204 
         {
205 
            if (!arena_l || arena_r) {BL; BR;};
206 
         };
207 
         BL; FR;
208 
         delay_ms(100);
209 
         STOPL; STOPR;
210 
      }
211 
 
212 
      if (sr>10)     // Nepritel vpravo
213 
      {
214 
         FL; FR;                 // popojed rovne
215 
         for(n=0; n<5000; n++)
216 
         {
217 
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
218 
         };
219 
         FL; BR;                     // otoc se na nej
220 
         for(n=0; n<10000; n++)
221 
         {
222 
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
223 
            if (f>5)
224 
            {
225 
               FL; FR;               // vytlac ho
226 
            };
227 
            if (sl>5) {BL; FR;};
228 
            if (sr>5) {FL; BR;};
229 
         };
230 
      }
231 
 
232 
      if (sl>10)     // Nepritel vlevo
233 
      {
234 
         FL; FR;                 // popojed rovne
235 
         for(n=0; n<5000; n++)
236 
         {
237 
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
238 
         };
239 
         BL; FR;                    // otoc se na nej
240 
         for(n=0; n<10000; n++)
241 
         {
242 
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
243 
            if (f>5)
244 
            {
245 
               FL; FR;              // vytlac ho
246 
            };
247 
            if (sl>5) {BL; FR;};
248 
            if (sr>5) {FL; BR;};
249 
         };
250 
      }
251 
 
252 
      if (f>10)      // Nepritel vpredu
253 
      {
254 
         BL; FR;
255 
         delay_ms(110);
256 
         FL; BR;
257 
         delay_ms(50);
258 
         STOPL; STOPR;
259 
      }
260 
 
261 
      if (b>10)      // Nepritel vzadu
262 
      {
263 
         BL; FR;
264 
         delay_ms(110);
265 
         FL; BR;
266 
         delay_ms(50);
267 
         STOPL; STOPR;
268 
      }
269 
 
270 
   } // while(true)
271 
}