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