Project

General

Profile

Revision 1461

updated all the library code to have sensible _init behavior.
Almost all of the library components have a global variable which gets set after init and the functions inside will fail with an error code if init has not been called. Also, the init functions themselves check this variable and will bail out without doing any damage if that init has already been called

View differences:

encoders.c
3 3
#include "ring_buffer.h"
4 4
#include <avr/io.h>
5 5

  
6
unsigned char encoders_initd=0;
7

  
6 8
unsigned int left_data_buf;
7 9
unsigned int right_data_buf;
8 10
char encoder_buf_index;
......
21 23

  
22 24
volatile short int data_ready;
23 25

  
24
void encoder_recv(char data);
26
int encoder_recv(char data);
25 27

  
26 28
//Helper Function Prototypes
27 29
inline void left_data_array_put(unsigned short int value);
......
49 51
/** 
50 52
* @brief Initializes encoder variables and the hardware interface.
51 53
*/
52
void encoders_init(void){
53
	int i;
54
int encoders_init(void){
55
  int i;
54 56
	
55
	data_ready=0;
57
  if(encoders_initd)
58
    return ERROR_INIT_ALREADY_INITD;
56 59

  
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);
60
  data_ready=0;
61

  
62
  if(spi_init(encoder_recv, encoder_recv_complete))
63
    return -1; //if spi was inited for something else, bail out
64
  encoder_buf_index = 0;
65
  left_data_buf = 0;
66
  right_data_buf= 0;
67
  left_data = -1;
68
  right_data = -1;
69

  
70
  //RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
71
  left_data_idx = 0;
72
  right_data_idx = 0;
73

  
74
  for(i = 0; i < BUFFER_SIZE; i++) {
75
    left_data_array[i] = 0;
76
  }
77
  for(i = 0; i < BUFFER_SIZE; i++) {
78
    right_data_array[i] = 0;
79
  }
80

  
81
  spi_transfer(5);
82

  
83
  encoders_initd=1;
84
  return 0;
74 85
}
75 86

  
76 87
/**
......
81 92
 *
82 93
 * @return the value of the specified encoder.
83 94
 *         -1 usually means low battery.
95
 *         -2 means the library was not properly initialized
84 96
 *         values above ENCODER_MAX usually means phyiscal problems with
85 97
 *         the encoder.
86 98
 **/
87 99
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;
100
  if(!encoders_initd)
101
    return -2;
102

  
103
  if(encoder==LEFT) {
104
    return left_data;
105
  }
106
  else if(encoder==RIGHT){
107
    return right_data;
108
  }
109
  else{
110
    return -1;
111
  }
92 112
}
93 113

  
94 114
/**
......
109 129
 *
110 130
 * @param encoder the encoder that you want to read, use LEFT or RIGHT
111 131
 *
112
 * @return The distance covered by the specified encoder.
132
 * @return The distance covered by the specified encoder, -2 if the library is not initialized
113 133
 **/
114 134
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;
135
  if(!encoders_initd)
136
    return -2;
137

  
138
  if(encoder==LEFT)
139
    return left_dx;
140
  else if(encoder==RIGHT)
141
    return right_dx;
142
  else
143
    return -1;
119 144
}
120 145

  
121 146
/** 
......
132 157
 */
