Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (10.8 KB)

1
/**
2
 * Copyright (c) 2007 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

    
26

    
27
/**
28
 * @file encoders.c
29
 * @brief Encoders
30
 *
31
 * Implementation of functions for encoders.
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 **/
35

    
36
#include <avr/io.h>
37

    
38
#include "dragonfly_defs.h"
39
#include "spi.h"
40
#include "ring_buffer.h"
41
#include "encoders.h"
42

    
43
unsigned char encoders_initd=0;
44

    
45
unsigned int left_data_buf;
46
unsigned int right_data_buf;
47
char encoder_buf_index;
48

    
49
unsigned int left_data;
50
unsigned int right_data;
51

    
52
unsigned int left_data_array[BUFFER_SIZE];
53
unsigned int right_data_array[BUFFER_SIZE];
54
int left_data_idx;
55
int right_data_idx;
56

    
57
int left_dx;
58
int right_dx;
59
long int timecount;
60

    
61
volatile short int data_ready;
62

    
63
int encoder_recv(char data);
64

    
65
//Helper Function Prototypes
66
inline void left_data_array_put(unsigned short int value);
67
inline unsigned int left_data_array_top(void);
68
inline unsigned int left_data_array_prev(void);
69
inline unsigned int left_data_array_bottom(void);
70

    
71
inline void right_data_array_put(unsigned short int value);
72
inline unsigned int right_data_array_top(void);
73
inline unsigned int right_data_array_prev(void);
74
inline unsigned int right_data_array_bottom(void);
75

    
76
//encoder_get_v helper function prototypes
77
int left_data_at(int index);
78
int right_data_at(int index);
79
int get_dx(char encoder, int index);
80

    
81
void encoder_recv_complete(void){
82
          encoder_buf_index = 0;
83
        data_ready++;
84
  
85
           spi_transfer(5);
86
}
87

    
88
/** 
89
* @brief Initializes encoder variables and the hardware interface.
90
*/
91
int encoders_init(void){
92
  int i;
93
        
94
  if(encoders_initd)
95
    return ERROR_INIT_ALREADY_INITD;
96

    
97
  data_ready=0;
98

    
99
  if(spi_init(encoder_recv, encoder_recv_complete))
100
    return -1; //if spi was inited for something else, bail out
101
  encoder_buf_index = 0;
102
  left_data_buf = 0;
103
  right_data_buf= 0;
104
  left_data = -1;
105
  right_data = -1;
106

    
107
  //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
108
  left_data_idx = 0;
109
  right_data_idx = 0;
110

    
111
  for(i = 0; i < BUFFER_SIZE; i++) {
112
    left_data_array[i] = 0;
113
  }
114
  for(i = 0; i < BUFFER_SIZE; i++) {
115
    right_data_array[i] = 0;
116
  }
117

    
118
  spi_transfer(5);
119

    
120
  encoders_initd=1;
121
  return 0;
122
}
123

    
124
/**
125
 * @brief Returns the specified encoders value
126
 *
127
 * @param encoder this is the encoder that you want to read. Valid arguments
128
 *          are LEFT and RIGHT
129
 *
130
 * @return the value of the specified encoder.
131
 *         -1 usually means low battery.
132
 *         -2 means the library was not properly initialized
133
 *         values above ENCODER_MAX usually means phyiscal problems with
134
 *         the encoder.
135
 **/
136
int encoder_read(char encoder){
137
  if(!encoders_initd)
138
    return -2;
139

    
140
  if(encoder==LEFT) {
141
    return left_data;
142
  }
143
  else if(encoder==RIGHT){
144
    return right_data;
145
  }
146
  else{
147
    return -1;
148
  }
149
}
150

    
151
/**
152
 * @brief Get total distance travelled by the specified encoder (in encoder ticks)
153
 *
154
 * @param encoder the encoder that you want to read, either LEFT or RIGHT
155
 *
156
 * @return The distance covered by the specified encoder.
157
 *
158
 * @note Simply calls encoder_get_dx.
159
 **/
160
int encoder_get_x(char encoder) {
161
        return encoder_get_dx(encoder);
162
}
163

    
164
/**
165
 * Gets the total distance covered by the specified encoder (in encoder clicks)
166
 *
167
 * @param encoder the encoder that you want to read, use LEFT or RIGHT
168
 *
169
 * @return The distance covered by the specified encoder, -2 if the library is not initialized
170
 **/
