   Problem when updating a Robots Map position, depending on movement.
  Hi,I'm doing something fundamentally wrong here - but I don't know what.I have a robot that travels around it's environment and creates a 2D cartesen occupancy map. Simple enough. Problem comes to when it performs a 180 degree arc turn. In one case it works fine, as I expect, in the other when it map updates it locates the robot is a place that's completely wrong:Case 1 - WorksManual arc-turn of 180 degrees, results in the map tracking the robot correctly, with it slighty shifted to the left or right (Depending on turn direction) and facing the opposite way to the way to was before. OKCase 2 - FailsAuto arc-turn of 180 degrees, results in the map placing the robot in completely the wrong position.The difference between the two is that in 1 the angle and distance travelled is changing slowly. In case 2 the angle and distance travelled is sudden (But through experiments is the same as it would have been for case 1). i.e. Overall distance travelled is 5cm say and angle it 180 degrees.Can anyone suggest where I'm going wrong here?Many thanksMarkCode snip as follows:<code>DistanceTraveled = ( ((abs(RobotInfo->WheelClicks[0] - m_LastWheelClicks0)*RobotInfo->RobotConfig.DistancePerClick))+((abs(RobotInfo->WheelClicks[1] - m_LastWheelClicks1)*RobotInfo->RobotConfig.DistancePerClick))  );DistanceTraveled = DistanceTraveled / 2.0;DistanceTraveled = DistanceTraveled/static_cast<float>(EnvironmentScaler_CM);HorizontalComponent = DistanceTraveled * cos(Utilities::Deg2Rad(static_cast<float>(RobotInfo->GyroOrientation)));VerticalComponent = DistanceTraveled * sin(Utilities::Deg2Rad(static_cast<float>(RobotInfo->GyroOrientation)));ActualCurrentPositionX = static_cast<float>(ActualCurrentPositionX + HorizontalComponent);ActualCurrentPositionY = static_cast<float>(ActualCurrentPositionY + VerticalComponent);</code>   
  Your code is pretty unbearable to read, but besides that, it seems to be just a problem with integration (since you didnt exactly define "completely wrong").In the manual case the robot has a bit of translation added with each "step" as the angle slowly changes. When you do it in auto, which sounds like it happens over 1 step, the distance travel the same but it's not even distributed out over incremental angle changes. So you end doing something like, rotating in place 180deg, and then translating 10cm, where as if it took place over 10 steps, you'd translate 1cm at each of 18degree angle increments. Which should give you a lot more accurate results.You'll need to maintain some kind of update step, regardless of the mode, to give you some form of deterministic output. Nothing should happen instantaneously when using an iterative method like you are. You have to still update in small steps as if reading the real robot data.It's a very common problem with euler integration.Manualy integrate a linear body with a fixed acceleration using euler integration at timesteps of 1hz and 10hz and you'll see a very significant difference.   ;  BzroomThanks for that the "It's a very common problem with euler integration" is good know, was starting to think I had something woefully wrong with my solution.Yep you assumption is correct case 1 the data is coming in between each step, in the second it's just one data packet after the motion is complete.Going to artificially break that up, should sort it out.CheersMark;  Our robot continuealy emits status regardless of the type of command we sent it. There are no commands which go off and do their own thing and then report back on completion. We watch it the whole time. If it ever deviates from the instructed path we stop it immediately.This was very beneficial in the early stages when we'd tell it to go one way and it'd go the opposite direction :P Ops, but at least we were tracking its status and not tracking the commands that we had sent.Also, if someone were to grab the robot and make it travel (in your case) continually watching the status will be able to track those changes, expecting a status report at the end of a command will not.   ;  Actually i have a better suggestion, something i would have do differently if i could start over.I'd put (what we call) the navigation, or dead reckoning in the robot itself. You could then have it update at your motor task frequency to acheive pretty high resolution.Besides the high resolution it would allow tracking even if communication were severed (not an issue in our application, the robot is somewhat incremental). When the connection was restablished a status report would indicate the displacement since the last report.In this way you could tell the robot to do a 180, and wait for the report, while internally it is tracking the displacement at high frequency. I would have much prefered to have done our robot this way. It would have made the control application very light but made incremental development changes more difficult. So that may be a factor in your case as well.But just think if your robot reported ActualPositionXY instead of just motor counts.Either way, artificially subdividing the movment like you suggested is not going to be very reliable. Perhaps there was an obstruction for the first half of the move, allowing one wheel to complete its move sooner. This is going to have a vastly different outcome then if both wheels were traveling at the same (desired) time. You'll want your tracking to make as few assumptions as possible, to be a secondary verification that is for the most part infalable.   ;  Umm good idea think I see what you mean.My set up, probably similar to yours - amonget other senor readings, the robot reports delta distance moved by the wheels and gyro orientation (Ignoring drift for now)So get the Navigation system to send current XY to the robot, and have the robot update those coordinates as it moves, sending info back to the navigation and mapping system to update.Hopefully the robots got enough memory left to do that (only 32K on the MPU :) )As I quickly discovered just breaking up one lage movement into smaller ones and integrating them, didn't work at all - not quite sure why I thought it might. Hey hoo.Mark   