Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1
#include "tank.h"
2
 
3
unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
4
unsigned int8 line;           // na ktere strane byla detekovana cara
5
unsigned int8 speed;          // rychlost zataceni
6
unsigned int8 last;           // kde byla cara, kdyz byly minule cidla mimo
7
unsigned int8 rovinka;        // pocitadlo pro zjisteni rovneho useku
8
int cirkus;
9
 
10
// Konstanty pro dynamiku pohybu
11
#define T_DIRA       120      // po jakem case zataceni se detekuje dira
12
#define FW_POMALU    170      // trochu mimo caru vnitrni pas
13
#define FW_ZATACKA   200      // rychlost vnejsiho kola pri zataceni
14
#define FW_STREDNE   190      // trochu mimo caru vnejsi pas
15
#define COUVANI      750      // couvnuti zpet na caru, po detekci diry
16
#define MAX_ROVINKA  (255-FW_STREDNE)
17
#define TRESHOLD     15        // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd
18
#define BUMPER_TRESHOLD 128
19
 
20
//motory            //Napred vypnout potom zapnout!
21
#define FR         output_low(PIN_B5); output_high(PIN_B4)  // Vpred
22
#define FL         output_low(PIN_B7); output_high(PIN_B6)
23
#define BR         output_low(PIN_B4); output_high(PIN_B5)  // Vzad
24
#define BL         output_low(PIN_B6); output_high(PIN_B7)
25
#define STOPR      output_low(PIN_B4);output_low(PIN_B5)    // Zastav
26
#define STOPL      output_low(PIN_B6);output_low(PIN_B7)
27
 
28
#define L 0b10  // left
29
#define R 0b01  // right
30
#define S 0b11  // straight
31
 
32
//cidla
33
#define RSENSOR    !C2OUT      // Senzory na caru
34
#define LSENSOR    !C1OUT
35
#define BUMPER     PIN_A4     // Senzor na cihlu
36
 
37
#define DIAG_SERVO      PIN_B3   // Propojka pro diagnosticky mod
38
#define DIAG_SENSORS    PIN_B2   // Propojka pro diagnosticky mod
39
 
40
#DEFINE SOUND_HI   PIN_A6     // komplementarni vystupy pro piezo pipak
41
#DEFINE SOUND_LO   PIN_A7
42
 
43
char AXstring[40];   // Buffer pro prenos telemetrie
44
 
45
// makro pro PWM
46
#define GO(motor, direction, power) if(get_timer0()<=power) \
47
{direction##motor;} else {stop##motor;}
48
 
49
#int_TIMER2
50
void TIMER2_isr()
51
{
52
   if (speed<255) speed++;
53
   if (rovinka<MAX_ROVINKA) rovinka++;
54
}
55
// Primitivni Pipani
56
void beep(unsigned int16 period, unsigned int16 length)
57
{
58
   unsigned int16 nn;
59
 
60
   for(nn=length; nn>0; nn--)
61
   {
62
     output_high(SOUND_HI);output_low(SOUND_LO);
63
     delay_us(period);
64
     output_high(SOUND_LO);output_low(SOUND_HI);
65
     delay_us(period);
66
   }
67
}
68
/******************************************************************************/
69
void diagnostika()
70
{
71
   unsigned int16 n;
72
 
73
   while (input(DIAG_SERVO))   // Propojka, ktera spousti diagnostiku
74
   {
75
      for (n=500; n<800; n+=100)
76
      {
77
         beep(n,n); //beep UP
78
      };
79
      Delay_ms(1000);
80
      //zastav vse
81
      STOPL; STOPR;
82
      //pravy pas
83
      FR; Delay_ms(1000); STOPR; Delay_ms(1000);
84
      BR; Delay_ms(1000); STOPR; Delay_ms(1000);
85
      Beep(880,100); Delay_ms(1000);
86
      //levy pas
87
      FL; Delay_ms(1000); STOPL; Delay_ms(1000);
88
      BL; Delay_ms(1000); STOPL; Delay_ms(1000);
89
      Beep(880,100); Delay_ms(1000);
90
      //oba pasy
91
      FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
92
      BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
93
   };
94
   while (input(DIAG_SENSORS))         // spusteni diagnostiky cidel
95
   {
96
      if (RSENSOR) beep(1000,1000);
97
      if (LSENSOR) beep(2000,2000);
98
      if ((read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD)) beep(3000,3000);
99
   };
100
}
101
///////////////////////////////////////////////////////////////////////////////
102
void OtocSe()                     // otoci se zpet, kdyz je prekazka
103
{
104
unsigned int16 n;
105
 
106
	STOPR;STOPL;
107
	beep(800,400);
108
	beep(2000,1000);
109
	beep(900,400);
110
 
111
   BR; FL; Delay_ms(100);           // otoc se 30° do prava
112
   STOPL; STOPR;
113
   beep(1000,1000);
114
 
115
   BR; FL;
116
   for(n=40000;n>0;n--)               // toc se, dokud nenarazis na caru
117
   {
118
      line = RSENSOR;         // cteni senzoru na caru
119
      line |= LSENSOR << 1;
120
      if (line!=0) break;
121
   }
122
   STOPR; STOPL;
123
 
124
   line=L; // caru jsme prejeli, tak je vlevo
125
   cirkus=0;
126
}
127
 
128
 
129
void main()
130
{
131
   unsigned int16 n; // pro FOR
132
   unsigned int16 i;
133
 
134
   STOPL; STOPR;     // prepne vystupy na ovladani motoru na output a zastavi
135
 
136
   setup_oscillator(OSC_4MHZ|OSC_INTRC);     // 4 MHz interni RC oscilator
137
 
138
   port_b_pullups(TRUE);      // pullups pro piano na diagnostiku
139
   setup_spi(FALSE);
140
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);  // Casovac pro PWM
141
 
142
   setup_timer_2(T2_DIV_BY_4,255,10);    // Casovac pro regulaci
143
                                         // preruseni kazdych 10ms
144
   setup_adc_ports(sAN2|VSS_VDD);      // nastaveni A/D prevodniku pro naraznik
145
   setup_adc(ADC_CLOCK_INTERNAL);
146
   set_adc_channel(2);
147
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);   // Casovac pro naraznik
148
   setup_ccp1(CCP_COMPARE_RESET_TIMER);
