Revision 04114d13 scout/scoutsim/src/sim_frame.cpp

View differences:

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