This is a video of my first fully autonomous round 1 attempt. I made the first corner and avoided two barrels, but then ran in to the inside fence. I don’t have any footage from the other rounds as my camera gave up shortly afterwards.
This is a video of the first few outdoor test runs of my AVC robot. As you can see, I had a lot of magnetometer issues, but eventually, the last run does make it to the waypoint, albeit a rather scenic route. 🙂
The morning of the competition arrived. After getting up early so I could be at the course as it opened to spend the final hour tuning my way points, I released when I arrived at the venue, I’d left my wallet in the hotel, and had to drive back to the hotel and get it, taking all the time I would have had to practice, driving up and down the motorway in Boulder. 😦
The competition began. I was in heat 8, so I had a good 45 minutes to tinker before my first run. I mounted a GoPro and double checked my way pints on Google Maps. I was kind of pleased to see that almost no one in the first few heats made it past the first corner. In fact by heat 8, I think only 2 robots had made it round the course, out of about 30.
It was time for my first run. I was actually nervous !? Even thought I knew I wouldn’t win, I wanted to make it round the course, and not look like an idiot. Also I was the only Brit in a sea of Yanks … I need to represent my country ! 🙂
The countdown began, then Go! I pressed the big red button and my bot shot off ! To my surprise and great relief in the right direction !!! and to my amazement, it made the first corner and turned 90 degrees at exactly the right waypoint, on to the second straight. My heart was pounding as my bot swerved to the 2nd way point. I had 3 waypoints on the second straight to avoid the barrels. It swerved and missed the first barrel, I was actually amazed it was working !
I turned again and aimed to miss the second barrel … but then it didn’t straighten up and started to veer in to the centre of the course … within a few seconds it had hit the boundary fencing, and toppled over. Disaster! I was out in the first round mid-way up the second straight.
Not bad all things considered. Seeing as it was the first time all that code had run at the same time, completely autonomously. Although I was disappointed, I was actually quite chuffed it had made the first corner and hit the first three waypoints. My code was working !
During the beak between rounds I debugged the code and analysed the log from the first run. The bot was working, but was veering to the left, so I adjusted the 3rd waypoint to pull the bot further in to the centre of the course.
The second round. Go! I hit the button and my bot shoots off in the right direction again, but where it had neatly turned 90 degree at the first way point on the first round, this time, it span in circles at the first way point, driving in endless circles until it hit a hay bail.
In theory that shouldn’t be possible. The code should have dropped out of the loop when the waypoint was close, plus the circle was also tight, i.e. full lock. There must be something else wrong.
I spent lunch time debugging the logs and analysing the data. To my horror, the magnetometer was completely out of whack. It would reading north to be 90 degree from where it should be. And in various walking tests didn’t even change readings as I rotated the bot. My magnetometer reading were the plague of this project.
I decided to run another calibration over lunch, it takes a while, as I have to change some connections as I can’t do it reliably over xBee, so I run the calibration over a direct serial connection.
It took about 30 mins to complete the calibration and the bot was reading north again. I had about 20 minutes left of lunch to test on the course before the final heat. I pressed the button for a final practice and the bot shot off in the right direction, first waypoint hit, second way point hit, barrel avoided. Third way point hit, this was the furthest I’d ever got on the course. The back straight was perfect. I purposely avoided the hoop, I wanted to get round, and wasn’t so bothered about points. On the third straight, it actually hit the ramp and completed a jump, without toppling. It hit the final corner turned perfectly and flew over the finish line ! I had completed the course perfectly ! Shame it was over lunch in a practice session and no one saw ! 😦 I do have some video from the GoPro to prove it through !
I decide not to touch ANYTHING and wait for the final round. More and more bots were completing the course. Each run they were getting better. It was now or never.
Finally the third round was here. Nate (The Owner of SparkFun!) was my referee for the final round. No pressure then !?
The referee counted down 5,4,3,2,1 – Go ! I paused slightly to let the faster bots shoot off before me, to try and minimise any potential collisions. Previous rounds had a number of collisions right off the start line, where out of control bots took smaller bots out completely. I saw one completely smashed to pieces. My bot shot off in the right direction … turned perfectly on the first courser … weaved perfectly in through the barrels, detoured slightly wide at the second corner, my heart pounded as it headed for the fence, but with a meter to go, it turned sharply and headed off past the hoop. It started to oscillate on the back straight – shame I never got the PID controller tuned I thought – the bot headed straight for the cameraman on the 3rd corner, again with a meter to spare turned sharply 90 degrees and headed down the final straight … missing the jump (shame. It made it perfectly in the practice, and I could have got some bonus points). The curb of the final corner loomed, the bot shimmied perfectly round it, turn sharply on the final straight and headed for the finish line. I held my breath as the bot bounced over the finish line perfectly, running over the SparkFun GoPro that was balanced on the start/finish line capturing the action … 2 or 3 meters after the finish line it ground to a halt. I was so happy ! I think I actually kissed it (which someone caught on camera) 3 months of late nights and constant problems culminated on a perfect run on the final round. I was elated. I’ll post a video …
What’s next ?
I want to finish the bot off, even though the competition is over. Not only to have something ready if I manage to find a way to enter again next year, but also to understand what’s wrong. I still don’t fully understand why the magnetometer is so shonky. It’s not the device, I’ve tried 3 different devices, and they all do the same strange things.
At the competition, nearly all the other bots has their magnetometer in the car, on the main PCB, not floating on a broom handle above it like mine. I need to understand what my issues are.
I want to finish the PID tuning and increase the speed. The RC chassis is very fast, and I’m running at about 1/10th speed. If I can remove the broom handle, the bot won’t topple over so easily and can turn faster. I also want to include a proportional speed control so the bot accelerates fast if it’s far away and on a straight heading, then brakes to make the turn as it gets close to the waypoint.
I also want to finish the obstacle avoidance, so if I do head for a barrel, I can veer away from it.
All in all and very pleasant way to spend a weekend. I got to talk to lot of interesting people with a similar passion, I got to spend the weekend in 90 degree sunshine and I got to indulge my passion for robotics and spend the weekend geeking out … 🙂
Thanks to everyone at SparkFun for running the event and organising it so well.
Roll on AVC 2014 … 🙂
Other posts in this series :
The mBed versions of the AVC PCBs arrived. I built the first version, with surface mount components in my oven at home. It works first time. It turns out I made one mistake on the PCB, using the wrong ratio SMD resistors with the battery divider in to the mBed, so I had to take them off and modify it with standard through hole resistors, as I couldn’t hand solder the 0602 surface mount resistors. I made the modification for the SPI connection for the IMU, and swapped a couple of sensor to different pins to give me SPI and Serial in the right places.
I mounted the circuit in a Tupperware box on the car to keep it water tight.
My first outside test of the complete system was very exciting, but a little disappointing. Not only did I manage to run my wife over with the car, giving her a nasty bruise, but also the car seemed to like to take the scenic route to the way point, rather than a more direct route. All down to magnetometer issues again. I was still getting erroneous and unstable readings. In fact this plagued me from start to finish on this project, and still hasn’t gone away.
The only solution I have found to improving the magnetometer readings is to re-run the magnetometer re-calibration process regularly (before each run). The numbers always come out slightly different?
The first end to end test of the system in my garden was one week before the competition. I still hadn’t written a lot of the other functions in the code. Sections of the code like; how to tell if I had reached a waypoint, loading way points from SD, obstacle avoidance using the sonar, and tuning the PID parameters. I also hadn’t actually mapped my waypoints for the route I was going to take through the course.
The PID tuning would have to wait. I was getting pretty good steering results just with the “P” terms. This would mean I was in danger of oscillations, but that was the least of my worries. I could hardly drive in a straight line at the moment …
I didn’t have time to write any sonar obstacle avoidance code. I had a plan to read the distance data from the sonar and if an object got within 2 metres, add an offset in to the steering calculation to veer slightly left or right to miss the object. However, I hadn’t quite worked out how to tell if I had “passed” it to remove the offset, as I only had a forward facing sonar. The other option was to just stop the bot if an object appeared less than 20cm in front of the bot, stop, turn a few degrees, go forwards, then continue under GPS control… I’ll get round to adding this later …
I arrived in the US for a conference about a week before the competition and spent the evenings in the hotel writing the missing code. I wrote a basic file reader to load way points and basic config data from file, plus as my xBee worked for about 2m at best, I wrote a simple logging function, which meant I had some debug data after each test run to investigate. My waypoint algorithm was relatively simple;
- Calculate the heading and distance to the next way point
- Use the PID control to steer on that heading.
- If the distance to the way point was less than 2m meters, we’re very close. At this point these it a danger the bot will drive in circles, as the turning radius is greater that the heading it needs to hit the way point exactly. So, once we get close, I no change the steering to steer to the waypoint, instead, continue on the heading I was on just before we got close. You can assume the heading I was on would hit the waypoint pretty close. So, if I stay on that heading, and monitor the GPS vector to the waypoint, as I pass the way point I should see the GPS vector increase to 90 degrees as I pass by the waypoint. i.e. the vector will go from straight ahead, to 10, 20, 30, 45, eventually 90 degrees. At this point I’m within 2m meters and its 45 degrees or 90 degrees to my left or right, so, that’s a pretty good assumption of hitting the way point, so I assume I’ve passed it, and move on to the next way point.
- Once the bot has visited all the way point it has loaded, it stops. Course complete.
I didn’t have time to write the obstacle avoidance code or tune the PID in time for the competition.
The day before the competition I spent the afternoon at the location planning the course and testing the code. I rewrote my magnetometer code again, this time using my own simple trigonometry rather than the library function. Plus I debugged the “hitting the way point” logic. By sunset, I’d navigated to a series of way points for the first time, but still careering off course occasionally. In addition I suddenly remembered that I hadn’t compensated for magnetic vs. true north, so added some Magnetic deviation code. This link has a great calculator for magnetic variance. It shows that Colorado has a deviation of about 8 degree, which would have been a big issue if I hadn’t remembered to compensate for it.
Other posts in this series :
The basic prototype worked on the bench, but as I built it on to the chassis, it started to have issues. The magnetometer would give very random and unreliable readings. I kind of assumed I’ve get some interference, but this was reading north sometimes 180 degree out. In addition, the reading changed when the motors ran and the servos moved. I assumed I was getting magnetic flux from the motors and servos, and this was interfering with the magnetometer. However, without the motors running, I was also getting bad drift from the magnetometer. For example, I would point the car north, take a magnetometer reading, then rotate the car 360 degrees, the reading wouldn’t give me a 360 change. When I get back to north I would read 280 odd degrees, in addition, if I did the same experiment in different places in the room, or at different times, I got different readings. The magnetometer be useless if I couldn’t reliably know where “north” was.
The xBee also had issues, and seemed to only work up to about 1-2m away … which wasn’t very useful. I only had 1mW series 1 devices, which on the data sheet said they’d get 600m, so I expected a bit better than 2m, but I assume this was also due to interference. This wasn’t a deal breaker, as most of the testing could be done with the car no actually driving. All the debugging was around the steering, so 2m was fine to just not have a cable in the way. But it was frustrating … I had to follow the car around carrying the laptop, instead of just sitting in the garden at a table and taking reading as it drove around.
I stuck with the xBee but I moved the magnetometer away from the main board and chassis, to try and reduce the interference. I assumed the interference was magnetic flux coming from the huge brushless motor and large powerful steering servo in the car. If I mounted the magnetometer away from these, perhaps I’d get a more reliable reading. And so was born “broom-handle-bot”
While working on the prototype I was also researching better GPS solutions. The basic GPS module I was using only had an accuracy of about 2-5m. This would be tricky to avoid a barrel or hit a jump 1m wide. I had stumbled across SBAS while research GPS issues and this potentially could give me accuracy down to 1-2m. Much better. You can read the wiki page for more details, but effectively (IIUIC) the SBAS system sends a separate signal from a different set of satellites that contains data to improve the accuracy of the main GPS signal. It compensates for atmospheric interface, and other transmission errors. There are different SBAS systems in different parts of the world, WAAS in North America and GLONASS in Europe. I would need a GPS receiver that supported both. Again, with some googling, I found NaviLock and the NL-622MP MD6 GPS Receiver. The data sheet confirmed it supported WAAS and GLONASS, it uses the latest u-Blox chipset, and came in a handy mountable self-contained puck. I ordered one and it arrived. The only downside was the output was RS232 (+/- 15v) not TTL, so I’d need a level shifter board to connect it to the mBed. If I had spent more time looking, they do do a TTL version, which would have been easier …
The very first test of the new GPS puck was transformational. The old GPS wouldn’t get a lock inside my house, which made testing a real pain. I would have to write the code, upload it, then disconnect the system, walk outside in to the garden, test it in the cold, then come back in and debug it, then repeat. This wouldn’t have been so bad, if this was on a Sunday afternoon, but due to my job and family commitments, nearly all my tinkering was done at night, between the hours of 10pm and 2am. It was also the back end of winter, and still snowy some weekend. Testing in the garden at midnight in the snow wasn’t fun.
The new GPS got a lock inside. Plus it got a lock really quickly, within seconds. The old GPS took minutes. It was clear from the first few outside tests that the accuracy was much better and the reading more stable. The GPS used the uBlox 6 chipset, with came with a very detailed and advance desktop config and debug utility.
The basic system was working. However I still had magnetometer issues. The reading would vary wildly if the magnetometer was close to the car. Even if it was some distance way, it wasn’t reliable. It would point north, then I would rotate it 180 and it wouldn’t point S, it would read 160 degrees, then I point it north again and it would read -30. If I rotated it 360 degrees sometimes it didn’t move, sometimes it rotated 250 degrees. I tried lots of different configurations. Even on the bench with no magnets or metal close it wasn’t reliable.
After some extensive Googling, it seemed I need to perform a calibration. This would calibrate out basic interference if the interference was constant, like a steel mounting screw that was always in the same relative position to the sensor. This link explains the process. You take lots of readings from every orientation, imaging mounting the sensor on gimbals and spinning it on all axis, so the tip of the sensor scribes the outline of a sphere. Ideally you want a data cloud of points in all orientations from all three sensors; x, y & z.
Once you have the data set you need to do three things;
- You need to remove “erroneous” readings, called outliners. There are clearly (statistically) incorrect. These are obvious to spot manually as the sit outside the circle/sphere of normal readings.
- You need to normalise the data in all three axis, so all the maximum readings from all the of the x, y & z sensors have the same magnitude
- You need to shift the data so to each set of readings is centred on zero.
Imagine a point cloud that is slightly oval shaped. The normalisation process shifts and scales the cloud so it’s a perfect circle, orgined around zero.These scaling and offset numbers are then used to scale the raw reading to give better accuracy.
This worked to an extent, and the magnetometer was noticeably more accurate and stable, but I still had problems. So I thought I’d tried a different approach. I had an IMU sensor from a previous project. This sensor has 9 DoF (gyro, accelerometer & magnetometer) plus a CPU and contains a kalman filter to process the data. It will give Euler angles, and all I was after was “yaw” – Phi. The issue was the device was not I2C, but serial or SPI. The serial interface had complex packet structure and I didn’t have time to write and debug a parser, so I opted for SPI. I modified the code to read SPI and got a basic Phi reading from the device.
This was more accurate. But it now had a different problem. The issue with yaw calculating with a 9 DoF board, is the yaw cannot use the accelerometer data, as it rotates around the accelerometer axis so it never changes. It can only merge the magnetometer and gyro data. And this is susceptible to gyro drift. This manifested itself in the yaw reading slowing rotating even when the unit was stationary. There was a function in the devices to zero the gyros when the unit was at a known stationary position, but after time the drift returned. That clearly wasn’t going to work.
The final option was to try the same basic magnetometer code, but with these IMU magnetometer readings. You can read all the raw gyro, accelerometer, magnetometer readings, plus the processed (filtered) version, plus the Euler angles from the device.
It turned out the process (filtered) raw magnetometer readings were pretty stable. So I had my final solution. However … about a week before the competition!
Other posts in this series :