Monday, June 18, 2012

A Car, Beta dog food and coding in the block house.

This last weekend I competed in the Sparkfun Autonomous vehicle competition.  I did this with a 1/5 scale RC car, a brand new NetBurner platform (NANO54415) and a lot of last minute coding. (My son and I call this last minute coding coding in the blockhouse.) As I was leaving the house to catch the plane to Denver my son asked my if I was ready and how was it going.  My one sentence answer was I'll be coding in the block house.  This refers to all of our rocket testing, one really wants to show up on site with everything done and ready to go. Reality does not always work that way and  final software is almost always the last thing that gets finished.  This case was very similar....

On to my story...
A month or so ago I had all of the Major car hardware working and shot this video

  This demonstrated that all of the basic hardware was working....
I then started working on the video vision system to find the bonus Arch.
I was capturing video, converting to red only and then edge finding... all of this was working
when spark fun published the course preview saying the arch would be fixed on one position so one could find it with precision navigation, no need to find it... Argh!!!! I'd spent weeks working on the vision and building a duplicate arch etc....



So at this point I sort of stopped working on the Car and tried to get my airborne entry flying.
This was my first strategic blunder, trying to do too much.
I was trying to do too much and on June 4th I gave up on the Plane and went back to the car.
In the process I wrote a simulator....



I also started driving around the a nearby building that was in a similar configuration to the SparkFun building.  In driving around this building I learned several things:

1)My test case in the parking lot had really clean walls, the test building walls were less clean, and had a section of windows where the Laser Range finder  sometimes went through and sometimes did not.

2)I really needed to add a wheel turn counter so I could control precisely where to wall follow and where to just do dead reckoning.

3)As I suspected the low cost L1 GPS was not consistently precise enough to drive around the building reliably.

