Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / libdragonfly / encoders.c @ 890

History | View | Annotate | Download (6.51 KB)

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

    
6
unsigned int left_data_buf;
7
unsigned int right_data_buf;
8
char encoder_buf_index;
9

    
10
unsigned int left_data;
11
unsigned int right_data;
12

    
13
unsigned int left_data_array[BUFFER_SIZE];
14
unsigned 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
int left_v;
23
int right_v;
24

    
25

    
26
volatile short int data_ready;
27

    
28
void encoder_recv(char data);
29

    
30
//Helper Function Prototypes
31
inline void left_data_array_put(unsigned short int value);
32
inline unsigned int left_data_array_top(void);
33
inline unsigned int left_data_array_prev(void);
34
inline unsigned int left_data_array_bottom(void);
35

    
36
inline void right_data_array_put(unsigned short int value);
37
inline unsigned int right_data_array_top(void);
38
inline unsigned int right_data_array_prev(void);
39
inline unsigned int right_data_array_bottom(void);
40

    
41

    
42
void encoder_recv_complete(void){
43
          encoder_buf_index = 0;
44
        data_ready++;
45
  
46
           spi_transfer(5);
47
}
48

    
49
/** 
50
* @brief Initializes encoder variables and the hardware interface.
51
*/
52
void encoders_init(void){
53
        int i;
54
        
55
        data_ready=0;
56

    
57
        spi_init(encoder_recv, encoder_recv_complete);
58
        encoder_buf_index = 0;
59
        left_data_buf = 0;
60
        right_data_buf= 0;
61
        left_data = -1;
62
        right_data = -1;
63
        
64
        
65
        left_v=0;
66
        right_v=0;
67
        
68
        //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
69
        left_data_idx = 0;
70
        right_data_idx = 0;
71
        for(i = 0; i < BUFFER_SIZE; i++) {
72
                left_data_array[i] = 0;
73
        }
74
        for(i = 0; i < BUFFER_SIZE; i++) {
75
                right_data_array[i] = 0;
76
        }
77
        spi_transfer(5);
78
}
79

    
80
/**
81
 * @brief Returns the specified encoders value
82
 *
83
 * @param encoder this is the encoder that you want to read. Valid arguments
84
 *          are LEFT and RIGHT
85
 *
86
 * @return the value of the specified encoder
87
 **/
88
int encoder_read(char encoder){
89
        
90
        if(encoder==LEFT) return left_data;
91
        else if(encoder==RIGHT) return right_data;
92
        else return -1;
93
}
94

    
95
/**
96
 * Gets the total distance covered by the specified encoder (in encoder clicks)
97
 *
98
 * @param encoder the encoder that you want to read, use LEFT or RIGHT
99
 *
100
 * @return The distance covered by the specified encoder.
101
 **/
102
int encoder_get_dx(char encoder) {
103
        
104
        if(encoder==LEFT) return left_dx;
105
        else if(encoder==RIGHT) return right_dx;
106
        else return -1;
107
}
108

    
109
/**
110
 * Resets the distance accumulator for the specified
111
 *  encoder.
112
 *
113
 * @param encoder the encoder that you want to reset distance for
114
 **/
