Author Archives: paynterf

Arduino SPI Data Exchange Between Two Arduinos in a Master/Slave Configuration

Leave a reply

posted 13 March 2020,

While updating my four wheel autonomous wall-following robot (aka Wall-E2), I ran into a roadblock when I couldn’t get my Teensy 3.2 IR demodulator/tracker module to communicate with the Mega 2560 main microcontroller over I2C.  This worked fine when I last tested it, but now it seems to have taken a vacation.  It’s been a while, and I have added some functionality since I originally installed and tested IR homing to the charging station, but it still should all work, right?

In any case, after trying (and failing) to get the I2C bus connection working, I thought I might try an alternate solution and just use SPI between the main controller (Mega 2560) and the IR homing controller (Teensy 3.2).  This would have the advantage of making the Teensy independent of the I2C bus, and also give me the chance to play with a part of the Arduino ecosystem that I haven’t used before.

As usual, I started this process with a lot of Googling, and quickly ran across Australian Nick Gammon’s  “SPI – Serial Peripheral Interface – for Arduino“  .  This post was way more than I ever wanted to know about SPI, but it sure is complete!

Then I moved on to trying to get SPI working for myself.  As noted above, my intended application is to transfer steering values from my IR detector/demodulator/homing module to the main Mega 2560 microcontroller. The Mega uses the steering data to adjust wheel speeds to home in on a charging station, so Wall-E2 can continue to roam our house autonomously.

It turns out that SPI between two Arduinos isn’t entirely straightforward, at least not at first.  I went through a bunch of iterations, and even more passes through the available documentation.  In the end though, I think I wrassled it into a reasonable facsimile of a working solution, as shown below.  The program repeatedly transfers a fixed string (“Hello World!”) from the UNO (master) to the Mega 2560 (slave), and then transfers the string version of a float value (3.159) from the slave to the master.

The Circuit:

The circuit and layout is just about as basic as it gets.  The UNO (master) connections are the default pinouts:

UNO (master) connected to Mega 2560 (slave) using default SPI pins on both ends

 

The Master:

The Slave:

The above configuration produced the following output:

Master:

 

Slave:

Now that I’ve had my fun and games with SPI, I’m still not at all sure I want to use it to connect the Mega to the Teensy IR Demodulator/Homing module.  While it is almost certain to work (eventually), it has some drawbacks

  • It requires 4 additional wires
  • It requires that I add an ISR and other code to the Teensy module, and companion code to the Mega
  • More things to go wrong.

So, I think I’ll try again to get the original I2C based code working, as it worked before, so it should work again.  If I can’t get it working after another good try, I’ll go with door #2 (SPI)

Stay tuned!

Frank

 

Updating the Four Wheel Robot

Leave a reply

Posted 29 February 2020,

Happy Leap Day!

For the last several months I have been using my older 2 wheel robot to investigate improved wall following techniques using relative heading from the onboard MPU6050 IMU module.  As the reader may recall (and if you can’t, look at this summarizing post) I had a heck of a time achieving reliable operation with the MPU6050 module mounted on the two wheel robot.  In the process of figuring all that out, and in collaboration with Homer Creutz, we also developed a nifty polling algorithm for obtaining heading information from the MPU6050, a method that has now been incorporated into Jeff Rowberg’s fantastic I2CDev ecosystem.

After getting the MPU6050 (and the metal-geared motors on the 2 wheel robot) to behave, I was also able to significantly enhance wall-following performance (at least for the 2 wheel robot).  Now it can start from any orientation relative to a nearby wall, figure out an approximate parallel heading, and then acquire and then track a specified offset distance from the wall – pretty cool, huh?

So, now it is time to integrate all this new stuff back into the 4-wheel robot, and see if it will translate to better autonomous wall-tracking, charging station acquisition, obstacle avoidance, and doing the laundry (well, maybe not the last one).  The major changes are:

  • Update the project with the newest MPU6050 libraries:
  • Revise the original 4 wheel code to use polling vs interrupt for heading values
  • Installation of the ‘FindParallelHeading()’ function and all its support routines
  • Integration of the parallel heading determination step into current wall tracking routines
  • Verification of improved functionality
  • Verification that the new work hasn’t degraded any existing functionality
  • Incorporating heading and heading-based turn capabilities into obstacle avoidance

To implement all the above, while attempting to insulate myself from the possibility of a major screwup, I created a brand-new Arduino project called ‘FourWD_WallE2_V2’ and started integrating the original code from ‘FourWD_WallE2_V1’ and ‘TwoWheelRobot_V2’

Update the project with the newest MPU6050 libraries:

The original FourWD_WallE2_V1 project used the older MPU6050_6Axis_MotionApps20.h library, but the two wheel robot uses the newer MPU6050_6Axis_MotionApps_V6_12.h one.  In addition, Homer Creutz had updated the new library even further since its incorporation into the two wheel robot.  The first step in updating the 4 wheel robot was to re-synchronize the library on my PC with the newer version on GitHub.  This was accomplished very easily – yay!  The next step was to copy most of the #includes and program constants from the original 4 wheel project into the new one, and then get the resulting skeleton program to compile.  This took a few tries and the addition of several files into the project folder as ‘local’ resource and header files, but it got done.  At the conclusion of this step, the project had empty setup() and loop() functions and no auxiliary/support functions, but it did compile – yay!

Revise the original 4 wheel code to use polling vs interrupt for heading values

The original project uses a flag modified by an ISR (Interrupt Service Routine) to mediate heading value acquisition.  The two wheel robot uses a polling based routine to do the same thing.  However, the algorithm used by the two wheel robot isn’t exactly the same as the one provided by Homer Creutz as part of the new MPU6050_6Axis_MotionApps_V6_12.h library.  In addition, the two wheel robot uses a different naming convention for the current heading value retrieved from the IMU. The 4-wheel robot uses ‘global_yawval’, and the 2-wheel one uses ‘IMUHdgValDeg’.  The 4-wheel robot uses ‘bool GetIMUHeadingDeg()’ to retrieve heading values, but the 2-wheel robot uses ‘bool UpdateIMUHdgValDeg()’ to better indicate it’s function.  So, all instances of ‘global_yawval’ will need to be changed to ‘IMUHdgValDeg’, and references to ‘GetIMUHeadingDeg()’ will have to instead reference ‘UpdateIMUHdgValDeg()’.

I started this step by copying the entire ‘setup()’ and ‘loop()’ function contents from the old 4-wheel robot project to the new one, and then working through the laborious process of getting everything to compile with the new variable and function names.  First I just started with ‘setup()’, and kept copying over the required support functions until I’d gotten everything.  For each support function I checked the corresponding function in the 2 wheel project to make sure I wasn’t missing an update or enhancement.  BTW, the combination of Microsoft’s Visual Studio and Visual Micro’s wonderful Arduino extension made this much easier, as the non-compiling code is highlighted in red in the margins of the VS edit window, reducing the need for multiple compiles  The affected functions were:

  • GetDayDateTimeStringFromDateTime(now, buffer): not in 2 wheel project
  • GetLeft/RightMedianDistCm():  Eliminated – these were never really used.  Replaced where necessary with GetAvgLeft/RightDistCm() from the two wheel project.
  • GetFrontDistCm(): Not used in 2 wheel project
  • dmpDataReady(): ISR for MPU6050 interrupts. Replaced with polling strategy
  • StopLeft/RightMotors(): Not used in 2 wheel robot – copied unchanged
  • SetLeft/RightMotorDir(): Not used in 2 wheel robot – copied unchanged
  • RunBothMotors(), RunBothMotorsMsec(): Not used in 2 wheel robot – copied unchanged
  • IsCharging(): Not used in 2 wheel robot – copied unchanged.

At this point, the entire setup() function compiles without error, and the setup() code runs properly. The next step is to add in the loop() functionality and then modify as necessary to replace interrupt-based heading acquisition with polling-based, replace ‘global_yawval’ with ‘IMUHdgValDeg’, and to replace ‘GetIMUHeadingDeg()’ with ‘UpdateIMUHdgValDeg()’

