Subversion Repositories svnkaklik

Rev

Rev 205 | 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 206 2007-03-21 10:02:02Z kakl $"
205 kakl 3 
 
203 kakl 4 
#include "cholerik.h"
5 
 
6 
// Konstanty
205 kakl 7 
#define TRESHOLD     0x250        // 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);
56 
   if (stav)
57 
   {
205 kakl 58 
      if (read_adc(ADC_READ_ONLY) > TRESHOLD) arena_l=TRUE; else arena_l=FALSE;
203 kakl 59 
      set_adc_channel(R); // prepnuti kanalu ADC, je treba min 10us na ustaleni
60 
      delay_us(10);
61 
      read_adc(ADC_START_ONLY);
62 
      set_pwm1_duty(27);      // 1:1
63 
   }
64 
   else
65 
   {
205 kakl 66 
      if (read_adc(ADC_READ_ONLY) > TRESHOLD) arena_r=TRUE; else arena_r=FALSE;
203 kakl 67 
      set_adc_channel(L); // prepnuti kanalu ADC, je treba min 10us na ustaleni
68 
      delay_us(10);
69 
      read_adc(ADC_START_ONLY);
70 
      set_pwm1_duty(55);      // 1:0
71 
   };
72 
 
73 
   if (GRAVITY) {if (g<255) g++;} else g=0;
74 
   if (g>3 && !diag) {FL; FR; while(TRUE);}; // kdyz nas preklopi, nedej se
75 
}
76 
 
77 
// Primitivni Pipani
78 
void beep(unsigned int16 period, unsigned int16 length)
79 
{
80 
   unsigned int16 nn;
81 
 
82 
   disable_interrupts(GLOBAL);
83 
   for(nn=length; nn>0; nn--)
84 
   {
85 
     output_high(SOUND_HI);output_low(SOUND_LO);
86 
     delay_us(period);
87 
     output_high(SOUND_LO);output_low(SOUND_HI);
88 
     delay_us(period);
89 
   }
90 
   enable_interrupts(GLOBAL);
91 
}
92 
 
93 
/******************************************************************************/
94 
inline void diagnostika()
95 
{
96 
   unsigned int16 n;
97 
 
98 
#ifdef DEBUG1
99 
   while (true)   // Diagnostika podvozku
100 
   {
101 
      for (n=500; n<800; n+=100)
102 
      {
103 
         beep(n,n); //beep UP
104 
      };
105 
      Delay_ms(1000);
106 
      //zastav vse
107 
      STOPL; STOPR;
108 
      //pravy pas
109 
      FR; Delay_ms(1000); STOPR; Delay_ms(1000);
110 
      BR; Delay_ms(1000); STOPR; Delay_ms(1000);
111 
      Beep(880,100); Delay_ms(1000);
112 
      //levy pas
113 
      FL; Delay_ms(1000); STOPL; Delay_ms(1000);
114 
      BL; Delay_ms(1000); STOPL; Delay_ms(1000);
115 
      Beep(880,100); Delay_ms(1000);
116 
      //oba pasy
117 
      FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
118 
      BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
119 
   };
120 
#endif
121 
 
122 
   if (GRAVITY) {
123 
   diag=TRUE;
124 
   enable_interrupts(INT_TIMER0);
125 
   enable_interrupts(GLOBAL);
126 
   while (true)         // Diagnostika cidel
127 
   {
128 
      if (g>100) beep(800,100);
129 
      Delay_ms(50);
130 
      if (arena_l) beep(1000,200);
131 
      Delay_ms(50);
132 
      if (arena_r) beep(2000,300);
133 
      Delay_ms(50);
134 
      if (sr>10) beep(3000,400);
135 
      Delay_ms(50);
136 
      if (f>10) beep(4000,500);
137 
      Delay_ms(50);
138 
      if (sl>10) beep(5000,500);
139 
      Delay_ms(50);
140 
      if (b>10) beep(6000,600);
141 
      Delay_ms(50);
142 
   }};
143 
}
144 
 
