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