notes:

  1. Revise UpdateWallFollowMotorspeeds as necessary to incorporate heading-based offset tracking
  2. Revise/Replace RollingTurn() & GetIMUHeadingDeg() as necessary – done
  3. Global replace of global_yawval with IMUHdgValDeg showed 25 replacements
  4. Replaced ‘GetIMUHeadingDeg()’ with ‘UpdateIMUHdgValDeg()’ from 2 wheel robot project
  5. Added GetCurrentFIFOPacket() from 2 wheel robot project
  6. Replaced ‘if(devStatus == 0)’ block with the one from 2 wheel project
  7. Had to comment out PrintWallFollowTelemetry(frontvar),  FillPacketFromCurrentState(CFRAMStatePacket* pkt), and DisplayHumanReadablePacket(CFRAMStatePacket* pkt) to get everything to compile.

At the conclusion of all the above, the _V2 project now compiles completely.

1 March 2020 Update:

After getting the entire program to compile, I decided to try some simple tests of heading-based turn capability, so I modified setup() to have the robot do some simple S-turns, and then a backup-and-turn procedure.  As the accompanying video shows, this seemed to work quite well.  This is very encouraging, as it demonstrates polling-based rather than interrupt-based MPU6050 heading value acquisition and verifies that the latest MPU6050 libraries work properly.

The next step was to incorporate the ‘command mode’ facility from the two wheel robot. This facility allows a user within range of the Wixel RF link to take over the robot and issue movement commands, like a crude RC controller.  After making these changes, I was able to take control of the robot and manually command some simple maneuvers as shown in the following video.

As shown above, the left 180 degree turn as currently implemented for the 4-wheel robot takes forever!  I’ll need to work on that.

04 March 2020 Update:

I lowered the value of  the OFFSIDE_MOTOR_SPEED constant while leaving the DRIVESIDE_MOTOR_SPEED constant unchanged to make turns more aggressive, as shown in the following set of three video clips.  In the first two, the OFFSIDE_MOTOR_SPEED is 0, while in the last one, it is 25.  I think I’ll leave it set to 25 for the foreseeable future.

 

Stay tuned,

Frank

06 April 2020 Update:

After a two-week trip to I2C hell and back, I’m ready to continue the project to update my autonomous wall-following robot with new heading-based turn and tracking capability. The off-road trip was caused by (I now believe) the combination of a couple of software bugs and an intermittent I2C ‘daisy-chain’ cable connecting the Arduino Mega controller to four I2C peripherals. See this post for the gory details.

Installation of the ‘FindParallelHeading()’ function and all its support routines:

In the TwoWheelRobot project, the ‘FindParallelHdg()’ function is used to orient the robot parallel to the nearest wall in preparation to approaching and then tracking a specified offset distance.  The algorithm first determines the parallel heading by turning the robot and monitoring the distance to the near wall.  Once the parallel heading is determined, the robot turns toward or away from the wall as necessary to capture and then track the desired offset distance.

Here’s a short video and telemetry from a representative run in my ‘indoor range’.

So, now to port this capability to the FourWD_WallE_V2 project:

08 April 2020 Update:

The current operating algorithm  for WallE2 is pretty simple.  Every 200 mSec or so it assesses the current situation and decides on a new operating mode.  This in turn allows the main code in loop() to decide what to do.

Here’s the code for GetOpMode()

In terms of the project to port heading-based specified-offset wall tracking to WallE2, the only pertinent result from GetOpMode() is the default MODE_WALLFOLLOW result.

In the main loop(), a switch(CurrentOpMode) decides what actions, if any need to occur.  Here’s the relevant section of the code.  In the MODE_WALLFOLLOW case, the first thing that happens is an update of the TRACKING CASE via

‘TrackingCase = (WallTrackingCases)GetTrackingDir()’

which returns one of several tracking cases;  TRACKING_RIGHT, TRACKING_LEFT, or TRACKING_NEITHER.  A switch(TrackingCase) handles each case separately.

The two-wheel robot code uses a very simple left/right distance check to determine which wall to track.  In the four-wheel code, the ‘GetTrackingDir()’ function uses a LR_PING_AVG_WINDOW_SIZE-point running average for left & right distances and returns TRACKING_LEFT, TRACKING_RIGHT, or TRACKING_NEITHER enum value.

09 April 2020 Update:

It looks like the two-wheel code actually checks the L/R distances twice; once in GetParallelHdg() and again in the main loop() code.  Once it determines which wall to track, then it uses the same code for both tracking directions, with a ‘turndirection’ boolean to control which way the robot actually turns (CW or CCW) to effect capture and tracking.

The four-wheel code uses two LR_PING_DIST_ARRAY_SIZE buffers – aRightDist & aLeftDist – to hold ‘ping’ measurements. These arrays are updated every MIN_PING_INTERVAL_MSEC with the latest left/right distances, pushing older measurements down in the stack.  The ‘GetTrackingDir()’ function computes the average of the LR_PING_AVG_WINDOW_SIZE latest measurements for tracking direction (left/right/neither) determination.

There are also two utility functions ‘GetAvgLeftDistCm()’ and  ‘GetAvgRightDistCm()’ that are used in several places, but they don’t do a running average of the last LR_PING_AVG_WINDOW_SIZE measurements; instead they do an average of the first LR_PING_AVG_WINDOW_SIZE ones!  Fortunately for the program right now the LR_PING_AVG_WINDOW_SIZE and LR_PING_DIST_ARRAY_SIZE values are the same — 3.

So, I think part of the port needs to involve normalizing the distance measurement situation.  I think the proper way to do this is to revise the ‘GetAvgLeftDistCm()’ & ‘GetAvgRightDistCm()’ functions to compute the running average as it done currently now in GetTrackingDir(), and then call those functions there.  This considerably simplifies GetTrackingDir() and increases its cohesion (in the software engineering sense) as it no longer contains any computations not directly related to its purpose. DONE in FourWD_WallE2_V3 project

The ‘GetTrackingDir()’ function is called in only one place – at the top of the ‘case MODE_WALLFOLLOW:’ block of the ‘ switch (CurrentOpMode)’ switch statement.  The ‘TrackingCase’ enum value returned by ‘GetTrackingDir()’ is then used  within the MODE_WALLFOLLOW:’ block in a new ‘switch (TrackingCase)’ switch statement to determine the appropriate action to be taken.  Here’s the relevant code section:

looking at just the ‘TRACKING_LEFT’ section,

There are three potential actions available in this section; a ‘back up and turn’ obstacle avoidance maneuver, a ‘step-turn to the right’ “upcoming obstacle” avoidance maneuver, and a “continue wall tracking” motor speed adjustment action.

It’s the “continue wall tracking” action that is of interest for porting the new tracking algorithm.  At this point, if this is the first time for the TRACKING_LEFT mode, the robot needs to execute the FindParallelHdg() routine, then capture the offset distance, and then start tracking.  If the previous mode was TRACKING_LEFT, then just continue tracking.

A potential problem with the port idea is that the ‘FindParallelHdg()’ and offset capture routines are ‘blocking’ functions, so if something else happens (like the robot runs into an obstacle), it might not ever recover.  In the current four wheel code, this is handled by checking for obstacle clearance each time through the MIN_PING_INTERVAL_MSEC interval check.  Maybe I can incorporate this idea into the ‘capture’ and ‘maintenance’ phases of the angle-based wall tracking algorithm.  Maybe something like the following state diagram?

Possible state diagram for the TRK_RIGHT case

11 April 2020 Update:

I’m concentrating on the TRACKING_RIGHT sub-case in MODE_WALLFOLLOW, because my ‘local’ (in my office) test range is optimized for tracking the right-hand wall, and I figure I should work our the bugs on one side first.

  • Ported the ‘FindParallelHdg()’ code from the two wheel to the four wheel project, and in the process I changed the name to ‘RotateToParallelOrientation()’ to more accurately describe what the function does.
  • In porting over the actual code that decides what ‘cut’ to use to capture and maintain the desired offset, I realized this should be it’s own function so it can be called from both the left and righthand tracking algorithms.  Then I discovered it already was a function in the two wheel program – but wasn’t being used that way for some unknown reason.  Ported the ‘MakeTrackingTurn()’ function to FourWD_WallE_V3

26 April 2020 Update – Charging Station:

While trying (and failing so far) to work out the wall-following ‘capture/maintain’ algorithms for the four wheel robot, the battery voltage got down to the point where the ‘GetOpMode()’ function was starting to return DEAD_BATTERY. So, I decided this was a good time to complete the required software & hardware modifications to the charging station to work with the new 90 mm x 10 mm wheels I recently added to the robot. To accommodate the larger  diameter wheels I raised the entire charging station electronics platform up by some 14  mm. To accommodate the much narrower wheel width, I had to completely redo the wheel guard geometry, which also required re-aligning the charging station approach guides.

