Revision 7f095440
Sepearated refresh rate for scouts and for the simulator.
scout/libscout/src/behaviors/line_follow.cpp | ||
---|---|---|
38 | 38 |
{ |
39 | 39 |
double line_loc = linesensor->readline(); |
40 | 40 |
|
41 |
ROS_INFO("Line location: %lf.\n", line_loc); |
|
42 |
|
|
43 | 41 |
motor_l = min(max((int) (-MOTOR_BASE + SCALE * line_loc), -128), 127); |
44 | 42 |
motor_r = min(max((int) (-MOTOR_BASE - SCALE * line_loc), -128), 127); |
45 | 43 |
|
scout/scoutsim/src/scout.cpp | ||
---|---|---|
410 | 410 |
float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2; |
411 | 411 |
|
412 | 412 |
// Find linear and angular movement in m |
413 |
float lin_dist = SCOUTSIM_REFRESH_RATE * (l_speed + r_speed) / 2;
|
|
414 |
float ang_dist = SCOUTSIM_REFRESH_RATE * (r_speed - l_speed);
|
|
413 |
float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2;
|
|
414 |
float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed);
|
|
415 | 415 |
|
416 | 416 |
Vector2 old_pos = pos; |
417 | 417 |
|
418 | 418 |
// Update encoders |
419 |
fl_ticks += (unsigned int) (motor_fl_speed * SCOUTSIM_REFRESH_RATE *
|
|
419 |
fl_ticks += (unsigned int) (motor_fl_speed * SIM_TIME_REFRESH_RATE *
|
|
420 | 420 |
ENCODER_TICKS_PER_METER); |
421 |
fr_ticks += (unsigned int) (motor_fr_speed * SCOUTSIM_REFRESH_RATE *
|
|
421 |
fr_ticks += (unsigned int) (motor_fr_speed * SIM_TIME_REFRESH_RATE *
|
|
422 | 422 |
ENCODER_TICKS_PER_METER); |
423 |
bl_ticks += (unsigned int) (motor_bl_speed * SCOUTSIM_REFRESH_RATE *
|
|
423 |
bl_ticks += (unsigned int) (motor_bl_speed * SIM_TIME_REFRESH_RATE *
|
|
424 | 424 |
ENCODER_TICKS_PER_METER); |
425 |
br_ticks += (unsigned int) (motor_br_speed * SCOUTSIM_REFRESH_RATE *
|
|
425 |
br_ticks += (unsigned int) (motor_br_speed * SIM_TIME_REFRESH_RATE *
|
|
426 | 426 |
ENCODER_TICKS_PER_METER); |
427 | 427 |
|
428 | 428 |
orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI)); |
scout/scoutsim/src/scout_constants.h | ||
---|---|---|
81 | 81 |
// Time it takes for the sonar to spin from position 0 to position 23 |
82 | 82 |
const float SONAR_HALF_SPIN_TIME = 0.5; |
83 | 83 |
|
84 |
const float SCOUTSIM_REFRESH_RATE = 0.05; // s |
|
84 |
const float REAL_TIME_REFRESH_RATE = 0.08; // s |
|
85 |
const float SIM_TIME_REFRESH_RATE = 0.05; // s |
|
85 | 86 |
} |
86 | 87 |
|
87 | 88 |
#endif |
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
68 | 68 |
srand(time(NULL)); |
69 | 69 |
|
70 | 70 |
update_timer = new wxTimer(this); |
71 |
update_timer->Start(SCOUTSIM_REFRESH_RATE * 1000);
|
|
71 |
update_timer->Start(REAL_TIME_REFRESH_RATE * 1000);
|
|
72 | 72 |
|
73 | 73 |
Connect(update_timer->GetId(), wxEVT_TIMER, |
74 | 74 |
wxTimerEventHandler(SimFrame::onUpdate), NULL, this); |
... | ... | |
316 | 316 |
SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight())); |
317 | 317 |
} |
318 | 318 |
|
319 |
// Runs every SCOUTSIM_REFRESH_RATE.
|
|
319 |
// Runs every REAL_TIME_REFRESH_RATE.
|
|
320 | 320 |
void SimFrame::onUpdate(wxTimerEvent& evt) |
321 | 321 |
{ |
322 | 322 |
ros::spinOnce(); |
... | ... | |
402 | 402 |
} |
403 | 403 |
else if (wxGetKeyState(WXK_LEFT)) |
404 | 404 |
{ |
405 |
teleop_l_speed = -TELEOP_PRECISE_SPEED * 2;
|
|
406 |
teleop_r_speed = TELEOP_PRECISE_SPEED * 2;
|
|
405 |
teleop_l_speed = -TELEOP_PRECISE_SPEED * 4;
|
|
406 |
teleop_r_speed = TELEOP_PRECISE_SPEED * 4;
|
|
407 | 407 |
} |
408 | 408 |
else if (wxGetKeyState(WXK_RIGHT)) |
409 | 409 |
{ |
410 |
teleop_l_speed = TELEOP_PRECISE_SPEED * 2;
|
|
411 |
teleop_r_speed = -TELEOP_PRECISE_SPEED * 2;
|
|
410 |
teleop_l_speed = TELEOP_PRECISE_SPEED * 4;
|
|
411 |
teleop_r_speed = -TELEOP_PRECISE_SPEED * 4;
|
|
412 | 412 |
} |
413 | 413 |
} |
414 | 414 |
|
... | ... | |
437 | 437 |
|
438 | 438 |
if (wxGetKeyState(WXK_LEFT)) |
439 | 439 |
{ |
440 |
teleop_fluid_omega -= TELEOP_FLUID_INC; |
|
440 |
teleop_fluid_omega -= TELEOP_FLUID_INC * 2;
|
|
441 | 441 |
} |
442 | 442 |
else if (wxGetKeyState(WXK_RIGHT)) |
443 | 443 |
{ |
444 |
teleop_fluid_omega += TELEOP_FLUID_INC; |
|
444 |
teleop_fluid_omega += TELEOP_FLUID_INC * 2;
|
|
445 | 445 |
} |
446 | 446 |
else |
447 | 447 |
{ |
... | ... | |
522 | 522 |
|
523 | 523 |
for (; it != end; ++it) |
524 | 524 |
{ |
525 |
it->second->update(SCOUTSIM_REFRESH_RATE,
|
|
525 |
it->second->update(SIM_TIME_REFRESH_RATE,
|
|
526 | 526 |
path_dc,sonar_dc,sonar_visual_on, |
527 | 527 |
path_image, lines_image, walls_image, |
528 | 528 |
path_dc.GetBackground().GetColour(), |
... | ... | |
532 | 532 |
|
533 | 533 |
for (unsigned int i=0; i<ghost_scouts.size(); ++i) |
534 | 534 |
{ |
535 |
ghost_scouts.at(i)->update(0.016, path_dc, sonar_dc,
|
|
535 |
ghost_scouts.at(i)->update(SIM_TIME_REFRESH_RATE, path_dc, sonar_dc,
|
|
536 | 536 |
path_dc.GetBackground().GetColour(), state); |
537 | 537 |
} |
538 | 538 |
|
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
79 | 79 |
#define ID_SONAR 10 |
80 | 80 |
|
81 | 81 |
// Absolute speeds (-128 - 127) |
82 |
#define TELEOP_PRECISE_SPEED 62
|
|
82 |
#define TELEOP_PRECISE_SPEED 50
|
|
83 | 83 |
#define TELEOP_FLUID_MAX_SPEED 100 |
84 | 84 |
#define TELEOP_FLUID_INC 8 |
85 | 85 |
|
Also available in: Unified diff