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