133 158
int encoder_get_v(char encoder){
134 159
    int vel1, vel2;
160

  
135 161
    vel1 = get_dx(encoder, 0);
136 162
    vel2 = get_dx(encoder, 1);
137 163

  
......
151 177
 *
152 178
 * @param encoder the encoder that you want to reset distance for
153 179
 **/
154
void encoder_rst_dx(char encoder) {
155
	
156
	if(encoder==LEFT) left_dx = 0;
157
	else if(encoder==RIGHT) right_dx = 0;
180
int encoder_rst_dx(char encoder) {
181

  
182
  if(!encoders_initd)
183
    return ERROR_LIBRARY_NOT_INITD;
184

  
185
  if(encoder==LEFT)
186
    left_dx = 0;
187
  else if(encoder==RIGHT)
188
    right_dx = 0;
189

  
190
  return 0;
158 191
}
159 192

  
160 193
/** 
......
168 201

  
169 202
/** 
170 203
* @brief Resets the encoder read counter.
204
* @return 0 if init succesfull, an error code otherwise
171 205
*/
172
void encoder_rst_tc(void) {
173
	timecount = 0;
206
int encoder_rst_tc(void) {
207
  if(!encoders_initd)
208
    return ERROR_LIBRARY_NOT_INITD;
209

  
210
  timecount = 0;
211

  
212
  return 0;
174 213
}
175 214

  
176 215
/** 
......
178 217
* Counter is reset on functions exit.
179 218
* 
180 219
* @param n 
220
*
221
* @return 0 if init succesfull, an error code otherwise
181 222
*/
182
void encoder_wait(int n){
183
	while(data_ready<n);
184
	data_ready=0;
223
int encoder_wait(int n){
224
  if(!encoders_initd)
225
    return ERROR_LIBRARY_NOT_INITD;
226

  
227
  while(data_ready<n);
228
  data_ready=0;
229

  
230
  return 0;
185 231
}
186 232

  
233
/**
234
 * @brief This functions recieves data from the encoders directly
235
 *
236
 * @note Full reads occur every 40 microseconds. This function should be called every 8 microseconds.
237
 *
238
 * @return 0 if init succesfull, an error code otherwise
239
 */
240
int encoder_recv(char data){
241
  short int dx;
187 242

  
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
	}	
243
  if(!encoders_initd)
244
    return ERROR_LIBRARY_NOT_INITD;
245

  
246
  //Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
247
  // second is offset by 1 bit.
248
  switch(encoder_buf_index){	
249
  case 0: 
250
    right_data_buf |= ((short)data)<<8 & 0xff00;
251
    break;
252
  case 1:
253
    right_data_buf |= ((short)data) & 0xff;
254
    break;
255
  case 2:
256
    left_data_buf |= (((short)data) << 9) & (0x7F << 9);
257
    break;
258
  case 3:
259
    left_data_buf |= (((short)data) << 1) & (0xFF<<1);
260
    break;
261
  case 4: left_data_buf |= (((short)data)>>7) & 0x1;
262
  }	
210 263
		
211
	encoder_buf_index = (encoder_buf_index + 1) % 5;
264
  encoder_buf_index = (encoder_buf_index + 1) % 5;
212 265

  
213
	if(encoder_buf_index==0) {
266
  if(encoder_buf_index==0) {
214 267
		
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;
268
    /*Error handling for the left encoder*/ 
269
    if(!(left_data_buf & OCF)) 
270
      left_data = ENCODER_DATA_NOT_READY; 
271
    if(left_data_buf & (COF | LIN))  
272
      left_data = ENCODER_MISALIGNED;
273
    else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
274
      left_data = ENCODER_MAGNET_FAILURE;
275
    else left_data = (left_data_buf>>5) & 1023;
223 276
			
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;
277
    /*Error handling for the right encoder*/ 
278
    if(!(right_data_buf & OCF))
279
      right_data = ENCODER_DATA_NOT_READY;	
280
    if(right_data_buf & (COF | LIN)) 
281
      right_data = ENCODER_MISALIGNED;
282
    else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
283
      right_data = ENCODER_MAGNET_FAILURE;
284
    else right_data = (right_data_buf>>5) & 1023;
232 285
			
233
  		left_data_buf = 0;
234
		right_data_buf = 0;
286
    left_data_buf = 0;
287
    right_data_buf = 0;
235 288

  
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);
289
    /*Note: Above 1023 is invalid data*/	
290
    if(!(left_data > 1023)) {
291
      //Reverse the left wheel since encoders are necessarily mounted backwards.
292
      left_data = 1023 - left_data;
293
      left_data_array_put(left_data);
241 294

  
242
			//Adjust left accumulator
243
			dx = left_data - left_data_array_prev();
295
      //Adjust left accumulator
296
      dx = left_data - left_data_array_prev();
244 297
			
245
			if(left_data_array_prev()==0)  dx=0;
298
      if(left_data_array_prev()==0)  dx=0;
246 299

  
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
		}
300
      if(dx > 512) left_dx += dx - 1023; //Underflow
301
      else if(dx < -512) left_dx += dx + 1023; //Overflow
302
      else left_dx += dx;
303
    }
251 304

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

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

  
259
			if(right_data_array_prev()==0) dx=0;
312
      if(right_data_array_prev()==0) dx=0;
260 313
			  
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
	}
314
      if(dx > 512) right_dx += dx - 1023; //underflow
315
      else if(dx < -512) right_dx += dx + 1023; //overflow 
316
      else right_dx += dx;
317
    }
318
  }
266 319

  
267
	//Increment timecount accumulator
268
	timecount++;
320
  //Increment timecount accumulator
321
  timecount++;
322

  
323
  return 0;
269 324
}
270 325

  
271 326

  

Also available in: Unified diff