Subversion Repositories svnkaklik

Rev

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

Rev Author Line No. Line
2 kaklik 1 
#include "tank.h"
2 
 
3 
#define  TXo PIN_B1           // To the transmitter modulator
4 
#include "AX25.c"             // podprogram pro prenos telemetrie
5 
 
6 
//motory            //Napred vypnout potom zapnout!
7 
#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred
8 
#define FL         output_low(PIN_B7); output_high(PIN_B6)
9 
#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad
10 
#define BL         output_low(PIN_B6); output_high(PIN_B7)
11 
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)
12 
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)
13 
 
14 
#define 	L 0b10  // left
15 
#define 	R 0b01  // right
16 
#define 	S 0b11  // straight
17 
 
18 
#define 	COUVANI		1600					// couvnuti po zjisteni diry
19 
#define 	MEZERA		5400					// za jak dlouho bude ztracena cara
20 
#define 	PRES_DIRU	1000					// velikost mezery v care
21 
#define	BRZDENI		5000				// doba ptrebna k zastaveni jednoho motoru
22 
 
23 
//cidla
24 
#define 	RSENSOR     C2OUT       // Senzory na caru
25 
#define 	LSENSOR     C1OUT
26 
#define	BUMPER	   PIN_A4		// sensor na cihlu
27 
 
28 
#define DIAG_SERVO      PIN_B2   // Propojka pro diagnosticky mod
29 
#define DIAG_SENSORS    PIN_B3   // Propojka pro diagnosticky mod
30 
 
31 
#DEFINE SOUND_HI   PIN_A7
32 
#DEFINE SOUND_LO   PIN_A6
33 
 
34 
char AXstring[40];   // Buffer pro prenos telemetrie
35 
 
