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
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