171
int encoder_get_dx(char encoder) {
172
  if(!encoders_initd)
173
    return -2;
174

    
175
  if(encoder==LEFT)
176
    return left_dx;
177
  else if(encoder==RIGHT)
178
    return right_dx;
179
  else
180
    return -1;
181
}
182

    
183
/** 
184
 * @brief Returns the approximated instantaneous velocity of the robot
185
 * in terms of encoder clicks.
186
 * 
187
 * @param encoder RIGHT or LEFT - the wheel you want the velocity for.
188
 * 
189
 * @return The instantaneous velocity for the given wheel or twice the ERR_VEL
190
 *         if an error occurs (1024 * 2 = 2048)
191
 *
192
 * @bug This uses hard coded values and results are inconsistent.
193
 *      Use at your own risk.
194
 */
195
int encoder_get_v(char encoder){
196
    int vel1, vel2;
197

    
198
    vel1 = get_dx(encoder, 0);
199
    vel2 = get_dx(encoder, 1);
200

    
201
    if (vel1 == ERR_VEL && vel2 == ERR_VEL)
202
        return ERR_VEL << 1;
203
    else if (vel2 == ERR_VEL)
204
        return vel1 << 1;
205
    else if (vel1 == ERR_VEL)
206
        return vel2 << 1;
207
    else
208
        return vel1 + vel2;
209
}
210

    
211
/**
212
 * Resets the distance accumulator for the specified
213
 *  encoder.
214
 *
215
 * @param encoder the encoder that you want to reset distance for
216
 **/
217
int encoder_rst_dx(char encoder) {
218

    
219
  if(!encoders_initd)
220
    return ERROR_LIBRARY_NOT_INITD;
221

    
222
  if(encoder==LEFT)
223
    left_dx = 0;
224
  else if(encoder==RIGHT)
225
    right_dx = 0;
226

    
227
  return 0;
228
}
229

    
230
/** 
231
* @brief Returns the number of encoder reads that have occurred.
232
* 
233
* @return The time count.
234
*/
235
int encoder_get_tc(void) {
236
        return timecount;
237
}
238

    
239
/** 
240
* @brief Resets the encoder read counter.
241
* @return 0 if init succesfull, an error code otherwise
242
*/
243
int encoder_rst_tc(void) {
244
  if(!encoders_initd)
245
    return ERROR_LIBRARY_NOT_INITD;
246

    
247
  timecount = 0;
248

    
249
  return 0;
250
}
251

    
252
/** 
253
* @brief Waits until n encoder reads have occurred.
254
* Counter is reset on functions exit.
255
* 
256
* @param n 
257
*
258
* @return 0 if init succesfull, an error code otherwise
259
*/
260
int encoder_wait(int n){
261
  if(!encoders_initd)
262
    return ERROR_LIBRARY_NOT_INITD;
263

    
264
  while(data_ready<n);
265
  data_ready=0;
266

    
267
  return 0;
268
}
269

    
270
/**
271
 * @brief This functions recieves data from the encoders directly
272
 *
273
 * @note Full reads occur every 40 microseconds. This function should be called every 8 microseconds.
274
 *
275
 * @return 0 if init succesfull, an error code otherwise
276
 */
