Integrating Time, Memory, and Heading Capability, Part VIII

Posted 13 September 2018

Now that I have worked out most of the problems associated with the MPU6050 6DOF IMU module, it was time to integrate the new heading-based turn algorithm into the main Wall-E2 operating system.  As I have done in many past projects over the last half-century or so, I started this process by documenting the entire OS, with particular emphasis on how Wall-E2 currently navigates around his world.  When I started doing this in the 1970’s, the medium I used was an MIT Engineering notebook, hand-written in ink.  Over the ensuing decades the medium has changed, but not the basic idea – the process of putting coherent sentences and paragraphs onto paper (or screen) forces me to think through what is – and is not – important/true.  I have solved many a seemingly intractable problem not with an oscilloscope or debugging tool, but by simply writing things down.  In the current iteration of this process, I use Microsoft Word (not for any particular reason, except that it is available and familiar)  initially, and then dump the results into a post like this one – see below ;-).

 

Description of FourWD_WallE2_V1 Navigation Algorithm

09/04/18

At the start of each pass through loop(), the software determines the current OPMODE given the current environment and the immediately previous OPMODE.  The existing OPMODEs are NONE, CHARGING, IRHOMING, WALLFOLLOW, and DEADBATTERY

  • NONE: Default OPMODE when no other mode can be found to apply to the situation.  As of this writing, the only use for this OPMODE is to initialize the PrevOpMode and CurrentOpMode loop variables in Setings()
  • CHARGING: set in GetOpMode() if the charger is physically connected (CHG_CONNECT_PIN goes HIGH) and the CHG_SIG_PIN is active (LOW). In the CurrentOpMode Switch the PrevOpMode is also set to CHARGING (so that both prev and current op modes are CHARGING), the motors are stopped, and MonitorChargeUntilDone() is called.
    • MonitorChargeUntilDone() blocks until charging is complete, or the BATT_CHG_TIMEOUT value is reached or the charger is physically disconnected (manually pulled out for some reason).
  • IRHOMING: Set in GetOpMode() when the call to IRBeamAvail() (checks IR beacon signal strength) returns TRUE.  In the CurrentOpMode Switch the PrevOpMode is also set to IRHOMING (so that both prev and current op modes are IRHOMING).  A blocking call is made to IRHomeToChgStn() with an ‘Avoidance Distance’ of 0 for ‘hungry’ or 30cm (for ‘full- no need to charge’. The idea here is that in the ‘full’ case, the robot will continue to home until near the charging station, and then break off.
    • IRHomeToChgStn(): sets up a PID and enters a loop, exited only when either the charger connects, or the robot gets stuck or it gets too close to the charging station (this can only happen in the ‘full’ case).  ‘Is Stuck’ is determined in IsStuck() if the front distance variance gets too small (i.e. the front distance isn’t changing).
  • WALLFOLLOW: This is the OpMode that is assigned by GetOpMode() when none of the other mode conditions apply. IOW, this is what the robot does when it isn’t doing anything else.  In the WALLFOLLOW Case section of the CurrentOpMode Switch, the wall-following operation is further broken down into a TrackingCase Switch, with  TRACKING_LEFT, TRACKING_RIGHT, TRACKING_NEITHER sub-modes, with state mode variables maintained for both the current and previous tracking modes.  Each time through the loop(), the various tracking cases make one adjustment to the left/right motor speeds – there are no blocking calls at all in the entire WALLFOLLOW section, with the exception of the ‘BackupAndTurn()’ calls in the TRACKING_LEFT and TRACKING_RIGHT cases when an obstruction or the ‘stuck’ condition is detected.
    • BackupAndTurn( bool bIsLeft, int motor_speed): The idea here is for the robot to back up and do a course change to extract itself from some situation. Up until now, this has been accomplished by making a timed turn one way or the other, but this hasn’t worked well because the correct time for turning on carpet is wildly different than the correct time on hard flooring.  The new heading sensor is intended to solve this problem.
    • Now that Wall-E2 can make accurate turns, the question becomes “what’s the best way to do obstruction-avoidance or stuck-recovery turns?”. If Wall-E2 is following a wall when it gets stuck, maybe it should back up slightly and try to go around, or maybe it should just turn around and go back the way it came.  Maybe a simple obstruction should be treated one way, but a ‘stuck’ condition treated another?  The ‘go back the way I came’ model is simple enough but might result in an uninteresting ‘ping-pong’ shuttle track where it stays until it runs out of battery.  A more complex response might allow the robot to go around obstacles and continue its journey?  Maybe it backs up slightly (wall-following in reverse, maybe?), and then makes an X degree turn away from the wall, runs straight for a second, and then starts wall following again.

09/05/18

The current ‘BackupAndTurn()’ routine takes bIsLeft, a Boolean representing the current tracking direction (left or right) and a motor speed.  All it does is call RollingTurnRev(bIsLeft, 1500), where 1500 is the time in millisecond to run the motors.

RollingTurnRev() just calls RunBothMotorsMsec() with the motor speed on one side set to MAX and on the other to OFF (we know this won’t work on Wall-E2, because the wheel base is too wide – he just locks up ☹.

RollingTurnRev() is called in two places; ExecDiscManeuver() and  BackupAndTurn(). BackupAndTurn() is called from 4 places;  TRACKING_NEITHER/RIGHT/LEFT, and IRHomeToChgStn().  In all these cases, the robot knows which (if any) wall is closer, so it can execute the proper rolling turn

From what I see so far, it appears all these cases can be handled by a turn routine that does the following:

  1. Moves straight backward for just a second or so (or maybe even less)
  2. Makes a 45° forward rolling turn away from the nearest wall. If there is no nearest wall, go opposite the way it went last time (requires a global Boolean to save this value)
  3. Makes another 45 turn in the opposite direction to the first one.  This will have the effect of a side-step maneuver, as shown in this post.

After this review, it was clear that all I had to do to integrate the new heading-based turn capability into Wall-E2’s OS was to replace the RollingTurnRev() function with a new ‘RollingTurn()’ function that takes flags for FWD/REV and for CCW/CW, and a parameter for the number of degrees to turn.  Since I had already demonstrated all the code blocks in stand-alone test programs, all I had to do was copy the appropriate code pieces into the appropriate spots in Wall-E2’s OS, and then spiff things up a bit here and there.  When I was done, I had a single function that could facilitate a range of maneuvers.

To test the newly integrated capability, I added some code to Wall-E2’s setup() function to perform a series of S-turns, each of which demonstrates a typical avoidance maneuver. For convenience, I told Wall-E2 to execute a ‘K-turn’ reversal and then S-turn his way back to me.  As can be seen in the following short video, this worked fairly well!

Now that I have the basic heading-based turn capability integrated into Wall-E2, the next step will be to demonstrate that Wall-E2 can use its new superpowers to avoid obstacles in ‘the real world’ (as real as it gets for Wall-E2, anyway).

Stay tuned!

Frank

 

Integrating Time, Memory, and Heading Capability, Part VII

Posted 29 August 2018

In my last post on this subject, I demonstrated Wall-E2’s new found ability to make heading-based turns instead of timing-based ones, making the turns much more terrain-independent.  Unfortunately, as I continued to test this capability, it became clear that Wall-E2’s heading superpower wasn’t quite ready for prime time.  Sometimes he turned 45 deg or even 180 deg when asked to do 90 – oops!

So, I went back to my small test setup – a spare Mega and a small solderless breadboard, as shown below, and started going through the problem slowly and methodically.  Eventually I figured out that most of the problem was caused by the way I was retrieving yaw data from the Inversense MPU6050 (I have the DFRobots version).  I had the module set up to produce interrupts at 20Hz, and the code was trying to keep up with that (unsuccessfully, as it turned out).  Once I figured that out, I backed the code off to where it only checks for heading changes at a 10Hz rate, and things started working much better.

Arduino Mega and small plugboard test setup for robot turn management

I also figured out that the algorithm I was using for detecting the desired heading was fatally flawed.  I was trying to watch for the case where the current heading passed the target heading, but with all the special cases (both directions, the -180 – to – +180 cut, etc) I kept getting it wrong.  I finally found this post that describes a very simple formula for comparing two compass headings.  The formula assumes both values are in degrees in the range 0-360.  Mine are in degrees but in the range -180 to +180, but I took care of that by adding 360 to negative headings.  After some experimentation I settled on a match ratio of about 0.90 for the ‘slow down’ threshold, and 0.98 for ‘match’.  The 0.98 threshold provides about a 6 degree error margin, which with 10 measurements/sec means that the robot would have to rotate more than 60 deg/sec to get through 6 deg between measurements.  Experiments show that a 90 deg turn takes about 3 sec, meaning 30 deg/sec.  So there should always be at least 2 measurements at 0.1sec/measurement in the 0.2 sec it takes the robot to rotate 6 deg – a 2/1 safety margin.

The short video clip below shows the robot doing a series of 180 deg K-turns, simulating an avoidance maneuver.

 

So, at this point I think I’m pretty much done with adding turn management capability to Wall-E2’s superpower repertoire; however, I still have to update Wall-E2’s operating system software to replace the current timed-turn routines with the new heading-based turn routines.

08 September 2018 Update:

In my current Wall-E2 OS, when the robot gets stuck or runs into an obstacle, it backs straight up, and makes a timed turn away from the nearest wall if there is one, otherwise it turns the opposite way it did from the last time it was in a similar situation.  This is all fine and good, but now that Wall-E2 has heading-aware turn capability, he should be able to respond a little more intelligently.  As indicated in the diagram below, the idea is that Wall-E2 could back straight up from an obstacle, and then go around it by making two linked 45-90º turns one way or the other (away from the nearest wall if there is one.

New avoidance maneuver made possible by Wall-E2’s new heading superpowers

As an experiment, I programmed this maneuver into Wall-E2 using linked 45º degree turns, just to see how it would work out.  As the following short video shows, it seems to work very well.

Stay tuned!

Frank

 

Mid-2018 Wall-E2 Project Status

Posted 26 August 2018

It’s been a year and a half since I last described the status and challenges in my ongoing campaign to create Wall-E2, an autonomous wall-following robot.  The name ‘Wall-E’ was taken from the 2006 movie of the same name.  In the movie, Wall-E was an autonomous trash-compactor robot that had all sorts of adventures, and my Wall-E2 autonomous wall-following robot certainly fills that bill!

From the previous system status report in early 2017, I described the following tasks:

Its been a year and a half since I updated the status of my ongoing campaign to create an autonomous wall-following robot.  The robot system consists of the following main subsystems:

  • Battery and charging subsystem
  • Drive subsystem (wheels, motors and motor drivers)
  • IR homing subsystem for charging station
  • LIDAR for front ranging and ultrasonic SONAR for left/right ranging
  • I2C Sensor subsystem (MPU6050 6DOF IMU, FRAM, RTC)
  • Operating system

Battery and charging subsystem:

Since the last update, the battery and charging system has been updated from dual 1-Amp single-cell Adafruit PB1000C chargers utilizing a 5V source to a TP-5100 2-amp dual-cell charger utilizing a 12V source.  This significantly simplified the entire system, as now the battery pack doesn’t have to be switched between series and parallel operation. Also, now the charging and supply leads are independent so the supply leads to the rest of the robot were upgraded to lower gauge wire to reduce the IR drops when supplying motor drive currents.  See this post for details.

Drive subsystem (wheels, motors and motor drivers):

The motors were upgraded to provide a better gear ratio, although this was done before I realized that most of the traction issues were caused by IR drops in the battery wiring.  The motor driver modules are unchanged, but I may later swap them out for more modern 3V-capable drivers so that I can swap in an Arduino Due microcontroller for the Mega (the Due has the same footprint/IO as the Mega, but has a much faster CPU and more memory)

 IR homing subsystem for charging station:

The IR homing subsystem utilizes a pulsed IR beacon on the charging station coupled with dual IR sensors in a flared sunshade housing, backed by a Teensy 3.5 CPU configured as a null pattern matched-filter.  The Teensy reports left/right homing error as a value between -1 and 1 over an I2C bus to the main microcontroller, which drives the motors to null out the signal.  As the system stands today, the operating system can successfully home in on the charging station and connect to the charger. The robot knows its current battery voltage (charge condition) and therefore can decide to connect to the charger or to avoid it.

LIDAR for front ranging and ultrasonic SONAR for left/right ranging:

The front/left/right ranging subsystem is one of the most mature subsystems on the robot.  The subsystem can successfully follow walls, and detect/recover from ‘stuck’ conditions.  The only thing this subsystem lacks is the ability to make consistent turns on different terrain, due to the lack of heading information (this will be supplied by the new tri-sensor module)

I2C Sensor subsystem (MPU6050 6DOF IMU, FRAM, RTC):

The I2C sensor subsystem is a new addition since the last update, and has yet to be fully integrated into he system.  The subsystem consists of a Inversense MPU6050 6DOF solid-state accelerometer, and Adafruit FRAM (Ferromagnetic RAM) and RTC (Real-Time Clock) modules.  The MPU6050 gives the robot the ability to sense relative heading changes, which makes it capable of executing consistent N-degree turns on both hard flooring like the kitchen and atrium areas and the carpet in the rest of the house. The FRAM and RTC units should allow the robot to remember its charge/discharge history, even through power ON/OFF cycles.

The relative heading capability has been tested off-line from the main operating system, but has not yet been integrated into the OS. Same for the FRAM/RTC modules.  Integration of this subsystem was stalled for quite a while due to problems with the Arduino I2C (Wire) library, but these problem were just recently resolved by switching to a more robust I2C library (SBWire).  See this post for details.

 

Operating system:

The operating system has evolved quite a bit over the course of this adventure, but its current state seems pretty stable.  The OS is implemented as a set of modes, as follows:

  • MODE_CHARGING: Occurs when the robot is physically connected to a charging station
  • MODE_IRHOMING: Occurs when a charging station beacon signal is detected
  • MODE_WALLFOLLOW: Occurs when the robot isn’t in any other mode.
  • MODE_DEADBATTERY: Occurs when the sensed battery voltage falls below DEAD_BATT_THRESH_VOLTS volts

 

 

Future Work Plans:

  • Complete the integration of the tri-sensor module: This entails adding the hardware and software required to sense loss of power so that the current date/time stamp can be written to the FRAM, along with the complementary ability to read out the last power cycle date/time stamp from the FRAM on power-up.  In addition, the current timed turn routines need to be replaced by the new heading-sensitive turn algorithms.
  • Investigate the idea of multiple charging stations with different IR beacon frequencies. The current matched filter algorithm forms a very narrow-band filter, to discriminate the desired IR beacon signal from unwanted ‘flooding’ from overhead lighting sources and sunlight.  The center frequency of the filter is set in software on the Teensy microcontroller, so it should be possible to have the Teensy routinely check for beacon signals at other signals, as long as the frequencies are far enough apart to prevent overlap.  The current filter center freq was more or less arbitrarily set to 520Hz – high enough to be well away from, and not a multiple of, 60Hz, but low enough for the Teensy processing rate.  Something like 435Hz (60*7.25) would probably work just as well, and is far enough away from 520Hz to be well outside the filter bandwidth (about +/- 10Hz IIRC).

Complete the implementation of the fixed charging station.

This task has been completed, and along the way the charging voltage was changed from 5V to 12V, to accommodate the new 12V on-board battery charging system.  See this post for details

Integrate the IR homing software from the 3-wheel robot into Wall-E2’s code base:

This task has also been accomplished.  See this post for details.

Integrating Time, Memory, and Heading Capability, Part VI

Posted 25 August 2018

In my previous posts, I have been describing my efforts to give Wall-E2, my autonomous wall-following robot, relative heading sensing ability using the DFRobots MPU6050 6DOF module.  As I went through this process, I discovered that the ‘standard’ Arduino Wire library was seriously defective, and the problem had been known, but not fixed for almost a decade!  Once I figured this out, I was able to fix my local copies of Wire.c/h and twi_c/h and all my hangup problems went away.  Subsequently I found another Wire library (SBWire by Shuning (Steve) Bian that also incorporates the necessary fixes, so I started using his library instead of my own local fixes.

Anyway, after all the I2C drama, I finally got the damned thing working, and ran some tests to demonstrate Wall-E2’s new-found ability to make reasonably precise and consistent turns.  In the first test I had Wall-E2 make a series of 90-deg (ish) turns, and in the second one I had him make some 180-deg (ish) K-turns to simulate what he might want to do after disconnecting from (or avoiding) a charging station.

Known defect in Arduino I2C code causes hangup problems

Posted 20 August 2018

In my continuing quest to add relative heading sensing to Wall-E2, my autonomous wall-following robot, I have been trying to make the Inversense MPU-6050 module sold by DFRobots work on my robot.

In my last post on this topic, I had finally figured out that the program lockup problems I had been experiencing were due to a well-known-but-never-fixed bug in twi.c the low-level code associated with the Arduino I2C library.  This utility program has a number of while() loops used to send and receive bytes across the I2C bus, and every one of them is prone to deadlock when the device(s) on the other end of the bus misbehaves at all.  Then the while() loop never exits, and whatever program is running dies a horrible death.

The weird thing about this problem is that it has been known for at least a decade (yep – 10 years!!!), and has actually been fixed multiple times by multiple people over this period, but the fixes have never made it into the ‘official’ Arduino Wire library.  This makes NO SENSE, as the Wire library code is open-source, and is available on GitHub.  I thought the whole idea behind open-source code and GitHub was that others could contribute code fixes in a reliable revision-tracked way, so that when someone finds a bug, it can be fixed quickly and then propagated out to all users.  Apparently the guys at Arduino never got the memo, because it is apparently next-to-impossible to get a ‘Pull Request’ containing the bug fix through the code-maintainer’s gauntlet.

Thinking this was just a logistics problem that I could solve with just a few hours of elbow grease, and would be a good training exercise for other open-source collaboration projects, I decided to take a swing at this problem myself – how hard could it be?

  • I thoroughly researched the technical issues, made the changes to my local copies of Wire.cpp/h and twi.c/h, and verified that they indeed fully solved the hangup problems
  • Found the releveant Arduino Wire library source tree on GitHub
  • Forked the Arduino Wire library source tree to my own GitHub Account
  • Cloned my fork of the Arduino Wire Library to my PC
  • Made all the relevant changes to my local repo, tested the result, and pushed the changes to my GitHub repo.
  • Created a ‘Pull Request’ with all the changes, with a descriptive note

By this time, I had expended a LOT of time, but that was OK as I had learned a lot that would pay off in future efforts, and besides I was finished – I thought!

Then I got a very nice email from the Arduino maintainer of the Wire library, listing all the things I had done wrong, and making it clear that the changes wouldn’t be merged into the ‘official’ Wire library until all was correct to their satisfaction.  When I looked at the list of problems, I realized most of it was about ‘whitespace’ mismatches between my submission and the official version.  Now, I don’t know about you, but I stopped thinking about whitespace a decade or so ago, when it became clear that whitespace was just a figment of the programmer’s mind, and had NOTHING WHATSOEVER to do with how well or poorly the code actually worked.  Now I was being asked to manually correct all the literally hundreds/thousands of places where my code had 2 spaces and the ‘official’ code had 3!  So, if I wanted this bugfix to get into the main distribution, I was going to  have to spend a HUGE amount of time dealing with nit-picking aesthetics that have nothing whatsoever to do with anything but somebody’s misplaced idea of right and wrong with respect to whitespace, for source files that are rarely, if ever, viewed by 99% of the Arduino programming community.  I mean, this would be like refusing to make a small, but important change to the maintenance manual for a car because the shop technician’t penmanship wasn’t up to par!  What is penmanship going to matter when known defects aren’t corrected?

So, I thought about that some more, and I came to realize why this I2C hangup bug has been around for so long – nobody’s pull request has ever made it through the ‘penmanship contest’ gauntlet; the Arduino maintainers are more interested in penmanship than in fixing clearly defective code that has (and still is) causing grief for anyone who tries to use the I2C bus.  My personal response to this problem was “screw them – I’m not going to spend all that effort just to please someone’s weird affection for whitespace, especially since my local copy of these files has already been fixed.

With just a little bit of searching, I found Steve Bian’s ‘SBWire’ library with timeouts added to all the while() loops in twi.c, and was quickly able to ascertain that Steve’s library did indeed solve my hangup problems.  Moreover, Steve actually answered my emails, and is unddoubtedly much more open to open-source collaboration than the guys at Arduino.

The sad thing about all this is that Arduino is not doing themselves any favors by making themselves part of the problem rather than part of the solution. If they aren’t going to actively maintain their baseline code distribution, it (and Arduino) will become irrelevant as users find other ways around the obstacles.

Frank

25 August update:

So, I did the same thing with Shuning (Steve) Bain’s SBWire library that I had done with Arduino’s Wire library.  Forked his repo, cloned it to my PC, made the small changes I wanted, pushed to my repo, and created a pull request.  Two Days later, Shuning had merged my changes into the library.  Now I do realize that SBWire isn’t ARduino Wire, so maybe a ‘higher standard’ might be justified for the ‘gold standard’ I2C library.  However, I think we could all agree that EIGHT FRIGGIN’ YEARS of known defects is probably a bit much!

So, my advice, if you’ve been having problems with I2C hangups, is to throw the Arduino Wire library in the nearest trashcan and use Shuning’s SBWire library

26 August Update:

I have been running SBWire on a little I2C test board, and I left it running over the weekend while my wife and I were away on a trip.  When I came back, some 95 hours later, the board was still running merrily.  I did note that the ‘lockup counter’ (the number of times the standard Wire library code would have locked up) stood at 14, or about once every 7 hours or so.  Actually I’m a bit surprised by this number, as in my personal experience the Wire library never lasted more than about 2 hours before locking up.

Just another reason to dump the Arduino Wire library and use something useful like SBWire ;-).

Integrating Time, Memory, and Heading Capability, Part V

Posted 10 August 2018

Well, it appears I spoke too soon about having solved the I2C hangup problem on my Wall-E2 wall-following robot.  In my last post on this subject, I described all the troubleshooting efforts I employed to nail down the cause of intermittent hangups when trying to use the MPU6050 6DOF IMU on the robot, along with several other I2C devices (a Teensy 3.5 used for IR homing, and Adafruit RTC, and FRAM modules).

After (I thought) figuring out that the I2C SCL/SDA line lengths were the root problem of the hangups I had been experiencing, my grandson Danny and I spent some quality time reworking Wall-E2’s layout to accommodate shorter line lengths.  Instead of mounting the IMU and it’s companion sensors on the second deck as before, we 3D printed a small plastic plate to attach to one of the hexagonal 2nd deck standoff posts and provide a 1st deck mounting area for the sensors.  The previous and new mounting locations are shown below:

2nd deck mounting location. The MPU6050 is the module with the illuminated blue LED toward the rear of the robot

1st deck mounting location for I2C sensors (lower right-hand corner of the photo). The Teensy 3.5 IR homing module is shown mounted on the IR detector housing (above the red plastic plate)

Unfortunately, as I was doing some final tests on this setup, I started experiencing hangups again.  After a day or so moping and some very choice words, I started all over again trying to figure out what happened.

On previous searches through the i-verse, I had run across several posts indicating that the Arduino Wire library had some basic problems with I2C bus edge conditions; there were several places where it uses several blocking ‘while()’ loops to transmit and receive data on the I2C bus, and there was no way to recover from a ‘while()’ loop where the exit condition was never satisfied.   After literally exhausting all the other possibilities, it was becoming apparent that this must be what was happening – the MPU6050 must occasionally fail to respond correctly to a I2C transaction, causing the associated ‘while()’ loop to never exit.

So, I started looking for solutions to this problem.  Again, I found some posts where folks had modified the low-level I2C bus handling code found in twi.c/.h, the code underlying the Android Wire class.  I found a post by ‘unaie’ (http://forum.arduino.cc/index.php/topic,19624.0.html) with the same complaint, but he also posted modified versions of twi.c and twi.h that solved these problems by forcing the ‘while()’ loops to exit after a set number of iterations, and resetting the I2C bus when this happens.  His modified versions can be downloaded at:

http://liken.otsoa.net/pub/ntwi/twi.h

http://liken.otsoa.net/pub/ntwi/twi.c

I downloaded these files and tried to replace the ‘stock’ twi.c/h with the modified versions. Unfortunately, unaie’s modifications were made on a quite old version of the files, and conflicted with the later ‘repeated start’ versions of these files that are in the current ‘wire’ library.

So, I did a ‘diff’ between the ‘repeated start’ version and unaie’s version, and created a modified version of the latest ‘repeated start’ twi.c/h.  In addition, I added a couple of functions to allow monitoring of the number of times a bus reset was required due to a ‘while()’ loop timeout.  When I was finished, I ran the sensor for over 24 hours with no failures, but in that time there were three instances where a ‘while()’ loop timed out and a I2C bus reset was required.  A small snippet of this run is shown below.  The blue line is the yaw value, and the plot snippet shows where I manually rotated the sensor just after 24 hours, and the horizontal orange line shows the number of bus resets.

Small snippet of 24-hour sensor run. blue line is reported yaw value; orange shows the I2C bus reset counter

So it is clear that, absent the lockup recovery modifications, the I2C bus would have locked up long before, and that with the modifications ‘while()’ loop deadlocks have been successfully handled.

11 August 2018 Update:

The sensor is still going strong after 44 hours with no hangups, and the reset counter is still holding at 3.

The complete twi.c & twi.h codes are included below:

 

Stay tuned!

Frank

 

Integrating Time, Memory, and Heading Capability, Part IV

Posted 07/26/18,

In my last post on this subject, I described my efforts to troubleshoot an intermittent ‘bad data’ problem I experienced with the Inversense MPU6050/DMP breakout board from DFRobots.  The calculated yaw value would occasionally ‘lose synch with reality’ and start varying wildly.  I finally tracked this down to occasional bad reads from the DMP FIFO; the read would extract bytes from two different packets, producing an invalid yaw value.  The fix for this problem is to monitor the FIFO ‘bytes remaining’ count and reset the FIFO whenever the ‘bytes remaining’ count is not an integral multiple of the packet size.

After demonstrating that I could run the MPU6050 for days (literally) without any bad data occurrences, I thought I was home free in my effort to provide relative heading information for precise turns, but I ran into yet another problem when I tried to integrate the MPU6050 back onto the robot.  This time the problem was a hangup problem; after some tens of minutes, the program would stop responding at all – as if it had gone into an infinite loop somewhere (which is exactly what was happening, but I’m getting ahead of myself…).

This problem turned out to be a ‘simple’ I2C wire-length issue; I was able to demonstrate that the problem would occur any time the I2C ICL/SDA wire length went beyond about 42 cm, and would not occur with wire lengths below about 30 cm.  The problem did not appear to be sensitive to I2C bus speed (at least not for the default 100KHz or for a reduced clock speed of 50KHz) or pullup resistor value – just to wire length.

The rest of this post is a copy of my troubleshooting notes from the effort to track this problem down and solve it.  I have found in the past that when facing a non-trivial problem with multiple possible causal factors, a troubleshooting journal is an absolute must.  In the pre-computer days (you do remember there was a time before ubiquitous computing don’t you?), I used a real MIT engineering notebook for this purpose, and then later on a 3-ring binder into which I added quadrille-ruled sheets covered with my notes and drawings.  Then I moved on to Word documents – nicer because I could include Excel plots, Visio drawings, photos, and other mixed media.  Now that I have graduated to a WordPress blog, I can use it as a repository of my working notes, while also allowing others to see the inner workings of a mad scientist’s mind ;-).

I2C Hangup problem with Inversense MPU6050

Stay tuned,

Frank

 

Integrating Time, Memory, and Heading Capability, Part III

Posted 12 July 2018

In the last installment of this particular saga, I described another chapter in my ongoing effort to add heading knowledge to Wall-E2’s (my autonomous wall-following robot) super powers.  In that post, I described my attempt to utilize the Inversense IMU 6050 6DOF breakout board from DFRobots.  I posted some results that showed problems with vibration screwing up the results, and then getting error-free results using an ‘air pillow’ (a piece of air-filled packing material).  At the time, this led me to believe that the cause of the bad data was motor vibration.  However, when I tried adding some foam vibration dampening material, it didn’t seem to help – I was still getting intermittent stretches of bad data, with or without the motors running.  Clearly I still didn’t understand what was happening.

Once again I ran away from the whole thing, to regroup and let my mind work on the problem for a while; back to basketball, bridge, and general goofing off.

After some more web research and just thinking about what I knew and didn’t know, I started to suspect that what I was seeing was an artifact of the way the sensor(s) communicated with the main controller via the I2C serial interface.  When yaw measurements went bad, they went really bad, rapidly cycling from positive to negative values, and this didn’t make a lot of sense.  Maybe the main controller wasn’t keeping up with the sensor data stream, and  the software was trying to form heading values using bits from two different measurements; this would explain why the heading sign changed from measurement to measurement.  Also, I wasn’t utilizing the INT pin on the IMU6050 module, just pulling data out of the FIFO as rapidly as possible; could that be part of the problem too?

So, I decided to start all over again with the IMU6050 sensor on an ASP plugboard, with a spare Arduino Mega 2560 controller identical to the one being used to run Wall-E2, as shown in the following photo.  I also hooked up the INT pin, and used Jeff Rowberg’s I2CDev materials and MPU6050 example programs as the starting point.

DFRobots Inversense IMU6050 breakout board (board with blue LED, between FRAM and RTC) on an ASP plugboard, controlled by an Arduino Mega 2560

After getting everything going, I ran some long-term tests to see I could produce ‘bad’ yaw readings independent of the robot platform.  And, of course, I couldn’t get the darned thing to fail, no matter how long I ran it.  Shown below is a 20-minute long plot

20-minute run with no observed problems

Next, I tried inserting some delays into the ‘do other stuff’ part of the main loop, to try and simulate the normal robot processing delays.  This had no effect up until the delay reached 40mSec or so, and then I started to see problems very similar to what I had seen before with both the MPU9250 and 6050 sensor setups.

On robot test displaying yaw value and bytes remaining in MPU6050 FIFO

Then I modified the code again to check for FIFO byte lengths that weren’t an integral multiple of the normal packet length (42 in this case), and to reset the FIFO if the condition was detected. This seemed to eliminate the ‘bad data’ effect, regardless of the amount of delay introduced in the processing portion of loop().

About 6 minutes with the motors running. Program modified to reset the FIFO whenever a non-modulo ‘bytes remaining’ condition was detected

Detail view of the last 100 seconds of the previous plot

Summary:

The Invensense MPU6050/DMP/FIFO combination is sensitive to delays in the main  processing loop, even when using the INT line with an Interrupt Service Routine (ISR).  When the main loop processing delays get beyond about 40mSec, the ‘mpu.getFIFOBytes(fifoBuffer, packetSize);’ call will occasionally not remove the correct number of bytes from the FIFO, leaving a non-modulo (packetsize) number of bytes remaining in the FIFO.  When this happens, the next read will get (and process) bytes from two different packets, resulting in wildly varying yaw value outputs.  This condition is (now, after knowing what’s going on) fairly easy to recognize, as the error condition generally causes adjacent yaw values to have different signs, resulting in a classic sawtooth output.

The way to eliminate this artifact is to check for non-modulo (packetsize) FIFO bytes remaining value each time, and reset the FIFO when this happens.  Whatever good data is still in the FIFO will be lost, but the data that you do get will be valid.

I have included below my test program, with the FIFO modulo check and FIFO reset mechanism.  Note that this program also includes my motor control code, which obviously will not work with your setup.

 

Stay tuned,

Frank

 

 

 

 

Integrating Time, Memory, and Heading Capability, Part II

Posted 06 June 2018

About a month ago in early May I started another try at adding time, memory, and heading super-powers to Wall-E2’s repertoire.  In this post, I described the effort to add a FRAM board, an RTC board, and a 9DOF MPU9050 IMU board to Wall-E2’s already-existing I2C bus, and in this post I described fabricating a permanent ‘tri-sensor board’ for installation on Wall-E2’s second deck.

After installing the tri-sensor board on the second deck, I did some tests to confirm Wall-E2’s new found ability to make accurate turns, and instead of confirming success, I witnessed complete failure – Wall-E2 was even worse at making turns now than in the past when he was just doing timed turns – WTF!!

Backing up and doing some more tests, it quickly became apparent that the MPU9050 IMU worked fine until Wall-E2’s wheel motors started running, but went completely ga-ga after that.  I tried some quick tests to try and isolate the problem, but didn’t get anywhere.  So, I did what I always do when faced with an apparently intractable problem – I ran away ;-).  OK, I didn’t really run away, but I decided to put the whole thing on the back burner for a while and let my subconscious work on it for a while (I have discovered over and over again that my subconscious is a lot smarter than I am).  In the meantime I  had some other stuff to do, like going to a duplicate bridge tournament and going to a grand-daughter’s HS graduation.

After doing some web research, I began to suspect that the problem might be associated with the complexity associated with the Sparkfun 9DOF MPU9050 chip and the associated yaw/pitch/roll computation software, so I decided to see if there were other IMU solutions out there.  I ran across the DFRobots MPU-6050 6DOF IMU breakout board.

Since I wasn’t planning to use this for anything but yaw (z-axis rotation) information, the 6DOF limitation wasn’t a problem, and the integrated Digital Motion Processor™ (DMP) should simplify the control software.  In addition, the IMU6050 board is 5V compatible, so I can eliminate the level-shifting hardware required for the Sparkfun 9250 unit.  So, I ordered a couple of units and started playing with them.  Using the supplied libraries from DFRobots and their little example program,  I was able to get the unit up and running fairly quickly.  To test the ability to report yaw excursions, I modified the test program to report only yaw, and manually turned the sensor around its Z-axis.  As shown below, the yaw progressed smoothly.

Manually turned sensor while recording yaw

Next, I mounted just the IMU6050 board on the robot’s 2nd deck, as shown below, and ran some tests with and without the motor running.

MPU6050 mounted on robot with double-sided tape

Sensor mounted on robot with double-sided tape, no motors running

As shown above, with the sensor mounted on the 2nd deck and with motors not running, I got a smooth response from the yaw sensor as the robot was manually rotated.  However, when I tried the same trick with the motors running, I got the following plot:

Sensor mounted on robot with double-sided tape, left rear motor running

Then I mounted the sensor on an ‘air pillow’ (one section of a piece of air-filled packing material) and tried again, once again getting a smooth plot, even with one motor running.

MPU 6050 mounted on robot with an ‘air pillow’ (piece of some air-filled packing material)

6050 sensor mounted on air pillow, left rear wheel motor running

 

Integrating Time, Memory, and Heading Capability

Posted 06 May 2018

For the last two months I have been working on adding some secondary, but still important capabilities to Wall-E2, my wall-following autonomous robot.  As I noted back in April, Wall-E2 still can’t tell which way he is heading, which (among other problems) means he can’t make accurate turns.  In addition, Wall-E2 can’t tell how long (or even if) he has been turned off, making it impossible to tell how long it has been since he last was charged.

In the intervening months, I have been able to obtain and test individual modules to address the above issues; the Sparkfun MPU9250 9DOF IMU breakout board to obtain heading information, and the combination of an Adafruit MB85RC256V FRAM breakout board and an Adafruit DS3221 RTC breakout board to capture the date/time of any power interruptions.

Since all three of the above breakout boards are I2C-capable, it should be feasible to run all three from a single I2C bus on Wall-E2’s main controller – an Arduino Mega 2560.  Unfortunately, the Sparkfun MPU9250 module isn’t 5V tolerant, so integrating all three isn’t as simple as just daisy-chaining them all together.  Back to Google for some more research, where I eventually uncovered Philips Application Note AN97055 by Herman Schutte of The Netherlands dealing with bidirectional level-shifters for just this problem. Turns out that Philips introduced the I2C bus back in 1980, so they may know a thing or two about the issues ;-).  In any case, this 1997 paper described a 2-MOSFET bidirectional level-shifter that completely addresses the issue, and seemed to be pretty straightforward to implement.  It took me a couple of tries, but I got it working, with the result that I can now run all three modules (RTC, FRAM and IMU) from an Arduino Mega 2560, with the level-shifting MOSFET’s placed between the IMU and the rest of the circuit, as shown in the photo below.

All three sensors operating a the same time, controlled over a single I2C channel.

In the above photo, the modules are (from left to right): Adafruit MB85RC256K FRAM breakout, Adafruit DS3231 RTC breakout, and Sparkfun MPU9250 9DOF IMU breakout.  The two 2N7000 level-shifter MOSFETs can be seen at the top left-hand corner of the Sparkfun IMU breakout board.

I put together a small test program, combining pieces from the software used to test the power-down date/time capture idea, and the software used to test the ability to use the Sparkfun IMU to accurately manage rotations.  This program doesn’t do much at all, but it does demonstrate that all three modules can be controlled at the same time over a single I2C channel, as shown in the following output.