145 
void main()
146 
{
147 
   unsigned int16 n; // for FOR
148 
 
149 
   STOPL; STOPR;     // zastavi motory
150 
 
151 
   setup_oscillator(OSC_8MHZ|OSC_INTRC);     // CPU clock 8MHz
152 
   setup_adc_ports(sAN2|sAN3|VSS_VDD);   // prevodniky na cidla na okraj areny
153 
   setup_adc(ADC_CLOCK_INTERNAL);
154 
   setup_spi(FALSE);
155 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4);  // Casovac pro SW PWM a cteni cidel
156 
   setup_timer_1(T1_DISABLED);
157 
   setup_timer_2(T2_DIV_BY_1,54,1); // Casovac pro PWM pro IR sensory cca 36kHz
158 
   setup_ccp1(CCP_PWM);    // HW PWM ON
159 
   set_pwm1_duty(27);      // 1:1
160 
   setup_comparator(NC_NC_NC_NC);
161 
   setup_vref(FALSE);
162 
 
163 
   set_adc_channel(R);
164 
 
165 
   Beep(1000,200);     //double beep
166 
   Delay_ms(50);
167 
   Beep(1000,200);
168 
   diagnostika();
169 
 
170 
   enable_interrupts(INT_TIMER0);
171 
   enable_interrupts(GLOBAL);
172 
/*---------------------------------------------------------------------------*/
173 
   for (n=1;n<=3;n++)   // 5s do zacatku souboje
174 
   {
175 
      Delay_ms(990);
176 
      Beep(1000,200);
177 
   }
178 
   Delay_ms(300);
179 
 
180 
   while(true)       // hlavni smycka
181 
   {
182 
LOOP:
183 
 
184 
      GO(L, F, 150); GO(R, F, 150);
185 
 
186 
      if (arena_r)
187 
      {
188 
         BL; BR;
189 
         delay_ms(100);
190 
         STOPL; BR;
191 
         for(n=0; n<5000; n++)
192 
         {
193 
            if (!arena_r || arena_l) {BL; BR;};
194 
         };
195 
         FL; BR;
196 
         delay_ms(100);
197 
         STOPL; STOPR;
198 
      }
199 
 
200 
      if (arena_l)
201 
      {
202 
         BL; BR;
203 
         delay_ms(100);
204 
         BL; STOPR;
205 
         for(n=0; n<5000; n++)
206 
         {
207 
            if (!arena_l || arena_r) {BL; BR;};
208 
         };
209 
         BL; FR;
210 
         delay_ms(100);
211 
         STOPL; STOPR;
212 
      }
213 
 
214 
      if (sr>10)     // Nepritel vpravo
215 
      {
216 
         FL; FR;                 // popojed rovne
217 
         for(n=0; n<5000; n++)
218 
         {
219 
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
220 
         };
221 
         FL; BR;                     // otoc se na nej
222 
         for(n=0; n<10000; n++)
223 
         {
224 
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
225 
            if (f>5)
226 
            {
227 
               FL; FR;               // vytlac ho
228 
            };
229 
            if (sl>5) {BL; FR;};
230 
            if (sr>5) {FL; BR;};
231 
         };
232 
      }
233 
 
234 
      if (sl>10)     // Nepritel vlevo
235 
      {
236 
         FL; FR;                 // popojed rovne
237 
         for(n=0; n<5000; n++)
238 
         {
239 
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
240 
         };
241 
         BL; FR;                    // otoc se na nej
242 
         for(n=0; n<10000; n++)
243 
         {
244 
            if (arena_l || arena_r) {BL; BR; delay_ms(100); goto LOOP;};
245 
            if (f>5)
246 
            {
247 
               FL; FR;              // vytlac ho
248 
            };
249 
            if (sl>5) {BL; FR;};
250 
            if (sr>5) {FL; BR;};
251 
         };
252 
      }
253 
 
254 
      if (f>10)      // Nepritel vpredu
255 
      {
256 
         BL; FR;
257 
         delay_ms(110);
258 
         FL; BR;
259 
         delay_ms(50);
260 
         STOPL; STOPR;
261 
      }
262 
 
263 
      if (b>10)      // Nepritel vzadu
264 
      {
265 
         BL; FR;
266 
         delay_ms(110);
267 
         FL; BR;
268 
         delay_ms(50);
269 
         STOPL; STOPR;
270 
      }
271 
 
272 
   } // while(true)
273 
}