At this point I made the second biggest strategic blunder of the contest.
I got the impression that spark fun AVC was a pretty low budget hacker event for most participants. In my leftovers from the LLC project I have a pair of Trimble BD950 L1,L2 survey grade RTK GPS receivers.  I also have a pair of survey grade choke ring  L1,L2 Omnistar capable antennas.  I thought about bolting this GPS to the car and using that for navigation.  I personally thought that using a $5K  GPS receiver setup while legal would be un-sportsman like.  (Note that the car that won used the same chassis I did and a really nice expensive GPS receiver. So lesson learned don't handicap yourself, if you have an advantage use it.....

So I went back to the shop and machined a couple of mounts to  mount an LED and photo transistor looking through the main drive gear on the Car.  I then hooked this up to an interrupt pin on the NetBurner and started counting pulses.... only this was not really reliable... and out comes the scope...
The photo transistor was not  reliably pulling the signal to logic low.  It was only going to about 1.2V and if I changed the resistive pull up it go noisy...  So I move the input from the interrupt to an A/D input.  Only to discover that on our beta release for the Netburner Nano54415 the only A/D example was polled not interrupt driven.  So since it was really my day job to write all the divers for the Nano time to write an interrupt driven a/D system for the nano... the first attempt ended up with a single channel A/D conversion interrupt rate at 1.2Mhz seemed like over kill for a pulse whose maximum rate was going to be 500hz or less.... so I turned the A/d clock way down to its minimum value and added in the other 8 A/D channels (only 6 are pined out the chip has 8)  and got the rate down to 20Khz or so.   The Odometer now was 100% perfectly reliable. (This is the eat your own dog food part.. Ie at Netburner we don't just make modules, we actually try to use them on a regular basis to do things so we understand the customers point of view)

In fact I did a trip around my test building and the dead reckoning was pretty good..
I changed my data logging format so I don't have a copy of the DR trace from my first DR run around the local building, I'm posting one from Sparkfun instead)
Here is a DR plot of two laps around the spark fun building Thursday night.... I drove from where my car was parked in the street out front and went around the building twice...



 The DR was almost good enough to to the task all by itself.... I was off about 15ft per lap.
I then started setting up Garbage cans in my drive way and using the LASER range finder to dodge them while staying on course....  I learned that having the laser scanner rotate 90 degrees
to look ahead while also measuring the side range for navigation was problematic. It was fast enough, but the timing of the servo drive and the main command loop were asynchronous enough that I was never really sure where the scanner was pointed and could not properly associate range results with what heading they were on...  As I planned on having spare of most everything I then took my spare range finder and mounted it 90 degrees to primary range finder... so now I had two range finders
This was the final physical I/O configuration...
  • 3 RC channels from RC receiver in to timer channels.
  • 8 RC channels out through a shift register using a timer channel.
  • USB serial port for talking to dev system, 
  • 1 Serial port for receiving data from the IMU (A DIY Drones IMU board)
  • 2 Serial port for receiving data from the Laser scanners.
  • 1 Serial port receiving data from the  Sparkfun Venus GPS.
  • 1 A/D for measuring the Odometer.
  • 1 DSPI talking to an micro SD card for log data
So then I modified my simulator to have two Laser ranges...


While using the laser range finder to map  barrels in my drive way I accidentally launch the car down the drive way.... still attached to my laptop thus ripping  the ethernet jack out of the back.
Ebay to the rescue I got a super rugged   lap top from Ebay three days later I had the joy of moving all my tools and code from old laptop to new laptop with out the benefit of ethernet.


I am now very much running out of time. On Tuesday the data from the side laser range finder is really flaky So Wednesday Morning I order another range finder so I have a spare and have it shipped to my hotel room in Boulder.

Wednesday night I go back out to my nearby test building and drive around a few times collecting data.  I then spend Wednesday evening fine tuning my data reduction method.  For my rocket project I'd written a custom data display application and that was a huge effort.  As this seemed to be a simpler project I used a different approach...

The logging was almost identical, IE I logged everything.  I stated out by logging to the SD card, but managing file names and when to flush the buffers etc... was a hassle, as I have 64M of available RAM I just logged to RAM and then at the end of the run FTP'ed that data set to the lap top. Quicker and easier than  the SD card in this case.   For a vehicle that might physically crash such as an airplane or helicopter, or rocket my strategy is to write to the SD card physical sectors directly so the data is always fresh, in this case with the car it did not really apply so I just left it in RAM. (More on the cost of this decision later)

So to reduce the data I wrote a program that would convert the binary log file in to a CSV  file and look at it with excel I could do linear plots and scatter plots etc...   worked, well but it was slow as to add plots or graphs I had to manually go though the excel interface and add charts...

I eventually figured out how to highlight specific ranges of the data on the scatter plot giving me the ability to tie a specific piece of data to a specific physical location. (See the DR plot above the Pink is  a highlighted section of data.   Properly viewing multiple rate data is (for me) an unsolved problem.
My Rocket display code worked well, but it was purely linear display, no way to tie the data to a physical location or map. I should probably write something to permanently put in my toolbox for this as its a thorn that keeps poppoing up over and over... either that or learn to wrote VBA code for excel.(yuck).

So Wednesday night I pack up everything, all the spares the car 4 sets of laptop batteries, etc...
I only had one set of Lipos for the car as I was concerned about bringing big Lipos on the plane so the previous week I'd sent 3 sets of Lipos for the car to my hotel in boulder UPS ground.
Soldering tools,  a spare everything....
(About 6 pm Wednesday I realize that while I have a spare IMU its got the wrong code in it, so I make a trip to my office where the code load for the IMU Was and reprogram the spare)

I get to bed by midnight or so... and leave the next morning at 5:30 am to catch the plane.
My flight leaves at 7:45 but I'm checking and carrying a lot of strange bits and pieces so I want to allow lots of time.  The check in goes fine and the security line is huge, I have my wife waiting in the waiting lot to take the Lipos from me if they don't clear security.

Good thing I had her wait as I go through security it dawns on me I've only gotten three of the four boxes and bags I needed, and I left one in the trunk of the car.... I call her pick that up and wait in line again.  Security lets me through.... does not even glance at the 4 laptop batteries or the 3 Large Lipos in my bag.

I arrive in Denver and go to get my rental car.... I have a reservation alas the previous week there was hail in Denver damaging more than half of the rental car fleet. it takes four hours to get my car. There were people in the back of the rental car line that had reservations for a car and weren't going to get a car.  There just weren't enough available...

So I drive to boulder, eat some lunch and check into my hotel room. I unpack everything and assemble the car.  I drive it around the hotel parking lot to make sure that all of the sensors are working... and head for Spark fun...  its clear that they are setting up for the AVC, but there are too many cars in the parking lot to take any useful data to I walk around mentally looking at things and go back to my room.  I return to Spark fun around 9 pm that night after all the Spark fun people have left and I'm one of three cars testing at Spark fun.

I drive the car around the building a few times recording all the laser ranges and GPS.
the side facing Laser range finder is still flaky even after swapping out the unit.
So I point the good laser range finder to the right and take two more runs around the building recording that.

I go back to the car make sure I've captured good data and go back to the room.
I get to bed around midnight and get up at 6:15 Friday morning.

I reduce all the data I took into a set of GPS coordinates and a set or range heading and odometer readings.  In the last week the AVC project has destroyed my training schedule for the Half Ironman I want to do on my 50th birthday in September , so I use an hour or so to clear my head and get some exercise. At around 9 am I go find the Boulder recreation department public pool and swim about 1.5 miles.  Side note swimming is usually a bit  hypoxic to begin with, swimming at 5000+ ft of elevation and at fast training pace is hard...)

I go find Lunch and then head back to the room.  There is a big empty parking lot that is blocked from traffic near the hotel.  I'm not sure what it was for, but it had barriers at the entrance and a bunch of light poles with big concrete bases that were goo barrel dodging simulations.

For practice I drive around four poles and reduce that data to a program that would navigate around the barrels  using the barrels and laser range to find the corners... This is working well and then I start testing the barrel dodging code and it all stops working.... it seems that I let the primary battery get too low and the Laser range finders all lost their calibration ARGHHHH!  Its now 2 pm and Spark fun has a tour at 3PM, I really want to take the spark fun tour so I drive out to Sparkfun and take the tour.  Back at the hotel at 4:30 pm and work on re calibrating the Laser range finders.... I decide to switch the laser range finder to raw count uncalibrated mode and just use raw counts an do the cal myself...

Here I realize what is probably Strategic blunder #0  assuming that other embedded programmers did their job.   The Data sheet for the Laser range finder says accurate to +/- 1 Yd no mention of resolution, yet the data print out in Ft mode was DIST:12.3F  IE it had lots of resolution, or so I thought... it seems the absolute raw resolution is about 0.5 yd's and the data after the decimal point was 100% meaning less.  I had much less resolution that I though I had and finding the arch based on this distance was going to be dicey....

I'm now really worried that the Laser range finding navigation scheme is not going to work at all.
So I do what I should have done weeks ago in San Diego, I write the GPS following code to have a back up plan.  I go out to my test parking lot  and take GPS coordinates and test this code I successfully drive around and around the parking lot using GPS.  It works real well and is more accurate in the open parking lot than it was near my test building.

So at this point its about 9pm and I have a Laser nav program and a Gps nav program set up for Spark fun... both untested on site....  I stop at McDonalds get a burger and a very large Ice tea caffeine supply and proceed to Spark fun.

There are about 5 groups driving around the building with their car.
Between 10PM and midnight I see one group drive their car into the pond twice.....

The first half of the course works well almost immediately....
Finding the first corner is a bit dicey and  the spark fun people have put up 2" diameter posts with a  8" diameter base all around the building on the edge of the sidewalk. The laser scanner keeps catching glimpses of these and thinking its too close to the building and as a result it heads for the pond..

My RC kill switch is fast and I save it several times... So I tune the algorithm to take 16 point averages and only use the ranges from the highest 8 or so readings...  I also start setting up exclusion zones where the system ignores the laser and just drives on heading... for some fixed distance.

My first around the building state machine had 8 states...
Numbers are odometer counts, reset means set odo back to zero...
/*
DR Version...
//Heading 85 deg Wall follow at 25 ft at 750 arm
//Hold heading till distance jumps by 20 ft
//Then coast 300
//Then turn to 167 Reset Odo
//Then coast 100
//Then follow wall at 28 ft till odo 2000.
//Hold Heading wait till distance jumps to 35 reset odo
//Coast 600
//Then -105 Deg reset odo
//Coast 200
//Wall follow at 42 Arm at odo 1100
//Hold Heading wait for Jump to 52
//Turn to -18 deg Reset Odo Coast 100
//Then wall follow at 20 ft. Arm at 2200
//When distance goes to 30 fto turn to 85 deg
//Coast 100
//Wall follow at 25 ft till odo 1200
//Done.
*/








I have two problems, First the parking lot is not flat, in some places its enough not flat that the car tips enough that the side laser scanner (still being flaky) points at the ground not the building....
So with a bit a cardboard and tape I re point the laser scanner up about 15 degrees.... things get much more consistent.

 I keep having a problem with the third turn, no tuning I do seems to make it work. Its now about 2am and I'm at less than peak performance....

So I switch to my GPS code and tune that a little, about 8 tries later the GPS version drives around the building, but in the next three attempts it only works 2/3 of the time. The variance in the low cost GPS is just too much... I save this code as  a back up and go back to laser scanner mode...

I immediately see what my issue is I forgot a break in a case statement, so no matter how much I told it to extend out the third turn it just went to the next state turn ... I fix this and a similar copy and paste error two steps later. Next attempt it extends the third turn out way too far and changing this back  and now we make it almost all the way around the building.. Fix up the forth turn and we finally make it all the way around the building autonomously with out error.  We do this twice more...
I've been runnign after the car all night to see what it is doing. so its speed is set to as fast as I can run... as the night wears on the speed gets set slower so I can keep up.

So I turn the speed back up start the car, watch to make sure it gets past the pond and then I go back the other way to see it come out... no car....
I find it in a bush very far from where it should be....

I try again same result slightly different bush...

I turn the speed back down  to where it worked...

Its in a different bush...

So I now run after it....  it works perfectly....
Again I run after  it works perfectly.... my knees are starting to hurt from doing wind sprints all night...

I try to launch it and not run after it... strange bush... doesn't work.

Then it dawns on me... when it looses sight of the RC transmitter the RC receiver goes into fail safe mode and stops the car... then when I come looking for it it sees the RC transmitter  before I see it and the RC Tx is still in autonomous mode so the car sees this as a manual -> autonomous toggle and starts thinking its at the beginning of the course!!!!! ARghhhh!  So I turn the speed up a little and walk ahead of the car as far as I can still retain manual control.... I then start the car and run as fast as I can and it goes around the building... whooo its all working.... then the laser scanner stops working at all!!!   Its starting to get light and the first person to show up around 5am with their car says to me early morning or late night.... I make some kind on unintelligible response and one look at me makes him worry that the zombie apocalypse has come to Boulder....

The flaky Laser sensor has felt like bad wiring from the very beginning, if disassemble the housing and hook it directly up to the laptop serial port and  power it with an external battery it works every time. I've pulled tugged, and re soldered all the wires from the LASER range finder back to the dev board about 2000 times and can never find a bad connection.....

It starts working again.... but in the process I break the glue joint on the mount....
So I need some glue to re glue the  mount and ask the spark fun people (now here setting up) where to find a hardware store and breakfast near by...   Or where can I find power to use the Hot Melt Glue gun in my took kit ...   They turn on power to the pits area and I hot melt glue the laser scanner mount back together... laser scanners are not both working...

At this point the 4th laptop battery is dead and I go to my car to get the charger only ot realize its in my room at the hotel...

Into the car and drive back to the hotel stopping at the same McDonald's to get breakfast.
Get the charger, change my shirt and try to wipe the grime off my face and hands...

Back to spark fun... its now 8:30 and the contest starts at 9.

I charge the laptop and put fresh Lipos on the car.   Sitting on the  bench test everything the side laser scanner is not working again...  I see a loose wire on the board...  The carrier board I'm using only has level translators for one RS-232 serial port. The IMU and GPS are TTL levels, the laser scanners are RS-232 so to get around this I jumper the CTS in line from the one port from the CTS pin to the RX pin on a different serial input (The board has 8 serial ports pinned out) My flaky wiring for the side laser scanner was a bad crimp on the jumper from the CTS to the RX pin... not on the external wiring at all.  I fix this and never again have a laser scanner problem for the rest of the day.


I turn the speed down to the speed I can run and the first heat goes reasonably well... we make it 3/4ths of the way around the building the light has changed and now the laser scanner can see through the window on the last turn, it sees this jump in range and thinks the corner of the building has passed and turns into the building....

So I extend the lock out range for corner detection and on the second heat still in slow as I can run mode  we make it all the way around in 1:06  We miss the arch so no arch bonus...
(I think this run was good for 4th or 5th place)



This photo was taken by my Friend Ben Brockert (@Wikkit) between the 2nd and third heat.


The last heat... I turn the barrel dodging code back on as last time we just plowed through the barrels... I turn the speed way up and change the code to ignore the RC receiver after going into autonomous mode.....   The car hauls down the straight, properly dodges a couple of barrels go straight through the arch and around the corner I run to the other side and it never comes out....
So I walk back and find it in my favorite bush  at this point the competition is over and I have a choice...  I can carry the 20lb car back to my laptop and download the data log to see what happened... or I can hit reset and manually drive the car back and loose the data log never knowing for sure what happened on the last lap....  I'm tired I press reset....