Dealing with angles (and averaging them)

The nasty thing about angle measurements is the fact that they roll over. That is to say if you add 10 degrees to 355 degrees you get 365, but what you really want is 5. So after every addition or subtraction you need to normalize the angle. By this I mean if the new angle is above 360.0, subtract 360 and if it less than 0.0 add 360.0.

If you are adding or subtracting one angle from another angle, then this pseudo code works:

if (ang > 360) {
  ang -= 360
} else if (ang < 0.0) {
  ang += 360

Of course if you want to use radians (and you should) then replace the 360.0 with 2*PI.

Now if you add up a bunch of angles, then the above code does not work. Assume you want 350+350+350. This gives you 1050. Normalize this with the above and you only subtract 360 from 1050 to get 690. This is still not a ‘legal angle’. You may be tempted to put the above pseudo code in a loop till such time as the angle is between 0 and 360, but from experience I would suggest against this. Years ago I did just this, but an error in the code caused the angle to approach infinity. What to guess how long it takes to normalize an absurdly large angle by iteratively subtracting 360 degrees at a time?

You may also be tempted to do some fancy modulo arithmetic to normalize the angle, but I also suggest against that. In the spirit of finding bugs as fast as possible, if you add 4 angles together, you should call the normalize routine 4 times (or pass in an argument for a fixed number of iterations). One the normalization is done, add an ASSERT to the code to check for 0-360.

Now the above assumed you were interested in angles in the range of 0 to 360. This is great for bearings and azimuths. But there are many other angles that are best measured in the range of -180 to +180. For these I have a different normalization routine. I have taken to calling the 0-360 angles Full Circle Angles (FCA) and the -180-+180 angles Half Circle Angles (HCA). Therefore I have an angles library that allows me to add subtract, and perform other operations on FCA, HCA, and a mixture of these angles.

In a previous post I mentioned the issue with averaging angles. Assuming the use of FCA angles, averaging an azimuth of 350 and 10 degrees does not yield the anticipated answer of 0 degrees but rather 180 (350+10)/2 = 180.

After a fair amount of Googling, I have found the answer to averaging. I should mention that there are in fact a gazillion ways to average based on your definition of what an average is for numbers that wrap around like angles. But using the definition of an average direction of motion, then the following works.

y = sin(a1) + sin(a2) + ... + sin(an)
x = cos(a1) + cos(a2) + ... + cos(an)
avg = atan2(y,x)

So a couple of caveats. Make sure you pass the right type of angles (deg/radians) into your trig functions. Second, look at the arguments to your atan2 function. They may be ordered (y,x) or (x,y).

So how does this work? Taking the sin of an angle and multiplying it by 1 gives the y component of a unit vector along your angle. The cosine gives you the x component. If you add up all the x and y values these give you the x and y magnitude of the sum of all the vectors. The resulting vector will be in the direction of the average. Now take the arctan of the x and y values and you get the resulting average heading.

Note that in some cases (such as in a complimentary filter) you may want to give unique weights to each angle. To do this, you can simply multiply the sines and cosines by the weight like this:

y = w1*sin(a1) + w2*sin(a2) + ... + wn*sin(an)
x = w1*cos(a1) + w2*cos(a2) + ... + wn*cos(an)
avg = atan2(y,x)

And that is how I deal with pesky angles!

Posted in Uncategorized | Leave a comment

On stepper motors

I have never designed a system with stepper motors in them, but I have used such systems in the past. After my first experiments with the SUNDAR, it became apparent that the best solution would be to replace the RC servor with a stepper. This I did using a small stepper and control board from Pololu. Both were simple to use and I had a somewhat working system in short order.

But it had warts. The motor did not seem to have much torque , and it would skip steps if I tried to make it spin faster than a crawl. I had been using full steps assuming I would get the fastest speed in this way, but it turns out the answer is more complicated. When using full steps, the coil current is either -100%, 0%, or 100%. Changing from one configuration to another is a large shock to the system. Its like getting hit with a hammer. The inertia of the rotor may prevent it from moving to the next step. But if you micro step, you are gradually ramping up one coil current as to decrease the other. The resulting torque is applied much smoother and the rotor can keep up.

So by decreasing the time between steps but using micro stepping I was able to at least double the motor speed without any lost steps. As a test, I ran the motor for one hour rotating a revolution one direction and then back the other. After the hour, the motor was still perfectly alignent. Now it is possible that I lost as many steps as I gained and had a zero sum gain, but I suspect that the motor simply did not miss any steps.

During this same test I also monitored the temperature of the motor. Rather surprisingly, the temperature went from ambient of 67F to about 130F! This is very different than a DC motor with little load. But a stepper always has the sum of its coils at 100% current and hence consuming 6 watts of power. 6 watts into a small chunk of metal will get hot. Consulting the datasheet the temperature rise was well within spec.

Since these initial tests, I have also done some mechanical work on the SUNDAR. I built a new platform to hold the motor and a new platform for the sensor. I am now working on a way to run the three needed wires from the base to the rotating sensor platform without getting tangled. I am also adding an optical beam break sensor to index the sensor platform on startup.

But at the same time, I am working with a group of elementary to middle school students on their FIRST LEGO League (FLL) research project. Been teaching them how to program Arduinos and the basics of electricity. Since they will be coming over to use my shop next weekend, I have been spending much of my time cleaning up to make room for them. Then the two weekends after that, I am a judge for FLL. Then after that we get close to Thanksgiving and after that Christmas. I do not see a way to get everything I need to complete done by June of next year for the 2014 AVC. Perhaps 2015?

Posted in Uncategorized | Leave a comment

Complimentary filter

I have been struggling with how to fuse all the data into one cohesive mess. In the case of heading I have several source. I have a magnetometer, the heading from GPS, the integrated value from the gyro, and if I can manage it, the solar compass.

They each have strengths and weaknesses. The magnetometer is great as long as the car is not moving, the servos moving, or it sitting on a man hole cover.

The GPS is great as long as the robot is moving a a reasonable speed and has a good lock.

The gyro has no ability to determine north, but for short periods of time can keep track of changes in heading.

The solar compass is fantastic as long as we have the sun and it is not too high in the sky.

So what is a poor robot to do? One solution is to use a Kalman filter to merge the data. But the math is complex and you need  mathematical model of your system to make best use of it. Now I have the habit of picking at everyone I know for knowledge (and even those I do not know). This has lead me to the complimentary filter.

The complimentary filter is nothing more than a weighted average. Rather than each heading having equal weight when taking an average, you assign each a unique weight. The more likely that the value is right the higher the weight you give it.

So how do you assign weights? The literature would have you use the parameter’s covariance. This is the inverse of the standard deviation. A parameter you have confidently measured would have a larger covariance than one you are not so sure about. There are lots of mathematical ways to determine covariance given sample sets of numbers. But in my case, I will take  simpler way out.

My weights will be the inverse of the expected accuracy of the system. Here are the cases:

GPS: Zero if velocity is under 2 meters/second, else it ramps up to 3 degrees at 6 m/s or above.

Magentometer: 5 degrees if motor is off, 180 degrees if on

SUNDAR: Based on the error it reports based on the signal to noise ratio

IMU: Starts with the accuracy of the device that last reset its heading. The accuracy then decreases by 90 degrees per minute before it gets reset by the best azimuth device.

My heading now becomes the weighted average using the equation:

heading = (h1*W1 + h2*W2 + h3*W3 + h4*W4)/(W1+W2+W3+W4)

The problem is this does not work for headings. To see why assume two equally weighted sensors providing the headings of 350 and 10 degrees. The average would be (350*1 + 10*1)/(1+1) or 180 degrees! Oops; wrong direction! We really should have gotten 0 or 360 degrees.

So how does one average headings? A future post will address that.

Posted in Uncategorized | Leave a comment

SUNDAR light sensor experiment

I have been sidelined for a very long time due to work. I have only had a few days off from work in the last 2 months. Makes working on my AVC robot difficult if not impossible. But I have begun to work on my SUNDAR project a bit. I really need to know how strong an ND filter to put in front of my light sensor so that it can look straight into the sun without saturating the sensor.

So I purchased two new sensors from AdaFruit. These are based on the GA1A12S202 chip from Sharp. I supplied them with 5V and digitized the data at 25Hz. The digital data I averaged over 4 seconds. I then delayed 26 seconds before doing it again. This gives me an averaged reading for each sensor every 30 seconds.

The first day the results looked like this: Lux

Both sensors show identical results and saturate during the day. The noisy data is from cloud cover.

The second day I placed a single layer of an ND film over one of the sensors:


Although the voltage drop was not appreciably lower, it was enough to drastically lower the measured Lux. This is due to the logarithmic response (and not linear) of the sensor.

Although this would seem to have dropped the light intensity on the sensor into a good range, I am not convinced yet due to a few issues. The first is that the sensor was not perpendicular to the sun as it will be in the SUNDAR. Second there were two layers of glass between the sun and the sensors. Thirdly, the last two days have been not perfectly clear sky days. I will need to continue running this experiment for a few more weeks at least.

This experiment does have the advantage of proving that it gets lighter during the day and darker at night.

Posted in SUNDAR | Leave a comment

Interesting bugs

Over the last few months, I have been sending to my software group at work a puzzle/quiz in the form of a bug I have found in my robot code. They seem to get a kick out of especialy since I offer a Peppermint Patty as a prize to the first right guess.


Over the weekend I spend a few frustrating hours tracking down a bug in this seemingly simple idiot proof code. All it does is take the absolute value of a long integer (32 bits on an ARM processor). 

long labs(long val)
    long retval;
    if (val < 0)
        retval = -val;
    } else {
        retval = val;
    return retval;

So most of the time this code does exactly what you might expect. It returns the absolute value of the parameter you pass in. All except in one case. Can you figure it out?


What does this code do and why do it this way?

uint32_t val;
val = (val << 3) + (val << 1);


What is the next larger number greater than 8,388,608.0 when using an IEEE float. 

a)      8,388,608.002
b)      8,388,608.2
c)      8,388,609.0
d)      8,388,680.0


