Subversion Repositories svnkaklik

Rev

Rev 206 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log

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