Project

General

Profile

Revision 1496

Reverted "libdragonfly" folder back to version before Init Checking was implemented and did "make dist" to recompile the library. BOM LEDs now shine
correctly.

View differences:

encoders.c
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"
1
#include "encoders.h"
39 2
#include "spi.h"
40 3
#include "ring_buffer.h"
41
#include "encoders.h"
4
#include <avr/io.h>
42 5

  
43
unsigned char encoders_initd=0;
44

  
45 6
unsigned int left_data_buf;
46 7
unsigned int right_data_buf;
47 8
char encoder_buf_index;
......
60 21

  
61 22
volatile short int data_ready;
62 23

  
63
int encoder_recv(char data);
24
void encoder_recv(char data);
64 25

  
65 26
//Helper Function Prototypes
66 27
inline void left_data_array_put(unsigned short int value);
......
88 49
/** 
89 50
* @brief Initializes encoder variables and the hardware interface.
90 51
*/
91
int encoders_init(void){
92
  int i;
52
void encoders_init(void){
53
	int i;
93 54
	
94
  if(encoders_initd)
95
    return ERROR_INIT_ALREADY_INITD;
55
	data_ready=0;
96 56

  
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;
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);
122 74
}
123 75

  
124 76
/**
......
129 81
 *
130 82
 * @return the value of the specified encoder.
131 83
 *         -1 usually means low battery.
132
 *         -2 means the library was not properly initialized
133 84
 *         values above ENCODER_MAX usually means phyiscal problems with
134 85
 *         the encoder.
135 86
 **/
136 87
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
  }
88
	
89
	if(encoder==LEFT) return left_data;
90
	else if(encoder==RIGHT) return right_data;
91
	else return -1;
149 92
}
150 93

  
151 94
/**
......
166 109
 *
167 110
 * @param encoder the encoder that you want to read, use LEFT or RIGHT
168 111
 *
169
 * @return The distance covered by the specified encoder, -2 if the library is not initialized
112
 * @return The distance covered by the specified encoder.
170 113
 **/
171 114
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;
115
	
116
	if(encoder==LEFT) return left_dx;
117
	else if(encoder==RIGHT) return right_dx;
118
	else return -1;
181 119
}
182 120

  
183 121
/** 
......
194 132
 */
195 133
int encoder_get_v(char encoder){
196 134
    int vel1, vel2;
197

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

  
......
214 151
 *
215 152
 * @param encoder the encoder that you want to reset distance for
216 153
 **/
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;
154
void encoder_rst_dx(char encoder) {
155
	
156
	if(encoder==LEFT) left_dx = 0;
157
	else if(encoder==RIGHT) right_dx = 0;
228 158
}
229 159

  
230 160
/** 
......
238 168

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

  
247
  timecount = 0;
248

  
249
  return 0;
172
void encoder_rst_tc(void) {
173
	timecount = 0;
250 174
}
251 175

  
252 176
/** 
......
254 178
* Counter is reset on functions exit.
255 179
* 
256 180
* @param n 
257
*
258
* @return 0 if init succesfull, an error code otherwise
259 181
*/
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;
182
void encoder_wait(int n){
183
	while(data_ready<n);
184
	data_ready=0;
268 185
}
269 186

  
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 187

  
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
  }	
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
	}	
300 210
		
301
  encoder_buf_index = (encoder_buf_index + 1) % 5;
211
	encoder_buf_index = (encoder_buf_index + 1) % 5;
302 212

  
303
  if(encoder_buf_index==0) {
213
	if(encoder_buf_index==0) {
304 214
		
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;
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;
313 223
			
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;
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;
322 232
			
323
    left_data_buf = 0;
324
    right_data_buf = 0;
233
  		left_data_buf = 0;
234
		right_data_buf = 0;
325 235

  
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);
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);
331 241

  
332
      //Adjust left accumulator
333
      dx = left_data - left_data_array_prev();
242
			//Adjust left accumulator
243
			dx = left_data - left_data_array_prev();
334 244
			
335
      if(left_data_array_prev()==0)  dx=0;
245
			if(left_data_array_prev()==0)  dx=0;
336 246

  
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
    }
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
		}
341 251

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

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

  
349
      if(right_data_array_prev()==0) dx=0;
259
			if(right_data_array_prev()==0) dx=0;
350 260
			  
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
  }
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
	}
356 266

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

  
360
  return 0;
267
	//Increment timecount accumulator
268
	timecount++;
361 269
}
362 270

  
363 271

  

Also available in: Unified diff