277
int encoder_recv(char data){
278
  short int dx;
279

    
280
  if(!encoders_initd)
281
    return ERROR_LIBRARY_NOT_INITD;
282

    
283
  //Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
284
  // second is offset by 1 bit.
285
  switch(encoder_buf_index){        
286
  case 0: 
287
    right_data_buf |= ((short)data)<<8 & 0xff00;
288
    break;
289
  case 1:
290
    right_data_buf |= ((short)data) & 0xff;
291
    break;
292
  case 2:
293
    left_data_buf |= (((short)data) << 9) & (0x7F << 9);
294
    break;
295
  case 3:
296
    left_data_buf |= (((short)data) << 1) & (0xFF<<1);
297
    break;
298
  case 4: left_data_buf |= (((short)data)>>7) & 0x1;
299
  }        
300
                
301
  encoder_buf_index = (encoder_buf_index + 1) % 5;
302

    
303
  if(encoder_buf_index==0) {
304
                
305
    /*Error handling for the left encoder*/ 
306
    if(!(left_data_buf & OCF)) 
307
      left_data = ENCODER_DATA_NOT_READY; 
308
    if(left_data_buf & (COF | LIN))  
309
      left_data = ENCODER_MISALIGNED;
310
    else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
311
      left_data = ENCODER_MAGNET_FAILURE;
312
    else left_data = (left_data_buf>>5) & 1023;
313
                        
314
    /*Error handling for the right encoder*/ 
315
    if(!(right_data_buf & OCF))
316
      right_data = ENCODER_DATA_NOT_READY;        
317
    if(right_data_buf & (COF | LIN)) 
318
      right_data = ENCODER_MISALIGNED;
319
    else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
320
      right_data = ENCODER_MAGNET_FAILURE;
321
    else right_data = (right_data_buf>>5) & 1023;
322
                        
323
    left_data_buf = 0;
324
    right_data_buf = 0;
325

    
326
    /*Note: Above 1023 is invalid data*/        
327
    if(!(left_data > 1023)) {
328
      //Reverse the left wheel since encoders are necessarily mounted backwards.
329
      left_data = 1023 - left_data;
330
      left_data_array_put(left_data);
331

    
332
      //Adjust left accumulator
333
      dx = left_data - left_data_array_prev();
334
                        
335
      if(left_data_array_prev()==0)  dx=0;
336

    
337
      if(dx > 512) left_dx += dx - 1023; //Underflow
338
      else if(dx < -512) left_dx += dx + 1023; //Overflow
339
      else left_dx += dx;
340
    }
341

    
342
    /*Above 1023 is invalid data*/        
343
    if(!(right_data > 1023)) {
344
      right_data_array_put(right_data);
345

    
346
      //Adjust right accumulator
347
      dx = right_data - right_data_array_prev();
348

    
349
      if(right_data_array_prev()==0) dx=0;
350
                          
351
      if(dx > 512) right_dx += dx - 1023; //underflow
352
      else if(dx < -512) right_dx += dx + 1023; //overflow 
353
      else right_dx += dx;
354
    }
355
  }
356

    
357
  //Increment timecount accumulator
358
  timecount++;
359

    
360
  return 0;
361
}
362

    
363

    
364
//Helper Functions
365
inline void left_data_array_put(unsigned short int value) {
366
        if(left_data_idx == BUFFER_SIZE-1)
367
                left_data_idx = 0;
368
        else
369
                left_data_idx++;
370
        left_data_array[left_data_idx] = value;
371
}
372

    
373
inline unsigned int left_data_array_top(void) {
374
        return left_data_array[left_data_idx];
375
}
376

    
377
inline unsigned int left_data_array_prev(void) {
378
        if(left_data_idx == 0)
379
                return left_data_array[BUFFER_SIZE-1];
380
        else
381
                return left_data_array[left_data_idx - 1];
382
}
383

    
384
inline unsigned int left_data_array_bottom(void) {
385
        if(left_data_idx == BUFFER_SIZE-1)
386
                return left_data_array[0];
387
        else
388
                return left_data_array[left_data_idx + 1];
389
}
390

    
391
inline void right_data_array_put(unsigned short int value) {
392
        if(right_data_idx == BUFFER_SIZE-1)
393
                right_data_idx = 0;
394
        else
395
                right_data_idx++;
396
        right_data_array[right_data_idx] = value;
397
}
398

    
399
inline unsigned int right_data_array_top(void) {
400
        return right_data_array[right_data_idx];
401
}
402

    
403
inline unsigned int right_data_array_prev(void) {
404
        if(right_data_idx == 0)
405
                return right_data_array[BUFFER_SIZE-1];
406
        else
407
                return right_data_array[right_data_idx - 1];
408
}
409

    
410
inline unsigned int right_data_array_bottom(void) {
411
        if(right_data_idx == BUFFER_SIZE-1)
412
                return right_data_array[0];
413
        else
414
                return right_data_array[right_data_idx + 1];
415
}
416

    
417
/* Helper functions for encoder_get_v */
418
int left_data_at(int index) {
419
    int tmp_idx = left_data_idx - index;
420
    if (tmp_idx < 0)
421
        tmp_idx += BUFFER_SIZE;
422
    return left_data_array[tmp_idx];
423
}
424

    
425
int right_data_at(int index) {
426
    int tmp_idx = right_data_idx - index;
427
    if (tmp_idx < 0)
428
        tmp_idx += BUFFER_SIZE;
429
    return right_data_array[tmp_idx];
430
}
431

    
432
int get_dx(char encoder, int index) {
433
    int dx, ctr;
434
    ctr = 0;
435
    dx = 1024;
436
    do {
437
        if (encoder == LEFT)
438
            dx = left_data_at(index+ctr) - left_data_at(index+ctr+38);
439
        else
440
            dx = right_data_at(index+ctr) - right_data_at(index+ctr+38);
441
        ctr++;
442
    } while ((dx > 30 || dx < -30) && ctr < 3); 
443
    if (dx > 30 || dx < -30)
444
        return ERR_VEL;
445
    return dx; 
446
}