Revision 339f64d2
Fixed warnings in libscout behaviors.
Some of these were actually serious errors. Others were little things.
scout/libscout/src/behaviors/Odometry.cpp | ||
---|---|---|
38 | 38 |
//Also, subtract the delta from y because positive y is down. |
39 | 39 |
scout_pos->x += total_dist*cos(-theta); |
40 | 40 |
scout_pos->y -= total_dist*sin(-theta); |
41 |
scout_pos->theta = fmod(theta, 2*M_PI);
|
|
41 |
scout_pos->theta = fmod(theta, (float)(2*M_PI));
|
|
42 | 42 |
|
43 | 43 |
//Save state for next time in. |
44 | 44 |
motor_fl_ticks = scout_enc.fl_ticks; |
scout/libscout/src/test_behaviors/Scheduler.cpp | ||
---|---|---|
92 | 92 |
Time t = ros::TIME_MAX; |
93 | 93 |
double time = t.toSec(); |
94 | 94 |
|
95 |
Order* best;
|
|
96 |
int best_index; |
|
95 |
//Order* best; // Unused; restore when needed
|
|
96 |
int best_index = 0;
|
|
97 | 97 |
for(unsigned int i=0; i<unassignedOrders->arraySize(); i++) |
98 | 98 |
{ |
99 | 99 |
Order order = unassignedOrders->peek(i); |
... | ... | |
102 | 102 |
|
103 | 103 |
if(end_time.toSec() + MAX_WAIT_TIME < time) //TODO: use another function |
104 | 104 |
{ |
105 |
best = ℴ |
|
105 |
//best = ℴ
|
|
106 | 106 |
best_index = i; |
107 | 107 |
time = end_time.toSec() + MAX_WAIT_TIME; |
108 | 108 |
} |
scout/libscout/src/test_behaviors/smart_runaround.cpp | ||
---|---|---|
290 | 290 |
float right_distance = readings[24]/1000.0; |
291 | 291 |
int right_idx = m_to_idx(right_distance); |
292 | 292 |
|
293 |
ROS_INFO("front: %d left: %d right: %d", front_distance, left_distance, right_distance); |
|
293 |
ROS_INFO("front: %lf left: %lf right: %lf", |
|
294 |
front_distance, left_distance, right_distance); |
|
294 | 295 |
if (right_distance == 0 || front_distance == 0 || left_distance == 0) |
295 | 296 |
return false; |
296 | 297 |
|
297 | 298 |
// determine relative distances on map, based on robot position |
298 |
int up_d, right_d, down_d, left_d; |
|
299 |
int up_d = 0; |
|
300 |
int right_d = 0; |
|
301 |
int down_d = 0; |
|
302 |
int left_d = 0; |
|
303 |
|
|
299 | 304 |
// determine upward distance |
300 | 305 |
switch (dir) |
301 | 306 |
{ |
... | ... | |
331 | 336 |
// map blocks above robot (on map) |
332 | 337 |
for(int u = 0; u < 8; u++) |
333 | 338 |
{ |
334 |
if(u = up_d) {
|
|
335 |
map[row-u][col] = (up_d)?WALL:SEEN;
|
|
339 |
if(u == up_d) {
|
|
340 |
map[row-u][col] = (up_d)?WALL:SEEN;
|
|
336 | 341 |
break; |
337 |
}
|
|
338 |
map[row-u][col] = SEEN;
|
|
342 |
}
|
|
343 |
map[row-u][col] = SEEN;
|
|
339 | 344 |
} |
340 | 345 |
|
341 | 346 |
// map blocks to right of robot |
342 | 347 |
for(int r = 0; r < 8; r++) |
343 | 348 |
{ |
344 |
if(r = right_d) {
|
|
345 |
map[row][col+r] = (right_d)?WALL:SEEN;
|
|
346 |
break; |
|
347 |
}
|
|
348 |
map[row][col+r] = SEEN;
|
|
349 |
if(r == right_d) {
|
|
350 |
map[row][col+r] = (right_d)?WALL:SEEN;
|
|
351 |
break;
|
|
352 |
}
|
|
353 |
map[row][col+r] = SEEN;
|
|
349 | 354 |
} |
350 | 355 |
|
351 | 356 |
// map blocks under robot (on map) |
352 | 357 |
for(int d = 0; d < 8; d++) |
353 | 358 |
{ |
354 |
if(d = down_d) {
|
|
355 |
map[row+d][col] = (down_d)?WALL:SEEN;
|
|
356 |
break; |
|
357 |
}
|
|
358 |
map[row+d][col] = SEEN;
|
|
359 |
if(d == down_d) {
|
|
360 |
map[row+d][col] = (down_d)?WALL:SEEN;
|
|
361 |
break;
|
|
362 |
}
|
|
363 |
map[row+d][col] = SEEN;
|
|
359 | 364 |
} |
360 | 365 |
|
361 | 366 |
// map blocks to left of robot |
362 | 367 |
for(int l = 0; l < 8; l++) |
363 | 368 |
{ |
364 |
if(l = left_d) {
|
|
365 |
map[row][col-l] = (left_d)?WALL:SEEN;
|
|
366 |
break; |
|
367 |
}
|
|
368 |
map[row][col-l] = SEEN;
|
|
369 |
if(l == left_d) {
|
|
370 |
map[row][col-l] = (left_d)?WALL:SEEN;
|
|
371 |
break;
|
|
372 |
}
|
|
373 |
map[row][col-l] = SEEN;
|
|
369 | 374 |
} |
370 | 375 |
|
371 | 376 |
return true; |
... | ... | |
378 | 383 |
{ |
379 | 384 |
// check whether there is a square portion with |
380 | 385 |
// only SEEN or WALL blocks, of the size of the map. |
381 |
int start_row = -1;
|
|
382 |
int start_col = -1;
|
|
386 |
//int start_row = -1; // Unused
|
|
387 |
//int start_col = -1; // Unused
|
|
383 | 388 |
int seen_width = 0; |
384 | 389 |
int seen_height = 0; |
385 | 390 |
for(int r = 0; r < MAP_LENGTH; r++) { |
386 |
for(int c = 0; c < MAP_LENGTH; c++) {
|
|
387 |
if(map[r][c] == UNSEEN) {
|
|
388 |
start_row = -1;
|
|
389 |
start_col = -1;
|
|
390 |
seen_width = 0;
|
|
391 |
seen_height = 0;
|
|
392 |
}
|
|
393 |
else {
|
|
394 |
start_row = r;
|
|
395 |
start_col = c;
|
|
396 |
seen_width++;
|
|
397 |
seen_height++;
|
|
398 |
}
|
|
399 |
if(seen_width >= 15)
|
|
400 |
return true;
|
|
401 |
}
|
|
391 |
for(int c = 0; c < MAP_LENGTH; c++) {
|
|
392 |
if(map[r][c] == UNSEEN) {
|
|
393 |
//start_row = -1;
|
|
394 |
//start_col = -1;
|
|
395 |
seen_width = 0;
|
|
396 |
seen_height = 0;
|
|
397 |
}
|
|
398 |
else {
|
|
399 |
//start_row = r;
|
|
400 |
//start_col = c;
|
|
401 |
seen_width++;
|
|
402 |
seen_height++;
|
|
403 |
}
|
|
404 |
if(seen_width >= 15)
|
|
405 |
return true;
|
|
406 |
}
|
|
402 | 407 |
} |
403 | 408 |
return false; |
404 | 409 |
} |
Also available in: Unified diff