Closing the Loop

It is going to work.

My latest great idea is processing the data off-line on my main computer. I can do this in a way such that I can re-analyze it repeatedly, pull out what I want from the data, and do it all without having to re-run the in-motion test.

I took Yukari out again, this time just driving it up and down the street. I ran it down the bike lane line, then over to the center line when the bike lane line ended. I did about 6 laps of this, and got good GPS data the whole time.

Once back, I ran the data through the Passenger program. This code is written in C++, just like the main robot firmware. It reads and parses the recorded data one packet at a time, but does so by reconstructing the variables which the robot originally had on-the-fly. The robot will be able to use this same code to do the same things. And what it is doing is integrating the gyro data, producing a full quaternion of orientation data. Then I transform the nose vector of the robot (it happens to be -Z), then take the atan2 of the z and x components of that vector. This is the gyro heading.

Of course it lacks any absolute reference, and that is what we will use the GPS heading for. If I walk up to the starting line with the robot in GPS lock, in the direction of the first waypoint, the GPS will remember the heading when it stops. I have tested this today.

There was one sick moment when I thought it wasn't going to work. The curve of GPS and gyro headings weren't anything alike. Then I found a bug dealing with going around the corner with TC, which made all the difference. As I was explaining to one of my friends, its like a speedometer. If you know that you are going at 50mph, then in 1 hour you will have gone 50 miles. If your clock calculation says instead that you have travelled for -12 hours, you will calculate that you have gone -600 miles.

I have the gyro turned down intentionally to 100Hz, with a bandwidth of 25Hz. The whole point of the low-pass filter is that the gyro is sampling itself as fast as it can (probably at 800Hz or more), then is taking the average to produce what the gyro would read if it were sampled at a much higher rate and integrated over a much longer time. This smooths out the noise and gives my robot brain less work to do.

Next steps:

1. Figure out how to relate the GPS and Gyro headings. I figure something like a Kalman filter, but somehow have the accuracy of the GPS heading be related to the distance travelled since the last turn.
2. Close the loop. Use the calculated heading to drive a heading control loop.
3. Turntable test. Tell the thing to steer to the north, then while the thing is on a turntable (or in my hands) rotate it back and forth and see how it steers.
4. Road test. Write code which drives forward for 10 seconds, while steering to the heading of the bike lane line. After 10 seconds, change the commanded heading to 90deg right for 1 second, then 90deg right again, then let it run for 10 more seconds.

Also: Last time I was totally panicked about motors and inductonium, so I put in a set of opto-isolators to protect the controller from the motors. This time, when I had the robot brain ground connected to the BEC, the GPS wouldn't lock. So, I am putting in isolators again, this time to break the ground loop.