When I was all done with the required physical mods, I discovered that although the robot would still home to the charging station, it wouldn’t shut off its motors when it finally connected to the charging probe. I could see from telemetry that the probe plug had successfully engaged the probe jack’s integrated switch, but the motors continued to run.   However, if I lifted the robot slightly off its wheels from the back while keeping it plugged in, the motors stopped immediately, and the charging operation proceeded normally.  It appeared for all the world like the plug was only partially engaged into the jack. So, I tried the same trick with the robot on its stand (this raises the robot up slightly so the motors can turn without the robot running off on me) and a second probe plug tied to the power supply but physically separate from the charging station, and this worked fine; as soon as the plug was pushed into the jack, the motors turned off and life was good.

So, back to the charging station; I thought maybe the plug wasn’t making full contact due to misalignment and after critically examining the geometry I made some adjustments.  However, this did not solve the problem, even when the plug was perfectly aligned with the jack.  But, with the plug alignment cone attached to the robot it is hard to see whether or not the plug is fully inserted into the jack, so I still thought that maybe I just needed to have the robot plug in with a bit more authority. To this end I modified the software to monitor the LIDAR distance measurement as the robot approached the charging station, and have the robot speed up to max wheel speed when it got within 20 cm.  I also printed up a ‘target panel’ for the charging station so the LIDAR would have a consistent target to work with.  This worked great, but still didn’t solve the problem; the robot clearly sped up at the end of the approach maneuver, but also still literally “spun its wheels” after hitting the charging station stops.   Lifting the back slightly still caused the motors to stop and for charging to proceed normally.  However, I was now convinced this phenomenon wasn’t due to plug/jack misalignment, and I had already confirmed that all electrical connections were correct. So, having eliminated the hardware, the software must be at fault

So, now I was forced to dissect the software controlling the transition from wall-following to IR homing to battery charge monitoring.  In order for the robot to transition from the IR Homing mode to the charge monitoring mode, the following conditions had to exist:

  • The physical charging jack switch must be OPEN, causing the voltage read by the associated MEGA pin to go HIGH.
  • 12V must be present at the charging jack +V pin
  • The Difference between the Total current (measured by the 1NA169 ‘high-side’ current sensor located between the charging jack and the battery charger) and the Run current (measured by the 1NA169 between the power switch and the rest of the robot) must be positive and greater than 0.5A.

Of the above conditions, I was able to directly measure the first two in both the ‘working’ (with the robot on its stand and using the auxiliary charging probe) and ‘non-working’ (when the robot engaged the charging plug automatically) conditions and verified that they were both met in both cases.  That left the third condition – the I_Total – I_Run condition.

The reason for the I_Total – I_Run condition is to properly manage charge cutoff at the 80% battery capacity point.  The robot has a resting (idle) current of about 0.2A, so a 0.5A difference would mean that the battery charge current has decreased to 0.3A, which is slightly above the recommended 90% capacity level (see this post for a more detailed discussion).  So, this condition is included in the ‘GetOpMode()’ algorithm for determining when the battery is charging, and when the battery is fully charged.  In normal operation, I_Total = 0 (nothing connected to the charger) and I_Run = Robot running current, so I_Total – I_Run < 0 and the IsCharging ()  function returns FALSE.  When the robot is connected to a charger, I_Total is usually around 1.5A initially, and I_Total – I_Run > 0.5, causing IsCharging() to return TRUE and the robot to enter the CHARGING mode, which disables the motors.  What I didn’t realize though is that the larger diameter wheels and better motors cause I_Run to be a lot higher than I had anticipated, which means that when the robot plugs into the charging station, the I_Run value goes over 2A with all four motors stalled. This in turn means that the and I_Total – I_Run > 0.5 condition is never met, IsCharging() continues to return FALSE, and the motors never turn off – OOPS!

So, how to fix this problem?  It appears that I don’t want to use the I_Total – I_Run > 0.5 condition as part of the IR Homing –> CHARGING mode transition, but I do want to use it as part of the CHARGING –> CHARGE_DISCONNECT mode transition. This should work, but this exercise got me thinking about how I the charge operation relates to the rest of the robot’s behavior.

The basic idea is for the robot to autonomously seek out and connect to on of a number of charging stations whenever it is ‘hungry’, defined by a battery voltage less than some set threshold, and to avoid those same stations when it isn’t.  When the robot is ‘hungry’ and a charging station signal is detected, the robot should home in on the station and connect to a charging probe, stop its motors, wait until the battery is 80% or so charged, and then back out of the station and go on its merry way.

As currently programmed, the robot operates in one of several modes as shown below.

The program calls a function called ‘GetOpMode’ every 400 msec to determine the proper mode based on sensor input and (to some degree) past history).  The GetOpMode() function is shown below:

The return value from GetOpMode() is used in a SWITCH block to determine the appropriate actions to be taken, as shown below, with the MODE_WALLFOLLOW case reduced to one line for clarity:

In the normal sequence of events, the MODE_IRHOMING case will be executed first. If a charging station signal is detected, the robot will call IRHomeToChgStn() whether or not it is ‘hungry’.  However, if it is ‘not hungry’ IRHomeToChgStn() will return FALSE with the robot oriented 90 degrees to the charging station, which should cause the program to re-enter WALL_FOLLOW mode.  If the robot is ‘hungry’ and successfully connects to the charging probe, IRHomeToChgStn() returns TRUE.  In either case, the program exits the SWITCH block and goes back the top of loop() to start all over again.  The return value of IRHomeToChgStn() is not actually ever used.

The next time GetOpMode() runs, if the robot is indeed connected to the charging station, GetOpMode() returns MODE_CHARGING.  When this section of the SWITCH statement is executed, the motors are stopped and MonitorChargeUntilDone() is called. This function is a blocking call, and doesn’t exit until the robot is fully charged or the timeout value has been reached.  When MonitorChargeUntilDone() returns, the robot backs away from the charging station, turns 90 deg, and returns to wall-following.

When looking back through the above paragraphs, it becomes clear that managing the charging process is broken into two separate but interdependent parts; GetOpMode() recognizes the conditions for entering charging and returns with MODE_CHARGING.  The MODE_CHARGING section of the SWITCH block in loop() actually executes the steps required to begin charging (like stopping the motors, turning off the red laser) and the steps required to disconnect at end-of-charge.

It also becomes clear that once the MODE_CHARGING section of the SWITCH statement is entered, it stays there until MonitorChargeUntilDone() returns at end-of-charge, and the robot disconnects from the charging station.  I think this means the GetOpMode() code can be significantly simplified and made much more readable.  Here’s the new version of GetOpMode():

 

29 April 2020 Update – Tracking (Again):

While screwing around with the charging station code, I managed to charge Wall-E2’s battery to the point where it refuses to connect to the charging station – the ‘Not Hungry’ condition. Rather than just let it run down by running the motors on its stand, I decided to continue working on the wall-following code improvements ported over from the two wheel model.

The last time I worked on the tracking code was back on 11 April when I ported the ‘RotateToParallelOrientation()’ and ‘MakeTrackingTurn()’ functions from the two wheel robot.

For reference, here’s where ended up with the TRACKING_RIGHT case from last time:

The significant changes in the code from where I started are:

  • Disabled ‘Far obstacle’ detection while in ‘capture’ mode to avoid obstacle avoidance step-turns in the middle of trying to capture the desired offset distance
  • When a ‘far obstacle’ IS detected, the StepTurn() function is now called immediately, rather than just starting the turn and letting it play out over multiple passes through the loop. This is now possible due to having accurate relative heading information, and is a huge improvement over the old timed-turn method.
  • Tried, and then abandoned the idea of using the front LIDAR measurement to directly acquire the desired offset distance by turning 45 deg from parallel, driving until the front distance was sqrt(2) (1.414) times the desired distance, and then turning back to parallel.  This failed miserably because the robot’s turning radius is WAY too large to make this work.
  • ported the offset capture/maintenance code directly from the two wheel robot.

So, now the capture/maintenance code in the four whee robot looks like this

