Revision c63c9752 scout/scoutsim/src/sim_frame.cpp

View differences:

scout/scoutsim/src/sim_frame.cpp
68 68
        srand(time(NULL));
69 69

  
70 70
        update_timer = new wxTimer(this);
71
        update_timer->Start(16);
71
        update_timer->Start(SCOUTSIM_REFRESH_RATE * 1000);
72 72

  
73 73
        Connect(update_timer->GetId(), wxEVT_TIMER,
74 74
                wxTimerEventHandler(SimFrame::onUpdate), NULL, this);
......
77 77

  
78 78
        images_path = ros::package::getPath("scoutsim") + "/images/";
79 79

  
80
        // 200 pixels per meter. @todo make this a constant elsewhere.
81
        meter = 200;
82

  
83 80
        map_base_name =  ros::package::getPath("scoutsim") + "/maps/" +
84 81
                           map_name + ".bmp";
85 82
        map_lines_name = ros::package::getPath("scoutsim") + "/maps/" +
......
160 157

  
161 158
        SetMenuBar(menuBar);
162 159

  
163
        width_in_meters = GetSize().GetWidth() / meter;
164
        height_in_meters = GetSize().GetHeight() / meter;
160
        width_in_meters = GetSize().GetWidth() / PIX_PER_METER;
161
        height_in_meters = GetSize().GetHeight() / PIX_PER_METER;
165 162
    }
166 163

  
167 164
    SimFrame::~SimFrame()
......
314 311
        SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight()));
315 312
    }
316 313

  
314
    // Runs every SCOUTSIM_REFRESH_RATE.
317 315
    void SimFrame::onUpdate(wxTimerEvent& evt)
318 316
    {
319 317
        ros::spinOnce();
......
393 391
        }
394 392
        else if (wxGetKeyState(WXK_LEFT))
395 393
        {
396
            teleop_l_speed = -TELEOP_PRECISE_SPEED;
397
            teleop_r_speed = TELEOP_PRECISE_SPEED;
394
            teleop_l_speed = -TELEOP_PRECISE_SPEED * 2;
395
            teleop_r_speed = TELEOP_PRECISE_SPEED * 2;
398 396
        }
399 397
        else if (wxGetKeyState(WXK_RIGHT))
400 398
        {
401
            teleop_l_speed = TELEOP_PRECISE_SPEED;
402
            teleop_r_speed = -TELEOP_PRECISE_SPEED;
399
            teleop_l_speed = TELEOP_PRECISE_SPEED * 2;
400
            teleop_r_speed = -TELEOP_PRECISE_SPEED * 2;
403 401
        }
404 402
    }
405 403

  
......
407 405
    {
408 406
        if (wxGetKeyState(WXK_UP))
409 407
        {
410
            teleop_fluid_speed += 2;
408
            teleop_fluid_speed += TELEOP_FLUID_INC * 2;
411 409
        }
412 410
        else if (wxGetKeyState(WXK_DOWN))
413 411
        {
414
            teleop_fluid_speed -= 2;
412
            teleop_fluid_speed -= TELEOP_FLUID_INC * 2;
415 413
        }
416 414
        else if (teleop_fluid_speed > 0)
417 415
        {
418
            teleop_fluid_speed -= 1;
416
            teleop_fluid_speed -= TELEOP_FLUID_INC;
419 417
        }
420 418
        else if (teleop_fluid_speed < 0)
421 419
        {
422
            teleop_fluid_speed += 1;
420
            teleop_fluid_speed += TELEOP_FLUID_INC;
423 421
        }
424 422

  
425 423
        if (wxGetKeyState(WXK_LEFT))
426 424
        {
427
            teleop_fluid_omega -= 1;
425
            teleop_fluid_omega -= TELEOP_FLUID_INC;
428 426
        }
429 427
        else if (wxGetKeyState(WXK_RIGHT))
430 428
        {
431
            teleop_fluid_omega += 1;
429
            teleop_fluid_omega += TELEOP_FLUID_INC;
432 430
        }
433 431
        else
434 432
        {
......
439 437
        {
440 438
            teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED;
441 439
        }
440
        else if (teleop_fluid_speed < -TELEOP_FLUID_MAX_SPEED)
441
        {
442
            teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED;
443
        }
442 444
        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2)
443 445
        {
444 446
            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2;
445 447
        }
448
        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED / 2)
449
        {
450
            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED / 2;
451
        }
446 452

  
447 453
        teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega;
448 454
        teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega;

Also available in: Unified diff