fixed bug causing robot-step to add old pos and veloc instead of new
fixed int/floating point error in robot-test