I ran a couple of trials in my ‘local’ (office) test range and they looked promising, so I tried a test in my ‘field’ (hallway outside my office) range, with very good results.  Starting from about 75 cm out from the right wall and oriented slightly toward the wall, the robot tracked into the 30 cm desired offset and then maintained that offset to the end of the hallway.  Interesting, it also attempted a ‘step-turn’ obstacle avoidance maneuver at the end, as shown in the following video:

Here is the relevant telemetry output from this run.

From the above telemetry and video it is clear the robot was successful in capturing and maintaining the desired wall offset.  Salient points are (leading numbers denote mSec from start):

  • parallel heading found at 30.984 deg – very close, and very efficient
  • Left/Right Avg Dist = 104/72. So the robot started out with a 40 cm error
  • 5522: In RollingTurn(CW, FWD,10.00). The capture process started with two quick 10-deg CW turns toward the right wall.
  • 5778:  After about 6 times through the loop, the ‘cut’ is increased another 10 deg toward the right wall.
  • 6057: Cut reduced by 10 deg with distance to wall = 64 cm
  • 6693: Cut increased by 10 deg with distance = 57 cm
  • 7017: Cut reduced by 10 deg with distance to wall = 55 cm
  • 7536: Cut increased by 10 deg with distance = 47 cm
  • 7867: Cut reduced by 10 deg with distance to wall = 46 cm
  • 8304: Cut reduced by 10 deg with distance to wall = 42 cm
  • 8727:  Cut reduced by 10 deg with distance to wall = 34 cm

So, it only took about 3 seconds after the initial ‘find parallel’ maneuver to close from 72 to 34 cm and to remove the entire initial ‘cut’ of 20 degrees.  Pretty successful, I would say!

30 April 2020 Update:

After the above successful test, I tried a wall-tracking run with the robot initially oriented slightly away from the wall, and this was a dismal failure.  Looking through the code, I became convinced that the algorithm as currently implemented will never work for the ‘away from wall case’, so now I’m busily rewriting the whole thing so it will work for both cases.

After a couple false starts, I think I now have a working ‘turn to parallel’ algorithm that works for both the ‘toward wall’ and ‘away from wall’ starting orientations.  Here’s a couple of video clips showing the operation. For purposes of this demonstration, I added short (1 sec) pauses between each step in the ‘find parallel’ operation so they can be easily identified in the video.  In actual operation each step should flow smoothly into the next one.

Here’s the code for the newly re-written ‘RotateToParallelOrientation()’:

In the above code, the actual inflection point detection routine was abstracted into its own function ‘FindInflectionPoint()’ as shown below:

At this point I think I have the wall-tracking case completely solved for the right-hand wall tracking case.  Now I have to port the algorithm to the left-hand wall and ‘neither wall’ tracking cases and make sure it all works.  But for now I’m pretty happy.

1 May 2020 Update: Obstacle Avoidance

Happy May Day!  In Ohio USA we have now been in ‘lockdown for two entire months – All of March and all of April.  According to our governor, we may be able to go out and play at least a little bit in the coming month – yay!

At the end of both the last two videos of successful ‘RotateToParallelOrientation()’ runs, the robot gets close to the end of the hallway and attempts a left/right step-turn maneuver, ending with its nose against the far wall.  The maneuver occurs way too close to the obstacle to do any good as a ‘in-line obstacle’ avoidance maneuver; at this distance, a 90 degree turn to follow the new wall would be a better deal.

When I looked into the telemetry log to determine what happened, I discovered the late step-turn was a consequence of an earlier decision to significantly shorten the ‘far obstacle’ detection distance, in order to avoid step-turns in the middle of the robot’s attempt to capture and then maintain the desired wall offset distance.  Unfortunately the detection distance wasn’t re-instated to its former value once the robot had captured the desired wall offset, so it didn’t detect the upcoming wall as an obstacle until too late.

And this line of thought brings up another issue; what defines the transition from the ‘approach’ state to the ‘distance hold’ state, anyway?  In previous work I had sketched out a proposed state diagram for the wall-following mode, as shown below:

Possible state diagram for the TRK_RIGHT case

And I now realize it is both incomplete and poorly labelled.  I now believe the ‘Capture’ state should be renamed to ‘Approach’, and the ‘Maintain’ state should be renamed ‘Offset Hold’.  Moreover, there should be a third ’90-deg left turn’ state that is entered if the step-turn maneuver fails to bypass the ‘far obstacle’.  Maybe something like:

02 May 2020 Update: Back to Charging Station Code

After a week of working with wall-following code, I managed to discharge Wall-E2’s battery to the point where it’s looking to be fed again, so I’m back on the charging station support code.  When we last left this portion of the saga, I had discovered the problem with Wall-E2 not shutting off its motors after connecting to the charging station, and was in the process of making the changes when Wall-E2 turned up its nose at the charging station and I had to go do other stuff for a while.

Now, back on the Charging Station case, I found that I had to make some significant changes to GetOpMode; I had to move the DEAD_BATTERY condition check to after the IR Homing and Charger connected mode checks; otherwise, the robot can’t home to a charging station with a low battery because the DEAD_BATTERY mode will be detected first – oops!  The new version of GetOpMode() is shown below:

After these (and some other minor improvements) I was able to get Wall-E2 to successfully home to a charging station, stop its motors (yay!) and commence charging.  As the following videos show, I was able to do this on my desktop ‘local’ range, and also ‘in the field’ (aka my atrium hallway).

After the robot connects to the charging station, the rear LED strip changes function to become a ‘fuel guage’, with ’empty’ on the right and ‘full’ on the left.  In the videos above, note that about 6 seconds after the robot connects, the LED strip changes to show ‘almost empty’.

Stay tuned,

Frank

 

 

 

 

 

 

 

Wall tracking: finding the heading parallel to the nearest wall

Leave a reply

Posted 14 February 2020,

Happy (American) Valentines Day! In my last post, I described my plan to use Wall-E2’s new relative heading super power to find the relative heading parallel to the nearest wall.  I ended that post with “…and not all that hard to program, either”.  Well, this turned out to be a bit of an exaggeration as things weren’t quite as easy as I first thought; the interaction of the physics of the robot and the time scales associated with ping measurements complicated things a bit.

Background:

For some time now I have been working on ways to enhance Wall-E2’s autonomous wall-tracking ability.  Wall-E2 can track walls fairly well, but lacks the ability to track a wall at a specified stand-off distance.  Currently, tracking occurs at whatever distance Wall-E2 first detects the nearest wall. While this isn’t terrible, I wanted to do better.

Unfortunately, the way in which the measured ‘ping’ distance to the nearest wall interacts with the relative orientation of the robot with respect to that wall makes it almost impossible to determine the actual offset distance, and therefore how to determine what to do to maintain a constant offset distance.  As shown in the following diagram, when the robot makes a turn, the measured distance to the wall will change just due to the orientation change, without the robot’s actual offset distance changing at all.

Without having some idea of the angle theta in the above diagram, making a judgement of where the robot is relative to the target offset distance is difficult, if not impossible.   This situation was the impetus for adding the MPU6050 Inertial Measurement Unit (IMU) to Wall-E2’s list of super powers. The general idea was that knowledge of relative headings would allow Wall-E2 to make accurate heading-controlled turns without relying solely on timing.  After a lot of work to eliminate RFI/EMI problems associated with the Pololu metal-geared motors on Wall-E2, I’m happy to say that the MPU6050 is now quite stable, and making turns of just a few degrees is quite possible.

However, acquiring and then maintaining a particular offset distance from the nearest wall is still not straightforward.  Back in early December last year I demonstrated the ability to acquire and then maintain a constant offset distance, but only if the robot started out reasonably parallel to the wall.  If the robot was oriented toward or away from the wall by more than a few degrees, it would not work.  So I needed to find a way to first orient the robot parallel to the nearest wall at any distance, so that my current acquisition & tracking algorithm would work successfully.

The basic idea behind finding the parallel heading is that when the robot is turned through a forward arc and the measured ‘ping’ distance decreases and then starts increasing, the robot’s relative heading at this inflection point is the desired parallel heading.   If the distance instead starts increasing, then the robot started out either parallel to or facing away from the wall.  In either case, reversing the turn back toward the wall will cause the measured distance to decrease to a minimum and then increase again.  As in the first case the heading at the point at which the measured distance starts to increase is the desired parallel heading.