115
void encoder_rst_dx(char encoder) {
116
        
117
        if(encoder==LEFT) left_dx = 0;
118
        else if(encoder==RIGHT) right_dx = 0;
119
}
120

    
121
/** 
122
* @brief Returns the number of encoder reads that have occurred.
123
* 
124
* @return The time count.
125
*/
126
int encoder_get_tc(void) {
127
        return timecount;
128
}
129

    
130
/** 
131
* @brief Resets the encoder read counter.
132
*/
133
void encoder_rst_tc(void) {
134
        timecount = 0;
135
}
136

    
137
/** 
138
* @brief Waits until n encoder reads have occurred.
139
* Counter is reset on functions exit.
140
* 
141
* @param n 
142
*/
143
void encoder_wait(int n){
144
        while(data_ready<n);
145
        data_ready=0;
146
}
147

    
148

    
149
//Full reads occur every 40 microseconds. This function should be called
150
//every 8 microseconds.
151
void encoder_recv(char data){
152
        short int dx;
153
        
154
        //Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
155
        // second is offset by 1 bit.
156
        switch(encoder_buf_index){        
157
        case 0: 
158
                right_data_buf |= ((short)data)<<8 & 0xff00;
159
                break;
160
        case 1:
161
                right_data_buf |= ((short)data) & 0xff;
162
                break;
163
        case 2:
164
                left_data_buf |= (((short)data) << 9) & (0x7F << 9);
165
                break;
166
        case 3:
167
                left_data_buf |= (((short)data) << 1) & (0xFF<<1);
168
                break;
169
        case 4: left_data_buf |= (((short)data)>>7) & 0x1;
170
        }        
171
                
172
        encoder_buf_index = (encoder_buf_index + 1) % 5;
173

    
174
        if(encoder_buf_index==0) {
175
                
176
                /*Error handling for the left encoder*/ 
177
                if(!(left_data_buf & OCF)) 
178
                        left_data = ENCODER_DATA_NOT_READY; 
179
                if(left_data_buf & (COF | LIN))  
180
                        left_data = ENCODER_MISALIGNED;
181
                else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
182
                        left_data = ENCODER_MAGNET_FAILURE;
183
                else left_data = (left_data_buf>>5) & 1023;
184
                        
185
                /*Error handling for the right encoder*/ 
186
                 if(!(right_data_buf & OCF))
187
                        right_data = ENCODER_DATA_NOT_READY;        
188
                if(right_data_buf & (COF | LIN)) 
189
                        right_data = ENCODER_MISALIGNED;
190
                else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
191
                        right_data = ENCODER_MAGNET_FAILURE;
192
                else right_data = (right_data_buf>>5) & 1023;
193
                        
194
                  left_data_buf = 0;
195
                right_data_buf = 0;
196

    
197
                /*Note: Above 1023 is invalid data*/        
198
                if(!(left_data > 1023)) {
199
                        //Reverse the left wheel since encoders are necessarily mounted backwards.
200
                        left_data = 1023 - left_data;
201
                        left_data_array_put(left_data);
202

    
203
                        //Adjust left accumulator
204
                        dx = left_data - left_data_array_prev();
205
                        
206
                        //Adjust velocity: save last dx
207
                        left_v = left_dx;
208
                        
209
                        if(left_data_array_prev()==0)  dx=0;
210

    
211
                        if(dx > 512) left_dx += dx - 1023; //Underflow
212
                        else if(dx < -512) left_dx += dx + 1023; //Overflow
213
                        else left_dx += dx;
214
                        
215
                        //Adjust velocity: update
216
                        left_v = left_dx - left_v;
217
                }
218

    
219
                /*Above 1023 is invalid data*/        
220
                if(!(right_data > 1023)) {
221
                        right_data_array_put(right_data);
222

    
223
                        //Adjust right accumulator
224
                        dx = right_data - right_data_array_prev();
225

    
226
                        if(right_data_array_prev()==0) dx=0;
227
                          
228
                        if(dx > 512) right_dx += dx - 1023; //underflow
229
                        else if(dx < -512) right_dx += dx + 1023; //overflow 
230
                        else right_dx += dx;
231
                }
232
        }
233

    
234
        //Increment timecount accumulator
235
        timecount++;
236
}
237

    
238

    
239
//Helper Functions
240
inline void left_data_array_put(unsigned short int value) {
241
        if(left_data_idx == BUFFER_SIZE-1)
242
                left_data_idx = 0;
243
        else
244
                left_data_idx++;
245
        left_data_array[left_data_idx] = value;
246
}
247

    
248
inline unsigned int left_data_array_top(void) {
249
        return left_data_array[left_data_idx];
250
}
251

    
252
inline unsigned int left_data_array_prev(void) {
253
        if(left_data_idx == 0)
254
                return left_data_array[BUFFER_SIZE-1];
255
        else
256
                return left_data_array[left_data_idx - 1];
257
}
258

    
259
inline unsigned int left_data_array_bottom(void) {
260
        if(left_data_idx == BUFFER_SIZE-1)
261
                return left_data_array[0];
262
        else
263
                return left_data_array[left_data_idx + 1];
264
}
265

    
266
inline void right_data_array_put(unsigned short int value) {
267
        if(right_data_idx == BUFFER_SIZE-1)
268
                right_data_idx = 0;
269
        else
270
                right_data_idx++;
271
        right_data_array[right_data_idx] = value;
272
}
273

    
274
inline unsigned int right_data_array_top(void) {
275
        return right_data_array[right_data_idx];
276
}
277

    
278
inline unsigned int right_data_array_prev(void) {
279
        if(right_data_idx == 0)
280
                return right_data_array[BUFFER_SIZE-1];
281
        else
282
                return right_data_array[right_data_idx - 1];
283
}
284

    
285
inline unsigned int right_data_array_bottom(void) {
286
        if(right_data_idx == BUFFER_SIZE-1)
287
                return right_data_array[0];
288
        else
289
                return right_data_array[right_data_idx + 1];
290
}
291