Over the weekend I was working on a section of code that would increase then decrease the speed of my robot every 6 seconds to check the stability of my PID control loop. My task was running at 5Hz and each call to GetTimeInMilliseconds returned a number in the sequence 0, 200, 400, 600, … Needless to say I had a bug in this code as it did not change the speed at the desired rate. What did I do wrong?

    uint32_t systemTime;
    double   speedSP;

    /* Get the current time in milliseconds */
    systemTime = GetTimeInMilliseconds();

    /* Alternate between fast and slow every n seconds */
    if  ( (systemTime % 6*1000) == 0)
      /* The error is not in this comparison as 4.0 is exactly represented as a float */
      if (speedSP == 4.0)
        speedSP = 2.0; /* Set speed setpoint at 2 m/s */
      } else {
        speedSP = 4.0; /* Set speed setpoint at 4 m/s */


Posted in AVC, Personal, Software | 4 Comments

Data collection. 1 of n

Today I took my robot to my test area. I collected two battery packs worth of data. I am now begging the long process of analyzing 15 megabytes of binary log files. Oh Joy! No sarcasm here. I love data.

So the first thing I did was to take one run of 2400 samples and plot the speed provided by the GPS and that I compute from the encoder. Here I was a bit worried that I got all the math right with gear ratios and wheel diameters. But as you can see, I was pretty much spot on.

One thing to notice is that the encoder speed is actually much more accurate at slow speeds and particularly at a dead stop. The GPS is never content to say it is not moving but the encoder is.


More data later!

Posted in AVC | Leave a comment

Long time no post

It has been a while since I last posted. A lot and a little has happened in the meantime. I went on vacation to London and had a great time, but I have not made a huge amount of progress on my AVC entry.

What I have gotten is a PID loop module that I will use for controlling speed and heading. I am trying to tune the speed parameters and found that trial and error is not the way to go. I really needed a way to collect some sudden changes and record the output. That allows you to use some math to get close.

I was not about to collect this data under computer control. The safest way would be under RC control. But to do this, I needed a way to record the PWM output to the ESC from the micro. Since my ARM processor has a Timer/Counter peripheral, I wrote some code to measure the pulse width. Amazingly, this seemed to work right out of the gate other than the counts were 1/2 of what I had calculated them to be. I almost let it go, but something told me to dig deeper.

Well I found the issue in the LowLevel initialization routine where the PLL and the main clocks get setup. I had divided the PLL by 4 and not the desired 2. I have been running my processor at 1/2 speed all this time. I changed the dividor expecting nothing to work properly, but was pleasently surprised to see everything working. This means I had properly normalized everything by the clock rate.

I hope to get out this weekend and collect the data I need to tune the speed PID loop. If I get this far, I will then try to stabilize the heading loop.

Speaking of heading, I have too many sources of heading. I have headings from the GPS, the magnetometer, the IMU, and by solar compass. I think I have given up trying to figure out Kalman filters so I am going to implement a Complementory filter. Should be simpler to implement. Just need to figure out a measure of accuracy for each source. I think GPS is best when moving at high speed and the EHE is low. Magnetometer is best when the car is not moving at all. The IMU is ok for short periods of time, and the solar compass is best if it is locked on the sun. Should be doable.

Posted in ARM, AVC, Uncategorized | Leave a comment

Major milestones fall!

Today I was able to check out a few major milestones today. The first is that I determined that I can indeed control the steering servo and ESC via the ARM processor. I was a bit worried that they would not take a 3V3 signal over the normal 5V0. But I can put that worry behind me.

The second milestone was my first test of controlling the steering as a function of cross track error. Although I initially got the sign backwards, as I moved the robot from one side of the course to the other, the steering wheels turned appropriately. Admittedly this was a horribly simple test, but it dis verify the basics of a whole bunch of math.

Next major milestone is to get something to provide a heading (gyro, GPS, magnetometer, …) to start working on the steering and waypoint navigation logic.

Posted in AVC | Leave a comment

LSM303 bug fixed!

After several very frustrating days, I have found the root cause of my bad readings. It was not in the sensor or in its setup. It was in fact the the TWI/I2C driver code that I took from the ATMEL SAM7 libraries. So far every bug that has taken me more than 2 days to find has been in the ATMEL SAM7 libraries. They are riddled with bugs and simplifications that are not mentioned.

The issue here was that the TWI/I2C peripheral is rather different than something like a UART or SPI bus. You start the thing and then you need to hang on for the ride as the peripheral clocks in data. As a byte arrives, you need to be right there to pull it out of the receive buffer. When the last but arrives, you then need to set the STOP condition bit to have the peripheral stop clocking in/out data.

Most of the time this works just fine. You activate the peripheral and sit in a tight loop watching the status register for new bytes. But what happens when an interrupt occurs in the middle of the polling loop? Perhaps an interrupt that takes more 23 microseconds (1 byte at 400kHz) to execute? Perhaps the RTOS tick timer that might need to swap in and out a task and might take several hundred micro seconds? Well the TWI peripheral continues to march along clocking in bytes that get overrun because you are not there to pull them out of the receive register. And because the LSM303 has an auto incrementing address scheme it wraps around back to the start of the data registers this is why I would seem to get all the bytes, but just in funky orders.

It certainly would have been nice had ATMEL included the interrupt disabling code, or at least MENTIONED that any interrupts could screw you over royally. From now on no ATMEL supplied SAM7 code is going to be used without a major scrubbing.

So the solution was simply to disable interrupts for the short amount time needed to clock in the 6 bytes of data (135 micro seconds). A better approach should I fell like being an overachiever would be to make the whole thing interrupt driven with a higher priority interrupt that the RTOS tick timer.

I ran the system for four hours today without a single fault. Before the fix, the problem would phase in and out (as the timer tick overlapped the TWI transfer) and would happen at least every 20 minutes.

“Thank you ATMEL”

Posted in ARM, AVC, Pololu 9DOF IMU | Leave a comment

Woes with a LSM303DLHC

I have a Pololu 9 DOF IMU (version 2) that uses an LSM303DLLC for the magnetometer and accelerometer. With these two sensors you can create a tilt compensated compass. It was my plan to do exactly that to get heading for my robot as I do not fully trust the GPS heading especially moving at slow speeds. But discussion on the DiyRovers forum may make me change my mind.

In any case, it did not take much effort to rehost the sample code from Pololu for my ARM processor. The biggest issue was writing the low level TWI/I2C device driver. Somehow I got suckered into trusting the sample code from Atmel. Even the latest library for the SAM7 was riddled with errors and simplifications. When I re wrote the driver, I was talking to the LSM303 in short order.

Since the IMU also has a L3GD20 3 axis gyro, I quickly wrote the interface for that and added. I then added to my navigation thread the code to sample the sensors and combine the accelerometer and magnetometer data into a heading. On the surface this seemed to work fairly well in a benign magnetic environment.

But in the presence of ferrous materials, the accuracy quickly goes out the window unless you perform some calibration. So I sent a block of time writing the calibration routine to find the various max and min values and rescale the data accordingly. It is not as good a fitting a 3D spheroid to the point cloud, but the math is radically simpler.

It was during the testing of the calibration, that I discovered that on occasion the values from the magnetometer would be completely bogus. Looking at the data more carefully it was obvious that bytes were being lost, corrupted, or swapped. I have now spent way too much time trying to track this problem down.

I have a suspesion of what is going on, but I refuse to make the software change till I get to root cause. Since this error only occurs a few times every 10 thousand samples, I could not see it for a while and think I had solved the problem.

Time passes….

Ok, Now I am no longer sure. I figured that I was reading the data registers at the same time they were being updated. The magnetometer is set for 15 Hz update rate and my nav update thread runs at 5Hz. Over time they were bound to phase in and cause problems.  But watching the I2C data relative to the “data ready” and error condition, I find no correlation.

Next I commented out the reading of the accelerometer  and Gyro and the problem goes away. But now I put them back exactly as they were and the problem still is not occurring. Great Snakes!


Posted in ARM, AVC, Pololu 9DOF IMU | Leave a comment