root / branches / encoders / code / projects / libdragonfly / encoders.c @ 674
History | View | Annotate | Download (1.62 KB)
1 |
#include "encoders.h" |
---|---|
2 |
#include "spi.h" |
3 |
#include <dragonfly_lib.h> |
4 |
|
5 |
void encoder_recv(char data); |
6 |
|
7 |
unsigned short int left_data_buf; |
8 |
unsigned short int right_data_buf; |
9 |
char buf_index;
|
10 |
|
11 |
unsigned short int left_data; |
12 |
unsigned short int right_data; |
13 |
|
14 |
void encoders_init(void){ |
15 |
spi_init(encoder_recv); |
16 |
buf_index = 0;
|
17 |
left_data_buf = 0;
|
18 |
right_data_buf= 0;
|
19 |
left_data = -1;
|
20 |
right_data = -1;
|
21 |
} |
22 |
|
23 |
int encoder_read(char encoder){ |
24 |
if(encoder==LEFT)
|
25 |
return left_data;
|
26 |
else if(encoder==RIGHT) |
27 |
return right_data;
|
28 |
else return -1; |
29 |
} |
30 |
|
31 |
int encoder_change(char encoder){ |
32 |
return 0; |
33 |
} |
34 |
|
35 |
char encoder_direction(char encoder){ |
36 |
return 0; |
37 |
} |
38 |
|
39 |
//Full reads occur every 40 microseconds. This function should be called
|
40 |
//every 8 microseconds.
|
41 |
void encoder_recv(char data){ |
42 |
if(buf_index < 2) |
43 |
right_data_buf |= (((short)data) << ((1-buf_index)*8)) & (0xF<<(1-buf_index)); |
44 |
|
45 |
else if (buf_index == 2) |
46 |
left_data_buf |= (((short)data) << 9) & (0x7F << 9); |
47 |
|
48 |
else if (buf_index == 3) |
49 |
left_data_buf |= ((short)(data) << 1) & (0xFF<<1); |
50 |
|
51 |
else if (buf_index == 4) |
52 |
left_data_buf |= (data>>7) & 0x1; |
53 |
|
54 |
|
55 |
buf_index = (buf_index + 1) % 5; |
56 |
|
57 |
if(buf_index==0){ |
58 |
if(left_data_buf & (OCF | COF | LIN))
|
59 |
left_data = INVALID; |
60 |
|
61 |
else if(((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)) |
62 |
left_data = MAGNET_FAILURE; |
63 |
|
64 |
else left_data = (left_data_buf>>5) & 1023; |
65 |
|
66 |
if(right_data_buf & (OCF | COF | LIN))
|
67 |
right_data = INVALID; |
68 |
|
69 |
else if ( ((left_data_buf & MagINCn) > 0) && ((left_data_buf & MagDECn) > 0)) |
70 |
left_data = MAGNET_FAILURE; |
71 |
|
72 |
else right_data = (right_data_buf>>5) & 1023; |
73 |
|
74 |
left_data_buf = 0;
|
75 |
right_data_buf = 0;
|
76 |
} |
77 |
} |