Subversion Repositories svnkaklik

Rev

Details | Last modification | View Log

Rev Author Line No. Line
2 kaklik 1
#include ".\main.h"
2
 
3
#use rtos (timer=2, minor_cycle=2ms)
4
 
5
unsigned int8 sensors;        // pomocna promenna pro cteni cidel na caru
6
unsigned int8 line;           // na ktere strane byla detekovana cara
7
unsigned int8 dira;           // pocita dobu po kterou je ztracena cara
8
unsigned int8 uhel;           // urcuje aktualni uhel zataceni
9
 
10
signed int16  Lmotor;                 // promene, ktere urcuji velikost vykonu na levem
11
signed int16  Rmotor;                 // a pravem motoru
12
 
13
#define  LMAX        190      // maximalni zatoceni doleva
14
#define  RMAX        60       // maximalni zatoceni doprava
15
#define  STRED       128      // sredni poloha zadniho kolecka
16
 
17
// servo
18
#define  SERVO PIN_A2
19
 
20
//motory
21
#define  FR         output_low(PIN_A7); output_high(PIN_A6)  // Vpred
22
#define  FL         output_low(PIN_A1); output_high(PIN_A0)
23
#define  BR         output_low(PIN_A6); output_high(PIN_A7)  // Vzad
24
#define  BL         output_low(PIN_A0); output_high(PIN_A1)
25
#define  STOPR      output_low(PIN_A6);output_low(PIN_A7)
26
#define  STOPL      output_low(PIN_A0);output_low(PIN_A1)
27
 
28
//HID
29
#define  LED1     PIN_B1      //oranzova
30
#define  LED2     PIN_B2      //zluta
31
 
32
#define  STROBE   PIN_B0
33
//#define  SW1      PIN_A2      // Motory On/off
34
 
35
// makro pro PWM
36
#define GO(motor, direction, power) if(get_timer0()<=power) \
37
{direction##motor;} else {stop##motor;}
38
 
39
 
40
////////////////////////////////////////////////////////////////////////////////
41
////////////////////////////////////////////////////////////////////////////////
42
/*
43
void diagnostika()
44
{
45
   if(input(SW1))STOPR;STOPL;While(TRUE);
46
//   if(LSENSOR==true) output_high(LED2); else output_low(LED2);
47
//   if(RSENSOR==true) output_high(LED1); else output_low(LED1);
48
}
49
*/
50
////////////////////////////////////////////////////////////////////////////////
51
#task (rate=16ms,max=2ms)
52
void zatoc()   // ridi servo ktere otaci kolem
53
{
54
unsigned int8 n;
55
   output_high(SERVO);
56
   delay_us(1200);
57
   for(n=uhel; n>0; n--);
58
   output_low(SERVO);
59
}
60
 
61
////////////////////////////////////////////////////////////////////////////////
62
#task (rate=2ms, max=500us)
63
void motory()        // umoznuje nezavisle rizeni vykonu na obou motorech
64
{
65
 
66
   if(Rmotor>0)                  //pravy motor
67
   {
68
      GO(R, F, Rmotor);
69
   }
70
 
71
   if(Rmotor==0) STOPR;
72
 
73
   if(Rmotor<0)
74
   {
75
      GO(R, B, abs(Rmotor));
76
   }
77
 
78
   if(Lmotor>0)                  // levy motor
79
   {
80
      GO(L, F, Lmotor);
81
   }
82
 
83
   if(Lmotor==0) STOPL;
84
 
85
   if(Lmotor<0)
86
   {
87
      GO(L, B, abs(Lmotor));
88
   }
89
}
90
////////////////////////////////////////////////////////////////////////////////
91
#task (rate=4ms, max=500us, queue=0)
92
void rizeni()
93
{
94
rizeni:
95
 
96
   sensors = spi_read(0);         // cteni senzoru na caru
97
   output_high(STROBE);
98
   output_low(STROBE);
99
 
100
   if(bit_test(sensors,0)) //*......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
101
   {
102
      uhel=LMAX;
103
      Lmotor=60;Rmotor=127;
104
      goto rizeni;
105
   }
106
   if(bit_test(sensors,6)) //......*//
107
   {
108
      uhel=RMAX;
109
      Rmotor=60;Lmotor=127;
110
      goto rizeni;
111
   }
112
   if(bit_test(sensors,1)) //.*.....//
113
   {
114
      uhel=STRED+30;
115
      Lmotor=70;Rmotor=127;
116
      goto rizeni;
117
   }
118
   if(bit_test(sensors,5)) //.....*.//
119
   {
120
      uhel=STRED-30;
121
      Rmotor=70;Lmotor=127;
122
      goto rizeni;
123
   }
124
   if(bit_test(sensors,2)) //..*....//
125
   {
126
      uhel=STRED+20;
127
      Lmotor=90;Rmotor=127;
128
      goto rizeni;
129
   }
130
   if(bit_test(sensors,4)) //....*..//
131
   {
132
      uhel=STRED-20;
133
      Rmotor=90;Lmotor=127;
134
      goto rizeni;
135
   }
136
   if(bit_test(sensors,3)) //...*...//
137
   {
138
      uhel=STRED;
139
      Lmotor=100;Rmotor=100;
140
      goto rizeni;
141
   }
142
 
143
/*STOPL;STOPR;
144
      if(bit_test(sensors,7))
145
      {
146
 
147
      }*/
148
}
149
////////////////////////////////////////////////////////////////////////////////
150
void main()
151
{
152
   setup_adc_ports(NO_ANALOGS);
153
   setup_adc(ADC_CLOCK_INTERNAL);
154
   setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
155
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
156
   setup_timer_1(T1_DISABLED);
157
   setup_oscillator(OSC_8MHZ|OSC_INTRC);
158
//  set_pwm1_duty(0);       // Spust PWM, ale zatim s trvalou 0 na vystupu
159
//   setup_ccp1(CCP_PWM);
160
//   setup_timer_2(T2_DIV_BY_16,200,1); // perioda
161
//   setup_timer_2(T2_DIV_BY_4,255,10);    // Casovac pro regulaci
162
 
163
   STOPR; STOPL;
164
   uhel=STRED;
165
//      diagnostika();
166
Lmotor=0;
167
Rmotor=0;
168
   rtos_run();
169
 
170
}