Subversion Repositories svnkaklik

Rev

Go to most recent revision | 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  STRED       128      // sredni poloha zadniho kolecka
14 
#define  BEAR1       20       // 3 stupne zataceni
15 
#define  BEAR2       40
16 
#define  BEAR3       60
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 
 
56 
   output_high(SERVO);
57 
   delay_us(1100);
58 
   for(n=uhel; n>0; n--);
59 
   output_low(SERVO);
60 
}
61 
 
62 
 
63 
void diag()   // ridi servo ktere otaci kolem
64 
{
65 
   unsigned int8 n;
66 
 
67 
   output_high(SERVO);
68 
   delay_us(1100);
69 
   for(n=uhel; n>0; n--);
70 
   output_low(SERVO);
71 
}
72 
 
73 
////////////////////////////////////////////////////////////////////////////////
74 
#task (rate=2ms, max=1ms)
75 
void rizeni()
76 
{
77 
   GO(L,F,Lmotor);GO(R,F,Rmotor);
78 
 
79 
   delay_us(500);
80 
 
81 
   output_low(STROBE);
82 
   sensors = spi_read(0);         // cteni senzoru na caru
83 
   output_high(STROBE);
84 
 
85 
   if(bit_test(sensors,0)) //|......//     // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
86 
   {
87 
      uhel=STRED+BEAR3;
88 
      Lmotor=0;Rmotor=190;
89 
      return;
90 
   }
91 
 
92 
   if(bit_test(sensors,6)) //......|//
93 
   {
94 
      Rmotor=0;Lmotor=190;
95 
      uhel=STRED-BEAR3;
96 
      return;
97 
 
98 
   }
99 
 
100 
   if(bit_test(sensors,1)) //.|.....//
101 
   {
102 
      uhel=STRED+BEAR2;
103 
      Lmotor=150;Rmotor=190;
104 
      return;
105 
   }
106 
 
107 
   if(bit_test(sensors,5)) //.....|.//
108 
   {
109 
      uhel=STRED-BEAR2;
110 
      Rmotor=150;Lmotor=190;
111 
      return;
112 
   }
113 
 
114 
   if (bit_test(sensors,2)) //..|....//
115 
   {
116 
      uhel=STRED+BEAR1;
117 
      Lmotor=150;Rmotor=190;
118 
      return;
119 
   }
120 
 
121 
   if (bit_test(sensors,4)) //....|..//
122 
   {
123 
      uhel=STRED-BEAR1;
124 
      Rmotor=150;Lmotor=190;
125 
      return;
126 
   }
127 
 
128 
   if(bit_test(sensors,3)) //...|...//
129 
   {
130 
      uhel=STRED;
131 
      Lmotor=190;Rmotor=190;
132 
      return;
133 
   }
134 
 
135 
/*STOPL;STOPR;
136 
      if(bit_test(sensors,7))
137 
      {
138 
 
139 
      }*/
140 
}
141 
 
142 
 
143 
////////////////////////////////////////////////////////////////////////////////
144 
void main()
145 
{
146 
   unsigned int16 i;
147 
 
148 
   setup_adc_ports(NO_ANALOGS);
149 
   setup_adc(ADC_CLOCK_INTERNAL);
150 
   setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
151 
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
152 
   setup_timer_1(T1_DISABLED);
153 
   setup_oscillator(OSC_8MHZ|OSC_INTRC);
154 
 
155 
   STOPR; STOPL;
156 
   uhel=STRED;
157 
 
158 
   for(i=0;i<100;i++)
159 
   {
160 
      diag();
161 
      delay_ms(16);
162 
   }
163 
//      diagnostika();
164 
Lmotor=0;
165 
Rmotor=0;
166 
   rtos_run();
167 
 
168 
}