Project

General

Profile

Statistics
| Revision:

root / branches / encoders / code / projects / libdragonfly / encoders.c @ 677

History | View | Annotate | Download (4.9 KB)

1
#include "encoders.h"
2
#include "spi.h"
3
#include <dragonfly_lib.h>
4
#include "ring_buffer.h"
5

    
6
unsigned short int left_data_buf;
7
unsigned short int right_data_buf;
8
char buf_index;
9

    
10
unsigned short int left_data;
11
unsigned short int right_data;
12

    
13
unsigned short int left_data_array[BUFFER_SIZE];
14
unsigned short int right_data_array[BUFFER_SIZE];
15
int left_data_idx;
16
int right_data_idx;
17

    
18
int left_dx;
19
int right_dx;
20
long int timecount;
21

    
22
void encoder_recv(char data);
23

    
24
//Helper Function Prototypes
25
inline void left_data_array_put(unsigned short int value);
26
inline unsigned short int left_data_array_top(void);
27
inline unsigned short int left_data_array_prev(void);
28
inline unsigned short int left_data_array_bottom(void);
29

    
30
inline void right_data_array_put(unsigned short int value);
31
inline unsigned short int right_data_array_top(void);
32
inline unsigned short int right_data_array_prev(void);
33
inline unsigned short int right_data_array_bottom(void);
34

    
35
//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);
36

    
37
void encoders_init(void){
38
        int i;
39

    
40
        spi_init(encoder_recv);
41
        buf_index = 0;
42
        left_data_buf = 0;
43
        right_data_buf= 0;
44
        left_data = -1;
45
        right_data = -1;
46
        //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
47
        left_data_idx = 0;
48
        right_data_idx = 0;
49
        for(i = 0; i < BUFFER_SIZE; i++) {
50
                left_data_array[i] = 0;
51
        }
52
        for(i = 0; i < BUFFER_SIZE; i++) {
53
                right_data_array[i] = 0;
54
        }
55
}
56

    
57
int encoder_read(char encoder){
58
        if(encoder==LEFT)
59
                return left_data;
60
        else if(encoder==RIGHT)
61
                return right_data;
62
        else return -1;
63
}
64

    
65
int encoder_change(char encoder){
66
        return 0;
67
}
68

    
69
char encoder_direction(char encoder){
70
        return 0;
71
}
72

    
73
int encoder_get_dx(char encoder) {
74
        if(encoder==LEFT)
75
                return left_dx;
76
        else if(encoder==RIGHT)
77
                return right_dx;
78
        else return -1;
79
}
80

    
81
void encoder_rst_dx(char encoder) {
82
        if(encoder==LEFT)
83
                left_dx = 0;
84
        else if(encoder==RIGHT)
85
                right_dx = 0;
86
}
87

    
88
int encoder_get_tc(void) {
89
        return timecount;
90
}
91

    
92
void encoder_rst_tc(void) {
93
        timecount = 0;
94
}
95

    
96

    
97

    
98
//Full reads occur every 40 microseconds. This function should be called
99
//every 8 microseconds.
100
void encoder_recv(char data){
101
        short int dx;
102
        
103
        if(buf_index < 2)
104
                right_data_buf |= (((short)data) << ((1-buf_index)*8)) & (0xFF<<(1-buf_index));
105
        
106
        else if (buf_index == 2)
107
                left_data_buf |= (((short)data) << 9) & (0x7F << 9);
108
        
109
        else if (buf_index == 3)
110
                left_data_buf |= (((short)data) << 1) & (0xFF<<1);
111
        
112
        else if (buf_index == 4)
113
                left_data_buf |= (((short)data)>>7) & 0x1;
114

    
115
        
116
        buf_index = (buf_index + 1) % 5;
117

    
118
        if(buf_index==0){
119
                if(left_data_buf & (OCF | COF | LIN)) 
120
                        left_data = INVALID;
121
                
122
                else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)) 
123
                        left_data = MAGNET_FAILURE;
124
                
125
                else left_data = (left_data_buf>>5) & 1023;
126
                
127
                if(right_data_buf & (OCF | COF | LIN)) 
128
                        right_data = INVALID;
129
                
130
                else if ( ((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)) 
131
                        left_data = MAGNET_FAILURE;
132
                
133
                else right_data = (right_data_buf>>5) & 1023;
134
        
135
                                                        
136
                
137
                left_data_buf = 0;
138
                right_data_buf = 0;
139
        }
140

    
141
        
142
        if(left_data < INVALID) {
143
                //Put new data onto data array
144
                left_data_array_put(left_data);
145

    
146
                //Adjust left accumulator
147
                dx = left_data - left_data_array_prev();
148
                if(dx > 5) { //underflow
149
                        left_dx += dx - 1023;
150
                }
151
                else if(dx < -5) { //overflow
152
                        left_dx += dx + 1023;
153
                }
154
                else {
155
                        left_dx += dx;
156
                }
157
        }
158

    
159
        if(right_data < INVALID) {
160
                //Put new data onto data array
161
                right_data_array_put(right_data);
162

    
163
                //Adjust right accumulator
164
                dx = right_data - right_data_array_prev();
165
                if(dx > 5) { //underflow
166
                        right_dx += dx - 1023;
167
                }
168
                else if(dx < -5) { //overflow
169
                        right_dx += dx + 1023;
170
                }
171
                else {
172
                        right_dx += dx;
173
                }
174
        }
175

    
176
        //Increment timecount accumulator
177
        timecount++;
178
}
179

    
180
//Helper Functions
181
inline void left_data_array_put(unsigned short int value) {
182
        if(left_data_idx == BUFFER_SIZE-1)
183
                left_data_idx = 0;
184
        else
185
                left_data_idx++;
186
        left_data_array[left_data_idx] = value;
187
}
188

    
189
inline unsigned short int left_data_array_top(void) {
190
        return left_data_array[left_data_idx];
191
}
192

    
193
inline unsigned short int left_data_array_prev(void) {
194
        if(left_data_idx == 0)
195
                return left_data_array[BUFFER_SIZE-1];
196
        else
197
                return left_data_array[left_data_idx - 1];
198
}
199

    
200
inline unsigned short int left_data_array_bottom(void) {
201
        if(left_data_idx == BUFFER_SIZE-1)
202
                return left_data_array[0];
203
        else
204
                return left_data_array[left_data_idx + 1];
205
}
206

    
207
inline void right_data_array_put(unsigned short int value) {
208
        if(right_data_idx == BUFFER_SIZE-1)
209
                right_data_idx = 0;
210
        else
211
                right_data_idx++;
212
        right_data_array[right_data_idx] = value;
213
}
214

    
215
inline unsigned short int right_data_array_top(void) {
216
        return right_data_array[right_data_idx];
217
}
218

    
219
inline unsigned short int right_data_array_prev(void) {
220
        if(right_data_idx == 0)
221
                return right_data_array[BUFFER_SIZE-1];
222
        else
223
                return right_data_array[right_data_idx - 1];
224
}
225

    
226
inline unsigned short int right_data_array_bottom(void) {
227
        if(right_data_idx == BUFFER_SIZE-1)
228
                return right_data_array[0];
229
        else
230
                return right_data_array[right_data_idx + 1];
231
}