Project

General

Profile

Statistics
| Revision:

root / branches / encoders / code / lib / src / libdragonfly / encoders.c @ 1345

History | View | Annotate | Download (7.98 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
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

    
38
void encoder_recv_complete(){
39
          encoder_buf_index = 0;
40
        data_ready++;
41
  
42
           spi_transfer(5);
43
}
44

    
45
/** 
46
* @brief Initializes encoder variables and the hardware interface.
47
*/
48
void encoders_init(void){
49
        int i;
50
        
51
        data_ready=0;
52

    
53
        spi_init(encoder_recv, encoder_recv_complete);
54
        encoder_buf_index = 0;
55
        left_data_buf = 0;
56
        right_data_buf= 0;
57
        left_data = -1;
58
        right_data = -1;
59
        
60
        //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
61
        left_data_idx = 0;
62
        right_data_idx = 0;
63
        for(i = 0; i < BUFFER_SIZE; i++) {
64
                left_data_array[i] = 0;
65
        }
66
        for(i = 0; i < BUFFER_SIZE; i++) {
67
                right_data_array[i] = 0;
68
        }
69
        spi_transfer(5);
70
}
71

    
72
/**
73
 * @brief Returns the specified encoders value
74
 *
75
 * @param encoder this is the encoder that you want to read. Valid arguments
76
 *          are LEFT and RIGHT
77
 *
78
 * @return the value of the specified encoder
79
 **/
80
int encoder_read(char encoder){
81
        
82
        if(encoder==LEFT) return left_data;
83
        else if(encoder==RIGHT) return right_data;
84
        else return -1;
85
}
86

    
87

    
88
/** 
89
* @brief Outputs encoder direction as FORWARD OR BACK
90
* A STUB! DO NOT use.
91
* 
92
* @param encoder The encoder you want the direction of.
93
* Valid arguments are right and left.
94
* 
95
* @return FORWARD or BACK (the constants)
96
*/
97
char encoder_direction(char encoder){
98
        return 0;
99
}
100

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

    
115
/**
116
 * Resets the distance accumulator for the specified
117
 *  encoder.
118
 *
119
 * @param encoder the encoder that you want to reset distance for
120
 **/
121
void encoder_rst_dx(char encoder) {
122
        
123
        if(encoder==LEFT) left_dx = 0;
124
        else if(encoder==RIGHT) right_dx = 0;
125
}
126

    
127
/** 
128
* @brief Returns the number of encoder reads that have occurred.
129
* 
130
* @return The time count.
131
*/
132
int encoder_get_tc(void) {
133
        return timecount;
134
}
135

    
136
/** 
137
* @brief Resets the encoder read counter.
138
*/
139
void encoder_rst_tc(void) {
140
        timecount = 0;
141
}
142

    
143
///////////////////////////////////////////////////////////////////////
144
int left_data_at(int index) {
145
        int tmp_idx = left_data_idx - index;
146
        if (tmp_idx < 0)
147
                tmp_idx += BUFFER_SIZE;
148
        return left_data_array[tmp_idx];
149
}
150

    
151
int right_data_at(int index) {
152
        int tmp_idx = right_data_idx - index;
153
        if (tmp_idx < 0)
154
                tmp_idx += BUFFER_SIZE;
155
        return right_data_array[tmp_idx];        
156
}
157

    
158

    
159
int get_dx(char encoder, int index) {
160
        int dx, ctr;
161
        ctr = 0;
162
        dx = 1024;
163
        do {
164
                if (encoder == LEFT)
165
                        dx = left_data_at(index+ctr+38) - left_data_at(index+ctr);
166
                else
167
                        dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);
168
                ctr++;
169
        } while ((dx > 30 || dx < -30) && ctr < 3);
170
        if (dx > 30 || dx < -30)
171
                return ERR_VEL;
172
        return dx;
173
}
174
/** 
175
* @brief Returns the approximated instantaneous velocity of the robot
176
* in terms of encoder clicks.
177
* 
178
* @param encoder RIGHT or LEFT - the wheel you want the velocity for.
179
* 
180
* @return The instantaneous velocity for the given wheel.
181
*/
182
int encoder_get_v(char encoder){
183
        /*
184
        if (encoder == LEFT)
185
                return left_data_array_bottom() - left_data_array_top();
186

187
        if (encoder == RIGHT)
188
                return right_data_array_top() - right_data_array_bottom();
189

190
        return -1;
191
        */
192
        int vel1, vel2, tmp;
193
        vel1 = get_dx(encoder, 0);
194
        vel2 = get_dx(encoder, 1);
195

    
196
        if (vel1 == ERR_VEL && vel2 == ERR_VEL)
197
                return ERR_VEL << 1;
198
        else if (vel2 == ERR_VEL)
199
                return vel1 << 1;
200
        else if (vel1 == ERR_VEL) 
201
                return vel2 << 1;
202
        else
203
                return vel1 + vel2;
204
}
205
/////////////////////////////////////////////////////////////////////////
206

    
207
/** 
208
* @brief Waits until n encoder reads have occurred.
209
* Counter is reset on functions exit.
210
* 
211
* @param n 
212
*/
213
void encoder_wait(int n){
214
        while(data_ready<n);
215
        data_ready=0;
216
}
217

    
218

    
219
//Full reads occur every 40 microseconds. This function should be called
220
//every 8 microseconds.
221
void encoder_recv(char data){
222
        short int dx;
223
        
224
        //Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
225
        // second is offset by 1 bit.
226
        switch(encoder_buf_index){        
227
        case 0: 
228
                right_data_buf |= ((short)data)<<8 & 0xff00;
229
                break;
230
        case 1:
231
                right_data_buf |= ((short)data) & 0xff;
232
                break;
233
        case 2:
234
                left_data_buf |= (((short)data) << 9) & (0x7F << 9);
235
                break;
236
        case 3:
237
                left_data_buf |= (((short)data) << 1) & (0xFF<<1);
238
                break;
239
        case 4: left_data_buf |= (((short)data)>>7) & 0x1;
240
        }        
241
                
242
        encoder_buf_index = (encoder_buf_index + 1) % 5;
243

    
244
        if(encoder_buf_index==0) {
245
                
246
                /*Error handling for the left encoder*/ 
247
                if(!(left_data_buf & OCF)) 
248
                        left_data = ENCODER_DATA_NOT_READY; 
249
                if(left_data_buf & (COF | LIN))  
250
                        left_data = ENCODER_MISALIGNED;
251
                else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
252
                        left_data = ENCODER_MAGNET_FAILURE;
253
                else left_data = (left_data_buf>>5) & 1023;
254
                        
255
                /*Error handling for the right encoder*/ 
256
                 if(!(right_data_buf & OCF))
257
                        right_data = ENCODER_DATA_NOT_READY;        
258
                if(right_data_buf & (COF | LIN)) 
259
                        right_data = ENCODER_MISALIGNED;
260
                else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
261
                        right_data = ENCODER_MAGNET_FAILURE;
262
                else right_data = (right_data_buf>>5) & 1023;
263
                        
264
                  left_data_buf = 0;
265
                right_data_buf = 0;
266

    
267
                /*Above 1023 is invalid data*/        
268
                if(!(left_data > 1023)) {
269
                        left_data_array_put(left_data);
270

    
271
                        //Adjust left accumulator
272
                        dx = - left_data + left_data_array_prev();
273
                        
274
                        if(left_data_array_prev()==0)  dx=0;
275

    
276
                        if(dx > 512) left_dx += dx - 1023; //Underflow
277
                        else if(dx < -512) left_dx += dx + 1023; //Overflow
278
                        else left_dx += dx;
279
                }
280

    
281
                /*Above 1023 is invalid data*/        
282
                if(!(right_data > 1023)) {
283
                        right_data_array_put(right_data);
284

    
285
                        //Adjust right accumulator
286
                        dx = right_data - right_data_array_prev();
287

    
288
                        if(right_data_array_prev()==0) dx=0;
289
                          
290
                        if(dx > 512) right_dx += dx - 1023; //underflow
291
                        else if(dx < -512) right_dx += dx + 1023; //overflow 
292
                        else right_dx += dx;
293
                }
294
        }
295

    
296
        //Increment timecount accumulator
297
        timecount++;
298
}
299

    
300

    
301
//Helper Functions
302
inline void left_data_array_put(unsigned short int value) {
303
        if(left_data_idx == BUFFER_SIZE-1)
304
                left_data_idx = 0;
305
        else
306
                left_data_idx++;
307
        left_data_array[left_data_idx] = value;
308
}
309

    
310
inline unsigned int left_data_array_top(void) {
311
        return left_data_array[left_data_idx];
312
}
313

    
314
inline unsigned int left_data_array_prev(void) {
315
        if(left_data_idx == 0)
316
                return left_data_array[BUFFER_SIZE-1];
317
        else
318
                return left_data_array[left_data_idx - 1];
319
}
320

    
321
inline unsigned int left_data_array_bottom(void) {
322
        if(left_data_idx == BUFFER_SIZE-1)
323
                return left_data_array[0];
324
        else
325
                return left_data_array[left_data_idx + 1];
326
}
327

    
328
inline void right_data_array_put(unsigned short int value) {
329
        if(right_data_idx == BUFFER_SIZE-1)
330
                right_data_idx = 0;
331
        else
332
                right_data_idx++;
333
        right_data_array[right_data_idx] = value;
334
}
335

    
336
inline unsigned int right_data_array_top(void) {
337
        return right_data_array[right_data_idx];
338
}
339

    
340
inline unsigned int right_data_array_prev(void) {
341
        if(right_data_idx == 0)
342
                return right_data_array[BUFFER_SIZE-1];
343
        else
344
                return right_data_array[right_data_idx - 1];
345
}
346

    
347
inline unsigned int right_data_array_bottom(void) {
348
        if(right_data_idx == BUFFER_SIZE-1)
349
                return right_data_array[0];
350
        else
351
                return right_data_array[right_data_idx + 1];
352
}
353