Statistics
| Revision:

root / demos / hunter_prey / lib / src / libdragonfly / encoders.c @ 1828

History | View | Annotate | Download (8.44 KB)

1
#include "encoders.h"
2
#include "spi.h"
3
#include "ring_buffer.h"
4
#include <avr/io.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
volatile short int data_ready;
23

    
24
void encoder_recv(char data);
25

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

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

    
37
//encoder_get_v helper function prototypes
38
int left_data_at(int index);
39
int right_data_at(int index);
40
int get_dx(char encoder, int index);
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
        //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
65
        left_data_idx = 0;
66
        right_data_idx = 0;
67
        for(i = 0; i < BUFFER_SIZE; i++) {
68
                left_data_array[i] = 0;
69
        }
70
        for(i = 0; i < BUFFER_SIZE; i++) {
71
                right_data_array[i] = 0;
72
        }
73
        spi_transfer(5);
74
}
75

    
76
/**
77
 * @brief Returns the specified encoders value
78
 *
79
 * @param encoder this is the encoder that you want to read. Valid arguments
80
 *          are LEFT and RIGHT
81
 *
82
 * @return the value of the specified encoder.
83
 *         -1 usually means low battery.
84
 *         values above ENCODER_MAX usually means phyiscal problems with
85
 *         the encoder.
86
 **/
87
int encoder_read(char encoder){
88
        
89
        if(encoder==LEFT) return left_data;
90
        else if(encoder==RIGHT) return right_data;
91
        else return -1;
92
}
93

    
94
/**
95
 * @brief Get total distance travelled by the specified encoder (in encoder ticks)
96
 *
97
 * @param encoder the encoder that you want to read, either LEFT or RIGHT
98
 *
99
 * @return The distance covered by the specified encoder.
100
 *
101
 * @note Simply calls encoder_get_dx.
102
 **/
103
int encoder_get_x(char encoder) {
104
        return encoder_get_dx(encoder);
105
}
106

    
107
/**
108
 * Gets the total distance covered by the specified encoder (in encoder clicks)
109
 *
110
 * @param encoder the encoder that you want to read, use LEFT or RIGHT
111
 *
112
 * @return The distance covered by the specified encoder.
113
 **/
114
int encoder_get_dx(char encoder) {
115
        
116
        if(encoder==LEFT) return left_dx;
117
        else if(encoder==RIGHT) return right_dx;
118
        else return -1;
119
}
120

    
121
/** 
122
 * @brief Returns the approximated instantaneous velocity of the robot
123
 * in terms of encoder clicks.
124
 * 
125
 * @param encoder RIGHT or LEFT - the wheel you want the velocity for.
126
 * 
127
 * @return The instantaneous velocity for the given wheel or twice the ERR_VEL
128
 *         if an error occurs (1024 * 2 = 2048)
129
 *
130
 * @bug This uses hard coded values and results are inconsistent.
131
 *      Use at your own risk.
132
 */
133
int encoder_get_v(char encoder){
134
    int vel1, vel2;
135
    vel1 = get_dx(encoder, 0);
136
    vel2 = get_dx(encoder, 1);
137

    
138
    if (vel1 == ERR_VEL && vel2 == ERR_VEL)
139
        return ERR_VEL << 1;
140
    else if (vel2 == ERR_VEL)
141
        return vel1 << 1;
142
    else if (vel1 == ERR_VEL)
143
        return vel2 << 1;
144
    else
145
        return vel1 + vel2;
146
}
147

    
148
/**
149
 * Resets the distance accumulator for the specified
150
 *  encoder.
151
 *
152
 * @param encoder the encoder that you want to reset distance for
153
 **/