Although the basic idea as described above is very straightforward, as usual there are some ‘gotchas’ in the actual implementation:

In order to minimize heading overshoot due to the robot’s mass and angular momentum,  the parallel heading search turns must be performed at lower-than-normal speeds.  After some experimentation I settled on a turn rate of about 60 deg/sec.  With the robot starting with an angle-in orientation of about 30 deg, this means that it takes about 1 second to sweep through to an angle-out orientation of about 30 deg.  With the Arduino UNO setup I’m using for the tests, I was getting distance measurements about every 30-50 mSec, so about 33 to 20 measurements/sec, or around 2-3 measurements/degree.

The lower turn rate significantly reduces the rate at which the ‘ping’ distance changes per unit time, making it much harder to detect the distance inflection point.  In effect, the lower turn rate flattens the ‘distance/degree’ slope, making inflection point detection more difficult.  At 20-30 measurements/degree and only a few cm change from max on one side to max on the other, there are a lot of identical measurements returned.

My initial cut at addressing the the above issue was to space the ping measurements further apart in time, thereby increasing the ‘distance/degree’ slope. After trying this (using a ‘elapsedMillisec’ variable) I realized that an equivalent method would be to simply increase the size of the inflection detection window (the number of times the ping measurement must be on the ‘other side’ of the inflection point in order to qualify as a valid inflection). After some experimentation, I arrived at a value of 20.

For some reason, it was much easier to find a good parallel heading value if the robot started out pointed toward the near wall. If it started out pointed away from the wall, the robot often stopped well short of or well after the actual parallel heading.  Eventually I developed a 4-turn process for this case to really nail down the parallel heading.  Here are some short videos demonstrating the algorithm.

Now that I can reliably determine the relative heading that orients the robot parallel to the nearest wall, I should be able to marry this capability with my already-developed algorithm for acquiring and maintaining a specific offset distance.

20 February 2020 Update:

So I combined the ‘find parallel heading’ feature with my already-existing angle-based tracking algorithm, and this worked fairly well.  Here’s a short video demonstrating the technique:

In the above video, the blue painter’s tape strips are marked every 10 cm, with a double-width mark at 30 cm (the desired offset distance). As the video shows, the robot first determines an approximate parallel heading, and from there starts the normal angle-based tracking algorithm.

Next, I tried an ‘enhancement’ to the above by having the robot move toward the wall on a 30-45 deg ‘cut’ from the parallel heading, and then turning back to parallel at the desired offset distance.  As the following video shows, this didn’t turn out so well.  If the robot doesn’t start out exactly parallel, then the ‘cut’ is either too steep or too shallow, resulting in a too-early or too-late turn back to the parallel heading.

So it looks like the ‘find parallel then start tracking’ approach works pretty well, but the ‘find parallel then drive to offset on a cut then back to parallel’ approach hasn’t been very successful.

27 February 2020 Update

After thinking about the difficulties I was encountering with my ‘FindParallel’ algorithm, I realized that the reason the robot was often overshooting the parallel orientation was due to small aberrations in ‘ping’ distance measurements that caused the ‘hit counter’ to reset to zero in the middle of an otherwise perfect arc of distance values.  The ‘hit counter’ is incremented each time the newest ‘ping’ distance measurement trends along the same line, and is reset to zero whenever the newest ping measurement breaks the trend. When the hit counter exceeds a preset level, the parallel condition is considered to be detected.  I thought I might be able to improve performance by making the algorithm a little more tolerant of such aberrations.  So, rather than having the ‘hit counter’ reset to zero, I changed the algorithm to decrement by a set amount rather than reset it to zero.  This markedly improved performance, as shown in the following videos.

There are four sections in the above video.  In the first clip, the robot starts out pointed away from the wall and outside the desired 30 cm offset. The ‘FindParallel’ algorithm executes, and then approaches and then tracks the wall at the desired 30 cm offset.  The next three clips show the same situation, except starting outside the 30 cm offset and pointed toward the wall, and then from inside the 30 cm offset, pointed away from and toward the wall.  In each case, the robot successfully acquires a reasonably parallel heading and then acquires and tracks the 30 cm offset distance.

Stay tuned!

Frank

 

Back to the future with Wall-E2. Wall-following Part VIII

Leave a reply

Posted 25 January 2020

About 6 weeks ago I posted that I had finally killed the “intermittent MPU6050 failure” dragon, by belatedly following Pololu’s recommendations for installing bypass capacitors on their metal-geared motors.  Unfortunately it turned out that my celebration was cut short by more annoying intermittent MPU6050 failures, so I was once again forced back to the drawing board.

This time I decided that the only way to figure out what was going on was to actively examine the I2C traffic in real time, to determine who exactly was doing what to whom.  So, over the course of the six week period between my last declaration of victory to this one, I created a Teensy based I2C bus ‘sniffer’ and used it to figure out what was going on.  I was able to determine that the ‘master’ micro-controller continued to operate normally through a failure, but the MPU6050 didn’t. I was also able to determine that just resetting the IMU would not allow the system to recover, but resetting the micro-controller often did.   Moreover, I was able to definitively show that the problem was caused by ‘contact bounce’ on one or more of the four 6″ male-male jumper wires connecting the micro-controller to the IMU.  Eliminating these jumpers also (I hope) eliminated the last piece of the “I2C Intermittent failure” puzzle.

Looking back over the entire I2C failure saga, I now realize that this was the classic case of multiple failure modes complicating the troubleshooting effort.  The RFI/EMI problem caused by the Pololu metal-geared motors completely overshadowed the issue of non-secure jumper connections. Then, after finally coming to my senses and installing the recommended bypass capacitors on the motors, the ‘contact bounce’ problem was unmasked.  I do love interesting problems, but this one went past ‘interesting’ and was well into ‘agonizing’ by the time I got it solved ;-).

After getting everything set up, I ran some wall tracking tests in my entry hall ‘test range’ with pretty good results, as shown in the short video clip below.

Stay tuned,

Frank

30 January 2020 Update:

Still having trouble with the initial approach to a wall from outside the target distance. The robot still has a tendency to dive into the wall, unable to cope with the problem of the measured distance increasing instead of decreasing when the robot turns into the wall.  This inverse relationship makes it almost impossible to use a simple ‘turn toward the wall and wait for the distance to count down’ technique.

After thinking about this for while, I realized that this would all be so much simpler if I cheated and started with the robot placed parallel to the target wall.  Then the robot could simply turn 45 deg toward the wall and proceed until the measured wall distance was appropriate (Dtgt / 0.707), and then turn parallel again.  Then I realized that I could easily determine the parallel condition by turning the robot toward and/or away from the wall while continuously measuring the distance; when the measurement goes through a minimum, then the robot is parallel to the wall.  Simple in concept, and not all that hard to program, either.

 

IMU Motor Noise Troubleshooting, Part III

Leave a reply

Posted 19 January 2020

In Part II of this saga, I described my continuing efforts to track down and fix the problem of intermittent failures associated with the MPU6050 IMU on my robot.  The MPU6050 IMU is required for the ability to make precise heading-based turns, which is in turn required to track walls at a designated stand-off distance.

This post summarizes the work to date and suggests new avenues of investigation for fully addressing the motor noise issue.

Summary of work to date:

  •  July 2019: First started working with heading-based turns, and first noticed the motor noise problem.  Basically the problem presented itself as frequent, abrupt, and wildly divergent heading readings when the motors were running, but perfectly stable readings when the motors are not running.  See this post for the details.
  • October 2019: Successfully demonstrated polling-based (vs interrupt-based) MPU6050 IMU management. This development meant that I could acquire yaw (heading) values on an as-needed basis rather than at a 20 or 200Hz rate, throwing away 99% of the results.  This was demonstrated in this post.
  • November 2019: Made another run at solving the motor noise problem using a home-brew optical isolator and  a 2-stage power filter.  After a LOT of work, I wound up discovering that most (but not all!) of the problem could be addressed with proper RF bypassing at the terminals of the metal-geared Pololu motors I was using.  See this post for the details.
  • Early December 2019:  Demonstrated heading-based wall offset tracking using my 2-motor robot, with RF bypassing installed on both Pololu metal gear motors.  No IMU failures were noticed during these runs.  See this post for details.
  • Early December 2019:  Reprised some of the motor driver testing performed back in May of 2019 (see this post), and again noticed MPU6050 IMU communication failures when the motors were running, but none when the motors weren’t running. This test was performed on the 2-motor robot using the Pololu motors with the RF bypassing in place. So clearly just the bypassing was not of and by itself sufficient to solve the problem; something else had to be going on.  See this post for the details.
  • Late December 2019 to mid-January 2020:  I decided I needed a tool to monitor the I2C bus traffic between the robot’s controller and the MPU6050 IMU – an I2C ‘sniffer’.  After some research, I found that the cheapest commercial sniffer cost about $330, and DIY sniffers were few and far between. I did, however, find a Teensy-based sniffer program by Kito, so I had a starting place.  After three major development stages, I had a Teensy 3.2 program that would reliably monitor I2C communications between an Arduino (Mega or Uno) master and a MPU6050 slave, using the polling approach developed earlier.  See this post, this post, and this post for the development details.

