Revision 674
Wrote encoders.c and encoders.h Would GREATLY appreciate it if someone could check if I got the receive function correct. There was a lot of bit twiddling. Useful
functions still need to be written, however receiving raw encoder readings should work (not tested... so don't bet on it)
encoders.c | ||
---|---|---|
1 |
#include "encoders.h" |
|
2 |
#include "spi.h" |
|
3 |
#include <dragonfly_lib.h> |
|
1 | 4 |
|
2 |
//Dummy file for now. |
|
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 |
} |
Also available in: Unified diff