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