154
void encoder_rst_dx(char encoder) {
155
        
156
        if(encoder==LEFT) left_dx = 0;
157
        else if(encoder==RIGHT) right_dx = 0;
158
}
159

    
160
/** 
161
* @brief Returns the number of encoder reads that have occurred.
162
* 
163
* @return The time count.
164
*/
165
int encoder_get_tc(void) {
166
        return timecount;
167
}
168

    
169
/** 
170
* @brief Resets the encoder read counter.
171
*/
172
void encoder_rst_tc(void) {
173
        timecount = 0;
174
}
175

    
176
/** 
177
* @brief Waits until n encoder reads have occurred.
178
* Counter is reset on functions exit.
179
* 
180
* @param n 
181
*/
182
void encoder_wait(int n){
183
        while(data_ready<n);
184
        data_ready=0;
185
}
186

    
187

    
188
//Full reads occur every 40 microseconds. This function should be called
189
//every 8 microseconds.
190
void encoder_recv(char data){
191
        short int dx;
192
        
193
        //Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
194
        // second is offset by 1 bit.
195
        switch(encoder_buf_index){        
196
        case 0: 
197
                right_data_buf |= ((short)data)<<8 & 0xff00;
198
                break;
199
        case 1:
200
                right_data_buf |= ((short)data) & 0xff;
201
                break;
202
        case 2:
203
                left_data_buf |= (((short)data) << 9) & (0x7F << 9);
204
                break;
205
        case 3:
206
                left_data_buf |= (((short)data) << 1) & (0xFF<<1);
207
                break;
208
        case 4: left_data_buf |= (((short)data)>>7) & 0x1;
209
        }        
210
                
211
        encoder_buf_index = (encoder_buf_index + 1) % 5;
212

    
213
        if(encoder_buf_index==0) {
214
                
215
                /*Error handling for the left encoder*/ 
216
                if(!(left_data_buf & OCF)) 
217
                        left_data = ENCODER_DATA_NOT_READY; 
218
                if(left_data_buf & (COF | LIN))  
219
                        left_data = ENCODER_MISALIGNED;
220
                else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
221
                        left_data = ENCODER_MAGNET_FAILURE;
222
                else left_data = (left_data_buf>>5) & 1023;
223
                        
224
                /*Error handling for the right encoder*/ 
225
                 if(!(right_data_buf & OCF))
226
                        right_data = ENCODER_DATA_NOT_READY;        
227
                if(right_data_buf & (COF | LIN)) 
228
                        right_data = ENCODER_MISALIGNED;
229
                else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
230
                        right_data = ENCODER_MAGNET_FAILURE;
231
                else right_data = (right_data_buf>>5) & 1023;
232
                        
233
                  left_data_buf = 0;
234
                right_data_buf = 0;
235

    
236
                /*Note: Above 1023 is invalid data*/        
237
                if(!(left_data > 1023)) {
238
                        //Reverse the left wheel since encoders are necessarily mounted backwards.
239
                        left_data = 1023 - left_data;
240
                        left_data_array_put(left_data);
241

    
242
                        //Adjust left accumulator
243
                        dx = left_data - left_data_array_prev();
244
                        
245
                        if(left_data_array_prev()==0)  dx=0;
246

    
247
                        if(dx > 512) left_dx += dx - 1023; //Underflow
248
                        else if(dx < -512) left_dx += dx + 1023; //Overflow
249
                        else left_dx += dx;
250
                }
251

    
252
                /*Above 1023 is invalid data*/        
253
                if(!(right_data > 1023)) {
254
                        right_data_array_put(right_data);
255

    
256
                        //Adjust right accumulator
257
                        dx = right_data - right_data_array_prev();
258

    
259
                        if(right_data_array_prev()==0) dx=0;
260
                          
261
                        if(dx > 512) right_dx += dx - 1023; //underflow
262
                        else if(dx < -512) right_dx += dx + 1023; //overflow 
263
                        else right_dx += dx;
264
                }
265
        }
266

    
267
        //Increment timecount accumulator
268
        timecount++;
269
}
270

    
271

    
272
//Helper Functions
273
inline void left_data_array_put(unsigned short int value) {
274
        if(left_data_idx == BUFFER_SIZE-1)
275
                left_data_idx = 0;
276
        else
277
                left_data_idx++;
278
        left_data_array[left_data_idx] = value;
279
}
280

    
281
inline unsigned int left_data_array_top(void) {
282
        return left_data_array[left_data_idx];
283
}
284

    
285
inline unsigned int left_data_array_prev(void) {
286
        if(left_data_idx == 0)
287
                return left_data_array[BUFFER_SIZE-1];
288
        else
289
                return left_data_array[left_data_idx - 1];
290
}
291

    
292
inline unsigned int left_data_array_bottom(void) {
293
        if(left_data_idx == BUFFER_SIZE-1)
294
                return left_data_array[0];
295
        else
296
                return left_data_array[left_data_idx + 1];
297
}
298

    
299
inline void right_data_array_put(unsigned short int value) {
300
        if(right_data_idx == BUFFER_SIZE-1)
301
                right_data_idx = 0;
302
        else
303
                right_data_idx++;
304
        right_data_array[right_data_idx] = value;
305
}
306

    
307
inline unsigned int right_data_array_top(void) {
308
        return right_data_array[right_data_idx];
309
}
310

    
311
inline unsigned int right_data_array_prev(void) {
312
        if(right_data_idx == 0)
313
                return right_data_array[BUFFER_SIZE-1];
314
        else
315
                return right_data_array[right_data_idx - 1];
316
}
317

    
318
inline unsigned int right_data_array_bottom(void) {
319
        if(right_data_idx == BUFFER_SIZE-1)
320
                return right_data_array[0];
321
        else
322
                return right_data_array[right_data_idx + 1];
323
}
324

    
325
/* Helper functions for encoder_get_v */
326
int left_data_at(int index) {
327
    int tmp_idx = left_data_idx - index;
328
    if (tmp_idx < 0)
329
        tmp_idx += BUFFER_SIZE;
330
    return left_data_array[tmp_idx];
331
}
332

    
333
int right_data_at(int index) {
334
    int tmp_idx = right_data_idx - index;
335
    if (tmp_idx < 0)
336
        tmp_idx += BUFFER_SIZE;
337
    return right_data_array[tmp_idx];
338
}
339

    
340
int get_dx(char encoder, int index) {
341
    int dx, ctr;
342
    ctr = 0;
343
    dx = 1024;
344
    do {
345
        if (encoder == LEFT)
346
            dx = left_data_at(index+ctr) - left_data_at(index+ctr+38);
347
        else
348
            dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);
349
        ctr++;
350
    } while ((dx > 30 || dx < -30) && ctr < 3); 
351
    if (dx > 30 || dx < -30)
352
        return ERR_VEL;
353
    return dx; 
354
}