Project

General

Profile

Revision fa857318

IDfa857318c5cf70645335548760f19fed4703af1b
Parent ab8736ac
Child 6ee555a3

Added by Priya about 11 years ago

Fixed bugs with how the simulator was doing math for pose estimation.

View differences:

scout/scoutsim/src/scout.cpp
415 415

  
416 416
        // Find linear and angular movement in m
417 417
        float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2;
418
        float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed);
418
        float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed) / SCOUT_WIDTH;
419 419

  
420 420
        //store currently teleop'd scoutname
421 421
        std::stringstream ss;
......
436 436

  
437 437
        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
438 438

  
439
        pos.x += sin(orient + PI/2.0) * lin_dist;
440
        pos.y += cos(orient + PI/2.0) * lin_dist;
439
        pos.x += cos(orient) * lin_dist;
440
        pos.y -= sin(orient) * lin_dist; //Subtract because of the way the simulator handles y directions.
441 441

  
442 442
        // Clamp to screen size
443 443
        if (pos.x < 0 || pos.x >= state.canvas_width
scout/scoutsim/src/scout_constants.h
66 66
    const float PIX_PER_METER = 200.0;
67 67

  
68 68
    // Scout dimensions, in meters
69
    const float SCOUT_WIDTH = 0.05;
70
    const float SCOUT_LENGTH = 0.2;
69
    const float SCOUT_WIDTH = 0.125;
70
    const float SCOUT_LENGTH = 0.23;
71 71

  
72 72
    // Position of sonar relative to robot center
73 73
    const float SCOUT_SONAR_X = -0.01;

Also available in: Unified diff