Programming Progress

I haven't posted in quite a while, and that is because there wasn't really anything to write about. I have done very little robot work in the last few months. The little I had been doing was mostly learning the programming language that I'm using (a limited subset of C++).

Over the past week I've spent a good amount of time on the Robot. I ended up making even more progress than I expected to. I wrote a library (the Arduino version of a class) to do smoothing of all the sensor readings via averaging. Doing it as a library should simplify the implementation of it for each of the sensors. Just getting that working would have made me happy since it required me learning some new formatting and syntax. The library went so quickly that I was able to integrate the functions in that library with my tilt-compensated compass drivers and create finished functions for measuring heading. I did a little bit of testing and it seems to work very well.

A month or so I ago I also did some work with a friend on servo control programming. We decided that the best way to go would be to use a library that someone else had created for doing the control functions. It does exactly what we need, and somebody already did the work for us!

So that is 4 significant parts of the programming(compass/accelerometer driver, tilt compensation, sensor smoothing, servo driver) that are effectively done. Still remaining to do are:

• wind direction sensor driver (should be easy)
• GPS driver
• navigation
• rudder control algorithm
• sail position control algorithm
• over-arching control loop

The only one of those that I anticipate being challenging is the GPS driver, so the programming is starting to look manageable. There is still a good amount of time that needs to be put in, but it doesn't seem too daunting anymore.

It works!!!!

In order to do the tilt compensation for the compass, I need to do a series of coordinate transformations. Unfortunately, I find coordinate transformations to be one of the most confusing things I’ve learned about in my engineering education. I’ve run across a couple instances where I’ve had to use them at work and now one instance where I have to use them for the robot. Every time I do, I get confused.

The basic idea of a coordinate transformation is reasonably easy to understand. Lets say you have a point sitting somewhere in space relative to an X and Y axes. Then imagine you have another pair of axes that are in the same plane and have the same origin, but are rotated at some angle relative to the original axes. You could figure out the coordinates of that point in the new axes by using trigonometry and a few Sin and Cos terms. The problem comes in when you want to deal with stuff in 3-axes and you want to do multiple rotations. The number of terms quickly get out of control and its hard to maintain an understanding of what each term intuitively means. That’s when you start using matrix math. That takes care of keeping all the terms in the equations straight, but you still have to make sure you know what you are doing. The order of which axes you rotate first matters, as does whether your second rotation is about the already rotated coordinate frame, or whether you do it about the original, fixed coordinate system. Lastly, you also need to decide whether you are rotating the data to a coordinate system or you are rotating a coordinate system to the data. Ugh.

So when it comes to the robot and the compass, I need to do a pair of rotations. There will be a rotation for the pitch of the boat (is the bow tilted up or down?) as well as for the roll (is it tilted over on its side?). The compass sensor will take readings while the boat is pitched and rolled, and I want to know what the compass readings would be if it were not pitched and rolled(sitting flat in the water but still pointing in the same direction). Its probably useful to note that I am assuming the sensor (both accelerometer and compass) X-axis will be aligned pointing towards the bow (front) of the boat, with the Y-axis pointing to the port (left) side, and the Z-axis will be pointing upwards along the mast.

The pitch rotation will be first. If the boat is pitched downwards, that is a positive rotation about the fixed y-axis (right-hand-rule). But we want to rotate that coordinate system the opposite direction to align it with the fixed coordinate system, so its really a negative rotation about the Y-axis. After the pitch compensation, the same is done for the roll compensation. The boat tipping to the starboard (right side) would be a positive rotation about the X-axis, so we have to do a negative rotation to compensate for it.

Each of these rotations creates a rotation matrix and since there are two of these rotations, there are two matrices that need to multiplied. Doing the requisite matrix multiplication produces the following two equations. Note that there would be a Z equation as well, but I have dropped that result after the compensation since you only need the X and Y components of the horizontal magnetic field to get the heading. Also note that in the equations below p = pitch and r = roll. The -1 in each of the trig functions signifies that the angle is the opposite of what the boat is actually tilted.

X = x*cos(-1*p) + y*sin(-1*p)*sin(-1*r) – z*cos(-1*r)* sin(-1*p)
Y = y*cos(-1*r) + z*sin(-1*r)

