Revision fa857318
| ID | fa857318c5cf70645335548760f19fed4703af1b |
Fixed bugs with how the simulator was doing math for pose estimation.
| 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