Revision 1191
move spline slave to sub-directory and update encoder_get_v function
| encoders.c | ||
|---|---|---|
| 19 | 19 |
int right_dx; |
| 20 | 20 |
long int timecount; |
| 21 | 21 |
|
| 22 |
int left_v; |
|
| 23 |
int right_v; |
|
| 24 |
|
|
| 25 |
|
|
| 26 | 22 |
volatile short int data_ready; |
| 27 | 23 |
|
| 28 | 24 |
void encoder_recv(char data); |
| ... | ... | |
| 61 | 57 |
left_data = -1; |
| 62 | 58 |
right_data = -1; |
| 63 | 59 |
|
| 64 |
|
|
| 65 |
left_v=0; |
|
| 66 |
right_v=0; |
|
| 67 |
|
|
| 68 | 60 |
//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE); |
| 69 | 61 |
left_data_idx = 0; |
| 70 | 62 |
right_data_idx = 0; |
| ... | ... | |
| 158 | 150 |
* @return The instantaneous velocity for the given wheel. |
| 159 | 151 |
*/ |
| 160 | 152 |
int encoder_get_v(char encoder){
|
| 161 |
int last, res=0; |
|
| 162 |
|
|
| 163 |
cli(); |
|
| 164 |
|
|
| 165 |
if(encoder==LEFT){
|
|
| 166 |
if(left_data_idx==0) |
|
| 167 |
last = BUFFER_SIZE - 1; |
|
| 168 |
else |
|
| 169 |
last = left_data_idx - 1; |
|
| 170 |
res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]); |
|
| 171 |
} |
|
| 172 |
if(encoder==RIGHT){
|
|
| 173 |
if(right_data_idx==0) |
|
| 174 |
last = BUFFER_SIZE - 1; |
|
| 175 |
else |
|
| 176 |
last = right_data_idx - 1; |
|
| 177 |
res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]); |
|
| 178 |
} |
|
| 179 |
|
|
| 180 |
sei(); |
|
| 153 |
if (encoder == LEFT) |
|
| 154 |
return left_data_array_bottom() - left_data_array_top(); |
|
| 181 | 155 |
|
| 182 |
while(res<MIN_V)//underflow |
|
| 183 |
res+=1024; |
|
| 184 |
while(res>MAX_V)//overflow |
|
| 185 |
res-=1024; |
|
| 156 |
if (encoder == RIGHT) |
|
| 157 |
return right_data_array_top() - right_data_array_bottom(); |
|
| 186 | 158 |
|
| 187 |
return res;
|
|
| 159 |
return -1; /* TODO: velocity could be -1, use another value for error */
|
|
| 188 | 160 |
} |
| 189 | 161 |
|
| 190 | 162 |
|
| ... | ... | |
| 253 | 225 |
left_data_array_put(left_data); |
| 254 | 226 |
|
| 255 | 227 |
//Adjust left accumulator |
| 256 |
dx = left_data - left_data_array_prev();
|
|
| 228 |
dx = - left_data + left_data_array_prev();
|
|
| 257 | 229 |
|
| 258 |
//Adjust velocity: save last dx |
|
| 259 |
left_v = left_dx; |
|
| 260 |
|
|
| 261 | 230 |
if(left_data_array_prev()==0) dx=0; |
| 262 | 231 |
|
| 263 | 232 |
if(dx > 512) left_dx += dx - 1023; //Underflow |
| 264 | 233 |
else if(dx < -512) left_dx += dx + 1023; //Overflow |
| 265 | 234 |
else left_dx += dx; |
| 266 |
|
|
| 267 |
//Adjust velocity: update |
|
| 268 |
left_v = left_dx - left_v; |
|
| 269 | 235 |
} |
| 270 | 236 |
|
| 271 | 237 |
/*Above 1023 is invalid data*/ |
Also available in: Unified diff