Revision 04114d13
| ID | 04114d139191ab6a6893f0af9770f0a8ed341778 |
Fixed teleop with new units, and changes the refresh rate to be closer to true m/s.
| scout/scoutsim/src/sim_frame.cpp | ||
|---|---|---|
| 314 | 314 |
// Runs every SCOUTSIM_REFRESH_RATE. |
| 315 | 315 |
void SimFrame::onUpdate(wxTimerEvent& evt) |
| 316 | 316 |
{
|
| 317 |
cout << frame_count << endl; |
|
| 317 | 318 |
ros::spinOnce(); |
| 318 | 319 |
|
| 319 | 320 |
teleop(); |
| ... | ... | |
| 324 | 325 |
{
|
| 325 | 326 |
Close(); |
| 326 | 327 |
} |
| 328 |
|
|
| 329 |
frame_count++; |
|
| 327 | 330 |
} |
| 328 | 331 |
|
| 329 | 332 |
void SimFrame::onPaint(wxPaintEvent& evt) |
| ... | ... | |
| 391 | 394 |
} |
| 392 | 395 |
else if (wxGetKeyState(WXK_LEFT)) |
| 393 | 396 |
{
|
| 394 |
teleop_l_speed = -TELEOP_PRECISE_SPEED; |
|
| 395 |
teleop_r_speed = TELEOP_PRECISE_SPEED; |
|
| 397 |
teleop_l_speed = -TELEOP_PRECISE_SPEED * 2;
|
|
| 398 |
teleop_r_speed = TELEOP_PRECISE_SPEED * 2;
|
|
| 396 | 399 |
} |
| 397 | 400 |
else if (wxGetKeyState(WXK_RIGHT)) |
| 398 | 401 |
{
|
| 399 |
teleop_l_speed = TELEOP_PRECISE_SPEED; |
|
| 400 |
teleop_r_speed = -TELEOP_PRECISE_SPEED; |
|
| 402 |
teleop_l_speed = TELEOP_PRECISE_SPEED * 2;
|
|
| 403 |
teleop_r_speed = -TELEOP_PRECISE_SPEED * 2;
|
|
| 401 | 404 |
} |
| 402 | 405 |
} |
| 403 | 406 |
|
| ... | ... | |
| 411 | 414 |
{
|
| 412 | 415 |
teleop_fluid_speed -= TELEOP_FLUID_INC * 2; |
| 413 | 416 |
} |
| 414 |
else if (teleop_fluid_speed > 0)
|
|
| 417 |
else if (teleop_fluid_speed > TELEOP_FLUID_INC)
|
|
| 415 | 418 |
{
|
| 416 | 419 |
teleop_fluid_speed -= TELEOP_FLUID_INC; |
| 417 | 420 |
} |
| 418 |
else if (teleop_fluid_speed < 0)
|
|
| 421 |
else if (teleop_fluid_speed < -TELEOP_FLUID_INC)
|
|
| 419 | 422 |
{
|
| 420 | 423 |
teleop_fluid_speed += TELEOP_FLUID_INC; |
| 421 | 424 |
} |
| 425 |
else |
|
| 426 |
{
|
|
| 427 |
teleop_fluid_speed = 0; |
|
| 428 |
} |
|
| 422 | 429 |
|
| 423 | 430 |
if (wxGetKeyState(WXK_LEFT)) |
| 424 | 431 |
{
|
| ... | ... | |
| 441 | 448 |
{
|
| 442 | 449 |
teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED; |
| 443 | 450 |
} |
| 444 |
if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2)
|
|
| 451 |
if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED) |
|
| 445 | 452 |
{
|
| 446 |
teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2;
|
|
| 453 |
teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED; |
|
| 447 | 454 |
} |
| 448 |
else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED / 2)
|
|
| 455 |
else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED) |
|
| 449 | 456 |
{
|
| 450 |
teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED / 2;
|
|
| 457 |
teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED; |
|
| 451 | 458 |
} |
| 452 | 459 |
|
| 453 |
teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega; |
|
| 454 |
teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega; |
|
| 460 |
int l_speed = teleop_fluid_speed + teleop_fluid_omega; |
|
| 461 |
int r_speed = teleop_fluid_speed - teleop_fluid_omega; |
|
| 462 |
|
|
| 463 |
teleop_l_speed = max(MIN_ABSOLUTE_SPEED, |
|
| 464 |
min(MAX_ABSOLUTE_SPEED, l_speed)); |
|
| 465 |
teleop_r_speed = max(MIN_ABSOLUTE_SPEED, |
|
| 466 |
min(MAX_ABSOLUTE_SPEED, r_speed)); |
|
| 455 | 467 |
} |
| 456 | 468 |
|
| 457 | 469 |
void SimFrame::teleop() |
| ... | ... | |
| 490 | 502 |
return; |
| 491 | 503 |
} |
| 492 | 504 |
|
| 493 |
if (frame_count % 3 == 0) |
|
| 494 |
{
|
|
| 495 |
path_image = path_bitmap.ConvertToImage(); |
|
| 496 |
Refresh(); |
|
| 497 |
} |
|
| 505 |
path_image = path_bitmap.ConvertToImage(); |
|
| 506 |
Refresh(); |
|
| 498 | 507 |
|
| 499 | 508 |
M_Scout::iterator it = scouts.begin(); |
| 500 | 509 |
M_Scout::iterator end = scouts.end(); |
| ... | ... | |
| 505 | 514 |
|
| 506 | 515 |
for (; it != end; ++it) |
| 507 | 516 |
{
|
| 508 |
it->second->update(0.016, path_dc,sonar_dc,sonar_visual_on, |
|
| 517 |
it->second->update(SCOUTSIM_REFRESH_RATE, |
|
| 518 |
path_dc,sonar_dc,sonar_visual_on, |
|
| 509 | 519 |
path_image, lines_image, walls_image, |
| 510 | 520 |
path_dc.GetBackground().GetColour(), |
| 511 |
sonar_dc.GetBackground().GetColour(),
|
|
| 521 |
sonar_dc.GetBackground().GetColour(),
|
|
| 512 | 522 |
state); |
| 513 | 523 |
} |
| 514 |
|
|
| 515 |
frame_count++; |
|
| 516 | 524 |
} |
| 517 | 525 |
|
| 518 | 526 |
bool SimFrame::clearCallback(std_srvs::Empty::Request&, |
Also available in: Unified diff