36 
#define GO(motor, direction, power) if(get_timer0()<=power) \
37 
                                    {direction##motor;} \
38 
                                    else \
39 
                                    {stop##motor;}
40 
 
41 
int movement;     // smer minuleho pohybu
42 
int line;         // na ktere strane byla detekovana cara
43 
unsigned int16 dira;			// pocitadlo pro nalezeni preruseni cary
44 
int speed,speedL,speedR;
45 
 
46 
// Primitivni Pipani
47 
void beep(unsigned int16 period, unsigned int16 length)
48 
{
49 
   unsigned int16 nn;
50 
 
51 
   for(nn=length; nn>0; nn--)
52 
   {
53 
     output_high(SOUND_HI);output_low(SOUND_LO);
54 
     delay_us(period);
55 
     output_high(SOUND_LO);output_low(SOUND_HI);
56 
     delay_us(period);
57 
   }
58 
}
59 
#int_TIMER2
60 
void TIMER2_isr()
61 
{
62 
   switch(line)             // upravime smer
63 
   {
64 
   case S:                  //obe cidla na care
65 
      if(speedL<200)speedL++;
66 
      if(speedR<200)speedR++;
67 
      break;               // vrat se zpet na cteni cidel
68 
   case L:                  // cara je pod levym cidlem, trochu zatocime
69 
      if (speedL>100)speedL -- ;
70 
      if (speedR<200)speedR ++ ;
71 
      break;
72 
   case R:                  // cara pod pravym cidlem
73 
      if (speedR>100)speedR -- ;
74 
      if (speedL<200)speedL ++ ;
75 
      break;
76 
   default:
77 
   }
78 
}
79 
// Diagnostika pohonu, hejbne vsema motorama ve vsech smerech
80 
void diagnostika()
81 
{
82 
   unsigned int16 n;
83 
 
84 
   while (input(DIAG_SERVO))   // Propojka, ktera spousti diagnostiku
85 
   {
86 
      for (n=500; n<800; n+=100)
87 
      {
88 
         beep(n,n); //beep UP
89 
      };
90 
      Delay_ms(1000);
91 
      //zastav vse
92 
      STOPL; STOPR;
93 
      //pravy pas
94 
      FR; Delay_ms(1000); STOPR; Delay_ms(1000);
95 
      BR; Delay_ms(1000); STOPR; Delay_ms(1000);
96 
      Beep(880,100); Delay_ms(1000);
97 
      //levy pas
98 
      FL; Delay_ms(1000); STOPL; Delay_ms(1000);
99 
      BL; Delay_ms(1000); STOPL; Delay_ms(1000);
100 
      Beep(880,100); Delay_ms(1000);
101 
      //oba pasy
102 
      FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
103 
      BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
104 
   };
105 
 
106 
   while (input(DIAG_SENSORS))
107 
   {
108 
      int ls, rs;
109 
		while(!input(BUMPER)){beep(1100,100); Delay_ms(50);}
110 
      set_adc_channel(RSENSOR);
111 
      Delay_us(20);
112 
      rs=read_adc();
113 
      set_adc_channel(LSENSOR);
114 
      Delay_us(20);
115 
      ls=read_adc();
116 
      sprintf(AXstring,"L: %U  R: %U\0", ls, rs);  // Convert DATA to String.
117 
      SendPacket(&AXstring[0]);
118 
      delay_ms(1000);
119 
   };
120 
}
121 
 
122 
void cikcak()
123 
{
124 
int n;
125 
	switch(movement)									// podivej se na jednu stranu
126 
	{
127 
	case L:
128 
				FL;BR;
129 
				movement=R;
130 
			break;
131 
	case R:
132 
				FR;BL;
133 
				movement=L;
134 
			break;
135 
	case S:
136 
				FR;BL;
137 
				movement=L;
138 
			break;
139 
	}
140 
	while (0==(RSENSOR|LSENSOR))
141 
	{
142 
		if (n==50)												// asi bude na druhe strane
143 
		{
144 
			STOPR;STOPL;
145 
			n=0;
146 
			switch(movement)
147 
			{
148 
			case L:
149 
						FL;BR;
150 
						movement=R;
151 
					break;
152 
			case R:
153 
						FR;BL;
154 
						movement=L;
155 
					break;
156 
			}
157 
		}
158 
		Delay_ms(5);
159 
		n++;
160 
	}
161 
	STOPL;STOPR;												// nasli jsme caru
162 
	line=S;
163 
}
164 
void objizdka()
165 
{
166 
	BL;BR;Delay_ms(300);
167 
	STOPR;STOPL;
168 
	beep(1000,1000);
169 
	Delay_ms(500);
170 
	beep(1000,1000);
171 
	Delay_ms(1000);
172 
 
173 
 
174 
 
175 
}
176 
 
177 
void pozordira()
178 
{
179 
			beep(800,500);
180 
			Delay_ms(50);
181 
			beep(800,500);
182 
			switch (movement)										//vrat se zpet na caru
183 
			{
184 
		  	case L:
185 
						STOPL;STOPR;
186 
        				BR;Delay_ms(COUVANI);STOPR;
187 
					break;
188 
        	case R:
189 
						STOPL;STOPR;
190 
        				BL;Delay_ms(COUVANI);STOPL;
191 
					break;
192 
			case S:
193 
						BL; BR; Delay_ms(COUVANI);
194 
						STOPL; STOPR;
195 
					break;
196 
			}
197 
 
198 
			FR;FL; Delay_ms(PRES_DIRU);	// popojedem dopredu mozna tam bude cara
199 
			STOPL; STOPR; movement=S;
200 
			cikcak();												// najdi caru
201 
			dira=0;
202 
}
203 
 
204 
void main()
205 
{
206 
   unsigned int16 rovinka;
207 
   int last;
208 
 
209 
   STOPL; STOPR;
210 
 
211 
   setup_oscillator(OSC_4MHZ|OSC_INTRC);     // 4 MHz interni RC oscilator
212 
   port_b_pullups(true);
213 
   setup_adc_ports(NO_ANALOGS|VSS_VDD);
214 
   setup_adc(ADC_OFF);
215 
   setup_spi(FALSE);
216 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM
217 
   setup_timer_1(T1_DISABLED);   // Casovac pro regulaci
218 
   setup_timer_2(T2_DIV_BY_16,50,16);
219 
   setup_ccp1(CCP_OFF);
220 
   setup_comparator(A0_VR_A1_VR);
221 
   setup_vref(VREF_HIGH|15);
222 
   enable_interrupts(INT_TIMER2);
223 
   enable_interrupts(GLOBAL);
224 
 
225 
	diagnostika();
226 
 
227 
   Beep(1000,200);     //double beep
228 
   Delay_ms(50);
229 
   Beep(1000,200);
230 
   Delay_ms(1000); // 1s
231 
 
232 
//	FL; FR;
233 
   movement=S;
234 
   line=S;
235 
   dira=0;
236 
   last=0;
237 
   rovinka=0;
238 
 
239 
   speed=speedL=speedR=200;
240 
 
241 
   while(true)
242 
   {
243 
      line = RSENSOR;			// precteni cidel
244 
      line |= LSENSOR << 1;      // sestav informaci o care
245 
 
246 
      if(line!=0)
247 
      {
248 
         GO(L, F, speedL); GO(R,F, speedR);
249 
      }
250 
      else{STOPR; STOPL;}
251 
//sem:
252 
/*		switch(line)             // upravime smer
253 
		{
254 
		case S:                  //obe cidla na care
255 
//         if (speedL<speedR) speedL=speedR;
256 
//         else speedR=speedL;
257 
			GO(L,F,speedL); GO(R,F,speedR)                // jedeme rovne
258 
//         if(rovinka<BRZDENI) rovinka++; //cara je rovne
259 
//         dira=0;                 // videli jsme caru, proto neni dira
260 
			continue;               // vrat se zpet na cteni cidel
261 
		case L:                  // cara je pod levym cidlem, trochu zatocime
262 
         GO(L, F, speedL); GO(R,F, speedR);
263 
//         if(rovinka<BRZDENI) rovinka++;   //cara je celkem rovne
264 
//         dira=0;
265 
			continue;
266 
		case R:                  // cara pod pravym cidlem
267 
         GO(R, F, speedR); GO(L, F, speedL);
268 
//         if(rovinka<BRZDENI) rovinka++;
269 
//         dira=0;
270 
			continue;
271 
		default:                 // cara neni pod zadnym cidlem
272 
		}*/
273 
 
274 
 
275 
      /*switch (last)                          // zatacka
276 
      {
277 
      case L:                                 // do leva
278 
            BL;STOPR;                     //zabrzdeni leveho motoru
279 
            for(;rovinka>0;rovinka--) //chvili pockej
280 
            {
281 
               line = RSENSOR;         // precteni cidel
282 
               line |= LSENSOR << 1;      // sestav informaci o care
283 
               if(line!=0) goto sem;  //kdyz najdes caru, zastav
284 
            }
285 
            STOPL; FR;       // pokracuj v zataceni
286 
            while(line==0)
287 
            {
288 
               line = RSENSOR;         // precteni cidel
289 
               line |= LSENSOR << 1;      // sestav informaci o care
290 
            }
291 
            movement=L;
292 
            rovinka=0;                //zataceli jsme, uz neni rovna cara
293 
           break;
294 
      case R:
295 
            BR; STOPL;                      // zabrzdeni praveho motoru
296 
            for(;rovinka>0;rovinka--)
297 
            {
298 
               line = RSENSOR;         // precteni cidel
299 
               line |= LSENSOR << 1;      // sestav informaci o care
300 
               if(line!=0) goto sem;
301 
            }
302 
            STOPR; FL;
303 
            while(line==0)
304 
            {
305 
               line = RSENSOR;         // precteni cidel
306 
               line |= LSENSOR << 1;      // sestav informaci o care
307 
            }
308 
            movement=R;
309 
            rovinka=0;                //zataceli jsme, uz neni rovna cara
310 
           break;
311 
      }*/
312 
   } // while(true)
313 
}