I have been running ‘field’ (i.e. in my house) tests with WallE3, my autonomous wall-following robot. Unfortunately, WallE3 has demonstrated an unfortunate tendency to lose its mind and start spinning out of control – “around and around the robot goes, and when he stops, nobody knows!”. After a number of trials where this happened, I realized I’m going to have to figure out how to detect this condition so I can get WallE3 to recover properly.
Fortunately, WallE3 knows its relative heading, thanks to its onboard GY-521 MPU-6050 3 Axis Accelerometer/Gyroscope. So, I thought I should be able to detect the ‘spinning’ condition by monitoring the relative heading numbers; if the relative heading values traversed a full 360⁰ within a reasonably short period of time like 3-5 sec, then the robot should be stopped and a recovery algorithm of some sort implemented.
As usual, a seemingly simple algorithm turns out to not be quite as simple as it seems at first glance. The first thing I tried to do was to use my new robot telemetry visualization tool to go back through my recorded telemetry files to find some runs where spinning occurred. Unfortunately, I couldn’t find any – bummer! Not to worry, I decide I could use Excel to ‘invent’ a spinning event by generating a series of monotonically increasing heading values. Then I used Excel and VBA to work out an algorithm for ‘help, I’m spinning’ detection. Shown below is a screenshot of the Excel spreadsheet, and a screenshot of the VBA code that does the detection.
Now to see if this idea actually works ‘in the wild’ (or at least ‘in the robot’)
13 June 2023 Update:
I wanted to capture data from a real ‘spinning’ event to further test the above algorithm, so naturally WallE3 has refused to cooperate, even after several trial runs. So, being the sneaky person I am, I decided to add ‘#define HEADINGS_ONLY’ and associated code section to WallE3’s code base, so I can capture heading date while manually spinning the robot. This worked well, and because it is in a #define block, it gets compiled out for normal operations. After getting that working, I captured a bunch of heading data and dropped it into my Excel setup to see how my VBA code worked with ‘real’ data. As it turned out, this exposed a bug in the algorithm – I had forgotten to handle the case where the cumulative heading is negative, but with a magnitude greater than 360. The fix was to compare the absolute value to 360, as shown in the revised code below:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 |
Option Explicit Sub Button1_Click() '06/09/23 another try at detecting robot spin anomaly 'The plan is to look at delta between current and previous heading. 'if the cumulative heading change becomes greater than 360deg, then 'the robot is spinning Dim currHdg, prevHdg, cumHdg, deltaHdg As Double Dim i As Integer Dim numHdgs As Long 'Find the last non-blank cell in column A(1) numHdgs = Cells(Rows.Count, 1).End(xlUp).Row For i = 3 To numHdgs currHdg = Cells(i - 1, 2).Value deltaHdg = Cells(i, 2).Value - currHdg If deltaHdg > 180 Then deltaHdg = deltaHdg - 360 ' End If ElseIf deltaHdg < -180 Then deltaHdg = deltaHdg + 360 End If Cells(i, 3).Value = deltaHdg cumHdg = deltaHdg + Cells(i - 1, 4).Value Cells(i, 4) = cumHdg If i = 74 Then Debug.Print ("i = 74") End If ' If cumHdg >= 360 Then If Abs(cumHdg) >= 360 Then MsgBox ("Hey! The robot is spinning out of control at line " & i & "!!!") Cells(i, 5) = "<<<< HERE!!!" cumHdg = 0 Cells(i, 4) = cumHdg End If Next i End Sub |
Stay tuned,
Frank