Current Effort:

With the above history in mind, I applied my new I2C sniffer tool to the Motor Noise Problem.  As usual, I started this using the simplest possible setup; an Arduino Mega acting as the I2C master running my polling based ‘MPU6050_MotorNoiseTest1’ program, and a Teensy 3.2 and a MPU6050 IMU module both mounted on a small plugboard, as shown below.

Arduino Mega I2C master, with Teensy I2C sniffer and MPU6050 module on a separate plugboard

I played around with this setup for a while, and captured at least one IMU communications failure with the sniffer active. The failure occurred when I was moving the plugboard around a bit to verify that the MPU6050 IMU heading values changed appropriately.  At some point I noticed the I2C monitor output had changed its character significantly, so I quickly stopped the sniffer program and opened the log file (see attached file below).

From the log I can see that things proceeded normally until 6012443 mSec ( 1.67 hours) and then changed to report that nothing was being received from register 0x72 (the FIFO count register). This continued until 6022224 mSec (9.8 seconds later) where it returned to what looks like normal operation.

So, my preliminary guess at what happened is the connection from the Mega to the Teensy/MPU6050 got dropped momentarily, and it took the Teensy a while to find another START sequence in the I2C data stream from the Mega, as the ‘2048’ number in “6017280: processed = 2048 elements in 3 mSec” means that the capture buffer overflowed before a START sequence was detected.  “At 6022240: processed = 1224 elements in 2 mSec” means that a normal Mega ‘burst’ was captured and operation returned to normal.

Since the Teensy I2C monitor is on the MPU6050 end of the male-male jumpers, It begins to look like the Mega was still doing fine, but the jumper connection burped on one end or the other.  More testing to follow.

Loader Loading...
EAD Logo Taking too long?

Reload Reload document
| Open Open in new tab

Download

 

Next, I moved the plugboard containing the Teensy I2C Sniffer and the MPU6050 module to my 2-motor robot, and used the existing Arduino Uno on the robot as the master, as shown below.

I loaded my MotorNoiseTest1 program on the Uno, and allowed it to run both motors at a steady rate, while monitoring the I2C traffic with the Teensy, and also monitoring the heading values being printed out by the Uno.  I started the program just before 1PM, and it was still running fine with no IMU errors at 10pm, more than 9 hours later!  The I2C sniffer log shows regular communication with the MPU6050, and the calculated yaw value based on the packet bytes received by the sniffer program matches the yaw value calculated in the Uno program. This is clear verification that the sniffer program will run ‘forever’, and that at least in this case, the two motor robot will also run ‘forever’ with no  yaw errors.

Based on my earlier experience with the captured I2C communications failure, I’m more inclined now to believe that motor vibration or other mechanical perturbation is causing a momentary I2C bus or power/ground lead disconnect.  More tests to follow:

21 January 2020 Update:

After the 10-hour run described above, I tried to induce some failures by fiddling with the I2C and power/ground jumper wires, and found that I could easily and reliably cause a failure by ‘flicking’ the wires with my finger or a pen.  After each failure, the built-in recovery routine of clearing the FIFO and resetting the DMP failed to restore communications.  However, manually resetting the UNO did allow the system to recover.

From the above, I believe it’s safe to say that the current male-male jumper connections between the UNO and the Teensy/IMU are unreliable, and are hopefully the only remaining failure mode.  I haven’t quite figured out how to replace the connections with something more reliable, but I’m working on it.  I moved the IMU module from the plugboard and plugged its I2C pins directly into the I2C sockets on the UNO.  Then I replaced the power & ground leads with a permanent twisted pair connection to the Wixel shield, as shown in the following photo.

MPU6050 plugged directly into Uno board, with pwr/gnd jumpers replaced with permanent twisted pair

Then I fired up the system and ran it for a while but was unable  to make it fail.  This is encouraging news to say the least.

Stay tuned,

Frank

Teensy I2C Sniffer for MPU6050 Part II

Leave a reply

Posted 13 January 2020,

In my last post on this subject, I described my efforts to build an I2C bus sniffer using a Teensy 3.2 micro-controller.  This post describes my efforts to move from a fixed array containing a 928-byte snapshot of an I2C bus conversation between an Arduino Mega 2560 and a MPU6050 IMU to a live, repeated-burst setup.

As the source for I2C traffic for the MPU6050 IMU I am using my MPU6050_MotorNoiseTest1 Arduino project with no motors or sensors connected.  All the code does is ask the MPU6050 for a yaw value every 200 mSec (the value of NAV_UPDATE_INTERVAL_MSEC), as shown below:

The Teensy code to monitor the I2C bus traffic is shown below.  When I first started working with this project, I copied Kito’s I2C sniffer code, which used Teensy’s Timer1 interval timer set to produce interrupts every 1 uSec, and an ISR to capture the data.  This turned out to be hard to deal with, as I couldn’t add instrumentation code to the ISR without overrunning the 1 uSec interrupt period, leading to confusing results.  So, for this part of the project I disabled the Timer1 interrupt, and called the ISR directly from the loop() function.  As others have pointed out, the Arduino loop() function does a lot of housekeeping in the background, so for top performance it is best to never let loop() execute, by placing another infinite loop inside loop() or inside setup().  This is what I did with the code designed to investigate whether or not the Teensy could keep up with an I2C bus running at 100Kbs.

The ‘capture_data()’ function (no longer used as an ISR) captures SCL & SDA states with a single port operation as shown

and then everything from a START pair (0xC followed by 0x4) to a STOP pair (0X4 followed by 0xC) inclusive is captured in the raw_data array.

Any I2C Sniffer project like this one assumes that I2C activity occurs in short bursts with fairly long pauses in between.  This is certainly the case with my robot project, as yaw data is only acquired every 200 mSec.  However, there is still the problem of determining when a I2C ‘burst’ has finished so the sniffer program can decode and print the results from the last burst.  In my investigation, it became clear that at the end of the burst both the SDA line goes HIGH and stays that way until the next START condition (a 0XC followed by a 0X4).  So then the question becomes “how many 0XC/0XC pairs do I have to wait before determining that the last burst is over?”

In order to answer this question I decided to use my trusty Tektronix 2236 O’Scope and Teeny’s ‘digitalReadFast’ and ‘digitalWriteFast’ functions to implement a hardware-based timing capability using Teensy pins 0,1, and 2 (MONITOR_OUT1, 2 & 3 respectively).  Among other things, this allowed me to definitively determine that a ‘idle’ (0XC/0XC) count of 1000 was too small, but an idle count of 2500 was plenty, without consuming too much of the available processing time.  It also turned out that ‘idle’ counts all the way up to 30,000 work too, but leave less time for processing.

O’Scope shot showing I2C traffic on the bottom trace, and the point at which 2500 0xC/0xC (Idle) pairs is reached on the top trace (the high-to-low transition)

As can be seen in the above photo, the I2C ‘sentence’ lasts about 15 mSec, and the ‘idle’ condition is detected about 5 mSec later for a total of about 20 mSec out of the nominal 200 mSec cycle time for my robot application. This leaves about 190 mSec for I2C sentence processing and display.

18 January 2020 Update:

Success!!  I now have a working Teensy 3.2 I2C Sniffer program that can continuously monitor the I2C traffic between my Arduino Mega test program acting as a I2C master and a MPU6050 IMU I2C slave.   The Teensy code is available on my GitHub account here.