A summary of the assumptions I’ve used are below:
- The accelerometer Z-axis reads +1.0 when the red light is facing the ceiling
- The accelerometer X-axis reads a negative value when it is tilted in a positive rotation about the y-axis (the +x-axis tilts down towards the floor)
- The accelerometer Y-axis reads a positive value when the sensor is tilted in a positive rotation about the x-axis (the +y-axis points up in the air)
- Positive rotation of the sensor about the x-axis gives a positive roll angle (roll to starboard)
- Positive rotation of the sensor about the y-axis gives a positive pitch angle (pitch downwards")
- Pitch and roll angles should be multiplied by -1.0 to make the angles that for the coordinate transforms (coordinate axes rotated the opposite way of the pitch and roll angles)
- Rotation matrix is created using the following transformations:
- Rotate the pitch angle in the fixed coordinate frame first
- Rotate the roll angle in the fixed coordinate frame second

After all that, it does actually work! It took me a lot of thinking and testing to get it to work, but it does work and it seems to work pretty well. It is a bit sensative to accelerations and nearby electronics (computer, phone, etc), but there isn't much I can do about that. Hopefully the accelerations will even out with time-averaging and hopefully we can keep the sensor far enough away from the other electronics.

I should say that I calculated the angles by an over-simplified way to get the tilt compensation working, but for large tilt angles its not accurate. I have the more accurate method figured out (I think), but I haven't tried it to confirm. I'll leave that for another time.

Trying To Tilt Compensate

I haven't been spending my 5 hours a week on this project, but I did spend a couple hours this week trying to put the compass and accelerometer readings together to get the tilt compensation working. I figured that I had the math down and the difficult part would be getting the microcontroller to talk to both the compass and the accelerometer at the same time. It turns out I was wrong on both accounts. It was surprisingly easy to merge my accelerometer and compass codes together, modify some variable names, and get both sensor readings to output to the computer screen. Unfortunately, the tilt compensation math didn't seem to work out, because the compensated outputs didn't make much sense.

Several weeks ago I had spent quite a bit of time trying to figure out the math for doing the tilt compensation. I believe its just a couple coordinate transformations, but it gets tricky when you start figuring out which axis to rotate about first and whether you use fixed axes or the transformed axes for the second rotation. I thought I figured all that out, reduced it to a couple simple equations, and verified it with a spreadsheet. Something isn't right, though.

It may be because I had mistakenly thought that when an axis is tilted downwards in the direction that gravity is pulling, it would read a positive value. It actually reads a negative value. I guess this makes sense if you think about accelerating the sensor along the postive x-direction. It would feel a force pulling it towards negative x, but it reads a positive value since it is accelerating in the positive x-direction. Just to try, I multiplied the inputs by -1 to see if that fixed it, but that didn't work. I don't think its a simple change like that. I might have to multiply the matrices in a different order or something. Unfortunately, I didn't really write out how I figured out the math last time, so I'll have to go through it again. When I have it figured out, I'll be sure to write a blog post on it.

Averaging

I haven't posted in a little while, but I have done some robot work. The main thing I did was to create some code to do averaging of sensor readings. That forced me to learn how to use arrays (groupings of variables), and it also made me realize that I need to think a bit more about how local or global my variables are.

On the Arduino (the brand of micro-controller I am using) website, there is actually an example program that does what they call "smoothing". It basically calculates the running average every time a new reading is put in the array. It does so by keeping a running sum of all the sensor values in the array and divides by the number of values in the array. I don't think that is ideal for our application. I'd much prefer to just periodically poll the sensors and store the data in arrays and then calculate the average only when I'm interested in the value. It seems like that would save calculation time.

I set it up to average the compass heading over a certain number of readings. I arranged it so that the number of readings could be changed just by changing one number at the top of the program. I played around with it a little bit, and I decided that rather than the readings being random, they seem to kind of drift around. The averaging happens so fast that unless I set it to average several thousand readings over a few seconds, it still moves around a bit (+/- 3 degrees or so). I think its just taking readings way faster than the readings are drifting, so its not averaging terribly well. As the programming gets more complex, there will be quite a bit more time between readings, so I think that will improve the averaging somewhat. There will have to be some testing/playing around to determine the optimal delay between readings and the total number of readings to average.

Processing Speed

It turns out I was wrong about not being able to output floating numbers from the microcontroller to the computer. Its just that there was a simple command that I was familar with that would conveniently string together outputs and put them together on one line for you. That function couldn't handle floating point numbers. I've figured out a simple way to do it so that it is in a format I like. I guess this just shows how little I know about programming.

With that thought in mind, I decided this would be a good time to test out how quickly the microcontroller can run through programming, particularly math intensive programming. How quickly it would be able to run some of the geometry functions (like arctan) will go a long way towards determining how frequently the controller will be able to cycle through its programming, as well as how tolerant it will be of the ineficient programming I will be feeding it. Doing a check on this would also give me the chance to learn the syntax of some useful functions like a for loop and commands for measuring time.

To do this I simply put a loop around a big chunk of my compass code forced the controller to do that loop 10,000 times, and checked the time immediately before and after. The loop consisted of:
- offsetting and scaling the three raw magnetometer readings (these were already stored in memory)
- going through 4 conditional checks to determine the proper way to calculate heading
- calculating the heading using and arctangent function
- converting from radians to degrees and then offsetting for declination

Doing all that 10,000 times takes approximately 2.3 seconds, which means it runs through that loop in less than 1/4300th of a second. Not bad for a cheap 16MHz processor. Kinda makes you wonder why your laptop is so slow, doesn't it? A modern laptop processor should be able to run instructions a couple thousand times faster than this microcontroller.

Slow Work on the Compass

Last night I tried to do something that I thought would be pretty easy. I was going to take just the horizontal components of the magnetometer (ignoring the large vertical component like a compass does) and output the heading. Unfortunately, I ran into a couple problems.

The big problem was that the outputs weren't working and didn't make much sense to me. It turns out that the problem was related to not being able to output floating point numbers (numbers like 3.14 that are not simple integers) to the computer screen. I thought that when I output float numbers as integers, it just lopped off the numbers after the decimal point. It does do that, but doing so somehow screws up the other outputs. This is becoming a significant problem for debugging, so I think that I might have to use an alternate method for outputting these values. I think I saw online that other people had created custom functions to output float numbers, so hopefully I can just copy their code into mine. That might be my next task.

After I figured out that this was the problem, I was able to change a couple things and output just the numbers I really needed. The code did work reasonably well. I put the sensor flat on a table next to an actual compass, and they read pretty similar. It seemed like the sensor was reading maybe 3 or 4 degrees off from the compass. Not terrible, but not great. I'm wondering if magnetometer readings might need to be recalibrated. There is definately some noise in the readings. It would wander by about +/- 3 degrees as it sat there. That should be ok since I plan on averaging the readings. It was also very sensitive to being tilted. Just 10 degrees or so of tilt would cause the reading to vary by maybe 30 degrees or so. Hopefully the tilt compensation I have planned for it will work well.

There is one more problem I thought of. The heading is calculated simply as arctan(ycomponent/xcomponent), but the arctan function only works from -90 degrees to +90 degrees, so you need to use a couple conditional "if" statements to get from 90 to 270 degrees. That worked, but I realized that there is a discontinuity that will cause a problem for averaging. Right now, it will read 269 degrees, but if you rotate a couple degrees further, it will read -89 degrees. While that is fine in most cases, if the boat is heading towards 270 degrees and is trying to average several compass readings, the average of 269 and -89 is 180 degrees, not 270 degrees! The same problem happens if you shift that discontinuity to zero degrees (or anywhere else). It also happens when using degrees or radians. I'll have to figure out a way to deal with that.

Tilt Sensor

We have a 3-axis accelerometer to go along with the 3-axis compass. Its actually all on the same circuit-board that is smaller than a stamp. The idea is to comebine the accelerometer and compass readings to give us a tilt-compensated compass. As mentioned in my last post, the magnetic field isn't a 2-dimensional thing, it has components in all 3 directions, so when you tilt a compass, it will give you incorrect readings (that is why the needle on any hand compass you buy has some room to level itself to gravity). Using the accelerometer tells us how much the boat is tilting and we can therefore compensate for that tilt when looking at the compass readings. That is going to require a bit of math before we get to that point, though.

There was already some code written to get the accelerometer talking to the microcontroller (debugging output goes from there to a computer). Much like the compass, the outputs need to be adjusted and normalized. Again, I'm modifying them to read -1000 to +1000 (+1000 is equal to 1-g) on each axis. The raw outputs for each axis are approximately:
X-axis: -256 to 270
Y-axis: -262 to 264
Z-axis: -267 to 242

After completing the output modification, I decided to calculate the angle that the sensor is tilted. On one axis that can be done as pitch=asin(x-reading/gravity). Since I scaled the readings to be 1000 for 1-g, gravity can be replaced by 1000 in the equation above. The same can be done for roll angle using the y-axis.

I was a bit worried about noise in the readings, but it turns out to not really be an issue. When I'm looking at the scaled results and holding it as steady as possible on a table, it does have some variability. For instance, it might range from 482 to 490. Due to the the Sine function, when you are close to zero, the sensor reading needs to change a relatively large amount to change even one degree. As the angle gets closer to 90 degrees, small changes in the reading produce large changes in angle. Realistically, we don't care too much about the compass reading if the boat is pitched 90 degrees downwards. We'd have bigger problems in that case.

Waves and other things might affect the acceleration somewhat and therefore the calculated angle and compass heading, but I'm hoping that we can take readings frequently and average them over time to smooth out those errors.

It works pretty nicely now. The readings for moderate tilt angles (up to 60 degrees at least) are pretty smooth and steady. Tilted at 45 degrees and held on a table or something solid, it will vary less than one degree, which seems good enough to me. When you get close to 90 degrees, it gets a bit wierd, but that is expected. Again, I'm not too worried about being able to precisely measure 86 or 89 degrees of roll.

It seems like I should have gotten more done tonight, but I was having problems with the microcontroller to computer interface, so that slowed me down a bit. For some reason, there does not appear to be any good way to output floating point numbers (decimals) to the computer. I looked it up online and other people had the same problem. It will, however, output the integer portion of a floating point number, so I ended up just multiplying the angles by 10 so I could see the first decimal place. Not a problem when this is actually in operation on the sailboat, but its annoying for testing and debugging the software.

Writing the blog posts also takes up a bit of time, but since I have a poor memory, I suspect some of these posts will be useful to look back on in the future.