root / branches / encoders / code / projects / libdragonfly / encoders.c @ 771
History | View | Annotate | Download (6.77 KB)
1 | 674 | justin | #include "encoders.h" |
---|---|---|---|
2 | #include "spi.h" |
||
3 | #include <dragonfly_lib.h> |
||
4 | 675 | justin | #include "ring_buffer.h" |
5 | 495 | kwoo | |
6 | 749 | bneuman | unsigned int left_data_buf; |
7 | unsigned int right_data_buf; |
||
8 | 674 | justin | char buf_index;
|
9 | |||
10 | 749 | bneuman | unsigned int left_data; |
11 | unsigned int right_data; |
||
12 | 674 | justin | |
13 | 749 | bneuman | unsigned int left_data_array[BUFFER_SIZE]; |
14 | unsigned int right_data_array[BUFFER_SIZE]; |
||
15 | 677 | jykong | int left_data_idx;
|
16 | int right_data_idx;
|
||
17 | 675 | justin | |
18 | 677 | jykong | int left_dx;
|
19 | int right_dx;
|
||
20 | long int timecount; |
||
21 | |||
22 | 771 | bneuman | int left_v;
|
23 | int right_v;
|
||
24 | |||
25 | volatile int recv_count; |
||
26 | |||
27 | volatile short int data_ready; |
||
28 | |||
29 | 677 | jykong | void encoder_recv(char data); |
30 | |||
31 | //Helper Function Prototypes
|
||
32 | inline void left_data_array_put(unsigned short int value); |
||
33 | 749 | bneuman | inline unsigned int left_data_array_top(void); |
34 | inline unsigned int left_data_array_prev(void); |
||
35 | inline unsigned int left_data_array_bottom(void); |
||
36 | 677 | jykong | |
37 | inline void right_data_array_put(unsigned short int value); |
||
38 | 749 | bneuman | inline unsigned int right_data_array_top(void); |
39 | inline unsigned int right_data_array_prev(void); |
||
40 | inline unsigned int right_data_array_bottom(void); |
||
41 | 677 | jykong | |
42 | //RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);
|
||
43 | |||
44 | 749 | bneuman | void encoder_recv_complete(void); |
45 | |||
46 | void encoder_recv_complete(){
|
||
47 | 771 | bneuman | |
48 | int dx;
|
||
49 | |||
50 | // usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
|
||
51 | // usb_puts("\r\n");
|
||
52 | data_ready++; |
||
53 | |||
54 | delay_ms(ENCODER_DELAY); |
||
55 | |||
56 | spi_transfer(5);
|
||
57 | 749 | bneuman | } |
58 | |||
59 | 752 | bneuman | void put_bin(char data){ |
60 | int i;
|
||
61 | 749 | bneuman | |
62 | 752 | bneuman | for(i=7;i>=0;i--) |
63 | usb_puti((data>>i)&1);
|
||
64 | usb_puts(" ");
|
||
65 | } |
||
66 | |||
67 | 674 | justin | void encoders_init(void){ |
68 | 677 | jykong | int i;
|
69 | 771 | bneuman | |
70 | data_ready=0;
|
||
71 | 677 | jykong | |
72 | 752 | bneuman | spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
|
73 | 674 | justin | buf_index = 0;
|
74 | left_data_buf = 0;
|
||
75 | right_data_buf= 0;
|
||
76 | left_data = -1;
|
||
77 | right_data = -1;
|
||
78 | 771 | bneuman | |
79 | recv_count=0;
|
||
80 | |||
81 | left_v=0;
|
||
82 | right_v=0;
|
||
83 | |||
84 | 677 | jykong | //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
|
85 | left_data_idx = 0;
|
||
86 | right_data_idx = 0;
|
||
87 | for(i = 0; i < BUFFER_SIZE; i++) { |
||
88 | left_data_array[i] = 0;
|
||
89 | } |
||
90 | for(i = 0; i < BUFFER_SIZE; i++) { |
||
91 | right_data_array[i] = 0;
|
||
92 | } |
||
93 | 749 | bneuman | spi_transfer(5);
|
94 | 674 | justin | } |
95 | |||
96 | int encoder_read(char encoder){ |
||
97 | if(encoder==LEFT)
|
||
98 | return left_data;
|
||
99 | else if(encoder==RIGHT) |
||
100 | return right_data;
|
||
101 | else return -1; |
||
102 | } |
||
103 | |||
104 | int encoder_change(char encoder){ |
||
105 | return 0; |
||
106 | } |
||
107 | |||
108 | char encoder_direction(char encoder){ |
||
109 | return 0; |
||
110 | } |
||
111 | |||
112 | 677 | jykong | int encoder_get_dx(char encoder) { |
113 | if(encoder==LEFT)
|
||
114 | return left_dx;
|
||
115 | else if(encoder==RIGHT) |
||
116 | return right_dx;
|
||
117 | else return -1; |
||
118 | } |
||
119 | |||
120 | void encoder_rst_dx(char encoder) { |
||
121 | if(encoder==LEFT)
|
||
122 | left_dx = 0;
|
||
123 | else if(encoder==RIGHT) |
||
124 | right_dx = 0;
|
||
125 | } |
||
126 | |||
127 | int encoder_get_tc(void) { |
||
128 | return timecount;
|
||
129 | } |
||
130 | |||
131 | void encoder_rst_tc(void) { |
||
132 | timecount = 0;
|
||
133 | } |
||
134 | |||
135 | |||
136 | 771 | bneuman | int encoder_get_v(char encoder){ |
137 | int last, res=0; |
||
138 | |||
139 | cli(); |
||
140 | |||
141 | if(encoder==LEFT){
|
||
142 | if(left_data_idx==0) |
||
143 | last = BUFFER_SIZE - 1;
|
||
144 | else
|
||
145 | last = left_data_idx - 1;
|
||
146 | res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]); |
||
147 | } |
||
148 | if(encoder==RIGHT){
|
||
149 | if(right_data_idx==0) |
||
150 | last = BUFFER_SIZE - 1;
|
||
151 | else
|
||
152 | last = right_data_idx - 1;
|
||
153 | res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]); |
||
154 | } |
||
155 | |||
156 | sei(); |
||
157 | |||
158 | while(res<MIN_V)//underflow |
||
159 | res+=1024;
|
||
160 | while(res>MAX_V)//overflow |
||
161 | res-=1024;
|
||
162 | |||
163 | return res;
|
||
164 | } |
||
165 | |||
166 | void encoder_wait(int n){ |
||
167 | while(data_ready<n);
|
||
168 | data_ready=0;
|
||
169 | } |
||
170 | |||
171 | |||
172 | 674 | justin | //Full reads occur every 40 microseconds. This function should be called
|
173 | //every 8 microseconds.
|
||
174 | void encoder_recv(char data){ |
||
175 | 749 | bneuman | //usb_puti(buf_index);
|
176 | 677 | jykong | short int dx; |
177 | |||
178 | 771 | bneuman | recv_count++; |
179 | //if(++recv_count==ENCODER_DELAY){
|
||
180 | |||
181 | recv_count=0;
|
||
182 | |||
183 | if(buf_index == 0) |
||
184 | right_data_buf |= ((short)data)<<8 & 0xff00; |
||
185 | 752 | bneuman | |
186 | 771 | bneuman | else if (buf_index == 1) |
187 | right_data_buf |= ((short)data) & 0xff; |
||
188 | 674 | justin | |
189 | 771 | bneuman | else if (buf_index == 2) |
190 | left_data_buf |= (((short)data) << 9) & (0x7F << 9); |
||
191 | 674 | justin | |
192 | 771 | bneuman | else if (buf_index == 3) |
193 | left_data_buf |= (((short)data) << 1) & (0xFF<<1); |
||
194 | |||
195 | else if (buf_index == 4) |
||
196 | left_data_buf |= (((short)data)>>7) & 0x1; |
||
197 | 752 | bneuman | |
198 | 674 | justin | |
199 | 771 | bneuman | buf_index = (buf_index + 1) % 5; |
200 | 677 | jykong | |
201 | 771 | bneuman | if(buf_index==0) { |
202 | if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF)){
|
||
203 | left_data = INVALID; |
||
204 | } |
||
205 | |||
206 | else if(((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)){ |
||
207 | left_data = MAGNET_FAILURE; |
||
208 | } |
||
209 | else{
|
||
210 | left_data = (left_data_buf>>5) & 1023; |
||
211 | } |
||
212 | |||
213 | if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)){
|
||
214 | right_data = INVALID; |
||
215 | } |
||
216 | else if ( ((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)){ |
||
217 | left_data = MAGNET_FAILURE; |
||
218 | } |
||
219 | else{
|
||
220 | right_data = (right_data_buf>>5) & 1023; |
||
221 | } |
||
222 | |||
223 | 677 | jykong | |
224 | 771 | bneuman | //if(left_data!=INVALID || right_data!=INVALID){
|
225 | // usb_puts("{");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("}\r\n");
|
||
226 | //}
|
||
227 | |||
228 | |||
229 | |||
230 | left_data_buf = 0;
|
||
231 | right_data_buf = 0;
|
||
232 | 752 | bneuman | |
233 | 771 | bneuman | |
234 | if(left_data < INVALID) {
|
||
235 | //Put new data onto data array
|
||
236 | left_data_array_put(left_data); |
||
237 | 752 | bneuman | |
238 | 771 | bneuman | //Adjust left accumulator
|
239 | //dx = left_data - left_data_array_prev();
|
||
240 | dx = left_data_array_prev() - left_data; |
||
241 | |||
242 | //Adjust velocity: save last dx
|
||
243 | left_v = left_dx; |
||
244 | 752 | bneuman | |
245 | 771 | bneuman | /*left dx was negative on robot 7, could just be that
|
246 | the encoder is backwards, so this may need to change
|
||
247 | back
|
||
248 | */
|
||
249 | 752 | bneuman | |
250 | |||
251 | 771 | bneuman | if(left_data_array_prev()==0) |
252 | dx=0;
|
||
253 | |||
254 | if(dx > 5) { //underflow |
||
255 | left_dx += dx - 1023;
|
||
256 | } |
||
257 | else if(dx < -5) { //overflow |
||
258 | left_dx += dx + 1023;
|
||
259 | } |
||
260 | else {
|
||
261 | left_dx += dx; |
||
262 | } |
||
263 | |||
264 | //Adjust velocity: update
|
||
265 | left_v = left_dx - left_v; |
||
266 | 677 | jykong | } |
267 | |||
268 | 771 | bneuman | if(right_data < INVALID) {
|
269 | //Put new data onto data array
|
||
270 | right_data_array_put(right_data); |
||
271 | 677 | jykong | |
272 | 771 | bneuman | //Adjust right accumulator
|
273 | dx = right_data - right_data_array_prev(); |
||
274 | 752 | bneuman | |
275 | 771 | bneuman | if(right_data_array_prev()==0) |
276 | dx=0;
|
||
277 | 752 | bneuman | |
278 | 771 | bneuman | if(dx > 5) { //underflow |
279 | right_dx += dx - 1023;
|
||
280 | } |
||
281 | else if(dx < -5) { //overflow |
||
282 | right_dx += dx + 1023;
|
||
283 | } |
||
284 | else {
|
||
285 | right_dx += dx; |
||
286 | } |
||
287 | 677 | jykong | } |
288 | 771 | bneuman | |
289 | |||
290 | 677 | jykong | } |
291 | |||
292 | 771 | bneuman | //Increment timecount accumulator
|
293 | timecount++; |
||
294 | |||
295 | //}
|
||
296 | 674 | justin | } |
297 | 677 | jykong | |
298 | //Helper Functions
|
||
299 | inline void left_data_array_put(unsigned short int value) { |
||
300 | if(left_data_idx == BUFFER_SIZE-1) |
||
301 | left_data_idx = 0;
|
||
302 | else
|
||
303 | left_data_idx++; |
||
304 | left_data_array[left_data_idx] = value; |
||
305 | } |
||
306 | |||
307 | 749 | bneuman | inline unsigned int left_data_array_top(void) { |
308 | 677 | jykong | return left_data_array[left_data_idx];
|
309 | } |
||
310 | |||
311 | 749 | bneuman | inline unsigned int left_data_array_prev(void) { |
312 | 677 | jykong | if(left_data_idx == 0) |
313 | return left_data_array[BUFFER_SIZE-1]; |
||
314 | else
|
||
315 | return left_data_array[left_data_idx - 1]; |
||
316 | } |
||
317 | |||
318 | 749 | bneuman | inline unsigned int left_data_array_bottom(void) { |
319 | 677 | jykong | if(left_data_idx == BUFFER_SIZE-1) |
320 | return left_data_array[0]; |
||
321 | else
|
||
322 | return left_data_array[left_data_idx + 1]; |
||
323 | } |
||
324 | |||
325 | inline void right_data_array_put(unsigned short int value) { |
||
326 | if(right_data_idx == BUFFER_SIZE-1) |
||
327 | right_data_idx = 0;
|
||
328 | else
|
||
329 | right_data_idx++; |
||
330 | right_data_array[right_data_idx] = value; |
||
331 | } |
||
332 | |||
333 | 749 | bneuman | inline unsigned int right_data_array_top(void) { |
334 | 677 | jykong | return right_data_array[right_data_idx];
|
335 | } |
||
336 | |||
337 | 749 | bneuman | inline unsigned int right_data_array_prev(void) { |
338 | 677 | jykong | if(right_data_idx == 0) |
339 | return right_data_array[BUFFER_SIZE-1]; |
||
340 | else
|
||
341 | return right_data_array[right_data_idx - 1]; |
||
342 | } |
||
343 | |||
344 | 749 | bneuman | inline unsigned int right_data_array_bottom(void) { |
345 | 677 | jykong | if(right_data_idx == BUFFER_SIZE-1) |
346 | return right_data_array[0]; |
||
347 | else
|
||
348 | return right_data_array[right_data_idx + 1]; |
||
349 | } |