A major challenge in creating the sniffer program was the requirement to sample the I2C SCL & SDA lines quickly enough to accurately detect the line transitions denoting all the different I2C signals.  With the I2C bus running at 100Kbs, SCL (clock) transitions occur every 5 uSec. Good sampling requires at least 2 and preferably more samples per SCL state.  As noted above, I started off by copying the ISR routine from Kito’s I2C sniffer, but discovered I needed to add some logic to zero in on the desired I2C bus states (IDLE, START, DATA & STOP), and the additional code made the ISR take more than the desired 1 uSec window.  After posting about this problem to Paul Stoffregen’s Teensy forum, I got some good pointers for speedup, incuding a post that mentioned the Teensy FASTRUN macro that runs functions from RAM rather than FLASH. As it turned out, adding this macro to the program allowed me to reduce the ISR cycle time from about 1.4 uSec to about .89 uSec – yay!  The final ISR routine is shown below:

Note the use of digitalWriteFast() calls to output timing pulses on Teensy hardware pins so I could use my trusty Tek 2236 100 MHz O’scope to verify proper timing.

Once I got the ISR running properly, then I focused on getting the data parsing algorithm integrated into the program.  I had previously shown that I could correctly parse simulated I2C traffic, so all the current challenge was to integrate the algorithm in a way that allowed continuous capture-decode-print cycles at at rate that could keep up with the desired 5 measurements/sec rate.  So, I instrumented the sniffer program to display the decoded IMU traffic, along with the calculated yaw value and the time required to perform the decode.

Here’s a short section of the printout from the test program, showing the time (in minutes), the yaw (relative heading ) value retrieved from the IMU, and left/right ping distances (unused in this application).

And here is the corresponding output from the I2C sniffer program

In the above printout, each printout shows the individual transmit & receive ‘sentences’ to/from the IMU, and the 28-byte packet received from the IMU containing, among other things, the values required to calculate a yaw (relative heading value).  As can be seen, the yaw value calculated from the received bytes, closely matches the yaw values retrieved using the test program.  In addition the last line of each section of the readout shows the time tag for the start of the decode process, and the total time taken to decode all the bytes in that particular burst.  From the data, it is clear that only 1-2 mSec is required to decode and display a full burst.

The complete I2C Sniffer program is available on my GitHub site here.  The complete test program that obtains a yaw value from the IMU every 200 mSec is shown below:

The above program was intended to help me troubleshoot the intermittent MPU6050 connection failures I have been experiencing for some time now.  The purpose of the new I2C sniffer project is to create a tool to log the actual I2C traffic between this  program and the IMU. The idea is that when a failure occurs, I can look back through the sniffer log to see what happened; did the Arduino Mega stop transmitting requests, or did the IMU simply stop responding, or something else entirely.

 

 

Teensy I2C Sniffer for MPU6050

Leave a reply

Posted 02 January 2020

On my last post on the I2C subject, I described an Excel VBA program to parse through a 928-byte array containing the captured I2C conversation between an Arduino Mega and a MPU6050 IMU module. The Arduino was running a very simple test program that repeatedly asked the MPU6050 to report the number of bytes available in its FIFO.  Then I used Kito’s I2C sniffer code to capture the SDA/SCL transitions, which I then copy/pasted into Excel.

This post describes the next step, which was to port the Excel VBA code into a Teensy sketch using the Arduino version of C++, moving toward the ultimate goal of a Teensy based, fast I2C sniffer that can be used to monitor and log long-term (hours to days) I2C bus conversations to determine what is causing the intermittent hangups I’m seeing with my Arduino Mega/MPU-6050 robot project.

The code port took a while, but mostly because of my own lack of understanding about the details of the I2C protocol and the specifics of the communication between the Arduino test program and my MPU6050 IMU module.  After working through these problems, the end result was surprisingly compact – less than 500 lines, including 50 lines of test data, LOTS of comments and debugging printouts, as shown below:

When I ran this program against the captured data, I got the following output:

which, when compared against the debug printout from Jeff Rowberg’s I2CDev program,

shows that my Teensy program correctly decoded the entire test dataset.

The next step in the process is to modify the above program to allow long-term real-time monitoring and logging of a live I2C bus. By ‘long-term’, I mean hours if not days, as the object of the exercise is to figure out why the I2C bus connection to the MPU6050 on my two-motor robot intermittently fails when the motors are running.  A failure can occur within minutes, or only after several hours, and there doesn’t seem to be any rhyme nor reason, except that the motors have to be running.

In normal operation, my two-motor robot obtains a heading value from the MPU6050 once every 200 mSec or so.  This I2C bus activity might comprise only 100 SCL/SDA transitions or so, and no other I2C bus activity takes place in the times between heading value requests.  So, there will be few mSec of burst activity, followed by a 150-190 mSec idle period.   To monitor and log in real time, I need some sort of FIFO arrangement, where the I2C transition data can be saved into the FIFO during the burst, and then processed and saved into a log file during the idle period.

While I was searching the web for I2C sniffer code, I also ran across this thread by tonton81 describing his template based circular buffer library for the Teensy line.  The thread started a little over two years ago, but has been quite active during that period, and tonton81 has made several bugfixes, updates, and enhancements to his code.  This might be just the thing for my project.

06 January 2020 Update:

After integrating tonton81’s circular buffer library into the project (thanks tonton81 for being so responsive with bugfixes!), I was able to demonstrate that the circular buffer version, when run against the same 928-byte simulated dataset, produced the same output as before, as shown below:

From the output above, it is clear that the Teensy can parse and print a typical 1000-byte burst in just a few mSec (3 in the above run), so it should have no problem keeping up with a 200 mSec data burst interval, and should be able to keep up with burst intervals down to around 10 mSec (100 bursts/sec!)  I suspect that the Teensy’s major problem will be not dying of boredom waiting for the next burst to process!

Here’s the full code (not including circular_buffer.h):

The next step in the project will be to modify the code (hopefully for the last time) to capture and process live I2C traffic bursts in real time.

I modified the interval processing code in loop() to reset the stop/restart Timer1 & clear FIFO each time the interval block is executed, and then reduced the processing interval from 200 to 50 mSec, to produce the following output:

The modified code:

and a short segment of the output:

Note that the processing block is indeed called every 50 mSec, and takes only a few mSec to complete.  The following is an O’scope image showing multiple 50 mSec periods.  As can be seen on the image, there is still a LOT of dead time between 928-byte bursts.

Top trace toggles at 50 mSec intervals to simulate periodic IMU data retrieval. Bottom trace shows IMU/MCU I2C communication bursts.

Stay tuned!

Frank

 

 

I2C Bus Sniffing with Excel VBA

Leave a reply

In my never-ending quest to figure out why my I2C connection to an MPU6050 dies intermittently, I decided to try and record the I2C bus conversation to see if I can determine if it is the MPU6050 or the microcontroller goes tits-up on me.

Of course, this adventure turned out to be a LOT more complicated than I thought – I mean, how hard could it be to find and run one of the many (or so I thought) I2C sniffer setups out there in the i-verse?  Well, after a fair bit of Googling and forum searches, I found that there just aren’t any good I2C sniffer programs out there, or at least nothing that I could find.

I did run across one promising program; it’s a Teensy 3.2 sniffer program written by ‘Kito’ and posted on the PJRC Teensy forum in this post.  I also found this program written for the Arduino Mega.  So, I created a small Arduino Mega test program connected to a MPU6050 using Jeff Rowberg’s I2CDev library.

This program sets up the connection to the MPU6050 and then once every 200 mSec tests the I2C connection, resets the FIFO, and then repeatedly checks the FIFO count to verify that the MPU6050 is actually doing something.

When I ran Kito’s I2C sniffer program on a Teensy 3.2 (taking care to switch the SCL & SDA lines as Kito’s code has it backwards), I get the following output

which isn’t very useful, when compared to the debug output from Jeff Rowberg’s I2CDev program, as follows:

As can be seen from Jeff’s output, there is a LOT of data being missed by Kito’s program. It gets the initial sequence right (S,Addr=0x68,W,N,P), but skips the 8-bit data sequence after the ‘W’, and mis-detects the following RESTART as a STOP.  The next sequence (S,Addr=0x68,R,N,P) is correct as far as the initial address is concerned, but again omits the 8-bit data value after the ‘R’ direction modifier.

