Revision fa857318
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 |
Also available in: Unified diff