Project

General

Profile

Statistics
| Revision:

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

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

    
58
        left_data = -1;
59
        right_data = -1;
60
        
61
        //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
62
        left_data_idx = 0;
63
        right_data_idx = 0;
64
        for(i = 0; i < BUFFER_SIZE; i++) {
65
                left_data_array[i] = 0;
66
        }
67
        for(i = 0; i < BUFFER_SIZE; i++) {
68
                right_data_array[i] = 0;
69
        }
70
        spi_transfer(5);
71
}
72

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

    
88

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

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

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

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

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

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

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

    
159

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

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

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

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

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

    
219

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

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

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

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

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

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

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

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

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

    
301

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

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

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

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

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

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

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

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