Notwithstanding its problems, Kito’s program, along with this I2C bus specifications document  did teach me a LOT about the I2C protocol and how to parse it effectively. In addition, Kito’s program showed me how to use direct port bus reads to bypass the overhead associated with ‘digitalRead()’ calls – nice!

I got lost pretty quickly trying to understand Kito’s programming logic, so I decided I would do what any good researcher does when trying to understand a complex situation – CHEAT!!  I modified Kito’s program to simply capture the I2C bus transitions associated with my little test program into a 1024 byte buffer, then stop and print the contents of the buffer out to the serial port.  Then I copy/pasted this output into an Excel spreadsheet and wrote a VBA script to parse through the output, line-by-line. By doing it this way, I could easily examine the result of each script change, and step through the script one line at a time, watching the parsing machinery run.

Here’s a partial output from the data capture program:

So then I copy/pasted this into Excel and wrote the following VBA script to parse the data:

The above script assumes the data is in column A, starting at A1. A partial output from the program is shown below, showing the first few sequences

The above output corresponds to this line in the debug output from Jeff Rowberg’s I2Cdev code:

So, the VBA program is parsing OK-ish, but is missing big chunks, and there are some weird 1 and 2 bit sequences floating around too.

After some more research, I finally figured out that part of the problem is that the I2C protocol allows a slave device to pull the SCL line low unilaterally to temporarily suspend transmissions until the slave device catches up.  This causes ‘NOP’ sequences to appear more or less randomly in the data stream.  So, I again modified Kito’s program to first capture a 1024 byte data sample, and then parse through the sample, eliminating any NOP sequences. The result is a ‘clean’ data sample.  Here’s the modified Kito program

and a partial output from the run:

After processing all 1024 transition codes, 96 invalid transitions were removed, resulting in 928 valid I2C transitions.

When this data was copy/pasted into my Excel VBA program, it was able to correctly parse the entire sample correctly, as shown below:

This corresponds to the following lines from Jeff’s program:

Although the VBA code correctly parsed all the data and missed nothing, there is still a small ‘fly in the ointment’; there is still an extra ‘0’ bit after every transmission sequence.  Instead of

we  have

with an extra ‘0’ between the ACK/NAK and the RESTART.  This appears in every transmission sequence, so it must be a real part of the I2C protocol, but I haven’t yet found an explanation for it.

In any case, it is clear that the Excel VBA program is correctly parsing the captured sequence, so I should now be able to port it into C++ code for my Teensy I2C sniffer.

Stay tuned!

Frank

 

 

 

 

 

 

 

 

Wall-E2 Motor Controller Study Part II

Leave a reply

Posted 07 December 2019 (Pearl Harbor Day)

Back in May of this year I posted about different motor driver possibilities for my 2-motor and 4-motor robots, and showed some results for two drivers (an Adafruit Featherwing and an Adafruit DRV8871).  However, I got kind of sidetracked after this post when I discovered the RFI/EMI problem with the MPU6050 IMU.  At the time, I blamed the RFI/EMI problem on the non-linear nature of the newer drivers, and went back to using the L298N linear driver.

After quite a bit of experimentation and work, it finally turned out that most (if not all) the RFI/EMI problem was the Pololu 20D metal-geared motors themselves, and properly suppressing the noise at the motor terminals with bypass capacitors solved the problem.  So I have decided to repeat some of my initial motor driver testing.

Adafruit DRV8871 Single Channel Motor Driver Testing:

I removed the L298N driver I have been using up until now on my 2-motor robot, and replaced it with two DRV8871 Single Channel Motor Drivers. The hookup with the DRV8871’s is actually significantly simpler than the L298N, requiring only two control lines per channel instead of three.  After the normal number of errors, I got it running and started some long-term tests with the motors running continuously (with varying speeds and directions) while polling the MPU6050 every 200 mSec for yaw data.

This test ran for over 5 hours without problems, and then the GY-521 (generic MPU6050) module stopped responding to requests for data.  This is the second MPU6050 module that  has behaved this way – running for an indefinite amount of time and then refusing to respond.

The good news is, the DRV8871 motor drivers worked flawlessly the entire time, and are efficient enough so the T0-3 size chip container was just barely warm to the touch.

I have run  several long-term tests now with the DRV8871 drivers and two different MPU6050 modules, and the longest error-free run has been about 5 hours as shown above.  However, I have also had the test terminate in less than 5 minutes, so this is clearly not reliable enough for prime time. Also, adding my 2-stage power supply filter back into the system did not seem to effectively suppress errors, so no I have no clue what is going on.

11 December 2019 Update:

I ran the system overnight with the same test program, except I commented out the motor run commands so the motors themselves did not run. The test ran for over 11 hours and was still running fine when I terminated it.  So, the motors definitely have to be running for the problem to occur.

15 December 2019 Update:

Hoping to maybe eliminate a variable, I changed from the UNO controller to a Teensy 3.2. As usual, this ‘small change’ took a LOT more time than I thought it would.

There aren’t any good example programs for interfacing a Teensy 3.x with the MPU6050, especially using the I2CDEV/MPU6050/DMP libraries. I had a huge problem trying to track down compile issues with the I2CDev libraries, but in the end it came down to figuring out a way to get I2CDev to use i2c_t3.h instead of Wire.h. I solved this by copying I2CDev.h/cpp from my Libraries folder to my local project/solution folder, and editing the code to define the I2C implementation as I2CDEV_ARDUINO_WIRE and then replacing “#include <Wire.h>” with “#include <i2c_t3.h> in the appropriate section as shown below. I’m sure there are more elegant ways of doing this (maybe adding a ‘I2CDEV_TEENSY_3.X’ section at the top?)

After making the above change, the project started compiling OK, and I was able to connect to the MPU6050 and pull off yaw values using the DMP.

The next problem occurred when I tried to run a test program with the Adafruit DRV8871 drivers.  The two control lines alternate between being used as digital outputs (for direction control) and analog/PWM outputs (for speed control). Unfortunately, once a Teensy 3.2 line has been written to with ‘analogWrite()’, it can no longer be used as a digital output without first running ‘pinMode([pin],OUTPUT)’ on it. This particular little ‘gotcha’ appears to have been there since around 2016, but got lost in the shuffle as it is still there in the latest TeensyDuino libraries.

After fixing that problem, I was successful in both getting yaw values from the MPU6050 using Jeff’s I2CDev libraries, and in driving my Pololu D20 metal-geared motors via the Adafruit DRV8871 motor driver modules.

After getting everything working, I took the time to fork Jeff Rowberg’s I2CDev library, edit I2CDev.h/cpp appropriately and create a pull request for Jeff.  The changes were merged into the Github master distro pretty quickly, so in theory at least, all a new Teensy 3.2 user has to do is grab Jeff’s I2CDevLib stuff and go.

Here’s my Teensy 3.2 program for testing the MPU6050/DMP and the Adafruit DRV8871 motor drivers:

Back to the future with Wall-E2. Wall-following Part VII

Leave a reply

Posted 06 December 2019

Back in August of this year (4 months ago!) I demonstrated successful wall tracking at an arbitrary offset distance using heading information from a MPU6050 IMU using my little 2-motor robot.   At the time (silly me) I thought the next step was to integrate this new capability back into my larger 4-motor robot and let it loose into the ‘wild’ (my house).  Unfortunately the EMI/RFI problem that I thought I had solved reared its ugly head again, and sent all my beautiful plans right into the crapper :-(.

So, I have spent the last four months once again tracking down and eliminating the issue (really for sure this time – honest!).  As it turned out, the solution was pretty simple, and one that Pololu, the supplier of the metal-geared motors I am using had already described in some detail in this post.  Along the way I learned a lot, and had a lot of fun, but to quote Abraham Lincoln “I feel like the man who was tarred and feathered and ridden out of town on a rail. To the man who asked him how he liked it, he said: ‘If it wasn’t for the honor of the thing, I’d rather walk.” 

In any case, the MPU6050 IMU on my little 2-motor robot with metal-geared motors is now happily churning out yaw values on a polled basis (no interrupt required, thank you!), and I’m back in business with angle-enhanced wall tracking.  Once I got everything working again, I ran some tests on my ‘local field test site’ (AKA my office) to verify that the algorithm still worked and I was still getting good tracking behavior.  I have included some Excel plots and a short video showing the results.

 

Stay tuned,

Frank