149
   CCP_1=(2^10)-1;                        // prevod kazdou 1ms
150
 
151
   setup_comparator(A0_VR_A1_VR);   // inicializace komparatoru pro cidla cary
152
   setup_vref(VREF_HIGH|TRESHOLD);        // 32 kroku od 0.25 do 0.75 Vdd
153
 
154
   Beep(1000,200);     //double beep
155
   Delay_ms(50);
156
   Beep(1000,200);
157
   Delay_ms(1000);      // 1s
158
 
159
   // povoleni rizeni rychlosti zataceni pres preruseni
160
   enable_interrupts(INT_TIMER2);
161
   enable_interrupts(GLOBAL);
162
 
163
/*---------------------------------------------------------------------------*/
164
   sensors=S;
165
   line=S;
166
   last=S;
167
   cirkus=0;
168
//   movement=S;
169
   speed=FW_POMALU;
170
 
171
   diagnostika();
172
   Delay_ms(500);
173
   Beep(1000,200);
174
   Delay_ms(500);
175
 
176
   while(true)       // hlavni smycka (jizda podle cary)
177
   {
178
      sensors = RSENSOR;         // cteni senzoru na caru
179
      sensors |= LSENSOR << 1;
180
 
181
      if (read_adc(ADC_READ_ONLY)<BUMPER_TRESHOLD) OtocSe();
182
 
183
      switch (sensors)  // zatacej podle toho, kde vidis caru
184
      {
185
         case S:                          // rovne
186
            GO(L, F, FW_STREDNE+rovinka); GO(R, F, FW_STREDNE+rovinka);
187
            continue;
188
         case L:                          // trochu vlevo
189
            GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
190
            line=L;
191
            continue;
192
         case R:                          // trochu vpravo
193
            GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
194
            line=R;
195
            continue;
196
         default:       // kdyz jsou obe cidla mimo caru, tak pokracuj dal
197
      };
198
      rovinka=0;
199
 
200
      if (last!=line)     // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi
201
      {
202
         last=line;
203
         speed=FW_ZATACKA;
204
         cirkus++;
205
         if (cirkus>10)
206
         {
207
            STOPL; STOPR;
208
            cirkus=0;
209
            disable_interrupts(GLOBAL);
210
            beep(1000,400);
211
            for(n=3000; n>3950; n--) beep(n,10);
212
      	   beep(2000,200);
213
	         beep(900,400);
214
            for(n=2950; n<3000; n++) beep(n,10);
215
            beep(4000,400);
216
      	   beep(1000,100);
217
	         beep(3000,400);
218
            Delay_ms(1000);
219
            enable_interrupts(GLOBAL);
220
         }
221
      };
222
 
223
      if (L==line)  // kdyz jsou obe cidla mimo caru, zatoc na caru
224
      {
225
         STOPL;
226
         GO(R, F, speed);
227
      }
228
      else
229
      {
230
         STOPR;
231
         GO(L, F, speed);
232
      }
233
 
234
   } // while(true)
235
}
236