Tag Archives: Wall Tracking

Help! I’m Spinning out of Control!!

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:

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

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

C# Drawing Text In Window with Y-up Transform

1 Reply

Posted 02 April 2023,

In a previous post I described my effort to use ‘Processsing’ to graphically depict the wall-following behavior of WallE3 my autonomous wall-following robot. This worked ‘ok’ (lower case ‘OK’), but with some significant issues that prompted me to try again using C#.Net. I have done quite a bit of work in C#, so I was pretty sure I could make something useful. However, I almost immediately ran into a problem that turned out to be non-trivial (at least to me) to solve.

The problem was that I wanted to use a traditional engineering/scientific coordinate system, with the origin at the lower left-hand corner of the viewing area, with x increasing to the right and y increasing upwards. Unfortunately, the default system in Windows has the origin at the top left-hand corner with x increasing to the right and y increasing downwards. Should be a piece of cake, right?

Well, it is, and it isn’t. Flipping the y-increase direction and moving the origin to bottom-left wasn’t that bad, but then I discovered that if you wish to draw some text (like ‘x’ and ‘y’ at the ends of coordinate axis marker lines), the ‘y’ shows up flipped vertically (the ‘x’ is also vertically flipped, but a vertically flipped ‘x’ is….. ‘x’ 😉).

So, I bumbled around in Google-land for a while and ran across a post where someone else (Andrew Norton, I think) was having (and had ‘solved’) the same issue. Here is his solution:

class RectangleWithText
{
    RectangleF m_extent = new RectangleF();
    string m_text = "";

    Font m_textFont = null;
    RectangleF m_textRect = new RectangleF();

    public RectangleWithText( RectangleF extent, string text )
    {
        m_extent = extent;
        m_text = text;
    }

    public void Draw( Graphics g )
    {
        var dashedGrayPen = new Pen( Color.Gray, 1.0f / g.DpiX ) { DashStyle = DashStyle.Dash };
        var brownPen = new Pen( Color.Brown, 1.0f / g.DpiX );

        // Draw rectangle itself
        g.DrawRectangle( brownPen, m_extent.X, m_extent.Y, m_extent.Width, m_extent.Height );

        // Draw text on it
        var extentCenter = new PointF( ( m_extent.Left + m_extent.Right ) / 2, ( m_extent.Bottom + m_extent.Top ) / 2 );
        DrawText( g, m_text, extentCenter, m_extent );

        }
    }

    private void DrawText( Graphics g, string text, PointF ptStart, RectangleF extent )
    {
        var gs = g.Save();

        // Inverse Y axis again - now it grow down;
        // if we don't do this, text will be drawn inverted
        g.ScaleTransform( 1.0f, -1.0f, MatrixOrder.Prepend );

        if ( m_textFont == null )
        {
            // Find the maximum appropriate text size to fix the extent
            float fontSize = 100.0f;
            Font fnt;
            SizeF textSize;
            do
            {
                fnt = new Font( "Arial", fontSize / g.DpiX, FontStyle.Bold, GraphicsUnit.Pixel );
                textSize = g.MeasureString( text, fnt );
                m_textRect = new RectangleF( new PointF( ptStart.X - textSize.Width / 2.0f, -ptStart.Y - textSize.Height / 2.0f ), textSize );

                var textRectInv = new RectangleF( m_textRect.X, -m_textRect.Y, m_textRect.Width, m_textRect.Height );
                if ( extent.Contains( textRectInv ) )
                    break;

                fontSize -= 1.0f;
                if ( fontSize <= 0 )
                {
                    fontSize = 1.0f;
                    break;
                }
            } while ( true );

            m_textFont = fnt;
        }

        // Create a StringFormat object with the each line of text, and the block of text centered on the page
        var stringFormat = new StringFormat()
        {
            Alignment = StringAlignment.Center,
            LineAlignment = StringAlignment.Center
        };
        g.DrawString( text, m_textFont, Brushes.Black, m_textRect, stringFormat );

        g.Restore( gs );
    }
}

class RectangleWithText

{

RectangleF m_extent = new RectangleF();

string m_text = "";

Font m_textFont = null;

RectangleF m_textRect = new RectangleF();

public RectangleWithText( RectangleF extent, string text )

{

m_extent = extent;

m_text = text;

}

public void Draw( Graphics g )

{

var dashedGrayPen = new Pen( Color.Gray, 1.0f / g.DpiX ) { DashStyle = DashStyle.Dash };

var brownPen = new Pen( Color.Brown, 1.0f / g.DpiX );

// Draw rectangle itself

g.DrawRectangle( brownPen, m_extent.X, m_extent.Y, m_extent.Width, m_extent.Height );

// Draw text on it

var extentCenter = new PointF( ( m_extent.Left + m_extent.Right ) / 2, ( m_extent.Bottom + m_extent.Top ) / 2 );

DrawText( g, m_text, extentCenter, m_extent );

}

private void DrawText( Graphics g, string text, PointF ptStart, RectangleF extent )

{

var gs = g.Save();

// Inverse Y axis again - now it grow down;

// if we don't do this, text will be drawn inverted

g.ScaleTransform( 1.0f, -1.0f, MatrixOrder.Prepend );

if ( m_textFont == null )

{

// Find the maximum appropriate text size to fix the extent

float fontSize = 100.0f;

Font fnt;

SizeF textSize;

{

fnt = new Font( "Arial", fontSize / g.DpiX, FontStyle.Bold, GraphicsUnit.Pixel );

textSize = g.MeasureString( text, fnt );

m_textRect = new RectangleF( new PointF( ptStart.X - textSize.Width / 2.0f, -ptStart.Y - textSize.Height / 2.0f ), textSize );

var textRectInv = new RectangleF( m_textRect.X, -m_textRect.Y, m_textRect.Width, m_textRect.Height );

if ( extent.Contains( textRectInv ) )

break;

fontSize -= 1.0f;

if ( fontSize <= 0 )

{

fontSize = 1.0f;

break;

}

} while ( true );

m_textFont = fnt;

}

// Create a StringFormat object with the each line of text, and the block of text centered on the page

var stringFormat = new StringFormat()

{

Alignment = StringAlignment.Center,

LineAlignment = StringAlignment.Center

};

g.DrawString( text, m_textFont, Brushes.Black, m_textRect, stringFormat );

g.Restore( gs );

}

So I fired up my VS2022 Community edition IDE and played with this for a while, and it worked – sort of. However, it seemed the text sizing and placement was ‘off’, and I couldn’t figure out why. After lots of playing around, I finally worked out what was happening, and was able to boil it down to what I thought was the simplest possible example. I put all the code into the ‘Paint’ event handler for a Windows Form project, as shown below:

    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        private void Form1_Paint(object sender, PaintEventArgs e)
        {
            //Purpose:  Demonstrate right-side up text labels in a drawing window configured for X-right Y-up increasing coordinate system
            //Step1: Change from Y-down to Y-up increasing using
            //      g.ScaleTransform(1.0f, -1.0f);
            //      g.TranslateTransform(0, -this.ClientRectangle.Height);
            //Step2: Draw the required geometry elements
            //Step3: Save the transforms with g.Save()
            //Step4: re-flip the drawing transform back to y-down increasing using
            //      g.ResetTransform(). This puts the origing back to the upper left-hand corner of the window
            //Step5: Draw the text as required, noting the requirement to take into account that Y=0 is at the top of the window, and 
            //             positive y goes down
            //Step6: (optional??) restore the x-right, y-up origin at bottom left transform with
            //      g.RestoreTransform();
            //Step7: Get drunk on your success!


            //Step1: Change from Y-down to Y-up increasing using
            Graphics g = e.Graphics;
            g.ScaleTransform(1.0f, -1.0f);//flip y
            g.TranslateTransform(0, -this.ClientRectangle.Height); //move origin to bottom-left corner
            g.TranslateTransform(100, 100);//move origin to right 100 and up 100 pix

            //draw coordinate system lines in new coordinate system
            //Step2: Draw the required geometry elements
            g.DrawLine(new Pen(Color.Black, 3), 0, 0, 100, 0);
            g.DrawLine(new Pen(Color.Black, 3), 0, 0, 0, 100);

            //Step3: Save the transforms with g.Save()
            GraphicsState transState = g.Save(); //do this so can restore later

            // Create string to draw.
            String drawString = "Sample Text";

            // Create font and brush.
            Font drawFont = new Font("Arial", 16);
            SolidBrush drawBrush = new SolidBrush(Color.Black);

            // Create point for upper-left corner of string (in y-up system).
            float x = 150.0F;
            float y = 50.0F;

            // Set format of string.
            StringFormat drawFormat = new StringFormat();
            drawFormat.Alignment = StringAlignment.Center;
            drawFormat.LineAlignment = StringAlignment.Far;

            // Draw string to screen.
            e.Graphics.DrawString(drawString, drawFont, drawBrush, x, y, drawFormat);//this is the 'inverted/reversed' text

            //Step4: re-flip the drawing transform back to y-down increasing
            //  note here that origin is still at (100, 100) measured x right, y up from bottom lh corner,
            //  so text placement has to take that into account
            g.ScaleTransform(1.0f, -1.0f);


            //Step5: Draw the text as required, noting the requirement to take into account that Y=0 is 100 units up from the bottome of the window,
            //and X = 0 is 100 units right of the left edge of the window, and positive y goes down
            e.Graphics.DrawString(drawString, drawFont, drawBrush, 0, -100, drawFormat); //the vertical coordinate line goes 100 units up from origin

            //Step6: (optional??) restore the x-right, y-up origin at bottom left transform with
            g.Restore(transState);

            //Step7: Get drunk on your success!
        }

    }

public partial class Form1 : Form

{

public Form1()

{

InitializeComponent();

}

private void Form1_Paint(object sender, PaintEventArgs e)

{

//Purpose: Demonstrate right-side up text labels in a drawing window configured for X-right Y-up increasing coordinate system

//Step1: Change from Y-down to Y-up increasing using

// g.ScaleTransform(1.0f, -1.0f);

// g.TranslateTransform(0, -this.ClientRectangle.Height);

//Step2: Draw the required geometry elements

//Step3: Save the transforms with g.Save()

//Step4: re-flip the drawing transform back to y-down increasing using

// g.ResetTransform(). This puts the origing back to the upper left-hand corner of the window

//Step5: Draw the text as required, noting the requirement to take into account that Y=0 is at the top of the window, and

// positive y goes down

//Step6: (optional??) restore the x-right, y-up origin at bottom left transform with

// g.RestoreTransform();

//Step7: Get drunk on your success!

//Step1: Change from Y-down to Y-up increasing using

Graphics g = e.Graphics;

g.ScaleTransform(1.0f, -1.0f);//flip y

g.TranslateTransform(0, -this.ClientRectangle.Height); //move origin to bottom-left corner

g.TranslateTransform(100, 100);//move origin to right 100 and up 100 pix

//draw coordinate system lines in new coordinate system

//Step2: Draw the required geometry elements

g.DrawLine(new Pen(Color.Black, 3), 0, 0, 100, 0);

g.DrawLine(new Pen(Color.Black, 3), 0, 0, 0, 100);

//Step3: Save the transforms with g.Save()

GraphicsState transState = g.Save(); //do this so can restore later

// Create string to draw.

String drawString = "Sample Text";

// Create font and brush.

Font drawFont = new Font("Arial", 16);

SolidBrush drawBrush = new SolidBrush(Color.Black);

// Create point for upper-left corner of string (in y-up system).

float x = 150.0F;

float y = 50.0F;

// Set format of string.

StringFormat drawFormat = new StringFormat();

drawFormat.Alignment = StringAlignment.Center;

drawFormat.LineAlignment = StringAlignment.Far;

// Draw string to screen.

e.Graphics.DrawString(drawString, drawFont, drawBrush, x, y, drawFormat);//this is the 'inverted/reversed' text

//Step4: re-flip the drawing transform back to y-down increasing

// note here that origin is still at (100, 100) measured x right, y up from bottom lh corner,

// so text placement has to take that into account

g.ScaleTransform(1.0f, -1.0f);

//Step5: Draw the text as required, noting the requirement to take into account that Y=0 is 100 units up from the bottome of the window,

//and X = 0 is 100 units right of the left edge of the window, and positive y goes down

e.Graphics.DrawString(drawString, drawFont, drawBrush, 0, -100, drawFormat); //the vertical coordinate line goes 100 units up from origin

//Step6: (optional??) restore the x-right, y-up origin at bottom left transform with

g.Restore(transState);

//Step7: Get drunk on your success!

}

When run, this produces the following output:

Original and flipped/aligned “Sample Text” drawings

In the above figure, the vertically flipped “Sample Text” was drawn after applying the transforms that flipped the y direction and moved the origin to 100,100 with respect to the bottom left-hand corner. The second correctly placed and oriented rendition of “Sample Text” was obtained after implementing steps 4-6 in the above code.

This was pretty cool, but I also wanted to be able to pull in robot telemetry data in Cm and display it in a way that makes sense. I found the ‘Graphics.PageUnit’ method, and I found a small example to show a rectangle drawn with the default ‘pixels’ setting and also with the ‘Point’ setting. I modified this to add the line ‘e.Graphics.PageUnit = GraphicsUnit.Millimeter;’ and got the following:

50w x 100h unit rectangle with top left corner at (20,20) units in pix, points, and mm

According to my trusty digital calipers, the orange ‘mm’ rectangle was very close to 50 x 100 mm (at least on my screen).

So, I *should* be able to combine these two effects and get what I’m after – a screen with the origin at the bottom, left-hand corner and calibrated in mm. My data is actually in cm, but the inherent 10:1 scale factor should work out pretty well, given that I’m working with distances from a few cm to as much as 10m.

03 April 2023 Update:

After a lot of fits and starts, I think I have finally arrived at a drawing algorithm that allows me to use a x-right, y-up coordinate system in which I can draw text correctly (i.e. it doesn’t display upside-down). I posted this in the Stack Overflow thread from a few years ago that gave me my first big clue about how to solve this problem, so hopefully it will help some other poor soul, and I’m also including it below. To use this example, create a Windows .NET form application and enable the ‘Paint’ and ‘ResizeEnd’ handlers (the ‘ResizeEnd’ handler isn’t strictly required, but it allows the user to re-run the example by just resizing the screen slightly). Then replace the contents of the Paint and Resize handlers with the example code, and also paste in the two helper functions in their entirety.

private void Form1_Paint(object sender, PaintEventArgs e)
  {
    //Step1: Change from Y-down to Y-up increasing using
    Graphics g = e.Graphics;

    g.PageUnit = GraphicsUnit.Millimeter; //  04/02/23 Changed Page units from pix to mm

    g.ScaleTransform(1.0f, -1.0f);//flip y
    Rectangle clientrect = this.ClientRectangle; //this is in pixels, NOT mm!

    PointF[] rectpoints = {new PointF(clientrect.X, clientrect.Y),new PointF(clientrect.X, clientrect.Bottom)};

    PointF[] ptf2 = { new PointF(clientrect.X, clientrect.Bottom) }; //have to have an array to use g.TransformPoints()
    g.TransformPoints(CoordinateSpace.Page, CoordinateSpace.Device, ptf2);

    g.TranslateTransform(ptf2[0].X, -ptf2[0].Y); //move origin to bottom-left corner
    g.TranslateTransform(20, 20);//move origin to right 20 and up 20 mm

    //Step2: draw coordinate system lines in new coordinate system
    g.DrawLine(new Pen(Color.Black, 1), 0, 0, 100, 0);
    g.DrawLine(new Pen(Color.Black, 1), 0, 0, 0, 100);

    //Step3: Save the transforms with g.Save()
    GraphicsState transState = g.Save(); //do this so can restore later

    //"Y" string.
    Rectangle Yrect = new Rectangle(-5,100, 10, 10); //this is in mm with origin at (20,20)mm from bottom left-hand corner of window
    StringFormat YdrawFormat = new StringFormat();
    YdrawFormat.Alignment = StringAlignment.Center;
    YdrawFormat.LineAlignment = StringAlignment.Center;

    //"X" string.
    Rectangle Xrect = new Rectangle(100,-5, 10, 10); //this is in mm with origin at (20,20)mm from bottom left-hand corner of window
    StringFormat XdrawFormat = new StringFormat();
    XdrawFormat.Alignment = StringAlignment.Center;
    XdrawFormat.LineAlignment = StringAlignment.Center;

    //now draw desired text
    //bool IsVerbose = true;
    bool IsVerbose = false;
    TextInsideRectangle(e, Yrect, "Y", YdrawFormat, IsVerbose);
    TextInsideRectangle(e, Xrect, "X", XdrawFormat, IsVerbose);
  }
  private void TextInsideRectangle(PaintEventArgs e, Rectangle rect, string text, StringFormat fmt, bool verbose)
  {
    //Purpose:  Place and size the given text inside the given rectangle
    //Inputs:
    //  PaintEventArgs e assumed to be set for y-up, origin at bottom left, mm scaling
    //  ptf = 2-element PointF array containing (rect.X, rect.Y), (rect.Width, rect.Height) in desired coord sys
    //  rect = Rectangle object in (x,y, width,height) format
    //  text = string object containing text to be displayed
    //Procedure:
    //  Step1: Save the current transform
    //  Step2: draw the given rectangle onto the Form surface (DEBUG only)
    //  Step3: Flip y axis so text is drawn correctly
    //  Step3: Draw the given text, centered in the given rectangle
    //  Step4: Iteratively modify the font size such that the given text fits inside the given rectangle
    //  Step5: Refresh the screen and redraw

    //Step1: Save the current transform
    Graphics g = e.Graphics;
    GraphicsState transState = g.Save(); //do this so can restore later

    //Step2: draw the given rectangle onto the Form surface (for debug only)
    if (verbose)
    {
        RectangleF mm_tgt_rect = rect;
        g.DrawRectangle(new Pen(Color.DarkCyan), Rectangle.Round(mm_tgt_rect));
    }

    //Step3: Flip y axis so text is drawn correctly
    g.ScaleTransform(1.0f, -1.0f, MatrixOrder.Prepend); //origin still at bottom left of screen

    //Step3: Draw the given text, centered in the given rectangle
    PointF rect_ctr = new PointF((rect.X + rect.Width/2), -rect.Y - rect.Height/2);

    // Create font and brush.
    int fontsize = 1;
    Font drawFont = new Font("Arial", fontsize);
    SolidBrush drawBrush = new SolidBrush(Color.Black);

    //Step4: Iteratively modify the font size such that the given text fits inside the given rectangle
    SizeF textSize = g.MeasureString(text, drawFont);

    if (verbose)
    {
        System.Diagnostics.Debug.WriteLine("text size is {0} x {1}, enclosing rect size is {2}, {3} ",
          textSize.Width, textSize.Height, rect.Width, rect.Height);
    }

    while (textSize.Width <= rect.Width && textSize.Height <= rect.Height)
    {
        if (verbose)
        {
          System.Diagnostics.Debug.WriteLine("text size of {0} x {1} is smaller than rect", textSize.Width, textSize.Height);
        }
        fontsize++;
        drawFont = new Font("Arial", fontsize);
        textSize = g.MeasureString(text, drawFont);
        e.Graphics.DrawString(text, drawFont, drawBrush, rect_ctr.X, rect_ctr.Y, fmt);//this is the 'inverted/reversed' text
    }

    if (verbose)
    {
        System.Diagnostics.Debug.WriteLine("text size of {0} x {1} is smaller than rect", textSize.Width, textSize.Height);
        System.Diagnostics.Debug.WriteLine("final text size = {0} x {1}: enclosing rect size is {2}, {3}",
          textSize.Width, textSize.Height, rect.Width, rect.Height);
    }

    //Step5: Refresh the screen and redraw
    e.Graphics.DrawString(text, drawFont, drawBrush, rect_ctr.X, rect_ctr.Y, fmt);
  }
  private void Form1_ResizeEnd(object sender, EventArgs e)
  {
    this.Invalidate();
  }
  private PointF[] ConvertRectangleToPointFArray(RectangleF rectF)
  {
    //Purpose: convert a Rectangle or RectangleF object into a 2-element array of PointF objects
    //Inputs:
    //  rectF = Rectangle or RectangleF object
    //Outputs:
    //  returns a 2-element array of PointF objects
    //  points[0] = PointF(rectF.X, rectF.Y)
    //  points[1] = PointF(rectF.Width,rectF.Height)

    PointF[] points = new PointF[2];
    points[0] = new PointF(rectF.X, rectF.Y);
    points[1] = new PointF(rectF.Width,rectF.Height);

    return points;
  }

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

private void Form1_Paint(object sender, PaintEventArgs e)

{

//Step1: Change from Y-down to Y-up increasing using

Graphics g = e.Graphics;

g.PageUnit = GraphicsUnit.Millimeter; // 04/02/23 Changed Page units from pix to mm

g.ScaleTransform(1.0f, -1.0f);//flip y

Rectangle clientrect = this.ClientRectangle; //this is in pixels, NOT mm!

PointF[] rectpoints = {new PointF(clientrect.X, clientrect.Y),new PointF(clientrect.X, clientrect.Bottom)};

PointF[] ptf2 = { new PointF(clientrect.X, clientrect.Bottom) }; //have to have an array to use g.TransformPoints()

g.TransformPoints(CoordinateSpace.Page, CoordinateSpace.Device, ptf2);

g.TranslateTransform(ptf2[0].X, -ptf2[0].Y); //move origin to bottom-left corner

g.TranslateTransform(20, 20);//move origin to right 20 and up 20 mm

//Step2: draw coordinate system lines in new coordinate system

g.DrawLine(new Pen(Color.Black, 1), 0, 0, 100, 0);

g.DrawLine(new Pen(Color.Black, 1), 0, 0, 0, 100);

//Step3: Save the transforms with g.Save()

GraphicsState transState = g.Save(); //do this so can restore later

//"Y" string.

Rectangle Yrect = new Rectangle(-5,100, 10, 10); //this is in mm with origin at (20,20)mm from bottom left-hand corner of window

StringFormat YdrawFormat = new StringFormat();

YdrawFormat.Alignment = StringAlignment.Center;

YdrawFormat.LineAlignment = StringAlignment.Center;

//"X" string.

Rectangle Xrect = new Rectangle(100,-5, 10, 10); //this is in mm with origin at (20,20)mm from bottom left-hand corner of window

StringFormat XdrawFormat = new StringFormat();

XdrawFormat.Alignment = StringAlignment.Center;

XdrawFormat.LineAlignment = StringAlignment.Center;

//now draw desired text

//bool IsVerbose = true;

bool IsVerbose = false;

TextInsideRectangle(e, Yrect, "Y", YdrawFormat, IsVerbose);

TextInsideRectangle(e, Xrect, "X", XdrawFormat, IsVerbose);

}

private void TextInsideRectangle(PaintEventArgs e, Rectangle rect, string text, StringFormat fmt, bool verbose)

{

//Purpose: Place and size the given text inside the given rectangle

//Inputs:

// PaintEventArgs e assumed to be set for y-up, origin at bottom left, mm scaling

// ptf = 2-element PointF array containing (rect.X, rect.Y), (rect.Width, rect.Height) in desired coord sys

// rect = Rectangle object in (x,y, width,height) format

// text = string object containing text to be displayed

//Procedure:

// Step1: Save the current transform

// Step2: draw the given rectangle onto the Form surface (DEBUG only)

// Step3: Flip y axis so text is drawn correctly

// Step3: Draw the given text, centered in the given rectangle

// Step4: Iteratively modify the font size such that the given text fits inside the given rectangle

// Step5: Refresh the screen and redraw

//Step1: Save the current transform

Graphics g = e.Graphics;

GraphicsState transState = g.Save(); //do this so can restore later

//Step2: draw the given rectangle onto the Form surface (for debug only)

if (verbose)

{

RectangleF mm_tgt_rect = rect;

g.DrawRectangle(new Pen(Color.DarkCyan), Rectangle.Round(mm_tgt_rect));

}

//Step3: Flip y axis so text is drawn correctly

g.ScaleTransform(1.0f, -1.0f, MatrixOrder.Prepend); //origin still at bottom left of screen

//Step3: Draw the given text, centered in the given rectangle

PointF rect_ctr = new PointF((rect.X + rect.Width/2), -rect.Y - rect.Height/2);

// Create font and brush.

int fontsize = 1;

Font drawFont = new Font("Arial", fontsize);

SolidBrush drawBrush = new SolidBrush(Color.Black);

//Step4: Iteratively modify the font size such that the given text fits inside the given rectangle

SizeF textSize = g.MeasureString(text, drawFont);

if (verbose)

{

System.Diagnostics.Debug.WriteLine("text size is {0} x {1}, enclosing rect size is {2}, {3} ",

textSize.Width, textSize.Height, rect.Width, rect.Height);

}

while (textSize.Width <= rect.Width && textSize.Height <= rect.Height)

{

if (verbose)

{

System.Diagnostics.Debug.WriteLine("text size of {0} x {1} is smaller than rect", textSize.Width, textSize.Height);

}

fontsize++;

drawFont = new Font("Arial", fontsize);

textSize = g.MeasureString(text, drawFont);

e.Graphics.DrawString(text, drawFont, drawBrush, rect_ctr.X, rect_ctr.Y, fmt);//this is the 'inverted/reversed' text

}

if (verbose)

{

System.Diagnostics.Debug.WriteLine("text size of {0} x {1} is smaller than rect", textSize.Width, textSize.Height);

System.Diagnostics.Debug.WriteLine("final text size = {0} x {1}: enclosing rect size is {2}, {3}",

textSize.Width, textSize.Height, rect.Width, rect.Height);

}

//Step5: Refresh the screen and redraw

e.Graphics.DrawString(text, drawFont, drawBrush, rect_ctr.X, rect_ctr.Y, fmt);

}

private void Form1_ResizeEnd(object sender, EventArgs e)

{

this.Invalidate();

}

private PointF[] ConvertRectangleToPointFArray(RectangleF rectF)

{

//Purpose: convert a Rectangle or RectangleF object into a 2-element array of PointF objects

//Inputs:

// rectF = Rectangle or RectangleF object

//Outputs:

// returns a 2-element array of PointF objects

// points[0] = PointF(rectF.X, rectF.Y)

// points[1] = PointF(rectF.Width,rectF.Height)

PointF[] points = new PointF[2];

points[0] = new PointF(rectF.X, rectF.Y);

points[1] = new PointF(rectF.Width,rectF.Height);

return points;

}

Here are a couple of screenshots of my form after running the example code. The first image shows the default Windows form size, with the top portion (and the ‘Y’ label) cut off. The second image shows the situation after resizing the form down a bit, allowing the ‘ResizeEnd’ handler to force the program to re-run and re-draw.

Default Form1 size doesn’t show top part of ‘Y’ coordinate line

Screenshot after dragging the bottom of the form down

More ‘WallE3_Complete_V2’ Testing

1 Reply

Posted 13 March 2023,

Now that I have the new Garmin LIDAR installed, It’s time to return to ‘real world’ testing. Here’s a short video of a run starting in our entry hallway and proceeding past two open doorways into our dining/living area:

As can be seen in the video, WallE3 handles the oblique turn to the left and the two open doorways perfectly, but then loses it’s way when (I think) the right-hand wall disappears entirely. Not sure what happened there. Here’s the telemetry from the entire run:

Msec	LCen	RCen	Front	Rear	FVar	RVar
14216	36.0	84.0	459	31.0	61316.5	713.9
14316	36.0	85.0	459	36.0	57641.1	677.7
14435	37.0	82.0	455	38.0	51058.0	643.3
14516	36.0	79.0	456	37.0	48133.6	612.2
14675	36.0	90.0	450	43.0	43071.8	580.7
14716	36.0	90.0	452	49.0	40894.0	549.9
14816	35.0	97.0	450	56.0	38961.2	520.6
14944	35.0	89.0	434	62.0	35959.5	494.0
15016	36.0	84.0	439	61.0	34760.5	468.6
15137	36.0	87.0	425	65.0	33153.4	445.7
15216	37.0	90.0	429	76.0	32610.9	429.6
15316	37.0	117.0	426	82.0	32262.8	418.8
15456	37.0	200.0	408	89.0	32288.2	415.1
15516	37.0	200.0	414	83.0	32460.1	405.2
15680	38.0	200.0	398	100.0	33358.1	415.1
15716	37.0	200.0	403	105.0	33888.5	431.4
15816	37.0	114.0	399	98.0	34500.8	436.2
15981	37.0	84.0	655	107.0	34627.8	453.1
16016	37.0	84.0	569	117.0	34634.2	485.9
16180	37.0	86.0	1000	126.0	41132.9	534.7
16216	37.0	95.0	856	117.0	42355.1	562.4
16316	36.0	200.0	952	200.0	44625.4	868.5
16444	36.0	200.0	511	200.0	44771.2	1158.2
16516	35.0	200.0	658	200.0	44567.3	1431.9
16647	34.0	200.0	499	200.0	44093.8	1690.3
16716	35.0	200.0	552	200.0	43435.6	1933.8
16816	33.0	200.0	516	200.0	42648.3	2163.0
16948	33.0	200.0	488	200.0	40456.0	2378.5
17016	33.0	200.0	497	200.0	38859.6	2580.8
17144	31.0	200.0	472	200.0	34694.3	2770.4
17216	31.0	200.0	480	200.0	31945.2	2947.7
17316	30.0	200.0	474	200.0	28734.2	3113.3
17480	30.0	200.0	529	200.0	20677.9	3267.7
17516	30.0	200.0	510	200.0	20615.4	3411.3
17679	30.0	200.0	684	200.0	21575.8	3544.7
17716	30.0	200.0	626	200.0	21700.8	3668.1
17816	30.0	200.0	664	200.0	21976.4	3782.2
17931	30.0	200.0	281	200.0	24192.7	3887.3
18016	32.0	200.0	408	200.0	24363.6	3983.9
18126	31.0	200.0	277	200.0	26523.7	4072.4
18216	32.0	200.0	320	200.0	27192.5	4153.1
18316	33.0	200.0	291	200.0	28089.1	4226.5
18426	33.0	200.0	258	200.0	30374.3	4293.0
18516	34.0	200.0	269	200.0	31355.0	4352.9
18629	34.0	200.0	247	200.0	33650.6	4406.7
18716	34.0	200.0	254	200.0	34675.4	4454.6
18816	34.0	200.0	249	200.0	35732.5	4497.0
18980	34.0	200.0	1000	200.0	45655.2	4534.3
19016	33.0	200.0	749	200.0	46579.3	4369.1
19180	33.0	200.0	381	200.0	46817.1	4163.8
19216	33.0	200.0	503	200.0	46549.5	3935.7
19316	33.0	200.0	421	200.0	46475.0	3708.8
19465	32.0	200.0	384	200.0	46474.7	3470.1
19516	32.0	200.0	396	200.0	46286.2	3205.4
19670	30.0	200.0	370	200.0	41748.6	2949.3
19716	31.0	200.0	378	200.0	36596.5	2702.4
19816	30.0	112.0	372	200.0	33825.3	2470.2
19981	29.0	99.0	368	200.0	29234.2	2248.6
20016	29.0	99.0	369	200.0	29291.3	2007.3
20180	29.0	105.0	960	200.0	38590.7	1768.8
20216	30.0	105.0	763	200.0	40327.4	1564.3
20316	29.0	109.0	894	200.0	43752.9	1372.4
20480	30.0	118.0	1000	200.0	54303.4	1197.2
20516	30.0	117.0	964	200.0	58593.1	987.9
20656	29.0	200.0	147	200.0	63526.8	834.3
20716	31.0	200.0	419	200.0	63624.2	690.5
20816	30.0	200.0	237	200.0	64887.5	513.7
20925	31.0	200.0	133	200.0	69615.9	362.0
21016	31.0	200.0	167	200.0	71348.9	237.4
21124	32.0	200.0	121	200.0	74684.3	135.0
21216	32.0	200.0	136	200.0	75331.8	0.0
21316	33.0	200.0	126	200.0	76393.0	0.0
21427	36.0	200.0	106	200.0	78663.8	0.0
21516	42.0	200.0	112	200.0	80091.6	0.0
21680	44.0	100.0	874	200.0	88066.8	0.0
21716	44.0	95.0	620	200.0	88310.9	0.0
21816	44.0	84.0	789	200.0	90480.9	0.0
21950	41.0	70.0	268	200.0	90559.5	0.0
22016	40.0	67.0	441	200.0	89859.8	0.0
22180	42.0	63.0	842	200.0	94125.1	0.0
22216	40.0	64.0	708	200.0	94173.2	0.0
22316	40.0	63.0	797	200.0	95054.4	0.0
22480	42.0	63.0	975	200.0	97893.1	0.0
22516	41.0	63.0	915	200.0	96349.6	0.0
22679	41.0	62.0	804	200.0	98380.4	0.0
22716	42.0	62.0	841	200.0	100113.0	0.0
22816	42.0	61.0	816	200.0	101802.3	0.0
22925	43.0	63.0	224	200.0	104800.0	0.0
23016	42.0	200.0	421	200.0	104624.7	0.0
23124	43.0	200.0	101	200.0	110791.7	0.0
23216	40.0	200.0	207	200.0	112225.4	0.0
23316	38.0	200.0	136	200.0	114619.6	0.0
23428	33.0	200.0	266	200.0	116132.4	0.0
23516	32.0	200.0	222	200.0	117308.0	0.0
23680	33.0	200.0	798	200.0	116311.5	0.0
23716	33.0	200.0	606	200.0	112256.7	0.0
23816	34.0	83.0	734	200.0	111960.1	0.0
23971	36.0	200.0	322	200.0	104123.7	0.0
24016	37.0	200.0	459	200.0	98337.4	0.0
24175	38.0	200.0	645	200.0	92677.5	0.0
24216	38.0	200.0	583	200.0	90939.8	0.0
24316	39.0	200.0	624	200.0	91353.5	0.0
24479	39.0	200.0	823	200.0	92171.1	0.0
24516	39.0	200.0	756	200.0	90667.2	0.0
24680	40.0	200.0	796	200.0	87823.2	0.0
24716	39.0	200.0	782	200.0	85383.6	0.0
24816	40.0	200.0	791	200.0	82935.7	0.0
24967	39.0	200.0	701	200.0	75131.1	0.0
25016	39.0	200.0	731	200.0	70765.3	0.0
25161	39.0	200.0	665	200.0	64631.1	0.0
25216	39.0	200.0	687	200.0	63313.8	0.0
25316	39.0	200.0	672	200.0	63403.2	0.0
25458	39.0	200.0	621	200.0	60461.9	0.0
25516	39.0	200.0	638	200.0	58124.0	0.0
25664	38.0	200.0	584	200.0	56519.6	0.0
25716	37.0	200.0	602	200.0	55444.0	0.0
25816	37.0	200.0	590	200.0	55242.8	0.0
25957	36.0	71.0	619	200.0	51665.6	0.0
26016	35.0	72.0	609	200.0	48720.9	0.0
26165	37.0	71.0	737	200.0	46505.4	0.0
26216	200.0	71.0	694	200.0	45767.0	0.0
26316	200.0	70.0	722	200.0	44802.7	0.0
In HandleAnomalousConditions with OPEN_DOORWAY error code detected
ANOMALY_OPEN_DOORWAY case detected

26323:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.99
IRHomingValTotalAvg = 91 
26338: gl_LeftCenterCm = 200.00 gl_RightCenterCm = 70.00
gl_LeftCenterCm > gl_RightCenterCm --> Calling TrackRightWallOffset(70)

TrackRightWallOffset(350.0, 0.0, 0.0, 70) called

TrackRightWallOffset: Start tracking offset of 70cm

Msec	26457	200.0	68.0	711	200.0	41847.9	0.0
26557	200.0	68.0	716	200.0	39478.4	0.0
26693	200.0	69.0	597	200.0	33803.1	0.0
26757	200.0	68.0	636	200.0	28547.0	0.0
26895	200.0	68.0	586	200.0	20239.8	0.0
26957	200.0	68.0	602	200.0	17479.2	0.0
27057	200.0	68.0	591	200.0	14651.6	0.0
27196	200.0	70.0	554	200.0	10917.9	0.0
27257	200.0	70.0	566	200.0	10608.9	0.0
27397	200.0	70.0	580	200.0	10586.1	0.0
27457	94.0	70.0	575	200.0	8594.3	0.0
27557	63.0	70.0	578	200.0	6542.2	0.0
27703	45.0	68.0	714	200.0	5895.6	0.0
27757	44.0	68.0	668	200.0	5892.7	0.0
27894	45.0	67.0	720	200.0	5871.4	0.0
27957	44.0	68.0	702	200.0	5391.5	0.0
28057	46.0	66.0	714	200.0	4923.0	0.0
28204	47.0	65.0	671	200.0	4355.7	0.0
28257	47.0	65.0	685	200.0	3974.5	0.0
28400	55.0	63.0	687	200.0	3295.5	0.0
28457	59.0	63.0	686	200.0	3269.0	0.0
28557	200.0	62.0	686	200.0	3242.4	0.0
28697	200.0	61.0	644	200.0	3099.0	0.0
28757	200.0	60.0	658	200.0	3094.8	0.0
28887	200.0	59.0	584	200.0	3191.9	0.0
28957	200.0	60.0	608	200.0	3206.6	0.0
29057	200.0	59.0	592	200.0	3248.1	0.0
29179	200.0	60.0	409	200.0	5286.8	0.0
29257	200.0	59.0	470	200.0	5769.1	0.0
29375	200.0	62.0	212	200.0	12524.8	0.0
29457	200.0	62.0	298	200.0	14509.5	0.0
29557	200.0	64.0	240	200.0	17186.3	0.0
29667	200.0	68.0	141	200.0	24880.8	0.0
29757	200.0	68.0	174	200.0	27580.2	0.0
29869	200.0	70.0	154	200.0	33230.3	0.0
29957	200.0	72.0	160	200.0	35536.5	0.0
30057	200.0	71.0	156	200.0	37698.5	0.0
30173	200.0	73.0	352	200.0	37922.3	0.0
30257	40.0	73.0	286	200.0	38775.1	0.0
30384	36.0	74.0	519	200.0	38309.2	0.0
30457	38.0	74.0	441	200.0	38241.4	0.0
30557	38.0	73.0	493	200.0	38031.1	0.0
30683	41.0	72.0	578	200.0	38051.3	0.0
30757	41.0	72.0	549	200.0	38040.7	0.0
30891	41.0	70.0	423	200.0	38006.3	0.0
30957	41.0	70.0	465	200.0	37862.1	0.0
31057	42.0	69.0	437	200.0	37766.2	0.0
31181	42.0	70.0	469	200.0	36536.2	0.0
31257	43.0	70.0	458	200.0	35426.2	0.0
31385	44.0	71.0	504	200.0	33457.2	0.0
31457	45.0	72.0	488	200.0	32168.4	0.0
31557	46.0	72.0	498	200.0	31032.2	0.0
31703	50.0	73.0	532	200.0	29001.2	0.0
31757	51.0	74.0	520	200.0	28127.1	0.0
31873	54.0	73.0	250	200.0	27170.0	0.0
31957	58.0	74.0	340	200.0	25988.4	0.0
32057	59.0	77.0	280	200.0	24978.9	0.0
32172	65.0	77.0	244	200.0	23407.3	0.0
32257	70.0	78.0	256	200.0	22570.1	0.0
32374	75.0	79.0	237	114.0	21148.3	145.0
32457	80.0	79.0	243	110.0	20601.5	297.5
32557	84.0	79.0	239	116.0	19757.4	424.0
32673	90.0	80.0	246	115.0	19102.4	547.9
32757	103.0	80.0	243	106.0	19266.9	695.2
32873	107.0	82.0	219	105.0	19252.1	838.7
32957	113.0	84.0	227	115.0	19177.2	944.0
33057	113.0	82.0	221	118.0	19436.7	1035.2
33172	119.0	86.0	206	122.0	19158.8	1110.7
33257	200.0	93.0	211	200.0	18687.5	1110.7
33370	115.0	114.0	187	200.0	18370.8	1110.7
33457	118.0	96.0	195	200.0	18088.9	1110.7
33557	117.0	103.0	189	200.0	17888.8	1110.7
33666	117.0	200.0	145	200.0	18774.8	1110.7
33758	111.0	200.0	159	200.0	19435.2	1110.7
33872	108.0	200.0	355	200.0	18738.1	1110.7
33957	111.0	200.0	289	98.0	18102.6	1251.0
34057	116.0	200.0	333	94.0	17863.0	1396.6
34204	200.0	200.0	490	115.0	17123.9	1471.0
34257	200.0	200.0	437	200.0	16107.2	1471.0
34365	200.0	200.0	114	200.0	16495.4	1471.0
34457	200.0	200.0	221	200.0	16389.7	1471.0
34557	200.0	200.0	149	200.0	16348.5	1471.0
34665	200.0	200.0	97	200.0	16864.0	1471.0
34757	126.0	200.0	114	200.0	16740.3	1471.0
34865	200.0	200.0	115	200.0	15955.6	1471.0
34957	200.0	200.0	114	200.0	15202.1	1471.0
35057	200.0	85.0	114	200.0	14484.9	1471.0
35173	200.0	85.0	392	200.0	12440.0	1471.0
35257	200.0	80.0	299	200.0	10823.6	1471.0
35375	200.0	77.0	172	200.0	9361.6	1471.0
35457	200.0	80.0	214	200.0	9359.9	1471.0
35557	200.0	85.0	186	200.0	9150.2	1471.0
35665	200.0	86.0	107	200.0	9631.0	1471.0
35757	200.0	88.0	133	200.0	9771.4	1471.0
35865	200.0	90.0	93	200.0	10334.9	1471.0
35957	200.0	91.0	106	200.0	10544.7	1471.0
36057	200.0	93.0	97	200.0	10771.2	1471.0
36165	200.0	96.0	79	200.0	11337.1	1471.0
36257	200.0	97.0	85	200.0	11553.9	1471.0
36365	200.0	96.0	68	200.0	12132.4	1471.0
36457	200.0	95.0	73	200.0	12386.9	1471.0
36557	200.0	96.0	69	200.0	12630.4	1471.0
36666	200.0	97.0	72	200.0	13074.0	1471.0
36757	200.0	100.0	71	200.0	13287.0	1471.0
36866	81.0	100.0	68	200.0	13705.5	1471.0
36957	68.0	97.0	69	200.0	13904.6	1471.0
37057	69.0	100.0	68	200.0	14088.6	1471.0
37175	86.0	99.0	113	200.0	14178.9	1471.0
37257	84.0	101.0	98	200.0	14258.5	1471.0
37365	78.0	102.0	29	200.0	14162.5	1393.9
In HandleAnomalousConditions with OBSTACLE_AHEAD error code detected

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

Msec LCen RCen Front Rear FVar RVar

14216 36.0 84.0 459 31.0 61316.5 713.9

14316 36.0 85.0 459 36.0 57641.1 677.7

14435 37.0 82.0 455 38.0 51058.0 643.3

14516 36.0 79.0 456 37.0 48133.6 612.2

14675 36.0 90.0 450 43.0 43071.8 580.7

14716 36.0 90.0 452 49.0 40894.0 549.9

14816 35.0 97.0 450 56.0 38961.2 520.6

14944 35.0 89.0 434 62.0 35959.5 494.0

15016 36.0 84.0 439 61.0 34760.5 468.6

15137 36.0 87.0 425 65.0 33153.4 445.7

15216 37.0 90.0 429 76.0 32610.9 429.6

15316 37.0 117.0 426 82.0 32262.8 418.8

15456 37.0 200.0 408 89.0 32288.2 415.1

15516 37.0 200.0 414 83.0 32460.1 405.2

15680 38.0 200.0 398 100.0 33358.1 415.1

15716 37.0 200.0 403 105.0 33888.5 431.4

15816 37.0 114.0 399 98.0 34500.8 436.2

15981 37.0 84.0 655 107.0 34627.8 453.1

16016 37.0 84.0 569 117.0 34634.2 485.9

16180 37.0 86.0 1000 126.0 41132.9 534.7

16216 37.0 95.0 856 117.0 42355.1 562.4

16316 36.0 200.0 952 200.0 44625.4 868.5

16444 36.0 200.0 511 200.0 44771.2 1158.2

16516 35.0 200.0 658 200.0 44567.3 1431.9

16647 34.0 200.0 499 200.0 44093.8 1690.3

16716 35.0 200.0 552 200.0 43435.6 1933.8

16816 33.0 200.0 516 200.0 42648.3 2163.0

16948 33.0 200.0 488 200.0 40456.0 2378.5

17016 33.0 200.0 497 200.0 38859.6 2580.8

17144 31.0 200.0 472 200.0 34694.3 2770.4

17216 31.0 200.0 480 200.0 31945.2 2947.7

17316 30.0 200.0 474 200.0 28734.2 3113.3

17480 30.0 200.0 529 200.0 20677.9 3267.7

17516 30.0 200.0 510 200.0 20615.4 3411.3

17679 30.0 200.0 684 200.0 21575.8 3544.7

17716 30.0 200.0 626 200.0 21700.8 3668.1

17816 30.0 200.0 664 200.0 21976.4 3782.2

17931 30.0 200.0 281 200.0 24192.7 3887.3

18016 32.0 200.0 408 200.0 24363.6 3983.9

18126 31.0 200.0 277 200.0 26523.7 4072.4

18216 32.0 200.0 320 200.0 27192.5 4153.1

18316 33.0 200.0 291 200.0 28089.1 4226.5

18426 33.0 200.0 258 200.0 30374.3 4293.0

18516 34.0 200.0 269 200.0 31355.0 4352.9

18629 34.0 200.0 247 200.0 33650.6 4406.7

18716 34.0 200.0 254 200.0 34675.4 4454.6

18816 34.0 200.0 249 200.0 35732.5 4497.0

18980 34.0 200.0 1000 200.0 45655.2 4534.3

19016 33.0 200.0 749 200.0 46579.3 4369.1

19180 33.0 200.0 381 200.0 46817.1 4163.8

19216 33.0 200.0 503 200.0 46549.5 3935.7

19316 33.0 200.0 421 200.0 46475.0 3708.8

19465 32.0 200.0 384 200.0 46474.7 3470.1

19516 32.0 200.0 396 200.0 46286.2 3205.4

19670 30.0 200.0 370 200.0 41748.6 2949.3

19716 31.0 200.0 378 200.0 36596.5 2702.4

19816 30.0 112.0 372 200.0 33825.3 2470.2

19981 29.0 99.0 368 200.0 29234.2 2248.6

20016 29.0 99.0 369 200.0 29291.3 2007.3

20180 29.0 105.0 960 200.0 38590.7 1768.8

20216 30.0 105.0 763 200.0 40327.4 1564.3

20316 29.0 109.0 894 200.0 43752.9 1372.4

20480 30.0 118.0 1000 200.0 54303.4 1197.2

20516 30.0 117.0 964 200.0 58593.1 987.9

20656 29.0 200.0 147 200.0 63526.8 834.3

20716 31.0 200.0 419 200.0 63624.2 690.5

20816 30.0 200.0 237 200.0 64887.5 513.7

20925 31.0 200.0 133 200.0 69615.9 362.0

21016 31.0 200.0 167 200.0 71348.9 237.4

21124 32.0 200.0 121 200.0 74684.3 135.0

21216 32.0 200.0 136 200.0 75331.8 0.0

21316 33.0 200.0 126 200.0 76393.0 0.0

21427 36.0 200.0 106 200.0 78663.8 0.0

21516 42.0 200.0 112 200.0 80091.6 0.0

21680 44.0 100.0 874 200.0 88066.8 0.0

21716 44.0 95.0 620 200.0 88310.9 0.0

21816 44.0 84.0 789 200.0 90480.9 0.0

21950 41.0 70.0 268 200.0 90559.5 0.0

22016 40.0 67.0 441 200.0 89859.8 0.0

22180 42.0 63.0 842 200.0 94125.1 0.0

22216 40.0 64.0 708 200.0 94173.2 0.0

22316 40.0 63.0 797 200.0 95054.4 0.0

22480 42.0 63.0 975 200.0 97893.1 0.0

22516 41.0 63.0 915 200.0 96349.6 0.0

22679 41.0 62.0 804 200.0 98380.4 0.0

22716 42.0 62.0 841 200.0 100113.0 0.0

22816 42.0 61.0 816 200.0 101802.3 0.0

22925 43.0 63.0 224 200.0 104800.0 0.0

23016 42.0 200.0 421 200.0 104624.7 0.0

23124 43.0 200.0 101 200.0 110791.7 0.0

23216 40.0 200.0 207 200.0 112225.4 0.0

23316 38.0 200.0 136 200.0 114619.6 0.0

23428 33.0 200.0 266 200.0 116132.4 0.0

23516 32.0 200.0 222 200.0 117308.0 0.0

23680 33.0 200.0 798 200.0 116311.5 0.0

23716 33.0 200.0 606 200.0 112256.7 0.0

23816 34.0 83.0 734 200.0 111960.1 0.0

23971 36.0 200.0 322 200.0 104123.7 0.0

24016 37.0 200.0 459 200.0 98337.4 0.0

24175 38.0 200.0 645 200.0 92677.5 0.0

24216 38.0 200.0 583 200.0 90939.8 0.0

24316 39.0 200.0 624 200.0 91353.5 0.0

24479 39.0 200.0 823 200.0 92171.1 0.0

24516 39.0 200.0 756 200.0 90667.2 0.0

24680 40.0 200.0 796 200.0 87823.2 0.0

24716 39.0 200.0 782 200.0 85383.6 0.0

24816 40.0 200.0 791 200.0 82935.7 0.0

24967 39.0 200.0 701 200.0 75131.1 0.0

25016 39.0 200.0 731 200.0 70765.3 0.0

25161 39.0 200.0 665 200.0 64631.1 0.0

25216 39.0 200.0 687 200.0 63313.8 0.0

25316 39.0 200.0 672 200.0 63403.2 0.0

25458 39.0 200.0 621 200.0 60461.9 0.0

25516 39.0 200.0 638 200.0 58124.0 0.0

25664 38.0 200.0 584 200.0 56519.6 0.0

25716 37.0 200.0 602 200.0 55444.0 0.0

25816 37.0 200.0 590 200.0 55242.8 0.0

25957 36.0 71.0 619 200.0 51665.6 0.0

26016 35.0 72.0 609 200.0 48720.9 0.0

26165 37.0 71.0 737 200.0 46505.4 0.0

26216 200.0 71.0 694 200.0 45767.0 0.0

26316 200.0 70.0 722 200.0 44802.7 0.0

In HandleAnomalousConditions with OPEN_DOORWAY error code detected

ANOMALY_OPEN_DOORWAY case detected

26323: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.99

IRHomingValTotalAvg = 91

26338: gl_LeftCenterCm = 200.00 gl_RightCenterCm = 70.00

gl_LeftCenterCm > gl_RightCenterCm --> Calling TrackRightWallOffset(70)

TrackRightWallOffset(350.0, 0.0, 0.0, 70) called

TrackRightWallOffset: Start tracking offset of 70cm

Msec 26457 200.0 68.0 711 200.0 41847.9 0.0

26557 200.0 68.0 716 200.0 39478.4 0.0

26693 200.0 69.0 597 200.0 33803.1 0.0

26757 200.0 68.0 636 200.0 28547.0 0.0

26895 200.0 68.0 586 200.0 20239.8 0.0

26957 200.0 68.0 602 200.0 17479.2 0.0

27057 200.0 68.0 591 200.0 14651.6 0.0

27196 200.0 70.0 554 200.0 10917.9 0.0

27257 200.0 70.0 566 200.0 10608.9 0.0

27397 200.0 70.0 580 200.0 10586.1 0.0

27457 94.0 70.0 575 200.0 8594.3 0.0

27557 63.0 70.0 578 200.0 6542.2 0.0

27703 45.0 68.0 714 200.0 5895.6 0.0

27757 44.0 68.0 668 200.0 5892.7 0.0

27894 45.0 67.0 720 200.0 5871.4 0.0

27957 44.0 68.0 702 200.0 5391.5 0.0

28057 46.0 66.0 714 200.0 4923.0 0.0

28204 47.0 65.0 671 200.0 4355.7 0.0

28257 47.0 65.0 685 200.0 3974.5 0.0

28400 55.0 63.0 687 200.0 3295.5 0.0

28457 59.0 63.0 686 200.0 3269.0 0.0

28557 200.0 62.0 686 200.0 3242.4 0.0

28697 200.0 61.0 644 200.0 3099.0 0.0

28757 200.0 60.0 658 200.0 3094.8 0.0

28887 200.0 59.0 584 200.0 3191.9 0.0

28957 200.0 60.0 608 200.0 3206.6 0.0

29057 200.0 59.0 592 200.0 3248.1 0.0

29179 200.0 60.0 409 200.0 5286.8 0.0

29257 200.0 59.0 470 200.0 5769.1 0.0

29375 200.0 62.0 212 200.0 12524.8 0.0

29457 200.0 62.0 298 200.0 14509.5 0.0

29557 200.0 64.0 240 200.0 17186.3 0.0

29667 200.0 68.0 141 200.0 24880.8 0.0

29757 200.0 68.0 174 200.0 27580.2 0.0

29869 200.0 70.0 154 200.0 33230.3 0.0

29957 200.0 72.0 160 200.0 35536.5 0.0

30057 200.0 71.0 156 200.0 37698.5 0.0

30173 200.0 73.0 352 200.0 37922.3 0.0

30257 40.0 73.0 286 200.0 38775.1 0.0

30384 36.0 74.0 519 200.0 38309.2 0.0

30457 38.0 74.0 441 200.0 38241.4 0.0

30557 38.0 73.0 493 200.0 38031.1 0.0

30683 41.0 72.0 578 200.0 38051.3 0.0

30757 41.0 72.0 549 200.0 38040.7 0.0

30891 41.0 70.0 423 200.0 38006.3 0.0

30957 41.0 70.0 465 200.0 37862.1 0.0

31057 42.0 69.0 437 200.0 37766.2 0.0

31181 42.0 70.0 469 200.0 36536.2 0.0

31257 43.0 70.0 458 200.0 35426.2 0.0

31385 44.0 71.0 504 200.0 33457.2 0.0

31457 45.0 72.0 488 200.0 32168.4 0.0

31557 46.0 72.0 498 200.0 31032.2 0.0

31703 50.0 73.0 532 200.0 29001.2 0.0

31757 51.0 74.0 520 200.0 28127.1 0.0

31873 54.0 73.0 250 200.0 27170.0 0.0

31957 58.0 74.0 340 200.0 25988.4 0.0

32057 59.0 77.0 280 200.0 24978.9 0.0

32172 65.0 77.0 244 200.0 23407.3 0.0

32257 70.0 78.0 256 200.0 22570.1 0.0

32374 75.0 79.0 237 114.0 21148.3 145.0

32457 80.0 79.0 243 110.0 20601.5 297.5

32557 84.0 79.0 239 116.0 19757.4 424.0

32673 90.0 80.0 246 115.0 19102.4 547.9

32757 103.0 80.0 243 106.0 19266.9 695.2

32873 107.0 82.0 219 105.0 19252.1 838.7

32957 113.0 84.0 227 115.0 19177.2 944.0

33057 113.0 82.0 221 118.0 19436.7 1035.2

33172 119.0 86.0 206 122.0 19158.8 1110.7

33257 200.0 93.0 211 200.0 18687.5 1110.7

33370 115.0 114.0 187 200.0 18370.8 1110.7

33457 118.0 96.0 195 200.0 18088.9 1110.7

33557 117.0 103.0 189 200.0 17888.8 1110.7

33666 117.0 200.0 145 200.0 18774.8 1110.7

33758 111.0 200.0 159 200.0 19435.2 1110.7

33872 108.0 200.0 355 200.0 18738.1 1110.7

33957 111.0 200.0 289 98.0 18102.6 1251.0

34057 116.0 200.0 333 94.0 17863.0 1396.6

34204 200.0 200.0 490 115.0 17123.9 1471.0

34257 200.0 200.0 437 200.0 16107.2 1471.0

34365 200.0 200.0 114 200.0 16495.4 1471.0

34457 200.0 200.0 221 200.0 16389.7 1471.0

34557 200.0 200.0 149 200.0 16348.5 1471.0

34665 200.0 200.0 97 200.0 16864.0 1471.0

34757 126.0 200.0 114 200.0 16740.3 1471.0

34865 200.0 200.0 115 200.0 15955.6 1471.0

34957 200.0 200.0 114 200.0 15202.1 1471.0

35057 200.0 85.0 114 200.0 14484.9 1471.0

35173 200.0 85.0 392 200.0 12440.0 1471.0

35257 200.0 80.0 299 200.0 10823.6 1471.0

35375 200.0 77.0 172 200.0 9361.6 1471.0

35457 200.0 80.0 214 200.0 9359.9 1471.0

35557 200.0 85.0 186 200.0 9150.2 1471.0

35665 200.0 86.0 107 200.0 9631.0 1471.0

35757 200.0 88.0 133 200.0 9771.4 1471.0

35865 200.0 90.0 93 200.0 10334.9 1471.0

35957 200.0 91.0 106 200.0 10544.7 1471.0

36057 200.0 93.0 97 200.0 10771.2 1471.0

36165 200.0 96.0 79 200.0 11337.1 1471.0

36257 200.0 97.0 85 200.0 11553.9 1471.0

36365 200.0 96.0 68 200.0 12132.4 1471.0

36457 200.0 95.0 73 200.0 12386.9 1471.0

36557 200.0 96.0 69 200.0 12630.4 1471.0

36666 200.0 97.0 72 200.0 13074.0 1471.0

36757 200.0 100.0 71 200.0 13287.0 1471.0

36866 81.0 100.0 68 200.0 13705.5 1471.0

36957 68.0 97.0 69 200.0 13904.6 1471.0

37057 69.0 100.0 68 200.0 14088.6 1471.0

37175 86.0 99.0 113 200.0 14178.9 1471.0

37257 84.0 101.0 98 200.0 14258.5 1471.0

37365 78.0 102.0 29 200.0 14162.5 1393.9

In HandleAnomalousConditions with OBSTACLE_AHEAD error code detected

Here’s the Excel plot of the run up to the point where the first open doorway is detected. Note this also covers the oblique turn. From the video, the oblique turn occurs at about six seconds from the start, which should put it somewhere in the 20,000 mSec area. However, comparing the video and the plot, it looks like this turn actually occurs at about 21,500 – 22,000 mSec, where the distance to the right-hand wall falls from the max 200 to about 60-70 cm. The turn itself goes very smoothly, and then the first open doorway condition occurs about four seconds later, at 26,200 mSec.

Segment from start to the first ‘open doorway’ detection

This next plot shows the period from the time of the first ‘open doorway’ detection to the end of the run, including the time where the robot passes the second open doorway (apparently without detecting it).

Segment from the first ‘open doorway’ detection to the end of the run

The left-hand wall distance starts out at 200 (max distance) due to the first open doorway. During this period the robot is tracking the right-hand wall (kitchen counter). When the left-hand distance returns to normal at about 27500 mSec, the robot continues to track the right-hand wall. Apparently the very short section of wall between the first and second doorways wasn’t enough to trigger the ‘open doorway’ condition. However, when the left-hand wall distance comes back down again after the second open doorway (at approximately 30400 mSec, the robot should have reverted to left-wall tracking, but it obviously didn’t. Soon thereafter both the left and right-hand distances started increasing to max values and the poor little robot lost its way – so sad!

After looking through the data and the code, I began to see that although the code could detect the ‘open doorway’ condition, it wasn’t smart enough to detect the end of the ‘open doorway’, so the robot continued to track the right-hand wall – “to infinity and beyond!”. After making some changes to fix the problem, I mad a run in my test range (aka my office) to test the changes. Here’s a photo of the setup:

The test wall are set up so the ‘wrong side’ wall starts before the open doorway, and ends after it, and the distance between the two walls was set such that the measured distance to either side would be less than MAX_TRACKING_DISTANCE_CM (100 cm).

Here’s the telemetry from the run, with an additional column added to show the current ANOMALY CODE:

Opening port
Port open
gl_pSerPort now points to active Serial (USB or Wixel)
7950: Starting setup() for WallE3_Complete_V2.ino

Checking for MPU6050 IMU at I2C Addr 0x68
MPU6050 connection successful
Initializing DMP...
Enabling DMP...
DMP ready! Waiting for MPU6050 drift rate to settle...
Calibrating...Retrieving Calibration Values


Msec	Hdg
10531	-0.054	-0.042
MPU6050 Ready at 10.53 Sec with delta = -0.013
Checking for Teensy 3.5 VL53L0X Controller at I2C addr 0x20
Teensy available at 11632 with gl_bVL53L0X_TeensyReady = 1. Waiting for Teensy setup() to finish
11635: got 1 from VL53L0X Teensy
Teensy setup() finished at 11737 mSec
VL53L0X Teensy Ready at 11739

Checking for Garmin LIDAR at Wire2 I2C addr 0x62
LIDAR Responded to query at Wire2 address 0x62!
Setting LIDAR acquisition repeat value to 0x02 for <100mSec measurement repeat delay
Initializing Front Distance Array...Done
Initializing Rear Distance Array...Done

Checking for Teensy 3.2 IRDET Controller at I2C addr 0x8
11784: IRDET Teensy Not Avail...
IRDET Teensy Ready at 11886
Fin1/Fin2/SteeringVal = 76	60	-0.6875
11888: Initializing IR Beam Value Averaging Array...Done
11891: Initializing IR Beam Value Averaging Array...Done
Battery Voltage = 7.96

14098:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.96
IRHomingValTotalAvg = 30 
14127: gl_LeftCenterCm = 16.00 gl_RightCenterCm = 48.00

TrackLeftWallOffset(350.0, 0.0, 0.0, 40) called

TrackLeftWallOffset: Start tracking offset of 40cm

Msec	LCen	RCen	Front	Rear	FVar	RVar	ANOMALYCODE
14239	16.0	50.0	448	30.0	61469.3	726.4	NONE
14339	16.0	50.0	446	28.0	57852.4	696.7	NONE
14480	16.0	45.0	315	69.0	53127.2	664.8	NONE
14539	16.0	41.0	358	80.0	50844.8	642.7	NONE
14672	24.0	45.0	272	65.0	48742.9	609.6	NONE
14739	29.0	61.0	300	49.0	47804.5	576.7	NONE
14839	36.0	117.0	281	26.0	47322.4	563.0	NONE
14954	39.0	200.0	324	34.0	46199.8	544.0	NONE
15039	39.0	41.0	309	66.0	46108.9	519.5	NONE
15151	39.0	40.0	156	92.0	50430.1	518.1	NONE
15239	37.0	200.0	207	87.0	52053.2	509.8	NONE
15339	38.0	200.0	173	59.0	54277.3	487.0	NONE
15458	37.0	89.0	411	77.0	54262.7	471.1	NONE
15539	37.0	82.0	331	109.0	54945.0	495.2	NONE
15648	37.0	94.0	105	200.0	62127.1	842.4	NONE
15739	38.0	98.0	180	62.0	64470.8	821.9	NONE
15839	36.0	200.0	130	75.0	67450.4	804.0	NONE
15948	34.0	200.0	95	114.0	74016.6	827.9	NONE
16039	32.0	200.0	106	200.0	76787.8	1144.9	NONE
16160	30.0	200.0	372	118.0	76567.9	1166.6	NONE
16239	29.0	99.0	283	93.0	76708.2	1155.7	NONE
16339	28.0	98.0	342	99.0	76245.0	1150.2	NONE
16456	27.0	101.0	405	108.0	74147.9	1154.4	NONE
16539	27.0	102.0	384	200.0	72731.9	1434.3	NONE
16655	27.0	100.0	228	200.0	70382.6	1698.8	NONE
16739	29.0	100.0	280	93.0	68130.6	1683.7	NONE
16839	31.0	100.0	245	86.0	65563.2	1668.3	NONE
16949	34.0	104.0	154	86.0	59923.0	1654.3	NONE
17039	36.0	101.0	184	88.0	55745.3	1642.0	NONE
17149	38.0	100.0	174	200.0	45111.3	1886.6	NONE
17239	38.0	98.0	177	200.0	38459.1	2118.3	NONE
17339	39.0	101.0	175	200.0	30873.0	2337.4	NONE
17455	39.0	102.0	363	200.0	12576.5	2544.4	NONE
17539	40.0	200.0	300	200.0	11969.3	2740.0	NONE
17653	39.0	200.0	332	200.0	10862.4	2924.4	NONE
17739	40.0	200.0	321	200.0	10288.4	3098.2	NONE
17839	40.0	200.0	328	200.0	9703.9	3261.8	NONE
17953	39.0	200.0	290	200.0	9010.9	3415.6	NONE
18039	39.0	59.0	302	200.0	8986.3	3560.0	NONE
18153	38.0	60.0	276	200.0	8802.8	3695.6	NONE
18239	38.0	59.0	284	200.0	8811.6	3822.7	NONE
18339	37.0	57.0	278	200.0	8785.7	3941.6	NONE
18449	36.0	57.0	277	200.0	8704.3	4052.8	NONE
18539	37.0	57.0	277	200.0	8624.2	4156.6	NONE
18649	36.0	56.0	264	200.0	8365.4	4253.5	NONE
18739	37.0	56.0	268	200.0	8151.1	4343.7	NONE
18839	37.0	54.0	265	200.0	8091.9	4427.6	NONE
18949	200.0	53.0	258	200.0	7488.6	4505.7	NONE
19039	200.0	53.0	260	200.0	7027.9	4369.0	NONE
In HandleAnomalousConditions with OPEN_DOORWAY error code detected
ANOMALY_OPEN_DOORWAY case detected

19046:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.90
IRHomingValTotalAvg = 66 
19062: gl_LeftCenterCm = 200.00 gl_RightCenterCm = 53.00
gl_LeftCenterCm > gl_RightCenterCm --> Calling TrackRightWallOffset(53)

TrackRightWallOffset(350.0, 0.0, 0.0, 53) called

TrackRightWallOffset: Start tracking offset of 53cm

Msec	LCen	RCen	Front	Rear	FVar	RVar	ANOMALYCODE
19181	200.0	52.0	258	200.0	6451.5	4041.0	OPEN_DOORWAY
19281	200.0	52.0	258	200.0	5965.8	3841.7	OPEN_DOORWAY
19391	200.0	53.0	216	200.0	5559.7	3781.3	OPEN_DOORWAY
19481	200.0	53.0	230	200.0	4999.9	3740.4	OPEN_DOORWAY
19590	200.0	53.0	225	200.0	3930.4	3640.2	OPEN_DOORWAY
19681	200.0	52.0	226	200.0	3767.1	3462.4	OPEN_DOORWAY
19781	200.0	54.0	225	200.0	3588.4	3154.9	OPEN_DOORWAY
19890	200.0	53.0	199	200.0	3631.8	2865.5	OPEN_DOORWAY
19981	200.0	54.0	207	200.0	3268.5	2701.6	OPEN_DOORWAY
20090	200.0	54.0	185	200.0	2664.1	2615.2	OPEN_DOORWAY
20181	200.0	54.0	192	200.0	2720.8	2503.5	OPEN_DOORWAY
20281	200.0	53.0	187	200.0	2788.3	2270.9	OPEN_DOORWAY
20389	67.0	52.0	179	200.0	2948.3	2099.2	OPEN_DOORWAY
20481	56.0	52.0	181	200.0	2864.5	2022.7	OPEN_DOORWAY
20589	55.0	53.0	139	200.0	3073.5	2022.7	OPEN_DOORWAY
20681	54.0	52.0	153	200.0	3141.1	1764.2	OPEN_DOORWAY
20781	53.0	54.0	143	200.0	3246.5	1549.5	OPEN_DOORWAY
20890	53.0	56.0	128	200.0	3592.9	1461.6	OPEN_DOORWAY
In HandleAnomalousConditions with ANOMALY_TRACKING_WRONG_WALL error code detected
ANOMALY_TRACKING_WRONG_WALL case detected

20899:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.91
IRHomingValTotalAvg = 88 
20915: gl_LeftCenterCm = 54.00 gl_RightCenterCm = 56.00

TrackLeftWallOffset(350.0, 0.0, 0.0, 40) called

TrackLeftWallOffset: Start tracking offset of 40cm

Msec	LCen	RCen	Front	Rear	FVar	RVar	ANOMALYCODE
21027	51.0	58.0	129	200.0	3400.4	1378.8	ANOMALY_TRACKING_WRONG_WALL
21127	49.0	59.0	130	200.0	3504.3	1209.2	ANOMALY_TRACKING_WRONG_WALL
21235	46.0	59.0	124	200.0	3444.7	1052.8	ANOMALY_TRACKING_WRONG_WALL
21327	44.0	59.0	126	200.0	3404.2	919.8	ANOMALY_TRACKING_WRONG_WALL
21435	43.0	60.0	124	200.0	3333.0	919.8	ANOMALY_TRACKING_WRONG_WALL
21527	43.0	61.0	124	200.0	3335.0	919.8	ANOMALY_TRACKING_WRONG_WALL
21627	47.0	62.0	124	200.0	3269.9	724.5	ANOMALY_TRACKING_WRONG_WALL
21735	49.0	60.0	132	200.0	3214.6	490.4	ANOMALY_TRACKING_WRONG_WALL
21827	49.0	60.0	129	200.0	3141.9	245.9	ANOMALY_TRACKING_WRONG_WALL
21935	49.0	55.0	100	200.0	3154.9	0.0	ANOMALY_TRACKING_WRONG_WALL
22027	48.0	53.0	109	200.0	3097.4	0.0	ANOMALY_TRACKING_WRONG_WALL
22127	49.0	65.0	103	200.0	3034.9	0.0	ANOMALY_TRACKING_WRONG_WALL
22235	49.0	200.0	80	200.0	3087.9	0.0	ANOMALY_TRACKING_WRONG_WALL
22327	51.0	200.0	87	200.0	3034.4	0.0	ANOMALY_TRACKING_WRONG_WALL
22435	49.0	200.0	78	200.0	2958.7	0.0	ANOMALY_TRACKING_WRONG_WALL
22527	46.0	200.0	81	200.0	2885.7	0.0	ANOMALY_TRACKING_WRONG_WALL
22627	44.0	200.0	79	200.0	2785.2	0.0	ANOMALY_TRACKING_WRONG_WALL
22735	42.0	200.0	46	200.0	2743.8	0.0	ANOMALY_TRACKING_WRONG_WALL
22827	40.0	200.0	57	200.0	2629.7	0.0	ANOMALY_TRACKING_WRONG_WALL
22935	40.0	200.0	76	200.0	2545.7	0.0	ANOMALY_TRACKING_WRONG_WALL
23027	50.0	200.0	69	200.0	2446.4	0.0	ANOMALY_TRACKING_WRONG_WALL
23127	60.0	200.0	73	200.0	2336.8	0.0	ANOMALY_TRACKING_WRONG_WALL
23235	65.0	200.0	55	200.0	2142.8	0.0	ANOMALY_TRACKING_WRONG_WALL

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

Opening port

Port open

gl_pSerPort now points to active Serial (USB or Wixel)

7950: Starting setup() for WallE3_Complete_V2.ino

Checking for MPU6050 IMU at I2C Addr 0x68

MPU6050 connection successful

Initializing DMP...

Enabling DMP...

DMP ready! Waiting for MPU6050 drift rate to settle...

Calibrating...Retrieving Calibration Values

Msec Hdg

10531 -0.054 -0.042

MPU6050 Ready at 10.53 Sec with delta = -0.013

Checking for Teensy 3.5 VL53L0X Controller at I2C addr 0x20

Teensy available at 11632 with gl_bVL53L0X_TeensyReady = 1. Waiting for Teensy setup() to finish

11635: got 1 from VL53L0X Teensy

Teensy setup() finished at 11737 mSec

VL53L0X Teensy Ready at 11739

Checking for Garmin LIDAR at Wire2 I2C addr 0x62

LIDAR Responded to query at Wire2 address 0x62!

Setting LIDAR acquisition repeat value to 0x02 for <100mSec measurement repeat delay

Initializing Front Distance Array...Done

Initializing Rear Distance Array...Done

Checking for Teensy 3.2 IRDET Controller at I2C addr 0x8

11784: IRDET Teensy Not Avail...

IRDET Teensy Ready at 11886

Fin1/Fin2/SteeringVal = 76 60 -0.6875

11888: Initializing IR Beam Value Averaging Array...Done

11891: Initializing IR Beam Value Averaging Array...Done

Battery Voltage = 7.96

14098: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.96

IRHomingValTotalAvg = 30

14127: gl_LeftCenterCm = 16.00 gl_RightCenterCm = 48.00

TrackLeftWallOffset(350.0, 0.0, 0.0, 40) called

TrackLeftWallOffset: Start tracking offset of 40cm

Msec LCen RCen Front Rear FVar RVar ANOMALYCODE

14239 16.0 50.0 448 30.0 61469.3 726.4 NONE

14339 16.0 50.0 446 28.0 57852.4 696.7 NONE

14480 16.0 45.0 315 69.0 53127.2 664.8 NONE

14539 16.0 41.0 358 80.0 50844.8 642.7 NONE

14672 24.0 45.0 272 65.0 48742.9 609.6 NONE

14739 29.0 61.0 300 49.0 47804.5 576.7 NONE

14839 36.0 117.0 281 26.0 47322.4 563.0 NONE

14954 39.0 200.0 324 34.0 46199.8 544.0 NONE

15039 39.0 41.0 309 66.0 46108.9 519.5 NONE

15151 39.0 40.0 156 92.0 50430.1 518.1 NONE

15239 37.0 200.0 207 87.0 52053.2 509.8 NONE

15339 38.0 200.0 173 59.0 54277.3 487.0 NONE

15458 37.0 89.0 411 77.0 54262.7 471.1 NONE

15539 37.0 82.0 331 109.0 54945.0 495.2 NONE

15648 37.0 94.0 105 200.0 62127.1 842.4 NONE

15739 38.0 98.0 180 62.0 64470.8 821.9 NONE

15839 36.0 200.0 130 75.0 67450.4 804.0 NONE

15948 34.0 200.0 95 114.0 74016.6 827.9 NONE

16039 32.0 200.0 106 200.0 76787.8 1144.9 NONE

16160 30.0 200.0 372 118.0 76567.9 1166.6 NONE

16239 29.0 99.0 283 93.0 76708.2 1155.7 NONE

16339 28.0 98.0 342 99.0 76245.0 1150.2 NONE

16456 27.0 101.0 405 108.0 74147.9 1154.4 NONE

16539 27.0 102.0 384 200.0 72731.9 1434.3 NONE

16655 27.0 100.0 228 200.0 70382.6 1698.8 NONE

16739 29.0 100.0 280 93.0 68130.6 1683.7 NONE

16839 31.0 100.0 245 86.0 65563.2 1668.3 NONE

16949 34.0 104.0 154 86.0 59923.0 1654.3 NONE

17039 36.0 101.0 184 88.0 55745.3 1642.0 NONE

17149 38.0 100.0 174 200.0 45111.3 1886.6 NONE

17239 38.0 98.0 177 200.0 38459.1 2118.3 NONE

17339 39.0 101.0 175 200.0 30873.0 2337.4 NONE

17455 39.0 102.0 363 200.0 12576.5 2544.4 NONE

17539 40.0 200.0 300 200.0 11969.3 2740.0 NONE

17653 39.0 200.0 332 200.0 10862.4 2924.4 NONE

17739 40.0 200.0 321 200.0 10288.4 3098.2 NONE

17839 40.0 200.0 328 200.0 9703.9 3261.8 NONE

17953 39.0 200.0 290 200.0 9010.9 3415.6 NONE

18039 39.0 59.0 302 200.0 8986.3 3560.0 NONE

18153 38.0 60.0 276 200.0 8802.8 3695.6 NONE

18239 38.0 59.0 284 200.0 8811.6 3822.7 NONE

18339 37.0 57.0 278 200.0 8785.7 3941.6 NONE

18449 36.0 57.0 277 200.0 8704.3 4052.8 NONE

18539 37.0 57.0 277 200.0 8624.2 4156.6 NONE

18649 36.0 56.0 264 200.0 8365.4 4253.5 NONE

18739 37.0 56.0 268 200.0 8151.1 4343.7 NONE

18839 37.0 54.0 265 200.0 8091.9 4427.6 NONE

18949 200.0 53.0 258 200.0 7488.6 4505.7 NONE

19039 200.0 53.0 260 200.0 7027.9 4369.0 NONE

In HandleAnomalousConditions with OPEN_DOORWAY error code detected

ANOMALY_OPEN_DOORWAY case detected

19046: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.90

IRHomingValTotalAvg = 66

19062: gl_LeftCenterCm = 200.00 gl_RightCenterCm = 53.00

gl_LeftCenterCm > gl_RightCenterCm --> Calling TrackRightWallOffset(53)

TrackRightWallOffset(350.0, 0.0, 0.0, 53) called

TrackRightWallOffset: Start tracking offset of 53cm

Msec LCen RCen Front Rear FVar RVar ANOMALYCODE

19181 200.0 52.0 258 200.0 6451.5 4041.0 OPEN_DOORWAY

19281 200.0 52.0 258 200.0 5965.8 3841.7 OPEN_DOORWAY

19391 200.0 53.0 216 200.0 5559.7 3781.3 OPEN_DOORWAY

19481 200.0 53.0 230 200.0 4999.9 3740.4 OPEN_DOORWAY

19590 200.0 53.0 225 200.0 3930.4 3640.2 OPEN_DOORWAY

19681 200.0 52.0 226 200.0 3767.1 3462.4 OPEN_DOORWAY

19781 200.0 54.0 225 200.0 3588.4 3154.9 OPEN_DOORWAY

19890 200.0 53.0 199 200.0 3631.8 2865.5 OPEN_DOORWAY

19981 200.0 54.0 207 200.0 3268.5 2701.6 OPEN_DOORWAY

20090 200.0 54.0 185 200.0 2664.1 2615.2 OPEN_DOORWAY

20181 200.0 54.0 192 200.0 2720.8 2503.5 OPEN_DOORWAY

20281 200.0 53.0 187 200.0 2788.3 2270.9 OPEN_DOORWAY

20389 67.0 52.0 179 200.0 2948.3 2099.2 OPEN_DOORWAY

20481 56.0 52.0 181 200.0 2864.5 2022.7 OPEN_DOORWAY

20589 55.0 53.0 139 200.0 3073.5 2022.7 OPEN_DOORWAY

20681 54.0 52.0 153 200.0 3141.1 1764.2 OPEN_DOORWAY

20781 53.0 54.0 143 200.0 3246.5 1549.5 OPEN_DOORWAY

20890 53.0 56.0 128 200.0 3592.9 1461.6 OPEN_DOORWAY

In HandleAnomalousConditions with ANOMALY_TRACKING_WRONG_WALL error code detected

ANOMALY_TRACKING_WRONG_WALL case detected

20899: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.91

IRHomingValTotalAvg = 88

20915: gl_LeftCenterCm = 54.00 gl_RightCenterCm = 56.00

TrackLeftWallOffset(350.0, 0.0, 0.0, 40) called

TrackLeftWallOffset: Start tracking offset of 40cm

Msec LCen RCen Front Rear FVar RVar ANOMALYCODE

21027 51.0 58.0 129 200.0 3400.4 1378.8 ANOMALY_TRACKING_WRONG_WALL

21127 49.0 59.0 130 200.0 3504.3 1209.2 ANOMALY_TRACKING_WRONG_WALL

21235 46.0 59.0 124 200.0 3444.7 1052.8 ANOMALY_TRACKING_WRONG_WALL

21327 44.0 59.0 126 200.0 3404.2 919.8 ANOMALY_TRACKING_WRONG_WALL

21435 43.0 60.0 124 200.0 3333.0 919.8 ANOMALY_TRACKING_WRONG_WALL

21527 43.0 61.0 124 200.0 3335.0 919.8 ANOMALY_TRACKING_WRONG_WALL

21627 47.0 62.0 124 200.0 3269.9 724.5 ANOMALY_TRACKING_WRONG_WALL

21735 49.0 60.0 132 200.0 3214.6 490.4 ANOMALY_TRACKING_WRONG_WALL

21827 49.0 60.0 129 200.0 3141.9 245.9 ANOMALY_TRACKING_WRONG_WALL

21935 49.0 55.0 100 200.0 3154.9 0.0 ANOMALY_TRACKING_WRONG_WALL

22027 48.0 53.0 109 200.0 3097.4 0.0 ANOMALY_TRACKING_WRONG_WALL

22127 49.0 65.0 103 200.0 3034.9 0.0 ANOMALY_TRACKING_WRONG_WALL

22235 49.0 200.0 80 200.0 3087.9 0.0 ANOMALY_TRACKING_WRONG_WALL

22327 51.0 200.0 87 200.0 3034.4 0.0 ANOMALY_TRACKING_WRONG_WALL

22435 49.0 200.0 78 200.0 2958.7 0.0 ANOMALY_TRACKING_WRONG_WALL

22527 46.0 200.0 81 200.0 2885.7 0.0 ANOMALY_TRACKING_WRONG_WALL

22627 44.0 200.0 79 200.0 2785.2 0.0 ANOMALY_TRACKING_WRONG_WALL

22735 42.0 200.0 46 200.0 2743.8 0.0 ANOMALY_TRACKING_WRONG_WALL

22827 40.0 200.0 57 200.0 2629.7 0.0 ANOMALY_TRACKING_WRONG_WALL

22935 40.0 200.0 76 200.0 2545.7 0.0 ANOMALY_TRACKING_WRONG_WALL

23027 50.0 200.0 69 200.0 2446.4 0.0 ANOMALY_TRACKING_WRONG_WALL

23127 60.0 200.0 73 200.0 2336.8 0.0 ANOMALY_TRACKING_WRONG_WALL

23235 65.0 200.0 55 200.0 2142.8 0.0 ANOMALY_TRACKING_WRONG_WALL

The robot starts the run tracking the left wall at the default offset (40cm), with an anomaly code of ANOMALY_NONE. This continues until the 19 sec mark (19,039 mSec), where the robot detects the ‘open doorway’ condition. This causes the code to re-assess the tracking condition, and it decides to track the right wall, starting at about 19,181 mSec.

During this segment, the AnomalyCode value is OPEN_DOORWAY. This continues until about 20,890 mSec where the ‘Tracking Wrong Wall’ condition is detected. Note that the actual/physical ‘open doorway condition ended at about 20,389 mSec when the left-side distance changed from 200 (max) to 67 cm, but it took another 0.5 sec for the algorithm to catch the change.

The ‘Tracking Wrong Wall’ detection caused the robot to once again re-assess the tracking configuration, whereupon it changed back to left-wall tracking at 21,027 mSec with the new anomaly code of ‘ANOMALY_TRACKING_WRONG_WALL’. Left side wall tracking continues until the run is terminated at 23,235 mSec. Note that the right-hand wall stops at about 22,235 mSec and the right side distance measurement goes to 200 (max) cm and stays there for the rest of the run.

Looking at the above, I believe the fixes I implemented were effective in addressing the ‘wandering robot syndrome’ I observed on the previous run. Next I will remove the debugging printout code, clean things up a bit, and then repeat the last ‘real-world’ run from before.

10 May 2023 Update:

I was definitely having problems with the ‘Open Doorway’ condition, so I wound up back in my ‘indoor range’ (AKA my office) to see if I could work through the issues. It turned out I was not detecting the onset or end of the ‘Open Doorway’ condition properly. I made some changes to the code and to the telemetry output to more thoroughly describe the action, and then ran the test again. The short movie and the telemetry output show the results:

230510 Open Doorway Run

14104:	End of setup(): Elapsed run time set to 0

0.0:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 8.09
IRHomingValTotalAvg = 36 
0.1: gl_LeftCenterCm = 18.10 gl_RightCenterCm = 103.80

TrackLeftWallOffset(350.0, 0.0, 0.0, 30) called

TrackLeftWallOffset: Start tracking offset of 30cm at 0.1


Sec	LCen	RCen	Front	Rear	FVar	RVar	HdgDeg	WWCnt	ACODE	TRKDIR
0.2	18.6	103.6	418	34.7	55332	12777	-0.2	0	NONE	LEFT
0.3	18.0	103.2	425	34.7	49642	12710	2.4	0	NONE	LEFT
0.4	18.6	120.3	422	44.7	44997	12633	9.7	0	NONE	LEFT
0.5	18.4	145.2	195	84.3	45966	12393	17.2	0	NONE	LEFT
0.6	19.2	148.8	168	89.1	48722	12178	21.2	0	NONE	LEFT
0.7	21.6	148.7	156	93.5	52650	11992	23.0	0	NONE	LEFT
0.8	22.6	151.4	148	98.4	57430	11829	24.1	0	NONE	LEFT
0.9	24.6	153.8	143	103.1	62752	11688	23.6	0	NONE	LEFT
1.0	27.7	149.1	145	107.3	68172	11571	21.4	0	NONE	LEFT
1.1	29.1	118.0	152	110.7	73316	11480	17.6	0	NONE	LEFT
1.2	30.0	93.4	183	104.9	77339	11447	12.0	0	NONE	LEFT
1.3	30.2	90.9	390	79.4	77600	11558	4.5	0	NONE	LEFT
1.4	31.2	93.6	377	78.9	77591	11704	-4.0	0	NONE	LEFT
1.5	31.8	114.1	360	84.1	77125	11847	-12.1	0	NONE	LEFT
1.6	32.5	134.0	531	65.3	75618	12108	-16.3	0	NONE	LEFT
1.7	32.5	159.2	348	68.3	73482	12372	-14.4	0	NONE	LEFT
1.8	31.8	156.6	351	84.5	69956	12573	-10.4	0	NONE	LEFT
1.9	30.4	139.5	344	102.4	64824	12713	-5.3	0	NONE	LEFT
2.0	29.9	126.4	364	109.0	57663	12837	1.5	0	NONE	LEFT
2.1	30.0	123.0	362	131.2	48293	12904	7.0	0	NONE	LEFT
2.2	31.4	117.7	360	145.2	36381	12946	8.1	0	NONE	LEFT
2.3	32.2	121.0	351	150.5	21565	12982	6.8	0	NONE	LEFT
2.4	33.3	126.3	367	154.9	12815	13012	3.0	0	NONE	LEFT
2.5	33.8	158.3	318	149.7	12354	13047	-2.4	0	NONE	LEFT
2.6	34.4	180.0	313	127.8	11859	13122	-7.0	0	NONE	LEFT
2.7	35.3	186.3	301	127.6	11352	13184	-8.3	0	NONE	LEFT
2.8	35.0	191.7	300	132.8	10732	13217	-7.1	0	NONE	LEFT
2.9	34.8	190.4	297	146.0	10091	13204	-3.6	0	NONE	LEFT
3.0	34.5	186.5	297	169.9	9691	13144	-0.8	0	NONE	LEFT
3.1	35.5	186.5	293	187.5	9001	13059	-0.3	0	NONE	LEFT
3.2	35.8	189.2	283	190.1	8137	12948	-1.7	0	NONE	LEFT
3.3	36.4	65.6	282	192.2	7120	12809	-3.1	0	NONE	LEFT
3.4	36.6	55.8	274	183.6	6003	12633	-4.6	0	NONE	LEFT
3.5	40.5	54.0	266	182.0	4893	12418	-5.6	0	NONE	LEFT
3.6	58.7	53.5	261	187.0	3862	12162	-4.7	0	NONE	LEFT
3.7	153.5	52.6	259	213.6	3209	11890	0.1	0	NONE	LEFT
3.8	152.9	52.7	297	221.9	3095	11585	6.8	0	OPEN_DOORWAY	LEFT

3.8:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.99
IRHomingValTotalAvg = 59 
3.8: gl_LeftCenterCm = 149.80 gl_RightCenterCm = 53.70

TrackRightWallOffset(350.0, 0.0, 0.0, 30) called

TrackRightWallOffset: Start tracking offset of 30cm at 3.8


Sec	LCen	RCen	Front	Rear	FVar	RVar	HdgDeg	WWCnt	ACODE	TRKDIR
4.0	112.6	56.7	95	80.5	4015	10868	22.8	0	NONE	RIGHT
4.1	109.8	61.5	77	64.7	4866	10491	28.4	0	NONE	RIGHT
4.2	116.2	67.0	74	64.9	6511	10001	26.8	0	NONE	RIGHT
4.2	123.6	53.8	77	72.8	7878	9365	21.5	0	NONE	RIGHT
4.4	112.5	42.2	285	88.2	7594	8563	13.2	0	NONE	RIGHT
4.5	111.8	36.9	264	122.8	7212	7594	4.6	0	NONE	RIGHT
4.6	121.8	33.5	219	246.5	6877	6757	-6.1	0	NONE	RIGHT
4.7	151.5	31.2	217	207.4	6529	5683	-17.7	0	NONE	RIGHT
4.8	160.8	32.6	215	240.9	6091	4631	-30.3	0	NONE	RIGHT
4.9	143.1	34.5	106	186.6	6396	3288	-36.9	0	NONE	RIGHT
5.0	148.2	37.4	108	186.3	6744	3194	-35.5	0	NONE	RIGHT
5.1	142.1	38.9	193	195.4	6527	3104	-30.4	0	NONE	RIGHT
5.2	141.3	38.8	193	219.0	6313	3063	-22.9	0	NONE	RIGHT
5.3	111.4	38.2	192	228.0	6078	3025	-13.7	0	NONE	RIGHT
5.4	44.3	38.4	191	250.8	5818	3084	-2.8	0	NONE	RIGHT
5.5	35.3	39.9	240	264.0	5565	3303	7.8	0	NONE	RIGHT
5.6	36.4	41.8	205	115.4	5305	3257	13.1	0	NONE	RIGHT
5.7	38.7	41.0	196	116.8	5061	3219	12.0	0	NONE	RIGHT
5.8	39.9	38.2	225	125.1	4847	3181	7.3	0	NONE	RIGHT
5.9	40.7	34.4	174	153.5	4663	3142	0.2	0	NONE	RIGHT
6.0	41.9	32.1	172	283.5	4493	3460	-9.1	0	NONE	RIGHT
6.1	43.5	34.1	171	263.2	4317	3665	-18.2	0	NONE	RIGHT
6.2	43.2	141.7	162	264.2	3994	3843	-22.0	0	NONE	RIGHT
6.2	42.5	144.0	156	277.9	3578	3988	-20.1	0	OPEN_DOORWAY	RIGHT

6.3:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.98
IRHomingValTotalAvg = 72 
6.3: gl_LeftCenterCm = 41.80 gl_RightCenterCm = 143.50

TrackLeftWallOffset(350.0, 0.0, 0.0, 30) called

TrackLeftWallOffset: Start tracking offset of 30cm at 6.3


Sec	LCen	RCen	Front	Rear	FVar	RVar	HdgDeg	WWCnt	ACODE	TRKDIR
6.4	40.4	140.9	138	272.0	2838	4175	-10.6	0	NONE	LEFT
6.5	39.8	143.8	138	300.8	2477	4276	-3.9	0	NONE	LEFT
6.6	40.8	152.5	139	306.0	2100	4375	-0.1	0	NONE	LEFT
6.7	41.6	150.7	131	305.7	1770	4493	-1.3	0	NONE	LEFT
6.8	42.6	142.4	123	326.3	1510	4747	-5.4	0	NONE	LEFT
6.9	44.2	138.6	126	313.7	1384	4920	-10.0	0	NONE	LEFT
7.0	43.3	141.2	118	306.7	1383	5094	-11.6	0	NONE	LEFT
7.1	44.2	143.9	111	310.0	1387	5294	-11.6	0	NONE	LEFT
7.2	44.1	146.9	113	322.4	1359	5539	-11.0	0	NONE	LEFT
7.3	44.4	152.2	108	325.1	1355	5783	-11.3	0	NONE	LEFT
7.4	44.6	154.1	100	324.2	1419	5987	-11.2	0	NONE	LEFT
7.5	44.5	158.5	97	328.7	1503	6119	-10.5	0	NONE	LEFT
7.6	45.0	160.5	92	342.9	1582	6285	-10.5	0	NONE	LEFT
7.7	45.3	162.6	86	346.5	1659	6451	-11.1	0	NONE	LEFT
7.8	45.7	163.1	80	339.7	1585	6597	-11.7	0	NONE	LEFT
7.9	46.9	161.1	64	345.6	1496	6806	-12.9	0	NONE	LEFT
8.0	50.4	158.5	52	345.8	1553	7029	-15.2	0	NONE	LEFT
8.1	58.5	161.6	48	346.2	1488	7243	-18.9	0	NONE	LEFT
8.2	62.5	181.1	49	325.8	1394	7359	-24.4	0	NONE	LEFT
8.3	57.7	293.4	56	230.4	1382	7299	-31.8	0	NONE	LEFT
8.4	53.7	278.8	51	188.5	1356	7286	-36.8	0	NONE	LEFT
8.5	55.1	96.4	46	189.5	1330	7282	-34.6	0	NONE	LEFT
8.6	97.5	98.2	42	195.6	1315	7307	-29.3	0	NONE	LEFT
8.7	103.3	197.4	38	292.1	1299	7356	-22.3	0	NONE	LEFT

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

14104: End of setup(): Elapsed run time set to 0

0.0: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 8.09

IRHomingValTotalAvg = 36

0.1: gl_LeftCenterCm = 18.10 gl_RightCenterCm = 103.80

TrackLeftWallOffset(350.0, 0.0, 0.0, 30) called

TrackLeftWallOffset: Start tracking offset of 30cm at 0.1

Sec LCen RCen Front Rear FVar RVar HdgDeg WWCnt ACODE TRKDIR

0.2 18.6 103.6 418 34.7 55332 12777 -0.2 0 NONE LEFT

0.3 18.0 103.2 425 34.7 49642 12710 2.4 0 NONE LEFT

0.4 18.6 120.3 422 44.7 44997 12633 9.7 0 NONE LEFT

0.5 18.4 145.2 195 84.3 45966 12393 17.2 0 NONE LEFT

0.6 19.2 148.8 168 89.1 48722 12178 21.2 0 NONE LEFT

0.7 21.6 148.7 156 93.5 52650 11992 23.0 0 NONE LEFT

0.8 22.6 151.4 148 98.4 57430 11829 24.1 0 NONE LEFT

0.9 24.6 153.8 143 103.1 62752 11688 23.6 0 NONE LEFT

1.0 27.7 149.1 145 107.3 68172 11571 21.4 0 NONE LEFT

1.1 29.1 118.0 152 110.7 73316 11480 17.6 0 NONE LEFT

1.2 30.0 93.4 183 104.9 77339 11447 12.0 0 NONE LEFT

1.3 30.2 90.9 390 79.4 77600 11558 4.5 0 NONE LEFT

1.4 31.2 93.6 377 78.9 77591 11704 -4.0 0 NONE LEFT

1.5 31.8 114.1 360 84.1 77125 11847 -12.1 0 NONE LEFT

1.6 32.5 134.0 531 65.3 75618 12108 -16.3 0 NONE LEFT

1.7 32.5 159.2 348 68.3 73482 12372 -14.4 0 NONE LEFT

1.8 31.8 156.6 351 84.5 69956 12573 -10.4 0 NONE LEFT

1.9 30.4 139.5 344 102.4 64824 12713 -5.3 0 NONE LEFT

2.0 29.9 126.4 364 109.0 57663 12837 1.5 0 NONE LEFT

2.1 30.0 123.0 362 131.2 48293 12904 7.0 0 NONE LEFT

2.2 31.4 117.7 360 145.2 36381 12946 8.1 0 NONE LEFT

2.3 32.2 121.0 351 150.5 21565 12982 6.8 0 NONE LEFT

2.4 33.3 126.3 367 154.9 12815 13012 3.0 0 NONE LEFT

2.5 33.8 158.3 318 149.7 12354 13047 -2.4 0 NONE LEFT

2.6 34.4 180.0 313 127.8 11859 13122 -7.0 0 NONE LEFT

2.7 35.3 186.3 301 127.6 11352 13184 -8.3 0 NONE LEFT

2.8 35.0 191.7 300 132.8 10732 13217 -7.1 0 NONE LEFT

2.9 34.8 190.4 297 146.0 10091 13204 -3.6 0 NONE LEFT

3.0 34.5 186.5 297 169.9 9691 13144 -0.8 0 NONE LEFT

3.1 35.5 186.5 293 187.5 9001 13059 -0.3 0 NONE LEFT

3.2 35.8 189.2 283 190.1 8137 12948 -1.7 0 NONE LEFT

3.3 36.4 65.6 282 192.2 7120 12809 -3.1 0 NONE LEFT

3.4 36.6 55.8 274 183.6 6003 12633 -4.6 0 NONE LEFT

3.5 40.5 54.0 266 182.0 4893 12418 -5.6 0 NONE LEFT

3.6 58.7 53.5 261 187.0 3862 12162 -4.7 0 NONE LEFT

3.7 153.5 52.6 259 213.6 3209 11890 0.1 0 NONE LEFT

3.8 152.9 52.7 297 221.9 3095 11585 6.8 0 OPEN_DOORWAY LEFT

3.8: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.99

IRHomingValTotalAvg = 59

3.8: gl_LeftCenterCm = 149.80 gl_RightCenterCm = 53.70

TrackRightWallOffset(350.0, 0.0, 0.0, 30) called

TrackRightWallOffset: Start tracking offset of 30cm at 3.8

Sec LCen RCen Front Rear FVar RVar HdgDeg WWCnt ACODE TRKDIR

4.0 112.6 56.7 95 80.5 4015 10868 22.8 0 NONE RIGHT

4.1 109.8 61.5 77 64.7 4866 10491 28.4 0 NONE RIGHT

4.2 116.2 67.0 74 64.9 6511 10001 26.8 0 NONE RIGHT

4.2 123.6 53.8 77 72.8 7878 9365 21.5 0 NONE RIGHT

4.4 112.5 42.2 285 88.2 7594 8563 13.2 0 NONE RIGHT

4.5 111.8 36.9 264 122.8 7212 7594 4.6 0 NONE RIGHT

4.6 121.8 33.5 219 246.5 6877 6757 -6.1 0 NONE RIGHT

4.7 151.5 31.2 217 207.4 6529 5683 -17.7 0 NONE RIGHT

4.8 160.8 32.6 215 240.9 6091 4631 -30.3 0 NONE RIGHT

4.9 143.1 34.5 106 186.6 6396 3288 -36.9 0 NONE RIGHT

5.0 148.2 37.4 108 186.3 6744 3194 -35.5 0 NONE RIGHT

5.1 142.1 38.9 193 195.4 6527 3104 -30.4 0 NONE RIGHT

5.2 141.3 38.8 193 219.0 6313 3063 -22.9 0 NONE RIGHT

5.3 111.4 38.2 192 228.0 6078 3025 -13.7 0 NONE RIGHT

5.4 44.3 38.4 191 250.8 5818 3084 -2.8 0 NONE RIGHT

5.5 35.3 39.9 240 264.0 5565 3303 7.8 0 NONE RIGHT

5.6 36.4 41.8 205 115.4 5305 3257 13.1 0 NONE RIGHT

5.7 38.7 41.0 196 116.8 5061 3219 12.0 0 NONE RIGHT

5.8 39.9 38.2 225 125.1 4847 3181 7.3 0 NONE RIGHT

5.9 40.7 34.4 174 153.5 4663 3142 0.2 0 NONE RIGHT

6.0 41.9 32.1 172 283.5 4493 3460 -9.1 0 NONE RIGHT

6.1 43.5 34.1 171 263.2 4317 3665 -18.2 0 NONE RIGHT

6.2 43.2 141.7 162 264.2 3994 3843 -22.0 0 NONE RIGHT

6.2 42.5 144.0 156 277.9 3578 3988 -20.1 0 OPEN_DOORWAY RIGHT

6.3: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.98

IRHomingValTotalAvg = 72

6.3: gl_LeftCenterCm = 41.80 gl_RightCenterCm = 143.50

TrackLeftWallOffset(350.0, 0.0, 0.0, 30) called

TrackLeftWallOffset: Start tracking offset of 30cm at 6.3

Sec LCen RCen Front Rear FVar RVar HdgDeg WWCnt ACODE TRKDIR

6.4 40.4 140.9 138 272.0 2838 4175 -10.6 0 NONE LEFT

6.5 39.8 143.8 138 300.8 2477 4276 -3.9 0 NONE LEFT

6.6 40.8 152.5 139 306.0 2100 4375 -0.1 0 NONE LEFT

6.7 41.6 150.7 131 305.7 1770 4493 -1.3 0 NONE LEFT

6.8 42.6 142.4 123 326.3 1510 4747 -5.4 0 NONE LEFT

6.9 44.2 138.6 126 313.7 1384 4920 -10.0 0 NONE LEFT

7.0 43.3 141.2 118 306.7 1383 5094 -11.6 0 NONE LEFT

7.1 44.2 143.9 111 310.0 1387 5294 -11.6 0 NONE LEFT

7.2 44.1 146.9 113 322.4 1359 5539 -11.0 0 NONE LEFT

7.3 44.4 152.2 108 325.1 1355 5783 -11.3 0 NONE LEFT

7.4 44.6 154.1 100 324.2 1419 5987 -11.2 0 NONE LEFT

7.5 44.5 158.5 97 328.7 1503 6119 -10.5 0 NONE LEFT

7.6 45.0 160.5 92 342.9 1582 6285 -10.5 0 NONE LEFT

7.7 45.3 162.6 86 346.5 1659 6451 -11.1 0 NONE LEFT

7.8 45.7 163.1 80 339.7 1585 6597 -11.7 0 NONE LEFT

7.9 46.9 161.1 64 345.6 1496 6806 -12.9 0 NONE LEFT

8.0 50.4 158.5 52 345.8 1553 7029 -15.2 0 NONE LEFT

8.1 58.5 161.6 48 346.2 1488 7243 -18.9 0 NONE LEFT

8.2 62.5 181.1 49 325.8 1394 7359 -24.4 0 NONE LEFT

8.3 57.7 293.4 56 230.4 1382 7299 -31.8 0 NONE LEFT

8.4 53.7 278.8 51 188.5 1356 7286 -36.8 0 NONE LEFT

8.5 55.1 96.4 46 189.5 1330 7282 -34.6 0 NONE LEFT

8.6 97.5 98.2 42 195.6 1315 7307 -29.3 0 NONE LEFT

8.7 103.3 197.4 38 292.1 1299 7356 -22.3 0 NONE LEFT

Salient points in the video and telemetry printout:

WallE3 captures and then tracks the desired 30cm offset with pretty decent accuracy up until 3.8sec where it encounters the ‘open doorway’ on the left. This results in an ‘OPEN_DOORWAY’ anomaly report, which in turn causes TrackLeftWallOffset() function to exit, which in turn causes the program to start over at the top of loop().
The ‘top of loop’ code reevaluates the tracking condition, and because the left distance is well over 100cm and the right distance is about 50cm, it decides to track the right wall instead.
The right wall is tracked from 4.0 to 6.2 sec (where the right wall ends) and again detects an ‘Open Doorway’ condition, which forces the loop() function to restart. This time the right distance is about 143cm and the left distance is about 42cm, so the code chooses the left wall for tracking
Left wall tracking continues from 6.4 to 8.7sec – the end of the run.

Stay tuned,

Frank

WallE3_Complete Testing

1 Reply

Posted 04 February 2023,

The ‘WallE3_Complete’ program was intended to incorporate all the improvements to my WallE3 autonomous wall tracking robot, as described in this post. At the end of this effort I showed that the ‘Complete_V1’ program did compile, and made a successful run on my ‘two-break’ test wall configuration. Here’s the video that was posted at the end of that previous post:

Successful ‘two-break’ wall run using ‘WallE3_Complete_V1’

This test is just the beginning of the effort to determine how well the ‘WallE3_Complete_V1’ program will perform. There’s lots more testing to come.

The next step is to port the above successful left-side algorithm to the right-side case. After the usual number of mistakes, I got the right-side tracking algorithm going as well, as shown in the following short video.

05 February 2023 Update:

Did my first ‘live’ test with WallE3 this evening, and now I’m wading through the telemetry. Here’s the video of the run:

An Excel plot of the run:

And here is the raw telemetry:

12087:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 8.26
IRHomingValTotalAvg = 62 
12105: glLeftCenterCm = -2097152000 glRightCenterCm = 0
glLeftCenterCm <= glRightCenterCm --> Calling TrackLeftWallOffset()

TrackLeftWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 350.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd	IRAvg
WallE3_Complete_V1.ino: In TrackLeftWallOffset: before while with errcode = NO_ANOMALIES

Msec	LF	LC	LR	LCCorr	Steer	Tweak	OffCm	Err	LastI	LastD	Out	Lspd	Rspd
12233	24.00	22.00	24.00	22.00	-0.03	-0.36	0.00	0.39	0.00	0.39	136.50	127	0
12333	24.00	22.00	24.00	22.00	0.02	-0.36	0.00	0.34	0.00	-0.05	119.00	127	0
12440	24.00	22.00	23.00	22.00	0.07	-0.36	0.00	0.29	0.00	-0.05	101.50	127	0
12533	24.00	22.00	23.00	21.00	0.13	-0.38	0.00	0.25	0.00	-0.04	87.50	127	0
12634	26.00	25.00	24.00	23.00	0.25	-0.34	0.00	0.09	0.00	-0.16	31.50	106	43
12732	28.00	27.00	25.00	23.00	0.32	-0.34	0.00	0.02	0.00	-0.07	7.00	82	68
12835	34.00	33.00	30.00	26.00	0.39	-0.28	0.00	-0.11	0.00	-0.13	-38.50	36	113
12932	39.00	39.00	35.00	28.00	0.47	-0.24	0.00	-0.23	0.00	-0.12	-80.50	0	127
13033	42.00	43.00	37.00	31.00	0.47	-0.18	0.00	-0.29	0.00	-0.06	-101.50	0	127
13136	43.00	44.00	38.00	32.00	0.47	-0.16	0.00	-0.31	0.00	-0.02	-108.50	0	127
13237	43.00	43.00	38.00	33.00	0.41	-0.14	0.00	-0.27	0.00	0.04	-94.50	0	127
13336	41.00	42.00	38.00	38.00	0.26	-0.04	0.00	-0.22	0.00	0.05	-77.00	0	127
13434	40.00	40.00	39.00	39.00	0.12	-0.02	0.00	-0.10	0.00	0.12	-35.00	40	110
13534	39.00	41.00	38.00	39.00	0.16	-0.02	0.00	-0.14	0.00	-0.04	-49.00	26	124
13633	41.00	42.00	41.00	42.00	0.04	0.04	0.00	-0.08	0.00	0.06	-28.00	47	103
13730	42.00	44.00	43.00	44.00	-0.02	0.08	0.00	-0.06	0.00	0.02	-21.00	54	96
13832	42.00	45.00	44.00	42.00	-0.20	0.04	0.00	0.16	0.00	0.22	56.00	127	19
13934	41.00	43.00	42.00	41.00	-0.16	0.02	0.00	0.14	0.00	-0.02	49.00	124	26
14029	38.00	41.00	41.00	37.00	-0.25	-0.06	0.00	0.31	0.00	0.17	108.50	127	0
14134	34.00	36.00	36.00	34.00	-0.17	-0.12	0.00	0.29	0.00	-0.02	101.50	127	0
14234	31.00	31.00	33.00	29.00	-0.19	-0.22	0.00	0.41	0.00	0.12	143.50	127	0
14341	30.00	28.00	29.00	28.00	0.08	-0.24	0.00	0.16	0.00	-0.25	56.00	127	19
14438	30.00	28.00	28.00	27.00	0.17	-0.26	0.00	0.09	0.00	-0.07	31.50	106	43
14533	30.00	29.00	27.00	25.00	0.29	-0.30	0.00	0.01	0.00	-0.08	3.50	78	71
14634	32.00	31.00	29.00	27.00	0.29	-0.26	0.00	-0.03	0.00	-0.04	-10.50	64	85
14733	33.00	32.00	30.00	29.00	0.23	-0.22	0.00	-0.01	0.00	0.02	-3.50	71	78
14837	34.00	35.00	32.00	30.00	0.31	-0.20	0.00	-0.11	0.00	-0.10	-38.50	36	113
14934	35.00	36.00	32.00	31.00	0.30	-0.18	0.00	-0.12	0.00	-0.01	-42.00	33	117
15033	36.00	37.00	34.00	34.00	0.24	-0.12	0.00	-0.12	0.00	0.00	-42.00	33	117
15134	36.00	37.00	35.00	36.00	0.16	-0.08	0.00	-0.08	0.00	0.04	-28.00	47	103
15236	37.00	37.00	35.00	35.00	0.17	-0.10	0.00	-0.07	0.00	0.01	-24.50	50	99
15335	36.00	37.00	35.00	36.00	0.14	-0.08	0.00	-0.06	0.00	0.01	-21.00	54	96
15441	37.00	37.00	35.00	36.00	0.13	-0.08	0.00	-0.05	0.00	0.01	-17.50	57	92
15541	37.00	36.00	35.00	34.00	0.18	-0.12	0.00	-0.06	0.00	-0.01	-21.00	53	96
15641	37.00	37.00	35.00	34.00	0.21	-0.12	0.00	-0.09	0.00	-0.03	-31.50	43	106
15738	37.00	37.00	35.00	36.00	0.16	-0.08	0.00	-0.08	0.00	0.01	-28.00	47	103
15840	36.00	36.00	35.00	35.00	0.11	-0.10	0.00	-0.01	0.00	0.07	-3.50	71	78
15934	36.00	36.00	36.00	36.00	0.04	-0.08	0.00	0.04	0.00	0.05	14.00	89	61
16041	35.00	36.00	34.00	36.00	0.07	-0.08	0.00	0.01	0.00	-0.03	3.50	78	71
16137	34.00	34.00	34.00	34.00	0.00	-0.12	0.00	0.12	0.00	0.11	42.00	117	33
16236	34.00	34.00	33.00	34.00	0.08	-0.12	0.00	0.04	0.00	-0.08	14.00	89	61
16338	33.00	32.00	32.00	31.00	0.10	-0.18	0.00	0.08	0.00	0.04	28.00	103	47
16441	32.00	32.00	32.00	32.00	0.05	-0.16	0.00	0.11	0.00	0.03	38.50	113	36
16538	32.00	32.00	30.00	30.00	0.18	-0.20	0.00	0.02	0.00	-0.09	7.00	82	68
16640	31.00	31.00	30.00	30.00	0.11	-0.20	0.00	0.09	0.00	0.07	31.50	106	43
16741	32.00	31.00	29.00	29.00	0.21	-0.22	0.00	0.01	0.00	-0.08	3.50	78	71
16841	31.00	31.00	30.00	30.00	0.17	-0.20	0.00	0.03	0.00	0.02	10.50	85	64
16941	31.00	31.00	30.00	30.00	0.16	-0.20	0.00	0.04	0.00	0.01	14.00	89	61
17041	32.00	31.00	29.00	28.00	0.25	-0.24	0.00	-0.01	0.00	-0.05	-3.50	71	78
17140	31.00	31.00	29.00	29.00	0.20	-0.22	0.00	0.02	0.00	0.03	7.00	82	68
17241	32.00	32.00	30.00	30.00	0.21	-0.20	0.00	-0.01	0.00	-0.03	-3.50	71	78
17340	32.00	32.00	30.00	30.00	0.20	-0.20	0.00	0.00	0.00	0.01	0.00	75	75
17441	31.00	31.00	30.00	30.00	0.15	-0.20	0.00	0.05	0.00	0.05	17.50	92	57
17541	32.00	33.00	30.00	32.00	0.13	-0.16	0.00	0.03	0.00	-0.02	10.50	85	64
17631	32.00	32.00	31.00	31.00	0.17	-0.18	0.00	0.01	0.00	-0.02	3.50	78	71
17736	33.00	32.00	30.00	29.00	0.23	-0.22	0.00	-0.01	0.00	-0.02	-3.50	71	78
17834	33.00	33.00	31.00	30.00	0.23	-0.20	0.00	-0.03	0.00	-0.02	-10.50	64	85
17932	33.00	33.00	31.00	31.00	0.19	-0.18	0.00	-0.01	0.00	0.02	-3.50	71	78
18034	34.00	34.00	31.00	30.00	0.29	-0.20	0.00	-0.09	0.00	-0.08	-31.50	43	106
18132	33.00	34.00	31.00	32.00	0.21	-0.16	0.00	-0.05	0.00	0.04	-17.50	57	92
18241	34.00	35.00	31.00	31.00	0.28	-0.18	0.00	-0.10	0.00	-0.05	-35.00	40	110
18341	34.00	35.00	32.00	33.00	0.20	-0.14	0.00	-0.06	0.00	0.04	-21.00	54	96
18441	34.00	34.00	31.00	30.00	0.29	-0.20	0.00	-0.09	0.00	-0.03	-31.50	43	106
18541	33.00	33.00	32.00	31.00	0.17	-0.18	0.00	0.01	0.00	0.10	3.50	78	71
18640	33.00	34.00	32.00	33.00	0.14	-0.14	0.00	0.00	0.00	-0.01	0.00	75	75
18734	33.00	33.00	32.00	32.00	0.14	-0.16	0.00	0.02	0.00	0.02	7.00	82	68
18840	32.00	33.00	30.00	31.00	0.17	-0.18	0.00	0.01	0.00	-0.01	3.50	78	71
18938	32.00	32.00	30.00	30.00	0.20	-0.20	0.00	0.00	0.00	-0.01	0.00	75	75
19036	31.00	32.00	29.00	30.00	0.19	-0.20	0.00	0.01	0.00	0.01	3.50	78	71
19138	31.00	31.00	29.00	30.00	0.14	-0.20	0.00	0.06	0.00	0.05	21.00	96	54
19240	31.00	31.00	28.00	29.00	0.21	-0.22	0.00	0.01	0.00	-0.05	3.50	78	71
19341	30.00	30.00	29.00	30.00	0.04	-0.20	0.00	0.16	0.00	0.15	56.00	127	19
19441	29.00	30.00	28.00	29.00	0.16	-0.22	0.00	0.06	0.00	-0.10	21.00	96	54
19541	28.00	29.00	27.00	28.00	0.14	-0.24	0.00	0.10	0.00	0.04	35.00	110	40
19641	29.00	29.00	27.00	27.00	0.21	-0.26	0.00	0.05	0.00	-0.05	17.50	92	57
19741	29.00	29.00	26.00	26.00	0.26	-0.28	0.00	0.02	0.00	-0.03	7.00	82	68
19841	29.00	30.00	27.00	28.00	0.23	-0.24	0.00	0.01	0.00	-0.01	3.50	78	71
19941	29.00	30.00	27.00	28.00	0.20	-0.24	0.00	0.04	0.00	0.03	14.00	89	61
20041	30.00	30.00	27.00	26.00	0.29	-0.28	0.00	-0.01	0.00	-0.05	-3.50	71	78
20141	30.00	31.00	27.00	28.00	0.27	-0.24	0.00	-0.03	0.00	-0.02	-10.50	64	85
20237	30.00	31.00	28.00	29.00	0.22	-0.22	0.00	0.00	0.00	0.03	0.00	75	75
20337	33.00	32.00	29.00	24.00	0.43	-0.32	0.00	-0.11	0.00	-0.11	-38.50	36	113
20441	38.00	39.00	30.00	16.00	0.79	-0.48	0.00	-0.31	0.00	-0.20	-108.50	0	127
20541	40.00	42.00	33.00	22.00	0.68	-0.36	0.00	-0.32	0.00	-0.01	-112.00	0	127
20641	43.00	45.00	37.00	26.00	0.61	-0.28	0.00	-0.33	0.00	-0.01	-115.50	0	127
20734	43.00	44.00	38.00	31.00	0.48	-0.18	0.00	-0.30	0.00	0.03	-105.00	0	127
20832	42.00	44.00	37.00	30.00	0.51	-0.20	0.00	-0.31	0.00	-0.01	-108.50	0	127
20933	40.00	44.00	37.00	40.00	0.25	0.00	0.00	-0.25	0.00	0.06	-87.50	0	127
21041	39.00	42.00	38.00	41.00	0.09	0.02	0.00	-0.11	0.00	0.14	-38.50	36	113
21140	40.00	42.00	41.00	40.00	-0.19	0.00	0.00	0.19	0.00	0.30	66.50	127	8
21233	43.00	48.00	50.00	24.00	-0.69	-0.32	0.00	1.01	0.00	0.82	353.50	127	0
21336	40.00	43.00	42.00	39.00	-0.26	-0.02	0.00	0.28	0.00	-0.73	98.00	127	0
21438	38.00	40.00	36.00	39.00	0.11	-0.02	0.00	-0.09	0.00	-0.37	-31.50	43	106
21538	38.00	39.00	35.00	33.00	0.33	-0.14	0.00	-0.19	0.00	-0.10	-66.50	8	127
21639	38.00	39.00	35.00	32.00	0.36	-0.16	0.00	-0.20	0.00	-0.01	-70.00	4	127
21740	37.00	40.00	36.00	39.00	0.10	-0.02	0.00	-0.08	0.00	0.12	-28.00	47	103
21833	38.00	39.00	36.00	37.00	0.16	-0.06	0.00	-0.10	0.00	-0.02	-35.00	40	110
21931	37.00	39.00	37.00	39.00	0.03	-0.02	0.00	-0.01	0.00	0.09	-3.50	71	78
22031	36.00	39.00	37.00	39.00	-0.05	-0.02	0.00	0.07	0.00	0.08	24.50	99	50
22133	34.00	38.00	35.00	37.00	-0.14	-0.06	0.00	0.20	0.00	0.13	70.00	127	5
22231	34.00	36.00	33.00	36.00	0.09	-0.08	0.00	-0.01	0.00	-0.21	-3.50	71	78
22335	32.00	33.00	32.00	33.00	-0.01	-0.14	0.00	0.15	0.00	0.16	52.50	127	22
22432	31.00	33.00	30.00	32.00	0.12	-0.16	0.00	0.04	0.00	-0.11	14.00	89	61
22532	29.00	30.00	28.00	30.00	0.08	-0.20	0.00	0.12	0.00	0.08	42.00	117	33
22635	29.00	30.00	28.00	29.00	0.14	-0.22	0.00	0.08	0.00	-0.04	28.00	103	47
22734	28.00	29.00	26.00	28.00	0.15	-0.24	0.00	0.09	0.00	0.01	31.50	106	43
22841	27.00	28.00	25.00	27.00	0.13	-0.26	0.00	0.13	0.00	0.04	45.50	120	29
22940	26.00	28.00	25.00	26.00	0.19	-0.28	0.00	0.09	0.00	-0.04	31.50	106	43
23041	27.00	27.00	25.00	25.00	0.20	-0.30	0.00	0.10	0.00	0.01	35.00	110	39
23141	26.00	29.00	24.00	27.00	0.19	-0.26	0.00	0.07	0.00	-0.03	24.50	99	50
23241	27.00	29.00	25.00	28.00	0.18	-0.24	0.00	0.06	0.00	-0.01	21.00	96	54
23341	28.00	30.00	26.00	28.00	0.21	-0.24	0.00	0.03	0.00	-0.03	10.50	85	64
23436	28.00	30.00	26.00	28.00	0.20	-0.24	0.00	0.04	0.00	0.01	14.00	89	61
23541	30.00	32.00	27.00	29.00	0.24	-0.22	0.00	-0.02	0.00	-0.06	-7.00	68	82
23641	31.00	32.00	29.00	31.00	0.16	-0.18	0.00	0.02	0.00	0.04	7.00	82	68
23737	32.00	34.00	30.00	31.00	0.23	-0.18	0.00	-0.05	0.00	-0.07	-17.50	57	92
23839	32.00	36.00	30.00	33.00	0.23	-0.14	0.00	-0.09	0.00	-0.04	-31.50	43	106
23935	33.00	36.00	31.00	33.00	0.25	-0.14	0.00	-0.11	0.00	-0.02	-38.50	36	113
24041	34.00	37.00	32.00	34.00	0.24	-0.12	0.00	-0.12	0.00	-0.01	-42.00	33	117
24141	34.00	37.00	32.00	34.00	0.23	-0.12	0.00	-0.11	0.00	0.01	-38.50	36	113
24238	35.00	38.00	32.00	34.00	0.28	-0.12	0.00	-0.16	0.00	-0.05	-56.00	19	127
24337	34.00	39.00	32.00	36.00	0.21	-0.08	0.00	-0.13	0.00	0.03	-45.50	29	120
24432	35.00	37.00	35.00	37.00	0.01	-0.06	0.00	0.05	0.00	0.18	17.50	92	57
24531	35.00	37.00	34.00	36.00	0.14	-0.08	0.00	-0.06	0.00	-0.11	-21.00	54	96
24634	34.00	36.00	34.00	36.00	-0.05	-0.08	0.00	0.13	0.00	0.19	45.50	120	29
24736	33.00	36.00	32.00	36.00	0.06	-0.08	0.00	0.02	0.00	-0.11	7.00	82	68
24834	31.00	35.00	31.00	35.00	0.03	-0.10	0.00	0.07	0.00	0.05	24.50	99	50
24934	31.00	33.00	30.00	33.00	0.07	-0.14	0.00	0.07	0.00	0.00	24.50	99	50
25036	29.00	32.00	28.00	32.00	0.02	-0.16	0.00	0.14	0.00	0.07	49.00	124	26
25136	28.00	30.00	27.00	30.00	0.06	-0.20	0.00	0.14	0.00	0.00	49.00	124	26
25234	41.00	41.00	29.00	10.00	1.00	-0.60	0.00	-0.40	0.00	-0.54	-140.00	0	127
25338	45.00	50.00	41.00	37.00	0.44	-0.06	0.00	-0.38	0.00	0.02	-133.00	0	127
25434	45.00	49.00	43.00	48.00	0.13	0.16	0.00	-0.29	0.00	0.09	-101.50	0	127
25535	44.00	49.00	43.00	48.00	0.12	0.16	0.00	-0.28	0.00	0.01	-98.00	0	127
25635	45.00	50.00	44.00	49.00	0.08	0.18	0.00	-0.26	0.00	0.02	-91.00	0	127
25731	48.00	51.00	49.00	51.00	-0.07	0.22	0.00	-0.15	0.00	0.11	-52.50	22	127
25829	51.00	54.00	45.00	33.00	0.58	-0.14	0.00	-0.44	0.00	-0.29	-154.00	0	127
25931	50.00	52.00	43.00	26.00	0.70	-0.28	0.00	-0.42	0.00	0.02	-147.00	0	127
26029	44.00	48.00	42.00	45.00	0.21	0.10	0.00	-0.31	0.00	0.11	-108.50	0	127
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Left
In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0)
Init hdg = -95.99 deg, Turn = 90.00  deg, tgt = -5.99 deg, timeout = 4.00 sec

Msec	Hdg	PrvHdg	dHdg	Rate	tgtDPS	err	Kp*err	Ival	Kd*Derr	speed	Match	Slope
26094	-100.46	-95.99	-4.47	149.05	45.00	-104.05	-72.84	-31.22	-0.00	0	0.48	0.48
26124	-103.58	-100.46	-3.11	103.73	45.00	-58.73	-41.11	-48.83	0.00	0	0.46	-0.02
26154	-106.23	-103.58	-2.66	88.63	45.00	-43.63	-30.54	-61.92	0.00	0	0.44	-0.01
26184	-107.97	-106.23	-1.74	57.89	45.00	-12.89	-9.02	-65.79	0.00	0	0.43	-0.01
26214	-108.71	-107.97	-0.74	24.58	45.00	20.42	14.30	-59.66	0.00	0	0.43	-0.00
26244	-108.85	-108.71	-0.15	4.85	45.00	40.15	28.10	-47.62	0.00	0	0.43	-0.00
26274	-109.03	-108.85	-0.18	5.90	45.00	39.10	27.37	-35.89	-0.00	0	0.43	-0.00
26304	-109.03	-109.03	0.00	0.02	45.00	44.98	31.48	-22.39	0.00	9	0.43	0.00
26334	-108.99	-109.03	0.04	1.36	45.00	43.64	30.55	-9.30	-0.00	21	0.43	0.00
26364	-108.99	-108.99	0.00	0.08	45.00	44.92	31.45	4.18	0.00	35	0.43	0.00
26394	-108.84	-108.99	0.15	4.90	45.00	40.10	28.07	16.21	-0.00	44	0.43	0.00
26424	-108.54	-108.84	0.30	9.91	45.00	35.09	24.56	26.73	-0.00	51	0.43	0.00
26454	-107.46	-108.54	1.08	36.00	45.00	9.00	6.30	29.43	-0.00	35	0.44	0.01
26484	-105.94	-107.46	1.52	50.79	45.00	-5.79	-4.05	27.70	-0.00	23	0.44	0.01
26517	-104.37	-105.94	1.57	52.18	45.00	-7.18	-5.02	25.54	-0.00	20	0.45	0.01
26547	-102.98	-104.37	1.39	46.34	45.00	-1.34	-0.94	25.14	0.00	24	0.46	0.01
26577	-101.85	-102.98	1.14	37.90	45.00	7.10	4.97	27.27	0.00	32	0.47	0.01
26607	-100.90	-101.85	0.95	31.70	45.00	13.30	9.31	31.26	0.00	40	0.47	0.01
26637	-99.91	-100.90	0.98	32.72	45.00	12.28	8.59	34.94	-0.00	43	0.48	0.01
26667	-98.77	-99.91	1.14	38.01	45.00	6.99	4.90	37.04	-0.00	41	0.48	0.01
26697	-97.38	-98.77	1.39	46.40	45.00	-1.40	-0.98	36.62	-0.00	35	0.49	0.01
26727	-95.87	-97.38	1.51	50.46	45.00	-5.46	-3.82	34.98	-0.00	31	0.50	0.01
26757	-94.36	-95.87	1.51	50.41	45.00	-5.41	-3.79	33.36	0.00	29	0.51	0.01
26787	-92.85	-94.36	1.50	50.06	45.00	-5.06	-3.54	31.84	0.00	28	0.52	0.01
26817	-91.48	-92.85	1.37	45.78	45.00	-0.78	-0.55	31.61	0.00	31	0.53	0.01
26847	-90.20	-91.48	1.28	42.66	45.00	2.34	1.64	32.31	0.00	33	0.53	0.01
26877	-88.96	-90.20	1.24	41.21	45.00	3.79	2.66	33.45	0.00	36	0.54	0.01
26907	-87.73	-88.96	1.23	41.12	45.00	3.88	2.72	34.61	0.00	37	0.55	0.01
26937	-86.44	-87.73	1.29	43.13	45.00	1.87	1.31	35.17	-0.00	36	0.55	0.01
26967	-85.06	-86.44	1.38	45.97	45.00	-0.97	-0.68	34.88	-0.00	34	0.56	0.01
26997	-83.65	-85.06	1.40	46.83	45.00	-1.83	-1.28	34.33	-0.00	33	0.57	0.01
27027	-82.24	-83.65	1.41	47.14	45.00	-2.14	-1.50	33.69	-0.00	32	0.58	0.01
27057	-80.82	-82.24	1.42	47.24	45.00	-2.24	-1.57	33.02	-0.00	31	0.58	0.01
27087	-79.34	-80.82	1.48	49.31	45.00	-4.31	-3.02	31.73	-0.00	28	0.59	0.01
27117	-77.89	-79.34	1.45	48.35	45.00	-3.35	-2.35	30.73	0.00	28	0.60	0.01
27147	-76.50	-77.89	1.40	46.53	45.00	-1.53	-1.07	30.27	0.00	29	0.61	0.01
27177	-75.24	-76.50	1.25	41.74	45.00	3.26	2.28	31.24	0.00	33	0.62	0.01
27207	-74.06	-75.24	1.18	39.37	45.00	5.63	3.94	32.93	0.00	36	0.62	0.01
27237	-72.89	-74.06	1.17	38.97	45.00	6.03	4.22	34.74	0.00	38	0.63	0.01
27267	-71.64	-72.89	1.26	41.91	45.00	3.09	2.16	35.67	-0.00	37	0.64	0.01
27297	-70.21	-71.64	1.43	47.56	45.00	-2.56	-1.79	34.90	-0.00	33	0.64	0.01
27327	-68.60	-70.21	1.61	53.74	45.00	-8.74	-6.12	32.28	-0.00	26	0.65	0.01
27357	-66.99	-68.60	1.60	53.45	45.00	-8.45	-5.92	29.74	0.00	23	0.66	0.01
27387	-65.66	-66.99	1.33	44.39	45.00	0.61	0.42	29.92	0.00	30	0.67	0.01
27417	-64.52	-65.66	1.15	38.21	45.00	6.79	4.75	31.96	0.00	36	0.67	0.01
27447	-63.41	-64.52	1.11	36.90	45.00	8.10	5.67	34.39	0.00	40	0.68	0.01
27477	-62.24	-63.41	1.17	39.09	45.00	5.91	4.14	36.16	-0.00	40	0.69	0.01
27507	-60.97	-62.24	1.26	42.15	45.00	2.85	1.99	37.02	-0.00	39	0.69	0.01
27537	-59.56	-60.97	1.41	47.01	45.00	-2.01	-1.41	36.41	-0.00	35	0.70	0.01
27567	-58.01	-59.56	1.55	51.59	45.00	-6.59	-4.62	34.43	-0.00	29	0.71	0.01
27597	-56.47	-58.01	1.54	51.45	45.00	-6.45	-4.52	32.50	0.00	27	0.72	0.01
27627	-55.06	-56.47	1.41	46.98	45.00	-1.98	-1.39	31.90	0.00	30	0.73	0.01
27657	-53.70	-55.06	1.36	45.28	45.00	-0.28	-0.20	31.82	0.00	31	0.73	0.01
27687	-52.42	-53.70	1.29	42.85	45.00	2.15	1.50	32.46	0.00	33	0.74	0.01
27717	-51.17	-52.42	1.25	41.62	45.00	3.38	2.37	33.48	0.00	35	0.75	0.01
27747	-49.87	-51.17	1.30	43.25	45.00	1.75	1.23	34.00	-0.00	35	0.76	0.01
27777	-48.55	-49.87	1.32	43.91	45.00	1.09	0.76	34.33	-0.00	35	0.76	0.01
27807	-47.18	-48.55	1.37	45.83	45.00	-0.83	-0.58	34.08	-0.00	33	0.77	0.01
27837	-45.75	-47.18	1.43	47.56	45.00	-2.56	-1.79	33.31	-0.00	31	0.78	0.01
27867	-44.35	-45.75	1.40	46.76	45.00	-1.76	-1.23	32.79	0.00	31	0.79	0.01
27897	-42.98	-44.35	1.37	45.67	45.00	-0.67	-0.47	32.59	0.00	32	0.79	0.01
27927	-41.64	-42.98	1.34	44.66	45.00	0.34	0.24	32.69	0.00	32	0.80	0.01
27957	-40.38	-41.64	1.26	42.00	45.00	3.00	2.10	33.59	0.00	35	0.81	0.01
27987	-39.09	-40.38	1.29	43.00	45.00	2.00	1.40	34.19	-0.00	35	0.82	0.01
28017	-37.77	-39.09	1.32	43.87	45.00	1.13	0.79	34.52	-0.00	35	0.82	0.01
28047	-36.43	-37.77	1.34	44.76	45.00	0.24	0.17	34.60	-0.00	34	0.83	0.01
28077	-35.04	-36.43	1.38	46.14	45.00	-1.14	-0.80	34.25	-0.00	33	0.84	0.01
28107	-33.68	-35.04	1.37	45.66	45.00	-0.66	-0.46	34.05	0.00	33	0.85	0.01
28137	-32.40	-33.68	1.27	42.36	45.00	2.64	1.85	34.85	0.00	36	0.85	0.01
28167	-31.14	-32.40	1.27	42.27	45.00	2.73	1.91	35.67	0.00	37	0.86	0.01
28197	-29.83	-31.14	1.30	43.40	45.00	1.60	1.12	36.15	-0.00	37	0.87	0.01
28227	-28.38	-29.83	1.45	48.40	45.00	-3.40	-2.38	35.13	-0.00	32	0.88	0.01
28257	-26.85	-28.38	1.53	51.08	45.00	-6.08	-4.26	33.30	-0.00	29	0.88	0.01
28287	-25.38	-26.85	1.47	48.84	45.00	-3.84	-2.69	32.15	0.00	29	0.89	0.01
28317	-24.01	-25.38	1.37	45.74	45.00	-0.74	-0.52	31.93	0.00	31	0.90	0.01
28347	-22.68	-24.01	1.33	44.28	45.00	0.72	0.50	32.15	0.00	32	0.91	0.01
28377	-21.40	-22.68	1.28	42.68	45.00	2.32	1.62	32.84	0.00	34	0.91	0.01
28407	-20.12	-21.40	1.28	42.62	45.00	2.38	1.66	33.55	0.00	35	0.92	0.01
28437	-18.80	-20.12	1.32	44.10	45.00	0.90	0.63	33.82	-0.00	34	0.93	0.01
28467	-17.43	-18.80	1.37	45.64	45.00	-0.64	-0.45	33.63	-0.00	33	0.94	0.01
28497	-16.02	-17.43	1.42	47.22	45.00	-2.22	-1.55	32.96	-0.00	31	0.94	0.01
28527	-14.61	-16.02	1.40	46.83	45.00	-1.83	-1.28	32.41	0.00	31	0.95	0.01
28557	-13.23	-14.61	1.38	46.07	45.00	-1.07	-0.75	32.09	0.00	31	0.96	0.01
28587	-11.94	-13.23	1.29	42.89	45.00	2.11	1.48	32.73	0.00	34	0.97	0.01
28617	-10.72	-11.94	1.22	40.83	45.00	4.17	2.92	33.98	0.00	36	0.97	0.01
28647	-9.42	-10.72	1.29	43.09	45.00	1.91	1.34	34.55	-0.00	35	0.98	0.01
28677	-8.01	-9.42	1.41	47.03	45.00	-2.03	-1.42	33.94	-0.00	32	0.99	0.01
28707	-6.65	-8.01	1.37	45.62	45.00	-0.62	-0.43	33.76	0.00	33	1.00	0.01
average turn rate = 43.7

28710:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 8.06
IRHomingValTotalAvg = 98 
28732: glLeftCenterCm = -2080374784 glRightCenterCm = 0
glLeftCenterCm <= glRightCenterCm --> Calling TrackLeftWallOffset()

TrackLeftWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 350.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd	IRAvg
WallE3_Complete_V1.ino: In TrackLeftWallOffset: before while with errcode = NO_ANOMALIES

Msec	LF	LC	LR	LCCorr	Steer	Tweak	OffCm	Err	LastI	LastD	Out	Lspd	Rspd
28859	35.00	38.00	32.00	31.00	0.36	-0.18	0.00	-0.18	0.00	-0.18	-63.00	11	127
28959	35.00	38.00	32.00	34.00	0.27	-0.12	0.00	-0.15	0.00	0.03	-52.50	22	127
29059	34.00	38.00	31.00	33.00	0.31	-0.14	0.00	-0.17	0.00	-0.02	-59.50	15	127
29159	34.00	37.00	32.00	35.00	0.19	-0.10	0.00	-0.09	0.00	0.08	-31.50	43	106
29264	33.00	36.00	32.00	35.00	0.13	-0.10	0.00	-0.03	0.00	0.06	-10.50	64	85
29367	33.00	36.00	33.00	36.00	0.06	-0.08	0.00	0.02	0.00	0.05	7.00	82	68
29467	34.00	36.00	33.00	35.00	0.13	-0.10	0.00	-0.03	0.00	-0.05	-10.50	64	85
29567	33.00	36.00	32.00	35.00	0.10	-0.10	0.00	0.00	0.00	0.03	0.00	75	75
29666	33.00	35.00	32.00	35.00	0.07	-0.10	0.00	0.03	0.00	0.03	10.50	85	64
29767	32.00	35.00	32.00	35.00	0.05	-0.10	0.00	0.05	0.00	0.02	17.50	92	57
29862	32.00	35.00	30.00	33.00	0.17	-0.14	0.00	-0.03	0.00	-0.08	-10.50	64	85
29962	31.00	34.00	30.00	34.00	0.08	-0.12	0.00	0.04	0.00	0.07	14.00	89	61
30065	25.00	29.00	29.00	21.00	-0.47	-0.38	0.00	0.85	0.00	0.81	297.50	127	0
30164	22.00	23.00	25.00	21.00	-0.26	-0.38	0.00	0.64	0.00	-0.21	224.00	127	0
30267	24.00	26.00	22.00	24.00	0.20	-0.32	0.00	0.12	0.00	-0.52	42.00	117	33
30366	26.00	27.00	24.00	26.00	0.18	-0.28	0.00	0.10	0.00	-0.02	35.00	110	40
30462	25.00	28.00	24.00	28.00	0.09	-0.24	0.00	0.15	0.00	0.05	52.50	127	22
30564	29.00	35.00	25.00	28.00	0.38	-0.24	0.00	-0.14	0.00	-0.29	-49.00	26	124
30661	749.00	824.00	705.00	207.00	1.00	3.34	0.00	-4.34	0.00	-4.20	-1519.00	0	127
30762	749.00	824.00	705.00	207.00	1.00	3.34	0.00	-4.34	0.00	0.00	-1519.00	0	127
30866	749.00	824.00	705.00	207.00	1.00	3.34	0.00	-4.34	0.00	0.00	-1519.00	0	127
30965	749.00	824.00	705.00	207.00	1.00	3.34	0.00	-4.34	0.00	0.00	-1519.00	0	127
31067	749.00	824.00	705.00	207.00	1.00	3.34	0.00	-4.34	0.00	0.00	-1519.00	0	127
31161	749.00	824.00	47.00	207.00	1.00	3.34	0.00	-4.34	0.00	0.00	-1519.00	0	127
31258	55.00	58.00	48.00	32.00	0.65	-0.16	0.00	-0.49	0.00	3.85	-171.50	0	127
31359	49.00	49.00	43.00	33.00	0.52	-0.14	0.00	-0.38	0.00	0.11	-133.00	0	127
31460	46.00	50.00	48.00	45.00	-0.26	0.10	0.00	0.16	0.00	0.54	56.00	127	19
31556	52.00	58.00	56.00	44.00	-0.43	0.08	0.00	0.35	0.00	0.19	122.50	127	0
31660	53.00	57.00	55.00	54.00	-0.19	0.28	0.00	-0.09	0.00	-0.44	-31.50	43	106
31757	46.00	50.00	48.00	46.00	-0.23	0.12	0.00	0.11	0.00	0.20	38.50	113	36
31861	43.00	47.00	44.00	46.00	-0.11	0.12	0.00	-0.01	0.00	-0.12	-3.50	71	78
31959	48.00	50.00	43.00	34.00	0.50	-0.12	0.00	-0.38	0.00	-0.37	-133.00	0	127
32056	49.00	53.00	44.00	36.00	0.50	-0.08	0.00	-0.42	0.00	-0.04	-147.00	0	127
32160	50.00	53.00	47.00	46.00	0.30	0.12	0.00	-0.42	0.00	0.00	-147.00	0	127
32259	50.00	53.00	48.00	48.00	0.25	0.16	0.00	-0.41	0.00	0.01	-143.50	0	127
32357	50.00	54.00	47.00	48.00	0.26	0.16	0.00	-0.42	0.00	-0.01	-147.00	0	127
32458	49.00	53.00	48.00	52.00	0.11	0.24	0.00	-0.35	0.00	0.07	-122.50	0	127
32558	49.00	53.00	49.00	53.00	0.02	0.26	0.00	-0.28	0.00	0.07	-98.00	0	127
32658	50.00	56.00	705.00	14.00	-1.00	-0.52	0.00	1.52	0.00	1.80	532.00	127	0
32762	749.00	824.00	705.00	207.00	1.00	3.34	0.00	-4.34	0.00	-5.86	-1519.00	0	127
32858	45.00	824.00	705.00	207.00	-1.00	3.34	0.00	-2.34	0.00	2.00	-819.00	0	127
32956	50.00	56.00	705.00	14.00	-1.00	-0.52	0.00	1.52	0.00	3.86	532.00	127	0
33061	50.00	49.00	705.00	12.00	-1.00	-0.56	0.00	1.56	0.00	0.04	546.00	127	0
33161	47.00	51.00	51.00	36.00	-0.49	-0.08	0.00	0.57	0.00	-0.99	199.50	127	0
33256	43.00	47.00	43.00	47.00	0.05	0.14	0.00	-0.19	0.00	-0.76	-66.50	8	127
33360	40.00	42.00	41.00	40.00	-0.15	0.00	0.00	0.15	0.00	0.34	52.50	127	22
33460	37.00	39.00	37.00	39.00	-0.05	-0.02	0.00	0.07	0.00	-0.08	24.50	99	50
33560	36.00	39.00	34.00	38.00	0.14	-0.04	0.00	-0.10	0.00	-0.17	-35.00	40	110
33661	35.00	38.00	32.00	34.00	0.27	-0.12	0.00	-0.15	0.00	-0.05	-52.50	22	127
33758	34.00	38.00	33.00	36.00	0.17	-0.08	0.00	-0.09	0.00	0.06	-31.50	43	106
33858	34.00	37.00	32.00	35.00	0.20	-0.10	0.00	-0.10	0.00	-0.01	-35.00	40	110
33957	33.00	36.00	33.00	36.00	0.03	-0.08	0.00	0.05	0.00	0.15	17.50	92	57
34061	32.00	35.00	32.00	35.00	0.00	-0.10	0.00	0.10	0.00	0.05	35.00	110	40
34160	31.00	34.00	30.00	34.00	0.07	-0.12	0.00	0.05	0.00	-0.05	17.50	92	57
34257	29.00	32.00	28.00	32.00	0.06	-0.16	0.00	0.10	0.00	0.05	35.00	110	40
34358	27.00	30.00	26.00	30.00	0.05	-0.20	0.00	0.15	0.00	0.05	52.50	127	22
34457	25.00	28.00	25.00	28.00	0.04	-0.24	0.00	0.20	0.00	0.05	70.00	127	5
34559	24.00	27.00	22.00	26.00	0.14	-0.28	0.00	0.14	0.00	-0.06	49.00	124	26
34661	22.00	25.00	21.00	25.00	0.11	-0.30	0.00	0.19	0.00	0.05	66.50	127	8
34760	21.00	24.00	20.00	23.00	0.16	-0.34	0.00	0.18	0.00	-0.01	63.00	127	11
34858	22.00	23.00	20.00	21.00	0.22	-0.38	0.00	0.16	0.00	-0.02	56.00	127	19
34960	38.00	41.00	26.00	10.00	1.00	-0.60	0.00	-0.40	0.00	-0.56	-140.00	0	127
35058	40.00	43.00	37.00	37.00	0.31	-0.06	0.00	-0.25	0.00	0.15	-87.50	0	127
35156	41.00	44.00	37.00	34.00	0.41	-0.12	0.00	-0.29	0.00	-0.04	-101.50	0	127
35259	42.00	44.00	39.00	40.00	0.26	0.00	0.00	-0.26	0.00	0.03	-91.00	0	127
35357	42.00	45.00	39.00	40.00	0.27	0.00	0.00	-0.27	0.00	-0.01	-94.50	0	127
35456	41.00	45.00	40.00	44.00	0.11	0.08	0.00	-0.19	0.00	0.08	-66.50	8	127
35561	42.00	46.00	42.00	46.00	-0.01	0.12	0.00	-0.11	0.00	0.08	-38.50	36	113
35660	44.00	48.00	44.00	48.00	0.01	0.16	0.00	-0.17	0.00	-0.06	-59.50	15	127
35758	48.00	51.00	44.00	42.00	0.35	0.04	0.00	-0.39	0.00	-0.22	-136.50	0	127
35857	48.00	51.00	43.00	34.00	0.52	-0.12	0.00	-0.40	0.00	-0.01	-140.00	0	127
35956	45.00	49.00	43.00	45.00	0.22	0.10	0.00	-0.32	0.00	0.08	-112.00	0	127
In HandleAnomalousConditions with OBSTACLE_AHEAD error code detected
OBSTACLE_AHEAD case detected with tracking case Left
In BackupAndTurn90Deg(0, 1, 200)with front distance = 32
MoveToDesiredFrontDistCm(40): Kp/Ki/Kd= 1.5/0.1/0.2
MTFD: at start, tgt = 40cm, curr_dist = 32, front/rear var = 13328.0/833.0
Msec	Fdist	TgtD	err	Ival	Out	Speed	FVar	RVar
37101	31	40	9.0	0.9	12.6	12	12802.2	762.9
37201	32	40	8.0	1.7	13.9	13	13107.7	734.6
37303	33	40	7.0	2.4	13.1	13	13773.5	707.6
37402	32	40	8.0	3.2	15.0	15	14762.2	678.3
37502	31	40	9.0	4.1	17.4	17	15945.0	640.9
37602	31	40	9.0	5.0	18.5	18	17218.5	603.2
37701	32	40	8.0	5.8	18.0	18	18431.5	571.2
37802	32	40	8.0	6.6	18.6	18	19446.0	540.0
37902	31	40	9.0	7.5	20.8	20	20131.4	503.0
37999	33	40	7.0	8.2	19.1	19	20295.0	466.7
38097	31	40	9.0	9.1	22.2	22	19799.2	436.3
38201	28	40	12.0	10.3	27.7	27	18452.5	405.8
38302	31	40	9.0	11.2	25.3	25	16007.0	372.5
38398	32	40	8.0	12.0	24.2	24	12270.2	340.6
38501	33	40	7.0	12.7	23.4	23	7011.5	312.0
38602	35	40	5.0	13.2	21.1	21	1.9	284.2
38699	39	40	1.0	13.3	15.6	15	4.2	257.3
MTFD: Stopped with front dist = 39 and errcode = WALL_OFFSET_DIST_AHEAD
In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0)
Init hdg = 148.51 deg, Turn = 90.00  deg, tgt = -121.49 deg, timeout = 4.00 sec

Msec	Hdg	PrvHdg	dHdg	Rate	tgtDPS	err	Kp*err	Ival	Kd*Derr	speed	Match	Slope
38758	148.50	148.51	-0.00	0.05	45.00	44.95	31.46	13.48	0.00	44	0.50	0.50
38788	148.47	148.50	-0.03	1.01	45.00	43.99	30.79	26.68	-0.00	57	0.50	-0.00
38818	148.93	148.47	0.45	15.13	45.00	29.87	20.91	35.64	-0.00	56	0.50	0.00
38848	149.93	148.93	1.00	33.37	45.00	11.63	8.14	39.13	-0.00	47	0.51	0.01
38878	151.80	149.93	1.87	62.36	45.00	-17.36	-12.15	33.92	-0.00	21	0.52	0.01
38908	153.93	151.80	2.13	71.04	45.00	-26.04	-18.23	26.11	-0.00	7	0.53	0.01
38938	155.81	153.93	1.88	62.64	45.00	-17.64	-12.35	20.82	0.00	8	0.54	0.01
38968	157.28	155.81	1.47	48.98	45.00	-3.98	-2.79	19.62	0.00	16	0.55	0.01
38998	158.27	157.28	0.99	32.85	45.00	12.15	8.50	23.27	0.00	31	0.55	0.01
39028	159.01	158.27	0.74	24.74	45.00	20.26	14.19	29.35	0.00	43	0.56	0.00
39058	159.81	159.01	0.81	26.85	45.00	18.15	12.70	34.79	-0.00	47	0.56	0.00
39088	160.92	159.81	1.11	36.94	45.00	8.06	5.64	37.21	-0.00	42	0.57	0.01
39118	162.34	160.92	1.42	47.45	45.00	-2.45	-1.72	36.47	-0.00	34	0.58	0.01
39148	163.92	162.34	1.57	52.34	45.00	-7.34	-5.14	34.27	-0.00	29	0.59	0.01
39178	165.53	163.92	1.62	53.96	45.00	-8.96	-6.27	31.58	-0.00	25	0.59	0.01
39208	167.02	165.53	1.49	49.71	45.00	-4.71	-3.29	30.17	0.00	26	0.60	0.01
39238	168.33	167.02	1.30	43.40	45.00	1.60	1.12	30.65	0.00	31	0.61	0.01
39268	169.54	168.33	1.22	40.50	45.00	4.50	3.15	32.00	0.00	35	0.62	0.01
39298	170.75	169.54	1.21	40.37	45.00	4.63	3.24	33.39	0.00	36	0.62	0.01
39328	171.95	170.75	1.20	39.97	45.00	5.03	3.52	34.90	0.00	38	0.63	0.01
39358	173.25	171.95	1.30	43.34	45.00	1.66	1.16	35.40	-0.00	36	0.64	0.01
39388	174.78	173.25	1.53	50.96	45.00	-5.96	-4.17	33.61	-0.00	29	0.65	0.01
39418	176.31	174.78	1.53	51.11	45.00	-6.11	-4.28	31.78	-0.00	27	0.65	0.01
39448	177.86	176.31	1.55	51.69	45.00	-6.69	-4.69	29.77	-0.00	25	0.66	0.01
39478	179.33	177.86	1.46	48.74	45.00	-3.74	-2.62	28.65	0.00	26	0.67	0.01
39508	-179.48	179.33	1.19	39.81	45.00	5.19	3.64	30.21	0.00	33	0.68	0.01
39538	-178.42	-179.48	1.06	35.32	45.00	9.68	6.78	33.11	0.00	39	0.68	0.01
39568	-177.27	-178.42	1.15	38.26	45.00	6.74	4.72	35.14	-0.00	39	0.69	0.01
39598	-175.90	-177.27	1.37	45.81	45.00	-0.81	-0.57	34.89	-0.00	34	0.70	0.01
39628	-174.40	-175.90	1.50	50.03	45.00	-5.03	-3.52	33.38	-0.00	29	0.71	0.01
39658	-172.88	-174.40	1.51	50.42	45.00	-5.42	-3.80	31.76	-0.00	27	0.71	0.01
39688	-171.45	-172.88	1.43	47.82	45.00	-2.82	-1.97	30.91	0.00	28	0.72	0.01
39718	-170.21	-171.45	1.24	41.42	45.00	3.58	2.51	31.98	0.00	34	0.73	0.01
39748	-169.01	-170.21	1.19	39.78	45.00	5.22	3.66	33.55	0.00	37	0.74	0.01
39778	-167.66	-169.01	1.35	44.97	45.00	0.03	0.02	33.56	-0.00	33	0.74	0.01
39808	-166.16	-167.66	1.51	50.25	45.00	-5.25	-3.68	31.98	-0.00	28	0.75	0.01
39838	-164.72	-166.16	1.44	47.87	45.00	-2.87	-2.01	31.12	0.00	29	0.76	0.01
39868	-163.15	-164.72	1.57	52.41	45.00	-7.41	-5.19	28.90	-0.00	23	0.77	0.01
39898	-161.37	-163.15	1.78	59.37	45.00	-14.37	-10.06	24.59	-0.00	14	0.78	0.01
39928	-159.85	-161.37	1.51	50.40	45.00	-5.40	-3.78	22.97	0.00	19	0.79	0.01
39958	-159.05	-159.85	0.81	26.86	45.00	18.14	12.70	28.41	0.00	41	0.79	0.00
39988	-158.52	-159.05	0.53	17.77	45.00	27.23	19.06	36.58	0.00	55	0.79	0.00
40018	-157.22	-158.52	1.29	43.09	45.00	1.91	1.34	37.15	-0.00	38	0.80	0.01
40051	-155.56	-157.22	1.66	55.34	45.00	-10.34	-7.24	34.05	-0.00	26	0.81	0.01
40081	-153.97	-155.56	1.60	53.19	45.00	-8.19	-5.73	31.59	0.00	25	0.82	0.01
40111	-152.50	-153.97	1.47	49.00	45.00	-4.00	-2.80	30.39	0.00	27	0.83	0.01
40141	-151.30	-152.50	1.20	39.86	45.00	5.14	3.60	31.94	0.00	35	0.83	0.01
40171	-150.27	-151.30	1.03	34.50	45.00	10.50	7.35	35.09	0.00	42	0.84	0.01
40201	-149.28	-150.27	0.99	32.94	45.00	12.06	8.44	38.70	0.00	47	0.85	0.01
40231	-147.96	-149.28	1.32	43.98	45.00	1.02	0.72	39.01	-0.00	39	0.85	0.01
40261	-146.50	-147.96	1.46	48.71	45.00	-3.71	-2.59	37.90	-0.00	35	0.86	0.01
40291	-145.13	-146.50	1.37	45.61	45.00	-0.61	-0.43	37.72	0.00	37	0.87	0.01
40321	-143.85	-145.13	1.28	42.65	45.00	2.35	1.64	38.42	0.00	40	0.88	0.01
40351	-142.45	-143.85	1.40	46.52	45.00	-1.52	-1.06	37.97	-0.00	36	0.88	0.01
40381	-141.00	-142.45	1.45	48.45	45.00	-3.45	-2.41	36.93	-0.00	34	0.89	0.01
40411	-139.62	-141.00	1.38	45.99	45.00	-0.99	-0.69	36.64	0.00	35	0.90	0.01
40441	-138.23	-139.62	1.39	46.38	45.00	-1.38	-0.96	36.22	-0.00	35	0.91	0.01
40471	-136.79	-138.23	1.44	47.88	45.00	-2.88	-2.02	35.36	-0.00	33	0.92	0.01
40501	-135.39	-136.79	1.40	46.74	45.00	-1.74	-1.22	34.84	0.00	33	0.92	0.01
40531	-134.24	-135.39	1.15	38.30	45.00	6.70	4.69	36.85	0.00	41	0.93	0.01
40561	-132.98	-134.24	1.26	42.06	45.00	2.94	2.06	37.73	-0.00	39	0.94	0.01
40591	-131.67	-132.98	1.31	43.69	45.00	1.31	0.91	38.12	-0.00	39	0.94	0.01
40621	-130.14	-131.67	1.53	51.09	45.00	-6.09	-4.27	36.29	-0.00	32	0.95	0.01
40651	-128.64	-130.14	1.50	49.91	45.00	-4.91	-3.44	34.82	0.00	31	0.96	0.01
40681	-127.20	-128.64	1.44	47.92	45.00	-2.92	-2.04	33.94	0.00	31	0.97	0.01
40711	-125.79	-127.20	1.41	47.09	45.00	-2.09	-1.46	33.32	0.00	31	0.98	0.01
40741	-124.45	-125.79	1.34	44.72	45.00	0.28	0.19	33.40	0.00	33	0.98	0.01
40771	-123.15	-124.45	1.29	43.15	45.00	1.85	1.29	33.96	0.00	35	0.99	0.01
average turn rate = 43.3
BackupAndTurn90Deg finished
40776:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 8.04
IRHomingValTotalAvg = 109 
40802: glLeftCenterCm = -2080374784 glRightCenterCm = 0
glLeftCenterCm <= glRightCenterCm --> Calling TrackLeftWallOffset()

TrackLeftWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 350.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd	IRAvg
WallE3_Complete_V1.ino: In TrackLeftWallOffset: before while with errcode = NO_ANOMALIES

Msec	LF	LC	LR	LCCorr	Steer	Tweak	OffCm	Err	LastI	LastD	Out	Lspd	Rspd
40928	38.00	41.00	40.00	40.00	-0.13	0.00	0.00	0.13	0.00	0.13	45.50	120	29
41028	35.00	40.00	40.00	26.00	-0.54	-0.28	0.00	0.82	0.00	0.69	287.00	127	0
41128	33.00	31.00	37.00	24.00	-0.41	-0.32	0.00	0.73	0.00	-0.09	255.50	127	0
41227	38.00	37.00	29.00	15.00	0.81	-0.50	0.00	-0.31	0.00	-1.04	-108.50	0	127
41328	47.00	50.00	35.00	13.00	1.00	-0.54	0.00	-0.46	0.00	-0.15	-161.00	0	127
41428	54.00	55.00	48.00	33.00	0.59	-0.14	0.00	-0.45	0.00	0.01	-157.50	0	127
41528	53.00	55.00	49.00	42.00	0.42	0.04	0.00	-0.46	0.00	-0.01	-161.00	0	127
41627	51.00	54.00	49.00	52.00	0.16	0.24	0.00	-0.40	0.00	0.06	-140.00	0	127
41728	51.00	53.00	48.00	46.00	0.30	0.12	0.00	-0.42	0.00	-0.02	-147.00	0	127
41829	49.00	51.00	46.00	45.00	0.27	0.10	0.00	-0.37	0.00	0.05	-129.50	0	127
41926	50.00	51.00	48.00	49.00	0.15	0.18	0.00	-0.33	0.00	0.04	-115.50	0	127
42026	50.00	52.00	48.00	48.00	0.21	0.16	0.00	-0.37	0.00	-0.04	-129.50	0	127
42129	51.00	53.00	48.00	45.00	0.32	0.10	0.00	-0.42	0.00	-0.05	-147.00	0	127
42229	51.00	54.00	48.00	47.00	0.30	0.14	0.00	-0.44	0.00	-0.02	-154.00	0	127
42326	52.00	55.00	49.00	49.00	0.28	0.18	0.00	-0.46	0.00	-0.02	-161.00	0	127
42427	53.00	55.00	49.00	47.00	0.32	0.14	0.00	-0.46	0.00	0.00	-161.00	0	127
42529	51.00	54.00	49.00	51.00	0.18	0.22	0.00	-0.40	0.00	0.06	-140.00	0	127
42627	51.00	53.00	48.00	47.00	0.27	0.14	0.00	-0.41	0.00	-0.01	-143.50	0	127

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

12087: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 8.26

IRHomingValTotalAvg = 62

12105: glLeftCenterCm = -2097152000 glRightCenterCm = 0

glLeftCenterCm <= glRightCenterCm --> Calling TrackLeftWallOffset()

TrackLeftWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 350.00 0.00 0.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd IRAvg

WallE3_Complete_V1.ino: In TrackLeftWallOffset: before while with errcode = NO_ANOMALIES

Msec LF LC LR LCCorr Steer Tweak OffCm Err LastI LastD Out Lspd Rspd

12233 24.00 22.00 24.00 22.00 -0.03 -0.36 0.00 0.39 0.00 0.39 136.50 127 0

12333 24.00 22.00 24.00 22.00 0.02 -0.36 0.00 0.34 0.00 -0.05 119.00 127 0

12440 24.00 22.00 23.00 22.00 0.07 -0.36 0.00 0.29 0.00 -0.05 101.50 127 0

12533 24.00 22.00 23.00 21.00 0.13 -0.38 0.00 0.25 0.00 -0.04 87.50 127 0

12634 26.00 25.00 24.00 23.00 0.25 -0.34 0.00 0.09 0.00 -0.16 31.50 106 43

12732 28.00 27.00 25.00 23.00 0.32 -0.34 0.00 0.02 0.00 -0.07 7.00 82 68

12835 34.00 33.00 30.00 26.00 0.39 -0.28 0.00 -0.11 0.00 -0.13 -38.50 36 113

12932 39.00 39.00 35.00 28.00 0.47 -0.24 0.00 -0.23 0.00 -0.12 -80.50 0 127

13033 42.00 43.00 37.00 31.00 0.47 -0.18 0.00 -0.29 0.00 -0.06 -101.50 0 127

13136 43.00 44.00 38.00 32.00 0.47 -0.16 0.00 -0.31 0.00 -0.02 -108.50 0 127

13237 43.00 43.00 38.00 33.00 0.41 -0.14 0.00 -0.27 0.00 0.04 -94.50 0 127

13336 41.00 42.00 38.00 38.00 0.26 -0.04 0.00 -0.22 0.00 0.05 -77.00 0 127

13434 40.00 40.00 39.00 39.00 0.12 -0.02 0.00 -0.10 0.00 0.12 -35.00 40 110

13534 39.00 41.00 38.00 39.00 0.16 -0.02 0.00 -0.14 0.00 -0.04 -49.00 26 124

13633 41.00 42.00 41.00 42.00 0.04 0.04 0.00 -0.08 0.00 0.06 -28.00 47 103

13730 42.00 44.00 43.00 44.00 -0.02 0.08 0.00 -0.06 0.00 0.02 -21.00 54 96

13832 42.00 45.00 44.00 42.00 -0.20 0.04 0.00 0.16 0.00 0.22 56.00 127 19

13934 41.00 43.00 42.00 41.00 -0.16 0.02 0.00 0.14 0.00 -0.02 49.00 124 26

14029 38.00 41.00 41.00 37.00 -0.25 -0.06 0.00 0.31 0.00 0.17 108.50 127 0

14134 34.00 36.00 36.00 34.00 -0.17 -0.12 0.00 0.29 0.00 -0.02 101.50 127 0

14234 31.00 31.00 33.00 29.00 -0.19 -0.22 0.00 0.41 0.00 0.12 143.50 127 0

14341 30.00 28.00 29.00 28.00 0.08 -0.24 0.00 0.16 0.00 -0.25 56.00 127 19

14438 30.00 28.00 28.00 27.00 0.17 -0.26 0.00 0.09 0.00 -0.07 31.50 106 43

14533 30.00 29.00 27.00 25.00 0.29 -0.30 0.00 0.01 0.00 -0.08 3.50 78 71

14634 32.00 31.00 29.00 27.00 0.29 -0.26 0.00 -0.03 0.00 -0.04 -10.50 64 85

14733 33.00 32.00 30.00 29.00 0.23 -0.22 0.00 -0.01 0.00 0.02 -3.50 71 78

14837 34.00 35.00 32.00 30.00 0.31 -0.20 0.00 -0.11 0.00 -0.10 -38.50 36 113

14934 35.00 36.00 32.00 31.00 0.30 -0.18 0.00 -0.12 0.00 -0.01 -42.00 33 117

15033 36.00 37.00 34.00 34.00 0.24 -0.12 0.00 -0.12 0.00 0.00 -42.00 33 117

15134 36.00 37.00 35.00 36.00 0.16 -0.08 0.00 -0.08 0.00 0.04 -28.00 47 103

15236 37.00 37.00 35.00 35.00 0.17 -0.10 0.00 -0.07 0.00 0.01 -24.50 50 99

15335 36.00 37.00 35.00 36.00 0.14 -0.08 0.00 -0.06 0.00 0.01 -21.00 54 96

15441 37.00 37.00 35.00 36.00 0.13 -0.08 0.00 -0.05 0.00 0.01 -17.50 57 92

15541 37.00 36.00 35.00 34.00 0.18 -0.12 0.00 -0.06 0.00 -0.01 -21.00 53 96

15641 37.00 37.00 35.00 34.00 0.21 -0.12 0.00 -0.09 0.00 -0.03 -31.50 43 106

15738 37.00 37.00 35.00 36.00 0.16 -0.08 0.00 -0.08 0.00 0.01 -28.00 47 103

15840 36.00 36.00 35.00 35.00 0.11 -0.10 0.00 -0.01 0.00 0.07 -3.50 71 78

15934 36.00 36.00 36.00 36.00 0.04 -0.08 0.00 0.04 0.00 0.05 14.00 89 61

16041 35.00 36.00 34.00 36.00 0.07 -0.08 0.00 0.01 0.00 -0.03 3.50 78 71

16137 34.00 34.00 34.00 34.00 0.00 -0.12 0.00 0.12 0.00 0.11 42.00 117 33

16236 34.00 34.00 33.00 34.00 0.08 -0.12 0.00 0.04 0.00 -0.08 14.00 89 61

16338 33.00 32.00 32.00 31.00 0.10 -0.18 0.00 0.08 0.00 0.04 28.00 103 47

16441 32.00 32.00 32.00 32.00 0.05 -0.16 0.00 0.11 0.00 0.03 38.50 113 36

16538 32.00 32.00 30.00 30.00 0.18 -0.20 0.00 0.02 0.00 -0.09 7.00 82 68

16640 31.00 31.00 30.00 30.00 0.11 -0.20 0.00 0.09 0.00 0.07 31.50 106 43

16741 32.00 31.00 29.00 29.00 0.21 -0.22 0.00 0.01 0.00 -0.08 3.50 78 71

16841 31.00 31.00 30.00 30.00 0.17 -0.20 0.00 0.03 0.00 0.02 10.50 85 64

16941 31.00 31.00 30.00 30.00 0.16 -0.20 0.00 0.04 0.00 0.01 14.00 89 61

17041 32.00 31.00 29.00 28.00 0.25 -0.24 0.00 -0.01 0.00 -0.05 -3.50 71 78

17140 31.00 31.00 29.00 29.00 0.20 -0.22 0.00 0.02 0.00 0.03 7.00 82 68

17241 32.00 32.00 30.00 30.00 0.21 -0.20 0.00 -0.01 0.00 -0.03 -3.50 71 78

17340 32.00 32.00 30.00 30.00 0.20 -0.20 0.00 0.00 0.00 0.01 0.00 75 75

17441 31.00 31.00 30.00 30.00 0.15 -0.20 0.00 0.05 0.00 0.05 17.50 92 57

17541 32.00 33.00 30.00 32.00 0.13 -0.16 0.00 0.03 0.00 -0.02 10.50 85 64

17631 32.00 32.00 31.00 31.00 0.17 -0.18 0.00 0.01 0.00 -0.02 3.50 78 71

17736 33.00 32.00 30.00 29.00 0.23 -0.22 0.00 -0.01 0.00 -0.02 -3.50 71 78

17834 33.00 33.00 31.00 30.00 0.23 -0.20 0.00 -0.03 0.00 -0.02 -10.50 64 85

17932 33.00 33.00 31.00 31.00 0.19 -0.18 0.00 -0.01 0.00 0.02 -3.50 71 78

18034 34.00 34.00 31.00 30.00 0.29 -0.20 0.00 -0.09 0.00 -0.08 -31.50 43 106

18132 33.00 34.00 31.00 32.00 0.21 -0.16 0.00 -0.05 0.00 0.04 -17.50 57 92

18241 34.00 35.00 31.00 31.00 0.28 -0.18 0.00 -0.10 0.00 -0.05 -35.00 40 110

18341 34.00 35.00 32.00 33.00 0.20 -0.14 0.00 -0.06 0.00 0.04 -21.00 54 96

18441 34.00 34.00 31.00 30.00 0.29 -0.20 0.00 -0.09 0.00 -0.03 -31.50 43 106

18541 33.00 33.00 32.00 31.00 0.17 -0.18 0.00 0.01 0.00 0.10 3.50 78 71

18640 33.00 34.00 32.00 33.00 0.14 -0.14 0.00 0.00 0.00 -0.01 0.00 75 75

18734 33.00 33.00 32.00 32.00 0.14 -0.16 0.00 0.02 0.00 0.02 7.00 82 68

18840 32.00 33.00 30.00 31.00 0.17 -0.18 0.00 0.01 0.00 -0.01 3.50 78 71

18938 32.00 32.00 30.00 30.00 0.20 -0.20 0.00 0.00 0.00 -0.01 0.00 75 75

19036 31.00 32.00 29.00 30.00 0.19 -0.20 0.00 0.01 0.00 0.01 3.50 78 71

19138 31.00 31.00 29.00 30.00 0.14 -0.20 0.00 0.06 0.00 0.05 21.00 96 54

19240 31.00 31.00 28.00 29.00 0.21 -0.22 0.00 0.01 0.00 -0.05 3.50 78 71

19341 30.00 30.00 29.00 30.00 0.04 -0.20 0.00 0.16 0.00 0.15 56.00 127 19

19441 29.00 30.00 28.00 29.00 0.16 -0.22 0.00 0.06 0.00 -0.10 21.00 96 54

19541 28.00 29.00 27.00 28.00 0.14 -0.24 0.00 0.10 0.00 0.04 35.00 110 40

19641 29.00 29.00 27.00 27.00 0.21 -0.26 0.00 0.05 0.00 -0.05 17.50 92 57

19741 29.00 29.00 26.00 26.00 0.26 -0.28 0.00 0.02 0.00 -0.03 7.00 82 68

19841 29.00 30.00 27.00 28.00 0.23 -0.24 0.00 0.01 0.00 -0.01 3.50 78 71

19941 29.00 30.00 27.00 28.00 0.20 -0.24 0.00 0.04 0.00 0.03 14.00 89 61

20041 30.00 30.00 27.00 26.00 0.29 -0.28 0.00 -0.01 0.00 -0.05 -3.50 71 78

20141 30.00 31.00 27.00 28.00 0.27 -0.24 0.00 -0.03 0.00 -0.02 -10.50 64 85

20237 30.00 31.00 28.00 29.00 0.22 -0.22 0.00 0.00 0.00 0.03 0.00 75 75

20337 33.00 32.00 29.00 24.00 0.43 -0.32 0.00 -0.11 0.00 -0.11 -38.50 36 113

20441 38.00 39.00 30.00 16.00 0.79 -0.48 0.00 -0.31 0.00 -0.20 -108.50 0 127

20541 40.00 42.00 33.00 22.00 0.68 -0.36 0.00 -0.32 0.00 -0.01 -112.00 0 127

20641 43.00 45.00 37.00 26.00 0.61 -0.28 0.00 -0.33 0.00 -0.01 -115.50 0 127

20734 43.00 44.00 38.00 31.00 0.48 -0.18 0.00 -0.30 0.00 0.03 -105.00 0 127

20832 42.00 44.00 37.00 30.00 0.51 -0.20 0.00 -0.31 0.00 -0.01 -108.50 0 127

20933 40.00 44.00 37.00 40.00 0.25 0.00 0.00 -0.25 0.00 0.06 -87.50 0 127

21041 39.00 42.00 38.00 41.00 0.09 0.02 0.00 -0.11 0.00 0.14 -38.50 36 113

21140 40.00 42.00 41.00 40.00 -0.19 0.00 0.00 0.19 0.00 0.30 66.50 127 8

21233 43.00 48.00 50.00 24.00 -0.69 -0.32 0.00 1.01 0.00 0.82 353.50 127 0

21336 40.00 43.00 42.00 39.00 -0.26 -0.02 0.00 0.28 0.00 -0.73 98.00 127 0

21438 38.00 40.00 36.00 39.00 0.11 -0.02 0.00 -0.09 0.00 -0.37 -31.50 43 106

21538 38.00 39.00 35.00 33.00 0.33 -0.14 0.00 -0.19 0.00 -0.10 -66.50 8 127

21639 38.00 39.00 35.00 32.00 0.36 -0.16 0.00 -0.20 0.00 -0.01 -70.00 4 127

21740 37.00 40.00 36.00 39.00 0.10 -0.02 0.00 -0.08 0.00 0.12 -28.00 47 103

21833 38.00 39.00 36.00 37.00 0.16 -0.06 0.00 -0.10 0.00 -0.02 -35.00 40 110

21931 37.00 39.00 37.00 39.00 0.03 -0.02 0.00 -0.01 0.00 0.09 -3.50 71 78

22031 36.00 39.00 37.00 39.00 -0.05 -0.02 0.00 0.07 0.00 0.08 24.50 99 50

22133 34.00 38.00 35.00 37.00 -0.14 -0.06 0.00 0.20 0.00 0.13 70.00 127 5

22231 34.00 36.00 33.00 36.00 0.09 -0.08 0.00 -0.01 0.00 -0.21 -3.50 71 78

22335 32.00 33.00 32.00 33.00 -0.01 -0.14 0.00 0.15 0.00 0.16 52.50 127 22

22432 31.00 33.00 30.00 32.00 0.12 -0.16 0.00 0.04 0.00 -0.11 14.00 89 61

22532 29.00 30.00 28.00 30.00 0.08 -0.20 0.00 0.12 0.00 0.08 42.00 117 33

22635 29.00 30.00 28.00 29.00 0.14 -0.22 0.00 0.08 0.00 -0.04 28.00 103 47

22734 28.00 29.00 26.00 28.00 0.15 -0.24 0.00 0.09 0.00 0.01 31.50 106 43

22841 27.00 28.00 25.00 27.00 0.13 -0.26 0.00 0.13 0.00 0.04 45.50 120 29

22940 26.00 28.00 25.00 26.00 0.19 -0.28 0.00 0.09 0.00 -0.04 31.50 106 43

23041 27.00 27.00 25.00 25.00 0.20 -0.30 0.00 0.10 0.00 0.01 35.00 110 39

23141 26.00 29.00 24.00 27.00 0.19 -0.26 0.00 0.07 0.00 -0.03 24.50 99 50

23241 27.00 29.00 25.00 28.00 0.18 -0.24 0.00 0.06 0.00 -0.01 21.00 96 54

23341 28.00 30.00 26.00 28.00 0.21 -0.24 0.00 0.03 0.00 -0.03 10.50 85 64

23436 28.00 30.00 26.00 28.00 0.20 -0.24 0.00 0.04 0.00 0.01 14.00 89 61

23541 30.00 32.00 27.00 29.00 0.24 -0.22 0.00 -0.02 0.00 -0.06 -7.00 68 82

23641 31.00 32.00 29.00 31.00 0.16 -0.18 0.00 0.02 0.00 0.04 7.00 82 68

23737 32.00 34.00 30.00 31.00 0.23 -0.18 0.00 -0.05 0.00 -0.07 -17.50 57 92

23839 32.00 36.00 30.00 33.00 0.23 -0.14 0.00 -0.09 0.00 -0.04 -31.50 43 106

23935 33.00 36.00 31.00 33.00 0.25 -0.14 0.00 -0.11 0.00 -0.02 -38.50 36 113

24041 34.00 37.00 32.00 34.00 0.24 -0.12 0.00 -0.12 0.00 -0.01 -42.00 33 117

24141 34.00 37.00 32.00 34.00 0.23 -0.12 0.00 -0.11 0.00 0.01 -38.50 36 113

24238 35.00 38.00 32.00 34.00 0.28 -0.12 0.00 -0.16 0.00 -0.05 -56.00 19 127

24337 34.00 39.00 32.00 36.00 0.21 -0.08 0.00 -0.13 0.00 0.03 -45.50 29 120

24432 35.00 37.00 35.00 37.00 0.01 -0.06 0.00 0.05 0.00 0.18 17.50 92 57

24531 35.00 37.00 34.00 36.00 0.14 -0.08 0.00 -0.06 0.00 -0.11 -21.00 54 96

24634 34.00 36.00 34.00 36.00 -0.05 -0.08 0.00 0.13 0.00 0.19 45.50 120 29

24736 33.00 36.00 32.00 36.00 0.06 -0.08 0.00 0.02 0.00 -0.11 7.00 82 68

24834 31.00 35.00 31.00 35.00 0.03 -0.10 0.00 0.07 0.00 0.05 24.50 99 50

24934 31.00 33.00 30.00 33.00 0.07 -0.14 0.00 0.07 0.00 0.00 24.50 99 50

25036 29.00 32.00 28.00 32.00 0.02 -0.16 0.00 0.14 0.00 0.07 49.00 124 26

25136 28.00 30.00 27.00 30.00 0.06 -0.20 0.00 0.14 0.00 0.00 49.00 124 26

25234 41.00 41.00 29.00 10.00 1.00 -0.60 0.00 -0.40 0.00 -0.54 -140.00 0 127

25338 45.00 50.00 41.00 37.00 0.44 -0.06 0.00 -0.38 0.00 0.02 -133.00 0 127

25434 45.00 49.00 43.00 48.00 0.13 0.16 0.00 -0.29 0.00 0.09 -101.50 0 127

25535 44.00 49.00 43.00 48.00 0.12 0.16 0.00 -0.28 0.00 0.01 -98.00 0 127

25635 45.00 50.00 44.00 49.00 0.08 0.18 0.00 -0.26 0.00 0.02 -91.00 0 127

25731 48.00 51.00 49.00 51.00 -0.07 0.22 0.00 -0.15 0.00 0.11 -52.50 22 127

25829 51.00 54.00 45.00 33.00 0.58 -0.14 0.00 -0.44 0.00 -0.29 -154.00 0 127

25931 50.00 52.00 43.00 26.00 0.70 -0.28 0.00 -0.42 0.00 0.02 -147.00 0 127

26029 44.00 48.00 42.00 45.00 0.21 0.10 0.00 -0.31 0.00 0.11 -108.50 0 127

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Left

In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0)

Init hdg = -95.99 deg, Turn = 90.00 deg, tgt = -5.99 deg, timeout = 4.00 sec

Msec Hdg PrvHdg dHdg Rate tgtDPS err Kp*err Ival Kd*Derr speed Match Slope

26094 -100.46 -95.99 -4.47 149.05 45.00 -104.05 -72.84 -31.22 -0.00 0 0.48 0.48

26124 -103.58 -100.46 -3.11 103.73 45.00 -58.73 -41.11 -48.83 0.00 0 0.46 -0.02

26154 -106.23 -103.58 -2.66 88.63 45.00 -43.63 -30.54 -61.92 0.00 0 0.44 -0.01

26184 -107.97 -106.23 -1.74 57.89 45.00 -12.89 -9.02 -65.79 0.00 0 0.43 -0.01

26214 -108.71 -107.97 -0.74 24.58 45.00 20.42 14.30 -59.66 0.00 0 0.43 -0.00

26244 -108.85 -108.71 -0.15 4.85 45.00 40.15 28.10 -47.62 0.00 0 0.43 -0.00

26274 -109.03 -108.85 -0.18 5.90 45.00 39.10 27.37 -35.89 -0.00 0 0.43 -0.00

26304 -109.03 -109.03 0.00 0.02 45.00 44.98 31.48 -22.39 0.00 9 0.43 0.00

26334 -108.99 -109.03 0.04 1.36 45.00 43.64 30.55 -9.30 -0.00 21 0.43 0.00

26364 -108.99 -108.99 0.00 0.08 45.00 44.92 31.45 4.18 0.00 35 0.43 0.00

26394 -108.84 -108.99 0.15 4.90 45.00 40.10 28.07 16.21 -0.00 44 0.43 0.00

26424 -108.54 -108.84 0.30 9.91 45.00 35.09 24.56 26.73 -0.00 51 0.43 0.00

26454 -107.46 -108.54 1.08 36.00 45.00 9.00 6.30 29.43 -0.00 35 0.44 0.01

26484 -105.94 -107.46 1.52 50.79 45.00 -5.79 -4.05 27.70 -0.00 23 0.44 0.01

26517 -104.37 -105.94 1.57 52.18 45.00 -7.18 -5.02 25.54 -0.00 20 0.45 0.01

26547 -102.98 -104.37 1.39 46.34 45.00 -1.34 -0.94 25.14 0.00 24 0.46 0.01

26577 -101.85 -102.98 1.14 37.90 45.00 7.10 4.97 27.27 0.00 32 0.47 0.01

26607 -100.90 -101.85 0.95 31.70 45.00 13.30 9.31 31.26 0.00 40 0.47 0.01

26637 -99.91 -100.90 0.98 32.72 45.00 12.28 8.59 34.94 -0.00 43 0.48 0.01

26667 -98.77 -99.91 1.14 38.01 45.00 6.99 4.90 37.04 -0.00 41 0.48 0.01

26697 -97.38 -98.77 1.39 46.40 45.00 -1.40 -0.98 36.62 -0.00 35 0.49 0.01

26727 -95.87 -97.38 1.51 50.46 45.00 -5.46 -3.82 34.98 -0.00 31 0.50 0.01

26757 -94.36 -95.87 1.51 50.41 45.00 -5.41 -3.79 33.36 0.00 29 0.51 0.01

26787 -92.85 -94.36 1.50 50.06 45.00 -5.06 -3.54 31.84 0.00 28 0.52 0.01

26817 -91.48 -92.85 1.37 45.78 45.00 -0.78 -0.55 31.61 0.00 31 0.53 0.01

26847 -90.20 -91.48 1.28 42.66 45.00 2.34 1.64 32.31 0.00 33 0.53 0.01

26877 -88.96 -90.20 1.24 41.21 45.00 3.79 2.66 33.45 0.00 36 0.54 0.01

26907 -87.73 -88.96 1.23 41.12 45.00 3.88 2.72 34.61 0.00 37 0.55 0.01

26937 -86.44 -87.73 1.29 43.13 45.00 1.87 1.31 35.17 -0.00 36 0.55 0.01

26967 -85.06 -86.44 1.38 45.97 45.00 -0.97 -0.68 34.88 -0.00 34 0.56 0.01

26997 -83.65 -85.06 1.40 46.83 45.00 -1.83 -1.28 34.33 -0.00 33 0.57 0.01

27027 -82.24 -83.65 1.41 47.14 45.00 -2.14 -1.50 33.69 -0.00 32 0.58 0.01

27057 -80.82 -82.24 1.42 47.24 45.00 -2.24 -1.57 33.02 -0.00 31 0.58 0.01

27087 -79.34 -80.82 1.48 49.31 45.00 -4.31 -3.02 31.73 -0.00 28 0.59 0.01

27117 -77.89 -79.34 1.45 48.35 45.00 -3.35 -2.35 30.73 0.00 28 0.60 0.01

27147 -76.50 -77.89 1.40 46.53 45.00 -1.53 -1.07 30.27 0.00 29 0.61 0.01

27177 -75.24 -76.50 1.25 41.74 45.00 3.26 2.28 31.24 0.00 33 0.62 0.01

27207 -74.06 -75.24 1.18 39.37 45.00 5.63 3.94 32.93 0.00 36 0.62 0.01

27237 -72.89 -74.06 1.17 38.97 45.00 6.03 4.22 34.74 0.00 38 0.63 0.01

27267 -71.64 -72.89 1.26 41.91 45.00 3.09 2.16 35.67 -0.00 37 0.64 0.01

27297 -70.21 -71.64 1.43 47.56 45.00 -2.56 -1.79 34.90 -0.00 33 0.64 0.01

27327 -68.60 -70.21 1.61 53.74 45.00 -8.74 -6.12 32.28 -0.00 26 0.65 0.01

27357 -66.99 -68.60 1.60 53.45 45.00 -8.45 -5.92 29.74 0.00 23 0.66 0.01

27387 -65.66 -66.99 1.33 44.39 45.00 0.61 0.42 29.92 0.00 30 0.67 0.01

27417 -64.52 -65.66 1.15 38.21 45.00 6.79 4.75 31.96 0.00 36 0.67 0.01

27447 -63.41 -64.52 1.11 36.90 45.00 8.10 5.67 34.39 0.00 40 0.68 0.01

27477 -62.24 -63.41 1.17 39.09 45.00 5.91 4.14 36.16 -0.00 40 0.69 0.01

27507 -60.97 -62.24 1.26 42.15 45.00 2.85 1.99 37.02 -0.00 39 0.69 0.01

27537 -59.56 -60.97 1.41 47.01 45.00 -2.01 -1.41 36.41 -0.00 35 0.70 0.01

27567 -58.01 -59.56 1.55 51.59 45.00 -6.59 -4.62 34.43 -0.00 29 0.71 0.01

27597 -56.47 -58.01 1.54 51.45 45.00 -6.45 -4.52 32.50 0.00 27 0.72 0.01

27627 -55.06 -56.47 1.41 46.98 45.00 -1.98 -1.39 31.90 0.00 30 0.73 0.01

27657 -53.70 -55.06 1.36 45.28 45.00 -0.28 -0.20 31.82 0.00 31 0.73 0.01

27687 -52.42 -53.70 1.29 42.85 45.00 2.15 1.50 32.46 0.00 33 0.74 0.01

27717 -51.17 -52.42 1.25 41.62 45.00 3.38 2.37 33.48 0.00 35 0.75 0.01

27747 -49.87 -51.17 1.30 43.25 45.00 1.75 1.23 34.00 -0.00 35 0.76 0.01

27777 -48.55 -49.87 1.32 43.91 45.00 1.09 0.76 34.33 -0.00 35 0.76 0.01

27807 -47.18 -48.55 1.37 45.83 45.00 -0.83 -0.58 34.08 -0.00 33 0.77 0.01

27837 -45.75 -47.18 1.43 47.56 45.00 -2.56 -1.79 33.31 -0.00 31 0.78 0.01

27867 -44.35 -45.75 1.40 46.76 45.00 -1.76 -1.23 32.79 0.00 31 0.79 0.01

27897 -42.98 -44.35 1.37 45.67 45.00 -0.67 -0.47 32.59 0.00 32 0.79 0.01

27927 -41.64 -42.98 1.34 44.66 45.00 0.34 0.24 32.69 0.00 32 0.80 0.01

27957 -40.38 -41.64 1.26 42.00 45.00 3.00 2.10 33.59 0.00 35 0.81 0.01

27987 -39.09 -40.38 1.29 43.00 45.00 2.00 1.40 34.19 -0.00 35 0.82 0.01

28017 -37.77 -39.09 1.32 43.87 45.00 1.13 0.79 34.52 -0.00 35 0.82 0.01

28047 -36.43 -37.77 1.34 44.76 45.00 0.24 0.17 34.60 -0.00 34 0.83 0.01

28077 -35.04 -36.43 1.38 46.14 45.00 -1.14 -0.80 34.25 -0.00 33 0.84 0.01

28107 -33.68 -35.04 1.37 45.66 45.00 -0.66 -0.46 34.05 0.00 33 0.85 0.01

28137 -32.40 -33.68 1.27 42.36 45.00 2.64 1.85 34.85 0.00 36 0.85 0.01

28167 -31.14 -32.40 1.27 42.27 45.00 2.73 1.91 35.67 0.00 37 0.86 0.01

28197 -29.83 -31.14 1.30 43.40 45.00 1.60 1.12 36.15 -0.00 37 0.87 0.01

28227 -28.38 -29.83 1.45 48.40 45.00 -3.40 -2.38 35.13 -0.00 32 0.88 0.01

28257 -26.85 -28.38 1.53 51.08 45.00 -6.08 -4.26 33.30 -0.00 29 0.88 0.01

28287 -25.38 -26.85 1.47 48.84 45.00 -3.84 -2.69 32.15 0.00 29 0.89 0.01

28317 -24.01 -25.38 1.37 45.74 45.00 -0.74 -0.52 31.93 0.00 31 0.90 0.01

28347 -22.68 -24.01 1.33 44.28 45.00 0.72 0.50 32.15 0.00 32 0.91 0.01

28377 -21.40 -22.68 1.28 42.68 45.00 2.32 1.62 32.84 0.00 34 0.91 0.01

28407 -20.12 -21.40 1.28 42.62 45.00 2.38 1.66 33.55 0.00 35 0.92 0.01

28437 -18.80 -20.12 1.32 44.10 45.00 0.90 0.63 33.82 -0.00 34 0.93 0.01

28467 -17.43 -18.80 1.37 45.64 45.00 -0.64 -0.45 33.63 -0.00 33 0.94 0.01

28497 -16.02 -17.43 1.42 47.22 45.00 -2.22 -1.55 32.96 -0.00 31 0.94 0.01

28527 -14.61 -16.02 1.40 46.83 45.00 -1.83 -1.28 32.41 0.00 31 0.95 0.01

28557 -13.23 -14.61 1.38 46.07 45.00 -1.07 -0.75 32.09 0.00 31 0.96 0.01

28587 -11.94 -13.23 1.29 42.89 45.00 2.11 1.48 32.73 0.00 34 0.97 0.01

28617 -10.72 -11.94 1.22 40.83 45.00 4.17 2.92 33.98 0.00 36 0.97 0.01

28647 -9.42 -10.72 1.29 43.09 45.00 1.91 1.34 34.55 -0.00 35 0.98 0.01

28677 -8.01 -9.42 1.41 47.03 45.00 -2.03 -1.42 33.94 -0.00 32 0.99 0.01

28707 -6.65 -8.01 1.37 45.62 45.00 -0.62 -0.43 33.76 0.00 33 1.00 0.01

average turn rate = 43.7

28710: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 8.06

IRHomingValTotalAvg = 98

28732: glLeftCenterCm = -2080374784 glRightCenterCm = 0

glLeftCenterCm <= glRightCenterCm --> Calling TrackLeftWallOffset()

TrackLeftWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 350.00 0.00 0.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd IRAvg

WallE3_Complete_V1.ino: In TrackLeftWallOffset: before while with errcode = NO_ANOMALIES

Msec LF LC LR LCCorr Steer Tweak OffCm Err LastI LastD Out Lspd Rspd

28859 35.00 38.00 32.00 31.00 0.36 -0.18 0.00 -0.18 0.00 -0.18 -63.00 11 127

28959 35.00 38.00 32.00 34.00 0.27 -0.12 0.00 -0.15 0.00 0.03 -52.50 22 127

29059 34.00 38.00 31.00 33.00 0.31 -0.14 0.00 -0.17 0.00 -0.02 -59.50 15 127

29159 34.00 37.00 32.00 35.00 0.19 -0.10 0.00 -0.09 0.00 0.08 -31.50 43 106

29264 33.00 36.00 32.00 35.00 0.13 -0.10 0.00 -0.03 0.00 0.06 -10.50 64 85

29367 33.00 36.00 33.00 36.00 0.06 -0.08 0.00 0.02 0.00 0.05 7.00 82 68

29467 34.00 36.00 33.00 35.00 0.13 -0.10 0.00 -0.03 0.00 -0.05 -10.50 64 85

29567 33.00 36.00 32.00 35.00 0.10 -0.10 0.00 0.00 0.00 0.03 0.00 75 75

29666 33.00 35.00 32.00 35.00 0.07 -0.10 0.00 0.03 0.00 0.03 10.50 85 64

29767 32.00 35.00 32.00 35.00 0.05 -0.10 0.00 0.05 0.00 0.02 17.50 92 57

29862 32.00 35.00 30.00 33.00 0.17 -0.14 0.00 -0.03 0.00 -0.08 -10.50 64 85

29962 31.00 34.00 30.00 34.00 0.08 -0.12 0.00 0.04 0.00 0.07 14.00 89 61

30065 25.00 29.00 29.00 21.00 -0.47 -0.38 0.00 0.85 0.00 0.81 297.50 127 0

30164 22.00 23.00 25.00 21.00 -0.26 -0.38 0.00 0.64 0.00 -0.21 224.00 127 0

30267 24.00 26.00 22.00 24.00 0.20 -0.32 0.00 0.12 0.00 -0.52 42.00 117 33

30366 26.00 27.00 24.00 26.00 0.18 -0.28 0.00 0.10 0.00 -0.02 35.00 110 40

30462 25.00 28.00 24.00 28.00 0.09 -0.24 0.00 0.15 0.00 0.05 52.50 127 22

30564 29.00 35.00 25.00 28.00 0.38 -0.24 0.00 -0.14 0.00 -0.29 -49.00 26 124

30661 749.00 824.00 705.00 207.00 1.00 3.34 0.00 -4.34 0.00 -4.20 -1519.00 0 127

30762 749.00 824.00 705.00 207.00 1.00 3.34 0.00 -4.34 0.00 0.00 -1519.00 0 127

30866 749.00 824.00 705.00 207.00 1.00 3.34 0.00 -4.34 0.00 0.00 -1519.00 0 127

30965 749.00 824.00 705.00 207.00 1.00 3.34 0.00 -4.34 0.00 0.00 -1519.00 0 127

31067 749.00 824.00 705.00 207.00 1.00 3.34 0.00 -4.34 0.00 0.00 -1519.00 0 127

31161 749.00 824.00 47.00 207.00 1.00 3.34 0.00 -4.34 0.00 0.00 -1519.00 0 127

31258 55.00 58.00 48.00 32.00 0.65 -0.16 0.00 -0.49 0.00 3.85 -171.50 0 127

31359 49.00 49.00 43.00 33.00 0.52 -0.14 0.00 -0.38 0.00 0.11 -133.00 0 127

31460 46.00 50.00 48.00 45.00 -0.26 0.10 0.00 0.16 0.00 0.54 56.00 127 19

31556 52.00 58.00 56.00 44.00 -0.43 0.08 0.00 0.35 0.00 0.19 122.50 127 0

31660 53.00 57.00 55.00 54.00 -0.19 0.28 0.00 -0.09 0.00 -0.44 -31.50 43 106

31757 46.00 50.00 48.00 46.00 -0.23 0.12 0.00 0.11 0.00 0.20 38.50 113 36

31861 43.00 47.00 44.00 46.00 -0.11 0.12 0.00 -0.01 0.00 -0.12 -3.50 71 78

31959 48.00 50.00 43.00 34.00 0.50 -0.12 0.00 -0.38 0.00 -0.37 -133.00 0 127

32056 49.00 53.00 44.00 36.00 0.50 -0.08 0.00 -0.42 0.00 -0.04 -147.00 0 127

32160 50.00 53.00 47.00 46.00 0.30 0.12 0.00 -0.42 0.00 0.00 -147.00 0 127

32259 50.00 53.00 48.00 48.00 0.25 0.16 0.00 -0.41 0.00 0.01 -143.50 0 127

32357 50.00 54.00 47.00 48.00 0.26 0.16 0.00 -0.42 0.00 -0.01 -147.00 0 127

32458 49.00 53.00 48.00 52.00 0.11 0.24 0.00 -0.35 0.00 0.07 -122.50 0 127

32558 49.00 53.00 49.00 53.00 0.02 0.26 0.00 -0.28 0.00 0.07 -98.00 0 127

32658 50.00 56.00 705.00 14.00 -1.00 -0.52 0.00 1.52 0.00 1.80 532.00 127 0

32762 749.00 824.00 705.00 207.00 1.00 3.34 0.00 -4.34 0.00 -5.86 -1519.00 0 127

32858 45.00 824.00 705.00 207.00 -1.00 3.34 0.00 -2.34 0.00 2.00 -819.00 0 127

32956 50.00 56.00 705.00 14.00 -1.00 -0.52 0.00 1.52 0.00 3.86 532.00 127 0

33061 50.00 49.00 705.00 12.00 -1.00 -0.56 0.00 1.56 0.00 0.04 546.00 127 0

33161 47.00 51.00 51.00 36.00 -0.49 -0.08 0.00 0.57 0.00 -0.99 199.50 127 0

33256 43.00 47.00 43.00 47.00 0.05 0.14 0.00 -0.19 0.00 -0.76 -66.50 8 127

33360 40.00 42.00 41.00 40.00 -0.15 0.00 0.00 0.15 0.00 0.34 52.50 127 22

33460 37.00 39.00 37.00 39.00 -0.05 -0.02 0.00 0.07 0.00 -0.08 24.50 99 50

33560 36.00 39.00 34.00 38.00 0.14 -0.04 0.00 -0.10 0.00 -0.17 -35.00 40 110

33661 35.00 38.00 32.00 34.00 0.27 -0.12 0.00 -0.15 0.00 -0.05 -52.50 22 127

33758 34.00 38.00 33.00 36.00 0.17 -0.08 0.00 -0.09 0.00 0.06 -31.50 43 106

33858 34.00 37.00 32.00 35.00 0.20 -0.10 0.00 -0.10 0.00 -0.01 -35.00 40 110

33957 33.00 36.00 33.00 36.00 0.03 -0.08 0.00 0.05 0.00 0.15 17.50 92 57

34061 32.00 35.00 32.00 35.00 0.00 -0.10 0.00 0.10 0.00 0.05 35.00 110 40

34160 31.00 34.00 30.00 34.00 0.07 -0.12 0.00 0.05 0.00 -0.05 17.50 92 57

34257 29.00 32.00 28.00 32.00 0.06 -0.16 0.00 0.10 0.00 0.05 35.00 110 40

34358 27.00 30.00 26.00 30.00 0.05 -0.20 0.00 0.15 0.00 0.05 52.50 127 22

34457 25.00 28.00 25.00 28.00 0.04 -0.24 0.00 0.20 0.00 0.05 70.00 127 5

34559 24.00 27.00 22.00 26.00 0.14 -0.28 0.00 0.14 0.00 -0.06 49.00 124 26

34661 22.00 25.00 21.00 25.00 0.11 -0.30 0.00 0.19 0.00 0.05 66.50 127 8

34760 21.00 24.00 20.00 23.00 0.16 -0.34 0.00 0.18 0.00 -0.01 63.00 127 11

34858 22.00 23.00 20.00 21.00 0.22 -0.38 0.00 0.16 0.00 -0.02 56.00 127 19

34960 38.00 41.00 26.00 10.00 1.00 -0.60 0.00 -0.40 0.00 -0.56 -140.00 0 127

35058 40.00 43.00 37.00 37.00 0.31 -0.06 0.00 -0.25 0.00 0.15 -87.50 0 127

35156 41.00 44.00 37.00 34.00 0.41 -0.12 0.00 -0.29 0.00 -0.04 -101.50 0 127

35259 42.00 44.00 39.00 40.00 0.26 0.00 0.00 -0.26 0.00 0.03 -91.00 0 127

35357 42.00 45.00 39.00 40.00 0.27 0.00 0.00 -0.27 0.00 -0.01 -94.50 0 127

35456 41.00 45.00 40.00 44.00 0.11 0.08 0.00 -0.19 0.00 0.08 -66.50 8 127

35561 42.00 46.00 42.00 46.00 -0.01 0.12 0.00 -0.11 0.00 0.08 -38.50 36 113

35660 44.00 48.00 44.00 48.00 0.01 0.16 0.00 -0.17 0.00 -0.06 -59.50 15 127

35758 48.00 51.00 44.00 42.00 0.35 0.04 0.00 -0.39 0.00 -0.22 -136.50 0 127

35857 48.00 51.00 43.00 34.00 0.52 -0.12 0.00 -0.40 0.00 -0.01 -140.00 0 127

35956 45.00 49.00 43.00 45.00 0.22 0.10 0.00 -0.32 0.00 0.08 -112.00 0 127

In HandleAnomalousConditions with OBSTACLE_AHEAD error code detected

OBSTACLE_AHEAD case detected with tracking case Left

In BackupAndTurn90Deg(0, 1, 200)with front distance = 32

MoveToDesiredFrontDistCm(40): Kp/Ki/Kd= 1.5/0.1/0.2

MTFD: at start, tgt = 40cm, curr_dist = 32, front/rear var = 13328.0/833.0

Msec Fdist TgtD err Ival Out Speed FVar RVar

37101 31 40 9.0 0.9 12.6 12 12802.2 762.9

37201 32 40 8.0 1.7 13.9 13 13107.7 734.6

37303 33 40 7.0 2.4 13.1 13 13773.5 707.6

37402 32 40 8.0 3.2 15.0 15 14762.2 678.3

37502 31 40 9.0 4.1 17.4 17 15945.0 640.9

37602 31 40 9.0 5.0 18.5 18 17218.5 603.2

37701 32 40 8.0 5.8 18.0 18 18431.5 571.2

37802 32 40 8.0 6.6 18.6 18 19446.0 540.0

37902 31 40 9.0 7.5 20.8 20 20131.4 503.0

37999 33 40 7.0 8.2 19.1 19 20295.0 466.7

38097 31 40 9.0 9.1 22.2 22 19799.2 436.3

38201 28 40 12.0 10.3 27.7 27 18452.5 405.8

38302 31 40 9.0 11.2 25.3 25 16007.0 372.5

38398 32 40 8.0 12.0 24.2 24 12270.2 340.6

38501 33 40 7.0 12.7 23.4 23 7011.5 312.0

38602 35 40 5.0 13.2 21.1 21 1.9 284.2

38699 39 40 1.0 13.3 15.6 15 4.2 257.3

MTFD: Stopped with front dist = 39 and errcode = WALL_OFFSET_DIST_AHEAD

In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0)

Init hdg = 148.51 deg, Turn = 90.00 deg, tgt = -121.49 deg, timeout = 4.00 sec

Msec Hdg PrvHdg dHdg Rate tgtDPS err Kp*err Ival Kd*Derr speed Match Slope

38758 148.50 148.51 -0.00 0.05 45.00 44.95 31.46 13.48 0.00 44 0.50 0.50

38788 148.47 148.50 -0.03 1.01 45.00 43.99 30.79 26.68 -0.00 57 0.50 -0.00

38818 148.93 148.47 0.45 15.13 45.00 29.87 20.91 35.64 -0.00 56 0.50 0.00

38848 149.93 148.93 1.00 33.37 45.00 11.63 8.14 39.13 -0.00 47 0.51 0.01

38878 151.80 149.93 1.87 62.36 45.00 -17.36 -12.15 33.92 -0.00 21 0.52 0.01

38908 153.93 151.80 2.13 71.04 45.00 -26.04 -18.23 26.11 -0.00 7 0.53 0.01

38938 155.81 153.93 1.88 62.64 45.00 -17.64 -12.35 20.82 0.00 8 0.54 0.01

38968 157.28 155.81 1.47 48.98 45.00 -3.98 -2.79 19.62 0.00 16 0.55 0.01

38998 158.27 157.28 0.99 32.85 45.00 12.15 8.50 23.27 0.00 31 0.55 0.01

39028 159.01 158.27 0.74 24.74 45.00 20.26 14.19 29.35 0.00 43 0.56 0.00

39058 159.81 159.01 0.81 26.85 45.00 18.15 12.70 34.79 -0.00 47 0.56 0.00

39088 160.92 159.81 1.11 36.94 45.00 8.06 5.64 37.21 -0.00 42 0.57 0.01

39118 162.34 160.92 1.42 47.45 45.00 -2.45 -1.72 36.47 -0.00 34 0.58 0.01

39148 163.92 162.34 1.57 52.34 45.00 -7.34 -5.14 34.27 -0.00 29 0.59 0.01

39178 165.53 163.92 1.62 53.96 45.00 -8.96 -6.27 31.58 -0.00 25 0.59 0.01

39208 167.02 165.53 1.49 49.71 45.00 -4.71 -3.29 30.17 0.00 26 0.60 0.01

39238 168.33 167.02 1.30 43.40 45.00 1.60 1.12 30.65 0.00 31 0.61 0.01

39268 169.54 168.33 1.22 40.50 45.00 4.50 3.15 32.00 0.00 35 0.62 0.01

39298 170.75 169.54 1.21 40.37 45.00 4.63 3.24 33.39 0.00 36 0.62 0.01

39328 171.95 170.75 1.20 39.97 45.00 5.03 3.52 34.90 0.00 38 0.63 0.01

39358 173.25 171.95 1.30 43.34 45.00 1.66 1.16 35.40 -0.00 36 0.64 0.01

39388 174.78 173.25 1.53 50.96 45.00 -5.96 -4.17 33.61 -0.00 29 0.65 0.01

39418 176.31 174.78 1.53 51.11 45.00 -6.11 -4.28 31.78 -0.00 27 0.65 0.01

39448 177.86 176.31 1.55 51.69 45.00 -6.69 -4.69 29.77 -0.00 25 0.66 0.01

39478 179.33 177.86 1.46 48.74 45.00 -3.74 -2.62 28.65 0.00 26 0.67 0.01

39508 -179.48 179.33 1.19 39.81 45.00 5.19 3.64 30.21 0.00 33 0.68 0.01

39538 -178.42 -179.48 1.06 35.32 45.00 9.68 6.78 33.11 0.00 39 0.68 0.01

39568 -177.27 -178.42 1.15 38.26 45.00 6.74 4.72 35.14 -0.00 39 0.69 0.01

39598 -175.90 -177.27 1.37 45.81 45.00 -0.81 -0.57 34.89 -0.00 34 0.70 0.01

39628 -174.40 -175.90 1.50 50.03 45.00 -5.03 -3.52 33.38 -0.00 29 0.71 0.01

39658 -172.88 -174.40 1.51 50.42 45.00 -5.42 -3.80 31.76 -0.00 27 0.71 0.01

39688 -171.45 -172.88 1.43 47.82 45.00 -2.82 -1.97 30.91 0.00 28 0.72 0.01

39718 -170.21 -171.45 1.24 41.42 45.00 3.58 2.51 31.98 0.00 34 0.73 0.01

39748 -169.01 -170.21 1.19 39.78 45.00 5.22 3.66 33.55 0.00 37 0.74 0.01

39778 -167.66 -169.01 1.35 44.97 45.00 0.03 0.02 33.56 -0.00 33 0.74 0.01

39808 -166.16 -167.66 1.51 50.25 45.00 -5.25 -3.68 31.98 -0.00 28 0.75 0.01

39838 -164.72 -166.16 1.44 47.87 45.00 -2.87 -2.01 31.12 0.00 29 0.76 0.01

39868 -163.15 -164.72 1.57 52.41 45.00 -7.41 -5.19 28.90 -0.00 23 0.77 0.01

39898 -161.37 -163.15 1.78 59.37 45.00 -14.37 -10.06 24.59 -0.00 14 0.78 0.01

39928 -159.85 -161.37 1.51 50.40 45.00 -5.40 -3.78 22.97 0.00 19 0.79 0.01

39958 -159.05 -159.85 0.81 26.86 45.00 18.14 12.70 28.41 0.00 41 0.79 0.00

39988 -158.52 -159.05 0.53 17.77 45.00 27.23 19.06 36.58 0.00 55 0.79 0.00

40018 -157.22 -158.52 1.29 43.09 45.00 1.91 1.34 37.15 -0.00 38 0.80 0.01

40051 -155.56 -157.22 1.66 55.34 45.00 -10.34 -7.24 34.05 -0.00 26 0.81 0.01

40081 -153.97 -155.56 1.60 53.19 45.00 -8.19 -5.73 31.59 0.00 25 0.82 0.01

40111 -152.50 -153.97 1.47 49.00 45.00 -4.00 -2.80 30.39 0.00 27 0.83 0.01

40141 -151.30 -152.50 1.20 39.86 45.00 5.14 3.60 31.94 0.00 35 0.83 0.01

40171 -150.27 -151.30 1.03 34.50 45.00 10.50 7.35 35.09 0.00 42 0.84 0.01

40201 -149.28 -150.27 0.99 32.94 45.00 12.06 8.44 38.70 0.00 47 0.85 0.01

40231 -147.96 -149.28 1.32 43.98 45.00 1.02 0.72 39.01 -0.00 39 0.85 0.01

40261 -146.50 -147.96 1.46 48.71 45.00 -3.71 -2.59 37.90 -0.00 35 0.86 0.01

40291 -145.13 -146.50 1.37 45.61 45.00 -0.61 -0.43 37.72 0.00 37 0.87 0.01

40321 -143.85 -145.13 1.28 42.65 45.00 2.35 1.64 38.42 0.00 40 0.88 0.01

40351 -142.45 -143.85 1.40 46.52 45.00 -1.52 -1.06 37.97 -0.00 36 0.88 0.01

40381 -141.00 -142.45 1.45 48.45 45.00 -3.45 -2.41 36.93 -0.00 34 0.89 0.01

40411 -139.62 -141.00 1.38 45.99 45.00 -0.99 -0.69 36.64 0.00 35 0.90 0.01

40441 -138.23 -139.62 1.39 46.38 45.00 -1.38 -0.96 36.22 -0.00 35 0.91 0.01

40471 -136.79 -138.23 1.44 47.88 45.00 -2.88 -2.02 35.36 -0.00 33 0.92 0.01

40501 -135.39 -136.79 1.40 46.74 45.00 -1.74 -1.22 34.84 0.00 33 0.92 0.01

40531 -134.24 -135.39 1.15 38.30 45.00 6.70 4.69 36.85 0.00 41 0.93 0.01

40561 -132.98 -134.24 1.26 42.06 45.00 2.94 2.06 37.73 -0.00 39 0.94 0.01

40591 -131.67 -132.98 1.31 43.69 45.00 1.31 0.91 38.12 -0.00 39 0.94 0.01

40621 -130.14 -131.67 1.53 51.09 45.00 -6.09 -4.27 36.29 -0.00 32 0.95 0.01

40651 -128.64 -130.14 1.50 49.91 45.00 -4.91 -3.44 34.82 0.00 31 0.96 0.01

40681 -127.20 -128.64 1.44 47.92 45.00 -2.92 -2.04 33.94 0.00 31 0.97 0.01

40711 -125.79 -127.20 1.41 47.09 45.00 -2.09 -1.46 33.32 0.00 31 0.98 0.01

40741 -124.45 -125.79 1.34 44.72 45.00 0.28 0.19 33.40 0.00 33 0.98 0.01

40771 -123.15 -124.45 1.29 43.15 45.00 1.85 1.29 33.96 0.00 35 0.99 0.01

average turn rate = 43.3

BackupAndTurn90Deg finished

40776: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 8.04

IRHomingValTotalAvg = 109

40802: glLeftCenterCm = -2080374784 glRightCenterCm = 0

glLeftCenterCm <= glRightCenterCm --> Calling TrackLeftWallOffset()

TrackLeftWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 350.00 0.00 0.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd IRAvg

WallE3_Complete_V1.ino: In TrackLeftWallOffset: before while with errcode = NO_ANOMALIES

Msec LF LC LR LCCorr Steer Tweak OffCm Err LastI LastD Out Lspd Rspd

40928 38.00 41.00 40.00 40.00 -0.13 0.00 0.00 0.13 0.00 0.13 45.50 120 29

41028 35.00 40.00 40.00 26.00 -0.54 -0.28 0.00 0.82 0.00 0.69 287.00 127 0

41128 33.00 31.00 37.00 24.00 -0.41 -0.32 0.00 0.73 0.00 -0.09 255.50 127 0

41227 38.00 37.00 29.00 15.00 0.81 -0.50 0.00 -0.31 0.00 -1.04 -108.50 0 127

41328 47.00 50.00 35.00 13.00 1.00 -0.54 0.00 -0.46 0.00 -0.15 -161.00 0 127

41428 54.00 55.00 48.00 33.00 0.59 -0.14 0.00 -0.45 0.00 0.01 -157.50 0 127

41528 53.00 55.00 49.00 42.00 0.42 0.04 0.00 -0.46 0.00 -0.01 -161.00 0 127

41627 51.00 54.00 49.00 52.00 0.16 0.24 0.00 -0.40 0.00 0.06 -140.00 0 127

41728 51.00 53.00 48.00 46.00 0.30 0.12 0.00 -0.42 0.00 -0.02 -147.00 0 127

41829 49.00 51.00 46.00 45.00 0.27 0.10 0.00 -0.37 0.00 0.05 -129.50 0 127

41926 50.00 51.00 48.00 49.00 0.15 0.18 0.00 -0.33 0.00 0.04 -115.50 0 127

42026 50.00 52.00 48.00 48.00 0.21 0.16 0.00 -0.37 0.00 -0.04 -129.50 0 127

42129 51.00 53.00 48.00 45.00 0.32 0.10 0.00 -0.42 0.00 -0.05 -147.00 0 127

42229 51.00 54.00 48.00 47.00 0.30 0.14 0.00 -0.44 0.00 -0.02 -154.00 0 127

42326 52.00 55.00 49.00 49.00 0.28 0.18 0.00 -0.46 0.00 -0.02 -161.00 0 127

42427 53.00 55.00 49.00 47.00 0.32 0.14 0.00 -0.46 0.00 0.00 -161.00 0 127

42529 51.00 54.00 49.00 51.00 0.18 0.22 0.00 -0.40 0.00 0.06 -140.00 0 127

42627 51.00 53.00 48.00 47.00 0.27 0.14 0.00 -0.41 0.00 -0.01 -143.50 0 127

This was a pretty good run. The robot captured (approximately) the desired offset, tracked the wall, negotiated the 45º break, managed to go past a mostly closed door, and then plunged headlong into the next room – oops!

When I started looking at the telemetry data, I realized that I now have too much data – especially the details about how the PID engine is managing. I think I need to seriously reduce the clutter if I want to have any chance at all of understanding WallE3’s behavior during tracking operations. Maybe something like:

Msec  LCen  RCen Front Rear FVar  RVar

1	Msec LCen RCen Front Rear FVar RVar

I modified the telemetry code in ‘..Complete_V1’ to just output the above parameters.

07 February Update:

Wall tracking seems to be going well at the moment. Not perfect, but OK. Now I’m starting to think about the ‘open door’ problem. This occurs when the robot is tracking a wall down a hallway, and encounters an open doorway on the tracking side – what to do? As it stands, the robot makes an abrupt turn into the open door and may or may not ever return. I’m now thinking that the robot should bypass open doorways if possible. If the other side of the hallway is continuous across the open doorway, then maybe the robot should switch to tracking that wall for the duration of the open doorway (or maybe for as long as that wall lasts?). In order to accomplish this, the robot must be aware of the current distance to the non-tracking side. Currently that information is available, but unused. To investigate this idea I set up a straight wall tracking configuration in my ‘test range’ (aka office) and added a short section of wall on the ‘other’ (non-tracked) side, and instrumented the program as noted above. Here’s the setup:

straight wall on tracked side, wall segment on non-tracked side

And here’s the raw telemetry from the run, and an Excel plot of the left and right side distances:

Msec	LCen	RCen	Front	Rear	FVar	RVar
12207	30.0	779.0	400	63.0	12762.2	703.8
12307	29.0	779.0	400	61.0	12503.0	660.0
12414	29.0	779.0	400	67.0	11874.3	619.7
12507	30.0	779.0	400	78.0	11513.1	587.6
12614	31.0	779.0	400	82.0	10716.2	559.0
12707	32.0	779.0	400	77.0	10287.8	526.2
12807	31.0	779.0	400	84.0	9844.2	499.4
12915	32.0	779.0	267	99.0	8530.9	490.2
13007	30.0	779.0	311	106.0	7917.5	490.2
13113	32.0	779.0	218	97.0	7060.8	474.9
13207	31.0	779.0	249	106.0	6616.9	471.0
13307	31.0	779.0	228	119.0	6270.4	488.0
13414	29.0	779.0	200	125.0	5914.1	513.9
13507	29.0	779.0	209	115.0	5763.6	516.8
13613	31.0	779.0	322	115.0	5178.3	517.4
13707	30.0	779.0	284	819.0	4939.5	11334.4
13807	28.0	779.0	309	819.0	4711.6	21664.0
13910	29.0	779.0	225	819.0	4673.6	31509.9
14007	28.0	779.0	253	819.0	4611.6	40875.7
14117	28.0	75.0	334	750.0	4399.7	48018.0
14207	29.0	55.0	307	819.0	4319.8	56477.0
14307	28.0	50.0	325	750.0	4253.3	62801.1
14413	26.0	48.0	326	750.0	4163.1	68740.6
14507	27.0	49.0	325	750.0	4136.1	74298.6
14612	26.0	47.0	353	750.0	4136.8	79478.3
14707	26.0	47.0	343	750.0	4136.8	84283.0
14807	28.0	46.0	349	750.0	4144.1	88715.7
14915	28.0	46.0	385	819.0	4266.4	94214.0
15007	31.0	46.0	373	819.0	4298.5	99266.9
15113	32.0	44.0	400	819.0	4459.7	103877.6
15207	33.0	43.0	391	750.0	4502.4	106740.7
15307	34.0	42.0	397	750.0	4546.9	109248.0
15417	36.0	41.0	344	750.0	4429.7	111402.5
15507	34.0	41.0	361	750.0	4353.5	113207.5
15612	38.0	41.0	306	750.0	4175.7	114666.0
15707	36.0	41.0	324	750.0	4068.4	115781.2
15807	36.0	43.0	312	750.0	3959.6	116556.0
15914	37.0	43.0	241	819.0	3966.2	118040.7
16007	36.0	46.0	264	819.0	3881.8	119113.4
16113	35.0	47.0	167	819.0	4384.3	119777.6
16207	34.0	49.0	199	750.0	4418.8	119111.6
16307	32.0	50.0	177	819.0	4517.4	119009.3
16410	33.0	50.0	156	750.0	5036.6	117660.4
16507	30.0	50.0	163	750.0	5345.2	115993.0
16614	30.0	52.0	206	750.0	5500.7	114010.0
16707	28.0	51.0	191	750.0	5588.2	111714.6
16807	23.0	779.0	201	750.0	5703.0	109109.5
16915	23.0	779.0	221	819.0	5657.7	106864.8
17007	22.0	779.0	214	750.0	5614.2	103008.5
17112	25.0	51.0	255	819.0	5507.0	99368.0
17207	26.0	779.0	241	750.0	5513.9	94625.8

Msec LCen RCen Front Rear FVar RVar

12207 30.0 779.0 400 63.0 12762.2 703.8

12307 29.0 779.0 400 61.0 12503.0 660.0

12414 29.0 779.0 400 67.0 11874.3 619.7

12507 30.0 779.0 400 78.0 11513.1 587.6

12614 31.0 779.0 400 82.0 10716.2 559.0

12707 32.0 779.0 400 77.0 10287.8 526.2

12807 31.0 779.0 400 84.0 9844.2 499.4

12915 32.0 779.0 267 99.0 8530.9 490.2

13007 30.0 779.0 311 106.0 7917.5 490.2

13113 32.0 779.0 218 97.0 7060.8 474.9

13207 31.0 779.0 249 106.0 6616.9 471.0

13307 31.0 779.0 228 119.0 6270.4 488.0

13414 29.0 779.0 200 125.0 5914.1 513.9

13507 29.0 779.0 209 115.0 5763.6 516.8

13613 31.0 779.0 322 115.0 5178.3 517.4

13707 30.0 779.0 284 819.0 4939.5 11334.4

13807 28.0 779.0 309 819.0 4711.6 21664.0

13910 29.0 779.0 225 819.0 4673.6 31509.9

14007 28.0 779.0 253 819.0 4611.6 40875.7

14117 28.0 75.0 334 750.0 4399.7 48018.0

14207 29.0 55.0 307 819.0 4319.8 56477.0

14307 28.0 50.0 325 750.0 4253.3 62801.1

14413 26.0 48.0 326 750.0 4163.1 68740.6

14507 27.0 49.0 325 750.0 4136.1 74298.6

14612 26.0 47.0 353 750.0 4136.8 79478.3

14707 26.0 47.0 343 750.0 4136.8 84283.0

14807 28.0 46.0 349 750.0 4144.1 88715.7

14915 28.0 46.0 385 819.0 4266.4 94214.0

15007 31.0 46.0 373 819.0 4298.5 99266.9

15113 32.0 44.0 400 819.0 4459.7 103877.6

15207 33.0 43.0 391 750.0 4502.4 106740.7

15307 34.0 42.0 397 750.0 4546.9 109248.0

15417 36.0 41.0 344 750.0 4429.7 111402.5

15507 34.0 41.0 361 750.0 4353.5 113207.5

15612 38.0 41.0 306 750.0 4175.7 114666.0

15707 36.0 41.0 324 750.0 4068.4 115781.2

15807 36.0 43.0 312 750.0 3959.6 116556.0

15914 37.0 43.0 241 819.0 3966.2 118040.7

16007 36.0 46.0 264 819.0 3881.8 119113.4

16113 35.0 47.0 167 819.0 4384.3 119777.6

16207 34.0 49.0 199 750.0 4418.8 119111.6

16307 32.0 50.0 177 819.0 4517.4 119009.3

16410 33.0 50.0 156 750.0 5036.6 117660.4

16507 30.0 50.0 163 750.0 5345.2 115993.0

16614 30.0 52.0 206 750.0 5500.7 114010.0

16707 28.0 51.0 191 750.0 5588.2 111714.6

16807 23.0 779.0 201 750.0 5703.0 109109.5

16915 23.0 779.0 221 819.0 5657.7 106864.8

17007 22.0 779.0 214 750.0 5614.2 103008.5

17112 25.0 51.0 255 819.0 5507.0 99368.0

17207 26.0 779.0 241 750.0 5513.9 94625.8

tracking left wall, with short segment of right wall encountered from 14 to 16.8 sec

As shown above, the right wall distance drops dramatically from around 800 (basically ‘infinite’) to around 50cm during the robot’s transit through this section. It seems reasonable that I should be able to define a new AnomalyCode value – say “OPEN_DOOR” to handle the case where the ‘tracking side’ distance becomes much larger than the ‘non-tracking side’ distance. In this case, the current tracking operation would be cancelled and the main loop would be re-entered from the top, whereupon (one hopes) that the tracking operation would shift to the other wall. When the ‘open door’ section was past, then the ‘off side’ tracking operation could continue, or revert back in some as-yet-to-be-determined fashion.

To start this investigation, I created a new project called ‘WallE3_Complete_V2’ as a clone of ‘WallE3_Complete_V1’ and started from there.

I added a new AnomalyCode – OPEN_DOORWAY in ‘enums.h’ and to AnomalyStrArray[]
added ‘if else()’ block in CheckForAnomalies() to call new ‘isOpenDoorWay()’ fcn
Added ‘isOpenDoorWay(WallTrackingCases trkdir) function to check for this condition
Added MAX_TRACKING_DISTANCE_CM = 100; to DISTANCE_MEASUREMENT_SUPPORT region
Added OPEN_DOORWAY case to HandleAnomalousConditions()
Revised CheckForAnomalies() to accept a TrackingCases parameter, so it can be passed to IsOpenDoorWay(WallTrackingCasestrkdir)

With the above changes, I was able to make a reasonably successful ‘open doorway’ run. Here’s the telemetry showing the robot switching from left-side tracking, to right-side tracking, and then back to left-side tracking.

Msec	LCen	RCen	Front	Rear	FVar	RVar
12236	27.0	52.0	353	73.0	11414.3	709.2
12336	26.0	53.0	353	75.0	10959.0	672.7
12443	28.0	52.0	164	79.0	9791.4	639.7
12536	30.0	49.0	227	74.0	9161.5	602.6
12642	35.0	49.0	106	83.0	8792.8	573.2
12736	37.0	53.0	146	95.0	8494.6	557.5
12836	39.0	73.0	119	94.0	8366.9	539.0
12940	39.0	63.0	332	83.0	7810.0	508.1
13036	39.0	83.0	261	75.0	7435.4	472.8
13144	39.0	779.0	400	85.0	7484.1	444.4
13236	39.0	779.0	353	103.0	7294.7	436.0
13336	39.0	779.0	384	98.0	7224.5	419.6
13441	38.0	779.0	383	100.0	7070.4	404.9
13536	38.0	779.0	383	118.0	6994.2	416.4
13641	38.0	779.0	373	118.0	6786.6	425.2
13736	37.0	779.0	376	115.0	6702.6	426.4
13836	35.0	779.0	374	117.0	6620.5	428.8
13942	35.0	779.0	365	750.0	6441.0	9266.7
14036	33.0	779.0	368	819.0	6374.7	19577.9
14148	33.0	779.0	398	819.0	6428.0	29410.0
14236	34.0	779.0	388	750.0	6433.2	36978.9
14336	34.0	106.0	394	819.0	6461.1	45906.4
14442	35.0	64.0	400	750.0	6565.4	52659.1
14536	35.0	50.0	398	750.0	6618.2	59028.0
14641	36.0	45.0	400	819.0	6744.8	66645.5
14736	37.0	44.0	399	819.0	6809.3	73805.0
14836	38.0	44.0	399	819.0	6875.2	80510.0
14940	38.0	44.0	372	750.0	6912.3	85261.0
15036	37.0	44.0	381	819.0	6943.5	91109.2
15146	39.0	44.0	353	750.0	6939.8	95090.3
15236	52.0	45.0	362	750.0	6934.2	98710.1
15336	824.0	47.0	356	750.0	6914.4	101972.0
In HandleAnomalousConditions with OPEN_DOORWAY error code detected
OPEN_DOORWAY case detected

15343:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.87
IRHomingValTotalAvg = 43 
15358: glLeftCenterCm = 824.00 glRightCenterCm = 48.00
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 350.00	0.00	0.00
WallE3_Complete_V2.ino: In TrackRightWallOffset: before while with errcode = NO_ANOMALIES

Msec	RF	RC	RR	RCCorr	Steer	Tweak	OffCm	Err	LastI	LastD	Out	Lspd	Rspd
15475	51.00	51.00	49.00	48.00	0.19	0.16	0.00	-0.35	0.00	-0.35	-122.50	127	0
15575	54.00	54.00	51.00	49.00	0.24	0.18	0.00	-0.42	0.00	-0.07	-147.00	127	0
15682	57.00	56.00	55.00	53.00	0.18	0.26	0.00	-0.44	0.00	-0.02	-154.00	127	0
15775	57.00	57.00	40.00	14.00	1.00	-0.52	0.00	-0.48	0.00	-0.04	-168.00	127	0
15879	56.00	57.00	57.00	56.00	-0.10	0.32	0.00	-0.22	0.00	0.26	-77.00	127	0
15975	56.00	56.00	55.00	55.00	0.11	0.30	0.00	-0.41	0.00	-0.19	-143.50	127	0
16075	57.00	56.00	58.00	54.00	-0.16	0.28	0.00	-0.12	0.00	0.29	-42.00	117	33
16178	58.00	59.00	24.00	15.00	1.00	-0.50	0.00	-0.50	0.00	-0.38	-175.00	127	0
16275	59.00	59.00	55.00	49.00	0.34	0.18	0.00	-0.52	0.00	-0.02	-182.00	127	0
16379	61.00	63.00	68.00	31.00	-0.71	-0.18	0.00	0.89	0.00	1.41	311.50	0	127
16475	67.00	68.00	785.00	17.00	-1.00	-0.46	0.00	1.46	0.00	0.57	511.00	0	127
16575	67.00	71.00	76.00	24.00	-0.89	-0.32	0.00	1.21	0.00	-0.25	423.50	0	127
16682	62.00	63.00	69.00	31.00	-0.70	-0.18	0.00	0.88	0.00	-0.33	308.00	0	127
16775	53.00	56.00	61.00	24.00	-0.77	-0.32	0.00	1.09	0.00	0.21	381.50	0	127
16878	47.00	49.00	50.00	41.00	-0.33	0.02	0.00	0.31	0.00	-0.78	108.50	0	127
16975	43.00	43.00	44.00	42.00	-0.10	0.04	0.00	0.06	0.00	-0.25	21.00	54	96
17075	41.00	40.00	40.00	40.00	0.02	0.00	0.00	-0.02	0.00	-0.08	-7.00	82	68
17181	40.00	39.00	39.00	38.00	0.09	-0.04	0.00	-0.05	0.00	-0.03	-17.50	92	57
17275	40.00	39.00	40.00	39.00	0.08	-0.02	0.00	-0.06	0.00	-0.01	-21.00	96	54
17385	42.00	40.00	38.00	32.00	0.37	-0.16	0.00	-0.21	0.00	-0.15	-73.50	127	1
17475	757.00	44.00	39.00	11.00	1.00	-0.58	0.00	-0.42	0.00	-0.21	-147.00	127	0
17575	757.00	779.00	40.00	196.00	1.00	3.12	0.00	-4.12	0.00	-3.70	-1442.00	127	0
In HandleAnomalousConditions with OPEN_DOORWAY error code detected
OPEN_DOORWAY case detected

17585:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.91
IRHomingValTotalAvg = 58 
17600: glLeftCenterCm = 75.00 glRightCenterCm = 779.00
glLeftCenterCm <= glRightCenterCm --> Calling TrackLeftWallOffset()

Msec	LCen	RCen	Front	Rear	FVar	RVar
17709	83.0	779.0	384	750.0	6944.9	90000.9
17809	86.0	44.0	387	819.0	6912.2	85421.8
17915	93.0	779.0	194	819.0	7251.6	80233.5
18009	88.0	779.0	258	819.0	7136.3	74827.7
18114	90.0	779.0	259	819.0	6861.6	69397.0
18209	84.0	779.0	258	819.0	6698.4	63441.8
18309	86.0	779.0	258	819.0	6519.0	57117.7
18413	86.0	779.0	209	95.0	6339.6	57651.2

Msec LCen RCen Front Rear FVar RVar

12236 27.0 52.0 353 73.0 11414.3 709.2

12336 26.0 53.0 353 75.0 10959.0 672.7

12443 28.0 52.0 164 79.0 9791.4 639.7

12536 30.0 49.0 227 74.0 9161.5 602.6

12642 35.0 49.0 106 83.0 8792.8 573.2

12736 37.0 53.0 146 95.0 8494.6 557.5

12836 39.0 73.0 119 94.0 8366.9 539.0

12940 39.0 63.0 332 83.0 7810.0 508.1

13036 39.0 83.0 261 75.0 7435.4 472.8

13144 39.0 779.0 400 85.0 7484.1 444.4

13236 39.0 779.0 353 103.0 7294.7 436.0

13336 39.0 779.0 384 98.0 7224.5 419.6

13441 38.0 779.0 383 100.0 7070.4 404.9

13536 38.0 779.0 383 118.0 6994.2 416.4

13641 38.0 779.0 373 118.0 6786.6 425.2

13736 37.0 779.0 376 115.0 6702.6 426.4

13836 35.0 779.0 374 117.0 6620.5 428.8

13942 35.0 779.0 365 750.0 6441.0 9266.7

14036 33.0 779.0 368 819.0 6374.7 19577.9

14148 33.0 779.0 398 819.0 6428.0 29410.0

14236 34.0 779.0 388 750.0 6433.2 36978.9

14336 34.0 106.0 394 819.0 6461.1 45906.4

14442 35.0 64.0 400 750.0 6565.4 52659.1

14536 35.0 50.0 398 750.0 6618.2 59028.0

14641 36.0 45.0 400 819.0 6744.8 66645.5

14736 37.0 44.0 399 819.0 6809.3 73805.0

14836 38.0 44.0 399 819.0 6875.2 80510.0

14940 38.0 44.0 372 750.0 6912.3 85261.0

15036 37.0 44.0 381 819.0 6943.5 91109.2

15146 39.0 44.0 353 750.0 6939.8 95090.3

15236 52.0 45.0 362 750.0 6934.2 98710.1

15336 824.0 47.0 356 750.0 6914.4 101972.0

In HandleAnomalousConditions with OPEN_DOORWAY error code detected

OPEN_DOORWAY case detected

15343: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.87

IRHomingValTotalAvg = 43

15358: glLeftCenterCm = 824.00 glRightCenterCm = 48.00

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 350.00 0.00 0.00

WallE3_Complete_V2.ino: In TrackRightWallOffset: before while with errcode = NO_ANOMALIES

Msec RF RC RR RCCorr Steer Tweak OffCm Err LastI LastD Out Lspd Rspd

15475 51.00 51.00 49.00 48.00 0.19 0.16 0.00 -0.35 0.00 -0.35 -122.50 127 0

15575 54.00 54.00 51.00 49.00 0.24 0.18 0.00 -0.42 0.00 -0.07 -147.00 127 0

15682 57.00 56.00 55.00 53.00 0.18 0.26 0.00 -0.44 0.00 -0.02 -154.00 127 0

15775 57.00 57.00 40.00 14.00 1.00 -0.52 0.00 -0.48 0.00 -0.04 -168.00 127 0

15879 56.00 57.00 57.00 56.00 -0.10 0.32 0.00 -0.22 0.00 0.26 -77.00 127 0

15975 56.00 56.00 55.00 55.00 0.11 0.30 0.00 -0.41 0.00 -0.19 -143.50 127 0

16075 57.00 56.00 58.00 54.00 -0.16 0.28 0.00 -0.12 0.00 0.29 -42.00 117 33

16178 58.00 59.00 24.00 15.00 1.00 -0.50 0.00 -0.50 0.00 -0.38 -175.00 127 0

16275 59.00 59.00 55.00 49.00 0.34 0.18 0.00 -0.52 0.00 -0.02 -182.00 127 0

16379 61.00 63.00 68.00 31.00 -0.71 -0.18 0.00 0.89 0.00 1.41 311.50 0 127

16475 67.00 68.00 785.00 17.00 -1.00 -0.46 0.00 1.46 0.00 0.57 511.00 0 127

16575 67.00 71.00 76.00 24.00 -0.89 -0.32 0.00 1.21 0.00 -0.25 423.50 0 127

16682 62.00 63.00 69.00 31.00 -0.70 -0.18 0.00 0.88 0.00 -0.33 308.00 0 127

16775 53.00 56.00 61.00 24.00 -0.77 -0.32 0.00 1.09 0.00 0.21 381.50 0 127

16878 47.00 49.00 50.00 41.00 -0.33 0.02 0.00 0.31 0.00 -0.78 108.50 0 127

16975 43.00 43.00 44.00 42.00 -0.10 0.04 0.00 0.06 0.00 -0.25 21.00 54 96

17075 41.00 40.00 40.00 40.00 0.02 0.00 0.00 -0.02 0.00 -0.08 -7.00 82 68

17181 40.00 39.00 39.00 38.00 0.09 -0.04 0.00 -0.05 0.00 -0.03 -17.50 92 57

17275 40.00 39.00 40.00 39.00 0.08 -0.02 0.00 -0.06 0.00 -0.01 -21.00 96 54

17385 42.00 40.00 38.00 32.00 0.37 -0.16 0.00 -0.21 0.00 -0.15 -73.50 127 1

17475 757.00 44.00 39.00 11.00 1.00 -0.58 0.00 -0.42 0.00 -0.21 -147.00 127 0

17575 757.00 779.00 40.00 196.00 1.00 3.12 0.00 -4.12 0.00 -3.70 -1442.00 127 0

In HandleAnomalousConditions with OPEN_DOORWAY error code detected

OPEN_DOORWAY case detected

17585: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.91

IRHomingValTotalAvg = 58

17600: glLeftCenterCm = 75.00 glRightCenterCm = 779.00

glLeftCenterCm <= glRightCenterCm --> Calling TrackLeftWallOffset()

Msec LCen RCen Front Rear FVar RVar

17709 83.0 779.0 384 750.0 6944.9 90000.9

17809 86.0 44.0 387 819.0 6912.2 85421.8

17915 93.0 779.0 194 819.0 7251.6 80233.5

18009 88.0 779.0 258 819.0 7136.3 74827.7

18114 90.0 779.0 259 819.0 6861.6 69397.0

18209 84.0 779.0 258 819.0 6698.4 63441.8

18309 86.0 779.0 258 819.0 6519.0 57117.7

18413 86.0 779.0 209 95.0 6339.6 57651.2

And here’s a short video showing the action:

10 February 2023 Update:

I thought of another way to ‘improve’ ‘Open Doorway’ handling. I think it would make a smoother transition from one wall to the other by using the current distance to the ‘off’ wall as the desired offset. To do this, the robot will need a global variable to hold the ‘current desired offset’, something like ‘glCurTrackingOffset’, or maybe two of them ‘glCurrentRightTrackingOffset’ and ‘glCurrentLeftTrackingOffset’. The idea would be that when the last AnomalyCode was ‘OPEN_DOORWAY’, then the appropriate non-standard distance would be used the next time through the loop.

Hmm, I guess this means we don’t need the above ‘gl_CurrentLeft/RightTrackingOffset’ variables, but we DO need a global variable to hold the last AnomalyCode, like ‘gl_LastAnomalyCode’. The idea would be that when the side distances are measured at the top of loop() to determine which side the robot should track, the current value of ‘gl_LastAnomalyCode’ will be checked. If it is ‘OPEN_DOORAY’, then TrackLeft/RightWallOffset() will be called using the actual distance to the off wall rather than WALL_OFFSET_TGTDIST_CM, and the ‘gl_LastAnomalyCode’ variable will be set to NO_ANOMALIES.

I made the changes described above, and then after the normal number of screwups, I got a pretty good 3-wall run using the ‘open doorway’ wall configuration shown in the above video. Here’s the telemetry:

12053:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.85
IRHomingValTotalAvg = 71 
12075: gl_LeftCenterCm = 14.00 gl_RightCenterCm = 79.00

TrackLeftWallOffset(350.0, 0.0, 0.0, 40) called

TrackLeftWallOffset: Start tracking offset of 40cm

Msec	LCen	RCen	Front	Rear	FVar	RVar
12186	14.0	79.0	384	65.0	12555.1	700.5
12286	13.0	81.0	384	73.0	12255.5	663.2
12392	14.0	86.0	400	78.0	11744.5	630.2
12486	15.0	101.0	394	76.0	11400.8	595.5
12591	18.0	91.0	358	82.0	10319.0	566.1
12686	20.0	81.0	370	30.0	9816.8	542.8
12786	28.0	102.0	362	24.0	9280.4	530.5
12889	41.0	107.0	194	27.0	8290.4	517.4
12986	48.0	107.0	250	27.0	7733.9	507.5
13097	50.0	89.0	235	35.0	6792.7	491.6
13186	50.0	81.0	240	52.0	6373.8	467.5
13286	45.0	101.0	236	114.0	6003.5	500.2
13391	41.0	65.0	92	101.0	6854.2	506.6
13486	41.0	779.0	140	85.0	7005.1	493.2
13598	41.0	779.0	400	99.0	6954.5	495.0
13686	40.0	117.0	313	116.0	6732.1	523.6
13786	45.0	779.0	371	108.0	6623.5	535.6
13891	43.0	779.0	357	104.0	6373.7	540.3
13986	41.0	779.0	361	819.0	6276.4	11503.3
14093	37.0	779.0	215	819.0	6419.8	21985.6
14186	34.0	779.0	263	750.0	6386.5	30145.3
14286	33.0	779.0	231	114.0	6453.8	30047.9
14392	34.0	779.0	400	750.0	6672.6	37784.0
14486	34.0	779.0	343	819.0	6660.1	46897.8
14593	33.0	779.0	386	819.0	6809.8	55547.5
14686	33.0	779.0	371	819.0	6852.6	63736.3
14786	34.0	779.0	381	750.0	6917.7	69830.2
14896	32.0	779.0	333	750.0	6920.4	75550.6
14986	32.0	779.0	349	819.0	6930.0	82462.0
15094	32.0	779.0	298	750.0	6930.2	87406.8
15186	33.0	779.0	315	750.0	6901.9	91987.6
15286	32.0	33.0	303	750.0	6863.4	96207.6
15392	33.0	33.0	172	750.0	7526.0	100069.9
15486	33.0	32.0	215	750.0	7571.5	103577.7
15592	32.0	31.0	219	819.0	7622.4	108065.3
15686	32.0	30.0	217	750.0	7611.7	110835.8
15786	32.0	30.0	218	750.0	7582.2	113261.2
15893	35.0	30.0	140	750.0	8060.7	115344.4
15986	824.0	31.0	166	750.0	8055.8	117088.5
In HandleAnomalousConditions with OPEN_DOORWAY error code detected
ANOMALY_OPEN_DOORWAY case detected

15993:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.76
IRHomingValTotalAvg = 75 
16009: gl_LeftCenterCm = 824.00 gl_RightCenterCm = 31.00
gl_LeftCenterCm > gl_RightCenterCm --> Calling TrackRightWallOffset(31)

TrackRightWallOffset(350.0, 0.0, 0.0, 31) called

TrackRightWallOffset: Start tracking offset of 31cm

Msec	LCen	RCen	Front	Rear	FVar	RVar
16128	91.0	31.0	151	819.0	8276.0	120677.2
16228	94.0	32.0	155	750.0	8377.9	121385.9
16333	84.0	33.0	252	750.0	7973.6	121767.7
16428	80.0	34.0	219	750.0	7917.4	121825.8
16533	86.0	35.0	121	750.0	8608.7	121563.2
16628	112.0	35.0	153	750.0	8818.9	120982.7
16728	824.0	36.0	131	750.0	9123.6	120087.6
16835	824.0	36.0	279	750.0	9131.6	118880.4
16928	824.0	36.0	229	750.0	8596.1	116759.9
17035	43.0	36.0	248	79.0	7726.0	116525.5
17128	46.0	35.0	241	819.0	7362.0	114825.6
17228	47.0	32.0	245	750.0	6975.3	112086.0
17334	48.0	28.0	270	750.0	6660.2	109071.0
17428	48.0	28.0	261	819.0	6451.9	106312.7
17532	47.0	26.0	294	819.0	6066.8	103228.9
17628	49.0	24.0	283	750.0	6059.0	98054.0
17728	48.0	24.0	290	750.0	6056.6	92329.1
17831	48.0	26.0	64	819.0	7435.4	86743.3
17928	47.0	27.0	139	819.0	7170.4	80655.7
18029	48.0	28.0	66	819.0	7544.6	74260.7
18128	47.0	29.0	90	819.0	7406.3	67789.4
18228	47.0	28.0	74	819.0	7284.5	62300.3
18332	48.0	29.0	42	819.0	7321.9	56114.6
18428	48.0	29.0	52	819.0	7416.9	49129.8
18534	49.0	29.0	250	819.0	6692.3	42061.1
18628	50.0	29.0	184	750.0	6475.1	34834.1
18728	50.0	779.0	228	750.0	6274.9	27085.4
In HandleAnomalousConditions with OPEN_DOORWAY error code detected
ANOMALY_OPEN_DOORWAY case detected

18735:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.83
IRHomingValTotalAvg = 50 
18751: gl_LeftCenterCm = 50.00 gl_RightCenterCm = 779.00
gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset(50)

TrackLeftWallOffset(350.0, 0.0, 0.0, 50) called

TrackLeftWallOffset: Start tracking offset of 50cm

Msec	LCen	RCen	Front	Rear	FVar	RVar
18869	52.0	779.0	210	819.0	5725.1	18997.0
18969	40.0	779.0	219	750.0	5740.1	18903.9
19073	38.0	779.0	199	750.0	5725.2	18903.9
19169	28.0	779.0	205	750.0	5711.8	10650.2
19272	18.0	779.0	158	750.0	5705.7	10650.2
19369	12.0	779.0	173	819.0	5687.0	10650.2
19469	11.0	779.0	163	91.0	5656.2	19401.3
19572	9.0	779.0	104	73.0	5867.5	28197.5
ENTERING COMMAND MODE:

100

101

102

103

104

105

106

107

108

109

110

111

112

113

12053: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.85

IRHomingValTotalAvg = 71

12075: gl_LeftCenterCm = 14.00 gl_RightCenterCm = 79.00

TrackLeftWallOffset(350.0, 0.0, 0.0, 40) called

TrackLeftWallOffset: Start tracking offset of 40cm

Msec LCen RCen Front Rear FVar RVar

12186 14.0 79.0 384 65.0 12555.1 700.5

12286 13.0 81.0 384 73.0 12255.5 663.2

12392 14.0 86.0 400 78.0 11744.5 630.2

12486 15.0 101.0 394 76.0 11400.8 595.5

12591 18.0 91.0 358 82.0 10319.0 566.1

12686 20.0 81.0 370 30.0 9816.8 542.8

12786 28.0 102.0 362 24.0 9280.4 530.5

12889 41.0 107.0 194 27.0 8290.4 517.4

12986 48.0 107.0 250 27.0 7733.9 507.5

13097 50.0 89.0 235 35.0 6792.7 491.6

13186 50.0 81.0 240 52.0 6373.8 467.5

13286 45.0 101.0 236 114.0 6003.5 500.2

13391 41.0 65.0 92 101.0 6854.2 506.6

13486 41.0 779.0 140 85.0 7005.1 493.2

13598 41.0 779.0 400 99.0 6954.5 495.0

13686 40.0 117.0 313 116.0 6732.1 523.6

13786 45.0 779.0 371 108.0 6623.5 535.6

13891 43.0 779.0 357 104.0 6373.7 540.3

13986 41.0 779.0 361 819.0 6276.4 11503.3

14093 37.0 779.0 215 819.0 6419.8 21985.6

14186 34.0 779.0 263 750.0 6386.5 30145.3

14286 33.0 779.0 231 114.0 6453.8 30047.9

14392 34.0 779.0 400 750.0 6672.6 37784.0

14486 34.0 779.0 343 819.0 6660.1 46897.8

14593 33.0 779.0 386 819.0 6809.8 55547.5

14686 33.0 779.0 371 819.0 6852.6 63736.3

14786 34.0 779.0 381 750.0 6917.7 69830.2

14896 32.0 779.0 333 750.0 6920.4 75550.6

14986 32.0 779.0 349 819.0 6930.0 82462.0

15094 32.0 779.0 298 750.0 6930.2 87406.8

15186 33.0 779.0 315 750.0 6901.9 91987.6

15286 32.0 33.0 303 750.0 6863.4 96207.6

15392 33.0 33.0 172 750.0 7526.0 100069.9

15486 33.0 32.0 215 750.0 7571.5 103577.7

15592 32.0 31.0 219 819.0 7622.4 108065.3

15686 32.0 30.0 217 750.0 7611.7 110835.8

15786 32.0 30.0 218 750.0 7582.2 113261.2

15893 35.0 30.0 140 750.0 8060.7 115344.4

15986 824.0 31.0 166 750.0 8055.8 117088.5

In HandleAnomalousConditions with OPEN_DOORWAY error code detected

ANOMALY_OPEN_DOORWAY case detected

15993: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.76

IRHomingValTotalAvg = 75

16009: gl_LeftCenterCm = 824.00 gl_RightCenterCm = 31.00

gl_LeftCenterCm > gl_RightCenterCm --> Calling TrackRightWallOffset(31)

TrackRightWallOffset(350.0, 0.0, 0.0, 31) called

TrackRightWallOffset: Start tracking offset of 31cm

Msec LCen RCen Front Rear FVar RVar

16128 91.0 31.0 151 819.0 8276.0 120677.2

16228 94.0 32.0 155 750.0 8377.9 121385.9

16333 84.0 33.0 252 750.0 7973.6 121767.7

16428 80.0 34.0 219 750.0 7917.4 121825.8

16533 86.0 35.0 121 750.0 8608.7 121563.2

16628 112.0 35.0 153 750.0 8818.9 120982.7

16728 824.0 36.0 131 750.0 9123.6 120087.6

16835 824.0 36.0 279 750.0 9131.6 118880.4

16928 824.0 36.0 229 750.0 8596.1 116759.9

17035 43.0 36.0 248 79.0 7726.0 116525.5

17128 46.0 35.0 241 819.0 7362.0 114825.6

17228 47.0 32.0 245 750.0 6975.3 112086.0

17334 48.0 28.0 270 750.0 6660.2 109071.0

17428 48.0 28.0 261 819.0 6451.9 106312.7

17532 47.0 26.0 294 819.0 6066.8 103228.9

17628 49.0 24.0 283 750.0 6059.0 98054.0

17728 48.0 24.0 290 750.0 6056.6 92329.1

17831 48.0 26.0 64 819.0 7435.4 86743.3

17928 47.0 27.0 139 819.0 7170.4 80655.7

18029 48.0 28.0 66 819.0 7544.6 74260.7

18128 47.0 29.0 90 819.0 7406.3 67789.4

18228 47.0 28.0 74 819.0 7284.5 62300.3

18332 48.0 29.0 42 819.0 7321.9 56114.6

18428 48.0 29.0 52 819.0 7416.9 49129.8

18534 49.0 29.0 250 819.0 6692.3 42061.1

18628 50.0 29.0 184 750.0 6475.1 34834.1

18728 50.0 779.0 228 750.0 6274.9 27085.4

In HandleAnomalousConditions with OPEN_DOORWAY error code detected

ANOMALY_OPEN_DOORWAY case detected

18735: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.83

IRHomingValTotalAvg = 50

18751: gl_LeftCenterCm = 50.00 gl_RightCenterCm = 779.00

gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset(50)

TrackLeftWallOffset(350.0, 0.0, 0.0, 50) called

TrackLeftWallOffset: Start tracking offset of 50cm

Msec LCen RCen Front Rear FVar RVar

18869 52.0 779.0 210 819.0 5725.1 18997.0

18969 40.0 779.0 219 750.0 5740.1 18903.9

19073 38.0 779.0 199 750.0 5725.2 18903.9

19169 28.0 779.0 205 750.0 5711.8 10650.2

19272 18.0 779.0 158 750.0 5705.7 10650.2

19369 12.0 779.0 173 819.0 5687.0 10650.2

19469 11.0 779.0 163 91.0 5656.2 19401.3

19572 9.0 779.0 104 73.0 5867.5 28197.5

ENTERING COMMAND MODE:

In the above telemetry:

12.1 – 16 sec: the robot starts out tracking the left wall at the default offset of 40cm.
16.0 sec: The left distance goes to 785mm and a OPEN_DOORWAY Anomaly code is emitted. This causes the tracking loop to exit and the main loop to run again. This time through, the TrackRightWallOffset function gets called with an offset of 50cm
16.1 – 18.7 sec: The robot tracks the right wall at nominally 50cm
18.7 sec: another OPEN_DOORWAY Anomaly code is emitted. This causes the tracking loop to exit and the main loop to run again. This time through, the TrackLeftWallOffset function gets called, also with an offset of 50cm (I *think* this is a coincidence, but it does look a bit suspicious).
18.8 – 19.6 sec: The left wall is tracked at a nominal 50cm
19.6 sec: The test was terminated.

This looks pretty good, but I had to cheat a little. During my initial runs the robot kept stopping with a STUCK_AHEAD error. When I looked through the telemetry I noticed the front variance value had indeed dropped to near zero, and then I noticed that the front distance measurement itself was holding pretty steady at 400, which is the max the Pulsed Light LIDAR can measure – rats!

There doesn’t seem to be very much I can do to get around the Pulsed Light LIDAR-LITE distance limitation, but this is pretty old technology – almost a decade out of date. Turns out Garmin bought Pulsed Light, and has been marketing the products themselves. Recently they came out with their “V4” version which uses an IR LED instead of a laser, and has a max distance of a whopping 10m! Not only that, but it is just as easy – if not easier – than the original LIDAR Lite to use, and draws much less power – such a deal!

So, I ordered one from Sparkfun, and when it arrives I’ll see if it lives up to the hype and if it will eliminate my false ‘STUCK_AHEAD’ problems

12 March 2023 Update:

After getting my new Garmin LIDAR-Lite V4/LED distance sensor (see this post and this post) tested and integrated into WallE3, my autonomous wall-following robot, I made some more test runs in my office “test range”. As noted above, the problem with my original Pulsed Light LIDAR was that it’s maximum range was about 4m; when the actual distance was greater than 4m, the sensor simply reported ‘400’(cm). This caused the calculated front distance variance to rapidly decrease below the ‘stuck’ threshold, even though in actuality the robot was doing fine. The new sensor, with a maximum range of about 10m should solve this problem. Here’s a recent run on my ‘4m range with an ‘open doorway’ configuration about two-thirds of the way down the track.

Test wall with an ‘open doorway’ about 2/3 of the way down the track

Here’s the telemetry printout from the run:

Msec	LCen	RCen	Front	Rear	FVar	RVar
14260	14.0	200.0	458	61.0	61314.1	696.7
14360	14.0	200.0	456	60.0	57648.2	653.4
14493	15.0	200.0	408	86.0	51515.7	630.5
14560	15.0	200.0	424	92.0	48762.1	614.3
14675	17.0	61.0	355	95.0	44980.3	600.8
14760	24.0	69.0	378	27.0	43308.1	581.0
14860	30.0	82.0	362	22.0	42013.1	571.4
14999	39.0	79.0	220	31.0	43295.5	553.5
15060	41.0	87.0	267	34.0	43667.4	535.5
15175	42.0	75.0	297	43.0	44325.5	512.8
15260	41.0	57.0	287	68.0	45024.5	487.8
15360	39.0	200.0	293	105.0	45806.8	500.7
15496	38.0	200.0	482	86.0	45135.4	487.2
15560	37.0	200.0	419	102.0	45247.6	492.7
15680	39.0	109.0	391	85.0	46070.3	478.1
15760	38.0	112.0	400	79.0	46505.2	460.5
15860	37.0	112.0	394	90.0	47006.4	451.1
15969	35.0	200.0	96	99.0	55106.2	451.2
16060	35.0	200.0	195	96.0	57349.8	447.5
16182	31.0	200.0	355	114.0	58197.3	467.9
16260	30.0	111.0	301	123.0	58810.9	502.9
16360	27.0	116.0	337	200.0	58948.2	800.0
16478	27.0	200.0	345	200.0	58433.6	1080.8
16560	28.0	200.0	342	113.0	57788.3	1084.7
16668	31.0	200.0	98	69.0	60261.9	1070.9
16760	35.0	200.0	179	54.0	59611.6	1070.9
16860	38.0	200.0	125	62.0	58995.7	1065.6
16968	40.0	200.0	85	72.0	56639.9	1056.5
17060	41.0	200.0	98	103.0	54095.4	1056.1
17172	40.0	115.0	246	200.0	44149.1	1317.8
17260	40.0	100.0	196	200.0	38267.3	1566.3
17360	41.0	102.0	229	200.0	31329.4	1802.1
17475	40.0	200.0	350	200.0	14513.0	2025.5
17560	40.0	200.0	309	200.0	14033.2	2237.2
17675	39.0	200.0	340	200.0	13116.1	2437.5
17760	38.0	200.0	329	200.0	12643.9	2626.9
17860	38.0	200.0	336	200.0	12156.2	2805.8
17978	38.0	200.0	363	200.0	11564.4	2974.8
18060	36.0	200.0	354	200.0	11373.3	3134.1
18211	37.0	200.0	500	200.0	12656.5	3284.3
18260	37.0	200.0	451	200.0	13062.1	3425.7
18360	36.0	200.0	483	200.0	13612.7	3558.8
18486	37.0	200.0	491	200.0	14796.0	3683.9
18560	37.0	34.0	488	200.0	15255.7	3801.4
18688	38.0	33.0	475	200.0	16186.1	3911.6
18760	39.0	32.0	479	200.0	16656.8	4015.0
18860	200.0	32.0	476	200.0	17068.5	4111.8
18981	200.0	32.0	437	200.0	17037.2	4202.5
In HandleAnomalousConditions with OPEN_DOORWAY error code detected
ANOMALY_OPEN_DOORWAY case detected

18988:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.91
IRHomingValTotalAvg = 106 
19003: gl_LeftCenterCm = 200.00 gl_RightCenterCm = 31.00
gl_LeftCenterCm > gl_RightCenterCm --> Calling TrackRightWallOffset(31)

TrackRightWallOffset(350.0, 0.0, 0.0, 31) called

TrackRightWallOffset: Start tracking offset of 31cm

Msec	LCen	RCen	Front	Rear	FVar	RVar
19122	200.0	31.0	441	200.0	16951.5	4191.6
19222	200.0	31.0	442	200.0	17125.4	4167.6
19332	200.0	31.0	262	200.0	17128.0	4125.1
19422	200.0	32.0	322	200.0	17035.0	4138.1
19560	200.0	32.0	373	200.0	14907.6	4153.8
19622	200.0	32.0	356	200.0	14507.4	4166.5
19722	200.0	33.0	367	200.0	14517.7	3932.8
19844	36.0	32.0	383	200.0	14554.1	3649.6
19922	35.0	31.0	377	200.0	14578.0	3387.6
20038	37.0	31.0	363	200.0	14594.0	3118.0
20122	38.0	30.0	367	200.0	14605.0	2871.0
20222	39.0	30.0	364	200.0	13377.4	2714.1
20338	40.0	31.0	366	200.0	11467.4	2662.9
20422	40.0	33.0	365	200.0	10358.2	2548.5
20538	41.0	200.0	136	200.0	9324.1	2473.2
20622	43.0	200.0	212	200.0	8361.4	2336.0
In HandleAnomalousConditions with OPEN_DOORWAY error code detected
ANOMALY_OPEN_DOORWAY case detected

20629:	Top of loop() - calling UpdateAllEnvironmentParameters()
Battery Voltage = 7.97
IRHomingValTotalAvg = 76 
20644: gl_LeftCenterCm = 43.00 gl_RightCenterCm = 200.00
gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset(43)

TrackLeftWallOffset(350.0, 0.0, 0.0, 43) called

TrackLeftWallOffset: Start tracking offset of 43cm

Msec	LCen	RCen	Front	Rear	FVar	RVar
20763	44.0	200.0	169	200.0	9399.7	2024.9
20863	46.0	200.0	180	200.0	9512.3	1905.3
20982	46.0	200.0	377	200.0	9140.5	1767.3
21063	45.0	200.0	311	200.0	9197.1	1677.7
21173	46.0	200.0	200	200.0	10200.2	1606.3
21263	46.0	72.0	237	200.0	10496.5	1606.3
21363	45.0	70.0	212	200.0	10907.0	1606.3
21472	46.0	68.0	181	200.0	12091.6	1502.4
21563	45.0	61.0	191	200.0	12584.1	1219.4
21672	44.0	58.0	176	200.0	13181.0	844.0
21763	44.0	54.0	181	200.0	13093.3	495.6
21863	44.0	55.0	177	200.0	13243.4	184.4
21972	43.0	50.0	161	200.0	13097.8	0.0
22063	43.0	45.0	166	200.0	12840.0	0.0
22180	43.0	44.0	179	200.0	12091.0	0.0

100

101

102

103

104

105

106

107

108

Msec LCen RCen Front Rear FVar RVar

14260 14.0 200.0 458 61.0 61314.1 696.7

14360 14.0 200.0 456 60.0 57648.2 653.4

14493 15.0 200.0 408 86.0 51515.7 630.5

14560 15.0 200.0 424 92.0 48762.1 614.3

14675 17.0 61.0 355 95.0 44980.3 600.8

14760 24.0 69.0 378 27.0 43308.1 581.0

14860 30.0 82.0 362 22.0 42013.1 571.4

14999 39.0 79.0 220 31.0 43295.5 553.5

15060 41.0 87.0 267 34.0 43667.4 535.5

15175 42.0 75.0 297 43.0 44325.5 512.8

15260 41.0 57.0 287 68.0 45024.5 487.8

15360 39.0 200.0 293 105.0 45806.8 500.7

15496 38.0 200.0 482 86.0 45135.4 487.2

15560 37.0 200.0 419 102.0 45247.6 492.7

15680 39.0 109.0 391 85.0 46070.3 478.1

15760 38.0 112.0 400 79.0 46505.2 460.5

15860 37.0 112.0 394 90.0 47006.4 451.1

15969 35.0 200.0 96 99.0 55106.2 451.2

16060 35.0 200.0 195 96.0 57349.8 447.5

16182 31.0 200.0 355 114.0 58197.3 467.9

16260 30.0 111.0 301 123.0 58810.9 502.9

16360 27.0 116.0 337 200.0 58948.2 800.0

16478 27.0 200.0 345 200.0 58433.6 1080.8

16560 28.0 200.0 342 113.0 57788.3 1084.7

16668 31.0 200.0 98 69.0 60261.9 1070.9

16760 35.0 200.0 179 54.0 59611.6 1070.9

16860 38.0 200.0 125 62.0 58995.7 1065.6

16968 40.0 200.0 85 72.0 56639.9 1056.5

17060 41.0 200.0 98 103.0 54095.4 1056.1

17172 40.0 115.0 246 200.0 44149.1 1317.8

17260 40.0 100.0 196 200.0 38267.3 1566.3

17360 41.0 102.0 229 200.0 31329.4 1802.1

17475 40.0 200.0 350 200.0 14513.0 2025.5

17560 40.0 200.0 309 200.0 14033.2 2237.2

17675 39.0 200.0 340 200.0 13116.1 2437.5

17760 38.0 200.0 329 200.0 12643.9 2626.9

17860 38.0 200.0 336 200.0 12156.2 2805.8

17978 38.0 200.0 363 200.0 11564.4 2974.8

18060 36.0 200.0 354 200.0 11373.3 3134.1

18211 37.0 200.0 500 200.0 12656.5 3284.3

18260 37.0 200.0 451 200.0 13062.1 3425.7

18360 36.0 200.0 483 200.0 13612.7 3558.8

18486 37.0 200.0 491 200.0 14796.0 3683.9

18560 37.0 34.0 488 200.0 15255.7 3801.4

18688 38.0 33.0 475 200.0 16186.1 3911.6

18760 39.0 32.0 479 200.0 16656.8 4015.0

18860 200.0 32.0 476 200.0 17068.5 4111.8

18981 200.0 32.0 437 200.0 17037.2 4202.5

In HandleAnomalousConditions with OPEN_DOORWAY error code detected

ANOMALY_OPEN_DOORWAY case detected

18988: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.91

IRHomingValTotalAvg = 106

19003: gl_LeftCenterCm = 200.00 gl_RightCenterCm = 31.00

gl_LeftCenterCm > gl_RightCenterCm --> Calling TrackRightWallOffset(31)

TrackRightWallOffset(350.0, 0.0, 0.0, 31) called

TrackRightWallOffset: Start tracking offset of 31cm

Msec LCen RCen Front Rear FVar RVar

19122 200.0 31.0 441 200.0 16951.5 4191.6

19222 200.0 31.0 442 200.0 17125.4 4167.6

19332 200.0 31.0 262 200.0 17128.0 4125.1

19422 200.0 32.0 322 200.0 17035.0 4138.1

19560 200.0 32.0 373 200.0 14907.6 4153.8

19622 200.0 32.0 356 200.0 14507.4 4166.5

19722 200.0 33.0 367 200.0 14517.7 3932.8

19844 36.0 32.0 383 200.0 14554.1 3649.6

19922 35.0 31.0 377 200.0 14578.0 3387.6

20038 37.0 31.0 363 200.0 14594.0 3118.0

20122 38.0 30.0 367 200.0 14605.0 2871.0

20222 39.0 30.0 364 200.0 13377.4 2714.1

20338 40.0 31.0 366 200.0 11467.4 2662.9

20422 40.0 33.0 365 200.0 10358.2 2548.5

20538 41.0 200.0 136 200.0 9324.1 2473.2

20622 43.0 200.0 212 200.0 8361.4 2336.0

In HandleAnomalousConditions with OPEN_DOORWAY error code detected

ANOMALY_OPEN_DOORWAY case detected

20629: Top of loop() - calling UpdateAllEnvironmentParameters()

Battery Voltage = 7.97

IRHomingValTotalAvg = 76

20644: gl_LeftCenterCm = 43.00 gl_RightCenterCm = 200.00

gl_LeftCenterCm <= gl_RightCenterCm --> Calling TrackLeftWallOffset(43)

TrackLeftWallOffset(350.0, 0.0, 0.0, 43) called

TrackLeftWallOffset: Start tracking offset of 43cm

Msec LCen RCen Front Rear FVar RVar

20763 44.0 200.0 169 200.0 9399.7 2024.9

20863 46.0 200.0 180 200.0 9512.3 1905.3

20982 46.0 200.0 377 200.0 9140.5 1767.3

21063 45.0 200.0 311 200.0 9197.1 1677.7

21173 46.0 200.0 200 200.0 10200.2 1606.3

21263 46.0 72.0 237 200.0 10496.5 1606.3

21363 45.0 70.0 212 200.0 10907.0 1606.3

21472 46.0 68.0 181 200.0 12091.6 1502.4

21563 45.0 61.0 191 200.0 12584.1 1219.4

21672 44.0 58.0 176 200.0 13181.0 844.0

21763 44.0 54.0 181 200.0 13093.3 495.6

21863 44.0 55.0 177 200.0 13243.4 184.4

21972 43.0 50.0 161 200.0 13097.8 0.0

22063 43.0 45.0 166 200.0 12840.0 0.0

22180 43.0 44.0 179 200.0 12091.0 0.0

As can be seen by examining the last four columns of the data (Front, Rear, Front Variance, Rear Variance), the measured front distance starts out at 458cm, well beyond the maximum range of the previous Pulsed Light LIDAR, and the Front Variance stays above 10,000 for all but a handful of readings (the ‘stuck’ threshold is 50). In contrast, the rear distance VL53L0X distance sensor tops out at 200cm, and the data shows that the rear variance does go to zero at the end of the run.

Here’s an Excel plot of the front and rear distances for the first part of the run, just before the robot adjusts to the ‘open doorway’ anomaly:

Front/Rear distances up to the ‘open doorway’ segment

Here’s a plot of the front and rear variances for the same segment:

Front and Rear variance values up to the ‘open doorway’ segment

As can be seen in the above plot, the front variance value stays above 10,000 for the entire portion up to the ‘open doorway’ segment, showing that the new Garmin LIDAR sensor is doing it’s job very nicely.

The next plot shows what happens when the robot reaches the ‘open doorway’ segment. The robot is happily tracking the left wall at a nominal 40cm offset when the left distance goes to 200cm (max sensor range) and the right distance drops to below 50cm – essentially the two side distance measurements trade places). This is the definition of the ‘open doorway’ condition, and this should cause the robot to start tracking the right wall instead of the left.

Left/Right distances up to the ‘open doorway’ segment

The next plot below is the same left/right distance plot, but during the ‘open doorway’ segment.

Left/Right distances during the ‘open doorway’ segment

The robot starts tracking the right-hand wall at about 19,100mSec, and this condition persists to about 19,800mSec, where the left and right distances switch again, and the robot starts tracking the left-hand wall again, as shown in the following plot:

Left/Right distances after the ‘open doorway’ segment

When all three segments are stitched together, you get the following plot:

The ‘open doorway’ segment centered around 19,200Msec is clearly visible, as the left and right distances switch.

Here’s a short video showing the entire run:

New Garmin LIDAR-Lite V4/LED, 4m test wall with ‘open doorway

Stay tuned!

The way forward from here

1 Reply

Posted 31 January 2023

I think I have now arrived at the point where the major sub-systems in my anonymous wall-following robot program are working well now. The last piece (I think) of the puzzle was the offset tracking algorithm (see https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/).

Wall-Track Tuning – just finished (I hope)
Move to Front/Rear/Left/Right Distance – These are generally working now.
Detection of and tracking/homing to a charging station via IR beam.
Error condition detection/handling – this is still an open question to me. The program handles a number of error conditions, but I’m sure there are some error conditions that it doesn’t handle, or doesn’t handle well (the ‘open doorway’ detection and handling issue, for one).

The last full program I see in my Arduino directory is ‘WallE3_WallTrack_V5’, although it appears that _V5 isn’t that much different than _V4. Below are the changes from V4 to V5:

There are a number of changes in _V5’s #pragma OFFSET_CAPTURE section, but as I just discovered in this post, the new wall tracking configuration with PID(350,0,0) and ‘tweak’ divisor 50 (as described at the bottom of this post) means that I don’t need a separate ‘offset capture’ feature at all – the normal offset tracking routine handles the capture portion quite well, thankyou.
There are some very minor changes in _V5’s TrackLeftWallOffset(), but this section will be replaced in its entirety with the algorithm from the above post
V5 has a function called OrientCorr() that doesn’t exist in _V4, but it was just for debugging support.

So, I think I will start by creating yet another Arduino project from scratch, with the intention of building up to a complete working program, with wall offset tracking, charging station detection/docking, and anomalous condition detection/handling. But what to call it? I think I will go with ‘WallE3_Complete_V1’ and see how that works. I think I will need to be careful during it’s construction, as I want to incorporate all the progress I have made in the various ‘part-task’ programs:

WallE3_AnomalyRecovery_V1/V2
WallE3_ChargingStn_V1/V2/V3
WallE3_FrontBackMotionTuning_V1
WallE3_ParallelFind_V1
WallE3_RollingTurn_V1
WallE3_SpinTurnTuning_V2
WallE3_WallTrack_V1-V5
WallE3_WallTrackTuning_V1-V5

I will start by creating WallE3_Complete_V1 as a new blank program, and then going carefully through each of the above ‘part-task’ programs (in alphabetical order each time, just to reduce the confusion factor) to pull in the relevant bits.

Includes:

It looks like the complete #includes section is:

#pragma region INCLUDES
#include <Wire.h>
#include "FlashTxx.h"		// TLC/T3x/T4x flash primitives
#include <elapsedMillis.h>
#include "MPU6050_6Axis_MotionApps612.h"  //01/18/22 changed to use the \I2CDevLib\Arduino\MPU6050\ version
#include "I2C_Anything.h" //needed for sending float data over I2C
#include "timelib.h" //added 01/01/22 for charge monitoring support
#include <math.h>
#include "enums.h" //12/20/2022 added to avoid intellisense errors assoc with enums
#pragma endregion Includes

#pragma region INCLUDES

#include <Wire.h>

#include "FlashTxx.h" // TLC/T3x/T4x flash primitives

#include <elapsedMillis.h>

#include "MPU6050_6Axis_MotionApps612.h" //01/18/22 changed to use the \I2CDevLib\Arduino\MPU6050\ version

#include "I2C_Anything.h" //needed for sending float data over I2C

#include "timelib.h" //added 01/01/22 for charge monitoring support

#include <math.h>

#include "enums.h" //12/20/2022 added to avoid intellisense errors assoc with enums

#pragma endregion Includes

Oddly though, many of my programs don’t #include <wire.h>, but they compile and run fine – no idea why. OK, the reason is – “I2C_Anything.h” also includes <wire.h>. When I look at ‘wire.h’, I see it has a ‘#ifndef TwoWire_h’ statement at the top, so adding #include <wire.h> at the top won’t cause a problem, and I like it just for its informational value.

After copying in the #includes, I right-clicked on the project name and selected ‘add->existing item…’ and added ‘enums.h’, ‘FlashTxx.h’ and ‘FlashTxx.cpp’ from the ‘…\Robot Common Files’ folder. Then I opened a CMD window and used mklink (see this link) to create hard links to ‘board.txt’ and ‘TeensyOTA1.ttl’. At this point, the minimalist program (only #defines and empty, setup() and loop() functions) compiles without error – yay!

#Define Section:

Next in line are all the #Defines:

#pragma region DEFINES
//02/29/16 hardware defines
//#define NO_MOTORS
//#define NO_MPU6050 //added 01/23/22
//#define IR_HOMING_ONLY
//#define NO_LIDAR
//#define NO_VL53L0X //01/08/22 now used for VL53L0X hardware
//#define NO_IRDET //added 04/05/17 for daytime in-atrium testing (too much ambient IR)
//#define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop
//#define NO_STUCK //added 03/10/19 to disable 'stuck' detection
//#define BATTERY_DISCHARGE //added 03/04/20 to discharge battery safely
#define NO_POST //added 04/12/20 to skip all the POST checks

//11/07/2020 moved all I2C Address declarations here
#define IRDET_I2C_ADDR 0x08
#define MPU6050_I2C_ADDR 0x68
#pragma endregion Program #Defines

#pragma region DEFINES

//02/29/16 hardware defines

//#define NO_MOTORS

//#define NO_MPU6050 //added 01/23/22

//#define IR_HOMING_ONLY

//#define NO_LIDAR

//#define NO_VL53L0X //01/08/22 now used for VL53L0X hardware

//#define NO_IRDET //added 04/05/17 for daytime in-atrium testing (too much ambient IR)

//#define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop

//#define NO_STUCK //added 03/10/19 to disable 'stuck' detection

//#define BATTERY_DISCHARGE //added 03/04/20 to discharge battery safely

#define NO_POST //added 04/12/20 to skip all the POST checks

//11/07/2020 moved all I2C Address declarations here

#define IRDET_I2C_ADDR 0x08

#define MPU6050_I2C_ADDR 0x68

#pragma endregion Program #Defines

TIME INTERVALS Section:

#pragma region TIME INTERVALS
const uint16_t MSEC_PER_IR_HOMING_ADJ = 150; //06/27/22 100ms is too fast for GetFrontDistCm()
const int MSEC_PER_DIST_UPDATE = 100; //10/02/22 rev to speed up rate indep of front LIDAR
const uint16_t WALL_TRACK_UPDATE_INTERVAL_MSEC = MSEC_PER_DIST_UPDATE;//10/02/22 rev to make these two intervals the same
const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //10/02/22 added to separate out slow front LIDAR from faster VL53L0X sensors
const uint16_t TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go

elapsedMillis MsecSinceLastAdj; //added 05/24/22 for ParallelOrientation() routine
elapsedMillis MsecSinceLastIRHomingAdj; //01/07/22 used for #ifdef IR_HOMING_ONLY block
elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops
elapsedMillis MsecSinceLastTurnRateUpdate;//heading/rate based turn support
elapsedMillis mSecSinceLastWallTrackUpdate;
elapsedMillis MsecSinceLastFrontDistUpdate; //10/02/22 slower update rate needed for front LIDAR
#pragma endregion TIME INTERVALS

#pragma region TIME INTERVALS

const uint16_t MSEC_PER_IR_HOMING_ADJ = 150; //06/27/22 100ms is too fast for GetFrontDistCm()

const int MSEC_PER_DIST_UPDATE = 100; //10/02/22 rev to speed up rate indep of front LIDAR

const uint16_t WALL_TRACK_UPDATE_INTERVAL_MSEC = MSEC_PER_DIST_UPDATE;//10/02/22 rev to make these two intervals the same

const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //10/02/22 added to separate out slow front LIDAR from faster VL53L0X sensors

const uint16_t TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go

elapsedMillis MsecSinceLastAdj; //added 05/24/22 for ParallelOrientation() routine

elapsedMillis MsecSinceLastIRHomingAdj; //01/07/22 used for #ifdef IR_HOMING_ONLY block

elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops

elapsedMillis MsecSinceLastTurnRateUpdate;//heading/rate based turn support

elapsedMillis mSecSinceLastWallTrackUpdate;

elapsedMillis MsecSinceLastFrontDistUpdate; //10/02/22 slower update rate needed for front LIDAR

#pragma endregion TIME INTERVALS

Note that the above const declarations for MSEC_PER_DIST_UPDATE and FRONT_DISTANCE_UPDATE_INTERVAL_MSEC could just as easily be in the DISTANCE_MEASUREMENT_SUPPORT section, but I decided to try and keep all the timing stuff together. I’ll also put these declarations in the DISTANCE_MEASUREMENT_SUPPORT section but commented out with a pointer to the timing section.

TELEMETRYSTRINGS Section:

#pragma region TELEMETRYSTRINGS
const char* AnomalyStrArray[] = { "NO_ANOMALIES", "STUCK_AHEAD", "STUCK_BEHIND", "OBSTACLE_AHEAD",
"WALL_OFFSET_DIST_AHEAD","OBSTACLE_BEHIND", "OPEN_CORNER", "DEAD_BATTERY", "CHARGER_CONNECTED" };//06/12/22 CHG_STN_AVAIL removed
//const char* TrkStrArray[] = { "None", "Left", "Left Capture", "Right", "Right Capture", "Neither" };
const char* TrkStrArray[] = { "None", "Left", "Right", "Neither" };//01/31/23 removed "_Capture" elements 
const char* IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD\tRearD";
const char* IRHomingTelemStrNoPings = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\n";
const char* LeftWallFollowTelemStr = "Msec\tLFront\tLCtr\tLRear\tFront\tFrontVar\tRear\tSteer\tOutput\tSetPt\tAdjDist\tLSpd\tRSpd";
const char* WallFollowTelemHdrStr = "Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\tR\tRvar\tSteer\tSet\tOutput\tLSpd\tRSpd\tIRAvg\n";
const char* WallFollowTelemStr = "%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\t\t%d\t%2.0f\t%d\t%2.0f\t%2.2f\t%2.2f\t%2.0f\t%d\t%d\t%lu\n";
//const char* ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tRearD\n"; //rev 01/30/21
const char* ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tbChging\n"; //rev 01/30/21
#pragma endregion Mode-specific telemetry header strings

#pragma region TELEMETRYSTRINGS

const char* AnomalyStrArray[] = { "NO_ANOMALIES", "STUCK_AHEAD", "STUCK_BEHIND", "OBSTACLE_AHEAD",

"WALL_OFFSET_DIST_AHEAD","OBSTACLE_BEHIND", "OPEN_CORNER", "DEAD_BATTERY", "CHARGER_CONNECTED" };//06/12/22 CHG_STN_AVAIL removed

//const char* TrkStrArray[] = { "None", "Left", "Left Capture", "Right", "Right Capture", "Neither" };

const char* TrkStrArray[] = { "None", "Left", "Right", "Neither" };//01/31/23 removed "_Capture" elements

const char* IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD\tRearD";

const char* IRHomingTelemStrNoPings = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\n";

const char* LeftWallFollowTelemStr = "Msec\tLFront\tLCtr\tLRear\tFront\tFrontVar\tRear\tSteer\tOutput\tSetPt\tAdjDist\tLSpd\tRSpd";

const char* WallFollowTelemHdrStr = "Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\tR\tRvar\tSteer\tSet\tOutput\tLSpd\tRSpd\tIRAvg\n";

const char* WallFollowTelemStr = "%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\t\t%d\t%2.0f\t%d\t%2.0f\t%2.2f\t%2.2f\t%2.0f\t%d\t%d\t%lu\n";

//const char* ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tRearD\n"; //rev 01/30/21

const char* ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tbChging\n"; //rev 01/30/21

#pragma endregion Mode-specific telemetry header strings

I removed the “_Capture” elements from TrkStrArray[] as these are no longer needed (wall offset capture now just part of TrackLeftRightOffset()). The rest should be OK for now.

DISTANCE_MEASUREMENT_SUPPORT Section:

#pragma region DISTANCE_MEASUREMENT_SUPPORT
//misc LIDAR and Ping sensor parameters
const uint16_t MIN_FRONT_OBSTACLE_DIST_CM = 30; //rev 04/28/17 for better obstacle handling
const uint16_t CHG_STN_AVOIDANCE_DIST_CM = 40; //added 03/11/17 for charge stn avoidance
const uint16_t MAX_FRONT_DISTANCE_CM = 400;
const uint16_t MAX_LR_DISTANCE_CM = 200;  //04/19/15 now using sep parameters for front and side sensors

//04/01/2015 added for 'stuck' detection support
const uint16_t FRONT_DIST_ARRAY_SIZE = 50; //11/22/20 doubled to acct for 10Hz update rate
const uint16_t FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19
const uint16_t LR_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg
const uint16_t REAR_DIST_ARRAY_SIZE = 50; //02/28/22 bumped to 50
const uint16_t LR_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ
const uint16_t STUCK_FRONT_VARIANCE_THRESHOLD = 50; //chg to 50 04/28/17
const uint16_t STUCK_REAR_VARIANCE_THRESHOLD = 50; //added 01/08/22
const uint16_t NO_LIDAR_FRONT_VAR_VAL = 10 * STUCK_FRONT_VARIANCE_THRESHOLD; //01/16/19

//04/28/19 added to reinstate l/r dist running avg
//06/28/20 chg to uint_16 to accommodate change from cm to mm
uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte
uint16_t aLeftDistCM[LR_DIST_ARRAY_SIZE];
uint16_t aRightDistCM[LR_DIST_ARRAY_SIZE];
uint16_t aRearDistCM[REAR_DIST_ARRAY_SIZE];
uint16_t curMinObstacleDistance = MIN_FRONT_OBSTACLE_DIST_CM;//added 03/11/17 for chg stn avoidance

//added 10/24/20
//const int STUCK_BACKUP_DISTANCE_CM = 25;
const uint16_t STUCK_BACKUP_DISTANCE_CM = 20; //11/21/28 shortened slightly
const uint16_t STUCK_FORWARD_DISTANCE_CM = 15;
const uint16_t MAX_REAR_DISTANCE_CM = 100; //03/01/22 rev to use actual experimental results
const uint16_t MIN_REAR_OBSTACLE_DIST_CM = 10;
const uint16_t STUCK_BACKUP_TIME_MSEC = 1000;
const uint16_t STUCK_FORWARD_TIME_MSEC = 1000;
//const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //01/31/23 this const was moved to 'TIME_INTERVALS'

#pragma endregion Distance Measurement Support

#pragma region DISTANCE_MEASUREMENT_SUPPORT

//misc LIDAR and Ping sensor parameters

const uint16_t MIN_FRONT_OBSTACLE_DIST_CM = 30; //rev 04/28/17 for better obstacle handling

const uint16_t CHG_STN_AVOIDANCE_DIST_CM = 40; //added 03/11/17 for charge stn avoidance

const uint16_t MAX_FRONT_DISTANCE_CM = 400;

const uint16_t MAX_LR_DISTANCE_CM = 200; //04/19/15 now using sep parameters for front and side sensors

//04/01/2015 added for 'stuck' detection support

const uint16_t FRONT_DIST_ARRAY_SIZE = 50; //11/22/20 doubled to acct for 10Hz update rate

const uint16_t FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19

const uint16_t LR_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg

const uint16_t REAR_DIST_ARRAY_SIZE = 50; //02/28/22 bumped to 50

const uint16_t LR_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ

const uint16_t STUCK_FRONT_VARIANCE_THRESHOLD = 50; //chg to 50 04/28/17

const uint16_t STUCK_REAR_VARIANCE_THRESHOLD = 50; //added 01/08/22

const uint16_t NO_LIDAR_FRONT_VAR_VAL = 10 * STUCK_FRONT_VARIANCE_THRESHOLD; //01/16/19

//04/28/19 added to reinstate l/r dist running avg

//06/28/20 chg to uint_16 to accommodate change from cm to mm

uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte

uint16_t aLeftDistCM[LR_DIST_ARRAY_SIZE];

uint16_t aRightDistCM[LR_DIST_ARRAY_SIZE];

uint16_t aRearDistCM[REAR_DIST_ARRAY_SIZE];

uint16_t curMinObstacleDistance = MIN_FRONT_OBSTACLE_DIST_CM;//added 03/11/17 for chg stn avoidance

//added 10/24/20

//const int STUCK_BACKUP_DISTANCE_CM = 25;

const uint16_t STUCK_BACKUP_DISTANCE_CM = 20; //11/21/28 shortened slightly

const uint16_t STUCK_FORWARD_DISTANCE_CM = 15;

const uint16_t MAX_REAR_DISTANCE_CM = 100; //03/01/22 rev to use actual experimental results

const uint16_t MIN_REAR_OBSTACLE_DIST_CM = 10;

const uint16_t STUCK_BACKUP_TIME_MSEC = 1000;

const uint16_t STUCK_FORWARD_TIME_MSEC = 1000;

//const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //01/31/23 this const was moved to 'TIME_INTERVALS'

#pragma endregion Distance Measurement Support

Copied this from ‘WallE3_AnomalyRecovery_V2’. It looks pretty complete. Note that I left the ‘STUCK_FORWARD/BACKUP_TIME_MSEC’ declarations here rather than in the ‘Timing’ section. Didn’t seem to warrant the attention.

PIN ASSIGNMENTS Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks pretty complete

MOVE TO DESIRED DISTANCE Section:

//04/25/21 moved here from MoveToDesiredFront/BackDist() functions
//01/31/23 TODO:  Globally change 'OffsetDistKx' to 'MoveToDist_Kx', 'OffsetDistOutput' to 'MoveToDistOutput', 'OffsetDistVal' to 'MoveToDistInput' 
#pragma region MoveToDesiredDist MOVE TO DESIRED DIST SUPPORT
//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/
const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode
const float OffsetDistKi = 0.1f;
const float OffsetDistKd = 0.2f;

//float OffsetDistSetpointCm, OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive
float OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive
double OffsetDistVal = 0; //has to be 'double'
#pragma endregion MoveToDesiredFront/Back/Left/RightDist()

//04/25/21 moved here from MoveToDesiredFront/BackDist() functions

//01/31/23 TODO: Globally change 'OffsetDistKx' to 'MoveToDist_Kx', 'OffsetDistOutput' to 'MoveToDistOutput', 'OffsetDistVal' to 'MoveToDistInput'

#pragma region MoveToDesiredDist MOVE TO DESIRED DIST SUPPORT

//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/

const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode

const float OffsetDistKi = 0.1f;

const float OffsetDistKd = 0.2f;

//float OffsetDistSetpointCm, OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive

float OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive

double OffsetDistVal = 0; //has to be 'double'

#pragma endregion MoveToDesiredFront/Back/Left/RightDist()

I changed the #pragma name from ‘FRONT_BACK OFFSET MOTION PID’ to ‘MOVE TO DESIRED DIST SUPPORT’ as that is a better description of what these parameters do. In addition, the ‘Offsetxxxx’ name is no longer relevant – it should be changed to ‘MoveToDistxxx’, but I don’t want to do that willy-nilly now. I’ll wait until the entire program will compile, and then (after one last check) I’ll make the change globally.

Charge Support Parameters Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

MOTOR_PARAMETERS Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

MPU6050_SUPPORT Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

WALL_FOLLOW_SUPPORT Section:

#pragma region WALL_FOLLOW_SUPPORT
float WALL_OFFSET_TRACK_Kp = 200;//07/09/22 from WallE3_WallTrackTuning.ino
float WALL_OFFSET_TRACK_Ki = 20;//07/09/22 from WallE3_WallTrackTuning.ino
float WALL_OFFSET_TRACK_Kd = 0;
float WallTrackSteerVal, WallTrackOutput, WallTrackSetPoint;
const uint16_t WALL_OFFSET_TGTDIST_CM = 40;
const uint16_t REAR_OBSTACLE_RECOVERY_DISTANCE_CM = 10; //added 09/03/22
const uint16_t WALL_OFFSET_APPR_ANGLE_MINDEG = 30;//added 09/08/21
const float WALL_TRACK_CAPTURE_MAX_STEERING_VAL = 0.8;//09/06/22 added for use in RunToDaylight()
//const float LEFT_WALL_PARALLEL_STEER_VALUE = -0.35; //added 10/08/22
//const float RIGHT_WALL_PARALLEL_STEER_VALUE = 0.25; //added 10/08/22
#pragma endregion WALL_FOLLOW_SUPPORT

#pragma region WALL_FOLLOW_SUPPORT

float WALL_OFFSET_TRACK_Kp = 200;//07/09/22 from WallE3_WallTrackTuning.ino

float WALL_OFFSET_TRACK_Ki = 20;//07/09/22 from WallE3_WallTrackTuning.ino

float WALL_OFFSET_TRACK_Kd = 0;

float WallTrackSteerVal, WallTrackOutput, WallTrackSetPoint;

const uint16_t WALL_OFFSET_TGTDIST_CM = 40;

const uint16_t REAR_OBSTACLE_RECOVERY_DISTANCE_CM = 10; //added 09/03/22

const uint16_t WALL_OFFSET_APPR_ANGLE_MINDEG = 30;//added 09/08/21

const float WALL_TRACK_CAPTURE_MAX_STEERING_VAL = 0.8;//09/06/22 added for use in RunToDaylight()

//const float LEFT_WALL_PARALLEL_STEER_VALUE = -0.35; //added 10/08/22

//const float RIGHT_WALL_PARALLEL_STEER_VALUE = 0.25; //added 10/08/22

#pragma endregion WALL_FOLLOW_SUPPORT

Copied from ‘WallE3_AnomalyRecovery_V2’, except I commented out the ‘LEFT/RIGHT_WALL_PARALLEL_STEER_VALUEs as these are no longer used.

HEADING_AND_RATE_BASED_TURN_PARAMETERS Section:

Only the ‘TurnRate_Kx’ parameters, the ‘HDG_NEAR_MATCH’, HDG_FULL_MATCH, ‘HDG_MIN_MATCH’ and ‘DEFAULT_TURN_RATE’ should be in this section. Everything else should be local to the ‘turn’ functions (I kept the ‘Prev_HdgDeg’ and ‘TurnRatePIDOutput’ at global scope for now to avoid lots of compile errors, but they should also be removed.

PARALLEL_FIND_SUPPORT Section:

#pragma region PARALLEL_FIND_SUPPORT
//05/24/22 added for new RotateToParallelOrientation() routine
//const int ParallelFindKp = 100;   
const float ParallelFindKp = 50.f;
const float ParallelFindKi = 10.f;
const float ParallelFindKd = 0.f;
const float ParallelFindSetpoint = 0.0; //09/22/20 moved here
const float ParallelFindThreshold = 0.01; //05/24/22
float ParallelFindOutput = 0; //05/24/22
#pragma endregion PARALLEL_FIND_SUPPORT

#pragma region PARALLEL_FIND_SUPPORT

//05/24/22 added for new RotateToParallelOrientation() routine

//const int ParallelFindKp = 100;

const float ParallelFindKp = 50.f;

const float ParallelFindKi = 10.f;

const float ParallelFindKd = 0.f;

const float ParallelFindSetpoint = 0.0; //09/22/20 moved here

const float ParallelFindThreshold = 0.01; //05/24/22

float ParallelFindOutput = 0; //05/24/22

#pragma endregion PARALLEL_FIND_SUPPORT

03 February 2023: The entire PARALLEL_FIND_SUPPORT section has been removed, as the new RotateToParallelOrientation() function no longer uses a PID engine

IR_HOMING_SUPPORT Section:

#pragma region IR_HOMING_SUPPORT
//uint8_t IR_HOMING_MODULE_SLAVE_ADDR = 8;  //uint8_t type reqd here for Wire.requestFrom() call //mvd to #define 02/02/23
float IRHomingSetpoint, IRHomingOutput;//10/06/17 chg input variable name to something more descriptive
double IRHomingLRSteeringVal = 0;//04/12/22 has to be double (8 bytes)

//these values all come from IR Homing Teensy via I2C calls
uint32_t IRFinalValue1 = 0;
uint32_t IRFinalValue2 = 0;
uint32_t IRHomingValTotalAvg = 0;

//01/11/22 started over again with WallE3 and home-grown PID algorithm
//01/15/22 reduce Kp to 150, keeping others the same
const float IRHomingKp = 75.f;
const float IRHomingKi = 00.f;
const float IRHomingKd = 40.f; //worse

//04/14/22 chg from const long to const uint32_t
const uint32_t IR_BEAM_DETECTION_CHANNEL_MAX = 2621440;
//const uint32_t IR_BEAM_DETECTION_THRESHOLD = 15000;
const uint32_t IR_BEAM_DETECTION_THRESHOLD = 25000;//05/08/22

const float IR_HOMING_STEERING_VAL_CAPTURE_THRESHOLD = 0.3; //added 01/30/21
const float IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD = 0.8; //added 03/30/21
const uint16_t IRHOMING_VALUE_ARRAY_SIZE = 3; //added 03/16/21 for value average support
long int aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE];//added 03/16/21 for value average support
const uint16_t IRHOMING_IAP_OFFSET_LOW_THRESHOLD_PCT = 75; //03/21/21 if wall offset is less than this relative to IAP offset, then adjust
const uint16_t IRHOMING_IAP_OFFSET_HIGH_THRESHOLD_PCT = 110; //03/23/21 if wall offset is more than this relative to IAP offset, then adjust
//const float IRHOMING_IAP_FINE_TUNE_STEERING_VALUE_THRESHOLD = 0.2; //03/21/21 fine-tune if more off-boresight than this
const float IRHOMING_IAP_STEERING_VALUE_THRESHOLD = 0.8; //05/28/22

//added 05/28/22 for better TurnToHomingBeacon() performance
const uint16_t IRHOMING_STEERVALUE_ARRAY_SIZE = 10;
float aIRHOMINGSTEERVALS[IRHOMING_STEERVALUE_ARRAY_SIZE];
const float IRHOMING_DISTANCE_OFFSET_RATIO = 0.3; //added 06/17/22
#pragma endregion IR_HOMING_SUPPORT

#pragma region IR_HOMING_SUPPORT

//uint8_t IR_HOMING_MODULE_SLAVE_ADDR = 8; //uint8_t type reqd here for Wire.requestFrom() call //mvd to #define 02/02/23

float IRHomingSetpoint, IRHomingOutput;//10/06/17 chg input variable name to something more descriptive

double IRHomingLRSteeringVal = 0;//04/12/22 has to be double (8 bytes)

//these values all come from IR Homing Teensy via I2C calls

uint32_t IRFinalValue1 = 0;

uint32_t IRFinalValue2 = 0;

uint32_t IRHomingValTotalAvg = 0;

//01/11/22 started over again with WallE3 and home-grown PID algorithm

//01/15/22 reduce Kp to 150, keeping others the same

const float IRHomingKp = 75.f;

const float IRHomingKi = 00.f;

const float IRHomingKd = 40.f; //worse

//04/14/22 chg from const long to const uint32_t

const uint32_t IR_BEAM_DETECTION_CHANNEL_MAX = 2621440;

//const uint32_t IR_BEAM_DETECTION_THRESHOLD = 15000;

const uint32_t IR_BEAM_DETECTION_THRESHOLD = 25000;//05/08/22

const float IR_HOMING_STEERING_VAL_CAPTURE_THRESHOLD = 0.3; //added 01/30/21

const float IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD = 0.8; //added 03/30/21

const uint16_t IRHOMING_VALUE_ARRAY_SIZE = 3; //added 03/16/21 for value average support

long int aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE];//added 03/16/21 for value average support

const uint16_t IRHOMING_IAP_OFFSET_LOW_THRESHOLD_PCT = 75; //03/21/21 if wall offset is less than this relative to IAP offset, then adjust

const uint16_t IRHOMING_IAP_OFFSET_HIGH_THRESHOLD_PCT = 110; //03/23/21 if wall offset is more than this relative to IAP offset, then adjust

//const float IRHOMING_IAP_FINE_TUNE_STEERING_VALUE_THRESHOLD = 0.2; //03/21/21 fine-tune if more off-boresight than this

const float IRHOMING_IAP_STEERING_VALUE_THRESHOLD = 0.8; //05/28/22

//added 05/28/22 for better TurnToHomingBeacon() performance

const uint16_t IRHOMING_STEERVALUE_ARRAY_SIZE = 10;

float aIRHOMINGSTEERVALS[IRHOMING_STEERVALUE_ARRAY_SIZE];

const float IRHOMING_DISTANCE_OFFSET_RATIO = 0.3; //added 06/17/22

#pragma endregion IR_HOMING_SUPPORT

Copied these from WallE3_AnomalyRecovery_V2. At some point the ‘IRHomingSetpoint’ variable should be changed from global to local scope, and ‘IRHomingLRSteeringVal’ should be renamed to ‘gl_IRHomingLRSteeringVal’ to show global scope. Same with ‘IRFinalValue1’, ‘IRFinalValue2’ and ‘IRHomingValTotalAvg’

GLOBAL_VARIABLES Section:

#pragma region GLOBAL_VARIABLES
float glBatteryVoltage;
uint16_t glLeftspeednum = MOTOR_SPEED_HALF;
uint16_t glRightspeednum = MOTOR_SPEED_HALF;

NavCases glNavCase = NavCases::NAV_WALLTRK;
WallTrackingCases glTrackingCase = WallTrackingCases::TRACKING_NEITHER; //added 01/05/16
WallTrackingCases glPrevTrackingCase = WallTrackingCases::TRACKING_LEFT; //only used decide which way to turn in the TRACKING_NEITHER case  
OpModes glPrevOpMode = OpModes::MODE_NONE; //added 03/08/17, rev to MODE_NONE 04/04/17
OpModes glCurrentOpMode = OpModes::MODE_NONE; //added 10/13/17 so can use in motor speed setting routines

//04/10/20 added for experiment to port heading based wall tracking from two wheel robot
TrackingState glCurrentTrackingState = TrackingState::TRK_RIGHT_NONE;
TrackingState glPrevTrackingState = TrackingState::TRK_RIGHT_NONE;

int16_t glFinalLeftSpeed = 0;
int16_t glFinalRightSpeed = 0;

//02/15/22 added from FourWD_WallE2_V12.ino
uint16_t glFrontCm = 0;
float glFrontvar = 0;//chg to float 03/23/22
float glRearvar = 0;//chg to float 03/23/22
//float glFrontmean = 0; 
//float glRearmean = 0;//06/12/22 not needed - local var is OK

//03/07/22 prepended 'gl_' 
bool gl_bIRBeamAvail = false;
bool gl_bChgConnect = false;
bool gl_bObstacleAhead = false;
bool gl_bWallOffsetDist = false;//03/27/22 added
bool gl_bObstacleBehind = false;
bool gl_bStuckAhead = false;
bool gl_bStuckAhead_Slow = false;
bool gl_bStuckBehind = false;
bool gl_bDeadBattery = false;

//Sensor data values
//03/01/22 rev to store all dists in cm vs mm
uint16_t glRightFrontCm;
uint16_t glRightCenterCm;
uint16_t glRightRearCm;
uint16_t glRearCm; //added 10/24/20
float glRightSteeringVal; //added 08/06/20

uint16_t glLeftFrontCm;
uint16_t glLeftCenterCm;
uint16_t glLeftRearCm;
float glLeftSteeringVal; //added 08/06/20

bool bVL53L0X_TeensyReady = false; //11/10/20 added to prevent bad data reads during Teensy setup()
Stream* gl_pSerPort = 0; //09/26/22 made global so can use everywhere.


//02/05/22 ported from FourWD_WallTrackTest_V4
#pragma endregion GLOBAL_VARIABLES

#pragma region GLOBAL_VARIABLES

float glBatteryVoltage;

uint16_t glLeftspeednum = MOTOR_SPEED_HALF;

uint16_t glRightspeednum = MOTOR_SPEED_HALF;

NavCases glNavCase = NavCases::NAV_WALLTRK;

WallTrackingCases glTrackingCase = WallTrackingCases::TRACKING_NEITHER; //added 01/05/16

WallTrackingCases glPrevTrackingCase = WallTrackingCases::TRACKING_LEFT; //only used decide which way to turn in the TRACKING_NEITHER case

OpModes glPrevOpMode = OpModes::MODE_NONE; //added 03/08/17, rev to MODE_NONE 04/04/17

OpModes glCurrentOpMode = OpModes::MODE_NONE; //added 10/13/17 so can use in motor speed setting routines

//04/10/20 added for experiment to port heading based wall tracking from two wheel robot

TrackingState glCurrentTrackingState = TrackingState::TRK_RIGHT_NONE;

TrackingState glPrevTrackingState = TrackingState::TRK_RIGHT_NONE;

int16_t glFinalLeftSpeed = 0;

int16_t glFinalRightSpeed = 0;

//02/15/22 added from FourWD_WallE2_V12.ino

uint16_t glFrontCm = 0;

float glFrontvar = 0;//chg to float 03/23/22

float glRearvar = 0;//chg to float 03/23/22

//float glFrontmean = 0;

//float glRearmean = 0;//06/12/22 not needed - local var is OK

//03/07/22 prepended 'gl_'

bool gl_bIRBeamAvail = false;

bool gl_bChgConnect = false;

bool gl_bObstacleAhead = false;

bool gl_bWallOffsetDist = false;//03/27/22 added

bool gl_bObstacleBehind = false;

bool gl_bStuckAhead = false;

bool gl_bStuckAhead_Slow = false;

bool gl_bStuckBehind = false;

bool gl_bDeadBattery = false;

//Sensor data values

//03/01/22 rev to store all dists in cm vs mm

uint16_t glRightFrontCm;

uint16_t glRightCenterCm;

uint16_t glRightRearCm;

uint16_t glRearCm; //added 10/24/20

float glRightSteeringVal; //added 08/06/20

uint16_t glLeftFrontCm;

uint16_t glLeftCenterCm;

uint16_t glLeftRearCm;

float glLeftSteeringVal; //added 08/06/20

bool bVL53L0X_TeensyReady = false; //11/10/20 added to prevent bad data reads during Teensy setup()

Stream* gl_pSerPort = 0; //09/26/22 made global so can use everywhere.

//02/05/22 ported from FourWD_WallTrackTest_V4

#pragma endregion GLOBAL_VARIABLES

It looks like most, if not all, of the global variables associated with TrackingCases, OpModes, and TrackingStates are no longer used. I left them in for now, but will go back through and remove unused vars when WallE3_Complete_V1 is finished.

SETUP():

SERIAL_PORTS Section:

Copied verbatim from ‘WallE3_AnomalyRecovery_V2’ – looks good

PIN_INITIALIZATION, SERIAL_PORTS, I2C_PORTS, MPU6050, VL53L0X_TEENSY Sections:

Copied all these verbatim from ‘WallE3_AnomalyRecovery_V2’ – these haven’t changed in literally years, so shouldn’t be an issue

LR_FRONT DISTANCE ARRAYS, #IFDEF DISTANCES_ONLY, IRDET_TEENSY, #IFDEF IR_HOMING_ONLY, IR_BEAM_STEERVAL_ARRAY, POST_CHECKS Sections:

Copied all these verbatim from ‘WallE3_AnomalyRecovery_V2’ – these haven’t changed in literally years, so shouldn’t be an issue. I did note, however, that the ‘POST_CHECKS’ section always runs as the ‘NO_POST’ #define isn’t used. Will leave as it is for now. Thinking a bit more about this – it seems that what I originally thought would be a potentially long, onerous, and not very useful POST hasn’t turned out that way. It is long and onerous, but very necessary, as it initializes and connects to all the peripheral equipment. So I think I will simply remove the #define NO_POST line, and rename the section that ‘ripples’ the rear LED’s from ‘#pragma POST_CHECKS’ to ‘#pragma FLASH_REAR_LEDS’

This complete the ‘setup()’ function – on to ‘loop()’!

Obviously, the contents of the loop() function varies widely across all the ‘part-task’ programs, so this will probably require a lot of work to ‘harmonize’ all the part-task features into a complete program. I think I will try to go through the ‘part-task’ programs in alpha order to see if I can pick out the salient features that should be included.

WallE3_AnomalyRecovery_V2:

loop() has just three main sections – IR_HOMING, CHARGING, and WALL_TRACKING. The first two above are specific operations associated with homing and connecting to the charging station, and the last one is very simple – it just calls either TrackRightWallOffset() or TrackLeftWallOffset().

WallE3_ChargingStn_V2:

This one has the same three sections as WallE3_AnomalyRecovery_V2 and AFAICT, they are identical.

WallE3_FrontBackMotionTuning_V1:

In addition to the same three sections as WallE3_AnomalyRecovery_V2, this one has ‘PARAMETER CAPTURE’ and ‘FRONT_BACK_MOTION_TEST’ sections. The ‘PARAMETER CAPTURE’ section captures test parameter value input from the user, and the ‘FRONT_BACK_MOTION_TEST’ section actually performs the motion with the user-entered parameters. So, we need to make sure that the actual functions used for testing are copied over and the global front/back motion PID values as well. From the ‘Move to a Specified Distance’ post, I see that the final PID values are (1.5, 0.1, 0.2), and these values were incorporated into the ‘WallE3_WallTrackTuning_V5’ as follows:

#pragma region FRONT_BACK OFFSET MOTION PID
//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/
const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode
const float OffsetDistKi = 0.1f;
const float OffsetDistKd = 0.2f;

float OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive
double OffsetDistVal = 0; //has to be 'double'
#pragma endregion FRONT_BACK OFFSET MOTION PID

#pragma region FRONT_BACK OFFSET MOTION PID

//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/

const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode

const float OffsetDistKi = 0.1f;

const float OffsetDistKd = 0.2f;

float OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive

double OffsetDistVal = 0; //has to be 'double'

#pragma endregion FRONT_BACK OFFSET MOTION PID

Uh-Oh, trouble ahead! When I looked at the OffsetDistKx values copied into _Complete_V1 from WallE3_AnomalyRecovery_V2, I see:

#pragma region FRONT_BACK OFFSET MOTION PID
const float OffsetDistKp = 2.f; //neg value creates PID in REVERSE mode
const float OffsetDistKi = 0.5f;
const float OffsetDistKd = 0.0f;

#pragma region FRONT_BACK OFFSET MOTION PID

const float OffsetDistKp = 2.f; //neg value creates PID in REVERSE mode

const float OffsetDistKi = 0.5f;

const float OffsetDistKd = 0.0f;

so, which set of values is correct? The WallE3_AnomalyRecovery_V2 project was created 9/26/22, while the Move to a Specified Distance, Revisited post is dated 24 December 2022, so much more recent. We’ll at least start with the later PID values of (1.5,0.1,0.2)

Copied the following functions from WallE3_FrontBackMotionTuning_V1 into WallE3_Complete_V1:

bool MoveToDesiredFrontDistCm(uint16_t offsetCm)
bool MoveToDesiredLeftDistCm(uint16_t offsetCm)
bool MoveToDesiredRightDistCm(uint16_t offsetCm)
bool MoveToDesiredRearDistCm(uint16_t offsetCm)

WallE3_ParallelFind_V1:

This program has the same structure in setup – with a PARAMETER CAPTURE section followed by the actual test code, all in setup(). However, when I tested this for functionality, I realized it a) only addressed the ‘left wall tracking’ case, and b) didn’t work even for that case.

So, I spent some quality time with this ‘part-task’ program and got it working fairly well, for both cases. See this post for the details and testing results.

After getting everything working, I copied the RotateToParallelOrientation() function from ‘WallE3_ChargingStn_V2’ into ‘WallE3_Complete_V1’ (in the WALL_TRACK_SUPPORT section) and then replaced the actual code with the code from the latest ‘WallE3_ParallelFind_V1’ part-test program.

After making the copy, there were a number of compile errors due to needed utility functions not being present. From ‘WallE3_ChargingStn_V2’ I copied in the following functions:

Entire HDG_BASED_TURN_SUPPORT section
Entire DISTANCE_MEASUREMENT_SUPPORT section
Entire MOTOR_SUPPORT section
Entire IR_HOMING_SUPPORT section
Entire VL53L0X_SUPPORT section
UpdateAllEnvironmentParameters()
Entire CHARGE_SUPPORT_FUNCTIONS section
copied ‘float glLeftCentCorrCm;’ from WallE3_ParallelFind_V1
IsStuckAhead(), IsStuckBehind(), IsIRBeamAvail(), GetWallOrientDeg(), CorrDistForOrient()

At this point the program still doesn’t compile, but I am going to stop and continue with looking at each part-task program in order, and then I’ll come back to the task of getting ‘Complete’ to compile

WallE3_RollingTurn_V1:

Based on the results described in ‘WallE3 Rolling Turn, Revisited’, I copied the ‘RollingTurn()’ function verbatim into ‘Complete’, and also added the ‘TURN_RATE_UPDATE_INTERVAL_MSEC’ constant to the ‘TIME INTERVALS’ section.

WallE3_SpinTurnTuning_V2:

Based on the ‘WallE3 Spin Turn, Revisited’ post, it looks like the original ‘SpinTurn()’ function is unaffected, but with a different set of PID values. The ‘final’ PID value set is (0.7,0.3,0). I did a file compare of the SpinTurn() functions between the SpinTurnTuning_V2 and ChargingStn_V2 programs, and found that they are functionally identical (ChargingStn_V2 uses TeePrint, and SpinTurnTuning_V2 uses gl_SerPort). So, I copied the SpinTurnTuning_V2 versions of both the SpinTurn() functions (one with and one without Kp/Ki/Kd as input parameters) to ‘Complete_V1’, and copied the Kp,Ki, & Kd values from SpinTurnTuning to ‘Complete_V1’.

WallE3_WallTrack_V1-V5:

From this post I see the following changes through ‘WallE3_WallTrack_V1-V5 series of programs:

WallE3_WallTrack_V2 vs WallE3_WallTrack_V1 (Created: 2/19/2022)

V2 moved all inline tracking code into TrackLeft/RightWallOffset() functions (later ported back into V1 – don’t know why)
V2 changed all ‘double’ declarations to ‘float’ due to change from Mega2560 to T3.5

WallE3_WallTrack_V3 vs WallE3_WallTrack_V2 (Created: 2/22/2022)

V3 Chg left/right/rear dists from mm to cm
V3 Concentrated all environmental updates into UpdateAllEnvironmentParameters();
V3 No longer using GetOpMode()

WallE3_WallTrack_V4 vs WallE3_WallTrack_V3 (Created: 3/25/2022)

V4 Added ‘RollingForwardTurn() function

WallE3_WallTrack_V5 vs WallE3_WallTrack_V4 (Created: 3/25/2022)

No real changes between V5 & V4

I *think* that I copied most pieces in from WallE3_WallTrack_V2, so I’m going to go back through the entire program, looking for V2-V5 differences.

Loop():

I think I have everything above loop() accounted for. Now to try and make some sense of loop(). I need to be cognizant of ‘WallE3_ChargingStn_V2’, ‘WallE3_WallTrack_V5’ and ‘WallE3_AnomalyRecovery_V2’ versions of ‘loop()’. Going through all the programs, I see the following differences in loop().

ChargingStn_V2 adds ‘UpdateAllEnvironmentParameters()’ compared to WallE3_WallTrack_V5.
WallE3_AnomalyRecovery_V2 also adds ‘UpdateAllEnvironmentParameters()’ and in addition changes all ‘myTeePrint.’ to ‘gl_SerPort->’ compared to ChargingStn_V2. So, I copied the WallE3_AnomalyRecovery_V2 versions of IR_HOMING, CHARGING, and WALL_TRACKING into Compare’s loop() function.

So, at this point I have all of the ‘pre-setup’, setup(), and loop() stuff in properly (I hope). This should compile, but probably won’t, so I’ll need to go through the PITA part of figuring out why, and fixing it – oh well.

I got IR_HOMING and CHARGING working (well, at least compiling), and now I’m working on WALL_TRACKING. For this section I need to decide which version of TrackRight/LeftWallOffset to use (or at least start with).

WallE3_WallTrackTuning_V5 ended up with a very successful setup with PID(350,0,0) and offset divisor of 50, but it only addressed left-side wall tracking, and didn’t use the actual ‘TrackLeftWallOffset()’ function. So first we need to move the test code into ‘TrackLeftWallOffset()’, and then port ‘TrackLeftWallOffset()’ into ‘TrackRightWallOffset()’.

Here’s the tracking code from WallE3_WallTrackTuning_V5:

if (isRightWall)
{
  //TrackRightWallOffset(WallTrack_Kp, WallTrack_Ki, WallTrack_Kd, OffCm);
}
else
{
  float lastError = 0;
  float lastInput = 0;
  float lastIval = 0;
  float lastDerror = 0;
  AnomalyCode errcode = CheckForAnomalies();

  //WallTrackSetPoint = -0.35; //04/17/22 this value holds robot parallel to left wall
  WallTrackSetPoint = 0; //04/17/22 this value holds robot parallel to left wall

  //01/14/23 added to bypass offset capture section
  gl_pSerPort->printf("Press any key to start\n");

  gl_pSerPort->printf("Front\tCenter\tRear\tSteer\n");
  while (!gl_pSerPort->available())
  {
	gl_pSerPort->read(); //remove this character from stream
	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	gl_pSerPort->printf("\n%2.2f\t%2.2f\t%2.2f\t%2.2f\n", glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glLeftSteeringVal);
	delay(500);
  }

  gl_pSerPort->printf("\n%s: TrackLeftWallOffset: Start tracking offset of %2.1f cm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n",
	__FILENAME__, OffCm, WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);
  gl_pSerPort->printf(LeftWallFollowTelemHdrStr);

  //Step4: Track the wall using PID algorithm
  mSecSinceLastWallTrackUpdate = 0; //added 09/04/21
  MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

  gl_pSerPort->printf("%s: In TrackLeftWallOffset: before while with errcode = %s\n", __FILENAME__, AnomalyStrArray[errcode]);
  //gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorr\tOffCm\tOffF\tSet\tSteer\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");
  gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorr\tSteer\tTweak\tOffCm\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");
  while (errcode == NO_ANOMALIES && !gl_bIRBeamAvail)
  {
	CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

	if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)
	{
	  mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

	  //03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()
	  UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls
	  errcode = CheckForAnomalies();

	  //from Teensy_7VL53L0X_I2C_Slave_V3.ino: LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post
	  glLeftCentCorrCm = CorrDistForOrient(glLeftCenterCm, glLeftSteeringVal);
	  //float offset_factor = (glLeftCentCorrCm - OffCm) / 10.f;
	  //float offset_factor = (glLeftCentCorrCm - OffCm) / 100.f; //01/28/23 from https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/
	  float offset_factor = (glLeftCentCorrCm - OffCm) / tweak_divisor; //01/28/23 from https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/
	  WallTrackSteerVal = glLeftSteeringVal + offset_factor;

	  ////update motor speeds, skipping bad values
	  ////if (!isnan(WallTrackSteerVal))
	  //if (abs(WallTrackSteerVal) < maxSteerVal)
	  //{
	  //  WallTrackOutput = PIDCalcs(glLeftCentCorrCm, OffCm, lastError, lastInput, lastIval, lastDerror,
	  //    WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);


	  if (!isnan(WallTrackSteerVal))
	  //if (!isnan(glLeftCentCorrCm))
	  {
		//gl_pSerPort->printf("just before PIDCalcs call: WallTrackSetPoint  = %2.2f\n", WallTrackSetPoint);
		//WallTrackOutput = PIDCalcs(glLeftCentCorrCm, OffCm, lastError, lastInput, lastIval, lastDerror,
		//  WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);
		WallTrackOutput = PIDCalcs(WallTrackSteerVal, 0, lastError, lastInput, lastIval, lastDerror,
		  WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

		//04/05/22 have to use local var here, as result could be negative
		int16_t leftSpdnum = MOTOR_SPEED_QTR + WallTrackOutput;
		int16_t rightSpdnum = MOTOR_SPEED_QTR - WallTrackOutput;

		//04/05/22 Left/rightSpdnum can be negative here - watch out!
		rightSpdnum = (rightSpdnum <= MOTOR_SPEED_HALF) ? rightSpdnum : MOTOR_SPEED_HALF; //result can still be neg
		glRightspeednum = (rightSpdnum > 0) ? rightSpdnum : 0; //result here must be positive
		leftSpdnum = (leftSpdnum <= MOTOR_SPEED_HALF) ? leftSpdnum : MOTOR_SPEED_HALF;//result can still be neg
		glLeftspeednum = (leftSpdnum > 0) ? leftSpdnum : 0; //result here must be positive

		MoveAhead(glLeftspeednum, glRightspeednum);

		//gl_pSerPort->printf(WallFollowTelemStr, millis(),
		//  glLeftFrontCm, glLeftCenterCm, glLeftRearCm,
		//  glLeftSteeringVal, orientdeg, orientcos, glLeftCentCorrCm,
		//  WallTrackSetPoint, lastError, WallTrackOutput,
		//  glLeftspeednum, glRightspeednum);
	  }
	//gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorrtOffCm\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");
	//gl_pSerPort->printf("Msec\tLF\tLC\tLR\tLCCorr\tSteer\tTweak
	  gl_pSerPort->printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f",
		millis(), glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glLeftCentCorrCm, glLeftSteeringVal, offset_factor);

	  //\tOffCm\tOffF\tSet\tSteer\tErr           
	  gl_pSerPort->printf("\t%2.2f\t%2.2f",
		//OffCm, offset_factor, WallTrackSetPoint, WallTrackSteerVal, lastError);
		OffCm, lastError);
	  //\tLastI\tLastD\tOut\Lspd\tRspd\n"); 
	  gl_pSerPort->printf("\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\n",
		lastIval, lastDerror, WallTrackOutput, glLeftspeednum, glRightspeednum);
	}
  }
}

100

101

102

103

104

105

106

107

if (isRightWall)

{

//TrackRightWallOffset(WallTrack_Kp, WallTrack_Ki, WallTrack_Kd, OffCm);

}

else

{

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

AnomalyCode errcode = CheckForAnomalies();

//WallTrackSetPoint = -0.35; //04/17/22 this value holds robot parallel to left wall

WallTrackSetPoint = 0; //04/17/22 this value holds robot parallel to left wall

//01/14/23 added to bypass offset capture section

gl_pSerPort->printf("Press any key to start\n");

gl_pSerPort->printf("Front\tCenter\tRear\tSteer\n");

while (!gl_pSerPort->available())

{

gl_pSerPort->read(); //remove this character from stream

GetRequestedVL53l0xValues(VL53L0X_LEFT);

gl_pSerPort->printf("\n%2.2f\t%2.2f\t%2.2f\t%2.2f\n", glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glLeftSteeringVal);

delay(500);

}

gl_pSerPort->printf("\n%s: TrackLeftWallOffset: Start tracking offset of %2.1f cm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n",

__FILENAME__, OffCm, WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

gl_pSerPort->printf(LeftWallFollowTelemHdrStr);

//Step4: Track the wall using PID algorithm

mSecSinceLastWallTrackUpdate = 0; //added 09/04/21

MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

gl_pSerPort->printf("%s: In TrackLeftWallOffset: before while with errcode = %s\n", __FILENAME__, AnomalyStrArray[errcode]);

//gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorr\tOffCm\tOffF\tSet\tSteer\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");

gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorr\tSteer\tTweak\tOffCm\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");

while (errcode == NO_ANOMALIES && !gl_bIRBeamAvail)

{

CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)

{

mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

//03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()

UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls

errcode = CheckForAnomalies();

//from Teensy_7VL53L0X_I2C_Slave_V3.ino: LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

glLeftCentCorrCm = CorrDistForOrient(glLeftCenterCm, glLeftSteeringVal);

//float offset_factor = (glLeftCentCorrCm - OffCm) / 10.f;

//float offset_factor = (glLeftCentCorrCm - OffCm) / 100.f; //01/28/23 from https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/

float offset_factor = (glLeftCentCorrCm - OffCm) / tweak_divisor; //01/28/23 from https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/

WallTrackSteerVal = glLeftSteeringVal + offset_factor;

////update motor speeds, skipping bad values

////if (!isnan(WallTrackSteerVal))

//if (abs(WallTrackSteerVal) < maxSteerVal)

//{

// WallTrackOutput = PIDCalcs(glLeftCentCorrCm, OffCm, lastError, lastInput, lastIval, lastDerror,

// WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

if (!isnan(WallTrackSteerVal))

//if (!isnan(glLeftCentCorrCm))

{

//gl_pSerPort->printf("just before PIDCalcs call: WallTrackSetPoint = %2.2f\n", WallTrackSetPoint);

//WallTrackOutput = PIDCalcs(glLeftCentCorrCm, OffCm, lastError, lastInput, lastIval, lastDerror,

// WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

WallTrackOutput = PIDCalcs(WallTrackSteerVal, 0, lastError, lastInput, lastIval, lastDerror,

WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

//04/05/22 have to use local var here, as result could be negative

int16_t leftSpdnum = MOTOR_SPEED_QTR + WallTrackOutput;

int16_t rightSpdnum = MOTOR_SPEED_QTR - WallTrackOutput;

//04/05/22 Left/rightSpdnum can be negative here - watch out!

rightSpdnum = (rightSpdnum <= MOTOR_SPEED_HALF) ? rightSpdnum : MOTOR_SPEED_HALF; //result can still be neg

glRightspeednum = (rightSpdnum > 0) ? rightSpdnum : 0; //result here must be positive

leftSpdnum = (leftSpdnum <= MOTOR_SPEED_HALF) ? leftSpdnum : MOTOR_SPEED_HALF;//result can still be neg

glLeftspeednum = (leftSpdnum > 0) ? leftSpdnum : 0; //result here must be positive

MoveAhead(glLeftspeednum, glRightspeednum);

//gl_pSerPort->printf(WallFollowTelemStr, millis(),

// glLeftFrontCm, glLeftCenterCm, glLeftRearCm,

// glLeftSteeringVal, orientdeg, orientcos, glLeftCentCorrCm,

// WallTrackSetPoint, lastError, WallTrackOutput,

// glLeftspeednum, glRightspeednum);

}

//gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorrtOffCm\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");

//gl_pSerPort->printf("Msec\tLF\tLC\tLR\tLCCorr\tSteer\tTweak

gl_pSerPort->printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f",

millis(), glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glLeftCentCorrCm, glLeftSteeringVal, offset_factor);

//\tOffCm\tOffF\tSet\tSteer\tErr

gl_pSerPort->printf("\t%2.2f\t%2.2f",

//OffCm, offset_factor, WallTrackSetPoint, WallTrackSteerVal, lastError);

OffCm, lastError);

//\tLastI\tLastD\tOut\Lspd\tRspd\n");

gl_pSerPort->printf("\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\n",

lastIval, lastDerror, WallTrackOutput, glLeftspeednum, glRightspeednum);

}

Comparing the above to TrackLeftWallOffset() from WallE3_WallTrack_V5….

WallE3_WallTrack_V5 has an extra ‘float spinRateDPS = 30;’ line for use in it’s now unneeded ‘OFFSET_CAPTURE’ section.
WallE3_WallTrackTuning_V5 adds ‘MsecSinceLastFrontDistUpdate = 0;’ and I think this is needed for the ‘final’ version.
WallE3_WallTrackTuning_V5 adds ‘&& !gl_bIRBeamAvail’ to the initial ‘while()’ statement
WallE3_WallTrackTuning_V5 computes the ‘offset_factor’ and adds it to WallTrackSteerVal.
The rest of the function is essentially the same, but WallE3_WallTrackTuning_V5 uses a custom inline telemetry output section rather than ‘PrintWallFollowTelemetry();’
WallE3_WallTrackTuning_V5 doesn’t have the line ‘digitalToggle(DURATION_MEASUREMENT_PIN2);’ for debug purposes – I should probably keep this.
WallE3_WallTrackTuning_V5 doesn’t have ‘HandleAnomalousConditions(errcode, TRACKING_LEFT);’ at the end. This should be kept as well.
WallE3_WallTrack_V5 uses ‘myTeePrint.’ instead of ‘gl_SerPort->’

So, I copied ‘TrackLeftWallOffset()’ to Complete_V1 and made the above changes. I only copied in the ‘parameterized’ version of ‘TrackLeftWallOffset()’ – the ‘parameterless’ version is no longer needed.

Removed ‘float spinRateDPS = 30;’
added ‘MsecSinceLastFrontDistUpdate = 0;’
Removed entire OFFSET_CAPTURE section
added ‘&& !gl_bIRBeamAvail’ to the initial ‘while()’ statement
Edited the PIDCalcs line to match WallTrackTuning_V5, except with ‘kp,ki,kd’ symbols instead of ‘WallTrack_Kp, WallTrack_Ki, WallTrack_Kd’
- Replaced ‘PrintWallFollowTelemetry() with custom telemetry printout
kept ‘digitalToggle(DURATION_MEASUREMENT_PIN2);’ for debug purposes
‘HandleAnomalousConditions(errcode, TRACKING_LEFT);’ at the end, and copied in the function code from WallE3_WallTrack_V5.

After finishing all this up and adding some required MISCELLANEOUS section functions, the ‘Complete_V1’ program compiles with only one error, as follows:

WallE3_Complete_V1.ino: 983:114: error: 'TrackRightWallOffset' was not declared in this scope

1	WallE3_Complete_V1.ino: 983:114: error: 'TrackRightWallOffset' was not declared in this scope

This error is due to the fact that I haven’t yet ported the WallE3_WallTrackTuning_V5 code to the ‘right side wall’ case. I think I’ll comment this out for now, until I can confirm that the robot can actually track the left side wall.

04 February 2023 Update:

Well, what a miracle! I commented out the call to ‘TrackRightWallOffset’, compiled the program, ran the left-side tracking test on my ‘two-break’ wall configuration, and it actually worked – YAY!! Here’s a short video showing the run.

Stay tuned!

Frank

WallE3 Wall Tracking, Revisited

Leave a reply

Posted 12/23/22

In the last couple of months I have made some significant improvements in WallE3’s capabilities. I started by completely re-doing the compensation algorithms for the seven ST Micro’s VL53L0X time-of-flight distance sensors (two each 3-element side-looking arrays and one rear-looking distance sensor). I followed this with improvements to both the ‘spin turn’ and ‘rolling turn’ features.

Next, I went back through my ‘MoveToDesiredLeft/Right/Front/RearDistCm()’ family of subroutines and made sure they were all working properly now with the much more accurate distance compensation algorithms. One interesting thing that came out of this effort was the realization that shorter measurement intervals (i.e. 50mSec vs 200mSec) produced an unintended side-effect of making ‘Stuck’ detections much more prevalent. This occurs because the appropriate (front or rear) 50-element distance array fills up much faster at 50mSec/measurement than it does at 200mSec/measurement, so identical (or nearly identical measurements will cause a stuck detection earlier (5 measurements/sec means a 50 element array will fill in 10 sec but 20meas/sec fills the array in 2.5sec. When I used 50mSec/meas in the ‘MoveToDesired…()’ routines, the robot would often exit the routine with a ‘stuck’ error code as it slowed down approaching the desired distance condition. These functions do fine with a more coarse time interval (eliminating the ‘stuck’ declarations), so I went back to 200mSec/measurement.

Now I am going to try to incorporate the above improvements into my previous wall track testing program, ‘WallE3_WallTrackTuning_V4’. As usual, I will start by creating ‘WallE3_WallTrackTuning_V5’ as a clone of ‘_V4’ and start making changes from there.

WallE3_WallTrackTuning_V5:

I am going to try and make WallE3_WallTrackTuning_V5 as ‘clean’ as possible, removing as much ‘dead’ code as possible and consolidating things like sensor measurement intervals.

Timing intervals:

Searching through the code for ‘elapsedMillis’ objects, I see the following global declarations:

elapsedMillis MsecSinceLastTurnRateUpdate;//heading/rate based turn support
elapsedMillis mSecSinceLastWallTrackUpdate;
elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops
elapsedMillis MsecSinceLastFrontDistUpdate; //10/02/22 slower update rate needed for front LIDAR

elapsedMillis MsecSinceLastTurnRateUpdate;//heading/rate based turn support

elapsedMillis mSecSinceLastWallTrackUpdate;

elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops

elapsedMillis MsecSinceLastFrontDistUpdate; //10/02/22 slower update rate needed for front LIDAR

Then I did a search for “MSEC” all upper case and found:

//in 'Distance Measurement Support
const uint16_t MSEC_PER_DIST_UPDATE = 200; //12/10/22 can't go any faster than 200mSec - due to longer meas budg for VL53L0X accuracy

//In 'Wall Follow Support'
const uint16_t TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go reliably
const uint16_t WALL_TRACK_UPDATE_INTERVAL_MSEC = 200;
const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //10/02/22 added to separate out slow front LIDAR from faster VL53L0X sensors
const uint16_t ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC = 200;

//in 'Distance Measurement Support

const uint16_t MSEC_PER_DIST_UPDATE = 200; //12/10/22 can't go any faster than 200mSec - due to longer meas budg for VL53L0X accuracy

//In 'Wall Follow Support'

const uint16_t TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go reliably

const uint16_t WALL_TRACK_UPDATE_INTERVAL_MSEC = 200;

const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //10/02/22 added to separate out slow front LIDAR from faster VL53L0X sensors

const uint16_t ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC = 200;

The front LIDAR sensor starts to generate errors for long distance measurements when the measurement interval falls below 200mSec

The VL53L0X time-of-flight sensors need a ‘measurement time’ of 50mSec or greater. This is handled by the VL53L0X array Teensy, but it means that UpdateAllEnvironmentParameters() shouldn’t be called more frequently than 20HZ.

The MPU6050 can support an update interval of 30mSec or greater, and this time is used for all turning operations.

Telemetry readouts should occur no more than once every 200mSec.

MoveToDesiredFront/Back/Left/RightDistCm()

Based on my recent work on these functions, it looks like PID = (1.5, 0.1, 0.2) will work for all cases, so all I have to do is modify the existing ‘OffsetDistKp/Ki/Kd’ values. Note that in my testing these were parameters to the function call instead of program constants, but now I can go back to just having the desired offset as the only parameter.

So, I copied each of the above functions to WallE3_WallTrackTuning_V5 from WallE3_FrontBackMotionTuning_V1, removed Kp,Ki,Kd from the sig, and replaced all occurrences with OffsetDistKp/Ki/Kd. I also ported the CorrDistForOrient() function, as it is required by the MoveToDesiredLeft/Right() versions

01 January 2023 Update:

After getting the ‘MoveTo…’ functions working, I discovered that ‘RotateToParallelOrientation()’ didn’t work well at all, and in fact found a note from my former self that the function was ‘fatally flawed’ – oops! So, I revisited my ‘WallE3_ParallelFind_V1’ part-task project to see if I could get it to work better now that VL53L0X distances are being reported as float vs integer objects, and after what I hope is much better sensor error compensation. As shown in this post, RotateToParallelOrientation() now works much better, albeit somewhat slowly, with PID = (20,4,0).

Tracking Left Wall with offset =  50 and PID = (10.0,0.0,0.0)
Enter 's' key to start
Msec	LF	LC	LR	Steer
In CaptureWallOffset(Left, 50.0)
R/C/F dists = 16.6	14.2	15.3 Steerval = -0.130, WallAngle = -7.43, Inside
In CaptureWallOffset():turndeg = -37.4
Just before call to  MoveToDesiredLeftDistCm(50.0);

In MoveToDesiredLeftDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2
	MTLD: at start, tgt = 50cm, corr_dist = 13.9, tgt_window = 2.0, front/rear var = 12985.0/790.4
Msec	Left	Steer	LCorr	TgtD	err	Ival	Dval	Out	Speed	FVar	RVar
44703	20.4	0.6	12.6	50	37.4	3.7	37.4	52.3	52	12702.1	754.1
44903	22.1	0.5	14.3	50	35.7	7.3	-1.7	61.1	61	12477.4	722.9
45103	25.7	0.6	15.9	50	34.1	10.7	-1.6	62.1	62	12309.2	687.0
45303	33.3	0.5	23.8	50	26.2	13.3	-7.9	54.2	54	12195.2	652.3
45503	40.9	0.5	28.5	50	21.5	15.5	-4.7	48.7	48	12133.4	616.8
45703	49.9	0.5	33.3	50	16.7	17.1	-4.8	43.1	43	12121.4	583.1
45903	54.3	0.4	44.4	50	5.6	17.7	-11.1	28.3	28	12156.6	555.0
46103	63.5	0.7	31.5	50	18.5	19.6	12.8	44.7	44	12236.3	527.4
46303	67.3	0.7	35.9	50	14.1	21.0	-4.4	42.9	42	12357.8	501.8
46503	79.8	0.3	65.9	50	-15.9	19.4	-30.0	1.5	1	12518.0	489.6
46703	79.8	0.2	72.2	50	-22.2	17.1	-6.3	-15.0	-14	12713.8	474.1
46903	80.6	0.3	66.6	50	-16.6	15.5	5.7	-10.5	-10	12941.9	470.6
47103	81.6	0.6	50.6	50	-0.6	15.4	16.0	11.4	11	13198.9	456.7
	MTLD: Stopped with left dist = 50.6 and errcode = NO_ANOMALIES

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
48315	103.8	70.3	66.2	0.4	0.0	-0.4	-1.6	-9.8	-9
48515	103.8	74.5	68.7	0.6	0.4	-0.6	-4.0	-15.6	-15
48715	103.8	72.2	68.5	0.4	0.6	-0.4	-5.4	-12.8	-12
48915	103.8	74.1	67.4	0.7	0.4	-0.7	-8.1	-21.5	-21
49115	103.8	69.9	67.5	0.2	0.7	-0.2	-9.1	-13.9	-13
49315	103.8	68.7	65.1	0.4	0.2	-0.4	-10.5	-17.7	-17
49515	103.8	70.5	65.9	0.5	0.4	-0.5	-12.4	-21.6	-21
49715	103.8	70.2	66.3	0.4	0.5	-0.4	-13.9	-21.7	-21
49915	103.8	70.3	65.0	0.5	0.4	-0.5	-16.0	-26.6	-26
50115	103.8	65.8	63.0	0.3	0.5	-0.3	-17.2	-22.8	-22
50315	103.8	64.4	60.9	0.3	0.3	-0.3	-18.6	-25.6	-25
50515	103.8	61.8	58.8	0.3	0.3	-0.3	-19.8	-25.8	-25
50715	103.8	60.1	56.0	0.4	0.3	-0.4	-21.4	-29.6	-29
50915	103.8	56.9	53.9	0.3	0.4	-0.3	-22.6	-28.6	-28
51115	103.8	56.3	53.8	0.2	0.3	-0.2	-23.6	-28.6	-28
51315	103.8	55.3	53.2	0.2	0.2	-0.2	-24.4	-28.6	-28
51515	103.8	55.5	52.2	0.3	0.2	-0.3	-25.8	-32.4	-32
51715	103.8	55.0	53.2	0.2	0.3	-0.2	-26.5	-30.1	-30
51915	103.8	56.6	55.2	0.1	0.2	-0.1	-27.0	-29.8	-29
52115	103.8	56.9	56.1	0.1	0.1	-0.1	-27.4	-29.0	-28
52315	103.8	58.3	58.5	-0.0	0.1	0.0	-27.3	-26.9	-26
Exited RTPO coarse tune with hdg/steerval/prev_steerval = 103.8/-0.0/0.1

Tracking Left Wall with offset = 50 and PID = (10.0,0.0,0.0)

Enter 's' key to start

Msec LF LC LR Steer

In CaptureWallOffset(Left, 50.0)

R/C/F dists = 16.6 14.2 15.3 Steerval = -0.130, WallAngle = -7.43, Inside

In CaptureWallOffset():turndeg = -37.4

Just before call to MoveToDesiredLeftDistCm(50.0);

In MoveToDesiredLeftDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2

MTLD: at start, tgt = 50cm, corr_dist = 13.9, tgt_window = 2.0, front/rear var = 12985.0/790.4

Msec Left Steer LCorr TgtD err Ival Dval Out Speed FVar RVar

44703 20.4 0.6 12.6 50 37.4 3.7 37.4 52.3 52 12702.1 754.1

44903 22.1 0.5 14.3 50 35.7 7.3 -1.7 61.1 61 12477.4 722.9

45103 25.7 0.6 15.9 50 34.1 10.7 -1.6 62.1 62 12309.2 687.0

45303 33.3 0.5 23.8 50 26.2 13.3 -7.9 54.2 54 12195.2 652.3

45503 40.9 0.5 28.5 50 21.5 15.5 -4.7 48.7 48 12133.4 616.8

45703 49.9 0.5 33.3 50 16.7 17.1 -4.8 43.1 43 12121.4 583.1

45903 54.3 0.4 44.4 50 5.6 17.7 -11.1 28.3 28 12156.6 555.0

46103 63.5 0.7 31.5 50 18.5 19.6 12.8 44.7 44 12236.3 527.4

46303 67.3 0.7 35.9 50 14.1 21.0 -4.4 42.9 42 12357.8 501.8

46503 79.8 0.3 65.9 50 -15.9 19.4 -30.0 1.5 1 12518.0 489.6

46703 79.8 0.2 72.2 50 -22.2 17.1 -6.3 -15.0 -14 12713.8 474.1

46903 80.6 0.3 66.6 50 -16.6 15.5 5.7 -10.5 -10 12941.9 470.6

47103 81.6 0.6 50.6 50 -0.6 15.4 16.0 11.4 11 13198.9 456.7

MTLD: Stopped with left dist = 50.6 and errcode = NO_ANOMALIES

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

48315 103.8 70.3 66.2 0.4 0.0 -0.4 -1.6 -9.8 -9

48515 103.8 74.5 68.7 0.6 0.4 -0.6 -4.0 -15.6 -15

48715 103.8 72.2 68.5 0.4 0.6 -0.4 -5.4 -12.8 -12

48915 103.8 74.1 67.4 0.7 0.4 -0.7 -8.1 -21.5 -21

49115 103.8 69.9 67.5 0.2 0.7 -0.2 -9.1 -13.9 -13

49315 103.8 68.7 65.1 0.4 0.2 -0.4 -10.5 -17.7 -17

49515 103.8 70.5 65.9 0.5 0.4 -0.5 -12.4 -21.6 -21

49715 103.8 70.2 66.3 0.4 0.5 -0.4 -13.9 -21.7 -21

49915 103.8 70.3 65.0 0.5 0.4 -0.5 -16.0 -26.6 -26

50115 103.8 65.8 63.0 0.3 0.5 -0.3 -17.2 -22.8 -22

50315 103.8 64.4 60.9 0.3 0.3 -0.3 -18.6 -25.6 -25

50515 103.8 61.8 58.8 0.3 0.3 -0.3 -19.8 -25.8 -25

50715 103.8 60.1 56.0 0.4 0.3 -0.4 -21.4 -29.6 -29

50915 103.8 56.9 53.9 0.3 0.4 -0.3 -22.6 -28.6 -28

51115 103.8 56.3 53.8 0.2 0.3 -0.2 -23.6 -28.6 -28

51315 103.8 55.3 53.2 0.2 0.2 -0.2 -24.4 -28.6 -28

51515 103.8 55.5 52.2 0.3 0.2 -0.3 -25.8 -32.4 -32

51715 103.8 55.0 53.2 0.2 0.3 -0.2 -26.5 -30.1 -30

51915 103.8 56.6 55.2 0.1 0.2 -0.1 -27.0 -29.8 -29

52115 103.8 56.9 56.1 0.1 0.1 -0.1 -27.4 -29.0 -28

52315 103.8 58.3 58.5 -0.0 0.1 0.0 -27.3 -26.9 -26

Exited RTPO coarse tune with hdg/steerval/prev_steerval = 103.8/-0.0/0.1

Offset Capture with ‘RotateToParallelOrientation’ ‘at end

02 January 2023 Update:

Starting to make some full-up left wall tracking runs, using the updated code from earlier work. In particular, I am trying to see if my older idea about combining an offset-driven steering angle modifier for the PID tracking algorithm will work. The ‘offset_factor’ incorporates the distance error into the reported steering angle, which in turn is used in the PID machine to drive the combined steering angle to 0.

glLeftCentCorrCm = CorrDistForOrient(glLeftCenterCm, glLeftSteeringVal);
float offset_factor = (glLeftCentCorrCm - OffCm) / 10.f;
WallTrackSteerVal = glLeftSteeringVal + offset_factor;

WallTrackSteerVal = (WallTrackSteerVal > 0.5) ? 0.5 : WallTrackSteerVal;
WallTrackSteerVal = (WallTrackSteerVal < -0.5) ? -0.5 : WallTrackSteerVal;

glLeftCentCorrCm = CorrDistForOrient(glLeftCenterCm, glLeftSteeringVal);

float offset_factor = (glLeftCentCorrCm - OffCm) / 10.f;

WallTrackSteerVal = glLeftSteeringVal + offset_factor;

WallTrackSteerVal = (WallTrackSteerVal > 0.5) ? 0.5 : WallTrackSteerVal;

WallTrackSteerVal = (WallTrackSteerVal < -0.5) ? -0.5 : WallTrackSteerVal;

Here’s an early run:

This worked, ‘sorta’. Part of the problem with this run is the robot’s orientation with respect to the wall at the start of the run. This is supposed to be parallel with the wall, but it obviously isn’t, and I don’t know why. Here’s the data from the ‘RotateToParallelOrientation()’ step

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
108955	27.61	39.40	36.60	0.28	0.00	-0.28	-1.12	-6.72	-6
109155	27.61	40.30	37.10	0.32	0.28	-0.32	-2.40	-8.80	-8
109355	27.61	38.80	36.50	0.23	0.32	-0.23	-3.32	-7.92	-7
109555	27.61	39.50	36.00	0.35	0.23	-0.35	-4.72	-11.72	-11
109755	27.61	40.00	36.90	0.31	0.35	-0.31	-5.96	-12.16	-12
109955	27.61	40.00	37.30	0.27	0.31	-0.27	-7.04	-12.44	-12
110155	27.61	39.80	36.30	0.35	0.27	-0.35	-8.44	-15.44	-15
110355	27.61	40.20	37.00	0.32	0.35	-0.32	-9.72	-16.12	-16
110555	27.61	39.40	36.50	0.29	0.32	-0.29	-10.88	-16.68	-16
110755	27.61	40.30	36.50	0.38	0.29	-0.38	-12.40	-20.00	-20
110955	27.61	40.30	36.80	0.35	0.38	-0.35	-13.80	-20.80	-20
111155	27.61	40.30	35.80	0.45	0.35	-0.45	-15.60	-24.60	-24
111355	27.61	39.90	36.40	0.35	0.45	-0.35	-17.00	-24.00	-24
111555	27.61	38.90	36.00	0.29	0.35	-0.29	-18.16	-23.96	-23
111755	27.61	37.20	35.40	0.18	0.29	-0.18	-18.88	-22.48	-22
111955	27.61	37.70	35.80	0.15	0.18	-0.15	-19.48	-22.48	-22
112155	27.61	37.70	35.70	0.20	0.15	-0.20	-20.28	-24.28	-24
112355	27.61	38.20	36.00	0.22	0.20	-0.22	-21.16	-25.56	-25
112555	27.61	37.50	37.00	0.05	0.22	-0.05	-21.36	-22.36	-22
112755	27.61	38.20	37.70	0.05	0.05	-0.05	-21.56	-22.56	-22
112955	27.61	39.00	38.80	0.02	0.05	-0.02	-21.64	-22.04	-22
113155	27.61	39.40	39.70	-0.03	0.02	0.03	-21.52	-20.92	-20
Exited RTPO coarse tune with hdg/steerval/prev_steerval = 27.6/-0.0/0.0

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

108955 27.61 39.40 36.60 0.28 0.00 -0.28 -1.12 -6.72 -6

109155 27.61 40.30 37.10 0.32 0.28 -0.32 -2.40 -8.80 -8

109355 27.61 38.80 36.50 0.23 0.32 -0.23 -3.32 -7.92 -7

109555 27.61 39.50 36.00 0.35 0.23 -0.35 -4.72 -11.72 -11

109755 27.61 40.00 36.90 0.31 0.35 -0.31 -5.96 -12.16 -12

109955 27.61 40.00 37.30 0.27 0.31 -0.27 -7.04 -12.44 -12

110155 27.61 39.80 36.30 0.35 0.27 -0.35 -8.44 -15.44 -15

110355 27.61 40.20 37.00 0.32 0.35 -0.32 -9.72 -16.12 -16

110555 27.61 39.40 36.50 0.29 0.32 -0.29 -10.88 -16.68 -16

110755 27.61 40.30 36.50 0.38 0.29 -0.38 -12.40 -20.00 -20

110955 27.61 40.30 36.80 0.35 0.38 -0.35 -13.80 -20.80 -20

111155 27.61 40.30 35.80 0.45 0.35 -0.45 -15.60 -24.60 -24

111355 27.61 39.90 36.40 0.35 0.45 -0.35 -17.00 -24.00 -24

111555 27.61 38.90 36.00 0.29 0.35 -0.29 -18.16 -23.96 -23

111755 27.61 37.20 35.40 0.18 0.29 -0.18 -18.88 -22.48 -22

111955 27.61 37.70 35.80 0.15 0.18 -0.15 -19.48 -22.48 -22

112155 27.61 37.70 35.70 0.20 0.15 -0.20 -20.28 -24.28 -24

112355 27.61 38.20 36.00 0.22 0.20 -0.22 -21.16 -25.56 -25

112555 27.61 37.50 37.00 0.05 0.22 -0.05 -21.36 -22.36 -22

112755 27.61 38.20 37.70 0.05 0.05 -0.05 -21.56 -22.56 -22

112955 27.61 39.00 38.80 0.02 0.05 -0.02 -21.64 -22.04 -22

113155 27.61 39.40 39.70 -0.03 0.02 0.03 -21.52 -20.92 -20

Exited RTPO coarse tune with hdg/steerval/prev_steerval = 27.6/-0.0/0.0

This certainly looks good – with a front/rear distance difference of only 0.3cm, and a steering value of 0.02. However, as shown in the following screengrab of the above video, the robot’s orientation just after the parallel find operation is anything but parallel

movie frame grab just after ‘RotateToParallelOrientation()’

I re-instrumented the ‘RotateToParallelOrientation’ function to print out 10 sets of front/back distances directly after completing it’s ‘ParallelFind’ operation, and made another run. The photo below shows the ending orientation, followed by the data

Robot orientation immediately after ‘RotateToParallelOrientation()’

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
33846	36.67	54.60	49.30	0.53	0.00	-0.53	-2.12	-12.72	-12
34046	36.67	54.90	50.10	0.48	0.53	-0.48	-4.04	-13.64	-13
34246	36.67	55.40	50.60	0.48	0.48	-0.48	-5.96	-15.56	-15
34446	36.67	56.60	50.10	0.65	0.48	-0.65	-8.56	-21.56	-21
34646	36.67	53.80	49.20	0.46	0.65	-0.46	-10.40	-19.60	-19
34846	36.67	54.10	48.50	0.56	0.46	-0.56	-12.64	-23.84	-23
35046	36.67	51.10	47.60	0.35	0.56	-0.35	-14.04	-21.04	-21
35246	36.67	49.90	46.30	0.36	0.35	-0.36	-15.48	-22.68	-22
35446	36.67	48.20	45.10	0.31	0.36	-0.31	-16.72	-22.92	-22
35646	36.67	46.70	43.90	0.28	0.31	-0.28	-17.84	-23.44	-23
35846	36.67	45.30	42.30	0.30	0.28	-0.30	-19.04	-25.04	-25
36046	36.67	43.80	41.30	0.25	0.30	-0.25	-20.04	-25.04	-25
36246	36.67	42.80	41.20	0.16	0.25	-0.16	-20.68	-23.88	-23
36446	36.67	42.10	40.90	0.12	0.16	-0.12	-21.16	-23.56	-23
36646	36.67	41.90	39.70	0.22	0.12	-0.22	-22.04	-26.44	-26
36846	36.67	41.60	39.80	0.18	0.22	-0.18	-22.76	-26.36	-26
37046	36.67	40.90	40.40	0.05	0.18	-0.05	-22.96	-23.96	-23
37246	36.67	40.50	40.30	0.02	0.05	-0.02	-23.04	-23.44	-23
37446	36.67	41.80	40.80	0.10	0.02	-0.10	-23.44	-25.44	-25
37646	36.67	42.40	42.00	0.04	0.10	-0.04	-23.60	-24.40	-24
37846	36.67	42.50	43.30	-0.08	0.04	0.08	-23.28	-21.68	-21
Exited RTPO coarse tune with hdg/steerval/prev_steerval = 36.67/-0.08/0.04
37854	36.67	42.50	43.30	-0.08
38057	36.67	44.00	44.50	-0.05
38259	36.67	44.00	44.10	-0.01
38462	36.67	43.50	44.00	-0.05
38664	36.67	43.50	44.10	-0.06
38867	36.67	43.20	44.00	-0.08
39069	36.67	43.50	44.30	-0.08
39272	36.67	43.50	44.30	-0.08
39474	36.67	44.40	44.50	-0.01
39676	36.67	44.40	44.20	0.02

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

33846 36.67 54.60 49.30 0.53 0.00 -0.53 -2.12 -12.72 -12

34046 36.67 54.90 50.10 0.48 0.53 -0.48 -4.04 -13.64 -13

34246 36.67 55.40 50.60 0.48 0.48 -0.48 -5.96 -15.56 -15

34446 36.67 56.60 50.10 0.65 0.48 -0.65 -8.56 -21.56 -21

34646 36.67 53.80 49.20 0.46 0.65 -0.46 -10.40 -19.60 -19

34846 36.67 54.10 48.50 0.56 0.46 -0.56 -12.64 -23.84 -23

35046 36.67 51.10 47.60 0.35 0.56 -0.35 -14.04 -21.04 -21

35246 36.67 49.90 46.30 0.36 0.35 -0.36 -15.48 -22.68 -22

35446 36.67 48.20 45.10 0.31 0.36 -0.31 -16.72 -22.92 -22

35646 36.67 46.70 43.90 0.28 0.31 -0.28 -17.84 -23.44 -23

35846 36.67 45.30 42.30 0.30 0.28 -0.30 -19.04 -25.04 -25

36046 36.67 43.80 41.30 0.25 0.30 -0.25 -20.04 -25.04 -25

36246 36.67 42.80 41.20 0.16 0.25 -0.16 -20.68 -23.88 -23

36446 36.67 42.10 40.90 0.12 0.16 -0.12 -21.16 -23.56 -23

36646 36.67 41.90 39.70 0.22 0.12 -0.22 -22.04 -26.44 -26

36846 36.67 41.60 39.80 0.18 0.22 -0.18 -22.76 -26.36 -26

37046 36.67 40.90 40.40 0.05 0.18 -0.05 -22.96 -23.96 -23

37246 36.67 40.50 40.30 0.02 0.05 -0.02 -23.04 -23.44 -23

37446 36.67 41.80 40.80 0.10 0.02 -0.10 -23.44 -25.44 -25

37646 36.67 42.40 42.00 0.04 0.10 -0.04 -23.60 -24.40 -24

37846 36.67 42.50 43.30 -0.08 0.04 0.08 -23.28 -21.68 -21

Exited RTPO coarse tune with hdg/steerval/prev_steerval = 36.67/-0.08/0.04

37854 36.67 42.50 43.30 -0.08

38057 36.67 44.00 44.50 -0.05

38259 36.67 44.00 44.10 -0.01

38462 36.67 43.50 44.00 -0.05

38664 36.67 43.50 44.10 -0.06

38867 36.67 43.20 44.00 -0.08

39069 36.67 43.50 44.30 -0.08

39272 36.67 43.50 44.30 -0.08

39474 36.67 44.40 44.50 -0.01

39676 36.67 44.40 44.20 0.02

According to the photo, the robot is definitely not oriented parallel to the wall. However, according to the telemetry data, it is (44.4 front, 44.2 rear, steerval = 0.02). Even curioser, the actual physical measurements taken using a tape measure show that the front/rear distances are about 47/44cm, or a steerval of about 0.3! Something is definitely wrong here.

Uncommented the #DISTANCES_ONLY define and re-ran, with the robot position/orientation unchanged:

Msec	LFront	LCenter	LRear	RFront	RCenter	RRear	Rear	LSteer	RSteer	LIDAR
8150	44.60	46.00	44.40	56.00	58.70	785.60	91.30	0.020	-72.960	0.00
8302	43.30	45.80	44.10	56.00	58.40	785.60	82.30	-0.080	-72.960	0.00
8502	44.00	45.80	44.70	56.90	59.50	785.60	85.60	-0.070	-72.870	0.00
8702	43.50	45.70	44.10	56.90	59.80	785.60	86.10	-0.060	-72.870	0.00
8902	43.70	46.00	44.30	56.80	58.70	785.60	94.60	-0.060	-72.880	0.00
9102	44.20	46.00	45.10	56.90	60.10	785.60	93.10	-0.090	-72.870	0.00
9302	43.00	46.70	44.70	56.30	56.80	785.60	80.50	-0.170	-72.800	0.00
9502	44.10	45.60	45.20	56.70	59.90	785.60	91.60	-0.110	-72.890	0.00
9702	44.10	46.00	44.30	56.90	59.80	785.60	81.00	-0.070	-72.870	0.00
9902	44.50	45.70	44.40	56.10	60.40	785.60	93.00	0.010	-72.950	0.00
10102	44.60	46.20	44.40	57.00	58.20	785.60	82.80	0.020	-72.860	0.00
10302	43.50	45.70	45.30	57.70	57.20	785.60	91.10	-0.180	-72.790	0.00
10502	44.20	46.30	44.50	58.00	58.40	785.60	80.00	-0.030	-72.760	0.00
10702	42.60	47.00	45.30	57.40	59.60	785.60	90.40	-0.270	-72.820	0.00
10902	43.50	45.60	45.20	56.30	59.40	785.60	84.60	-0.170	-72.930	0.00
11102	44.00	46.60	45.00	56.20	58.90	72.60	92.10	-0.100	-1.640	0.00
11302	42.70	46.30	44.90	57.30	58.40	785.60	80.80	-0.220	-72.830	0.00
11502	44.00	45.70	44.80	56.00	58.20	785.60	93.90	-0.080	-72.960	0.00
11702	43.20	46.00	44.70	57.70	58.30	785.60	81.40	-0.150	-72.790	0.00

Msec	LFront	LCenter	LRear	RFront	RCenter	RRear	Rear	LSteer	RSteer	LIDAR
11902	43.50	46.00	44.80	56.90	58.60	785.60	96.80	-0.130	-72.870	0.00
12102	43.40	45.50	44.20	56.60	59.60	785.60	81.80	-0.080	-72.900	0.00
12302	44.30	45.50	45.30	57.10	59.40	785.60	88.40	-0.100	-72.850	0.00
12502	43.80	45.30	45.10	56.90	58.50	785.60	88.60	-0.130	-72.870	0.00
12702	43.70	46.30	44.90	57.70	60.50	785.60	85.20	-0.120	-72.830	0.00
12902	43.50	45.70	45.10	56.10	60.00	785.60	97.40	-0.160	-72.950	0.00
13102	43.50	46.20	44.50	55.90	61.10	785.60	84.60	-0.100	-72.970	0.00
13302	44.00	46.40	44.60	57.60	61.60	785.60	93.60	-0.060	-72.800	0.00
13502	43.40	46.80	44.40	56.40	59.00	785.60	81.60	-0.100	-72.920	0.00
13702	43.60	46.40	44.30	56.50	60.50	785.60	91.90	-0.070	-72.910	0.00
13902	43.20	46.20	45.50	56.10	60.10	785.60	81.70	-0.230	-72.950	0.00
14102	43.40	46.80	44.80	56.40	59.50	67.30	91.70	-0.140	-1.090	0.00
14302	43.50	46.10	45.10	57.00	58.40	785.60	80.60	-0.160	-72.860	0.00
14502	44.50	46.50	45.00	56.10	58.60	785.60	90.90	-0.050	-72.950	0.00

Msec LFront LCenter LRear RFront RCenter RRear Rear LSteer RSteer LIDAR

8150 44.60 46.00 44.40 56.00 58.70 785.60 91.30 0.020 -72.960 0.00

8302 43.30 45.80 44.10 56.00 58.40 785.60 82.30 -0.080 -72.960 0.00

8502 44.00 45.80 44.70 56.90 59.50 785.60 85.60 -0.070 -72.870 0.00

8702 43.50 45.70 44.10 56.90 59.80 785.60 86.10 -0.060 -72.870 0.00

8902 43.70 46.00 44.30 56.80 58.70 785.60 94.60 -0.060 -72.880 0.00

9102 44.20 46.00 45.10 56.90 60.10 785.60 93.10 -0.090 -72.870 0.00

9302 43.00 46.70 44.70 56.30 56.80 785.60 80.50 -0.170 -72.800 0.00

9502 44.10 45.60 45.20 56.70 59.90 785.60 91.60 -0.110 -72.890 0.00

9702 44.10 46.00 44.30 56.90 59.80 785.60 81.00 -0.070 -72.870 0.00

9902 44.50 45.70 44.40 56.10 60.40 785.60 93.00 0.010 -72.950 0.00

10102 44.60 46.20 44.40 57.00 58.20 785.60 82.80 0.020 -72.860 0.00

10302 43.50 45.70 45.30 57.70 57.20 785.60 91.10 -0.180 -72.790 0.00

10502 44.20 46.30 44.50 58.00 58.40 785.60 80.00 -0.030 -72.760 0.00

10702 42.60 47.00 45.30 57.40 59.60 785.60 90.40 -0.270 -72.820 0.00

10902 43.50 45.60 45.20 56.30 59.40 785.60 84.60 -0.170 -72.930 0.00

11102 44.00 46.60 45.00 56.20 58.90 72.60 92.10 -0.100 -1.640 0.00

11302 42.70 46.30 44.90 57.30 58.40 785.60 80.80 -0.220 -72.830 0.00

11502 44.00 45.70 44.80 56.00 58.20 785.60 93.90 -0.080 -72.960 0.00

11702 43.20 46.00 44.70 57.70 58.30 785.60 81.40 -0.150 -72.790 0.00

Msec LFront LCenter LRear RFront RCenter RRear Rear LSteer RSteer LIDAR

11902 43.50 46.00 44.80 56.90 58.60 785.60 96.80 -0.130 -72.870 0.00

12102 43.40 45.50 44.20 56.60 59.60 785.60 81.80 -0.080 -72.900 0.00

12302 44.30 45.50 45.30 57.10 59.40 785.60 88.40 -0.100 -72.850 0.00

12502 43.80 45.30 45.10 56.90 58.50 785.60 88.60 -0.130 -72.870 0.00

12702 43.70 46.30 44.90 57.70 60.50 785.60 85.20 -0.120 -72.830 0.00

12902 43.50 45.70 45.10 56.10 60.00 785.60 97.40 -0.160 -72.950 0.00

13102 43.50 46.20 44.50 55.90 61.10 785.60 84.60 -0.100 -72.970 0.00

13302 44.00 46.40 44.60 57.60 61.60 785.60 93.60 -0.060 -72.800 0.00

13502 43.40 46.80 44.40 56.40 59.00 785.60 81.60 -0.100 -72.920 0.00

13702 43.60 46.40 44.30 56.50 60.50 785.60 91.90 -0.070 -72.910 0.00

13902 43.20 46.20 45.50 56.10 60.10 785.60 81.70 -0.230 -72.950 0.00

14102 43.40 46.80 44.80 56.40 59.50 67.30 91.70 -0.140 -1.090 0.00

14302 43.50 46.10 45.10 57.00 58.40 785.60 80.60 -0.160 -72.860 0.00

14502 44.50 46.50 45.00 56.10 58.60 785.60 90.90 -0.050 -72.950 0.00

In the above data, the front distance varies from 42.6 to 44.6cm with an average of 43.7cm, and the rear distance varies from 44.1 to 45.5cm with an average of 44.8cm.

So the program thinks the front/back distances are closer together than the tape measure does (44.5/45.0 vs 47/44). This is a pretty big discrepancy. Rotating the robot to be physically parallel with front/rear distances = 40cm, I get:

Msec	LFront	LCenter	LRear	RFront	RCenter	RRear	Rear	LSteer	RSteer	LIDAR
287902	38.40	41.10	38.10	78.60	92.40	785.60	819.00	0.030	-70.700	0.00
288102	38.70	40.50	38.20	76.70	94.20	785.60	750.30	0.050	-70.890	0.00
288302	39.00	40.40	38.30	77.10	93.40	785.60	819.00	0.070	-70.850	0.00
288502	38.60	40.70	38.30	78.70	93.50	785.60	819.00	0.030	-70.690	0.00
288702	39.20	40.50	38.10	75.80	92.70	785.60	750.30	0.110	-70.750	0.00
288902	38.90	41.60	37.70	78.10	91.90	92.20	819.00	0.120	-1.410	0.00
289102	38.30	40.80	38.10	77.90	92.70	87.60	750.30	0.020	-0.970	0.00
289302	38.20	41.40	37.60	78.50	92.00	785.60	819.00	0.060	-70.710	0.00
289502	39.60	40.50	37.90	79.20	91.40	84.90	750.30	0.170	-0.570	0.00
289702	39.20	40.40	38.30	76.70	94.80	785.60	819.00	0.090	-70.890	0.00
289902	38.90	39.50	38.00	78.10	92.70	92.70	750.30	0.090	-1.460	0.00
290102	39.10	41.10	37.70	78.00	92.40	785.60	819.00	0.140	-70.760	0.00
290302	38.50	40.60	38.10	77.50	93.50	785.60	750.30	0.040	-70.810	0.00
290502	38.80	41.10	36.70	77.10	93.10	95.00	819.00	0.210	-1.790	0.00
290702	38.90	41.30	37.50	77.30	93.10	91.70	750.30	0.140	-1.440	0.00

Msec LFront LCenter LRear RFront RCenter RRear Rear LSteer RSteer LIDAR

287902 38.40 41.10 38.10 78.60 92.40 785.60 819.00 0.030 -70.700 0.00

288102 38.70 40.50 38.20 76.70 94.20 785.60 750.30 0.050 -70.890 0.00

288302 39.00 40.40 38.30 77.10 93.40 785.60 819.00 0.070 -70.850 0.00

288502 38.60 40.70 38.30 78.70 93.50 785.60 819.00 0.030 -70.690 0.00

288702 39.20 40.50 38.10 75.80 92.70 785.60 750.30 0.110 -70.750 0.00

288902 38.90 41.60 37.70 78.10 91.90 92.20 819.00 0.120 -1.410 0.00

289102 38.30 40.80 38.10 77.90 92.70 87.60 750.30 0.020 -0.970 0.00

289302 38.20 41.40 37.60 78.50 92.00 785.60 819.00 0.060 -70.710 0.00

289502 39.60 40.50 37.90 79.20 91.40 84.90 750.30 0.170 -0.570 0.00

289702 39.20 40.40 38.30 76.70 94.80 785.60 819.00 0.090 -70.890 0.00

289902 38.90 39.50 38.00 78.10 92.70 92.70 750.30 0.090 -1.460 0.00

290102 39.10 41.10 37.70 78.00 92.40 785.60 819.00 0.140 -70.760 0.00

290302 38.50 40.60 38.10 77.50 93.50 785.60 750.30 0.040 -70.810 0.00

290502 38.80 41.10 36.70 77.10 93.10 95.00 819.00 0.210 -1.790 0.00

290702 38.90 41.30 37.50 77.30 93.10 91.70 750.30 0.140 -1.440 0.00

When the robot is physically parallel, the reported front distance varies from 38.2 to 39.2cm with an average of 38.8cm, and the rear distance varies from 36.7 to 38.3cm with an average of 37.9. The left steering value varies from 0.02 (38.8/38.1) to 0.14 (38.9/37.5) with an average of 0.09.

Well, it looks like the average reported distances and steering values are pretty close to reality, so maybe my original calibration efforts aren’t entirely screwed up. However, it is abundantly clear at this point that my current ‘RotateToParallelOrientation()’ algorithm isn’t reliable, due to very noisy distance value measurements.

01/09/23 Update:

After getting ‘RotateToParallelOrientation()’ working better (now it just uses the array front/rear distance measurements to calculate the off-parallel angle, and then does a ‘SpinTurn’ by that amount), I resumed the effort (see the 02 January Update above) trying to determine if my older algorithm for combining the raw steering value with an ‘offset adjustment factor’ based on the robot’s distance from the desired offset distance would now work better given the improvements I have made in VL53L0X sensor error compensation and off-parallel distance measurement compensation.

As it turns out, the answer seems to be ‘no’. After a multitude of runs with my test wall set up for two 30-45deg ‘breaks’, I couldn’t find any set of PID values that would allow the robot to track the wall – it always either took off for parts unknown, or crashed into the wall at some point.

So, back to the original algorithm of using the wall offset distance directly in the PID engine.

11 January 2023 Update:

I’m confused – not an unusual state for me to be in – but still…..

After all the above improvements, I still was unable to produce reasonable tracking performance using either the steering value or the offset distance as the parameter to be controlled. And, even more confusing, I have an entire post dedicated to demonstrating successful wall tracking using the orientation-angle-corrected distance to the wall as the input to the PID engine, with the desired wall offset as the setpoint, as shown here:

WallTrackOutput = PIDCalcs(glLeftCentCorrCm, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,
            kp, ki, kd);

1 2	WallTrackOutput = PIDCalcs(glLeftCentCorrCm, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror, kp, ki, kd);

With this algorithm, I settled on PID(3,0,1) as the best parameter set, with the result shown in this short video (copied from the above post):

Wall tracking using corrected distance measure as input, and desired offset as the set-point

And then, I have another post demonstrating that using the steering value as input and 0 as the setpoint also works, as shown in this short video with PID(300,0,300)

Right-side wall tracking using steering value as input with PID = (300,0,300)

Here’s the data and short video from a run on my longer ‘4 meter’ test range with two 30º breaks:

In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00
R/C/F dists = 136	117	127 Steerval = -0.090, CutAngle = 29.00
(cutAngleDeg > 0 && abs(cutAngleDeg) < 30 -- setting to +30
R/C/F dists = 136	117	127 Steerval = -0.090, SteerAngle = -5.14, CutAngle = 30.00
CutAngDeg > 0, SteerAngleDeg < cutAngleDeg
In SpinTurn(CW, 35.14, 30.00)
Init hdg = -0.08 deg, Turn = 35.14  deg, tgt = 35.06 deg, timeout = 2.34 sec

Msec	Hdg	PrvHdg	dHdg	Rate	tgtDPS	err	Kp*err	Ival	Kd*Derr	speed	Match	Slope
11361	0.05	-0.08	0.13	4.49	30.00	25.51	45.92	25.51	10.20	61	0.81	0.81
11391	1.28	0.05	1.23	40.88	30.00	-10.88	-19.58	14.63	-14.56	50	0.81	0.01
11421	3.31	1.28	2.03	67.70	30.00	-37.70	-67.86	0.00	-10.73	50	0.82	0.01
11451	5.52	3.31	2.22	73.85	30.00	-43.85	-78.94	0.00	-2.46	50	0.84	0.01
11481	7.87	5.52	2.34	78.13	30.00	-48.13	-86.64	0.00	-1.71	50	0.85	0.01
11511	10.30	7.87	2.43	81.05	30.00	-51.05	-91.89	0.00	-1.17	50	0.86	0.01
11541	12.83	10.30	2.53	84.47	30.00	-54.47	-98.05	0.00	-1.37	50	0.88	0.01
11571	15.41	12.83	2.58	85.85	30.00	-55.85	-100.54	0.00	-0.55	50	0.89	0.01
11601	18.03	15.41	2.62	87.32	30.00	-57.32	-103.18	0.00	-0.59	50	0.91	0.01
11631	20.70	18.03	2.67	89.17	30.00	-59.17	-106.50	0.00	-0.74	50	0.92	0.01
11661	23.36	20.70	2.66	88.50	30.00	-58.50	-105.30	0.00	0.27	50	0.93	0.01
11691	26.02	23.36	2.66	88.82	30.00	-58.82	-105.87	0.00	-0.13	50	0.95	0.01
11721	28.79	26.02	2.76	92.13	30.00	-62.13	-111.83	0.00	-1.32	50	0.97	0.02
11751	31.48	28.79	2.69	89.76	30.00	-59.76	-107.57	0.00	0.94	50	0.98	0.01
11781	34.20	31.48	2.72	90.74	30.00	-60.74	-109.33	0.00	-0.39	50	1.00	0.02
Completed turn with yaw = 34.20, tgt = 35.06, and match = 0.995
fudgeFactorMM = -100

at approach start: cut angle = 30.000, adjfactor = 0.866, num = 400.00, tgt dist = 361.88
At start - err = prev_err = -138
R/C/F dists = 156	208	223 Steerval = 0.670, SteerAngle = 38.29, CutAngle = 30.00, err/prev_err = -138
Msec	LFront	LCtr	LRear	Front	Rear	Err	P_err
13913	228	206	155	170	206	-133	-138
14020	250	231	179	173	243	-111	-133
14127	312	310	237	168	304	-49	-111
14234	381	395	313	159	393	19	-49
At end of offset capture - prev_res*res = -931
correct back to parallel
In SpinTurn(CCW, 30.00, 30.00)
Init hdg = 45.41 deg, Turn = 30.00  deg, tgt = 15.41 deg, timeout = 2.00 sec

Msec	Hdg	PrvHdg	dHdg	Rate	tgtDPS	err	Kp*err	Ival	Kd*Derr	speed	Match	Slope
14294	45.26	45.41	-0.15	5.12	30.00	24.88	44.79	24.88	9.95	59	0.83	0.83
14324	44.29	45.26	-0.98	32.52	30.00	-2.52	-4.53	22.37	-10.96	50	0.84	0.01
14354	42.61	44.29	-1.68	55.95	30.00	-25.95	-46.71	0.00	-9.37	50	0.85	0.01
14384	40.64	42.61	-1.96	65.48	30.00	-35.48	-63.87	0.00	-3.81	50	0.86	0.01
14414	38.30	40.64	-2.34	78.08	30.00	-48.08	-86.55	0.00	-5.04	50	0.87	0.01
14444	35.51	38.30	-2.79	92.90	30.00	-62.90	-113.22	0.00	-5.93	50	0.89	0.02
14474	32.31	35.51	-3.21	106.88	30.00	-76.88	-138.39	0.00	-5.59	50	0.91	0.02
14504	28.88	32.31	-3.42	114.15	30.00	-84.15	-151.47	0.00	-2.91	50	0.93	0.02
14534	25.25	28.88	-3.64	121.22	30.00	-91.22	-164.19	0.00	-2.83	50	0.95	0.02
14564	21.33	25.25	-3.91	130.41	30.00	-100.41	-180.74	0.00	-3.68	50	0.97	0.02
14594	17.50	21.33	-3.84	127.92	30.00	-97.92	-176.26	0.00	1.00	50	0.99	0.02
14624	13.63	17.50	-3.87	128.94	30.00	-98.94	-178.09	0.00	-0.41	50	0.99	0.00
Completed turn with yaw = 13.63, tgt = 15.41, and match = 0.990

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
15692 	389	416	377 	0.14	0.00	-0.14	0.00 	-40.80	0.00	-0.00 	-40.80	34	115
15742 	390	404	377 	0.13	0.00	-0.13	0.00 	-40.20	0.00	0.00 	-40.20	34	115
15792 	391	405	371 	0.20	0.00	-0.20	0.00 	-61.50	0.00	-0.00 	-61.50	13	136
15842 	386	412	378 	0.09	0.00	-0.09	0.00 	-27.60	0.00	0.00 	-27.60	47	102
15892 	386	412	378 	0.09	0.00	-0.09	0.00 	-27.60	0.00	0.00 	-27.60	47	102
15942 	386	405	383 	0.04	0.00	-0.04	0.00 	-10.50	0.00	0.00 	-10.50	64	85
15992 	386	413	383 	0.04	0.00	-0.04	0.00 	-12.90	0.00	-0.00 	-12.90	62	87
16042 	388	407	381 	0.08	0.00	-0.08	0.00 	-23.10	0.00	-0.00 	-23.10	51	98
16092 	381	411	388 	-0.06	0.00	0.06	0.00 	17.70	0.00	0.00 	17.70	92	57
16142 	370	403	381 	-0.11	0.00	0.11	0.00 	32.10	0.00	0.00 	32.10	107	42
16192 	376	394	380 	-0.05	0.00	0.05	0.00 	13.80	0.00	-0.00 	13.80	88	61
16242 	366	384	374 	-0.10	0.00	0.10	0.00 	28.80	0.00	0.00 	28.80	103	46
16292 	362	384	374 	-0.10	0.00	0.10	0.00 	28.80	0.00	0.00 	28.80	103	46
16342 	362	375	370 	-0.10	0.00	0.10	0.00 	31.50	0.00	0.00 	31.50	106	43
16392 	351	372	357 	-0.09	0.00	0.09	0.00 	26.40	0.00	-0.00 	26.40	101	48
16442 	337	364	346 	-0.13	0.00	0.13	0.00 	37.80	0.00	0.00 	37.80	112	37
16492 	332	352	321 	0.06	0.00	-0.06	0.00 	-18.60	0.00	-0.00 	-18.60	56	93
16542 	325	336	319 	-0.00	0.00	0.00	0.00 	1.20	0.00	0.00 	1.20	76	73
16592 	315	323	308 	-0.01	0.00	0.01	0.00 	2.10	0.00	0.00 	2.10	77	72
16642 	310	324	300 	0.02	0.00	-0.02	0.00 	-7.20	0.00	-0.00 	-7.20	67	82
16692 	310	314	301 	0.00	0.00	-0.00	0.00 	-1.20	0.00	0.00 	-1.20	73	76
16742 	302	304	293 	-0.01	0.00	0.01	0.00 	1.80	0.00	0.00 	1.80	76	73
16792 	294	301	290 	-0.06	0.00	0.06	0.00 	17.70	0.00	0.00 	17.70	92	57
16842 	289	295	288 	-0.09	0.00	0.09	0.00 	28.50	0.00	0.00 	28.50	103	46
16892 	283	289	279 	-0.07	0.00	0.07	0.00 	21.30	0.00	-0.00 	21.30	96	53
16942 	283	287	275 	-0.03	0.00	0.03	0.00 	9.90	0.00	-0.00 	9.90	84	65
16992 	269	277	277 	-0.20	0.00	0.20	0.00 	60.90	0.00	0.00 	60.90	135	14
17042 	270	269	272 	-0.15	0.00	0.15	0.00 	45.30	0.00	-0.00 	45.30	120	29
17092 	263	268	255 	-0.05	0.00	0.05	0.00 	15.60	0.00	-0.00 	15.60	90	59
17142 	257	261	253 	-0.06	0.00	0.06	0.00 	17.70	0.00	0.00 	17.70	92	57
17192 	243	253	237 	-0.09	0.00	0.09	0.00 	26.10	0.00	0.00 	26.10	101	48
17242 	250	248	243 	-0.08	0.00	0.08	0.00 	24.60	0.00	-0.00 	24.60	99	50
17292 	241	249	238 	-0.12	0.00	0.12	0.00 	36.30	0.00	0.00 	36.30	111	38
17342 	259	255	234 	0.11	0.00	-0.11	0.00 	-31.50	0.00	-0.00 	-31.50	43	106
17392 	270	261	237 	0.19	0.00	-0.19	0.00 	-57.30	0.00	-0.00 	-57.30	17	132
17442 	273	269	245 	0.15	0.00	-0.15	0.00 	-44.70	0.00	0.00 	-44.70	30	119
17492 	287	282	245 	0.30	0.00	-0.30	0.00 	-90.60	0.00	-0.00 	-90.60	0	165
17542 	293	293	256 	0.26	0.00	-0.26	0.00 	-78.90	0.00	0.00 	-78.90	0	153
17592 	299	288	260 	0.28	0.00	-0.28	0.00 	-83.40	0.00	-0.00 	-83.40	0	158
17642 	302	292	272 	0.19	0.00	-0.19	0.00 	-57.60	0.00	0.00 	-57.60	17	132
17692 	300	292	272 	0.17	0.00	-0.17	0.00 	-51.60	0.00	0.00 	-51.60	23	126
17742 	302	290	276 	0.15	0.00	-0.15	0.00 	-45.00	0.00	0.00 	-45.00	30	120
17792 	288	306	282 	-0.03	0.00	0.03	0.00 	10.20	0.00	0.00 	10.20	85	64
17842 	298	305	278 	0.11	0.00	-0.11	0.00 	-31.50	0.00	-0.00 	-31.50	43	106
17892 	302	300	281 	0.11	0.00	-0.11	0.00 	-33.00	0.00	-0.00 	-33.00	42	108
17942 	304	305	288 	0.06	0.00	-0.06	0.00 	-19.50	0.00	0.00 	-19.50	55	94
17992 	302	296	292 	-0.00	0.00	0.00	0.00 	1.20	0.00	0.00 	1.20	76	73
18042 	294	307	288 	-0.03	0.00	0.03	0.00 	9.90	0.00	0.00 	9.90	84	65
18092 	301	307	285 	0.07	0.00	-0.07	0.00 	-20.10	0.00	-0.00 	-20.10	54	95
18142 	291	299	289 	-0.08	0.00	0.08	0.00 	24.30	0.00	0.00 	24.30	99	50
18192 	294	297	286 	-0.02	0.00	0.02	0.00 	6.90	0.00	-0.00 	6.90	81	68
18242 	283	300	283 	-0.10	0.00	0.10	0.00 	30.00	0.00	0.00 	30.00	105	45
18292 	290	274	290 	-0.13	0.00	0.13	0.00 	37.80	0.00	0.00 	37.80	112	37
18342 	280	281	277 	-0.09	0.00	0.09	0.00 	26.70	0.00	-0.00 	26.70	101	48
18392 	272	279	269 	-0.09	0.00	0.09	0.00 	27.30	0.00	0.00 	27.30	102	47
18442 	272	267	260 	-0.01	0.00	0.01	0.00 	3.90	0.00	-0.00 	3.90	78	71
18492 	269	275	267 	-0.11	0.00	0.11	0.00 	31.50	0.00	0.00 	31.50	106	43
18542 	268	274	264 	-0.09	0.00	0.09	0.00 	25.80	0.00	-0.00 	25.80	100	49
18592 	267	271	263 	-0.09	0.00	0.09	0.00 	26.70	0.00	0.00 	26.70	101	48
18642 	272	267	257 	0.02	0.00	-0.02	0.00 	-5.10	0.00	-0.00 	-5.10	69	80
18692 	269	262	248 	0.07	0.00	-0.07	0.00 	-21.60	0.00	-0.00 	-21.60	53	96
18742 	268	267	254 	0.01	0.00	-0.01	0.00 	-2.10	0.00	0.00 	-2.10	72	77
18792 	268	276	247 	0.09	0.00	-0.09	0.00 	-25.80	0.00	-0.00 	-25.80	49	100
18842 	271	267	258 	-0.00	0.00	0.00	0.00 	0.90	0.00	0.00 	0.90	75	74
18892 	264	270	256 	-0.05	0.00	0.05	0.00 	15.00	0.00	0.00 	15.00	90	60
18942 	272	268	248 	0.11	0.00	-0.11	0.00 	-32.40	0.00	-0.00 	-32.40	42	107
18992 	258	272	256 	-0.11	0.00	0.11	0.00 	32.40	0.00	0.00 	32.40	107	42
19042 	266	264	251 	0.01	0.00	-0.01	0.00 	-4.20	0.00	-0.00 	-4.20	70	79
19092 	262	265	253 	-0.05	0.00	0.05	0.00 	13.50	0.00	0.00 	13.50	88	61
19142 	265	274	253 	-0.01	0.00	0.01	0.00 	1.80	0.00	-0.00 	1.80	76	73
19192 	272	266	259 	-0.00	0.00	0.00	0.00 	1.20	0.00	-0.00 	1.20	76	73
19242 	268	273	264 	-0.09	0.00	0.09	0.00 	26.10	0.00	0.00 	26.10	101	48
19292 	274	279	263 	-0.01	0.00	0.01	0.00 	3.30	0.00	-0.00 	3.30	78	71
19342 	272	274	258 	0.01	0.00	-0.01	0.00 	-4.20	0.00	-0.00 	-4.20	70	79
19392 	280	276	269 	-0.01	0.00	0.01	0.00 	4.20	0.00	0.00 	4.20	79	70
19442 	282	283	264 	0.06	0.00	-0.06	0.00 	-18.90	0.00	-0.00 	-18.90	56	93
19492 	275	276	266 	-0.03	0.00	0.03	0.00 	10.20	0.00	0.00 	10.20	85	64
19542 	287	285	265 	0.10	0.00	-0.10	0.00 	-31.50	0.00	-0.00 	-31.50	43	106
19592 	287	285	265 	0.10	0.00	-0.10	0.00 	-31.50	0.00	0.00 	-31.50	43	106
19642 	286	286	267 	0.05	0.00	-0.05	0.00 	-13.80	0.00	0.00 	-13.80	61	88
19692 	289	281	267 	0.10	0.00	-0.10	0.00 	-30.30	0.00	-0.00 	-30.30	44	105
19742 	283	296	274 	-0.01	0.00	0.01	0.00 	4.20	0.00	0.00 	4.20	79	70
19792 	293	290	271 	0.11	0.00	-0.11	0.00 	-33.00	0.00	-0.00 	-33.00	42	108
19842 	289	290	284 	-0.06	0.00	0.06	0.00 	18.00	0.00	0.00 	18.00	93	57
19892 	293	284	275 	0.06	0.00	-0.06	0.00 	-19.20	0.00	-0.00 	-19.20	55	94
19942 	294	283	277 	0.05	0.00	-0.05	0.00 	-15.90	0.00	0.00 	-15.90	59	90
19992 	284	285	272 	0.00	0.00	-0.00	0.00 	-1.50	0.00	0.00 	-1.50	73	76
20042 	288	284	278 	-0.02	0.00	0.02	0.00 	4.80	0.00	0.00 	4.80	79	70
20092 	284	279	278 	-0.06	0.00	0.06	0.00 	18.30	0.00	0.00 	18.30	93	56
20142 	291	290	280 	0.00	0.00	0.00	0.00 	0.00	0.00	-0.00 	0.00	75	75
20192 	285	284	281 	-0.08	0.00	0.08	0.00 	22.80	0.00	0.00 	22.80	97	52
20242 	291	290	276 	0.04	0.00	-0.04	0.00 	-12.00	0.00	-0.00 	-12.00	63	87
20292 	285	286	285 	-0.11	0.00	0.11	0.00 	34.20	0.00	0.00 	34.20	109	40
20342 	268	268	282 	-0.27	0.00	0.27	0.00 	81.60	0.00	0.00 	81.60	156	0
20392 	248	246	276 	-0.43	0.00	0.43	0.00 	130.20	0.00	0.00 	130.20	200	0
20442 	248	246	276 	-0.43	0.00	0.43	0.00 	130.20	0.00	0.00 	130.20	200	0
20492 	227	236	253 	-0.42	0.00	0.42	0.00 	127.20	0.00	-0.00 	127.20	200	0
20542 	215	209	237 	-0.41	0.00	0.41	0.00 	123.30	0.00	-0.00 	123.30	198	0
20592 	198	204	222 	-0.44	0.00	0.44	0.00 	130.80	0.00	0.00 	130.80	200	0
20642 	185	181	198 	-0.35	0.00	0.35	0.00 	104.70	0.00	-0.00 	104.70	179	0
20692 	171	160	190 	-0.43	0.00	0.43	0.00 	129.00	0.00	0.00 	129.00	200	0
20742 	161	143	173 	-0.38	0.00	0.38	0.00 	113.10	0.00	-0.00 	113.10	188	0
20792 	151	127	158 	-0.34	0.00	0.34	0.00 	102.90	0.00	-0.00 	102.90	177	0
20842 	140	126	149 	-0.36	0.00	0.36	0.00 	109.20	0.00	0.00 	109.20	184	0
20892 	140	126	141 	-0.28	0.00	0.28	0.00 	85.20	0.00	-0.00 	85.20	160	0
20942 	152	129	142 	-0.17	0.00	0.17	0.00 	51.30	0.00	-0.00 	51.30	126	23
20992 	152	129	142 	-0.17	0.00	0.17	0.00 	51.30	0.00	0.00 	51.30	126	23
21042 	169	136	144 	-0.01	0.00	0.01	0.00 	4.20	0.00	-0.00 	4.20	79	70
21092 	186	156	149 	0.13	0.00	-0.13	0.00 	-37.80	0.00	-0.00 	-37.80	37	112
21142 	201	176	164 	0.15	0.00	-0.15	0.00 	-43.80	0.00	-0.00 	-43.80	31	118
21192 	230	223	181 	0.31	0.00	-0.31	0.00 	-93.90	0.00	-0.00 	-93.90	0	168
21242 	255	230	199 	0.39	0.00	-0.39	0.00 	-117.00	0.00	-0.00 	-117.00	0	192
21292 	261	254	216 	0.30	0.00	-0.30	0.00 	-91.20	0.00	0.00 	-91.20	0	166
21342 	275	260	227 	0.34	0.00	-0.34	0.00 	-102.00	0.00	-0.00 	-102.00	0	177
21392 	274	260	227 	0.34	0.00	-0.34	0.00 	-102.00	0.00	0.00 	-102.00	0	177
21442 	274	255	236 	0.23	0.00	-0.23	0.00 	-70.50	0.00	0.00 	-70.50	4	145
21492 	280	264	241 	0.25	0.00	-0.25	0.00 	-76.20	0.00	-0.00 	-76.20	0	151
21542 	275	265	240 	0.21	0.00	-0.21	0.00 	-64.50	0.00	0.00 	-64.50	10	139
21592 	275	261	250 	0.11	0.00	-0.11	0.00 	-33.30	0.00	0.00 	-33.30	41	108
21642 	273	260	253 	0.06	0.00	-0.06	0.00 	-18.00	0.00	0.00 	-18.00	57	93
21692 	275	263	258 	0.03	0.00	-0.03	0.00 	-9.90	0.00	0.00 	-9.90	65	84
21742 	274	265	263 	-0.03	0.00	0.03	0.00 	7.50	0.00	0.00 	7.50	82	67
21792 	273	261	269 	-0.10	0.00	0.10	0.00 	29.70	0.00	0.00 	29.70	104	45
21842 	273	261	269 	-0.10	0.00	0.10	0.00 	29.70	0.00	0.00 	29.70	104	45
21892 	272	265	276 	-0.18	0.00	0.18	0.00 	52.50	0.00	0.00 	52.50	127	22
21942 	268	263	263 	-0.09	0.00	0.09	0.00 	26.10	0.00	-0.00 	26.10	101	48
21992 	265	256	266 	-0.15	0.00	0.15	0.00 	46.20	0.00	0.00 	46.20	121	28
22042 	257	255	257 	-0.14	0.00	0.14	0.00 	43.50	0.00	-0.00 	43.50	118	31
22092 	256	243	252 	-0.12	0.00	0.12	0.00 	35.10	0.00	-0.00 	35.10	110	39
22142 	254	245	247 	-0.09	0.00	0.09	0.00 	25.50	0.00	-0.00 	25.50	100	49
22192 	250	237	249 	-0.15	0.00	0.15	0.00 	45.90	0.00	0.00 	45.90	120	29
22242 	248	237	243 	-0.11	0.00	0.11	0.00 	33.90	0.00	-0.00 	33.90	108	41
22292 	250	245	242 	-0.08	0.00	0.08	0.00 	22.50	0.00	-0.00 	22.50	97	52
22342 	258	236	242 	-0.00	0.00	0.00	0.00 	1.20	0.00	-0.00 	1.20	76	73
22392 	258	236	242 	-0.00	0.00	0.00	0.00 	1.20	0.00	0.00 	1.20	76	73
22442 	251	239	244 	-0.09	0.00	0.09	0.00 	27.30	0.00	0.00 	27.30	102	47
22492 	265	245	247 	0.03	0.00	-0.03	0.00 	-7.50	0.00	-0.00 	-7.50	67	82
22542 	262	239	241 	0.05	0.00	-0.05	0.00 	-14.70	0.00	-0.00 	-14.70	60	89
22592 	272	243	252 	0.04	0.00	-0.04	0.00 	-12.90	0.00	0.00 	-12.90	62	87
22642 	271	255	248 	0.09	0.00	-0.09	0.00 	-25.50	0.00	-0.00 	-25.50	49	100
22692 	276	255	252 	0.09	0.00	-0.09	0.00 	-28.50	0.00	-0.00 	-28.50	46	103
22742 	281	257	258 	0.09	0.00	-0.09	0.00 	-26.10	0.00	0.00 	-26.10	48	101
22792 	276	259	259 	0.03	0.00	-0.03	0.00 	-8.70	0.00	0.00 	-8.70	66	83
22842 	280	255	259 	0.06	0.00	-0.06	0.00 	-19.50	0.00	-0.00 	-19.50	55	94
22892 	279	265	260 	0.05	0.00	-0.05	0.00 	-16.50	0.00	0.00 	-16.50	58	91
22942 	279	260	266 	-0.01	0.00	0.01	0.00 	3.00	0.00	0.00 	3.00	78	72
22992 	277	259	261 	0.02	0.00	-0.02	0.00 	-5.70	0.00	-0.00 	-5.70	69	80
23042 	276	253	260 	0.01	0.00	-0.01	0.00 	-3.90	0.00	0.00 	-3.90	71	78
23092 	281	258	265 	0.02	0.00	-0.02	0.00 	-5.40	0.00	-0.00 	-5.40	69	80
23142 	280	252	262 	0.03	0.00	-0.03	0.00 	-9.60	0.00	-0.00 	-9.60	65	84
23192 	274	249	261 	-0.02	0.00	0.02	0.00 	6.30	0.00	0.00 	6.30	81	68
23242 	274	243	260 	-0.02	0.00	0.02	0.00 	5.10	0.00	-0.00 	5.10	80	69
23292 	267	244	256 	-0.05	0.00	0.05	0.00 	13.80	0.00	0.00 	13.80	88	61
23342 	266	240	252 	-0.02	0.00	0.02	0.00 	6.00	0.00	-0.00 	6.00	81	69
23392 	261	243	253 	-0.08	0.00	0.08	0.00 	23.10	0.00	0.00 	23.10	98	51
23442 	252	240	249 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36
23492 	254	240	249 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

In TrackLeftWallOffset with Kp/Ki/Kd = 300.00 0.00 0.00

R/C/F dists = 136 117 127 Steerval = -0.090, CutAngle = 29.00

(cutAngleDeg > 0 && abs(cutAngleDeg) < 30 -- setting to +30

R/C/F dists = 136 117 127 Steerval = -0.090, SteerAngle = -5.14, CutAngle = 30.00

CutAngDeg > 0, SteerAngleDeg < cutAngleDeg

In SpinTurn(CW, 35.14, 30.00)

Init hdg = -0.08 deg, Turn = 35.14 deg, tgt = 35.06 deg, timeout = 2.34 sec

Msec Hdg PrvHdg dHdg Rate tgtDPS err Kp*err Ival Kd*Derr speed Match Slope

11361 0.05 -0.08 0.13 4.49 30.00 25.51 45.92 25.51 10.20 61 0.81 0.81

11391 1.28 0.05 1.23 40.88 30.00 -10.88 -19.58 14.63 -14.56 50 0.81 0.01

11421 3.31 1.28 2.03 67.70 30.00 -37.70 -67.86 0.00 -10.73 50 0.82 0.01

11451 5.52 3.31 2.22 73.85 30.00 -43.85 -78.94 0.00 -2.46 50 0.84 0.01

11481 7.87 5.52 2.34 78.13 30.00 -48.13 -86.64 0.00 -1.71 50 0.85 0.01

11511 10.30 7.87 2.43 81.05 30.00 -51.05 -91.89 0.00 -1.17 50 0.86 0.01

11541 12.83 10.30 2.53 84.47 30.00 -54.47 -98.05 0.00 -1.37 50 0.88 0.01

11571 15.41 12.83 2.58 85.85 30.00 -55.85 -100.54 0.00 -0.55 50 0.89 0.01

11601 18.03 15.41 2.62 87.32 30.00 -57.32 -103.18 0.00 -0.59 50 0.91 0.01

11631 20.70 18.03 2.67 89.17 30.00 -59.17 -106.50 0.00 -0.74 50 0.92 0.01

11661 23.36 20.70 2.66 88.50 30.00 -58.50 -105.30 0.00 0.27 50 0.93 0.01

11691 26.02 23.36 2.66 88.82 30.00 -58.82 -105.87 0.00 -0.13 50 0.95 0.01

11721 28.79 26.02 2.76 92.13 30.00 -62.13 -111.83 0.00 -1.32 50 0.97 0.02

11751 31.48 28.79 2.69 89.76 30.00 -59.76 -107.57 0.00 0.94 50 0.98 0.01

11781 34.20 31.48 2.72 90.74 30.00 -60.74 -109.33 0.00 -0.39 50 1.00 0.02

Completed turn with yaw = 34.20, tgt = 35.06, and match = 0.995

fudgeFactorMM = -100

at approach start: cut angle = 30.000, adjfactor = 0.866, num = 400.00, tgt dist = 361.88

At start - err = prev_err = -138

R/C/F dists = 156 208 223 Steerval = 0.670, SteerAngle = 38.29, CutAngle = 30.00, err/prev_err = -138

Msec LFront LCtr LRear Front Rear Err P_err

13913 228 206 155 170 206 -133 -138

14020 250 231 179 173 243 -111 -133

14127 312 310 237 168 304 -49 -111

14234 381 395 313 159 393 19 -49

At end of offset capture - prev_res*res = -931

correct back to parallel

In SpinTurn(CCW, 30.00, 30.00)

Init hdg = 45.41 deg, Turn = 30.00 deg, tgt = 15.41 deg, timeout = 2.00 sec

Msec Hdg PrvHdg dHdg Rate tgtDPS err Kp*err Ival Kd*Derr speed Match Slope

14294 45.26 45.41 -0.15 5.12 30.00 24.88 44.79 24.88 9.95 59 0.83 0.83

14324 44.29 45.26 -0.98 32.52 30.00 -2.52 -4.53 22.37 -10.96 50 0.84 0.01

14354 42.61 44.29 -1.68 55.95 30.00 -25.95 -46.71 0.00 -9.37 50 0.85 0.01

14384 40.64 42.61 -1.96 65.48 30.00 -35.48 -63.87 0.00 -3.81 50 0.86 0.01

14414 38.30 40.64 -2.34 78.08 30.00 -48.08 -86.55 0.00 -5.04 50 0.87 0.01

14444 35.51 38.30 -2.79 92.90 30.00 -62.90 -113.22 0.00 -5.93 50 0.89 0.02

14474 32.31 35.51 -3.21 106.88 30.00 -76.88 -138.39 0.00 -5.59 50 0.91 0.02

14504 28.88 32.31 -3.42 114.15 30.00 -84.15 -151.47 0.00 -2.91 50 0.93 0.02

14534 25.25 28.88 -3.64 121.22 30.00 -91.22 -164.19 0.00 -2.83 50 0.95 0.02

14564 21.33 25.25 -3.91 130.41 30.00 -100.41 -180.74 0.00 -3.68 50 0.97 0.02

14594 17.50 21.33 -3.84 127.92 30.00 -97.92 -176.26 0.00 1.00 50 0.99 0.02

14624 13.63 17.50 -3.87 128.94 30.00 -98.94 -178.09 0.00 -0.41 50 0.99 0.00

Completed turn with yaw = 13.63, tgt = 15.41, and match = 0.990

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00 0.00 0.00

Msec Fdir Cdir Rdir Steer Set error Ival Kp*err Ki*Ival Kd*Din Output Lspd Rspd

15692 389 416 377 0.14 0.00 -0.14 0.00 -40.80 0.00 -0.00 -40.80 34 115

15742 390 404 377 0.13 0.00 -0.13 0.00 -40.20 0.00 0.00 -40.20 34 115

15792 391 405 371 0.20 0.00 -0.20 0.00 -61.50 0.00 -0.00 -61.50 13 136

15842 386 412 378 0.09 0.00 -0.09 0.00 -27.60 0.00 0.00 -27.60 47 102

15892 386 412 378 0.09 0.00 -0.09 0.00 -27.60 0.00 0.00 -27.60 47 102

15942 386 405 383 0.04 0.00 -0.04 0.00 -10.50 0.00 0.00 -10.50 64 85

15992 386 413 383 0.04 0.00 -0.04 0.00 -12.90 0.00 -0.00 -12.90 62 87

16042 388 407 381 0.08 0.00 -0.08 0.00 -23.10 0.00 -0.00 -23.10 51 98

16092 381 411 388 -0.06 0.00 0.06 0.00 17.70 0.00 0.00 17.70 92 57

16142 370 403 381 -0.11 0.00 0.11 0.00 32.10 0.00 0.00 32.10 107 42

16192 376 394 380 -0.05 0.00 0.05 0.00 13.80 0.00 -0.00 13.80 88 61

16242 366 384 374 -0.10 0.00 0.10 0.00 28.80 0.00 0.00 28.80 103 46

16292 362 384 374 -0.10 0.00 0.10 0.00 28.80 0.00 0.00 28.80 103 46

16342 362 375 370 -0.10 0.00 0.10 0.00 31.50 0.00 0.00 31.50 106 43

16392 351 372 357 -0.09 0.00 0.09 0.00 26.40 0.00 -0.00 26.40 101 48

16442 337 364 346 -0.13 0.00 0.13 0.00 37.80 0.00 0.00 37.80 112 37

16492 332 352 321 0.06 0.00 -0.06 0.00 -18.60 0.00 -0.00 -18.60 56 93

16542 325 336 319 -0.00 0.00 0.00 0.00 1.20 0.00 0.00 1.20 76 73

16592 315 323 308 -0.01 0.00 0.01 0.00 2.10 0.00 0.00 2.10 77 72

16642 310 324 300 0.02 0.00 -0.02 0.00 -7.20 0.00 -0.00 -7.20 67 82

16692 310 314 301 0.00 0.00 -0.00 0.00 -1.20 0.00 0.00 -1.20 73 76

16742 302 304 293 -0.01 0.00 0.01 0.00 1.80 0.00 0.00 1.80 76 73

16792 294 301 290 -0.06 0.00 0.06 0.00 17.70 0.00 0.00 17.70 92 57

16842 289 295 288 -0.09 0.00 0.09 0.00 28.50 0.00 0.00 28.50 103 46

16892 283 289 279 -0.07 0.00 0.07 0.00 21.30 0.00 -0.00 21.30 96 53

16942 283 287 275 -0.03 0.00 0.03 0.00 9.90 0.00 -0.00 9.90 84 65

16992 269 277 277 -0.20 0.00 0.20 0.00 60.90 0.00 0.00 60.90 135 14

17042 270 269 272 -0.15 0.00 0.15 0.00 45.30 0.00 -0.00 45.30 120 29

17092 263 268 255 -0.05 0.00 0.05 0.00 15.60 0.00 -0.00 15.60 90 59

17142 257 261 253 -0.06 0.00 0.06 0.00 17.70 0.00 0.00 17.70 92 57

17192 243 253 237 -0.09 0.00 0.09 0.00 26.10 0.00 0.00 26.10 101 48

17242 250 248 243 -0.08 0.00 0.08 0.00 24.60 0.00 -0.00 24.60 99 50

17292 241 249 238 -0.12 0.00 0.12 0.00 36.30 0.00 0.00 36.30 111 38

17342 259 255 234 0.11 0.00 -0.11 0.00 -31.50 0.00 -0.00 -31.50 43 106

17392 270 261 237 0.19 0.00 -0.19 0.00 -57.30 0.00 -0.00 -57.30 17 132

17442 273 269 245 0.15 0.00 -0.15 0.00 -44.70 0.00 0.00 -44.70 30 119

17492 287 282 245 0.30 0.00 -0.30 0.00 -90.60 0.00 -0.00 -90.60 0 165

17542 293 293 256 0.26 0.00 -0.26 0.00 -78.90 0.00 0.00 -78.90 0 153

17592 299 288 260 0.28 0.00 -0.28 0.00 -83.40 0.00 -0.00 -83.40 0 158

17642 302 292 272 0.19 0.00 -0.19 0.00 -57.60 0.00 0.00 -57.60 17 132

17692 300 292 272 0.17 0.00 -0.17 0.00 -51.60 0.00 0.00 -51.60 23 126

17742 302 290 276 0.15 0.00 -0.15 0.00 -45.00 0.00 0.00 -45.00 30 120

17792 288 306 282 -0.03 0.00 0.03 0.00 10.20 0.00 0.00 10.20 85 64

17842 298 305 278 0.11 0.00 -0.11 0.00 -31.50 0.00 -0.00 -31.50 43 106

17892 302 300 281 0.11 0.00 -0.11 0.00 -33.00 0.00 -0.00 -33.00 42 108

17942 304 305 288 0.06 0.00 -0.06 0.00 -19.50 0.00 0.00 -19.50 55 94

17992 302 296 292 -0.00 0.00 0.00 0.00 1.20 0.00 0.00 1.20 76 73

18042 294 307 288 -0.03 0.00 0.03 0.00 9.90 0.00 0.00 9.90 84 65

18092 301 307 285 0.07 0.00 -0.07 0.00 -20.10 0.00 -0.00 -20.10 54 95

18142 291 299 289 -0.08 0.00 0.08 0.00 24.30 0.00 0.00 24.30 99 50

18192 294 297 286 -0.02 0.00 0.02 0.00 6.90 0.00 -0.00 6.90 81 68

18242 283 300 283 -0.10 0.00 0.10 0.00 30.00 0.00 0.00 30.00 105 45

18292 290 274 290 -0.13 0.00 0.13 0.00 37.80 0.00 0.00 37.80 112 37

18342 280 281 277 -0.09 0.00 0.09 0.00 26.70 0.00 -0.00 26.70 101 48

18392 272 279 269 -0.09 0.00 0.09 0.00 27.30 0.00 0.00 27.30 102 47

18442 272 267 260 -0.01 0.00 0.01 0.00 3.90 0.00 -0.00 3.90 78 71

18492 269 275 267 -0.11 0.00 0.11 0.00 31.50 0.00 0.00 31.50 106 43

18542 268 274 264 -0.09 0.00 0.09 0.00 25.80 0.00 -0.00 25.80 100 49

18592 267 271 263 -0.09 0.00 0.09 0.00 26.70 0.00 0.00 26.70 101 48

18642 272 267 257 0.02 0.00 -0.02 0.00 -5.10 0.00 -0.00 -5.10 69 80

18692 269 262 248 0.07 0.00 -0.07 0.00 -21.60 0.00 -0.00 -21.60 53 96

18742 268 267 254 0.01 0.00 -0.01 0.00 -2.10 0.00 0.00 -2.10 72 77

18792 268 276 247 0.09 0.00 -0.09 0.00 -25.80 0.00 -0.00 -25.80 49 100

18842 271 267 258 -0.00 0.00 0.00 0.00 0.90 0.00 0.00 0.90 75 74

18892 264 270 256 -0.05 0.00 0.05 0.00 15.00 0.00 0.00 15.00 90 60

18942 272 268 248 0.11 0.00 -0.11 0.00 -32.40 0.00 -0.00 -32.40 42 107

18992 258 272 256 -0.11 0.00 0.11 0.00 32.40 0.00 0.00 32.40 107 42

19042 266 264 251 0.01 0.00 -0.01 0.00 -4.20 0.00 -0.00 -4.20 70 79

19092 262 265 253 -0.05 0.00 0.05 0.00 13.50 0.00 0.00 13.50 88 61

19142 265 274 253 -0.01 0.00 0.01 0.00 1.80 0.00 -0.00 1.80 76 73

19192 272 266 259 -0.00 0.00 0.00 0.00 1.20 0.00 -0.00 1.20 76 73

19242 268 273 264 -0.09 0.00 0.09 0.00 26.10 0.00 0.00 26.10 101 48

19292 274 279 263 -0.01 0.00 0.01 0.00 3.30 0.00 -0.00 3.30 78 71

19342 272 274 258 0.01 0.00 -0.01 0.00 -4.20 0.00 -0.00 -4.20 70 79

19392 280 276 269 -0.01 0.00 0.01 0.00 4.20 0.00 0.00 4.20 79 70

19442 282 283 264 0.06 0.00 -0.06 0.00 -18.90 0.00 -0.00 -18.90 56 93

19492 275 276 266 -0.03 0.00 0.03 0.00 10.20 0.00 0.00 10.20 85 64

19542 287 285 265 0.10 0.00 -0.10 0.00 -31.50 0.00 -0.00 -31.50 43 106

19592 287 285 265 0.10 0.00 -0.10 0.00 -31.50 0.00 0.00 -31.50 43 106

19642 286 286 267 0.05 0.00 -0.05 0.00 -13.80 0.00 0.00 -13.80 61 88

19692 289 281 267 0.10 0.00 -0.10 0.00 -30.30 0.00 -0.00 -30.30 44 105

19742 283 296 274 -0.01 0.00 0.01 0.00 4.20 0.00 0.00 4.20 79 70

19792 293 290 271 0.11 0.00 -0.11 0.00 -33.00 0.00 -0.00 -33.00 42 108

19842 289 290 284 -0.06 0.00 0.06 0.00 18.00 0.00 0.00 18.00 93 57

19892 293 284 275 0.06 0.00 -0.06 0.00 -19.20 0.00 -0.00 -19.20 55 94

19942 294 283 277 0.05 0.00 -0.05 0.00 -15.90 0.00 0.00 -15.90 59 90

19992 284 285 272 0.00 0.00 -0.00 0.00 -1.50 0.00 0.00 -1.50 73 76

20042 288 284 278 -0.02 0.00 0.02 0.00 4.80 0.00 0.00 4.80 79 70

20092 284 279 278 -0.06 0.00 0.06 0.00 18.30 0.00 0.00 18.30 93 56

20142 291 290 280 0.00 0.00 0.00 0.00 0.00 0.00 -0.00 0.00 75 75

20192 285 284 281 -0.08 0.00 0.08 0.00 22.80 0.00 0.00 22.80 97 52

20242 291 290 276 0.04 0.00 -0.04 0.00 -12.00 0.00 -0.00 -12.00 63 87

20292 285 286 285 -0.11 0.00 0.11 0.00 34.20 0.00 0.00 34.20 109 40

20342 268 268 282 -0.27 0.00 0.27 0.00 81.60 0.00 0.00 81.60 156 0

20392 248 246 276 -0.43 0.00 0.43 0.00 130.20 0.00 0.00 130.20 200 0

20442 248 246 276 -0.43 0.00 0.43 0.00 130.20 0.00 0.00 130.20 200 0

20492 227 236 253 -0.42 0.00 0.42 0.00 127.20 0.00 -0.00 127.20 200 0

20542 215 209 237 -0.41 0.00 0.41 0.00 123.30 0.00 -0.00 123.30 198 0

20592 198 204 222 -0.44 0.00 0.44 0.00 130.80 0.00 0.00 130.80 200 0

20642 185 181 198 -0.35 0.00 0.35 0.00 104.70 0.00 -0.00 104.70 179 0

20692 171 160 190 -0.43 0.00 0.43 0.00 129.00 0.00 0.00 129.00 200 0

20742 161 143 173 -0.38 0.00 0.38 0.00 113.10 0.00 -0.00 113.10 188 0

20792 151 127 158 -0.34 0.00 0.34 0.00 102.90 0.00 -0.00 102.90 177 0

20842 140 126 149 -0.36 0.00 0.36 0.00 109.20 0.00 0.00 109.20 184 0

20892 140 126 141 -0.28 0.00 0.28 0.00 85.20 0.00 -0.00 85.20 160 0

20942 152 129 142 -0.17 0.00 0.17 0.00 51.30 0.00 -0.00 51.30 126 23

20992 152 129 142 -0.17 0.00 0.17 0.00 51.30 0.00 0.00 51.30 126 23

21042 169 136 144 -0.01 0.00 0.01 0.00 4.20 0.00 -0.00 4.20 79 70

21092 186 156 149 0.13 0.00 -0.13 0.00 -37.80 0.00 -0.00 -37.80 37 112

21142 201 176 164 0.15 0.00 -0.15 0.00 -43.80 0.00 -0.00 -43.80 31 118

21192 230 223 181 0.31 0.00 -0.31 0.00 -93.90 0.00 -0.00 -93.90 0 168

21242 255 230 199 0.39 0.00 -0.39 0.00 -117.00 0.00 -0.00 -117.00 0 192

21292 261 254 216 0.30 0.00 -0.30 0.00 -91.20 0.00 0.00 -91.20 0 166

21342 275 260 227 0.34 0.00 -0.34 0.00 -102.00 0.00 -0.00 -102.00 0 177

21392 274 260 227 0.34 0.00 -0.34 0.00 -102.00 0.00 0.00 -102.00 0 177

21442 274 255 236 0.23 0.00 -0.23 0.00 -70.50 0.00 0.00 -70.50 4 145

21492 280 264 241 0.25 0.00 -0.25 0.00 -76.20 0.00 -0.00 -76.20 0 151

21542 275 265 240 0.21 0.00 -0.21 0.00 -64.50 0.00 0.00 -64.50 10 139

21592 275 261 250 0.11 0.00 -0.11 0.00 -33.30 0.00 0.00 -33.30 41 108

21642 273 260 253 0.06 0.00 -0.06 0.00 -18.00 0.00 0.00 -18.00 57 93

21692 275 263 258 0.03 0.00 -0.03 0.00 -9.90 0.00 0.00 -9.90 65 84

21742 274 265 263 -0.03 0.00 0.03 0.00 7.50 0.00 0.00 7.50 82 67

21792 273 261 269 -0.10 0.00 0.10 0.00 29.70 0.00 0.00 29.70 104 45

21842 273 261 269 -0.10 0.00 0.10 0.00 29.70 0.00 0.00 29.70 104 45

21892 272 265 276 -0.18 0.00 0.18 0.00 52.50 0.00 0.00 52.50 127 22

21942 268 263 263 -0.09 0.00 0.09 0.00 26.10 0.00 -0.00 26.10 101 48

21992 265 256 266 -0.15 0.00 0.15 0.00 46.20 0.00 0.00 46.20 121 28

22042 257 255 257 -0.14 0.00 0.14 0.00 43.50 0.00 -0.00 43.50 118 31

22092 256 243 252 -0.12 0.00 0.12 0.00 35.10 0.00 -0.00 35.10 110 39

22142 254 245 247 -0.09 0.00 0.09 0.00 25.50 0.00 -0.00 25.50 100 49

22192 250 237 249 -0.15 0.00 0.15 0.00 45.90 0.00 0.00 45.90 120 29

22242 248 237 243 -0.11 0.00 0.11 0.00 33.90 0.00 -0.00 33.90 108 41

22292 250 245 242 -0.08 0.00 0.08 0.00 22.50 0.00 -0.00 22.50 97 52

22342 258 236 242 -0.00 0.00 0.00 0.00 1.20 0.00 -0.00 1.20 76 73

22392 258 236 242 -0.00 0.00 0.00 0.00 1.20 0.00 0.00 1.20 76 73

22442 251 239 244 -0.09 0.00 0.09 0.00 27.30 0.00 0.00 27.30 102 47

22492 265 245 247 0.03 0.00 -0.03 0.00 -7.50 0.00 -0.00 -7.50 67 82

22542 262 239 241 0.05 0.00 -0.05 0.00 -14.70 0.00 -0.00 -14.70 60 89

22592 272 243 252 0.04 0.00 -0.04 0.00 -12.90 0.00 0.00 -12.90 62 87

22642 271 255 248 0.09 0.00 -0.09 0.00 -25.50 0.00 -0.00 -25.50 49 100

22692 276 255 252 0.09 0.00 -0.09 0.00 -28.50 0.00 -0.00 -28.50 46 103

22742 281 257 258 0.09 0.00 -0.09 0.00 -26.10 0.00 0.00 -26.10 48 101

22792 276 259 259 0.03 0.00 -0.03 0.00 -8.70 0.00 0.00 -8.70 66 83

22842 280 255 259 0.06 0.00 -0.06 0.00 -19.50 0.00 -0.00 -19.50 55 94

22892 279 265 260 0.05 0.00 -0.05 0.00 -16.50 0.00 0.00 -16.50 58 91

22942 279 260 266 -0.01 0.00 0.01 0.00 3.00 0.00 0.00 3.00 78 72

22992 277 259 261 0.02 0.00 -0.02 0.00 -5.70 0.00 -0.00 -5.70 69 80

23042 276 253 260 0.01 0.00 -0.01 0.00 -3.90 0.00 0.00 -3.90 71 78

23092 281 258 265 0.02 0.00 -0.02 0.00 -5.40 0.00 -0.00 -5.40 69 80

23142 280 252 262 0.03 0.00 -0.03 0.00 -9.60 0.00 -0.00 -9.60 65 84

23192 274 249 261 -0.02 0.00 0.02 0.00 6.30 0.00 0.00 6.30 81 68

23242 274 243 260 -0.02 0.00 0.02 0.00 5.10 0.00 -0.00 5.10 80 69

23292 267 244 256 -0.05 0.00 0.05 0.00 13.80 0.00 0.00 13.80 88 61

23342 266 240 252 -0.02 0.00 0.02 0.00 6.00 0.00 -0.00 6.00 81 69

23392 261 243 253 -0.08 0.00 0.08 0.00 23.10 0.00 0.00 23.10 98 51

23442 252 240 249 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

23492 254 240 249 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

Using steerval only. Note monotonically decreasing distance

Even more confusing, it appears that the earlier (September 2021) trial using the steering value input also used the measured center distance to modulate the steering value so the robot would tend to track the steering value but also trend toward the desired wall offset distance. Here’s the tracking code from FourWD_WallTrackTest_V3:

WallTrackSteerVal = RightSteeringVal + (Lidar_RightCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 250.f;

//update motor speeds, skipping bad values
if (!isnan(WallTrackSteerVal))
{
	//10/12/20 now this executes every time, with interval controlled by timer ISR
	//09/03/21 rev fcn type to double vs void to eliminate output param
	//PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
	//	kp, ki, kd, WallTrackOutput);
	WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
		kp, ki, kd);

WallTrackSteerVal = RightSteeringVal + (Lidar_RightCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 250.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

//10/12/20 now this executes every time, with interval controlled by timer ISR

//09/03/21 rev fcn type to double vs void to eliminate output param

//PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

// kp, ki, kd, WallTrackOutput);

WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

kp, ki, kd);

In the above code snippet, ‘Lidar_RightCenter’ is in mm, so WALL_OFFSET_TGTDIST_CM must be multiplied by 10 to match units.

At this point I am thoroughly confused, (but hopeful, since I have evidence from an earlier version of myself that something (actually two somethings) actually work. I believe the next step is to see if I can use my WallE3_WallTrackTuning_V5 code to consolidate everything down to something that works.

12 January 2023 Update:

I went back and loaded up WallE3_WallTrack_V2.ino and ran it on my 4m ‘range’ with two 30º breaks. The robot tracked amazingly well, as shown in the following telemetry output and Excel plot

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
15780 	381	411	364 	0.18	0.00	-0.18	0.00 	-54.30	0.00	-0.00 	-54.30	20	129
15830 	387	410	371 	0.17	0.00	-0.17	0.00 	-51.00	0.00	0.00 	-51.00	24	126
15880 	375	413	373 	0.03	0.00	-0.03	0.00 	-9.90	0.00	0.00 	-9.90	65	84
15930 	381	406	376 	0.06	0.00	-0.06	0.00 	-16.80	0.00	-0.00 	-16.80	58	91
15980 	381	418	366 	0.17	0.00	-0.17	0.00 	-50.40	0.00	-0.00 	-50.40	24	125
16030 	382	414	367 	0.16	0.00	-0.16	0.00 	-49.20	0.00	0.00 	-49.20	25	124
16080 	375	413	365 	0.11	0.00	-0.11	0.00 	-33.90	0.00	0.00 	-33.90	41	108
16130 	386	403	381 	0.05	0.00	-0.05	0.00 	-15.90	0.00	0.00 	-15.90	59	90
16180 	383	403	381 	0.05	0.00	-0.05	0.00 	-15.90	0.00	0.00 	-15.90	59	90
16230 	383	408	383 	0.01	0.00	-0.01	0.00 	-2.40	0.00	0.00 	-2.40	72	77
16280 	381	409	383 	-0.01	0.00	0.01	0.00 	3.30	0.00	0.00 	3.30	78	71
16330 	380	417	398 	-0.16	0.00	0.16	0.00 	48.90	0.00	0.00 	48.90	123	26
16380 	386	414	385 	0.02	0.00	-0.02	0.00 	-7.20	0.00	-0.00 	-7.20	67	82
16430 	370	395	373 	-0.04	0.00	0.04	0.00 	10.50	0.00	0.00 	10.50	85	64
16480 	352	387	363 	-0.12	0.00	0.12	0.00 	36.90	0.00	0.00 	36.90	111	38
16530 	350	374	358 	-0.11	0.00	0.11	0.00 	31.80	0.00	-0.00 	31.80	106	43
16580 	339	360	358 	-0.23	0.00	0.23	0.00 	69.00	0.00	0.00 	69.00	144	6
16630 	339	360	358 	-0.23	0.00	0.23	0.00 	69.00	0.00	0.00 	69.00	144	6
16680 	324	348	335 	-0.16	0.00	0.16	0.00 	48.60	0.00	-0.00 	48.60	123	26
16730 	314	345	305 	0.04	0.00	-0.04	0.00 	-10.50	0.00	-0.00 	-10.50	64	85
16780 	307	326	295 	0.05	0.00	-0.05	0.00 	-13.80	0.00	-0.00 	-13.80	61	88
16830 	293	320	298 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36
16880 	281	303	286 	-0.07	0.00	0.07	0.00 	20.10	0.00	-0.00 	20.10	95	54
16930 	281	300	278 	-0.07	0.00	0.07	0.00 	21.00	0.00	0.00 	21.00	96	54
16980 	279	287	274 	-0.06	0.00	0.06	0.00 	18.90	0.00	-0.00 	18.90	93	56
17030 	267	288	263 	-0.07	0.00	0.07	0.00 	21.60	0.00	0.00 	21.60	96	53
17080 	266	284	250 	0.04	0.00	-0.04	0.00 	-13.20	0.00	-0.00 	-13.20	61	88
17130 	255	270	254 	-0.12	0.00	0.12	0.00 	36.00	0.00	0.00 	36.00	111	39
17180 	257	270	247 	-0.03	0.00	0.03	0.00 	9.00	0.00	-0.00 	9.00	84	66
17230 	250	271	240 	-0.03	0.00	0.03	0.00 	8.70	0.00	-0.00 	8.70	83	66
17280 	241	267	240 	-0.12	0.00	0.12	0.00 	36.90	0.00	0.00 	36.90	111	38
17330 	244	262	231 	-0.01	0.00	0.01	0.00 	2.40	0.00	-0.00 	2.40	77	72
17380 	237	263	235 	-0.12	0.00	0.12	0.00 	35.10	0.00	0.00 	35.10	110	39
17430 	236	262	237 	-0.15	0.00	0.15	0.00 	44.40	0.00	0.00 	44.40	119	30
17480 	240	256	235 	-0.09	0.00	0.09	0.00 	28.20	0.00	-0.00 	28.20	103	46
17530 	242	251	237 	-0.10	0.00	0.10	0.00 	29.70	0.00	0.00 	29.70	104	45
17580 	254	261	231 	0.09	0.00	-0.09	0.00 	-27.30	0.00	-0.00 	-27.30	47	102
17630 	265	267	230 	0.22	0.00	-0.22	0.00 	-65.10	0.00	-0.00 	-65.10	9	140
17680 	274	289	243 	0.20	0.00	-0.20	0.00 	-59.70	0.00	0.00 	-59.70	15	134
17730 	275	279	253 	0.10	0.00	-0.10	0.00 	-29.70	0.00	0.00 	-29.70	45	104
17780 	275	292	258 	0.06	0.00	-0.06	0.00 	-18.60	0.00	0.00 	-18.60	56	93
17830 	277	287	257 	0.09	0.00	-0.09	0.00 	-26.10	0.00	-0.00 	-26.10	48	101
17880 	279	292	261 	0.07	0.00	-0.07	0.00 	-21.60	0.00	0.00 	-21.60	53	96
17930 	286	289	261 	0.06	0.00	-0.06	0.00 	-17.70	0.00	0.00 	-17.70	57	92
17980 	275	303	267 	-0.02	0.00	0.02	0.00 	5.10	0.00	0.00 	5.10	80	69
18030 	283	305	270 	0.03	0.00	-0.03	0.00 	-10.50	0.00	-0.00 	-10.50	64	85
18080 	286	297	271 	0.05	0.00	-0.05	0.00 	-14.10	0.00	-0.00 	-14.10	60	89
18130 	286	303	273 	0.03	0.00	-0.03	0.00 	-9.90	0.00	0.00 	-9.90	65	84
18180 	285	299	267 	0.08	0.00	-0.08	0.00 	-23.70	0.00	-0.00 	-23.70	51	98
18230 	284	302	270 	0.04	0.00	-0.04	0.00 	-12.60	0.00	0.00 	-12.60	62	87
18280 	283	295	275 	-0.02	0.00	0.02	0.00 	7.50	0.00	0.00 	7.50	82	67
18330 	281	311	262 	0.10	0.00	-0.10	0.00 	-30.30	0.00	-0.00 	-30.30	44	105
18380 	283	299	269 	0.04	0.00	-0.04	0.00 	-11.70	0.00	0.00 	-11.70	63	86
18430 	278	304	268 	0.00	0.00	-0.00	0.00 	-1.20	0.00	0.00 	-1.20	73	76
18480 	279	297	271 	-0.02	0.00	0.02	0.00 	6.90	0.00	0.00 	6.90	81	68
18530 	283	303	268 	0.05	0.00	-0.05	0.00 	-15.90	0.00	-0.00 	-15.90	59	90
18580 	280	294	268 	0.01	0.00	-0.01	0.00 	-4.20	0.00	0.00 	-4.20	70	79
18630 	281	292	271 	-0.01	0.00	0.01	0.00 	2.40	0.00	0.00 	2.40	77	72
18680 	275	288	268 	-0.04	0.00	0.04	0.00 	12.60	0.00	0.00 	12.60	87	62
18730 	272	294	266 	-0.05	0.00	0.05	0.00 	13.80	0.00	0.00 	13.80	88	61
18780 	273	282	259 	0.02	0.00	-0.02	0.00 	-6.60	0.00	-0.00 	-6.60	68	81
18830 	264	291	265 	-0.12	0.00	0.12	0.00 	35.70	0.00	0.00 	35.70	110	39
18880 	270	273	266 	-0.09	0.00	0.09	0.00 	26.10	0.00	-0.00 	26.10	101	48
18930 	267	282	259 	-0.04	0.00	0.04	0.00 	11.40	0.00	-0.00 	11.40	86	63
18980 	268	283	258 	-0.02	0.00	0.02	0.00 	5.10	0.00	-0.00 	5.10	80	69
19030 	263	279	256 	-0.05	0.00	0.05	0.00 	15.30	0.00	0.00 	15.30	90	59
19080 	261	269	252 	-0.04	0.00	0.04	0.00 	12.30	0.00	-0.00 	12.30	87	62
19130 	259	280	241 	0.06	0.00	-0.06	0.00 	-18.00	0.00	-0.00 	-18.00	56	93
19180 	257	274	251 	-0.07	0.00	0.07	0.00 	19.80	0.00	0.00 	19.80	94	55
19230 	252	275	246 	-0.06	0.00	0.06	0.00 	19.50	0.00	-0.00 	19.50	94	55
19280 	259	273	241 	0.00	0.00	-0.00	0.00 	-0.90	0.00	-0.00 	-0.90	74	75
19330 	259	282	238 	0.09	0.00	-0.09	0.00 	-27.60	0.00	-0.00 	-27.60	47	102
19380 	250	272	244 	-0.07	0.00	0.07	0.00 	20.40	0.00	0.00 	20.40	95	54
19430 	252	274	249 	-0.10	0.00	0.10	0.00 	28.80	0.00	0.00 	28.80	103	46
19480 	246	270	246 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36
19530 	248	270	248 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36
19580 	250	269	244 	-0.07	0.00	0.07	0.00 	21.30	0.00	-0.00 	21.30	96	53
19630 	250	270	241 	-0.04	0.00	0.04	0.00 	12.00	0.00	-0.00 	12.00	87	63
19680 	250	277	235 	0.03	0.00	-0.03	0.00 	-8.10	0.00	-0.00 	-8.10	66	83
19730 	246	274	235 	-0.02	0.00	0.02	0.00 	4.80	0.00	0.00 	4.80	79	70
19780 	246	266	240 	-0.07	0.00	0.07	0.00 	22.20	0.00	0.00 	22.20	97	52
19830 	251	268	239 	-0.01	0.00	0.01	0.00 	3.60	0.00	-0.00 	3.60	78	71
19880 	250	275	241 	-0.03	0.00	0.03	0.00 	10.50	0.00	0.00 	10.50	85	64
19930 	251	274	231 	0.07	0.00	-0.07	0.00 	-22.20	0.00	-0.00 	-22.20	52	97
19980 	255	278	241 	0.02	0.00	-0.02	0.00 	-5.40	0.00	0.00 	-5.40	69	80
20030 	255	278	241 	0.02	0.00	-0.02	0.00 	-5.40	0.00	0.00 	-5.40	69	80
20080 	264	290	240 	0.13	0.00	-0.13	0.00 	-39.00	0.00	-0.00 	-39.00	36	114
20130 	280	289	251 	0.18	0.00	-0.18	0.00 	-53.70	0.00	-0.00 	-53.70	21	128
20180 	285	303	250 	0.25	0.00	-0.25	0.00 	-75.90	0.00	-0.00 	-75.90	0	150
20230 	278	301	266 	0.02	0.00	-0.02	0.00 	-6.30	0.00	0.00 	-6.30	68	81
20280 	269	291	272 	-0.14	0.00	0.14	0.00 	41.70	0.00	0.00 	41.70	116	33
20330 	261	287	275 	-0.25	0.00	0.25	0.00 	75.90	0.00	0.00 	75.90	150	0
20380 	245	285	263 	-0.30	0.00	0.30	0.00 	88.50	0.00	0.00 	88.50	163	0
20430 	245	285	263 	-0.30	0.00	0.30	0.00 	88.50	0.00	0.00 	88.50	163	0
20480 	220	239	232 	-0.28	0.00	0.28	0.00 	84.30	0.00	-0.00 	84.30	159	0
20530 	204	225	226 	-0.39	0.00	0.39	0.00 	118.50	0.00	0.00 	118.50	193	0
20580 	201	217	206 	-0.23	0.00	0.23	0.00 	69.90	0.00	-0.00 	69.90	144	5
20630 	186	203	201 	-0.35	0.00	0.35	0.00 	104.10	0.00	0.00 	104.10	179	0
20680 	173	194	185 	-0.33	0.00	0.33	0.00 	97.80	0.00	-0.00 	97.80	172	0
20730 	163	173	178 	-0.38	0.00	0.38	0.00 	113.10	0.00	0.00 	113.10	188	0
20780 	159	160	169 	-0.34	0.00	0.34	0.00 	102.00	0.00	-0.00 	102.00	177	0
20830 	159	160	169 	-0.34	0.00	0.34	0.00 	102.00	0.00	0.00 	102.00	177	0
20880 	159	155	162 	-0.28	0.00	0.28	0.00 	82.50	0.00	-0.00 	82.50	157	0
20930 	162	154	160 	-0.23	0.00	0.23	0.00 	67.80	0.00	-0.00 	67.80	142	7
20980 	174	170	160 	-0.09	0.00	0.09	0.00 	27.00	0.00	-0.00 	27.00	102	48
21030 	188	192	168 	-0.01	0.00	0.01	0.00 	2.40	0.00	-0.00 	2.40	77	72
21080 	209	221	175 	0.16	0.00	-0.16	0.00 	-48.30	0.00	-0.00 	-48.30	26	123
21130 	227	244	186 	0.25	0.00	-0.25	0.00 	-76.20	0.00	-0.00 	-76.20	0	151
21180 	246	258	205 	0.27	0.00	-0.27	0.00 	-80.40	0.00	-0.00 	-80.40	0	155
21230 	265	278	218 	0.35	0.00	-0.35	0.00 	-104.40	0.00	-0.00 	-104.40	0	179
21280 	265	278	218 	0.35	0.00	-0.35	0.00 	-104.40	0.00	0.00 	-104.40	0	179
21330 	273	282	233 	0.28	0.00	-0.28	0.00 	-84.60	0.00	0.00 	-84.60	0	159
21380 	263	290	241 	0.11	0.00	-0.11	0.00 	-33.00	0.00	0.00 	-33.00	42	108
21430 	269	278	244 	0.13	0.00	-0.13	0.00 	-38.40	0.00	-0.00 	-38.40	36	113
21480 	272	286	252 	0.09	0.00	-0.09	0.00 	-25.80	0.00	0.00 	-25.80	49	100
21530 	272	287	253 	0.08	0.00	-0.08	0.00 	-23.10	0.00	0.00 	-23.10	51	98
21580 	266	285	263 	-0.09	0.00	0.09	0.00 	25.50	0.00	0.00 	25.50	100	49
21630 	277	295	263 	0.04	0.00	-0.04	0.00 	-10.50	0.00	-0.00 	-10.50	64	85
21680 	274	295	263 	0.04	0.00	-0.04	0.00 	-10.50	0.00	0.00 	-10.50	64	85
21730 	274	293	263 	0.00	0.00	-0.00	0.00 	-0.90	0.00	0.00 	-0.90	74	75
21780 	279	299	260 	0.09	0.00	-0.09	0.00 	-26.70	0.00	-0.00 	-26.70	48	101
21830 	280	294	266 	0.03	0.00	-0.03	0.00 	-10.20	0.00	0.00 	-10.20	64	85
21880 	277	302	272 	-0.05	0.00	0.05	0.00 	14.40	0.00	0.00 	14.40	89	60
21930 	279	304	268 	0.01	0.00	-0.01	0.00 	-4.20	0.00	-0.00 	-4.20	70	79
21980 	276	295	277 	-0.11	0.00	0.11	0.00 	34.50	0.00	0.00 	34.50	109	40
22030 	283	298	272 	0.01	0.00	-0.01	0.00 	-2.40	0.00	-0.00 	-2.40	72	77
22080 	278	295	274 	-0.06	0.00	0.06	0.00 	19.50	0.00	0.00 	19.50	94	55
22130 	282	309	270 	0.03	0.00	-0.03	0.00 	-8.70	0.00	-0.00 	-8.70	66	83
22180 	283	301	276 	-0.03	0.00	0.03	0.00 	8.70	0.00	0.00 	8.70	83	66
22230 	286	306	278 	-0.01	0.00	0.01	0.00 	4.20	0.00	-0.00 	4.20	79	70
22280 	293	312	278 	0.06	0.00	-0.06	0.00 	-18.60	0.00	-0.00 	-18.60	56	93
22330 	293	312	278 	0.06	0.00	-0.06	0.00 	-18.60	0.00	0.00 	-18.60	56	93
22380 	300	320	280 	0.12	0.00	-0.12	0.00 	-36.00	0.00	-0.00 	-36.00	39	111
22430 	297	320	284 	0.05	0.00	-0.05	0.00 	-15.00	0.00	0.00 	-15.00	60	90
22480 	306	322	291 	0.07	0.00	-0.07	0.00 	-21.60	0.00	-0.00 	-21.60	53	96
22530 	306	322	291 	0.07	0.00	-0.07	0.00 	-21.60	0.00	0.00 	-21.60	53	96
22580 	300	324	284 	0.08	0.00	-0.08	0.00 	-25.20	0.00	-0.00 	-25.20	49	100
22630 	302	324	292 	0.02	0.00	-0.02	0.00 	-7.20	0.00	0.00 	-7.20	67	82
22680 	302	328	290 	0.05	0.00	-0.05	0.00 	-14.40	0.00	-0.00 	-14.40	60	89
22730 	300	320	295 	-0.03	0.00	0.03	0.00 	9.00	0.00	0.00 	9.00	84	66
22780 	298	330	288 	0.03	0.00	-0.03	0.00 	-9.00	0.00	-0.00 	-9.00	66	84
22830 	294	323	287 	-0.01	0.00	0.01	0.00 	2.10	0.00	0.00 	2.10	77	72
22880 	296	319	292 	-0.04	0.00	0.04	0.00 	12.30	0.00	0.00 	12.30	87	62
22930 	290	319	287 	-0.05	0.00	0.05	0.00 	15.30	0.00	0.00 	15.30	90	59
22980 	293	324	278 	0.07	0.00	-0.07	0.00 	-22.20	0.00	-0.00 	-22.20	52	97

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00 0.00 0.00

Msec Fdir Cdir Rdir Steer Set error Ival Kp*err Ki*Ival Kd*Din Output Lspd Rspd

15780 381 411 364 0.18 0.00 -0.18 0.00 -54.30 0.00 -0.00 -54.30 20 129

15830 387 410 371 0.17 0.00 -0.17 0.00 -51.00 0.00 0.00 -51.00 24 126

15880 375 413 373 0.03 0.00 -0.03 0.00 -9.90 0.00 0.00 -9.90 65 84

15930 381 406 376 0.06 0.00 -0.06 0.00 -16.80 0.00 -0.00 -16.80 58 91

15980 381 418 366 0.17 0.00 -0.17 0.00 -50.40 0.00 -0.00 -50.40 24 125

16030 382 414 367 0.16 0.00 -0.16 0.00 -49.20 0.00 0.00 -49.20 25 124

16080 375 413 365 0.11 0.00 -0.11 0.00 -33.90 0.00 0.00 -33.90 41 108

16130 386 403 381 0.05 0.00 -0.05 0.00 -15.90 0.00 0.00 -15.90 59 90

16180 383 403 381 0.05 0.00 -0.05 0.00 -15.90 0.00 0.00 -15.90 59 90

16230 383 408 383 0.01 0.00 -0.01 0.00 -2.40 0.00 0.00 -2.40 72 77

16280 381 409 383 -0.01 0.00 0.01 0.00 3.30 0.00 0.00 3.30 78 71

16330 380 417 398 -0.16 0.00 0.16 0.00 48.90 0.00 0.00 48.90 123 26

16380 386 414 385 0.02 0.00 -0.02 0.00 -7.20 0.00 -0.00 -7.20 67 82

16430 370 395 373 -0.04 0.00 0.04 0.00 10.50 0.00 0.00 10.50 85 64

16480 352 387 363 -0.12 0.00 0.12 0.00 36.90 0.00 0.00 36.90 111 38

16530 350 374 358 -0.11 0.00 0.11 0.00 31.80 0.00 -0.00 31.80 106 43

16580 339 360 358 -0.23 0.00 0.23 0.00 69.00 0.00 0.00 69.00 144 6

16630 339 360 358 -0.23 0.00 0.23 0.00 69.00 0.00 0.00 69.00 144 6

16680 324 348 335 -0.16 0.00 0.16 0.00 48.60 0.00 -0.00 48.60 123 26

16730 314 345 305 0.04 0.00 -0.04 0.00 -10.50 0.00 -0.00 -10.50 64 85

16780 307 326 295 0.05 0.00 -0.05 0.00 -13.80 0.00 -0.00 -13.80 61 88

16830 293 320 298 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

16880 281 303 286 -0.07 0.00 0.07 0.00 20.10 0.00 -0.00 20.10 95 54

16930 281 300 278 -0.07 0.00 0.07 0.00 21.00 0.00 0.00 21.00 96 54

16980 279 287 274 -0.06 0.00 0.06 0.00 18.90 0.00 -0.00 18.90 93 56

17030 267 288 263 -0.07 0.00 0.07 0.00 21.60 0.00 0.00 21.60 96 53

17080 266 284 250 0.04 0.00 -0.04 0.00 -13.20 0.00 -0.00 -13.20 61 88

17130 255 270 254 -0.12 0.00 0.12 0.00 36.00 0.00 0.00 36.00 111 39

17180 257 270 247 -0.03 0.00 0.03 0.00 9.00 0.00 -0.00 9.00 84 66

17230 250 271 240 -0.03 0.00 0.03 0.00 8.70 0.00 -0.00 8.70 83 66

17280 241 267 240 -0.12 0.00 0.12 0.00 36.90 0.00 0.00 36.90 111 38

17330 244 262 231 -0.01 0.00 0.01 0.00 2.40 0.00 -0.00 2.40 77 72

17380 237 263 235 -0.12 0.00 0.12 0.00 35.10 0.00 0.00 35.10 110 39

17430 236 262 237 -0.15 0.00 0.15 0.00 44.40 0.00 0.00 44.40 119 30

17480 240 256 235 -0.09 0.00 0.09 0.00 28.20 0.00 -0.00 28.20 103 46

17530 242 251 237 -0.10 0.00 0.10 0.00 29.70 0.00 0.00 29.70 104 45

17580 254 261 231 0.09 0.00 -0.09 0.00 -27.30 0.00 -0.00 -27.30 47 102

17630 265 267 230 0.22 0.00 -0.22 0.00 -65.10 0.00 -0.00 -65.10 9 140

17680 274 289 243 0.20 0.00 -0.20 0.00 -59.70 0.00 0.00 -59.70 15 134

17730 275 279 253 0.10 0.00 -0.10 0.00 -29.70 0.00 0.00 -29.70 45 104

17780 275 292 258 0.06 0.00 -0.06 0.00 -18.60 0.00 0.00 -18.60 56 93

17830 277 287 257 0.09 0.00 -0.09 0.00 -26.10 0.00 -0.00 -26.10 48 101

17880 279 292 261 0.07 0.00 -0.07 0.00 -21.60 0.00 0.00 -21.60 53 96

17930 286 289 261 0.06 0.00 -0.06 0.00 -17.70 0.00 0.00 -17.70 57 92

17980 275 303 267 -0.02 0.00 0.02 0.00 5.10 0.00 0.00 5.10 80 69

18030 283 305 270 0.03 0.00 -0.03 0.00 -10.50 0.00 -0.00 -10.50 64 85

18080 286 297 271 0.05 0.00 -0.05 0.00 -14.10 0.00 -0.00 -14.10 60 89

18130 286 303 273 0.03 0.00 -0.03 0.00 -9.90 0.00 0.00 -9.90 65 84

18180 285 299 267 0.08 0.00 -0.08 0.00 -23.70 0.00 -0.00 -23.70 51 98

18230 284 302 270 0.04 0.00 -0.04 0.00 -12.60 0.00 0.00 -12.60 62 87

18280 283 295 275 -0.02 0.00 0.02 0.00 7.50 0.00 0.00 7.50 82 67

18330 281 311 262 0.10 0.00 -0.10 0.00 -30.30 0.00 -0.00 -30.30 44 105

18380 283 299 269 0.04 0.00 -0.04 0.00 -11.70 0.00 0.00 -11.70 63 86

18430 278 304 268 0.00 0.00 -0.00 0.00 -1.20 0.00 0.00 -1.20 73 76

18480 279 297 271 -0.02 0.00 0.02 0.00 6.90 0.00 0.00 6.90 81 68

18530 283 303 268 0.05 0.00 -0.05 0.00 -15.90 0.00 -0.00 -15.90 59 90

18580 280 294 268 0.01 0.00 -0.01 0.00 -4.20 0.00 0.00 -4.20 70 79

18630 281 292 271 -0.01 0.00 0.01 0.00 2.40 0.00 0.00 2.40 77 72

18680 275 288 268 -0.04 0.00 0.04 0.00 12.60 0.00 0.00 12.60 87 62

18730 272 294 266 -0.05 0.00 0.05 0.00 13.80 0.00 0.00 13.80 88 61

18780 273 282 259 0.02 0.00 -0.02 0.00 -6.60 0.00 -0.00 -6.60 68 81

18830 264 291 265 -0.12 0.00 0.12 0.00 35.70 0.00 0.00 35.70 110 39

18880 270 273 266 -0.09 0.00 0.09 0.00 26.10 0.00 -0.00 26.10 101 48

18930 267 282 259 -0.04 0.00 0.04 0.00 11.40 0.00 -0.00 11.40 86 63

18980 268 283 258 -0.02 0.00 0.02 0.00 5.10 0.00 -0.00 5.10 80 69

19030 263 279 256 -0.05 0.00 0.05 0.00 15.30 0.00 0.00 15.30 90 59

19080 261 269 252 -0.04 0.00 0.04 0.00 12.30 0.00 -0.00 12.30 87 62

19130 259 280 241 0.06 0.00 -0.06 0.00 -18.00 0.00 -0.00 -18.00 56 93

19180 257 274 251 -0.07 0.00 0.07 0.00 19.80 0.00 0.00 19.80 94 55

19230 252 275 246 -0.06 0.00 0.06 0.00 19.50 0.00 -0.00 19.50 94 55

19280 259 273 241 0.00 0.00 -0.00 0.00 -0.90 0.00 -0.00 -0.90 74 75

19330 259 282 238 0.09 0.00 -0.09 0.00 -27.60 0.00 -0.00 -27.60 47 102

19380 250 272 244 -0.07 0.00 0.07 0.00 20.40 0.00 0.00 20.40 95 54

19430 252 274 249 -0.10 0.00 0.10 0.00 28.80 0.00 0.00 28.80 103 46

19480 246 270 246 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

19530 248 270 248 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

19580 250 269 244 -0.07 0.00 0.07 0.00 21.30 0.00 -0.00 21.30 96 53

19630 250 270 241 -0.04 0.00 0.04 0.00 12.00 0.00 -0.00 12.00 87 63

19680 250 277 235 0.03 0.00 -0.03 0.00 -8.10 0.00 -0.00 -8.10 66 83

19730 246 274 235 -0.02 0.00 0.02 0.00 4.80 0.00 0.00 4.80 79 70

19780 246 266 240 -0.07 0.00 0.07 0.00 22.20 0.00 0.00 22.20 97 52

19830 251 268 239 -0.01 0.00 0.01 0.00 3.60 0.00 -0.00 3.60 78 71

19880 250 275 241 -0.03 0.00 0.03 0.00 10.50 0.00 0.00 10.50 85 64

19930 251 274 231 0.07 0.00 -0.07 0.00 -22.20 0.00 -0.00 -22.20 52 97

19980 255 278 241 0.02 0.00 -0.02 0.00 -5.40 0.00 0.00 -5.40 69 80

20030 255 278 241 0.02 0.00 -0.02 0.00 -5.40 0.00 0.00 -5.40 69 80

20080 264 290 240 0.13 0.00 -0.13 0.00 -39.00 0.00 -0.00 -39.00 36 114

20130 280 289 251 0.18 0.00 -0.18 0.00 -53.70 0.00 -0.00 -53.70 21 128

20180 285 303 250 0.25 0.00 -0.25 0.00 -75.90 0.00 -0.00 -75.90 0 150

20230 278 301 266 0.02 0.00 -0.02 0.00 -6.30 0.00 0.00 -6.30 68 81

20280 269 291 272 -0.14 0.00 0.14 0.00 41.70 0.00 0.00 41.70 116 33

20330 261 287 275 -0.25 0.00 0.25 0.00 75.90 0.00 0.00 75.90 150 0

20380 245 285 263 -0.30 0.00 0.30 0.00 88.50 0.00 0.00 88.50 163 0

20430 245 285 263 -0.30 0.00 0.30 0.00 88.50 0.00 0.00 88.50 163 0

20480 220 239 232 -0.28 0.00 0.28 0.00 84.30 0.00 -0.00 84.30 159 0

20530 204 225 226 -0.39 0.00 0.39 0.00 118.50 0.00 0.00 118.50 193 0

20580 201 217 206 -0.23 0.00 0.23 0.00 69.90 0.00 -0.00 69.90 144 5

20630 186 203 201 -0.35 0.00 0.35 0.00 104.10 0.00 0.00 104.10 179 0

20680 173 194 185 -0.33 0.00 0.33 0.00 97.80 0.00 -0.00 97.80 172 0

20730 163 173 178 -0.38 0.00 0.38 0.00 113.10 0.00 0.00 113.10 188 0

20780 159 160 169 -0.34 0.00 0.34 0.00 102.00 0.00 -0.00 102.00 177 0

20830 159 160 169 -0.34 0.00 0.34 0.00 102.00 0.00 0.00 102.00 177 0

20880 159 155 162 -0.28 0.00 0.28 0.00 82.50 0.00 -0.00 82.50 157 0

20930 162 154 160 -0.23 0.00 0.23 0.00 67.80 0.00 -0.00 67.80 142 7

20980 174 170 160 -0.09 0.00 0.09 0.00 27.00 0.00 -0.00 27.00 102 48

21030 188 192 168 -0.01 0.00 0.01 0.00 2.40 0.00 -0.00 2.40 77 72

21080 209 221 175 0.16 0.00 -0.16 0.00 -48.30 0.00 -0.00 -48.30 26 123

21130 227 244 186 0.25 0.00 -0.25 0.00 -76.20 0.00 -0.00 -76.20 0 151

21180 246 258 205 0.27 0.00 -0.27 0.00 -80.40 0.00 -0.00 -80.40 0 155

21230 265 278 218 0.35 0.00 -0.35 0.00 -104.40 0.00 -0.00 -104.40 0 179

21280 265 278 218 0.35 0.00 -0.35 0.00 -104.40 0.00 0.00 -104.40 0 179

21330 273 282 233 0.28 0.00 -0.28 0.00 -84.60 0.00 0.00 -84.60 0 159

21380 263 290 241 0.11 0.00 -0.11 0.00 -33.00 0.00 0.00 -33.00 42 108

21430 269 278 244 0.13 0.00 -0.13 0.00 -38.40 0.00 -0.00 -38.40 36 113

21480 272 286 252 0.09 0.00 -0.09 0.00 -25.80 0.00 0.00 -25.80 49 100

21530 272 287 253 0.08 0.00 -0.08 0.00 -23.10 0.00 0.00 -23.10 51 98

21580 266 285 263 -0.09 0.00 0.09 0.00 25.50 0.00 0.00 25.50 100 49

21630 277 295 263 0.04 0.00 -0.04 0.00 -10.50 0.00 -0.00 -10.50 64 85

21680 274 295 263 0.04 0.00 -0.04 0.00 -10.50 0.00 0.00 -10.50 64 85

21730 274 293 263 0.00 0.00 -0.00 0.00 -0.90 0.00 0.00 -0.90 74 75

21780 279 299 260 0.09 0.00 -0.09 0.00 -26.70 0.00 -0.00 -26.70 48 101

21830 280 294 266 0.03 0.00 -0.03 0.00 -10.20 0.00 0.00 -10.20 64 85

21880 277 302 272 -0.05 0.00 0.05 0.00 14.40 0.00 0.00 14.40 89 60

21930 279 304 268 0.01 0.00 -0.01 0.00 -4.20 0.00 -0.00 -4.20 70 79

21980 276 295 277 -0.11 0.00 0.11 0.00 34.50 0.00 0.00 34.50 109 40

22030 283 298 272 0.01 0.00 -0.01 0.00 -2.40 0.00 -0.00 -2.40 72 77

22080 278 295 274 -0.06 0.00 0.06 0.00 19.50 0.00 0.00 19.50 94 55

22130 282 309 270 0.03 0.00 -0.03 0.00 -8.70 0.00 -0.00 -8.70 66 83

22180 283 301 276 -0.03 0.00 0.03 0.00 8.70 0.00 0.00 8.70 83 66

22230 286 306 278 -0.01 0.00 0.01 0.00 4.20 0.00 -0.00 4.20 79 70

22280 293 312 278 0.06 0.00 -0.06 0.00 -18.60 0.00 -0.00 -18.60 56 93

22330 293 312 278 0.06 0.00 -0.06 0.00 -18.60 0.00 0.00 -18.60 56 93

22380 300 320 280 0.12 0.00 -0.12 0.00 -36.00 0.00 -0.00 -36.00 39 111

22430 297 320 284 0.05 0.00 -0.05 0.00 -15.00 0.00 0.00 -15.00 60 90

22480 306 322 291 0.07 0.00 -0.07 0.00 -21.60 0.00 -0.00 -21.60 53 96

22530 306 322 291 0.07 0.00 -0.07 0.00 -21.60 0.00 0.00 -21.60 53 96

22580 300 324 284 0.08 0.00 -0.08 0.00 -25.20 0.00 -0.00 -25.20 49 100

22630 302 324 292 0.02 0.00 -0.02 0.00 -7.20 0.00 0.00 -7.20 67 82

22680 302 328 290 0.05 0.00 -0.05 0.00 -14.40 0.00 -0.00 -14.40 60 89

22730 300 320 295 -0.03 0.00 0.03 0.00 9.00 0.00 0.00 9.00 84 66

22780 298 330 288 0.03 0.00 -0.03 0.00 -9.00 0.00 -0.00 -9.00 66 84

22830 294 323 287 -0.01 0.00 0.01 0.00 2.10 0.00 0.00 2.10 77 72

22880 296 319 292 -0.04 0.00 0.04 0.00 12.30 0.00 0.00 12.30 87 62

22930 290 319 287 -0.05 0.00 0.05 0.00 15.30 0.00 0.00 15.30 90 59

22980 293 324 278 0.07 0.00 -0.07 0.00 -22.20 0.00 -0.00 -22.20 52 97

WallE3_WallTrack_V2’s tracking algorithm uses the difference between the desired and measured offset distances to ‘tweak’ the steering value, as discussed above, so clearly this works – or at least doesn’t screw things up too badly. In the above telemetry output, the ‘Steer’ column is the steering value after the offset distance adjustment shown here

//have to weight value by both angle and wall offset
WallTrackSteerVal = glLeftSteeringVal + (glLidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

1 2	//have to weight value by both angle and wall offset WallTrackSteerVal = glLeftSteeringVal + (glLidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

So at the point where the robot hit the minimum center distance of about 154mm, the steering value adjustment would be (154-400)/1000 = -0.246. The total steering value term at this point was 0.23, which means the ‘raw’ steering value was +0.016 and the offset distance error term accounted for ~97% of the total. This is good evidence that including the the distance offset term works.

My new new new plan is to focus on my October 2022 post that uses the orientation angle corrected offset distance as the input to the PID engine, and see if I can incorporate this, along with all my recent updates/bugfixes into WallE3_WallTrackTuning_V5

More Wall Track PID Tuning Work

1 Reply

Posted 15 October 2022

While working on my new ‘RunToDaylight’ algorithm for WallE3, my autonomous wall-following robot, I noticed that when WallE3 finds a wall to track after travelling in the direction of most front distance, it doesn’t do a very good job at all, oscillating crazily back and forth, and sometimes running head-first into the wall it is supposedly trying to track. This is somewhat disconcerting, as I thought I had long ago established a good wall tracking algorithm. So, I decided to once again plunge headlong into the swamps and jungles of my wall-tracking algorithm, hoping against hope that I won’t get eaten by mosquitos, snakes or crocodiles.

I went back to my latest part-task program, ‘WallE3_WallTrackTuning’. This program actually does OK when the robot starts out close to the wall, as the ‘CaptureWallOffset()’ routine is pretty effective. However, I noticed that when the robot starts out within +/- 4cm from the defined offset value, it isn’t terribly robust; the robot typically just goes straight with very few adjustments, even as it gets farther and farther away from the offset distance – oops!

So, I created yet another part-task program ‘WallE3_WallTrackTuning_V2’ to see if I could figure out why it isn’t working so well. With ‘WallE3_WallTrackTuning’ I simply called either TrackLeftWallOffset() or TrackRightWallOffset() and fed it the user-entered offset and PID values. However, this time I decided to pare down the code to the absolute minimum required to track a wall, as shown below (user input code not shown for clarity):

  while (true)
  {
    //10/16/22 starting over with wall track tuning
    float kp = WallTrack_Kp;
    float ki = WallTrack_Ki;
    float kd = WallTrack_Kd;

    float lastError = 0;
    float lastInput = 0;
    float lastIval = 0;
    float lastDerror = 0;
    WallTrackSetPoint = OffCm; //04/17/22 this value holds robot parallel to left wall

    gl_pSerPort->printf("\nTrackLeftWallOffset: Start tracking offset of %2.1fcm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n",
      WallTrackSetPoint, kp, ki, kd);

    gl_pSerPort->printf(WallFollowTelemHdrStr);

    //Step4: Track the wall using PID algorithm
    mSecSinceLastWallTrackUpdate = 0; //added 09/04/21
    MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

    while (true)
    {
      CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

      if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)
      {
        mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

        //03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()
        UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls

        //from Teensy_7VL53L0X_I2C_Slave_V3.ino: LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post
        float orientdeg = GetWallOrientDeg(glLeftSteeringVal);
        float orientrad = (PI / 180.f) * orientdeg; 
        float orientcos = cosf(orientrad);
        glLeftCentCorrCm = glLeftCenterCm * orientcos;

        if (!isnan(glLeftCentCorrCm))
        {
          //gl_pSerPort->printf("just before PIDCalcs call: WallTrackSetPoint  = %2.2f\n", WallTrackSetPoint);
          WallTrackOutput = PIDCalcs(glLeftCentCorrCm, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,
            kp, ki, kd);

          //04/05/22 have to use local var here, as result could be negative
          int16_t leftSpdnum = MOTOR_SPEED_QTR + WallTrackOutput;
          int16_t rightSpdnum = MOTOR_SPEED_QTR - WallTrackOutput;

          //04/05/22 Left/rightSpdnum can be negative here - watch out!
          rightSpdnum = (rightSpdnum <= MOTOR_SPEED_HALF) ? rightSpdnum : MOTOR_SPEED_HALF; //result can still be neg
          glRightspeednum = (rightSpdnum > 0) ? rightSpdnum : 0; //result here must be positive
          leftSpdnum = (leftSpdnum <= MOTOR_SPEED_HALF) ? leftSpdnum : MOTOR_SPEED_HALF;//result can still be neg
          glLeftspeednum = (leftSpdnum > 0) ? leftSpdnum : 0; //result here must be positive

          MoveAhead(glLeftspeednum, glRightspeednum);

          gl_pSerPort->printf(WallFollowTelemStr, millis(),
            glLeftFrontCm, glLeftCenterCm, glLeftRearCm, 
            glLeftSteeringVal, orientdeg, orientcos, glLeftCentCorrCm,
            WallTrackSetPoint, lastError, WallTrackOutput,
            glLeftspeednum, glRightspeednum);
        }
      }
    }


    //09/29/22 if we get to here, hold in loop for user input
    mSecSinceLastWallTrackUpdate = 0;
    while (true)
    {
      CheckForUserInput();

      if (mSecSinceLastWallTrackUpdate >= 10 * WALL_TRACK_UPDATE_INTERVAL_MSEC)
      {
        mSecSinceLastWallTrackUpdate -= 10 * WALL_TRACK_UPDATE_INTERVAL_MSEC;
        gl_pSerPort->printf("After exit from TrackRightWallOffset\n");
      }
    }
  }

while (true)

{

//10/16/22 starting over with wall track tuning

float kp = WallTrack_Kp;

float ki = WallTrack_Ki;

float kd = WallTrack_Kd;

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

WallTrackSetPoint = OffCm; //04/17/22 this value holds robot parallel to left wall

gl_pSerPort->printf("\nTrackLeftWallOffset: Start tracking offset of %2.1fcm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n",

WallTrackSetPoint, kp, ki, kd);

gl_pSerPort->printf(WallFollowTelemHdrStr);

//Step4: Track the wall using PID algorithm

mSecSinceLastWallTrackUpdate = 0; //added 09/04/21

MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

while (true)

{

CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)

{

mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

//03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()

UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls

//from Teensy_7VL53L0X_I2C_Slave_V3.ino: LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

float orientdeg = GetWallOrientDeg(glLeftSteeringVal);

float orientrad = (PI / 180.f) * orientdeg;

float orientcos = cosf(orientrad);

glLeftCentCorrCm = glLeftCenterCm * orientcos;

if (!isnan(glLeftCentCorrCm))

{

//gl_pSerPort->printf("just before PIDCalcs call: WallTrackSetPoint = %2.2f\n", WallTrackSetPoint);

WallTrackOutput = PIDCalcs(glLeftCentCorrCm, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,

kp, ki, kd);

//04/05/22 have to use local var here, as result could be negative

int16_t leftSpdnum = MOTOR_SPEED_QTR + WallTrackOutput;

int16_t rightSpdnum = MOTOR_SPEED_QTR - WallTrackOutput;

//04/05/22 Left/rightSpdnum can be negative here - watch out!

rightSpdnum = (rightSpdnum <= MOTOR_SPEED_HALF) ? rightSpdnum : MOTOR_SPEED_HALF; //result can still be neg

glRightspeednum = (rightSpdnum > 0) ? rightSpdnum : 0; //result here must be positive

leftSpdnum = (leftSpdnum <= MOTOR_SPEED_HALF) ? leftSpdnum : MOTOR_SPEED_HALF;//result can still be neg

glLeftspeednum = (leftSpdnum > 0) ? leftSpdnum : 0; //result here must be positive

MoveAhead(glLeftspeednum, glRightspeednum);

gl_pSerPort->printf(WallFollowTelemStr, millis(),

glLeftFrontCm, glLeftCenterCm, glLeftRearCm,

glLeftSteeringVal, orientdeg, orientcos, glLeftCentCorrCm,

WallTrackSetPoint, lastError, WallTrackOutput,

glLeftspeednum, glRightspeednum);

}

//09/29/22 if we get to here, hold in loop for user input

mSecSinceLastWallTrackUpdate = 0;

while (true)

{

CheckForUserInput();

if (mSecSinceLastWallTrackUpdate >= 10 * WALL_TRACK_UPDATE_INTERVAL_MSEC)

{

mSecSinceLastWallTrackUpdate -= 10 * WALL_TRACK_UPDATE_INTERVAL_MSEC;

gl_pSerPort->printf("After exit from TrackRightWallOffset\n");

}

The big change from previous versions was to go back to using the desired offset distance as the setpoint for the PID algorithm, and using the measured distance from the (left or right) center VL53L0X sensor as the input to be controlled. Now one might be excused from wondering why the heck I wasn’t doing this all along, as it does seem logical that if you want to control the robot’s distance from the near wall, you should use the desired distance as the setpoint and the measured distance as the input – duh!

Well, way back in the beginning of time, when I changed over from dual ultrasonic ‘Ping’ sensors to dual arrays of three VL53L0X LIDAR sensors well over 18 months ago, I wound up using a combination of the ‘steering value’ ( (front – rear)/100 ) and the reported center distance – desired offset as the input to the PID calc routine, as shown in the following code snippet:

	while (true)
	{
		CheckForUserInput(); //this is a bit recursive, but should still work (I hope)
 
		//now using TIMER5 100 mSec interrupt for timing
		if (bTimeForNavUpdate)
		{
			//sinceLastComputeTime -= WALL_TRACK_UPDATE_INTERVAL_MSEC;
			bTimeForNavUpdate = false;
 
			//GetRequestedVL53l0xValues(VL53L0X_LEFT); //now done in TIMER5 ISR
 
			//have to weight value by both angle and wall offset
			WallTrackSteerVal = LeftSteeringVal + (Lidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;
 
			//update motor speeds, skipping bad values
			if (!isnan(WallTrackSteerVal))
			{
				//10/12/20 now this executes every time, with interval controlled by timer ISR
				PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
					kp, ki, kd, WallTrackOutput);
				//PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, WALL_TRACK_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
				//	kp, ki, kd, WallTrackOutput);
 
				int speed = 0;
 
				int leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;
				int rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;
 
				rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL;
				rightspeednum = (rightspeednum > 0) ? rightspeednum : 0;
				leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL;
				leftspeednum = (leftspeednum > 0) ? leftspeednum : 0;
 
				MoveAhead(leftspeednum, rightspeednum);
 
				mySerial.printf("%lu \t%d\t%d\t%d \t%2.2f\t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%d\t%d\n",
					millis(),
					Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
					WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,
					kp * lastError, ki * lastIval, kd * lastDerror,
					WallTrackOutput, leftspeednum, rightspeednum);
			}
		}
	}

while (true)

{

CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

//now using TIMER5 100 mSec interrupt for timing

if (bTimeForNavUpdate)

{

//sinceLastComputeTime -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

bTimeForNavUpdate = false;

//GetRequestedVL53l0xValues(VL53L0X_LEFT); //now done in TIMER5 ISR

//have to weight value by both angle and wall offset

WallTrackSteerVal = LeftSteeringVal + (Lidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

//10/12/20 now this executes every time, with interval controlled by timer ISR

PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

kp, ki, kd, WallTrackOutput);

//PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, WALL_TRACK_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

// kp, ki, kd, WallTrackOutput);

int speed = 0;

int leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

int rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL;

rightspeednum = (rightspeednum > 0) ? rightspeednum : 0;

leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL;

leftspeednum = (leftspeednum > 0) ? leftspeednum : 0;

MoveAhead(leftspeednum, rightspeednum);

mySerial.printf("%lu \t%d\t%d\t%d \t%2.2f\t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%d\t%d\n",

millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,

kp * lastError, ki * lastIval, kd * lastDerror,

WallTrackOutput, leftspeednum, rightspeednum);

}

This is the line that calculates the input value to the PID:

//have to weight value by both angle and wall offset
WallTrackSteerVal = LeftSteeringVal + (Lidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

1 2	//have to weight value by both angle and wall offset WallTrackSteerVal = LeftSteeringVal + (Lidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

This actually worked pretty well, but as I discovered recently, it is very difficult to integrate two very different physical mechanisms in the same calculations – almost literally oranges and apples. When the offset is small, the steering value term dominates and the robot simply continues more or less – but not quite – parallel to the wall, meaning that it slowly diverges from the desired offset – getting closer or further away. When the divergence gets large enough the offset term dominates and the robot turns toward the desired offset, but it is almost impossible to get PID terms that are large enough to actually make corrections without being so large as to cause wild oscillations.

The above problem didn’t really come to a head until just recently when I started having problems with tracking where the robot started off at or close to the desired offset value and generally parallel, meaning both terms in the above expression were near zero – for this case the behavior was a bit erratic to say the least.

So, back to the basics. The following plot and short video show the robot’s behavior with the new setup (offset = 40cm, PID = (10,0,0)):

Tracking left wall with desired offset = 40cm

Desired offset = 40cm, PID = (10,0,0)

With this setup, the robot tracks the desired 40cm offset very well (average of 41.77cm), with a very slow oscillation. I’m sure I can tweak this up a bit with a slightly higher ‘P’ value and then adding a small amount of ‘I’, but even with the most basic parameter set the system is obviously stable.

20 October 2022 Update:

I made another run with PID(10,0,0), but this time I started the run with the robot displaced about 7cm from the 40cm offset. As shown in the plot and short video, this caused quite a large oscillation; not quite disastrous, but probably would have been if my test wall had been any longer.

PID(10,0,0) with robot initial distance from wall = 33cm

PID(10,0,0) with initial position at 33cm

After looking at the data from this run, I decided to try lowering the P value from 10 to 5, thinking that the lower value would reduce the oscillation magnitude with a non-zero initial displacement from the desired setpoint. As the following plot and short video shows, the result was much better.

221022 PID(5,0,0) init dist 33cm

So then I tried PID(3,0,0), again with an initial placement of 33cm from the wall, 7cm from the setpoint of 40cm

PID(3,0,0) init dist 33cm, avg for all points ~41.3cm

As shown by the plot and video, PID(3,0,0) does a very good job of recovering from the large initial offset and then maintaining the desired 40cm offset. This result makes me start to wonder if my separate ‘Approach and Capture’ stage is required at all. However, a subsequent run with PID (3,0,0) but with an initial placement of 15cm (25cm error) disabused me of any thoughts like that!

After talking this over with my PID-expert stepson, he recommended that I continue to decrease the ‘P’ term until the robot never quite gets to the desired setpoint before running out of room, and then adding some (possibly very small) amount of ‘D’ to hasten capture of the desired setpoint. So, I continued, starting with a ‘P’ value of 2, as shown below:

The ‘Err’ term is the actual PID error term, not multiplied by P as before

This result was a bit unexpected, as I thought such a ‘straight-line’ trajectory should have ended before going past the 40cm setpoint, indicating that I had achieved my ‘not quite controlling’ value of ‘P’. However, after thinking about a bit and looking at the actual data (shown below), I think what this shows is that the robot case is fundamentally different than most PID control applications in that reducing the error term (and thus the ‘drive’ signal) doesn’t necessarily change the robot’s trajectory, as the robot will tend to travel in a straight line in the absence of a contravening error term. In other words, removing the ‘drive’ due to the error term doesn’t cause the robot to slow down, as it would in a normal motor drive scenario.

Msec	LF	LC	LR	Steer	Deg	Cos	LCCorr	Set	Err	Output	LSpd	RSpd
105681	31	33	33	-0.28	-16	0.96	31.7	40	8.3	16.6	91	58
105781	30	33	32	-0.12	-6.86	0.99	32.8	40	7.2	14.5	89	60
105886	31	33	32	-0.07	-4	1	32.9	40	7.1	14.2	89	60
105980	31	33	34	-0.28	-16	0.96	31.7	40	8.3	16.6	91	58
106087	31	33	33	-0.19	-10.86	0.98	32.4	40	7.6	15.2	90	59
106182	32	34	33	-0.16	-9.14	0.99	33.6	40	6.4	12.9	87	62
106280	32	34	34	-0.16	-9.14	0.99	33.6	40	6.4	12.9	87	62
106388	33	34	33	-0.04	-2.29	1	34	40	6	12.1	87	62
106482	33	35	34	-0.04	-2.29	1	35	40	5	10.1	85	64
106589	34	37	36	-0.16	-9.14	0.99	36.5	40	3.5	6.9	81	68
106683	35	35	36	-0.07	-4	1	34.9	40	5.1	10.2	85	64
106781	36	38	36	-0.08	-4.57	1	37.9	40	2.1	4.2	79	70
106885	36	39	38	-0.26	-14.86	0.97	37.7	40	2.3	4.6	79	70
106981	37	38	39	-0.23	-13.14	0.97	37	40	3	6	80	69
107084	38	40	39	-0.12	-6.86	0.99	39.7	40	0.3	0.6	75	74
107179	38	42	41	-0.3	-17.14	0.96	40.1	40	-0.1	-0.3	74	75
107279	39	43	41	-0.22	-12.57	0.98	42	40	-2	-3.9	71	78
107388	40	43	41	-0.14	-8	0.99	42.6	40	-2.6	-5.2	69	80
107479	42	43	42	-0.03	-1.71	1	43	40	-3	-6	69	80
107586	42	46	44	-0.25	-14.29	0.97	44.6	40	-4.6	-9.2	65	84
107680	45	47	45	0.05	2.86	1	46.9	40	-6.9	-13.9	61	88
107780	44	49	47	-0.39	-22.29	0.93	45.3	40	-5.3	-10.7	64	85
107886	45	48	48	-0.27	-15.43	0.96	46.3	40	-6.3	-12.5	62	87
107979	47	49	50	-0.25	-14.29	0.97	47.5	40	-7.5	-15	60	89
108086	48	48	50	-0.31	-17.71	0.95	45.7	40	-5.7	-11.4	63	86
108179	48	49	51	-0.34	-19.43	0.94	46.2	40	-6.2	-12.4	62	87
108280	48	51	52	-0.35	-20	0.94	47.9	40	-7.9	-15.8	59	90
108384	49	50	53	-0.35	-20	0.94	47	40	-7	-14	61	88
108481	50	53	52	-0.16	-9.14	0.99	52.3	40	-12.3	-24.7	50	99
108586	51	53	52	-0.11	-6.29	0.99	52.7	40	-12.7	-25.4	49	100
108680	52	55	53	-0.13	-7.43	0.99	54.5	40	-14.5	-29.1	45	104
108781	50	54	55	-0.51	-29.14	0.87	47.2	40	-7.2	-14.3	60	89
108885	50	53	53	-0.21	-12	0.98	51.8	40	-11.8	-23.7	51	98
108978	52	55	56	-0.43	-24.57	0.91	50	40	-10	-20	54	95
109083	52	54	56	-0.4	-22.86	0.92	49.8	40	-9.8	-19.5	55	94
109181	52	54	55	-0.27	-15.43	0.96	52.1	40	-12.1	-24.1	50	99

Msec LF LC LR Steer Deg Cos LCCorr Set Err Output LSpd RSpd

105681 31 33 33 -0.28 -16 0.96 31.7 40 8.3 16.6 91 58

105781 30 33 32 -0.12 -6.86 0.99 32.8 40 7.2 14.5 89 60

105886 31 33 32 -0.07 -4 1 32.9 40 7.1 14.2 89 60

105980 31 33 34 -0.28 -16 0.96 31.7 40 8.3 16.6 91 58

106087 31 33 33 -0.19 -10.86 0.98 32.4 40 7.6 15.2 90 59

106182 32 34 33 -0.16 -9.14 0.99 33.6 40 6.4 12.9 87 62

106280 32 34 34 -0.16 -9.14 0.99 33.6 40 6.4 12.9 87 62

106388 33 34 33 -0.04 -2.29 1 34 40 6 12.1 87 62

106482 33 35 34 -0.04 -2.29 1 35 40 5 10.1 85 64

106589 34 37 36 -0.16 -9.14 0.99 36.5 40 3.5 6.9 81 68

106683 35 35 36 -0.07 -4 1 34.9 40 5.1 10.2 85 64

106781 36 38 36 -0.08 -4.57 1 37.9 40 2.1 4.2 79 70

106885 36 39 38 -0.26 -14.86 0.97 37.7 40 2.3 4.6 79 70

106981 37 38 39 -0.23 -13.14 0.97 37 40 3 6 80 69

107084 38 40 39 -0.12 -6.86 0.99 39.7 40 0.3 0.6 75 74

107179 38 42 41 -0.3 -17.14 0.96 40.1 40 -0.1 -0.3 74 75

107279 39 43 41 -0.22 -12.57 0.98 42 40 -2 -3.9 71 78

107388 40 43 41 -0.14 -8 0.99 42.6 40 -2.6 -5.2 69 80

107479 42 43 42 -0.03 -1.71 1 43 40 -3 -6 69 80

107586 42 46 44 -0.25 -14.29 0.97 44.6 40 -4.6 -9.2 65 84

107680 45 47 45 0.05 2.86 1 46.9 40 -6.9 -13.9 61 88

107780 44 49 47 -0.39 -22.29 0.93 45.3 40 -5.3 -10.7 64 85

107886 45 48 48 -0.27 -15.43 0.96 46.3 40 -6.3 -12.5 62 87

107979 47 49 50 -0.25 -14.29 0.97 47.5 40 -7.5 -15 60 89

108086 48 48 50 -0.31 -17.71 0.95 45.7 40 -5.7 -11.4 63 86

108179 48 49 51 -0.34 -19.43 0.94 46.2 40 -6.2 -12.4 62 87

108280 48 51 52 -0.35 -20 0.94 47.9 40 -7.9 -15.8 59 90

108384 49 50 53 -0.35 -20 0.94 47 40 -7 -14 61 88

108481 50 53 52 -0.16 -9.14 0.99 52.3 40 -12.3 -24.7 50 99

108586 51 53 52 -0.11 -6.29 0.99 52.7 40 -12.7 -25.4 49 100

108680 52 55 53 -0.13 -7.43 0.99 54.5 40 -14.5 -29.1 45 104

108781 50 54 55 -0.51 -29.14 0.87 47.2 40 -7.2 -14.3 60 89

108885 50 53 53 -0.21 -12 0.98 51.8 40 -11.8 -23.7 51 98

108978 52 55 56 -0.43 -24.57 0.91 50 40 -10 -20 54 95

109083 52 54 56 -0.4 -22.86 0.92 49.8 40 -9.8 -19.5 55 94

109181 52 54 55 -0.27 -15.43 0.96 52.1 40 -12.1 -24.1 50 99

23 October 2022 Update:

In previous work on this subject, I had already recognized that the ‘capture’ and ‘track’ phases of Wall-E’s behavior required separate treatment, and had implemented this with a ‘CaptureWallOffset()’ function to handle the ‘capture’ phase. This function calculates the amount of rotation needed to achieve an approximately 30 deg orientation angle with respect to the near wall, then moves forward to the desired wall offset value, and then turns back to parallel the near wall.

So, my next step is to re-integrate this ‘CaptureWallOffset()’ routine with my current distance-only based offset tracking algorithm. The idea is to essentially eliminate the problem space where the distance-only PID algorithm fails, so the PID only has to handle initial conditions very near the desired setpoint. When the ‘CaptureWallOffset()’ routine completes, the robot should be oriented parallel to the wall, and at a distance close to (but not necessarily the same as) the desired setpoint. Based on the above results, I think I will change the setpoint from the current constant value (40 cm at present) to match the measured distance from the wall at the point of ‘CaptureWallOffset()’ routine completion. This will guarantee that the PID starts out with the input matching the setpoint – i.e. zero error.

With this new setup, I made a run with the robot starting just 13cm from the wall. The CaptureWallOffset() routine moved the robot from the initial 13cm to about 37cm, with the robot ending up nearly parallel. The PID tracking algorithm started with an initial error term of +3.3, and tracked very well with a ‘P’ value of 10. See the plot and short video below. The video shows both the capture and track phases, but the plot only shows the track portion of the run.

PID tracking after completion of CaptureWallOffset()

Here’s a run with PID(3,0,0), starting at an offset of 22cm.

24 October 2022 Update:

While reading through some more PID theory/practice articles, I was once again reminded that smaller time intervals generally produce better results, and that struck a bit of a chord. Some time back I settled on a time interval of about 200mSec, but while I was working with my ‘WallTrackTuning_V2’ program I realized that this interval was due to the time required by the PulsedLight LIDAR to produce a front distance measurement. I discovered this when I tried to reduce the overall update time from 200 to 100mSec and got lots of errors from GetFrontDistCm(). After figuring this out, I modified the code to use a 200mSec time interval for the front LIDAR, and 100mSec for the VL53L0X side distance sensors.

So, it occurred to me that I might be able to reduce the side measurement interval even further, so I instrumented the robot to toggle a digital output (I borrowed the output for the red laser pointer) at the start and end of the wall tracking adjustment cycle, as shown in the code snippet below:

      if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)
      {
        digitalToggle(RED_LASER_DIODE_PIN); //10/24/22 toggle duration measurement pulse
        mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

        //03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()
        UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls

        //from Teensy_7VL53L0X_I2C_Slave_V3.ino: LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post
        float orientdeg = GetWallOrientDeg(glLeftSteeringVal);
        float orientrad = (PI / 180.f) * orientdeg; 
        float orientcos = cosf(orientrad);
        glLeftCentCorrCm = glLeftCenterCm * orientcos;

        if (!isnan(glLeftCentCorrCm))
        {
          //gl_pSerPort->printf("just before PIDCalcs call: WallTrackSetPoint  = %2.2f\n", WallTrackSetPoint);
          WallTrackOutput = PIDCalcs(glLeftCentCorrCm, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,
            kp, ki, kd);

          //04/05/22 have to use local var here, as result could be negative
          int16_t leftSpdnum = MOTOR_SPEED_QTR + WallTrackOutput;
          int16_t rightSpdnum = MOTOR_SPEED_QTR - WallTrackOutput;

          //04/05/22 Left/rightSpdnum can be negative here - watch out!
          rightSpdnum = (rightSpdnum <= MOTOR_SPEED_HALF) ? rightSpdnum : MOTOR_SPEED_HALF; //result can still be neg
          glRightspeednum = (rightSpdnum > 0) ? rightSpdnum : 0; //result here must be positive
          leftSpdnum = (leftSpdnum <= MOTOR_SPEED_HALF) ? leftSpdnum : MOTOR_SPEED_HALF;//result can still be neg
          glLeftspeednum = (leftSpdnum > 0) ? leftSpdnum : 0; //result here must be positive

          MoveAhead(glLeftspeednum, glRightspeednum);

          gl_pSerPort->printf(WallFollowTelemStr, millis(),
            glLeftFrontCm, glLeftCenterCm, glLeftRearCm, 
            glLeftSteeringVal, orientdeg, orientcos, glLeftCentCorrCm,
            WallTrackSetPoint, lastError, WallTrackOutput,
            glLeftspeednum, glRightspeednum);
        }
        digitalToggle(RED_LASER_DIODE_PIN); //10/24/22 toggle duration measurement pulse

if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)

{

digitalToggle(RED_LASER_DIODE_PIN); //10/24/22 toggle duration measurement pulse

mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

//03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()

UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls

//from Teensy_7VL53L0X_I2C_Slave_V3.ino: LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

float orientdeg = GetWallOrientDeg(glLeftSteeringVal);

float orientrad = (PI / 180.f) * orientdeg;

float orientcos = cosf(orientrad);

glLeftCentCorrCm = glLeftCenterCm * orientcos;

if (!isnan(glLeftCentCorrCm))

{

//gl_pSerPort->printf("just before PIDCalcs call: WallTrackSetPoint = %2.2f\n", WallTrackSetPoint);

WallTrackOutput = PIDCalcs(glLeftCentCorrCm, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,

kp, ki, kd);

//04/05/22 have to use local var here, as result could be negative

int16_t leftSpdnum = MOTOR_SPEED_QTR + WallTrackOutput;

int16_t rightSpdnum = MOTOR_SPEED_QTR - WallTrackOutput;

//04/05/22 Left/rightSpdnum can be negative here - watch out!

rightSpdnum = (rightSpdnum <= MOTOR_SPEED_HALF) ? rightSpdnum : MOTOR_SPEED_HALF; //result can still be neg

glRightspeednum = (rightSpdnum > 0) ? rightSpdnum : 0; //result here must be positive

leftSpdnum = (leftSpdnum <= MOTOR_SPEED_HALF) ? leftSpdnum : MOTOR_SPEED_HALF;//result can still be neg

glLeftspeednum = (leftSpdnum > 0) ? leftSpdnum : 0; //result here must be positive

MoveAhead(glLeftspeednum, glRightspeednum);

gl_pSerPort->printf(WallFollowTelemStr, millis(),

glLeftFrontCm, glLeftCenterCm, glLeftRearCm,

glLeftSteeringVal, orientdeg, orientcos, glLeftCentCorrCm,

WallTrackSetPoint, lastError, WallTrackOutput,

glLeftspeednum, glRightspeednum);

}

digitalToggle(RED_LASER_DIODE_PIN); //10/24/22 toggle duration measurement pulse

Using my handy-dandy Hanmatek DSO, I was able to capture the pin activity, as shown in the following plot:

Wall track update cycle activity with 100mSec interval (20mSec/div)

As shown above, the update code takes a bit less than 20mSec to complete, and idles for the remaining 80mSec or so, waiting for the 100mSec time period to elapse. So, I should be able to reduce the time interval by at least a factor of two. I changed the update interval from 100mSec to 50mSec, and got the activity plot shown below:

Wall track update cycle activity with 50mSec interval (20mSec/div)

The above plot has the same 20mSec/div time scale as the previous one; as can be seen, there is still plenty of ‘idle’ time between wall tracking updates. Now to see if this actually changes the robot’s behavior.

As shown in the next plot and video, I ran another ‘sandbox’ test, this time with the update interval set to 50mSec vice 100mSec, and with an 11Cm initial offset.

PID(5,0,0), initial distance 11Cm

Then I ran it again, except this time with a PID of (10,0,0):

PID(10,0,0), initial distance 11Cm

This wasn’t at all what I expected. I thought the larger ‘P’ value would cause the robot to more closely track the desired offset, but that isn’t what happened. Everything is fine for the first two seconds (140,000 to 142,000 mSec), but then the robot starts weaving dramatically- to the point where the motor values max out at 127 on one side and 0 on the other – bummer. Looks like I need another consulting session with my PID wizard stepson!

25 October 2022 Update:

My PID wiz stepson liked my idea of breaking the wall tracking problem into an offset capture phase, followed by a wall tracking phase, but wasn’t particularly impressed with my thinking about reducing the PID update interval while simultaneously increasing the P value to 10, so, I’m back to taking more data. The following run is just the wall tracking phase, with 50mSec update interval and a P value of 3.

As can be seen, the robot doesn’t really make any corrections – just goes in a straight line more or less. However, the left/right wheel speed data does show the correct trend (left wheel drive decreasing, right wheel drive increasing), so maybe a non-zero ‘I’ value would do the trick?

Here’s a run with PID(3,0.5,0):

In the above plot the I value does indeed cause the robot to track back to the target distance, but then goes well past the target before starting to turn back. Too much I?

Here’s another run with PID(3,0.1,0) – looking pretty good!

This looks pretty good; with I = 0.1 the robot definitely adjusted back toward the target distance, but in a much smoother manner than with I = 0.5. OK, time to let the master view these results and see if I’m even in the right PID universe.

One thing to mention in these runs – they are performed in my office, and the total run length is a little over 2m (210Cm), so I’m only seeing just one correction maneuver. Maybe if I start out with a small offset from the target value? Nope – that won’t work – at least not tonight; my current code changes the setpoint from the entered value (40Cm in this case) to the actual offset value (36Cm on this run) before starting the run. Curses! Foiled again!

27 October 2022 Update:

Today I had some time to see how the PID handles wall-tracking runs starting with a small offset from the desired value. First I started with a run essentially identical to the last run from two days ago, just to make sure nothing significant had changed (shouldn’t, but who knows for sure), as shown below:

Then I tried a run with the same PID values, but with a small initial offset from the desired 40Cm:

As can be seen, the robot didn’t seem to handle this very well; there was an initial correction away from the wall (toward the desired offset), but the robot cruised well past the setpoint before correcting back toward the wall. This same behavior repeated when the robot went past the setpoint on the way back toward the wall.

To see which way I needed to move with the ‘I’ value, a made another run with the ‘I’ value changed from 0.1 to 0.25, as shown below:

Now the corrections were much more dramatic, and tracking was much less accurate. on the second correction (when the robot passed through the desired setpoint going away from the wall), the motor drive values maxed out (127 on the left, 0 on the right).

Next I tried an ‘I’ value of 0.05, as shown below:

This looks much nicer – deviations from the desired offset are much smaller, and everything is much smoother. However, I’m a little reluctant to declare victory here, as it may well be that the ‘I’ value is so small now that it may not be making any difference at all, and what I’m seeing is just the natural straight-line behavior of the robot. In addition, the robot may not be able to track the wall around some of the 45deg wall direction changes found in this house.

28 October 2022 Update:

I decided to rearrange my office ‘sandbox’ to provide additional running room for my wall-following robot. By setting up my sandbox ‘walls’ diagonally across my office, I was able to achieve a run length of almost 4 meters (3.94 to be exact). Here is a plot and short video from a run with PID(3,0.1,0):

I was very encouraged by this run. The robot tracked very well for most of the run, deviating slightly at the very end. I’m particularly pleased by the 1.4sec period from about 129400 (129.4sec) to about 130800 (130.8sec); during this period the left & right wheel motor drive values were pretty constant, indicating that the PID was actively controlling motor speeds to obtain the desired the wall offset. I’m not sure what caused the deviation at the end, but it might have something to do with the ‘wall’ material (black art board with white paper taped to the bottom half) in that section. However, after replacing that section with white foam core, the turn-out behavior persisted, so it wasn’t the wall properties causing the problem.

After looking at the data and the video for a while, I concluded that the divergence at the end of the run was real. During the first part of the run, the robot was close enough to the setpoint so that no significant correction was required. However, as soon as the natural straight-line behavior diverged enough from the set point to cause the PID to produce a non-small output, the tracking performance was seriously degraded. In other words, the PID was making things worse, not better – rats.

So, I tried another run, this time adding just a smidge of ‘D’, on the theory that this would still allow the PID to drive the robot back toward the setpoint, but not quite as wildly as before. With PID (3, 0.1, 0.1) I got the following plot:

As can be seen, things are quite a bit nicer, and the robot seemed to track fairly well for the entire 4m run.

Tried another run this morning with PID(3,0,0.1), i.e. removing the ‘I’ factor entirely, but leaving the ‘D’ parameter at 0.1 as in my last run yesterday. As can be seen in the following plot and short video, the results were very encouraging.

Made another run with ‘D’ bumped to 0.5 – looks even better.

Next, I investigated Wall-E3’s ability to handle wall angle changes. As the following plot and video shows, it actually does quite well with PID(3,0,0.5)

transients at end of run are due to encountering another angled wall – not shown in video

30 October 2022 Update

After a few more trials, I think I ended up with PID(3,0,1) as a reasonable compromise. With this setup, Wall-E3 can navigate both concave and convex wall angle changes, as shown in the following plot and short video.

As an aside, I also investigated several other PID triplets of the form (K*3,0,K*1) to see if other values of K besides 1 would produce the same behavior. At first I thought both K = 2 and K = 3 did well, but after a couple of trials I found myself back at K = 1. I’m not sure why there is anything magic about K = 1, but it’s hard to get around the fact that K = 2 and K = 3 did not work as well tracking my ‘sandbox’ walls.

At this point, I think it may be time to back-port the above results into my WallE3_AnomalyRecovery_V2.sln project, and then ultimately back into my main robot control project.

06 November 2022 Update:

Well, now I know why my past efforts at wall tracking didn’t rely exclusively on offset distance as measured by the 3 VL53L0X sensors on each side of the robot. The problem is that the reported distance is only accurate when the robot is parallel to the wall; any off-parallel orientation causes the reported distance to increase, even though the robot body is at the same distance. In the above work I thought I could beat this problem by compensating the distance measurement by the cosine of the off-parallel angle. This works (sort of) but causes the control loop to lag way behind what the robot is actually doing. Moreover, since there can be small variations in the distance reported by the VL53L0X array, the robot can be physically parallel to the wall while the sensors report an off-parallel orientation, or alternatively, the robot can be physically off-parallel (getting closer or farther away) to the wall, while the sensors report that it is parallel and consequently no correction is required. This is why, in previous versions, I tried to incorporate a absolute distance measurement along with orientation information into a single PID loop (didn’t work very well).

09 November 2022 Update:

After beating my head against the problem of tracking the nearby wall using a three-element array of VL53L0X distance sensors and a PID algorithm, I finally decided it just wasn’t wasn’t working well enough to rely on for generalized wall tracking. It does work, but has a pretty horrendous failure mode. If the robot ever gets past about 45 deg orientation w/r/t the near wall, the distance values from the VL53L0X sensor become invalid and the robot goes crazy.

So, I have been spending my night-time sleep preparation time (where I do some of my best thinking) trying to think of different ways of skinning this particular cat, as follows:

The robot needs to be able to accurately track a given offset
Must have enough agility to accommodate abrupt wall direction changes (90 deg changes are easy – but there are several 45 deg changes in our house)
Must handle obstacles appropriately.

It’s that first item on the list that I can’t seem to handle with the typical PID algorithm. So, I started to think about alternative schemes, and the one I decided to experiment with was the idea of implementing a zig-zag tracking algorithm using my already-proven SpinTurn() function. SpinTurn() uses relative heading output from my MP6050 MPU to implement CW/CCW turns, and has proven to be quite reliable (after Homer Creutz and I beat MPU6050 FIFO management into submission).

I modified one of my Wall Track Tuning programs to implement the ‘zig-zag’ algorithm, and ran some tests in my office ‘sandbox’. As the following Excel plot and short video shows, it actually did quite well, considering it was the product of a semi-dream state thought process!

As can be seen from the above, the robot did a decent job of tracking the desired 40Cm offset (average distance for the run was 39.75Cm), especially for the first iteration. I should be able to tweak the algorithm to track the wall faster and with less of a ‘drunken sailor’ behavior.

Stay tuned,

Frank

Additional Work on Wall Tracking Algorithm

Leave a reply

Posted 15 July 2022,

After getting everything working (or so I thought) in my sandbox, I started running into problems again with wall tracking. It just wasn’t very smooth at all. So, I decided to create a part-task version of my Wall-E3 code (WallE3_WallTrackTuning.ino) to just tackle PID tuning for left/right wall tracking. this version allows PID and offset values to be entered interactively to facilitate faster tuning. After a number of runs, I wound up with a PID set of (200,20,0). This produce a very nice, smooth tracking behavior.

In addition, I went back through all my code and re-educated myself on exactly how my current wall offset capture algorithm developed and whether or not it was, in fact, what I wanted. I started by diagramming all (I hope) relevant initial orientation and offset cases, as shown in the following Visio chart.

In the above figure, four basic configurations are diagrammed; The first two are for left-side tracking with the robot starting in three different configurations inside the desired 40cm tracking offset, and three more outside the tracking offset. The second two are the same as the first, but for right-side tracking.

The algorithm is based on knowing the robot’s orientation w/r/t the local wall, which is determined by the expression ‘Steer = (Front – Rear)/100’, implemented in the Teensy 3.5 MCU that manages the VL53L0X lidar array. This result is available to the main program as ‘glLeftSteeringVal’ and ‘glRightSteeringVal’. The steering values are proportional to the orientation angle in degrees, calculated as OrientDeg = steerval/0.0175.

Expressions for which way and how much to turn to achieve the desired capture approach angle of +/- 30 degrees were determined for each of the 12 starting configurations shown (numbered 1-12 in the above figure). An examination of the resulting expressions showed that they could be collapsed down into just two different calls to the ‘SpinTurn(isCCW, numdeg)’ subroutine – one for left-side tracking, and one for right-side tracking, as shown by the bold-face expressions above.

New Wall-Following Capability For Wall-E3

2 Replies

Posted 28 March, 2022

I’ve been working with Wall-E3, my new Teensy 3.5-powered autonomous wall-following robot. I’ve gotten left-wall and right-wall tracking working pretty well, but the transition from one wall to the next (typically right-angle) wall was pretty awkward. The robot basically ran right up to the next wall, stopped, backed up, and then made a right-angle turn to follow the next wall. So, I am trying to make that transition a bit smoother.

After trying a few different ideas, the one I settled on was to use my current very successful ‘SpinTurn()’ function to do the transition. I modified my ‘CheckForAnomalies()’ function to add a check for forward distance less than twice the desired offset distance. When this distance is detected, the robot stops and then makes a right-angle ‘spin’ turn (one sides wheels go forward, the other sides wheels go backward) in the direction away from the currently tracked wall, and then re-enters ‘Track’ mode causing it to track the next wall normally. Here’s a short video showing the process:

robot tracking left wall, then making a ‘spin’ turn to follow the next wall

Now that I have it working for left-wall tracking, it should be easy to port it to the right-wall tracking condition.

07 April 2022 Update:

Well, as usual, what I thought would be easy has turned out to be anything but. I was able to port the left wall tracking algorithm to the right side, but as I was testing the result, I noticed that Wall-E3 doesn’t really track the right (or left, for that matter) wall. After it ‘captures’ the desired wall offset and turns back to the parallel orientation, it basically goes straight ahead (same speed applied to both side’s motors). If the initial orientation is close to parallel, it looks like it is tracking, but it isn’t.

So, I tried a number of ideas to actually get it to track the desired offset, but they all resulted in poor-to-catastrophic tracking. After working the problem, I began to see that, as always, the issue is the errors associated with the VL53L0X sensor distance measurements. There are two distinct types of errors – an initial ‘calibration’ error associated with sensor-to-sensor variation, and the measurement error that occurs when the robot isn’t oriented parallel to the measured surface.

Calibration Errors:

Each individual VL53L0X sensor gets a slightly different value for the distance to the target, and sometimes ‘slightly’ can be pretty big – 2-3cm at 20cm, for instance. Up until now I had been ignoring these errors, but the time had come to do something about. So, as I always do when troubleshooting an issue, I started taking data. I ran a bunch of trials for all seven VL53L0X sensors at various distances. After gathering the data, I used Excel’s curve-fitting capability to fit a linear equation to the points, as shown below:

The linear-fit equations gave me a starting point, but they still had to be tweaked a bit to provide the best possible match between what the VL53L0X sensor reported and the actual measurement. Again I used Excel to tweak the equations to give the best match as shown below:

Left-side ‘tweaked’ correction expression

Right-side ‘tweaked’ correction expression

The expressions shown in red are the ones used to correct the VL53L0X-measured distances to be as close as possible to the actual distances (10cm, 20cm, 30cm).

The above corrections were coded into a set of seven ‘correction’ functions for the Teensy 3.5 program that manages the two VL53L0X arrays and the single VL53L0X rear distance sensor.

Correction functions in Teensy_7VL53L0X_I2C_Slave_V4.ino

While this did, indeed, solve a lot of problems – especially with the calculations for wall offset capture initial approach angle, it still didn’t entirely address Wall-E3’s inconsistent offset tracking performance.

Orientation Angle Induced Errors:

Wall-E3 tracks walls by offset by comparing the center VL53L0X measurement to the desired offset, and adjusting the left/right motor speeds to turn the robot in the desired direction. Unfortunately, the turn also throws off the measurements as now the sensors are pointing off-perpendicular, and return a different distance than the actual robot-to-wall perpendicular distance. I tried adjusting the PID controller algorithm to control the robot’s steering angle rather than the offset distance, and then calculating a new steering angle each time – this worked, but not very well.

So, the solution (I think) is to come up with a distance correction factor for off-perpendicular orientations. Going through the trigonometry, I came up with this expression:

corrdist=measdist*cos⁡(steeringAngle)

I programmed this into the following function:

uint16_t OrientCorr(uint16_t meas_dist, float steerval)
{
  //double corr_ang_rad = atan2(100*steerval,83.f); //83mm between front & rear sensors
  double corr_ang_rad = atan2(100*steerval,83.f); //83mm between front & rear sensors
  double corr_dist = meas_dist * cos(corr_ang_rad);

  myTeePrint.printf("OrientCorr: meas = %d, steer = %2.2f, corr_ang_rad = %2.2f, corr_dist = %2.2f\n", meas_dist, steerval, corr_ang_rad, corr_dist);

  return (uint16_t)round(corr_dist);
}

uint16_t OrientCorr(uint16_t meas_dist, float steerval)

{

//double corr_ang_rad = atan2(100*steerval,83.f); //83mm between front & rear sensors

double corr_ang_rad = atan2(100*steerval,83.f); //83mm between front & rear sensors

double corr_dist = meas_dist * cos(corr_ang_rad);

myTeePrint.printf("OrientCorr: meas = %d, steer = %2.2f, corr_ang_rad = %2.2f, corr_dist = %2.2f\n", meas_dist, steerval, corr_ang_rad, corr_dist);

return (uint16_t)round(corr_dist);

}

and then ran some tests to verify that the correction algorithm was having the desired effect. Here’s the setup:

and here are some Excel plots showing the results

Distance correction for off-perpendicular angles

As can be seen from the above plot, the corrected distance (gray curve) is pretty constant for angles of -30, 0 and +30 degrees.

09 April 2022 Update:

I have been thinking about the above orientation angle induced errors issue for a couple of days. I wasn’t really happy with that correction as shown in the above Excel plot, and it occurred to me that I didn’t really have to strictly abide by the above correction expression derived from the actual geometry. What I really wanted was a correction that would be accurate at low (or zero) offset angles, but would slightly over-correct for orientation angles in the +/- 30 deg range. In this way, when the PID engine adjusts the motor speeds to correct for an offset error, the system doesn’t try to run away. In fact, for a slight overcorrection algorithm, the center distance reported by the robot might actually go down rather than up for off-perpendicular angles. This would tend to make the PID think that it was over-correcting instead of under-correcting as it does with uncorrected distance reporting.

So, I went back to my test setup, and made some more measurements of corrected vs uncorrected center distances for -30, 0, and 30 degree orientations, for varying values of ‘tweak’ values in he correction expression, as shown in the Excel plots below:

Out of the above correction values, I like the “cos(1.1*corr_ang_rad)” configuration the best. The correction doesn’t modify the center distance at all for the parallel case, and produces a very slight over-correction at the +/- 30 degree orientation cases.

I added the ‘1.1*’ correction to the ‘OrientCorr()’ function and performed another right wall tracking test in my office ‘sandbox’ as shown in the short video below:

Right wall tracking with sensor calibration and orientation correction applied

Here is the telemetry output for this run:

In TrackRightWallOffset with Kp/Ki/Kd = 300.00	0.00	10.00

In TrackRightWallOffset with RR/RC/RF = 30	30	30
straight window = 8 cm
R/C/F dists = 30	30	30 Steerval = 0.020, WallAngle = 1.14, Inside
In SpinTurn(CCW, 28.86, 45.00)
R/C/F dists = 27	30	30 Steerval = 0.350, WallOrientDeg = 20.00

approach start: orient angle = 20.000, tgt_distCm = 40
At start - err = prev_err = -10
R/C/F dists = 27	30	30 Steerval = 0.350, OrientDeg = 20.00, err/prev_err = -10
R/C/F dists = 27	30	30 Steerval = 0.350, OrientDeg = 20.00, err/prev_err = -10
Msec	RFront	RCtr	RRear	Orient	Front	Rear	Err	P_err
68368: Error in GetFrontDistCm() - 0 replaced with 60
68384	31	30	28	17.1	118	46	-10	-10
68496	34	33	30	22.3	125	56	-7	-10
68609	36	36	32	24.0	119	60	-4	-7
68720	39	38	34	28.0	115	66	-2	-4
68831	44	41	36	44.0	108	68	1	-2
At end of offset capture - prev_res*res = -2
correct back to parallel (Right side): RF/RC/RR/Orient = 44	41	36	44.00
In SpinTurn(CW, 44.00, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	10.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
69557	728	794	816		49	47	50		95	10899	73	39363	-0.10	-0.07	8.70	66	83
69657	728	794	816		50	51	52		99	10901	80	39332	-0.18	-0.11	20.60	54	95
69756	728	794	816		50	48	50		93	10910	86	39318	0.00	-0.08	-22.50	97	52
69858	728	794	816		49	46	48		90	10917	94	39277	0.09	-0.06	-44.30	119	30
69958	728	794	816		46	44	47		86	10927	87	39265	-0.10	-0.04	15.90	59	90
70055	728	114	816		44	45	45		76	10956	102	39252	-0.11	-0.05	18.00	57	93
70157	728	108	816		43	41	43		77	10976	819	48383	-0.02	-0.01	3.50	71	78
70255	728	109	816		39	38	40		74	10994	123	48334	-0.09	0.02	32.00	43	107
70357	109	99	816		36	36	38		70	11023	750	55303	-0.19	0.04	67.80	7	127
70455	112	103	816		34	33	34		65	11056	104	55318	-0.02	0.07	28.40	46	103
70557	728	116	816		29	30	31		55	11101	819	63702	-0.15	0.10	73.40	1	127
70655	728	120	816		27	27	28		48	11170	106	63708	-0.08	0.13	63.40	11	127
70757	728	794	816		24	24	25		43	11246	110	63715	-0.11	0.15	77.50	0	127
70854	728	794	816		22	22	24		37	11329	118	55334	-0.19	0.15	101.20	0	127
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither
In SpinTurn(CCW, 90.00, 45.00)
71682: glLeftCenterCm = 794 glRightCenterCm = 25
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
71686: TrackRightWallOffset(), glRightFrontCm = 26

In TrackRightWallOffset with Kp/Ki/Kd = 300.00	0.00	10.00

In TrackRightWallOffset with RR/RC/RF = 26	25	26
straight window = 8 cm
R/C/F dists = 26	25	26 Steerval = -0.060, WallAngle = -3.43, Inside
In SpinTurn(CCW, 33.43, 45.00)
R/C/F dists = 26	30	32 Steerval = 0.610, WallOrientDeg = 34.86

approach start: orient angle = 34.857, tgt_distCm = 40
At start - err = prev_err = -8
R/C/F dists = 26	30	32 Steerval = 0.610, OrientDeg = 34.86, err/prev_err = -8
R/C/F dists = 27	33	33 Steerval = 0.580, OrientDeg = 33.14, err/prev_err = -8
Msec	RFront	RCtr	RRear	Orient	Front	Rear	Err	P_err
72138	33	33	27	33.1	219	25	-7	-8
72254	40	38	31	40.0	195	29	-3	-7
72365	44	43	34	43.4	187	32	3	-3
At end of offset capture - prev_res*res = -9
correct back to parallel (Right side): RF/RC/RR/Orient = 44	43	34	43.43
In SpinTurn(CW, 43.43, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	10.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
73091	110	112	122		40	38	39		151	11228	29	55621	0.09	0.02	-20.30	95	54
73189	728	110	816		39	40	41		153	11220	35	55688	-0.14	0.00	39.90	35	114
73291	728	112	816		40	38	39		150	11210	37	55762	0.18	0.02	-45.00	120	29
73394	728	111	816		40	38	39		146	11195	36	55854	0.06	0.02	-13.20	88	61
73492	114	115	120		38	37	38		141	11171	41	55917	0.07	0.03	-12.00	87	63
73589	728	113	816		38	38	38		134	11147	50	55921	-0.03	0.02	14.10	60	89
73692	728	116	816		36	37	36		129	11102	56	55913	0.05	0.03	-5.30	80	69
73789	112	115	816		36	35	36		124	11068	55	55971	0.00	0.05	14.30	60	89
73890	728	117	816		35	33	34		118	11028	64	56027	0.14	0.07	-19.80	94	55
73991	728	120	816		34	33	34		112	10988	69	56080	0.00	0.07	19.60	55	94
74090	728	120	816		34	33	32		109	10942	67	56165	0.11	0.07	-10.90	85	64
74189	728	122	816		33	32	32		103	10880	72	56246	0.06	0.08	5.40	69	80
74290	728	122	816		32	31	32		100	10825	85	56296	0.03	0.09	17.60	57	92
74393	728	794	816		32	30	31		92	10778	91	56321	0.10	0.10	0.60	74	75
74489	728	120	816		31	30	31		85	10736	86	56382	-0.01	0.10	31.90	43	106
74589	728	794	816		30	30	30		81	10687	92	56196	-0.01	0.10	33.00	42	108
74689	728	119	816		30	29	30		78	10611	104	56033	-0.01	0.11	35.90	39	110
74793	728	123	816		30	28	29		74	10519	107	55890	0.11	0.12	4.10	70	79
74890	728	122	816		30	29	29		71	10414	107	55749	0.12	0.11	-2.80	77	72
74990	728	794	816		31	29	29		67	10296	119	55629	0.20	0.11	-26.20	101	48
75089	728	115	816		31	30	29		63	10341	122	55523	0.15	0.10	-15.40	90	59
75189	728	119	816		31	31	30		59	10388	111	55452	0.09	0.09	-0.50	75	74
75290	728	115	816		32	31	30		52	10501	750	62042	0.20	0.09	-31.90	106	43
75392	728	119	816		33	31	30		47	10633	819	69966	0.30	0.09	-62.00	127	12
75490	728	123	816		32	32	31		40	10775	819	77492	0.12	0.08	-13.70	88	61
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither
In SpinTurn(CCW, 90.00, 45.00)
76435: glLeftCenterCm = 794 glRightCenterCm = 26
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
76440: TrackRightWallOffset(), glRightFrontCm = 27

In TrackRightWallOffset with Kp/Ki/Kd = 300.00	0.00	10.00

In TrackRightWallOffset with RR/RC/RF = 27	25	26
straight window = 8 cm
R/C/F dists = 27	25	26 Steerval = 0.010, WallAngle = 0.57, Inside
In SpinTurn(CCW, 29.43, 45.00)
R/C/F dists = 26	28	31 Steerval = 0.440, WallOrientDeg = 25.14

approach start: orient angle = 25.143, tgt_distCm = 40
At start - err = prev_err = -9
R/C/F dists = 26	28	31 Steerval = 0.440, OrientDeg = 25.14, err/prev_err = -9
R/C/F dists = 26	28	31 Steerval = 0.440, OrientDeg = 25.14, err/prev_err = -9
Msec	RFront	RCtr	RRear	Orient	Front	Rear	Err	P_err
76821: Error in GetFrontDistCm() - 0 replaced with 71
76832	32	31	26	33.7	159	40	-9	-9
76944	37	35	30	38.3	171	42	-5	-9
77059	41	39	33	42.3	167	46	-1	-5
77171	43	42	36	42.9	165	50	2	-1
At end of offset capture - prev_res*res = -2
correct back to parallel (Right side): RF/RC/RR/Orient = 43	42	36	42.86
In SpinTurn(CW, 42.86, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	10.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
77927	728	794	816		42	41	41		116	10841	49	56047	0.08	-0.01	-26.10	101	48
78023	728	794	816		43	41	42		116	10846	46	56107	0.10	-0.01	-32.80	107	42
78126	728	794	816		42	41	45		119	10845	53	56159	-0.26	-0.01	71.40	3	127
78224	728	794	816		42	41	44		117	10841	58	56198	-0.21	-0.01	60.50	14	127
78326	728	794	816		39	39	38		109	10841	54	56270	0.11	0.01	-27.00	102	48
78427	728	794	816		38	36	37		101	10844	56	56357	0.08	0.04	-12.60	87	62
78525	728	794	816		37	35	36		96	10850	65	56427	0.12	0.05	-20.70	95	54
78623	728	794	816		36	36	34		91	10852	72	56493	0.21	0.04	-50.00	125	25
78726	728	794	816		35	35	35		87	10848	71	56543	0.01	0.05	9.90	65	84
78828	728	794	816		35	34	36		84	10841	78	56607	-0.13	0.06	55.50	19	127
78927	728	794	816		34	33	34		80	10836	90	47712	-0.07	0.07	42.50	32	117
79025	728	794	816		33	31	33		74	10836	94	47751	0.04	0.09	15.90	59	90
79124	728	794	816		32	31	31		71	10836	85	40235	0.04	0.09	15.00	60	90
79223	728	794	816		32	31	32		66	10839	103	40236	-0.03	0.09	35.30	39	110
79324	728	794	816		30	29	29		61	10845	110	30500	0.11	0.11	1.20	73	76
79424	728	794	816		30	30	29		56	10855	102	30502	0.18	0.10	-23.20	98	51
79523	728	794	816		31	30	29		50	10862	108	30502	0.27	0.10	-50.10	125	24
79625	728	794	816		31	30	29		43	10877	126	30505	0.21	0.10	-33.60	108	41
79725	728	794	816		31	30	29		38	10884	126	30344	0.18	0.10	-24.30	99	50
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither
In SpinTurn(CCW, 90.00, 45.00)
80675: glLeftCenterCm = 794 glRightCenterCm = 24
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
80680: TrackRightWallOffset(), glRightFrontCm = 25

In TrackRightWallOffset with Kp/Ki/Kd = 300.00	0.00	10.00

In TrackRightWallOffset with RR/RC/RF = 25	24	25
straight window = 8 cm
R/C/F dists = 25	24	25 Steerval = 0.050, WallAngle = 2.86, Inside
In SpinTurn(CCW, 27.14, 45.00)
R/C/F dists = 25	26	28 Steerval = 0.300, WallOrientDeg = 17.14

approach start: orient angle = 17.143, tgt_distCm = 40
At start - err = prev_err = -12
R/C/F dists = 25	26	28 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -12
R/C/F dists = 25	26	28 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -12
Msec	RFront	RCtr	RRear	Orient	Front	Rear	Err	P_err
81023: Error in GetFrontDistCm() - 0 replaced with 85
81031: Error in GetFrontDistCm() - 0 replaced with 85
81035	28	27	24	24.0	85	31	-13	-12
81151	32	32	27	31.4	205	40	-8	-13
81265	35	33	29	34.3	201	45	-7	-8
81378	38	36	31	38.3	196	49	-4	-7
81496	42	38	35	32.0	191	50	-2	-4
81611	45	44	38	17.7	189	55	2	-2
At end of offset capture - prev_res*res = -4
correct back to parallel (Right side): RF/RC/RR/Orient = 45	44	38	17.71
In SpinTurn(CW, 17.71, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	10.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
82121	113	117	126		44	46	42		134	11189	51	30097	0.19	-0.06	-72.50	127	2
82221	109	111	127		46	45	43		129	11171	57	30079	0.25	-0.05	-89.50	127	0
82321	106	110	119		45	44	43		127	11143	58	30075	0.20	-0.04	-72.60	127	2
82419	108	111	115		46	46	47		127	11105	55	30075	-0.07	-0.06	0.50	74	75
82522	728	114	120		50	49	49		128	11055	61	30081	0.02	-0.09	-31.80	106	43
82621	112	116	816		50	47	50		109	11015	71	30077	-0.03	-0.07	-12.70	87	62
82721	728	794	816		48	46	52		104	10975	69	30073	-0.32	-0.06	75.00	0	127
82820	728	794	126		44	44	46		101	10930	73	30072	-0.18	-0.04	43.20	31	118
82919	728	794	816		43	42	43		110	10854	83	30074	0.05	-0.02	-18.90	93	56
83024	728	121	816		38	37	41		264	11010	82	30086	-0.29	0.03	92.10	0	127
83124	728	794	816		35	35	37		263	11116	83	30090	-0.18	0.05	69.90	5	127
83222	728	794	816		32	32	32		163	10944	85	30098	0.01	0.08	22.60	52	97
83319	728	794	816		30	30	32		87	10840	97	30103	-0.14	0.10	70.30	4	127
83421	728	794	128		30	29	30		82	10725	101	30106	-0.01	0.11	37.20	37	112
83520	728	124	816		29	27	27		81	10581	99	30110	0.18	0.13	-13.30	88	61
83621	728	122	816		28	27	28		77	10425	102	30115	-0.04	0.13	48.80	26	123
83720	728	123	816		28	27	26		73	10555	117	30114	0.17	0.13	-9.90	84	65
83819	728	117	816		28	28	28		68	10656	108	30115	0.05	0.12	19.90	55	94
83919	728	120	816		30	29	28		66	10746	750	30115	0.29	0.11	-51.50	126	23
84018	114	123	816		31	29	27		63	10839	819	30115	0.36	0.11	-74.30	127	0
84119	112	120	816		31	30	28		57	10950	819	30115	0.29	0.10	-57.60	127	17
84217	112	118	126		31	30	30		51	11065	750	37818	0.10	0.10	-1.90	76	73
84317	728	794	816		31	30	31		45	11196	750	45125	0.00	0.10	29.00	46	104
84418	728	122	127		31	29	29		39	11339	819	53775	0.18	0.11	-19.30	94	55

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

In TrackRightWallOffset with Kp/Ki/Kd = 300.00 0.00 10.00

In TrackRightWallOffset with RR/RC/RF = 30 30 30

straight window = 8 cm

R/C/F dists = 30 30 30 Steerval = 0.020, WallAngle = 1.14, Inside

In SpinTurn(CCW, 28.86, 45.00)

R/C/F dists = 27 30 30 Steerval = 0.350, WallOrientDeg = 20.00

approach start: orient angle = 20.000, tgt_distCm = 40

At start - err = prev_err = -10

R/C/F dists = 27 30 30 Steerval = 0.350, OrientDeg = 20.00, err/prev_err = -10

Msec RFront RCtr RRear Orient Front Rear Err P_err

68368: Error in GetFrontDistCm() - 0 replaced with 60

68384 31 30 28 17.1 118 46 -10 -10

68496 34 33 30 22.3 125 56 -7 -10

68609 36 36 32 24.0 119 60 -4 -7

68720 39 38 34 28.0 115 66 -2 -4

68831 44 41 36 44.0 108 68 1 -2

At end of offset capture - prev_res*res = -2

correct back to parallel (Right side): RF/RC/RR/Orient = 44 41 36 44.00

In SpinTurn(CW, 44.00, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00 0.00 10.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

69557 728 794 816 49 47 50 95 10899 73 39363 -0.10 -0.07 8.70 66 83

69657 728 794 816 50 51 52 99 10901 80 39332 -0.18 -0.11 20.60 54 95

69756 728 794 816 50 48 50 93 10910 86 39318 0.00 -0.08 -22.50 97 52

69858 728 794 816 49 46 48 90 10917 94 39277 0.09 -0.06 -44.30 119 30

69958 728 794 816 46 44 47 86 10927 87 39265 -0.10 -0.04 15.90 59 90

70055 728 114 816 44 45 45 76 10956 102 39252 -0.11 -0.05 18.00 57 93

70157 728 108 816 43 41 43 77 10976 819 48383 -0.02 -0.01 3.50 71 78

70255 728 109 816 39 38 40 74 10994 123 48334 -0.09 0.02 32.00 43 107

70357 109 99 816 36 36 38 70 11023 750 55303 -0.19 0.04 67.80 7 127

70455 112 103 816 34 33 34 65 11056 104 55318 -0.02 0.07 28.40 46 103

70557 728 116 816 29 30 31 55 11101 819 63702 -0.15 0.10 73.40 1 127

70655 728 120 816 27 27 28 48 11170 106 63708 -0.08 0.13 63.40 11 127

70757 728 794 816 24 24 25 43 11246 110 63715 -0.11 0.15 77.50 0 127

70854 728 794 816 22 22 24 37 11329 118 55334 -0.19 0.15 101.20 0 127

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither

In SpinTurn(CCW, 90.00, 45.00)

71682: glLeftCenterCm = 794 glRightCenterCm = 25

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

71686: TrackRightWallOffset(), glRightFrontCm = 26

In TrackRightWallOffset with Kp/Ki/Kd = 300.00 0.00 10.00

In TrackRightWallOffset with RR/RC/RF = 26 25 26

straight window = 8 cm

R/C/F dists = 26 25 26 Steerval = -0.060, WallAngle = -3.43, Inside

In SpinTurn(CCW, 33.43, 45.00)

R/C/F dists = 26 30 32 Steerval = 0.610, WallOrientDeg = 34.86

approach start: orient angle = 34.857, tgt_distCm = 40

At start - err = prev_err = -8

R/C/F dists = 26 30 32 Steerval = 0.610, OrientDeg = 34.86, err/prev_err = -8

R/C/F dists = 27 33 33 Steerval = 0.580, OrientDeg = 33.14, err/prev_err = -8

Msec RFront RCtr RRear Orient Front Rear Err P_err

72138 33 33 27 33.1 219 25 -7 -8

72254 40 38 31 40.0 195 29 -3 -7

72365 44 43 34 43.4 187 32 3 -3

At end of offset capture - prev_res*res = -9

correct back to parallel (Right side): RF/RC/RR/Orient = 44 43 34 43.43

In SpinTurn(CW, 43.43, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00 0.00 10.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

73091 110 112 122 40 38 39 151 11228 29 55621 0.09 0.02 -20.30 95 54

73189 728 110 816 39 40 41 153 11220 35 55688 -0.14 0.00 39.90 35 114

73291 728 112 816 40 38 39 150 11210 37 55762 0.18 0.02 -45.00 120 29

73394 728 111 816 40 38 39 146 11195 36 55854 0.06 0.02 -13.20 88 61

73492 114 115 120 38 37 38 141 11171 41 55917 0.07 0.03 -12.00 87 63

73589 728 113 816 38 38 38 134 11147 50 55921 -0.03 0.02 14.10 60 89

73692 728 116 816 36 37 36 129 11102 56 55913 0.05 0.03 -5.30 80 69

73789 112 115 816 36 35 36 124 11068 55 55971 0.00 0.05 14.30 60 89

73890 728 117 816 35 33 34 118 11028 64 56027 0.14 0.07 -19.80 94 55

73991 728 120 816 34 33 34 112 10988 69 56080 0.00 0.07 19.60 55 94

74090 728 120 816 34 33 32 109 10942 67 56165 0.11 0.07 -10.90 85 64

74189 728 122 816 33 32 32 103 10880 72 56246 0.06 0.08 5.40 69 80

74290 728 122 816 32 31 32 100 10825 85 56296 0.03 0.09 17.60 57 92

74393 728 794 816 32 30 31 92 10778 91 56321 0.10 0.10 0.60 74 75

74489 728 120 816 31 30 31 85 10736 86 56382 -0.01 0.10 31.90 43 106

74589 728 794 816 30 30 30 81 10687 92 56196 -0.01 0.10 33.00 42 108

74689 728 119 816 30 29 30 78 10611 104 56033 -0.01 0.11 35.90 39 110

74793 728 123 816 30 28 29 74 10519 107 55890 0.11 0.12 4.10 70 79

74890 728 122 816 30 29 29 71 10414 107 55749 0.12 0.11 -2.80 77 72

74990 728 794 816 31 29 29 67 10296 119 55629 0.20 0.11 -26.20 101 48

75089 728 115 816 31 30 29 63 10341 122 55523 0.15 0.10 -15.40 90 59

75189 728 119 816 31 31 30 59 10388 111 55452 0.09 0.09 -0.50 75 74

75290 728 115 816 32 31 30 52 10501 750 62042 0.20 0.09 -31.90 106 43

75392 728 119 816 33 31 30 47 10633 819 69966 0.30 0.09 -62.00 127 12

75490 728 123 816 32 32 31 40 10775 819 77492 0.12 0.08 -13.70 88 61

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither

In SpinTurn(CCW, 90.00, 45.00)

76435: glLeftCenterCm = 794 glRightCenterCm = 26

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

76440: TrackRightWallOffset(), glRightFrontCm = 27

In TrackRightWallOffset with Kp/Ki/Kd = 300.00 0.00 10.00

In TrackRightWallOffset with RR/RC/RF = 27 25 26

straight window = 8 cm

R/C/F dists = 27 25 26 Steerval = 0.010, WallAngle = 0.57, Inside

In SpinTurn(CCW, 29.43, 45.00)

R/C/F dists = 26 28 31 Steerval = 0.440, WallOrientDeg = 25.14

approach start: orient angle = 25.143, tgt_distCm = 40

At start - err = prev_err = -9

R/C/F dists = 26 28 31 Steerval = 0.440, OrientDeg = 25.14, err/prev_err = -9

Msec RFront RCtr RRear Orient Front Rear Err P_err

76821: Error in GetFrontDistCm() - 0 replaced with 71

76832 32 31 26 33.7 159 40 -9 -9

76944 37 35 30 38.3 171 42 -5 -9

77059 41 39 33 42.3 167 46 -1 -5

77171 43 42 36 42.9 165 50 2 -1

At end of offset capture - prev_res*res = -2

correct back to parallel (Right side): RF/RC/RR/Orient = 43 42 36 42.86

In SpinTurn(CW, 42.86, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00 0.00 10.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

77927 728 794 816 42 41 41 116 10841 49 56047 0.08 -0.01 -26.10 101 48

78023 728 794 816 43 41 42 116 10846 46 56107 0.10 -0.01 -32.80 107 42

78126 728 794 816 42 41 45 119 10845 53 56159 -0.26 -0.01 71.40 3 127

78224 728 794 816 42 41 44 117 10841 58 56198 -0.21 -0.01 60.50 14 127

78326 728 794 816 39 39 38 109 10841 54 56270 0.11 0.01 -27.00 102 48

78427 728 794 816 38 36 37 101 10844 56 56357 0.08 0.04 -12.60 87 62

78525 728 794 816 37 35 36 96 10850 65 56427 0.12 0.05 -20.70 95 54

78623 728 794 816 36 36 34 91 10852 72 56493 0.21 0.04 -50.00 125 25

78726 728 794 816 35 35 35 87 10848 71 56543 0.01 0.05 9.90 65 84

78828 728 794 816 35 34 36 84 10841 78 56607 -0.13 0.06 55.50 19 127

78927 728 794 816 34 33 34 80 10836 90 47712 -0.07 0.07 42.50 32 117

79025 728 794 816 33 31 33 74 10836 94 47751 0.04 0.09 15.90 59 90

79124 728 794 816 32 31 31 71 10836 85 40235 0.04 0.09 15.00 60 90

79223 728 794 816 32 31 32 66 10839 103 40236 -0.03 0.09 35.30 39 110

79324 728 794 816 30 29 29 61 10845 110 30500 0.11 0.11 1.20 73 76

79424 728 794 816 30 30 29 56 10855 102 30502 0.18 0.10 -23.20 98 51

79523 728 794 816 31 30 29 50 10862 108 30502 0.27 0.10 -50.10 125 24

79625 728 794 816 31 30 29 43 10877 126 30505 0.21 0.10 -33.60 108 41

79725 728 794 816 31 30 29 38 10884 126 30344 0.18 0.10 -24.30 99 50

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither

In SpinTurn(CCW, 90.00, 45.00)

80675: glLeftCenterCm = 794 glRightCenterCm = 24

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

80680: TrackRightWallOffset(), glRightFrontCm = 25

In TrackRightWallOffset with Kp/Ki/Kd = 300.00 0.00 10.00

In TrackRightWallOffset with RR/RC/RF = 25 24 25

straight window = 8 cm

R/C/F dists = 25 24 25 Steerval = 0.050, WallAngle = 2.86, Inside

In SpinTurn(CCW, 27.14, 45.00)

R/C/F dists = 25 26 28 Steerval = 0.300, WallOrientDeg = 17.14

approach start: orient angle = 17.143, tgt_distCm = 40

At start - err = prev_err = -12

R/C/F dists = 25 26 28 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -12

Msec RFront RCtr RRear Orient Front Rear Err P_err

81023: Error in GetFrontDistCm() - 0 replaced with 85

81031: Error in GetFrontDistCm() - 0 replaced with 85

81035 28 27 24 24.0 85 31 -13 -12

81151 32 32 27 31.4 205 40 -8 -13

81265 35 33 29 34.3 201 45 -7 -8

81378 38 36 31 38.3 196 49 -4 -7

81496 42 38 35 32.0 191 50 -2 -4

81611 45 44 38 17.7 189 55 2 -2

At end of offset capture - prev_res*res = -4

correct back to parallel (Right side): RF/RC/RR/Orient = 45 44 38 17.71

In SpinTurn(CW, 17.71, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00 0.00 10.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

82121 113 117 126 44 46 42 134 11189 51 30097 0.19 -0.06 -72.50 127 2

82221 109 111 127 46 45 43 129 11171 57 30079 0.25 -0.05 -89.50 127 0

82321 106 110 119 45 44 43 127 11143 58 30075 0.20 -0.04 -72.60 127 2

82419 108 111 115 46 46 47 127 11105 55 30075 -0.07 -0.06 0.50 74 75

82522 728 114 120 50 49 49 128 11055 61 30081 0.02 -0.09 -31.80 106 43

82621 112 116 816 50 47 50 109 11015 71 30077 -0.03 -0.07 -12.70 87 62

82721 728 794 816 48 46 52 104 10975 69 30073 -0.32 -0.06 75.00 0 127

82820 728 794 126 44 44 46 101 10930 73 30072 -0.18 -0.04 43.20 31 118

82919 728 794 816 43 42 43 110 10854 83 30074 0.05 -0.02 -18.90 93 56

83024 728 121 816 38 37 41 264 11010 82 30086 -0.29 0.03 92.10 0 127

83124 728 794 816 35 35 37 263 11116 83 30090 -0.18 0.05 69.90 5 127

83222 728 794 816 32 32 32 163 10944 85 30098 0.01 0.08 22.60 52 97

83319 728 794 816 30 30 32 87 10840 97 30103 -0.14 0.10 70.30 4 127

83421 728 794 128 30 29 30 82 10725 101 30106 -0.01 0.11 37.20 37 112

83520 728 124 816 29 27 27 81 10581 99 30110 0.18 0.13 -13.30 88 61

83621 728 122 816 28 27 28 77 10425 102 30115 -0.04 0.13 48.80 26 123

83720 728 123 816 28 27 26 73 10555 117 30114 0.17 0.13 -9.90 84 65

83819 728 117 816 28 28 28 68 10656 108 30115 0.05 0.12 19.90 55 94

83919 728 120 816 30 29 28 66 10746 750 30115 0.29 0.11 -51.50 126 23

84018 114 123 816 31 29 27 63 10839 819 30115 0.36 0.11 -74.30 127 0

84119 112 120 816 31 30 28 57 10950 819 30115 0.29 0.10 -57.60 127 17

84217 112 118 126 31 30 30 51 11065 750 37818 0.10 0.10 -1.90 76 73

84317 728 794 816 31 30 31 45 11196 750 45125 0.00 0.10 29.00 46 104

84418 728 122 127 31 29 29 39 11339 819 53775 0.18 0.11 -19.30 94 55

Looking at the video and the telemetry, the first leg starts with the normal offset capture maneuver, which ends with the robot about 44cm from the wall. Then it makes a pretty distinct correction toward the wall, overshoots the desired offset, and winds up the leg at about 22cm from the wall.

The second leg again starts with a capture maneuver to about 43cm. Then it stabilizes at about 32cm from the wall – nice.

The third leg maneuvers to about 43cm, and then again stabilizes at about 30cm.

The fourth leg was a bit anomalous, as it appeared to way overcorrect after capturing the desired 40cm offset, but I couldn’t find anything in the telemetry to explain it. It’s a mystery!

It’s clear from the above that I no longer need to correct for orientation angle induced errors during the offset maneuver, as these are now handled by my recent ‘global’ correction code. This will probably help with subsequent offset tracking, as the initial offset should be closer to the offset target at the start of the tracking phase. We’ll see…

After a number of trial runs, I finally settled (as much as anything is ‘settled’ in the Wall-E world) on PID = (400,5,40). Here’s a short video showing performance in this configuration:

And, once again, I still have to port this configuration and code back to the left-side wall tracking configuration. Here’s a short video of left-side wall tracking. Interestingly, my ‘random walk’ PID tuning technique resulted in significantly different PID values (300,0,200) vs (400,5,40) than the right side. No clue why.

At this point, I believe I have gone about as far as I can at the moment for wall tracking. WallE3 now can consistently track the walls in my office ‘sandbox’ using either the left-side or the right-side wall for reference. My plan going forward is to ‘archive’ this version (WallE3_WallTrack_V5) by copying it to a new project. The new project will have the goal of integrating charging station homing/connection into the system.

In preparation, I recently modified the charging station lead-in structure to accommodate the wider wheelbase on WallE3, as shown in the following photo:

17 April 2022 Update:

Well, I spoke too soon when I said above that wall-tracking was “settled”. I ran into a couple of significant problems; first, when the robot is already close to the proper offset, it is supposed to just turn to parallel the wall and then go into tracking mode, but on a number of occasions Wall-E3 ran out of control into the next wall. Secondly, wall tracking was anything but smooth, and I couldn’t get it to reliably track the desired offset. So, back to the drawing board (again).

The ‘close enough’ failures were being caused by a flaw in the ‘RotateToParallelOrientation() routine; as the robot approached the parallel orientation, the PID controller also started slowing the rotation speed, to the point where the robot wasn’t rotating anymore – just going straight ahead. If the actual parallel orientation wasn’t reached, the robot just kept going straight ahead forever – oops! The fix for this was to abandon the RotateToParallelOrientation() subroutine entirely, and just use WallOrientDeg() to get the current angular offset from parallel, and SpinTurn() to turn that angular amount back to parallel. RotateToParallelOrientation() is only used in two places (TrackRight/LeftWallOffset()), so the entire function can be removed as well.

The issue with offset tracking continues to bedevil me. When the robot is turned to approach the offset, the measured distances go the wrong way, so the PID tends to ‘wind up’ and drive the robot toward or away from the wall, rather than smoothly approaching the offset. I thought I had the answer to this by ‘tweaking’ the distance corrections due to off-parallel angles, but sadly, this did not help.

So, I removed the off-angle distance correction and went back to just tracking the steering angle – a value proportional to the difference between the front and rear side distance measurements. Now tracking was much more stable, but the robot traveled in a straight line slightly toward the wall. After a few trials, I realized that the robot was doing exactly what I told it to do – drive the front/back measurement error to zero, but unfortunately ‘zero’ did not equate to ‘parallel’. After scratching my head for a while, I realized that rather than using zero as the setpoint, I should use the value that causes the robot to travel parallel to the wall – which turned out to be about 0.25. Using this value I could increase the Kp value back up to 400 or so, and this resulted in very good tracking of whatever offset resulted from the ‘offset approach’ phase of the tracking algorithm. Just this step was a huge improvement in tracking performance, but it wasn’t quite ‘offset tracking’ yet as it didn’t pay any attention to the actual offset – just the difference between the front and back wall offset measurements.

Once I had this working, I was able to re-incorporate my earlier idea of biasing the actual steering value with a term that is proportional to the actual offset, i.e.

WallTrackSteerVal = glRightSteeringVal + (float)(glRightCenterCm – offsetCm) / 50.f;

This, coupled with the empirically determined steering value setpoint of 0.25 resulted in a very stable, very precise tracking performance, as shown in the short video below and the associated telemetry and Excel plots.

PID (400,0,0), SetPoint = +0.25

approach start: orient angle = 16.571, tgt_distCm = 35
At start: meas_dist = 15, corr_dist = 14, err = prev_err = -21
R/C/F dists = 13	15	15 Steerval = 0.290, OrientDeg = 16.57, err/prev_err = -21
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
11024	16	15	12	16.6	0.3	14	-21	-21
11135	18	17	14	16.6	0.3	16	-19	-21
11249	20	18	16	16.6	0.3	17	-18	-19
11365	22	22	18	16.6	0.3	21	-14	-18
11480	26	24	21	16.6	0.3	23	-12	-14
11591	29	27	23	16.6	0.3	25	-10	-12
11702	31	30	26	16.6	0.3	28	-7	-10
11814	34	32	29	16.6	0.3	30	-5	-7
11927	36	34	31	16.6	0.3	32	-3	-5
12037	41	38	35	16.6	0.3	36	1	-3
At end of offset capture - prev_res*res = -3
correct back to parallel (Right side): RF/RC/RR/Orient = 41	38	35	16.57
In SpinTurn(CW, 16.57, 30.00)

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
12519	728	794	816		41	39	39		90	11753	60	11683	0.21	0.25	16	59	91
12619	728	794	816		41	39	38		90	11797	70	11628	0.31	0.25	-24	99	51
12719	728	794	816		41	40	38		87	11752	69	11576	0.39	0.25	-56	127	19
12817	728	794	816		41	40	38		87	11588	63	11529	0.37	0.25	-48	123	27
12917	728	113	816		42	40	38		80	11311	70	11482	0.40	0.25	-60	127	14
13016	728	794	816		42	40	40		75	10891	78	11436	0.20	0.25	20	55	95
13118	728	794	816		42	41	40		71	10302	84	11392	0.23	0.25	 8	67	83
13220	728	794	816		43	40	39		66	9523	79	11350	0.36	0.25	-44	119	30
13321	728	794	816		42	40	39		62	9693	84	11309	0.22	0.25	12	63	87
13416	728	794	816		42	40	39		59	9870	93	11273	0.27	0.25	-8	83	67
13519	728	794	816		40	39	40		57	10068	93	11237	-0.02	0.25	108	0	127
13618	728	794	816		40	39	38		54	10261	91	11202	0.14	0.25	44	31	119
13716	728	794	816		39	38	38		51	10455	107	11177	0.09	0.25	64	11	127
13817	728	794	816		40	37	37		45	10663	112	11156	0.20	0.25	20	54	95
13918	728	794	816		39	38	36		41	10879	109	11132	0.29	0.25	-16	91	58
14017	728	794	816		40	39	36		35	11109	115	11114	0.38	0.25	-52	127	23
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Right
In SpinTurn(CCW, 90.00, 45.00)
IRHomingValTotalAvg = 199 
15351: glLeftCenterCm = 794 glRightCenterCm = 24
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
15355: TrackRightWallOffset(), glRightFrontCm = 26

In TrackRightWallOffset with Kp/Ki/Kd = 400.00	0.00	0.00

In TrackRightWallOffset with RR/RC/RF = 24	24	26
straight window = 8 cm
R/C/F dists = 24	24	26 Steerval = 0.170, WallAngle = 9.71, Inside
In SpinTurn(CCW, 20.29, 45.00)
R/C/F dists = 26	31	32 Steerval = 0.590, WallOrientDeg = 33.71

approach start: orient angle = 33.714, tgt_distCm = 35
At start: meas_dist = 31, corr_dist = 26, err = prev_err = -9
R/C/F dists = 26	31	32 Steerval = 0.590, OrientDeg = 33.71, err/prev_err = -9
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
15712	33	31	27	33.7	0.6	25	-10	-9
15826	37	34	29	33.7	0.6	28	-7	-10
15940	38	37	32	33.7	0.6	30	-5	-7
16053	42	41	36	33.7	0.6	34	-1	-5
16165	46	44	39	33.7	0.6	36	1	-1
At end of offset capture - prev_res*res = -1
correct back to parallel (Right side): RF/RC/RR/Orient = 46	44	39	33.71
In SpinTurn(CW, 33.71, 30.00)

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
16862	112	110	125		43	43	42		108	11196	64	11178	0.25	0.25	 0	75	75
16964	728	110	816		45	43	42		108	11162	65	11179	0.27	0.25	-8	82	67
17060	728	111	127		44	42	40		104	11135	61	11186	0.39	0.25	-56	127	19
17161	110	114	124		42	42	41		102	11105	70	11186	0.19	0.25	24	51	99
17263	728	114	126		44	42	41		100	11077	75	11184	0.30	0.25	-20	94	55
17362	728	115	816		43	42	41		97	11038	71	11186	0.23	0.25	 8	67	83
17461	728	116	126		43	41	41		95	10989	77	11185	0.24	0.25	 4	71	79
17562	728	113	816		44	41	41		90	10938	86	11183	0.36	0.25	-44	119	30
17664	728	109	124		43	41	41		84	10886	91	11180	0.24	0.25	 4	71	79
17763	728	118	816		41	41	41		82	10825	90	11179	0.07	0.25	72	3	127
17860	728	794	127		42	39	39		79	10762	90	11178	0.29	0.25	-16	91	59
17961	728	115	816		41	40	39		73	10708	106	11182	0.23	0.25	 8	67	83
18059	728	120	816		42	39	39		69	10656	106	11186	0.21	0.25	16	59	91
18161	728	116	816		41	39	39		64	10608	105	11189	0.15	0.25	40	35	115
18262	728	121	816		40	39	36		60	10550	113	11198	0.35	0.25	-40	115	35
18360	728	121	816		40	38	38		58	10482	124	11217	0.17	0.25	32	42	107
18459	728	121	816		40	38	37		53	10405	117	11228	0.22	0.25	12	63	87
18561	728	121	816		40	38	39		49	10600	750	19677	0.07	0.25	72	3	127
18659	728	794	816		40	39	37		43	10806	819	29443	0.24	0.25	 4	71	79
18762	728	120	816		40	39	36		36	11021	819	38752	0.37	0.25	-48	122	27
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Right
In SpinTurn(CCW, 90.00, 45.00)
IRHomingValTotalAvg = 273 
19919: glLeftCenterCm = 794 glRightCenterCm = 25
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
19923: TrackRightWallOffset(), glRightFrontCm = 28

In TrackRightWallOffset with Kp/Ki/Kd = 400.00	0.00	0.00

In TrackRightWallOffset with RR/RC/RF = 26	25	28
straight window = 8 cm
R/C/F dists = 26	25	28 Steerval = 0.180, WallAngle = 10.29, Inside
In SpinTurn(CCW, 19.71, 45.00)
R/C/F dists = 25	28	30 Steerval = 0.490, WallOrientDeg = 28.00

approach start: orient angle = 28.000, tgt_distCm = 35
At start: meas_dist = 28, corr_dist = 25, err = prev_err = -10
R/C/F dists = 25	28	30 Steerval = 0.520, OrientDeg = 29.71, err/prev_err = -10
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
20308	30	28	25	29.7	0.5	24	-11	-10
20420	32	30	26	29.7	0.5	26	-9	-11
20533	33	32	29	29.7	0.5	26	-9	-9
20647	36	33	30	29.7	0.5	28	-7	-9
20761	38	36	32	29.7	0.5	31	-4	-7
20876	41	38	36	29.7	0.5	33	-2	-4
20988	42	42	38	29.7	0.5	36	1	-2
At end of offset capture - prev_res*res = -2
correct back to parallel (Right side): RF/RC/RR/Orient = 42	42	38	29.71
In SpinTurn(CW, 29.71, 30.00)

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
21657	728	794	816		45	42	41		97	10859	68	29659	0.40	0.25	-60	127	14
21758	728	794	816		46	42	43		99	10841	69	29645	0.26	0.25	-4	79	71
21856	728	794	816		44	44	45		98	10811	65	29634	0.04	0.25	84	0	127
21956	728	794	816		45	43	44		95	10782	68	29638	0.14	0.25	44	31	119
22055	728	794	816		44	41	42		91	10754	77	29623	0.21	0.25	16	59	91
22154	728	794	816		42	40	40		85	10722	73	29601	0.21	0.25	16	59	91
22258	728	123	816		42	40	41		82	10686	77	29588	0.14	0.25	44	31	119
22356	728	794	816		42	39	39		77	10648	87	29574	0.24	0.25	 4	71	79
22459	728	794	816		41	39	40		74	10604	90	29566	0.16	0.25	36	39	111
22555	728	794	816		40	38	38		70	10564	89	29551	0.19	0.25	24	51	99
22656	728	794	816		39	37	37		68	10513	93	29539	0.08	0.25	68	7	127
22758	728	794	816		40	38	37		64	10459	107	29526	0.22	0.25	12	63	87
22855	728	123	816		42	39	37		60	10395	111	29512	0.49	0.25	-96	127	0
22957	728	121	816		43	42	39		56	10314	106	29497	0.43	0.25	-72	127	3
23057	728	794	816		42	42	39		52	10491	112	29495	0.46	0.25	-84	127	0
23156	728	794	816		43	41	40		49	10673	121	29492	0.32	0.25	-28	103	46
23256	728	794	816		44	41	41		43	10870	126	29488	0.34	0.25	-36	111	39
23355	728	794	816		43	41	40		40	11071	750	37177	0.39	0.25	-56	127	18
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Right
In SpinTurn(CCW, 90.00, 45.00)
IRHomingValTotalAvg = 193 
24722: glLeftCenterCm = 794 glRightCenterCm = 30
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
24726: TrackRightWallOffset(), glRightFrontCm = 31

In TrackRightWallOffset with Kp/Ki/Kd = 400.00	0.00	0.00

In TrackRightWallOffset with RR/RC/RF = 30	30	31
straight window = 8 cm
R/C/F dists = 30	30	31 Steerval = 0.140, WallAngle = 8.00, Inside
In SpinTurn(CCW, 22.00, 45.00)
R/C/F dists = 27	29	30 Steerval = 0.300, WallOrientDeg = 17.14

approach start: orient angle = 17.143, tgt_distCm = 35
At start: meas_dist = 29, corr_dist = 28, err = prev_err = -7
R/C/F dists = 27	29	30 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -7
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
25109	31	29	27	17.1	0.3	27	-8	-7
25220	32	30	29	17.1	0.3	28	-7	-8
25331	33	31	28	17.1	0.3	29	-6	-7
25446	34	31	30	17.1	0.3	29	-6	-6
25559	35	33	31	17.1	0.3	31	-4	-6
25674	35	34	32	17.1	0.3	32	-3	-4
25787	37	35	32	17.1	0.3	33	-2	-3
25899	39	37	33	17.1	0.3	35	0	-2
At end of offset capture - prev_res*res = 0
correct back to parallel (Right side): RF/RC/RR/Orient = 39	37	33	17.14
In SpinTurn(CW, 17.14, 30.00)

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
26414	111	113	816		41	39	39		99	10861	74	37154	0.21	0.25	16	59	91
26512	113	117	123		41	40	38		100	10811	70	37167	0.27	0.25	-8	83	67
26612	109	113	118		42	38	39		96	10759	70	37170	0.29	0.25	-16	91	58
26714	111	115	124		41	39	38		94	10698	80	37163	0.34	0.25	-36	111	39
26811	109	117	119		40	38	37		88	10645	88	37159	0.27	0.25	-8	83	67
26912	111	113	124		39	38	37		84	10588	84	37172	0.24	0.25	 4	71	79
27012	113	117	816		40	38	36		80	10527	87	37178	0.23	0.25	 8	67	82
27111	728	120	124		38	37	36		77	10460	98	37165	0.12	0.25	52	23	127
27212	110	116	127		37	35	35		75	10384	101	37171	0.09	0.25	64	11	127
27314	728	114	122		36	36	36		72	10304	101	37178	-0.01	0.25	104	0	127
27412	112	117	816		35	34	33		66	10218	106	37177	0.16	0.25	36	39	111
27511	112	112	816		36	34	33		64	10126	116	37174	0.26	0.25	-4	79	71
27610	728	118	127		38	36	34		62	10261	750	44549	0.28	0.25	-12	87	62
27711	728	117	125		38	38	36		60	10406	750	51599	0.24	0.25	 4	71	79
27813	728	112	126		40	37	35		57	10556	819	53316	0.38	0.25	-52	127	23
27911	113	116	816		41	38	35		53	10713	819	53316	0.54	0.25	-116	127	0
28012	728	116	128		41	40	37		46	10888	819	53316	0.36	0.25	-44	119	30
28113	113	114	119		42	40	38		41	11075	750	59737	0.40	0.25	-60	127	14
28212	104	108	120		41	39	39		36	11273	750	65748	0.26	0.25	-4	79	71

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

approach start: orient angle = 16.571, tgt_distCm = 35

At start: meas_dist = 15, corr_dist = 14, err = prev_err = -21

R/C/F dists = 13 15 15 Steerval = 0.290, OrientDeg = 16.57, err/prev_err = -21

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

11024 16 15 12 16.6 0.3 14 -21 -21

11135 18 17 14 16.6 0.3 16 -19 -21

11249 20 18 16 16.6 0.3 17 -18 -19

11365 22 22 18 16.6 0.3 21 -14 -18

11480 26 24 21 16.6 0.3 23 -12 -14

11591 29 27 23 16.6 0.3 25 -10 -12

11702 31 30 26 16.6 0.3 28 -7 -10

11814 34 32 29 16.6 0.3 30 -5 -7

11927 36 34 31 16.6 0.3 32 -3 -5

12037 41 38 35 16.6 0.3 36 1 -3

At end of offset capture - prev_res*res = -3

correct back to parallel (Right side): RF/RC/RR/Orient = 41 38 35 16.57

In SpinTurn(CW, 16.57, 30.00)

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00 0.00 0.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

12519 728 794 816 41 39 39 90 11753 60 11683 0.21 0.25 16 59 91

12619 728 794 816 41 39 38 90 11797 70 11628 0.31 0.25 -24 99 51

12719 728 794 816 41 40 38 87 11752 69 11576 0.39 0.25 -56 127 19

12817 728 794 816 41 40 38 87 11588 63 11529 0.37 0.25 -48 123 27

12917 728 113 816 42 40 38 80 11311 70 11482 0.40 0.25 -60 127 14

13016 728 794 816 42 40 40 75 10891 78 11436 0.20 0.25 20 55 95

13118 728 794 816 42 41 40 71 10302 84 11392 0.23 0.25 8 67 83

13220 728 794 816 43 40 39 66 9523 79 11350 0.36 0.25 -44 119 30

13321 728 794 816 42 40 39 62 9693 84 11309 0.22 0.25 12 63 87

13416 728 794 816 42 40 39 59 9870 93 11273 0.27 0.25 -8 83 67

13519 728 794 816 40 39 40 57 10068 93 11237 -0.02 0.25 108 0 127

13618 728 794 816 40 39 38 54 10261 91 11202 0.14 0.25 44 31 119

13716 728 794 816 39 38 38 51 10455 107 11177 0.09 0.25 64 11 127

13817 728 794 816 40 37 37 45 10663 112 11156 0.20 0.25 20 54 95

13918 728 794 816 39 38 36 41 10879 109 11132 0.29 0.25 -16 91 58

14017 728 794 816 40 39 36 35 11109 115 11114 0.38 0.25 -52 127 23

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Right

In SpinTurn(CCW, 90.00, 45.00)

IRHomingValTotalAvg = 199

15351: glLeftCenterCm = 794 glRightCenterCm = 24

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

15355: TrackRightWallOffset(), glRightFrontCm = 26

In TrackRightWallOffset with Kp/Ki/Kd = 400.00 0.00 0.00

In TrackRightWallOffset with RR/RC/RF = 24 24 26

straight window = 8 cm

R/C/F dists = 24 24 26 Steerval = 0.170, WallAngle = 9.71, Inside

In SpinTurn(CCW, 20.29, 45.00)

R/C/F dists = 26 31 32 Steerval = 0.590, WallOrientDeg = 33.71

approach start: orient angle = 33.714, tgt_distCm = 35

At start: meas_dist = 31, corr_dist = 26, err = prev_err = -9

R/C/F dists = 26 31 32 Steerval = 0.590, OrientDeg = 33.71, err/prev_err = -9

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

15712 33 31 27 33.7 0.6 25 -10 -9

15826 37 34 29 33.7 0.6 28 -7 -10

15940 38 37 32 33.7 0.6 30 -5 -7

16053 42 41 36 33.7 0.6 34 -1 -5

16165 46 44 39 33.7 0.6 36 1 -1

At end of offset capture - prev_res*res = -1

correct back to parallel (Right side): RF/RC/RR/Orient = 46 44 39 33.71

In SpinTurn(CW, 33.71, 30.00)

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00 0.00 0.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

16862 112 110 125 43 43 42 108 11196 64 11178 0.25 0.25 0 75 75

16964 728 110 816 45 43 42 108 11162 65 11179 0.27 0.25 -8 82 67

17060 728 111 127 44 42 40 104 11135 61 11186 0.39 0.25 -56 127 19

17161 110 114 124 42 42 41 102 11105 70 11186 0.19 0.25 24 51 99

17263 728 114 126 44 42 41 100 11077 75 11184 0.30 0.25 -20 94 55

17362 728 115 816 43 42 41 97 11038 71 11186 0.23 0.25 8 67 83

17461 728 116 126 43 41 41 95 10989 77 11185 0.24 0.25 4 71 79

17562 728 113 816 44 41 41 90 10938 86 11183 0.36 0.25 -44 119 30

17664 728 109 124 43 41 41 84 10886 91 11180 0.24 0.25 4 71 79

17763 728 118 816 41 41 41 82 10825 90 11179 0.07 0.25 72 3 127

17860 728 794 127 42 39 39 79 10762 90 11178 0.29 0.25 -16 91 59

17961 728 115 816 41 40 39 73 10708 106 11182 0.23 0.25 8 67 83

18059 728 120 816 42 39 39 69 10656 106 11186 0.21 0.25 16 59 91

18161 728 116 816 41 39 39 64 10608 105 11189 0.15 0.25 40 35 115

18262 728 121 816 40 39 36 60 10550 113 11198 0.35 0.25 -40 115 35

18360 728 121 816 40 38 38 58 10482 124 11217 0.17 0.25 32 42 107

18459 728 121 816 40 38 37 53 10405 117 11228 0.22 0.25 12 63 87

18561 728 121 816 40 38 39 49 10600 750 19677 0.07 0.25 72 3 127

18659 728 794 816 40 39 37 43 10806 819 29443 0.24 0.25 4 71 79

18762 728 120 816 40 39 36 36 11021 819 38752 0.37 0.25 -48 122 27

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Right

In SpinTurn(CCW, 90.00, 45.00)

IRHomingValTotalAvg = 273

19919: glLeftCenterCm = 794 glRightCenterCm = 25

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

19923: TrackRightWallOffset(), glRightFrontCm = 28

In TrackRightWallOffset with Kp/Ki/Kd = 400.00 0.00 0.00

In TrackRightWallOffset with RR/RC/RF = 26 25 28

straight window = 8 cm

R/C/F dists = 26 25 28 Steerval = 0.180, WallAngle = 10.29, Inside

In SpinTurn(CCW, 19.71, 45.00)

R/C/F dists = 25 28 30 Steerval = 0.490, WallOrientDeg = 28.00

approach start: orient angle = 28.000, tgt_distCm = 35

At start: meas_dist = 28, corr_dist = 25, err = prev_err = -10

R/C/F dists = 25 28 30 Steerval = 0.520, OrientDeg = 29.71, err/prev_err = -10

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

20308 30 28 25 29.7 0.5 24 -11 -10

20420 32 30 26 29.7 0.5 26 -9 -11

20533 33 32 29 29.7 0.5 26 -9 -9

20647 36 33 30 29.7 0.5 28 -7 -9

20761 38 36 32 29.7 0.5 31 -4 -7

20876 41 38 36 29.7 0.5 33 -2 -4

20988 42 42 38 29.7 0.5 36 1 -2

At end of offset capture - prev_res*res = -2

correct back to parallel (Right side): RF/RC/RR/Orient = 42 42 38 29.71

In SpinTurn(CW, 29.71, 30.00)

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00 0.00 0.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

21657 728 794 816 45 42 41 97 10859 68 29659 0.40 0.25 -60 127 14

21758 728 794 816 46 42 43 99 10841 69 29645 0.26 0.25 -4 79 71

21856 728 794 816 44 44 45 98 10811 65 29634 0.04 0.25 84 0 127

21956 728 794 816 45 43 44 95 10782 68 29638 0.14 0.25 44 31 119

22055 728 794 816 44 41 42 91 10754 77 29623 0.21 0.25 16 59 91

22154 728 794 816 42 40 40 85 10722 73 29601 0.21 0.25 16 59 91

22258 728 123 816 42 40 41 82 10686 77 29588 0.14 0.25 44 31 119

22356 728 794 816 42 39 39 77 10648 87 29574 0.24 0.25 4 71 79

22459 728 794 816 41 39 40 74 10604 90 29566 0.16 0.25 36 39 111

22555 728 794 816 40 38 38 70 10564 89 29551 0.19 0.25 24 51 99

22656 728 794 816 39 37 37 68 10513 93 29539 0.08 0.25 68 7 127

22758 728 794 816 40 38 37 64 10459 107 29526 0.22 0.25 12 63 87

22855 728 123 816 42 39 37 60 10395 111 29512 0.49 0.25 -96 127 0

22957 728 121 816 43 42 39 56 10314 106 29497 0.43 0.25 -72 127 3

23057 728 794 816 42 42 39 52 10491 112 29495 0.46 0.25 -84 127 0

23156 728 794 816 43 41 40 49 10673 121 29492 0.32 0.25 -28 103 46

23256 728 794 816 44 41 41 43 10870 126 29488 0.34 0.25 -36 111 39

23355 728 794 816 43 41 40 40 11071 750 37177 0.39 0.25 -56 127 18

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Right

In SpinTurn(CCW, 90.00, 45.00)

IRHomingValTotalAvg = 193

24722: glLeftCenterCm = 794 glRightCenterCm = 30

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

24726: TrackRightWallOffset(), glRightFrontCm = 31

In TrackRightWallOffset with Kp/Ki/Kd = 400.00 0.00 0.00

In TrackRightWallOffset with RR/RC/RF = 30 30 31

straight window = 8 cm

R/C/F dists = 30 30 31 Steerval = 0.140, WallAngle = 8.00, Inside

In SpinTurn(CCW, 22.00, 45.00)

R/C/F dists = 27 29 30 Steerval = 0.300, WallOrientDeg = 17.14

approach start: orient angle = 17.143, tgt_distCm = 35

At start: meas_dist = 29, corr_dist = 28, err = prev_err = -7

R/C/F dists = 27 29 30 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -7

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

25109 31 29 27 17.1 0.3 27 -8 -7

25220 32 30 29 17.1 0.3 28 -7 -8

25331 33 31 28 17.1 0.3 29 -6 -7

25446 34 31 30 17.1 0.3 29 -6 -6

25559 35 33 31 17.1 0.3 31 -4 -6

25674 35 34 32 17.1 0.3 32 -3 -4

25787 37 35 32 17.1 0.3 33 -2 -3

25899 39 37 33 17.1 0.3 35 0 -2

At end of offset capture - prev_res*res = 0

correct back to parallel (Right side): RF/RC/RR/Orient = 39 37 33 17.14

In SpinTurn(CW, 17.14, 30.00)

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00 0.00 0.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

26414 111 113 816 41 39 39 99 10861 74 37154 0.21 0.25 16 59 91

26512 113 117 123 41 40 38 100 10811 70 37167 0.27 0.25 -8 83 67

26612 109 113 118 42 38 39 96 10759 70 37170 0.29 0.25 -16 91 58

26714 111 115 124 41 39 38 94 10698 80 37163 0.34 0.25 -36 111 39

26811 109 117 119 40 38 37 88 10645 88 37159 0.27 0.25 -8 83 67

26912 111 113 124 39 38 37 84 10588 84 37172 0.24 0.25 4 71 79

27012 113 117 816 40 38 36 80 10527 87 37178 0.23 0.25 8 67 82

27111 728 120 124 38 37 36 77 10460 98 37165 0.12 0.25 52 23 127

27212 110 116 127 37 35 35 75 10384 101 37171 0.09 0.25 64 11 127

27314 728 114 122 36 36 36 72 10304 101 37178 -0.01 0.25 104 0 127

27412 112 117 816 35 34 33 66 10218 106 37177 0.16 0.25 36 39 111

27511 112 112 816 36 34 33 64 10126 116 37174 0.26 0.25 -4 79 71

27610 728 118 127 38 36 34 62 10261 750 44549 0.28 0.25 -12 87 62

27711 728 117 125 38 38 36 60 10406 750 51599 0.24 0.25 4 71 79

27813 728 112 126 40 37 35 57 10556 819 53316 0.38 0.25 -52 127 23

27911 113 116 816 41 38 35 53 10713 819 53316 0.54 0.25 -116 127 0

28012 728 116 128 41 40 37 46 10888 819 53316 0.36 0.25 -44 119 30

28113 113 114 119 42 40 38 41 11075 750 59737 0.40 0.25 -60 127 14

28212 104 108 120 41 39 39 36 11273 750 65748 0.26 0.25 -4 79 71

All four wall sections – note straight lines are due to gaps between wall sections

So now I think I finally (I’ve only been working on this for the last three years!) have a wall tracking algorithm that actually makes sense and does what it is supposed to do – track the wall at a constant offset – yay!!

After getting the right side working, I ported everything back to the left side, with some differences; for the left side approach phase, I wound up using a fudge factor of 10cm vice 5cm to get the approach to stop near the desired offset. Also, the base steering value setpoint was -0.35 instead of +0.25, and the input (WallTrackSteerVal) wound up being

  WallTrackSteerVal = glLeftSteeringVal + ((float)glLeftCenterCm - (float)offsetCm) / 25.f;

1	WallTrackSteerVal = glLeftSteeringVal + ((float)glLeftCenterCm - (float)offsetCm) / 25.f;

With these settings, Wall-E3 seemed pretty comfortable navigating around my office ‘sandbox’, as shown in the following short video:

Stay tuned,

Frank

Wall-E3 Right Wall Following Trial

Leave a reply

Posted 23 March 2022,

Earlier this month I was able to demonstrate a multi-lap left-side wall tracking run by Wall-E3 in my office ‘sandbox’. This post describes my efforts to extend this capability to right-side wall tracking.

Since I already had the left-side wall tracking algorithm “in the can”, I thought it would be a piece of cake to extend this capability to right-side tracking. Little did I know that this would turn into yet another adventure in Wonderland – but at least when I finally made it back out of the rabbit-hole, the result was a distinct improvement over the left-side algorithm I started with. Here’s the left-side code:

void TrackLeftWallOffset(float kp, float ki, float kd, float offsetCm)
{
  //Purpose: Track left wall at specified offset
  // Inputs:
  //  kp,ki,kd = float objects denoting PID parameters for PIDCalcs()
  //  offsetCm = float denoting desired wall offset, in Cm
  // Plan:
  //  Step1: If robot is already close to desired offset, just turn parallel and go
  //  Step1: Otherwise, get current robot orientation and offset wrt wall
  //  Step2: Move to offset distance and parallel to wall
  //  Step3: Track wall at desired offset 
  //Notes:
  //	06/21/21 modified to do a turn, then straight, then track 0 steer val

  mSecSinceLastWallTrackUpdate = 0;
  float lastError = 0;
  float lastInput = 0;
  float lastIval = 0;
  float lastDerror = 0;
  float spinRateDPS = 30;

  AnomalyCode errcode = NO_ANOMALIES;//03/06/22 mvd here to make global to function

  GetRequestedVL53l0xValues(VL53L0X_LEFT); //update glLeftSteeringVal

  myTeePrint.printf("\nIn TrackLeftWallOffset with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);
  myTeePrint.printf("\nIn TrackLeftWallOffset with LR/LC/LF = %d\t%d\t%d\n", glLeftRearCm, glLeftCenterCm, glLeftFrontCm);

//Step1: If robot is already close to desired offset, just turn parallel and go
  if (abs(WALL_OFFSET_TGTDIST_CM - glLeftCenterCm) <= (uint16_t)((float)WALL_OFFSET_TGTDIST_CM/10.f))
  {
    myTeePrint.printf("At %d with desired offset = %d: close enough  - just parallel and go\n", 
      glLeftCenterCm, WALL_OFFSET_TGTDIST_CM);

    RotateToParallelOrientation(TRACKING_LEFT);
  }
  else //OK, have to move to offset distance before rotating back to parallel
  {
    //Step1: Get current robot orientation and offset wrt wall
    float cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(glLeftCenterCm);//positive inside, negative outside desired offset
    //myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, CutAngle = %2.2f\n",
    //  glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, cutAngleDeg);

    //07/05/21 implement min cut angle
    if (cutAngleDeg < 0 && abs(cutAngleDeg) < WALL_OFFSET_APPR_ANGLE_MINDEG)
    {
      //myTeePrint.printf("(cutAngleDeg < 0 && abs(cutAngleDeg) < %d -- setting to -%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);
      cutAngleDeg = -WALL_OFFSET_APPR_ANGLE_MINDEG;
    }
    else if (cutAngleDeg > 0 && cutAngleDeg < WALL_OFFSET_APPR_ANGLE_MINDEG)
    {
      //myTeePrint.printf("(cutAngleDeg > 0 && abs(cutAngleDeg) < %d -- setting to +%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);
      cutAngleDeg = WALL_OFFSET_APPR_ANGLE_MINDEG;
    }

    //Step2: get the steering angle from the steering value, for comparison to the computed cut angle
    //positive value means robot is oriented away from wall
    float WallOrientDeg = GetWallOrientDeg(glLeftSteeringVal);

    //myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",
    //  glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, WallOrientDeg, cutAngleDeg);

    //Step3: now decide which way to turn.
    int16_t fudgeFactorCm = 0; //07/05/21 added so can change for inside vs outside starting condx
    if (cutAngleDeg > 0) //robot inside desired offset distance
    {
      if (WallOrientDeg > cutAngleDeg)  // --> WallOrientDeg also > 0, have to turn CCW
      {
        //myTeePrint.printf("CutAngDeg > 0, WallOrientDeg > cutAngleDeg - Turn CCW to cut angle\n");
        SpinTurn(true, WallOrientDeg - cutAngleDeg, spinRateDPS); //
      }
      else //(WallOrientDeg <= cutAngleDeg)// --> WallOrientDeg could be < 0
      {
        //myTeePrint.printf("CutAngDeg > 0, WallOrientDeg < cutAngleDeg\n");
        SpinTurn(false, cutAngleDeg - WallOrientDeg, spinRateDPS); //turn diff between WallOrientDeg & cutAngleDeg
      }

      fudgeFactorCm = -10; //07/05/21 don't need fudge factor for inside start condx
    }
    else // cutAngleDeg < 0 --> robot outside desired offset distance
    {
      if (WallOrientDeg > cutAngleDeg)  // --> WallOrientDeg may also be > 0
      {
        //robot turned too far toward wall
        myTeePrint.printf("CutAngDeg < 0, WallOrientDeg > cutAngleDeg\n");
        SpinTurn(true, WallOrientDeg - cutAngleDeg, spinRateDPS); //
      }
      else// (WallOrientDeg < cutAngleDeg)// --> WallOrientDeg must also be < 0
      {
        //robot turned to far away from wall
        myTeePrint.printf("CutAngDeg < 0, WallOrientDeg <= 0\n");
        SpinTurn(false, cutAngleDeg - WallOrientDeg, spinRateDPS); //null out WallOrientDeg & then turn addnl cutAngleDeg
      }

      fudgeFactorCm = 10; //07/05/21 need fudge factor for outside start condx
    }

    //myTeePrint.printf("fudgeFactorCm = %d\n", fudgeFactorCm);
    //delay(1000);

    //Step4: Figure out how far to travel to get to desired offset
    //adjust so offset capture occurs at correct perpendicular offset
    float adjfactor = cos(PI * cutAngleDeg / 180.f);
    float numerator = (float)WALL_OFFSET_TGTDIST_CM;
    float adjOffsetCm = (numerator / adjfactor);
    adjOffsetCm += fudgeFactorCm; //fudge factor for distance measurements lagging behind robot's travel. 
    //myTeePrint.printf("\nat approach start: cut angle = %2.3f, adjfactor = %2.3f, num = %2.2f, adjOffsetCm = %2.2f\n",
      //cutAngleDeg, adjfactor, numerator, adjOffsetCm);

    GetRequestedVL53l0xValues(VL53L0X_ALL); //added 09/05/21
    int16_t err = glLeftFrontCm - (uint16_t)adjOffsetCm; //neg for inside going out, pos for outside going in
    int16_t prev_err = err;
    //myTeePrint.printf("At start - err = prev_err = %ld\n", err, prev_err);

    WallOrientDeg = GetWallOrientDeg(glLeftSteeringVal);
    myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f, err/prev_err = %li\n",
      glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, WallOrientDeg, cutAngleDeg, err);


    //myTeePrint.printf("Msec\tLFront\tLCtr\tLRear\tFront\tRear\tErr\tP_err\n");

    //Step5: Move to desired offset
    //02/25/22 added check for error conditions (stuck, obstacle, dead battery, etc)
    AnomalyCode errcode = NO_ANOMALIES;
    while (((cutAngleDeg > 0 && err < 0) || (cutAngleDeg <= 0 && err >= 0)) && prev_err * err > 0 && errcode == NO_ANOMALIES) //sign change makes this < 0
    {
      prev_err = err;

      //MoveAhead(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);
      MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
      delay(100);
      glFrontdistval = GetFrontDistCm();
      GetRequestedVL53l0xValues(VL53L0X_ALL);
      err = glLeftFrontCm - adjOffsetCm; //06/29/21 now adj for slant dist vs perp dist
      //myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",
      //  millis(),
      //  glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glFrontdistval, glRearCm, err, prev_err);

      CheckForUserInput();
      errcode = CheckForErrorCondx();
    }

    //Step6: now turn back the same (unadjusted) amount
    //myTeePrint.printf("At end of offset capture - prev_res*res = %ld\n", prev_err * err);
    //myTeePrint.printf("correct back to parallel (left side)\n");
    SpinTurn(!(cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg
  }
  WallTrackSetPoint = 0; //moved here 6/22/21

  myTeePrint.printf("\nTrackLeftWallOffset: Start tracking offset of %2.2fcm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", offsetCm, kp, ki, kd);
  //myTeePrint.printf("Msec\tFdir\tCdir\tRdir\tRearD\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");
  myTeePrint.printf("Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\t\tR\tRvar\t\tSteer\tSet\tOutput\n");

  //Step7: Track the wall using PID algorithm
  mSecSinceLastWallTrackUpdate = 0; //added 09/04/21
  while (errcode == NO_ANOMALIES)
  {
    CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

    //09/04/21 back to elapsedMillis timing
    if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)
    {
      digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse

      mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

      //03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()
      UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls
      errcode = CheckForErrorCondx();

      //have to weight value by both angle and wall offset
      WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - WALL_OFFSET_TGTDIST_CM) / 1000.f;

      //update motor speeds, skipping bad values
      if (!isnan(WallTrackSteerVal))
      {
        WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,
          kp, ki, kd);

        int glLeftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;
        int glRightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

        glRightspeednum = (glRightspeednum <= MOTOR_SPEED_HALF) ? glRightspeednum : MOTOR_SPEED_HALF;
        glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0;
        glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_HALF) ? glLeftspeednum : MOTOR_SPEED_HALF;
        glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0;

        MoveAhead(glLeftspeednum, glRightspeednum);

        //myTeePrint.printf("Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\tR\tRvar\t\tSteer\tSet\tOutput\n");
        myTeePrint.printf("%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\t\t%d\t%d\t%2.2f\t%2.2f\t\t%2.2f\t%2.2f\t%2.2f\n",
          millis(),
          glLeftFrontCm, glLeftCenterCm, glLeftRearCm,
          glRightFrontCm, glRightCenterCm, glRightRearCm, 
          glFrontdistval, glFrontvar, glRearCm, glRearvar,
          WallTrackSteerVal, WallTrackSetPoint, WallTrackOutput);
      }
      digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse

    }
  }

  //Step8: Handle any anomalous conditions
  HandleAnomalousConditions(errcode, TRACKING_LEFT);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

void TrackLeftWallOffset(float kp, float ki, float kd, float offsetCm)

{

//Purpose: Track left wall at specified offset

// Inputs:

// kp,ki,kd = float objects denoting PID parameters for PIDCalcs()

// offsetCm = float denoting desired wall offset, in Cm

// Plan:

// Step1: If robot is already close to desired offset, just turn parallel and go

// Step1: Otherwise, get current robot orientation and offset wrt wall

// Step2: Move to offset distance and parallel to wall

// Step3: Track wall at desired offset

//Notes:

// 06/21/21 modified to do a turn, then straight, then track 0 steer val

mSecSinceLastWallTrackUpdate = 0;

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

float spinRateDPS = 30;

AnomalyCode errcode = NO_ANOMALIES;//03/06/22 mvd here to make global to function

GetRequestedVL53l0xValues(VL53L0X_LEFT); //update glLeftSteeringVal

myTeePrint.printf("\nIn TrackLeftWallOffset with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

myTeePrint.printf("\nIn TrackLeftWallOffset with LR/LC/LF = %d\t%d\t%d\n", glLeftRearCm, glLeftCenterCm, glLeftFrontCm);

//Step1: If robot is already close to desired offset, just turn parallel and go

if (abs(WALL_OFFSET_TGTDIST_CM - glLeftCenterCm) <= (uint16_t)((float)WALL_OFFSET_TGTDIST_CM/10.f))

{

myTeePrint.printf("At %d with desired offset = %d: close enough - just parallel and go\n",

glLeftCenterCm, WALL_OFFSET_TGTDIST_CM);

RotateToParallelOrientation(TRACKING_LEFT);

}

else //OK, have to move to offset distance before rotating back to parallel

{

//Step1: Get current robot orientation and offset wrt wall

float cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(glLeftCenterCm);//positive inside, negative outside desired offset

//myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, CutAngle = %2.2f\n",

// glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, cutAngleDeg);

//07/05/21 implement min cut angle

if (cutAngleDeg < 0 && abs(cutAngleDeg) < WALL_OFFSET_APPR_ANGLE_MINDEG)

{

//myTeePrint.printf("(cutAngleDeg < 0 && abs(cutAngleDeg) < %d -- setting to -%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);

cutAngleDeg = -WALL_OFFSET_APPR_ANGLE_MINDEG;

}

else if (cutAngleDeg > 0 && cutAngleDeg < WALL_OFFSET_APPR_ANGLE_MINDEG)

{

//myTeePrint.printf("(cutAngleDeg > 0 && abs(cutAngleDeg) < %d -- setting to +%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);

cutAngleDeg = WALL_OFFSET_APPR_ANGLE_MINDEG;

}

//Step2: get the steering angle from the steering value, for comparison to the computed cut angle

//positive value means robot is oriented away from wall

float WallOrientDeg = GetWallOrientDeg(glLeftSteeringVal);

//myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",

// glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, WallOrientDeg, cutAngleDeg);

//Step3: now decide which way to turn.

int16_t fudgeFactorCm = 0; //07/05/21 added so can change for inside vs outside starting condx

if (cutAngleDeg > 0) //robot inside desired offset distance

{

if (WallOrientDeg > cutAngleDeg) // --> WallOrientDeg also > 0, have to turn CCW

{

//myTeePrint.printf("CutAngDeg > 0, WallOrientDeg > cutAngleDeg - Turn CCW to cut angle\n");

SpinTurn(true, WallOrientDeg - cutAngleDeg, spinRateDPS); //

}

else //(WallOrientDeg <= cutAngleDeg)// --> WallOrientDeg could be < 0

{

//myTeePrint.printf("CutAngDeg > 0, WallOrientDeg < cutAngleDeg\n");

SpinTurn(false, cutAngleDeg - WallOrientDeg, spinRateDPS); //turn diff between WallOrientDeg & cutAngleDeg

}

fudgeFactorCm = -10; //07/05/21 don't need fudge factor for inside start condx

}

else // cutAngleDeg < 0 --> robot outside desired offset distance

{

if (WallOrientDeg > cutAngleDeg) // --> WallOrientDeg may also be > 0

{

//robot turned too far toward wall

myTeePrint.printf("CutAngDeg < 0, WallOrientDeg > cutAngleDeg\n");

SpinTurn(true, WallOrientDeg - cutAngleDeg, spinRateDPS); //

}

else// (WallOrientDeg < cutAngleDeg)// --> WallOrientDeg must also be < 0

{

//robot turned to far away from wall

myTeePrint.printf("CutAngDeg < 0, WallOrientDeg <= 0\n");

SpinTurn(false, cutAngleDeg - WallOrientDeg, spinRateDPS); //null out WallOrientDeg & then turn addnl cutAngleDeg

}

fudgeFactorCm = 10; //07/05/21 need fudge factor for outside start condx

}

//myTeePrint.printf("fudgeFactorCm = %d\n", fudgeFactorCm);

//delay(1000);

//Step4: Figure out how far to travel to get to desired offset

//adjust so offset capture occurs at correct perpendicular offset

float adjfactor = cos(PI * cutAngleDeg / 180.f);

float numerator = (float)WALL_OFFSET_TGTDIST_CM;

float adjOffsetCm = (numerator / adjfactor);

adjOffsetCm += fudgeFactorCm; //fudge factor for distance measurements lagging behind robot's travel.

//myTeePrint.printf("\nat approach start: cut angle = %2.3f, adjfactor = %2.3f, num = %2.2f, adjOffsetCm = %2.2f\n",

//cutAngleDeg, adjfactor, numerator, adjOffsetCm);

GetRequestedVL53l0xValues(VL53L0X_ALL); //added 09/05/21

int16_t err = glLeftFrontCm - (uint16_t)adjOffsetCm; //neg for inside going out, pos for outside going in

int16_t prev_err = err;

//myTeePrint.printf("At start - err = prev_err = %ld\n", err, prev_err);

WallOrientDeg = GetWallOrientDeg(glLeftSteeringVal);

myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f, err/prev_err = %li\n",

glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, WallOrientDeg, cutAngleDeg, err);

//myTeePrint.printf("Msec\tLFront\tLCtr\tLRear\tFront\tRear\tErr\tP_err\n");

//Step5: Move to desired offset

//02/25/22 added check for error conditions (stuck, obstacle, dead battery, etc)

AnomalyCode errcode = NO_ANOMALIES;

while (((cutAngleDeg > 0 && err < 0) || (cutAngleDeg <= 0 && err >= 0)) && prev_err * err > 0 && errcode == NO_ANOMALIES) //sign change makes this < 0

{

prev_err = err;

//MoveAhead(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

delay(100);

glFrontdistval = GetFrontDistCm();

GetRequestedVL53l0xValues(VL53L0X_ALL);

err = glLeftFrontCm - adjOffsetCm; //06/29/21 now adj for slant dist vs perp dist

//myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",

// millis(),

// glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glFrontdistval, glRearCm, err, prev_err);

CheckForUserInput();

errcode = CheckForErrorCondx();

}

//Step6: now turn back the same (unadjusted) amount

//myTeePrint.printf("At end of offset capture - prev_res*res = %ld\n", prev_err * err);

//myTeePrint.printf("correct back to parallel (left side)\n");

SpinTurn(!(cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

}

WallTrackSetPoint = 0; //moved here 6/22/21

myTeePrint.printf("\nTrackLeftWallOffset: Start tracking offset of %2.2fcm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", offsetCm, kp, ki, kd);

//myTeePrint.printf("Msec\tFdir\tCdir\tRdir\tRearD\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

myTeePrint.printf("Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\t\tR\tRvar\t\tSteer\tSet\tOutput\n");

//Step7: Track the wall using PID algorithm

mSecSinceLastWallTrackUpdate = 0; //added 09/04/21

while (errcode == NO_ANOMALIES)

{

CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

//09/04/21 back to elapsedMillis timing

if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)

{

digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse

mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

//03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()

UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls

errcode = CheckForErrorCondx();

//have to weight value by both angle and wall offset

WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - WALL_OFFSET_TGTDIST_CM) / 1000.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,

kp, ki, kd);

int glLeftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

int glRightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

glRightspeednum = (glRightspeednum <= MOTOR_SPEED_HALF) ? glRightspeednum : MOTOR_SPEED_HALF;

glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0;

glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_HALF) ? glLeftspeednum : MOTOR_SPEED_HALF;

glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0;

MoveAhead(glLeftspeednum, glRightspeednum);

//myTeePrint.printf("Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\tR\tRvar\t\tSteer\tSet\tOutput\n");

myTeePrint.printf("%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\t\t%d\t%d\t%2.2f\t%2.2f\t\t%2.2f\t%2.2f\t%2.2f\n",

millis(),

glLeftFrontCm, glLeftCenterCm, glLeftRearCm,

glRightFrontCm, glRightCenterCm, glRightRearCm,

glFrontdistval, glFrontvar, glRearCm, glRearvar,

WallTrackSteerVal, WallTrackSetPoint, WallTrackOutput);

}

digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse

}

//Step8: Handle any anomalous conditions

HandleAnomalousConditions(errcode, TRACKING_LEFT);

}

The above code works, in the sense that it allows Wall-E3 to successfully track the left-side wall of my ‘sandbox’. However, as I worked on porting the left-side tracking code to the right side, I kept thinking – this is awful code – surely there is a better way?

After letting this problem percolate for few days, I decided to see if I could approach the problem a little more logically. I realized there were two major conditions associated with the problem – namely is the robot’s initial position inside or outside the desired offset distance K? In addition, the robot can start out parallel to the wall, or pointed toward or away from the wall. Ignoring the ‘started out parallel’ degenerate case, this reminded me of a 3-parameter Karnaugh map configuration, so I started sketching it out in my notebook, and then later in a Word document, as shown below:

As shown above, I broke the 3-parameter into two 2-parameter Karnaugh maps, and the output is denoted by α_T. After a few minutes it became obvious that the formula for α_T is pretty simple – its either α_R – α_A1 or α_R – α_A2depending on whether the robot starts out outside or inside the desired offset distance. In code, this boils down to one line, as shown at the bottom of the Karnaugh map above, using the C++ ‘?’ trinary operator, and choosing CW vs CCW is easy too, as a negative result implies CCW, and a positive one implies CW. The actual code block is shown below:

    //Step3: now decide which way to turn.
    float turndeg = 0; 
    float appAngle = 0;
    if (bInsideOffset) //approach angle = +30
    {
      appAngle = 30;
      turndeg = WallOrientDeg - appAngle;
      fudgeFactorCm = -10;
    }
    else
    {
      appAngle = -30;
      turndeg = WallOrientDeg - appAngle;
      fudgeFactorCm = 10; 
    }

    SpinTurn((turndeg < 0), abs(turndeg));

//Step3: now decide which way to turn.

float turndeg = 0;

float appAngle = 0;

if (bInsideOffset) //approach angle = +30

{

appAngle = 30;

turndeg = WallOrientDeg - appAngle;

fudgeFactorCm = -10;

}

else

{

appAngle = -30;

turndeg = WallOrientDeg - appAngle;

fudgeFactorCm = 10;

}

SpinTurn((turndeg < 0), abs(turndeg));

Here’s a short video of Wall-E3 navigating the office ‘sandbox’ while tracking the right-side wall.

So, it looks like Wall-E3 now has tracking ability for both left-side and right-side walls, although I still have to clean things up and port the simpler right-side code into TrackLeftWallOffset().

25 March 2022 Update:

Well, that was easy! I just got through porting the new right-side wall tracking algorithm over to TrackLeftWallOffset(), and right out of the box was able to demonstrate successful left-wall tracking in my office ‘sandbox’.

At this point I believe I’m going to consider the ‘WallE3_WallTrack_V3’ project ‘finished’ (in the sense that most, if not all, my wall tracking goals have been met with this version), and move on to V4, thereby limiting the possible damage from my next inevitable descent through the rabbit hole into wonderland.

Stay tuned,

Frank