Author Archives: paynterf

Temperature Display for 3D Printer Enclosure

Posted 29 July 2021,

In my ongoing quest to convert my MakerGear M3-ID printer from a nice bench decorative piece into a real functioning printer.

Recently I have been having real problems with using dissolvable filaments with my MakerGear M3-ID dual-extruder 3D printer. I couldn’t get either the PVA (water soluble) or HIPS (Limonene soluble) filaments to stick worth a damn to the BuildTak surface. In the process of troubleshooting the problems, I discovered that the M3-ID has real trouble getting the print bed temperature above 100C – at least in my nicely air-conditioned lab spaces. So, I went on the hunt for a decent enclosure, and found this 3D Upfitters model.

3D Upfitters Enclosure for the MakerGear M3 series (M3 shown, but the same enclosure works for the M3-ID)

The dimensions shown for the enclosure look like they would work for my setup, so I ordered one – we’ll see. In the meantime I started thinking that I might like to control (or at least monitor) the internal temperature, especially the ambient temperature at the location of the Octoprint module and control electronics. My worry is that at high bed temperatures, the ambient temps might get worryingly close to the max temps for the control electronics. 3D Upfitters does offer a temperature readout, but I was pretty sure it wouldn’t accurately represent the ambient temps around the electronics, so I decided I would modify an earlier project to create a custom temperature probe, using the venerable Nokia 5110 LCD display, a DHT11 temperature/humidity sensor, and a Teensy 3.2.

I decided to use the Teensy 3.2 micro-controller rather than an Arduino UNO to avoid the issue with 5/3.3V level conversion and because I’ve used this item several times before in other projects. I’m sure there are cheaper alternatives, but this is what I had available, and they are rock-solid products. Here’s the schematic:

And here are some photos:

And here is the code that interfaces to the sensor and drives the display:

/*
This is the core graphics library for all our displays, providing a common
set of graphics primitives (points, lines, circles, etc.).  It needs to be
paired with a hardware-specific library for each display device we carry
(to handle the lower-level functions).

Adafruit invests time and resources providing this open source code, please
support Adafruit & open-source hardware by purchasing products from Adafruit!

Copyright (c) 2013 Adafruit Industries.  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

- Redistributions of source code must retain the above copyright notice,
  this list of conditions and the following disclaimer.
- Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
 */

#include "Adafruit_GFX.h"
#include "glcdfont.c"
#ifdef __AVR__
#include <avr/pgmspace.h>
#elif defined(ESP8266) || defined(ESP32)
#include <pgmspace.h>
#endif

// Many (but maybe not all) non-AVR board installs define macros
// for compatibility with existing PROGMEM-reading AVR code.
// Do our own checks and defines here for good measure...

#ifndef pgm_read_byte
#define pgm_read_byte(addr) (*(const unsigned char *)(addr))
#endif
#ifndef pgm_read_word
#define pgm_read_word(addr) (*(const unsigned short *)(addr))
#endif
#ifndef pgm_read_dword
#define pgm_read_dword(addr) (*(const unsigned long *)(addr))
#endif

// Pointers are a peculiar case...typically 16-bit on AVR boards,
// 32 bits elsewhere.  Try to accommodate both...

#if !defined(__INT_MAX__) || (__INT_MAX__ > 0xFFFF)
#define pgm_read_pointer(addr) ((void *)pgm_read_dword(addr))
#else
#define pgm_read_pointer(addr) ((void *)pgm_read_word(addr))
#endif

inline GFXglyph *pgm_read_glyph_ptr(const GFXfont *gfxFont, uint8_t c) {
#ifdef __AVR__
  return &(((GFXglyph *)pgm_read_pointer(&gfxFont->glyph))[c]);
#else
  // expression in __AVR__ section may generate "dereferencing type-punned
  // pointer will break strict-aliasing rules" warning In fact, on other
  // platforms (such as STM32) there is no need to do this pointer magic as
  // program memory may be read in a usual way So expression may be simplified
  return gfxFont->glyph + c;
#endif //__AVR__
}

inline uint8_t *pgm_read_bitmap_ptr(const GFXfont *gfxFont) {
#ifdef __AVR__
  return (uint8_t *)pgm_read_pointer(&gfxFont->bitmap);
#else
  // expression in __AVR__ section generates "dereferencing type-punned pointer
  // will break strict-aliasing rules" warning In fact, on other platforms (such
  // as STM32) there is no need to do this pointer magic as program memory may
  // be read in a usual way So expression may be simplified
  return gfxFont->bitmap;
#endif //__AVR__
}

#ifndef min
#define min(a, b) (((a) < (b)) ? (a) : (b))
#endif

#ifndef _swap_int16_t
#define _swap_int16_t(a, b)                                                    \
  {                                                                            \
    int16_t t = a;                                                             \
    a = b;                                                                     \
    b = t;                                                                     \
  }
#endif

/**************************************************************************/
/*!
   @brief    Instatiate a GFX context for graphics! Can only be done by a
   superclass
   @param    w   Display width, in pixels
   @param    h   Display height, in pixels
*/
/**************************************************************************/
Adafruit_GFX::Adafruit_GFX(int16_t w, int16_t h) : WIDTH(w), HEIGHT(h) {
  _width = WIDTH;
  _height = HEIGHT;
  rotation = 0;
  cursor_y = cursor_x = 0;
  textsize_x = textsize_y = 1;
  textcolor = textbgcolor = 0xFFFF;
  wrap = true;
  _cp437 = false;
  gfxFont = NULL;
}

/**************************************************************************/
/*!
   @brief    Write a line.  Bresenham's algorithm - thx wikpedia
    @param    x0  Start point x coordinate
    @param    y0  Start point y coordinate
    @param    x1  End point x coordinate
    @param    y1  End point y coordinate
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::writeLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
                             uint16_t color) {
#if defined(ESP8266)
  yield();
#endif
  int16_t steep = abs(y1 - y0) > abs(x1 - x0);
  if (steep) {
    _swap_int16_t(x0, y0);
    _swap_int16_t(x1, y1);
  }

  if (x0 > x1) {
    _swap_int16_t(x0, x1);
    _swap_int16_t(y0, y1);
  }

  int16_t dx, dy;
  dx = x1 - x0;
  dy = abs(y1 - y0);

  int16_t err = dx / 2;
  int16_t ystep;

  if (y0 < y1) {
    ystep = 1;
  } else {
    ystep = -1;
  }

  for (; x0 <= x1; x0++) {
    if (steep) {
      writePixel(y0, x0, color);
    } else {
      writePixel(x0, y0, color);
    }
    err -= dy;
    if (err < 0) {
      y0 += ystep;
      err += dx;
    }
  }
}

/**************************************************************************/
/*!
   @brief    Start a display-writing routine, overwrite in subclasses.
*/
/**************************************************************************/
void Adafruit_GFX::startWrite() {}

/**************************************************************************/
/*!
   @brief    Write a pixel, overwrite in subclasses if startWrite is defined!
    @param   x   x coordinate
    @param   y   y coordinate
   @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::writePixel(int16_t x, int16_t y, uint16_t color) {
  drawPixel(x, y, color);
}

/**************************************************************************/
/*!
   @brief    Write a perfectly vertical line, overwrite in subclasses if
   startWrite is defined!
    @param    x   Top-most x coordinate
    @param    y   Top-most y coordinate
    @param    h   Height in pixels
   @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::writeFastVLine(int16_t x, int16_t y, int16_t h,
                                  uint16_t color) {
  // Overwrite in subclasses if startWrite is defined!
  // Can be just writeLine(x, y, x, y+h-1, color);
  // or writeFillRect(x, y, 1, h, color);
  drawFastVLine(x, y, h, color);
}

/**************************************************************************/
/*!
   @brief    Write a perfectly horizontal line, overwrite in subclasses if
   startWrite is defined!
    @param    x   Left-most x coordinate
    @param    y   Left-most y coordinate
    @param    w   Width in pixels
   @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::writeFastHLine(int16_t x, int16_t y, int16_t w,
                                  uint16_t color) {
  // Overwrite in subclasses if startWrite is defined!
  // Example: writeLine(x, y, x+w-1, y, color);
  // or writeFillRect(x, y, w, 1, color);
  drawFastHLine(x, y, w, color);
}

/**************************************************************************/
/*!
   @brief    Write a rectangle completely with one color, overwrite in
   subclasses if startWrite is defined!
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    w   Width in pixels
    @param    h   Height in pixels
   @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::writeFillRect(int16_t x, int16_t y, int16_t w, int16_t h,
                                 uint16_t color) {
  // Overwrite in subclasses if desired!
  fillRect(x, y, w, h, color);
}

/**************************************************************************/
/*!
   @brief    End a display-writing routine, overwrite in subclasses if
   startWrite is defined!
*/
/**************************************************************************/
void Adafruit_GFX::endWrite() {}

/**************************************************************************/
/*!
   @brief    Draw a perfectly vertical line (this is often optimized in a
   subclass!)
    @param    x   Top-most x coordinate
    @param    y   Top-most y coordinate
    @param    h   Height in pixels
   @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::drawFastVLine(int16_t x, int16_t y, int16_t h,
                                 uint16_t color) {
  startWrite();
  writeLine(x, y, x, y + h - 1, color);
  endWrite();
}

/**************************************************************************/
/*!
   @brief    Draw a perfectly horizontal line (this is often optimized in a
   subclass!)
    @param    x   Left-most x coordinate
    @param    y   Left-most y coordinate
    @param    w   Width in pixels
   @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::drawFastHLine(int16_t x, int16_t y, int16_t w,
                                 uint16_t color) {
  startWrite();
  writeLine(x, y, x + w - 1, y, color);
  endWrite();
}

/**************************************************************************/
/*!
   @brief    Fill a rectangle completely with one color. Update in subclasses if
   desired!
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    w   Width in pixels
    @param    h   Height in pixels
   @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::fillRect(int16_t x, int16_t y, int16_t w, int16_t h,
                            uint16_t color) {
  startWrite();
  for (int16_t i = x; i < x + w; i++) {
    writeFastVLine(i, y, h, color);
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief    Fill the screen completely with one color. Update in subclasses if
   desired!
    @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::fillScreen(uint16_t color) {
  fillRect(0, 0, _width, _height, color);
}

/**************************************************************************/
/*!
   @brief    Draw a line
    @param    x0  Start point x coordinate
    @param    y0  Start point y coordinate
    @param    x1  End point x coordinate
    @param    y1  End point y coordinate
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
                            uint16_t color) {
  // Update in subclasses if desired!
  if (x0 == x1) {
    if (y0 > y1)
      _swap_int16_t(y0, y1);
    drawFastVLine(x0, y0, y1 - y0 + 1, color);
  } else if (y0 == y1) {
    if (x0 > x1)
      _swap_int16_t(x0, x1);
    drawFastHLine(x0, y0, x1 - x0 + 1, color);
  } else {
    startWrite();
    writeLine(x0, y0, x1, y1, color);
    endWrite();
  }
}

/**************************************************************************/
/*!
   @brief    Draw a circle outline
    @param    x0   Center-point x coordinate
    @param    y0   Center-point y coordinate
    @param    r   Radius of circle
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::drawCircle(int16_t x0, int16_t y0, int16_t r,
                              uint16_t color) {
#if defined(ESP8266)
  yield();
#endif
  int16_t f = 1 - r;
  int16_t ddF_x = 1;
  int16_t ddF_y = -2 * r;
  int16_t x = 0;
  int16_t y = r;

  startWrite();
  writePixel(x0, y0 + r, color);
  writePixel(x0, y0 - r, color);
  writePixel(x0 + r, y0, color);
  writePixel(x0 - r, y0, color);

  while (x < y) {
    if (f >= 0) {
      y--;
      ddF_y += 2;
      f += ddF_y;
    }
    x++;
    ddF_x += 2;
    f += ddF_x;

    writePixel(x0 + x, y0 + y, color);
    writePixel(x0 - x, y0 + y, color);
    writePixel(x0 + x, y0 - y, color);
    writePixel(x0 - x, y0 - y, color);
    writePixel(x0 + y, y0 + x, color);
    writePixel(x0 - y, y0 + x, color);
    writePixel(x0 + y, y0 - x, color);
    writePixel(x0 - y, y0 - x, color);
  }
  endWrite();
}

/**************************************************************************/
/*!
    @brief    Quarter-circle drawer, used to do circles and roundrects
    @param    x0   Center-point x coordinate
    @param    y0   Center-point y coordinate
    @param    r   Radius of circle
    @param    cornername  Mask bit #1 or bit #2 to indicate which quarters of
   the circle we're doing
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::drawCircleHelper(int16_t x0, int16_t y0, int16_t r,
                                    uint8_t cornername, uint16_t color) {
  int16_t f = 1 - r;
  int16_t ddF_x = 1;
  int16_t ddF_y = -2 * r;
  int16_t x = 0;
  int16_t y = r;

  while (x < y) {
    if (f >= 0) {
      y--;
      ddF_y += 2;
      f += ddF_y;
    }
    x++;
    ddF_x += 2;
    f += ddF_x;
    if (cornername & 0x4) {
      writePixel(x0 + x, y0 + y, color);
      writePixel(x0 + y, y0 + x, color);
    }
    if (cornername & 0x2) {
      writePixel(x0 + x, y0 - y, color);
      writePixel(x0 + y, y0 - x, color);
    }
    if (cornername & 0x8) {
      writePixel(x0 - y, y0 + x, color);
      writePixel(x0 - x, y0 + y, color);
    }
    if (cornername & 0x1) {
      writePixel(x0 - y, y0 - x, color);
      writePixel(x0 - x, y0 - y, color);
    }
  }
}

/**************************************************************************/
/*!
   @brief    Draw a circle with filled color
    @param    x0   Center-point x coordinate
    @param    y0   Center-point y coordinate
    @param    r   Radius of circle
    @param    color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::fillCircle(int16_t x0, int16_t y0, int16_t r,
                              uint16_t color) {
  startWrite();
  writeFastVLine(x0, y0 - r, 2 * r + 1, color);
  fillCircleHelper(x0, y0, r, 3, 0, color);
  endWrite();
}

/**************************************************************************/
/*!
    @brief  Quarter-circle drawer with fill, used for circles and roundrects
    @param  x0       Center-point x coordinate
    @param  y0       Center-point y coordinate
    @param  r        Radius of circle
    @param  corners  Mask bits indicating which quarters we're doing
    @param  delta    Offset from center-point, used for round-rects
    @param  color    16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void Adafruit_GFX::fillCircleHelper(int16_t x0, int16_t y0, int16_t r,
                                    uint8_t corners, int16_t delta,
                                    uint16_t color) {

  int16_t f = 1 - r;
  int16_t ddF_x = 1;
  int16_t ddF_y = -2 * r;
  int16_t x = 0;
  int16_t y = r;
  int16_t px = x;
  int16_t py = y;

  delta++; // Avoid some +1's in the loop

  while (x < y) {
    if (f >= 0) {
      y--;
      ddF_y += 2;
      f += ddF_y;
    }
    x++;
    ddF_x += 2;
    f += ddF_x;
    // These checks avoid double-drawing certain lines, important
    // for the SSD1306 library which has an INVERT drawing mode.
    if (x < (y + 1)) {
      if (corners & 1)
        writeFastVLine(x0 + x, y0 - y, 2 * y + delta, color);
      if (corners & 2)
        writeFastVLine(x0 - x, y0 - y, 2 * y + delta, color);
    }
    if (y != py) {
      if (corners & 1)
        writeFastVLine(x0 + py, y0 - px, 2 * px + delta, color);
      if (corners & 2)
        writeFastVLine(x0 - py, y0 - px, 2 * px + delta, color);
      py = y;
    }
    px = x;
  }
}

/**************************************************************************/
/*!
   @brief   Draw a rectangle with no fill color
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    w   Width in pixels
    @param    h   Height in pixels
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::drawRect(int16_t x, int16_t y, int16_t w, int16_t h,
                            uint16_t color) {
  startWrite();
  writeFastHLine(x, y, w, color);
  writeFastHLine(x, y + h - 1, w, color);
  writeFastVLine(x, y, h, color);
  writeFastVLine(x + w - 1, y, h, color);
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a rounded rectangle with no fill color
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    w   Width in pixels
    @param    h   Height in pixels
    @param    r   Radius of corner rounding
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::drawRoundRect(int16_t x, int16_t y, int16_t w, int16_t h,
                                 int16_t r, uint16_t color) {
  int16_t max_radius = ((w < h) ? w : h) / 2; // 1/2 minor axis
  if (r > max_radius)
    r = max_radius;
  // smarter version
  startWrite();
  writeFastHLine(x + r, y, w - 2 * r, color);         // Top
  writeFastHLine(x + r, y + h - 1, w - 2 * r, color); // Bottom
  writeFastVLine(x, y + r, h - 2 * r, color);         // Left
  writeFastVLine(x + w - 1, y + r, h - 2 * r, color); // Right
  // draw four corners
  drawCircleHelper(x + r, y + r, r, 1, color);
  drawCircleHelper(x + w - r - 1, y + r, r, 2, color);
  drawCircleHelper(x + w - r - 1, y + h - r - 1, r, 4, color);
  drawCircleHelper(x + r, y + h - r - 1, r, 8, color);
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a rounded rectangle with fill color
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    w   Width in pixels
    @param    h   Height in pixels
    @param    r   Radius of corner rounding
    @param    color 16-bit 5-6-5 Color to draw/fill with
*/
/**************************************************************************/
void Adafruit_GFX::fillRoundRect(int16_t x, int16_t y, int16_t w, int16_t h,
                                 int16_t r, uint16_t color) {
  int16_t max_radius = ((w < h) ? w : h) / 2; // 1/2 minor axis
  if (r > max_radius)
    r = max_radius;
  // smarter version
  startWrite();
  writeFillRect(x + r, y, w - 2 * r, h, color);
  // draw four corners
  fillCircleHelper(x + w - r - 1, y + r, r, 1, h - 2 * r - 1, color);
  fillCircleHelper(x + r, y + r, r, 2, h - 2 * r - 1, color);
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a triangle with no fill color
    @param    x0  Vertex #0 x coordinate
    @param    y0  Vertex #0 y coordinate
    @param    x1  Vertex #1 x coordinate
    @param    y1  Vertex #1 y coordinate
    @param    x2  Vertex #2 x coordinate
    @param    y2  Vertex #2 y coordinate
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
                                int16_t x2, int16_t y2, uint16_t color) {
  drawLine(x0, y0, x1, y1, color);
  drawLine(x1, y1, x2, y2, color);
  drawLine(x2, y2, x0, y0, color);
}

/**************************************************************************/
/*!
   @brief     Draw a triangle with color-fill
    @param    x0  Vertex #0 x coordinate
    @param    y0  Vertex #0 y coordinate
    @param    x1  Vertex #1 x coordinate
    @param    y1  Vertex #1 y coordinate
    @param    x2  Vertex #2 x coordinate
    @param    y2  Vertex #2 y coordinate
    @param    color 16-bit 5-6-5 Color to fill/draw with
*/
/**************************************************************************/
void Adafruit_GFX::fillTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1,
                                int16_t x2, int16_t y2, uint16_t color) {

  int16_t a, b, y, last;

  // Sort coordinates by Y order (y2 >= y1 >= y0)
  if (y0 > y1) {
    _swap_int16_t(y0, y1);
    _swap_int16_t(x0, x1);
  }
  if (y1 > y2) {
    _swap_int16_t(y2, y1);
    _swap_int16_t(x2, x1);
  }
  if (y0 > y1) {
    _swap_int16_t(y0, y1);
    _swap_int16_t(x0, x1);
  }

  startWrite();
  if (y0 == y2) { // Handle awkward all-on-same-line case as its own thing
    a = b = x0;
    if (x1 < a)
      a = x1;
    else if (x1 > b)
      b = x1;
    if (x2 < a)
      a = x2;
    else if (x2 > b)
      b = x2;
    writeFastHLine(a, y0, b - a + 1, color);
    endWrite();
    return;
  }

  int16_t dx01 = x1 - x0, dy01 = y1 - y0, dx02 = x2 - x0, dy02 = y2 - y0,
          dx12 = x2 - x1, dy12 = y2 - y1;
  int32_t sa = 0, sb = 0;

  // For upper part of triangle, find scanline crossings for segments
  // 0-1 and 0-2.  If y1=y2 (flat-bottomed triangle), the scanline y1
  // is included here (and second loop will be skipped, avoiding a /0
  // error there), otherwise scanline y1 is skipped here and handled
  // in the second loop...which also avoids a /0 error here if y0=y1
  // (flat-topped triangle).
  if (y1 == y2)
    last = y1; // Include y1 scanline
  else
    last = y1 - 1; // Skip it

  for (y = y0; y <= last; y++) {
    a = x0 + sa / dy01;
    b = x0 + sb / dy02;
    sa += dx01;
    sb += dx02;
    /* longhand:
    a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
    b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
    */
    if (a > b)
      _swap_int16_t(a, b);
    writeFastHLine(a, y, b - a + 1, color);
  }

  // For lower part of triangle, find scanline crossings for segments
  // 0-2 and 1-2.  This loop is skipped if y1=y2.
  sa = (int32_t)dx12 * (y - y1);
  sb = (int32_t)dx02 * (y - y0);
  for (; y <= y2; y++) {
    a = x1 + sa / dy12;
    b = x0 + sb / dy02;
    sa += dx12;
    sb += dx02;
    /* longhand:
    a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
    b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
    */
    if (a > b)
      _swap_int16_t(a, b);
    writeFastHLine(a, y, b - a + 1, color);
  }
  endWrite();
}

// BITMAP / XBITMAP / GRAYSCALE / RGB BITMAP FUNCTIONS ---------------------

/**************************************************************************/
/*!
   @brief      Draw a PROGMEM-resident 1-bit image at the specified (x,y)
   position, using the specified foreground color (unset bits are transparent).
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with monochrome bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, const uint8_t bitmap[],
                              int16_t w, int16_t h, uint16_t color) {

  int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte
  uint8_t byte = 0;

  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte <<= 1;
      else
        byte = pgm_read_byte(&bitmap[j * byteWidth + i / 8]);
      if (byte & 0x80)
        writePixel(x + i, y, color);
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief      Draw a PROGMEM-resident 1-bit image at the specified (x,y)
   position, using the specified foreground (for set bits) and background (unset
   bits) colors.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with monochrome bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
    @param    color 16-bit 5-6-5 Color to draw pixels with
    @param    bg 16-bit 5-6-5 Color to draw background with
*/
/**************************************************************************/
void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, const uint8_t bitmap[],
                              int16_t w, int16_t h, uint16_t color,
                              uint16_t bg) {

  int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte
  uint8_t byte = 0;

  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte <<= 1;
      else
        byte = pgm_read_byte(&bitmap[j * byteWidth + i / 8]);
      writePixel(x + i, y, (byte & 0x80) ? color : bg);
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief      Draw a RAM-resident 1-bit image at the specified (x,y) position,
   using the specified foreground color (unset bits are transparent).
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with monochrome bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
    @param    color 16-bit 5-6-5 Color to draw with
*/
/**************************************************************************/
void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, uint8_t *bitmap, int16_t w,
                              int16_t h, uint16_t color) {

  int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte
  uint8_t byte = 0;

  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte <<= 1;
      else
        byte = bitmap[j * byteWidth + i / 8];
      if (byte & 0x80)
        writePixel(x + i, y, color);
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief      Draw a RAM-resident 1-bit image at the specified (x,y) position,
   using the specified foreground (for set bits) and background (unset bits)
   colors.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with monochrome bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
    @param    color 16-bit 5-6-5 Color to draw pixels with
    @param    bg 16-bit 5-6-5 Color to draw background with
*/
/**************************************************************************/
void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, uint8_t *bitmap, int16_t w,
                              int16_t h, uint16_t color, uint16_t bg) {

  int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte
  uint8_t byte = 0;

  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte <<= 1;
      else
        byte = bitmap[j * byteWidth + i / 8];
      writePixel(x + i, y, (byte & 0x80) ? color : bg);
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief      Draw PROGMEM-resident XBitMap Files (*.xbm), exported from GIMP.
   Usage: Export from GIMP to *.xbm, rename *.xbm to *.c and open in editor.
   C Array can be directly used with this function.
   There is no RAM-resident version of this function; if generating bitmaps
   in RAM, use the format defined by drawBitmap() and call that instead.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with monochrome bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
    @param    color 16-bit 5-6-5 Color to draw pixels with
*/
/**************************************************************************/
void Adafruit_GFX::drawXBitmap(int16_t x, int16_t y, const uint8_t bitmap[],
                               int16_t w, int16_t h, uint16_t color) {

  int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte
  uint8_t byte = 0;

  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte >>= 1;
      else
        byte = pgm_read_byte(&bitmap[j * byteWidth + i / 8]);
      // Nearly identical to drawBitmap(), only the bit order
      // is reversed here (left-to-right = LSB to MSB):
      if (byte & 0x01)
        writePixel(x + i, y, color);
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a PROGMEM-resident 8-bit image (grayscale) at the specified
   (x,y) pos. Specifically for 8-bit display devices such as IS31FL3731; no
   color reduction/expansion is performed.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with grayscale bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
*/
/**************************************************************************/
void Adafruit_GFX::drawGrayscaleBitmap(int16_t x, int16_t y,
                                       const uint8_t bitmap[], int16_t w,
                                       int16_t h) {
  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      writePixel(x + i, y, (uint8_t)pgm_read_byte(&bitmap[j * w + i]));
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a RAM-resident 8-bit image (grayscale) at the specified (x,y)
   pos. Specifically for 8-bit display devices such as IS31FL3731; no color
   reduction/expansion is performed.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with grayscale bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
*/
/**************************************************************************/
void Adafruit_GFX::drawGrayscaleBitmap(int16_t x, int16_t y, uint8_t *bitmap,
                                       int16_t w, int16_t h) {
  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      writePixel(x + i, y, bitmap[j * w + i]);
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a PROGMEM-resident 8-bit image (grayscale) with a 1-bit mask
   (set bits = opaque, unset bits = clear) at the specified (x,y) position.
   BOTH buffers (grayscale and mask) must be PROGMEM-resident.
   Specifically for 8-bit display devices such as IS31FL3731; no color
   reduction/expansion is performed.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with grayscale bitmap
    @param    mask  byte array with mask bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
*/
/**************************************************************************/
void Adafruit_GFX::drawGrayscaleBitmap(int16_t x, int16_t y,
                                       const uint8_t bitmap[],
                                       const uint8_t mask[], int16_t w,
                                       int16_t h) {
  int16_t bw = (w + 7) / 8; // Bitmask scanline pad = whole byte
  uint8_t byte = 0;
  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte <<= 1;
      else
        byte = pgm_read_byte(&mask[j * bw + i / 8]);
      if (byte & 0x80) {
        writePixel(x + i, y, (uint8_t)pgm_read_byte(&bitmap[j * w + i]));
      }
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a RAM-resident 8-bit image (grayscale) with a 1-bit mask
   (set bits = opaque, unset bits = clear) at the specified (x,y) position.
   BOTH buffers (grayscale and mask) must be RAM-residentt, no mix-and-match
   Specifically for 8-bit display devices such as IS31FL3731; no color
   reduction/expansion is performed.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with grayscale bitmap
    @param    mask  byte array with mask bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
*/
/**************************************************************************/
void Adafruit_GFX::drawGrayscaleBitmap(int16_t x, int16_t y, uint8_t *bitmap,
                                       uint8_t *mask, int16_t w, int16_t h) {
  int16_t bw = (w + 7) / 8; // Bitmask scanline pad = whole byte
  uint8_t byte = 0;
  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte <<= 1;
      else
        byte = mask[j * bw + i / 8];
      if (byte & 0x80) {
        writePixel(x + i, y, bitmap[j * w + i]);
      }
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a PROGMEM-resident 16-bit image (RGB 5/6/5) at the specified
   (x,y) position. For 16-bit display devices; no color reduction performed.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with 16-bit color bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
*/
/**************************************************************************/
void Adafruit_GFX::drawRGBBitmap(int16_t x, int16_t y, const uint16_t bitmap[],
                                 int16_t w, int16_t h) {
  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      writePixel(x + i, y, pgm_read_word(&bitmap[j * w + i]));
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a RAM-resident 16-bit image (RGB 5/6/5) at the specified (x,y)
   position. For 16-bit display devices; no color reduction performed.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with 16-bit color bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
*/
/**************************************************************************/
void Adafruit_GFX::drawRGBBitmap(int16_t x, int16_t y, uint16_t *bitmap,
                                 int16_t w, int16_t h) {
  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      writePixel(x + i, y, bitmap[j * w + i]);
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a PROGMEM-resident 16-bit image (RGB 5/6/5) with a 1-bit mask
   (set bits = opaque, unset bits = clear) at the specified (x,y) position. BOTH
   buffers (color and mask) must be PROGMEM-resident. For 16-bit display
   devices; no color reduction performed.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with 16-bit color bitmap
    @param    mask  byte array with monochrome mask bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
*/
/**************************************************************************/
void Adafruit_GFX::drawRGBBitmap(int16_t x, int16_t y, const uint16_t bitmap[],
                                 const uint8_t mask[], int16_t w, int16_t h) {
  int16_t bw = (w + 7) / 8; // Bitmask scanline pad = whole byte
  uint8_t byte = 0;
  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte <<= 1;
      else
        byte = pgm_read_byte(&mask[j * bw + i / 8]);
      if (byte & 0x80) {
        writePixel(x + i, y, pgm_read_word(&bitmap[j * w + i]));
      }
    }
  }
  endWrite();
}

/**************************************************************************/
/*!
   @brief   Draw a RAM-resident 16-bit image (RGB 5/6/5) with a 1-bit mask (set
   bits = opaque, unset bits = clear) at the specified (x,y) position. BOTH
   buffers (color and mask) must be RAM-resident. For 16-bit display devices; no
   color reduction performed.
    @param    x   Top left corner x coordinate
    @param    y   Top left corner y coordinate
    @param    bitmap  byte array with 16-bit color bitmap
    @param    mask  byte array with monochrome mask bitmap
    @param    w   Width of bitmap in pixels
    @param    h   Height of bitmap in pixels
*/
/**************************************************************************/
void Adafruit_GFX::drawRGBBitmap(int16_t x, int16_t y, uint16_t *bitmap,
                                 uint8_t *mask, int16_t w, int16_t h) {
  int16_t bw = (w + 7) / 8; // Bitmask scanline pad = whole byte
  uint8_t byte = 0;
  startWrite();
  for (int16_t j = 0; j < h; j++, y++) {
    for (int16_t i = 0; i < w; i++) {
      if (i & 7)
        byte <<= 1;
      else
        byte = mask[j * bw + i / 8];
      if (byte & 0x80) {
        writePixel(x + i, y, bitmap[j * w + i]);
      }
    }
  }
  endWrite();
}

// TEXT- AND CHARACTER-HANDLING FUNCTIONS ----------------------------------

// Draw a character
/**************************************************************************/
/*!
   @brief   Draw a single character
    @param    x   Bottom left corner x coordinate
    @param    y   Bottom left corner y coordinate
    @param    c   The 8-bit font-indexed character (likely ascii)
    @param    color 16-bit 5-6-5 Color to draw chraracter with
    @param    bg 16-bit 5-6-5 Color to fill background with (if same as color,
   no background)
    @param    size  Font magnification level, 1 is 'original' size
*/
/**************************************************************************/
void Adafruit_GFX::drawChar(int16_t x, int16_t y, unsigned char c,
                            uint16_t color, uint16_t bg, uint8_t size) {
  drawChar(x, y, c, color, bg, size, size);
}

// Draw a character
/**************************************************************************/
/*!
   @brief   Draw a single character
    @param    x   Bottom left corner x coordinate
    @param    y   Bottom left corner y coordinate
    @param    c   The 8-bit font-indexed character (likely ascii)
    @param    color 16-bit 5-6-5 Color to draw chraracter with
    @param    bg 16-bit 5-6-5 Color to fill background with (if same as color,
   no background)
    @param    size_x  Font magnification level in X-axis, 1 is 'original' size
    @param    size_y  Font magnification level in Y-axis, 1 is 'original' size
*/
/**************************************************************************/
void Adafruit_GFX::drawChar(int16_t x, int16_t y, unsigned char c,
                            uint16_t color, uint16_t bg, uint8_t size_x,
                            uint8_t size_y) {

  if (!gfxFont) { // 'Classic' built-in font

    if ((x >= _width) ||              // Clip right
        (y >= _height) ||             // Clip bottom
        ((x + 6 * size_x - 1) < 0) || // Clip left
        ((y + 8 * size_y - 1) < 0))   // Clip top
      return;

    if (!_cp437 && (c >= 176))
      c++; // Handle 'classic' charset behavior

    startWrite();
    for (int8_t i = 0; i < 5; i++) { // Char bitmap = 5 columns
      uint8_t line = pgm_read_byte(&font[c * 5 + i]);
      for (int8_t j = 0; j < 8; j++, line >>= 1) {
        if (line & 1) {
          if (size_x == 1 && size_y == 1)
            writePixel(x + i, y + j, color);
          else
            writeFillRect(x + i * size_x, y + j * size_y, size_x, size_y,
                          color);
        } else if (bg != color) {
          if (size_x == 1 && size_y == 1)
            writePixel(x + i, y + j, bg);
          else
            writeFillRect(x + i * size_x, y + j * size_y, size_x, size_y, bg);
        }
      }
    }
    if (bg != color) { // If opaque, draw vertical line for last column
      if (size_x == 1 && size_y == 1)
        writeFastVLine(x + 5, y, 8, bg);
      else
        writeFillRect(x + 5 * size_x, y, size_x, 8 * size_y, bg);
    }
    endWrite();

  } else { // Custom font

    // Character is assumed previously filtered by write() to eliminate
    // newlines, returns, non-printable characters, etc.  Calling
    // drawChar() directly with 'bad' characters of font may cause mayhem!

    c -= (uint8_t)pgm_read_byte(&gfxFont->first);
    GFXglyph *glyph = pgm_read_glyph_ptr(gfxFont, c);
    uint8_t *bitmap = pgm_read_bitmap_ptr(gfxFont);

    uint16_t bo = pgm_read_word(&glyph->bitmapOffset);
    uint8_t w = pgm_read_byte(&glyph->width), h = pgm_read_byte(&glyph->height);
    int8_t xo = pgm_read_byte(&glyph->xOffset),
           yo = pgm_read_byte(&glyph->yOffset);
    uint8_t xx, yy, bits = 0, bit = 0;
    int16_t xo16 = 0, yo16 = 0;

    if (size_x > 1 || size_y > 1) {
      xo16 = xo;
      yo16 = yo;
    }

    // Todo: Add character clipping here

    // NOTE: THERE IS NO 'BACKGROUND' COLOR OPTION ON CUSTOM FONTS.
    // THIS IS ON PURPOSE AND BY DESIGN.  The background color feature
    // has typically been used with the 'classic' font to overwrite old
    // screen contents with new data.  This ONLY works because the
    // characters are a uniform size; it's not a sensible thing to do with
    // proportionally-spaced fonts with glyphs of varying sizes (and that
    // may overlap).  To replace previously-drawn text when using a custom
    // font, use the getTextBounds() function to determine the smallest
    // rectangle encompassing a string, erase the area with fillRect(),
    // then draw new text.  This WILL infortunately 'blink' the text, but
    // is unavoidable.  Drawing 'background' pixels will NOT fix this,
    // only creates a new set of problems.  Have an idea to work around
    // this (a canvas object type for MCUs that can afford the RAM and
    // displays supporting setAddrWindow() and pushColors()), but haven't
    // implemented this yet.

    startWrite();
    for (yy = 0; yy < h; yy++) {
      for (xx = 0; xx < w; xx++) {
        if (!(bit++ & 7)) {
          bits = pgm_read_byte(&bitmap[bo++]);
        }
        if (bits & 0x80) {
          if (size_x == 1 && size_y == 1) {
            writePixel(x + xo + xx, y + yo + yy, color);
          } else {
            writeFillRect(x + (xo16 + xx) * size_x, y + (yo16 + yy) * size_y,
                          size_x, size_y, color);
          }
        }
        bits <<= 1;
      }
    }
    endWrite();

  } // End classic vs custom font
}
/**************************************************************************/
/*!
    @brief  Print one byte/character of data, used to support print()
    @param  c  The 8-bit ascii character to write
*/
/**************************************************************************/
size_t Adafruit_GFX::write(uint8_t c) {
  if (!gfxFont) { // 'Classic' built-in font

    if (c == '\n') {              // Newline?
      cursor_x = 0;               // Reset x to zero,
      cursor_y += textsize_y * 8; // advance y one line
    } else if (c != '\r') {       // Ignore carriage returns
      if (wrap && ((cursor_x + textsize_x * 6) > _width)) { // Off right?
        cursor_x = 0;                                       // Reset x to zero,
        cursor_y += textsize_y * 8; // advance y one line
      }
      drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize_x,
               textsize_y);
      cursor_x += textsize_x * 6; // Advance x one char
    }

  } else { // Custom font

    if (c == '\n') {
      cursor_x = 0;
      cursor_y +=
          (int16_t)textsize_y * (uint8_t)pgm_read_byte(&gfxFont->yAdvance);
    } else if (c != '\r') {
      uint8_t first = pgm_read_byte(&gfxFont->first);
      if ((c >= first) && (c <= (uint8_t)pgm_read_byte(&gfxFont->last))) {
        GFXglyph *glyph = pgm_read_glyph_ptr(gfxFont, c - first);
        uint8_t w = pgm_read_byte(&glyph->width),
                h = pgm_read_byte(&glyph->height);
        if ((w > 0) && (h > 0)) { // Is there an associated bitmap?
          int16_t xo = (int8_t)pgm_read_byte(&glyph->xOffset); // sic
          if (wrap && ((cursor_x + textsize_x * (xo + w)) > _width)) {
            cursor_x = 0;
            cursor_y += (int16_t)textsize_y *
                        (uint8_t)pgm_read_byte(&gfxFont->yAdvance);
          }
          drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize_x,
                   textsize_y);
        }
        cursor_x +=
            (uint8_t)pgm_read_byte(&glyph->xAdvance) * (int16_t)textsize_x;
      }
    }
  }
  return 1;
}

/**************************************************************************/
/*!
    @brief   Set text 'magnification' size. Each increase in s makes 1 pixel
   that much bigger.
    @param  s  Desired text size. 1 is default 6x8, 2 is 12x16, 3 is 18x24, etc
*/
/**************************************************************************/
void Adafruit_GFX::setTextSize(uint8_t s) { setTextSize(s, s); }

/**************************************************************************/
/*!
    @brief   Set text 'magnification' size. Each increase in s makes 1 pixel
   that much bigger.
    @param  s_x  Desired text width magnification level in X-axis. 1 is default
    @param  s_y  Desired text width magnification level in Y-axis. 1 is default
*/
/**************************************************************************/
void Adafruit_GFX::setTextSize(uint8_t s_x, uint8_t s_y) {
  textsize_x = (s_x > 0) ? s_x : 1;
  textsize_y = (s_y > 0) ? s_y : 1;
}

/**************************************************************************/
/*!
    @brief      Set rotation setting for display
    @param  x   0 thru 3 corresponding to 4 cardinal rotations
*/
/**************************************************************************/
void Adafruit_GFX::setRotation(uint8_t x) {
  rotation = (x & 3);
  switch (rotation) {
  case 0:
  case 2:
    _width = WIDTH;
    _height = HEIGHT;
    break;
  case 1:
  case 3:
    _width = HEIGHT;
    _height = WIDTH;
    break;
  }
}

/**************************************************************************/
/*!
    @brief Set the font to display when print()ing, either custom or default
    @param  f  The GFXfont object, if NULL use built in 6x8 font
*/
/**************************************************************************/
void Adafruit_GFX::setFont(const GFXfont *f) {
  if (f) {          // Font struct pointer passed in?
    if (!gfxFont) { // And no current font struct?
      // Switching from classic to new font behavior.
      // Move cursor pos down 6 pixels so it's on baseline.
      cursor_y += 6;
    }
  } else if (gfxFont) { // NULL passed.  Current font struct defined?
    // Switching from new to classic font behavior.
    // Move cursor pos up 6 pixels so it's at top-left of char.
    cursor_y -= 6;
  }
  gfxFont = (GFXfont *)f;
}

/**************************************************************************/
/*!
    @brief    Helper to determine size of a character with current font/size.
       Broke this out as it's used by both the PROGMEM- and RAM-resident
   getTextBounds() functions.
    @param    c     The ascii character in question
    @param    x     Pointer to x location of character
    @param    y     Pointer to y location of character
    @param    minx  Minimum clipping value for X
    @param    miny  Minimum clipping value for Y
    @param    maxx  Maximum clipping value for X
    @param    maxy  Maximum clipping value for Y
*/
/**************************************************************************/
void Adafruit_GFX::charBounds(unsigned char c, int16_t *x, int16_t *y,
                              int16_t *minx, int16_t *miny, int16_t *maxx,
                              int16_t *maxy) {

  if (gfxFont) {

    if (c == '\n') { // Newline?
      *x = 0;        // Reset x to zero, advance y by one line
      *y += textsize_y * (uint8_t)pgm_read_byte(&gfxFont->yAdvance);
    } else if (c != '\r') { // Not a carriage return; is normal char
      uint8_t first = pgm_read_byte(&gfxFont->first),
              last = pgm_read_byte(&gfxFont->last);
      if ((c >= first) && (c <= last)) { // Char present in this font?
        GFXglyph *glyph = pgm_read_glyph_ptr(gfxFont, c - first);
        uint8_t gw = pgm_read_byte(&glyph->width),
                gh = pgm_read_byte(&glyph->height),
                xa = pgm_read_byte(&glyph->xAdvance);
        int8_t xo = pgm_read_byte(&glyph->xOffset),
               yo = pgm_read_byte(&glyph->yOffset);
        if (wrap && ((*x + (((int16_t)xo + gw) * textsize_x)) > _width)) {
          *x = 0; // Reset x to zero, advance y by one line
          *y += textsize_y * (uint8_t)pgm_read_byte(&gfxFont->yAdvance);
        }
        int16_t tsx = (int16_t)textsize_x, tsy = (int16_t)textsize_y,
                x1 = *x + xo * tsx, y1 = *y + yo * tsy, x2 = x1 + gw * tsx - 1,
                y2 = y1 + gh * tsy - 1;
        if (x1 < *minx)
          *minx = x1;
        if (y1 < *miny)
          *miny = y1;
        if (x2 > *maxx)
          *maxx = x2;
        if (y2 > *maxy)
          *maxy = y2;
        *x += xa * tsx;
      }
    }

  } else { // Default font

    if (c == '\n') {        // Newline?
      *x = 0;               // Reset x to zero,
      *y += textsize_y * 8; // advance y one line
      // min/max x/y unchaged -- that waits for next 'normal' character
    } else if (c != '\r') { // Normal char; ignore carriage returns
      if (wrap && ((*x + textsize_x * 6) > _width)) { // Off right?
        *x = 0;                                       // Reset x to zero,
        *y += textsize_y * 8;                         // advance y one line
      }
      int x2 = *x + textsize_x * 6 - 1, // Lower-right pixel of char
          y2 = *y + textsize_y * 8 - 1;
      if (x2 > *maxx)
        *maxx = x2; // Track max x, y
      if (y2 > *maxy)
        *maxy = y2;
      if (*x < *minx)
        *minx = *x; // Track min x, y
      if (*y < *miny)
        *miny = *y;
      *x += textsize_x * 6; // Advance x one char
    }
  }
}

/**************************************************************************/
/*!
    @brief    Helper to determine size of a string with current font/size. Pass
   string and a cursor position, returns UL corner and W,H.
    @param    str     The ascii string to measure
    @param    x       The current cursor X
    @param    y       The current cursor Y
    @param    x1      The boundary X coordinate, set by function
    @param    y1      The boundary Y coordinate, set by function
    @param    w      The boundary width, set by function
    @param    h      The boundary height, set by function
*/
/**************************************************************************/
void Adafruit_GFX::getTextBounds(const char *str, int16_t x, int16_t y,
                                 int16_t *x1, int16_t *y1, uint16_t *w,
                                 uint16_t *h) {
  uint8_t c; // Current character

  *x1 = x;
  *y1 = y;
  *w = *h = 0;

  int16_t minx = _width, miny = _height, maxx = -1, maxy = -1;

  while ((c = *str++))
    charBounds(c, &x, &y, &minx, &miny, &maxx, &maxy);

  if (maxx >= minx) {
    *x1 = minx;
    *w = maxx - minx + 1;
  }
  if (maxy >= miny) {
    *y1 = miny;
    *h = maxy - miny + 1;
  }
}

/**************************************************************************/
/*!
    @brief    Helper to determine size of a string with current font/size. Pass
   string and a cursor position, returns UL corner and W,H.
    @param    str    The ascii string to measure (as an arduino String() class)
    @param    x      The current cursor X
    @param    y      The current cursor Y
    @param    x1     The boundary X coordinate, set by function
    @param    y1     The boundary Y coordinate, set by function
    @param    w      The boundary width, set by function
    @param    h      The boundary height, set by function
*/
/**************************************************************************/
void Adafruit_GFX::getTextBounds(const String &str, int16_t x, int16_t y,
                                 int16_t *x1, int16_t *y1, uint16_t *w,
                                 uint16_t *h) {
  if (str.length() != 0) {
    getTextBounds(const_cast<char *>(str.c_str()), x, y, x1, y1, w, h);
  }
}

/**************************************************************************/
/*!
    @brief    Helper to determine size of a PROGMEM string with current
   font/size. Pass string and a cursor position, returns UL corner and W,H.
    @param    str     The flash-memory ascii string to measure
    @param    x       The current cursor X
    @param    y       The current cursor Y
    @param    x1      The boundary X coordinate, set by function
    @param    y1      The boundary Y coordinate, set by function
    @param    w      The boundary width, set by function
    @param    h      The boundary height, set by function
*/
/**************************************************************************/
void Adafruit_GFX::getTextBounds(const __FlashStringHelper *str, int16_t x,
                                 int16_t y, int16_t *x1, int16_t *y1,
                                 uint16_t *w, uint16_t *h) {
  uint8_t *s = (uint8_t *)str, c;

  *x1 = x;
  *y1 = y;
  *w = *h = 0;

  int16_t minx = _width, miny = _height, maxx = -1, maxy = -1;

  while ((c = pgm_read_byte(s++)))
    charBounds(c, &x, &y, &minx, &miny, &maxx, &maxy);

  if (maxx >= minx) {
    *x1 = minx;
    *w = maxx - minx + 1;
  }
  if (maxy >= miny) {
    *y1 = miny;
    *h = maxy - miny + 1;
  }
}

/**************************************************************************/
/*!
    @brief      Invert the display (ideally using built-in hardware command)
    @param   i  True if you want to invert, false to make 'normal'
*/
/**************************************************************************/
void Adafruit_GFX::invertDisplay(boolean i) {
  // Do nothing, must be subclassed if supported by hardware
}

/***************************************************************************/

/**************************************************************************/
/*!
   @brief    Create a simple drawn button UI element
*/
/**************************************************************************/
Adafruit_GFX_Button::Adafruit_GFX_Button(void) { _gfx = 0; }

/**************************************************************************/
/*!
   @brief    Initialize button with our desired color/size/settings
   @param    gfx     Pointer to our display so we can draw to it!
   @param    x       The X coordinate of the center of the button
   @param    y       The Y coordinate of the center of the button
   @param    w       Width of the buttton
   @param    h       Height of the buttton
   @param    outline  Color of the outline (16-bit 5-6-5 standard)
   @param    fill  Color of the button fill (16-bit 5-6-5 standard)
   @param    textcolor  Color of the button label (16-bit 5-6-5 standard)
   @param    label  Ascii string of the text inside the button
   @param    textsize The font magnification of the label text
*/
/**************************************************************************/
// Classic initButton() function: pass center & size
void Adafruit_GFX_Button::initButton(Adafruit_GFX *gfx, int16_t x, int16_t y,
                                     uint16_t w, uint16_t h, uint16_t outline,
                                     uint16_t fill, uint16_t textcolor,
                                     char *label, uint8_t textsize) {
  // Tweak arguments and pass to the newer initButtonUL() function...
  initButtonUL(gfx, x - (w / 2), y - (h / 2), w, h, outline, fill, textcolor,
               label, textsize);
}

/**************************************************************************/
/*!
   @brief    Initialize button with our desired color/size/settings
   @param    gfx     Pointer to our display so we can draw to it!
   @param    x       The X coordinate of the center of the button
   @param    y       The Y coordinate of the center of the button
   @param    w       Width of the buttton
   @param    h       Height of the buttton
   @param    outline  Color of the outline (16-bit 5-6-5 standard)
   @param    fill  Color of the button fill (16-bit 5-6-5 standard)
   @param    textcolor  Color of the button label (16-bit 5-6-5 standard)
   @param    label  Ascii string of the text inside the button
   @param    textsize_x The font magnification in X-axis of the label text
   @param    textsize_y The font magnification in Y-axis of the label text
*/
/**************************************************************************/
// Classic initButton() function: pass center & size
void Adafruit_GFX_Button::initButton(Adafruit_GFX *gfx, int16_t x, int16_t y,
                                     uint16_t w, uint16_t h, uint16_t outline,
                                     uint16_t fill, uint16_t textcolor,
                                     char *label, uint8_t textsize_x,
                                     uint8_t textsize_y) {
  // Tweak arguments and pass to the newer initButtonUL() function...
  initButtonUL(gfx, x - (w / 2), y - (h / 2), w, h, outline, fill, textcolor,
               label, textsize_x, textsize_y);
}

/**************************************************************************/
/*!
   @brief    Initialize button with our desired color/size/settings, with
   upper-left coordinates
   @param    gfx     Pointer to our display so we can draw to it!
   @param    x1       The X coordinate of the Upper-Left corner of the button
   @param    y1       The Y coordinate of the Upper-Left corner of the button
   @param    w       Width of the buttton
   @param    h       Height of the buttton
   @param    outline  Color of the outline (16-bit 5-6-5 standard)
   @param    fill  Color of the button fill (16-bit 5-6-5 standard)
   @param    textcolor  Color of the button label (16-bit 5-6-5 standard)
   @param    label  Ascii string of the text inside the button
   @param    textsize The font magnification of the label text
*/
/**************************************************************************/
void Adafruit_GFX_Button::initButtonUL(Adafruit_GFX *gfx, int16_t x1,
                                       int16_t y1, uint16_t w, uint16_t h,
                                       uint16_t outline, uint16_t fill,
                                       uint16_t textcolor, char *label,
                                       uint8_t textsize) {
  initButtonUL(gfx, x1, y1, w, h, outline, fill, textcolor, label, textsize,
               textsize);
}

/**************************************************************************/
/*!
   @brief    Initialize button with our desired color/size/settings, with
   upper-left coordinates
   @param    gfx     Pointer to our display so we can draw to it!
   @param    x1       The X coordinate of the Upper-Left corner of the button
   @param    y1       The Y coordinate of the Upper-Left corner of the button
   @param    w       Width of the buttton
   @param    h       Height of the buttton
   @param    outline  Color of the outline (16-bit 5-6-5 standard)
   @param    fill  Color of the button fill (16-bit 5-6-5 standard)
   @param    textcolor  Color of the button label (16-bit 5-6-5 standard)
   @param    label  Ascii string of the text inside the button
   @param    textsize_x The font magnification in X-axis of the label text
   @param    textsize_y The font magnification in Y-axis of the label text
*/
/**************************************************************************/
void Adafruit_GFX_Button::initButtonUL(Adafruit_GFX *gfx, int16_t x1,
                                       int16_t y1, uint16_t w, uint16_t h,
                                       uint16_t outline, uint16_t fill,
                                       uint16_t textcolor, char *label,
                                       uint8_t textsize_x, uint8_t textsize_y) {
  _x1 = x1;
  _y1 = y1;
  _w = w;
  _h = h;
  _outlinecolor = outline;
  _fillcolor = fill;
  _textcolor = textcolor;
  _textsize_x = textsize_x;
  _textsize_y = textsize_y;
  _gfx = gfx;
  strncpy(_label, label, 9);
}

/**************************************************************************/
/*!
   @brief    Draw the button on the screen
   @param    inverted Whether to draw with fill/text swapped to indicate
   'pressed'
*/
/**************************************************************************/
void Adafruit_GFX_Button::drawButton(boolean inverted) {
  uint16_t fill, outline, text;

  if (!inverted) {
    fill = _fillcolor;
    outline = _outlinecolor;
    text = _textcolor;
  } else {
    fill = _textcolor;
    outline = _outlinecolor;
    text = _fillcolor;
  }

  uint8_t r = min(_w, _h) / 4; // Corner radius
  _gfx->fillRoundRect(_x1, _y1, _w, _h, r, fill);
  _gfx->drawRoundRect(_x1, _y1, _w, _h, r, outline);

  _gfx->setCursor(_x1 + (_w / 2) - (strlen(_label) * 3 * _textsize_x),
                  _y1 + (_h / 2) - (4 * _textsize_y));
  _gfx->setTextColor(text);
  _gfx->setTextSize(_textsize_x, _textsize_y);
  _gfx->print(_label);
}

/**************************************************************************/
/*!
    @brief    Helper to let us know if a coordinate is within the bounds of the
   button
    @param    x       The X coordinate to check
    @param    y       The Y coordinate to check
    @returns  True if within button graphics outline
*/
/**************************************************************************/
boolean Adafruit_GFX_Button::contains(int16_t x, int16_t y) {
  return ((x >= _x1) && (x < (int16_t)(_x1 + _w)) && (y >= _y1) &&
          (y < (int16_t)(_y1 + _h)));
}

/**************************************************************************/
/*!
   @brief    Query whether the button was pressed since we last checked state
   @returns  True if was not-pressed before, now is.
*/
/**************************************************************************/
boolean Adafruit_GFX_Button::justPressed() { return (currstate && !laststate); }

/**************************************************************************/
/*!
   @brief    Query whether the button was released since we last checked state
   @returns  True if was pressed before, now is not.
*/
/**************************************************************************/
boolean Adafruit_GFX_Button::justReleased() {
  return (!currstate && laststate);
}

// -------------------------------------------------------------------------

// GFXcanvas1, GFXcanvas8 and GFXcanvas16 (currently a WIP, don't get too
// comfy with the implementation) provide 1-, 8- and 16-bit offscreen
// canvases, the address of which can be passed to drawBitmap() or
// pushColors() (the latter appears only in a couple of GFX-subclassed TFT
// libraries at this time).  This is here mostly to help with the recently-
// added proportionally-spaced fonts; adds a way to refresh a section of the
// screen without a massive flickering clear-and-redraw...but maybe you'll
// find other uses too.  VERY RAM-intensive, since the buffer is in MCU
// memory and not the display driver...GXFcanvas1 might be minimally useful
// on an Uno-class board, but this and the others are much more likely to
// require at least a Mega or various recent ARM-type boards (recommended,
// as the text+bitmap draw can be pokey).  GFXcanvas1 requires 1 bit per
// pixel (rounded up to nearest byte per scanline), GFXcanvas8 is 1 byte
// per pixel (no scanline pad), and GFXcanvas16 uses 2 bytes per pixel (no
// scanline pad).
// NOT EXTENSIVELY TESTED YET.  MAY CONTAIN WORST BUGS KNOWN TO HUMANKIND.

/**************************************************************************/
/*!
   @brief    Instatiate a GFX 1-bit canvas context for graphics
   @param    w   Display width, in pixels
   @param    h   Display height, in pixels
*/
/**************************************************************************/
GFXcanvas1::GFXcanvas1(uint16_t w, uint16_t h) : Adafruit_GFX(w, h) {
  uint16_t bytes = ((w + 7) / 8) * h;
  if ((buffer = (uint8_t *)malloc(bytes))) {
    memset(buffer, 0, bytes);
  }
}

/**************************************************************************/
/*!
   @brief    Delete the canvas, free memory
*/
/**************************************************************************/
GFXcanvas1::~GFXcanvas1(void) {
  if (buffer)
    free(buffer);
}

/**************************************************************************/
/*!
    @brief  Draw a pixel to the canvas framebuffer
    @param  x     x coordinate
    @param  y     y coordinate
    @param  color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void GFXcanvas1::drawPixel(int16_t x, int16_t y, uint16_t color) {
#ifdef __AVR__
  // Bitmask tables of 0x80>>X and ~(0x80>>X), because X>>Y is slow on AVR
  static const uint8_t PROGMEM GFXsetBit[] = {0x80, 0x40, 0x20, 0x10,
                                              0x08, 0x04, 0x02, 0x01},
                               GFXclrBit[] = {0x7F, 0xBF, 0xDF, 0xEF,
                                              0xF7, 0xFB, 0xFD, 0xFE};
#endif

  if (buffer) {
    if ((x < 0) || (y < 0) || (x >= _width) || (y >= _height))
      return;

    int16_t t;
    switch (rotation) {
    case 1:
      t = x;
      x = WIDTH - 1 - y;
      y = t;
      break;
    case 2:
      x = WIDTH - 1 - x;
      y = HEIGHT - 1 - y;
      break;
    case 3:
      t = x;
      x = y;
      y = HEIGHT - 1 - t;
      break;
    }

    uint8_t *ptr = &buffer[(x / 8) + y * ((WIDTH + 7) / 8)];
#ifdef __AVR__
    if (color)
      *ptr |= pgm_read_byte(&GFXsetBit[x & 7]);
    else
      *ptr &= pgm_read_byte(&GFXclrBit[x & 7]);
#else
    if (color)
      *ptr |= 0x80 >> (x & 7);
    else
      *ptr &= ~(0x80 >> (x & 7));
#endif
  }
}

/**************************************************************************/
/*!
    @brief  Fill the framebuffer completely with one color
    @param  color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void GFXcanvas1::fillScreen(uint16_t color) {
  if (buffer) {
    uint16_t bytes = ((WIDTH + 7) / 8) * HEIGHT;
    memset(buffer, color ? 0xFF : 0x00, bytes);
  }
}

/**************************************************************************/
/*!
   @brief    Instatiate a GFX 8-bit canvas context for graphics
   @param    w   Display width, in pixels
   @param    h   Display height, in pixels
*/
/**************************************************************************/
GFXcanvas8::GFXcanvas8(uint16_t w, uint16_t h) : Adafruit_GFX(w, h) {
  uint32_t bytes = w * h;
  if ((buffer = (uint8_t *)malloc(bytes))) {
    memset(buffer, 0, bytes);
  }
}

/**************************************************************************/
/*!
   @brief    Delete the canvas, free memory
*/
/**************************************************************************/
GFXcanvas8::~GFXcanvas8(void) {
  if (buffer)
    free(buffer);
}

/**************************************************************************/
/*!
    @brief  Draw a pixel to the canvas framebuffer
    @param  x   x coordinate
    @param  y   y coordinate
    @param  color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void GFXcanvas8::drawPixel(int16_t x, int16_t y, uint16_t color) {
  if (buffer) {
    if ((x < 0) || (y < 0) || (x >= _width) || (y >= _height))
      return;

    int16_t t;
    switch (rotation) {
    case 1:
      t = x;
      x = WIDTH - 1 - y;
      y = t;
      break;
    case 2:
      x = WIDTH - 1 - x;
      y = HEIGHT - 1 - y;
      break;
    case 3:
      t = x;
      x = y;
      y = HEIGHT - 1 - t;
      break;
    }

    buffer[x + y * WIDTH] = color;
  }
}

/**************************************************************************/
/*!
    @brief  Fill the framebuffer completely with one color
    @param  color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void GFXcanvas8::fillScreen(uint16_t color) {
  if (buffer) {
    memset(buffer, color, WIDTH * HEIGHT);
  }
}

void GFXcanvas8::writeFastHLine(int16_t x, int16_t y, int16_t w,
                                uint16_t color) {

  if ((x >= _width) || (y < 0) || (y >= _height))
    return;
  int16_t x2 = x + w - 1;
  if (x2 < 0)
    return;

  // Clip left/right
  if (x < 0) {
    x = 0;
    w = x2 + 1;
  }
  if (x2 >= _width)
    w = _width - x;

  int16_t t;
  switch (rotation) {
  case 1:
    t = x;
    x = WIDTH - 1 - y;
    y = t;
    break;
  case 2:
    x = WIDTH - 1 - x;
    y = HEIGHT - 1 - y;
    break;
  case 3:
    t = x;
    x = y;
    y = HEIGHT - 1 - t;
    break;
  }

  memset(buffer + y * WIDTH + x, color, w);
}

/**************************************************************************/
/*!
   @brief    Instatiate a GFX 16-bit canvas context for graphics
   @param    w   Display width, in pixels
   @param    h   Display height, in pixels
*/
/**************************************************************************/
GFXcanvas16::GFXcanvas16(uint16_t w, uint16_t h) : Adafruit_GFX(w, h) {
  uint32_t bytes = w * h * 2;
  if ((buffer = (uint16_t *)malloc(bytes))) {
    memset(buffer, 0, bytes);
  }
}

/**************************************************************************/
/*!
   @brief    Delete the canvas, free memory
*/
/**************************************************************************/
GFXcanvas16::~GFXcanvas16(void) {
  if (buffer)
    free(buffer);
}

/**************************************************************************/
/*!
    @brief  Draw a pixel to the canvas framebuffer
    @param  x   x coordinate
    @param  y   y coordinate
    @param  color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void GFXcanvas16::drawPixel(int16_t x, int16_t y, uint16_t color) {
  if (buffer) {
    if ((x < 0) || (y < 0) || (x >= _width) || (y >= _height))
      return;

    int16_t t;
    switch (rotation) {
    case 1:
      t = x;
      x = WIDTH - 1 - y;
      y = t;
      break;
    case 2:
      x = WIDTH - 1 - x;
      y = HEIGHT - 1 - y;
      break;
    case 3:
      t = x;
      x = y;
      y = HEIGHT - 1 - t;
      break;
    }

    buffer[x + y * WIDTH] = color;
  }
}

/**************************************************************************/
/*!
    @brief  Fill the framebuffer completely with one color
    @param  color 16-bit 5-6-5 Color to fill with
*/
/**************************************************************************/
void GFXcanvas16::fillScreen(uint16_t color) {
  if (buffer) {
    uint8_t hi = color >> 8, lo = color & 0xFF;
    if (hi == lo) {
      memset(buffer, lo, WIDTH * HEIGHT * 2);
    } else {
      uint32_t i, pixels = WIDTH * HEIGHT;
      for (i = 0; i < pixels; i++)
        buffer[i] = color;
    }
  }
}

/**************************************************************************/
/*!
    @brief  Reverses the "endian-ness" of each 16-bit pixel within the
            canvas; little-endian to big-endian, or big-endian to little.
            Most microcontrollers (such as SAMD) are little-endian, while
            most displays tend toward big-endianness. All the drawing
            functions (including RGB bitmap drawing) take care of this
            automatically, but some specialized code (usually involving
            DMA) can benefit from having pixel data already in the
            display-native order. Note that this does NOT convert to a
            SPECIFIC endian-ness, it just flips the bytes within each word.
*/
/**************************************************************************/
void GFXcanvas16::byteSwap(void) {
  if (buffer) {
    uint32_t i, pixels = WIDTH * HEIGHT;
    for (i = 0; i < pixels; i++)
      buffer[i] = __builtin_bswap16(buffer[i]);
  }
}

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

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

751

752

753

754

755

756

757

758

759

760

761

762

763

764

765

766

767

768

769

770

771

772

773

774

775

776

777

778

779

780

781

782

783

784

785

786

787

788

789

790

791

792

793

794

795

796

797

798

799

800

801

802

803

804

805

806

807

808

809

810

811

812

813

814

815

816

817

818

819

820

821

822

823

824

825

826

827

828

829

830

831

832

833

834

835

836

837

838

839

840

841

842

843

844

845

846

847

848

849

850

851

852

853

854

855

856

857

858

859

860

861

862

863

864

865

866

867

868

869

870

871

872

873

874

875

876

877

878

879

880

881

882

883

884

885

886

887

888

889

890

891

892

893

894

895

896

897

898

899

900

901

902

903

904

905

906

907

908

909

910

911

912

913

914

915

916

917

918

919

920

921

922

923

924

925

926

927

928

929

930

931

932

933

934

935

936

937

938

939

940

941

942

943

944

945

946

947

948

949

950

951

952

953

954

955

956

957

958

959

960

961

962

963

964

965

966

967

968

969

970

971

972

973

974

975

976

977

978

979

980

981

982

983

984

985

986

987

988

989

990

991

992

993

994

995

996

997

998

999

1000

1001

1002

1003

1004

1005

1006

1007

1008

1009

1010

1011

1012

1013

1014

1015

1016

1017

1018

1019

1020

1021

1022

1023

1024

1025

1026

1027

1028

1029

1030

1031

1032

1033

1034

1035

1036

1037

1038

1039

1040

1041

1042

1043

1044

1045

1046

1047

1048

1049

1050

1051

1052

1053

1054

1055

1056

1057

1058

1059

1060

1061

1062

1063

1064

1065

1066

1067

1068

1069

1070

1071

1072

1073

1074

1075

1076

1077

1078

1079

1080

1081

1082

1083

1084

1085

1086

1087

1088

1089

1090

1091

1092

1093

1094

1095

1096

1097

1098

1099

1100

1101

1102

1103

1104

1105

1106

1107

1108

1109

1110

1111

1112

1113

1114

1115

1116

1117

1118

1119

1120

1121

1122

1123

1124

1125

1126

1127

1128

1129

1130

1131

1132

1133

1134

1135

1136

1137

1138

1139

1140

1141

1142

1143

1144

1145

1146

1147

1148

1149

1150

1151

1152

1153

1154

1155

1156

1157

1158

1159

1160

1161

1162

1163

1164

1165

1166

1167

1168

1169

1170

1171

1172

1173

1174

1175

1176

1177

1178

1179

1180

1181

1182

1183

1184

1185

1186

1187

1188

1189

1190

1191

1192

1193

1194

1195

1196

1197

1198

1199

1200

1201

1202

1203

1204

1205

1206

1207

1208

1209

1210

1211

1212

1213

1214

1215

1216

1217

1218

1219

1220

1221

1222

1223

1224

1225

1226

1227

1228

1229

1230

1231

1232

1233

1234

1235

1236

1237

1238

1239

1240

1241

1242

1243

1244

1245

1246

1247

1248

1249

1250

1251

1252

1253

1254

1255

1256

1257

1258

1259

1260

1261

1262

1263

1264

1265

1266

1267

1268

1269

1270

1271

1272

1273

1274

1275

1276

1277

1278

1279

1280

1281

1282

1283

1284

1285

1286

1287

1288

1289

1290

1291

1292

1293

1294

1295

1296

1297

1298

1299

1300

1301

1302

1303

1304

1305

1306

1307

1308

1309

1310

1311

1312

1313

1314

1315

1316

1317

1318

1319

1320

1321

1322

1323

1324

1325

1326

1327

1328

1329

1330

1331

1332

1333

1334

1335

1336

1337

1338

1339

1340

1341

1342

1343

1344

1345

1346

1347

1348

1349

1350

1351

1352

1353

1354

1355

1356

1357

1358

1359

1360

1361

1362

1363

1364

1365

1366

1367

1368

1369

1370

1371

1372

1373

1374

1375

1376

1377

1378

1379

1380

1381

1382

1383

1384

1385

1386

1387

1388

1389

1390

1391

1392

1393

1394

1395

1396

1397

1398

1399

1400

1401

1402

1403

1404

1405

1406

1407

1408

1409

1410

1411

1412

1413

1414

1415

1416

1417

1418

1419

1420

1421

1422

1423

1424

1425

1426

1427

1428

1429

1430

1431

1432

1433

1434

1435

1436

1437

1438

1439

1440

1441

1442

1443

1444

1445

1446

1447

1448

1449

1450

1451

1452

1453

1454

1455

1456

1457

1458

1459

1460

1461

1462

1463

1464

1465

1466

1467

1468

1469

1470

1471

1472

1473

1474

1475

1476

1477

1478

1479

1480

1481

1482

1483

1484

1485

1486

1487

1488

1489

1490

1491

1492

1493

1494

1495

1496

1497

1498

1499

1500

1501

1502

1503

1504

1505

1506

1507

1508

1509

1510

1511

1512

1513

1514

1515

1516

1517

1518

1519

1520

1521

1522

1523

1524

1525

1526

1527

1528

1529

1530

1531

1532

1533

1534

1535

1536

1537

1538

1539

1540

1541

1542

1543

1544

1545

1546

1547

1548

1549

1550

1551

1552

1553

1554

1555

1556

1557

1558

1559

1560

1561

1562

1563

1564

1565

1566

1567

1568

1569

1570

1571

1572

1573

1574

1575

1576

1577

1578

1579

1580

1581

1582

1583

1584

1585

1586

1587

1588

1589

1590

1591

1592

1593

1594

1595

1596

1597

1598

1599

1600

1601

1602

1603

1604

1605

1606

1607

1608

1609

1610

1611

1612

1613

1614

1615

1616

1617

1618

1619

1620

1621

1622

1623

1624

1625

1626

1627

1628

1629

1630

1631

1632

1633

1634

1635

1636

1637

1638

1639

1640

1641

1642

1643

1644

1645

1646

1647

1648

1649

1650

1651

1652

1653

1654

1655

1656

1657

1658

1659

1660

1661

1662

1663

1664

1665

1666

1667

1668

1669

1670

1671

1672

1673

1674

1675

1676

1677

1678

1679

1680

1681

1682

1683

1684

1685

1686

1687

1688

1689

1690

1691

1692

1693

1694

1695

1696

1697

1698

1699

1700

1701

1702

1703

1704

1705

1706

1707

1708

1709

1710

1711

1712

1713

1714

1715

1716

1717

1718

1719

1720

1721

1722

1723

1724

1725

1726

1727

1728

1729

1730

1731

1732

1733

1734

1735

1736

1737

1738

1739

1740

1741

1742

1743

1744

1745

1746

1747

1748

1749

1750

1751

1752

1753

1754

1755

1756

1757

1758

1759

1760

1761

1762

1763

1764

1765

1766

1767

1768

1769

1770

1771

1772

1773

1774

1775

1776

1777

1778

1779

1780

1781

1782

1783

1784

1785

1786

1787

1788

1789

1790

1791

1792

1793

1794

1795

1796

1797

1798

1799

1800

1801

1802

1803

1804

1805

1806

1807

1808

1809

1810

1811

1812

1813

1814

1815

1816

1817

1818

1819

1820

1821

1822

1823

1824

1825

1826

1827

1828

1829

1830

1831

1832

1833

1834

1835

1836

1837

1838

1839

1840

1841

1842

1843

1844

1845

1846

1847

1848

1849

1850

1851

1852

1853

1854

1855

1856

1857

1858

1859

1860

1861

1862

1863

1864

1865

1866

1867

1868

1869

1870

1871

1872

1873

1874

1875

1876

1877

1878

1879

1880

1881

1882

1883

1884

1885

1886

1887

1888

1889

1890

1891

1892

1893

1894

1895

1896

1897

1898

1899

1900

1901

1902

1903

1904

1905

1906

1907

1908

1909

1910

1911

1912

1913

1914

1915

1916

1917

1918

1919

1920

1921

1922

1923

1924

1925

1926

1927

1928

1929

1930

1931

1932

1933

1934

1935

1936

1937

1938

1939

1940

1941

1942

1943

1944

1945

1946

1947

1948

1949

1950

1951

1952

1953

1954

1955

1956

1957

1958

1959

1960

1961

1962

1963

1964

1965

1966

1967

1968

1969

1970

1971

1972

1973

1974

1975

1976

1977

1978

1979

1980

1981

1982

1983

1984

1985

1986

1987

1988

1989

1990

1991

1992

1993

1994

1995

1996

1997

1998

1999

2000

2001

2002

2003

2004

2005

2006

2007

2008

2009

2010

2011

2012

2013

2014

2015

2016

2017

2018

2019

2020

2021

2022

2023

2024

2025

2026

2027

2028

2029

2030

2031

2032

2033

2034

2035

2036

This is the core graphics library for all our displays, providing a common

set of graphics primitives (points, lines, circles, etc.). It needs to be

paired with a hardware-specific library for each display device we carry

(to handle the lower-level functions).

Adafruit invests time and resources providing this open source code, please

support Adafruit & open-source hardware by purchasing products from Adafruit!

Redistribution and use in source and binary forms, with or without

modification, are permitted provided that the following conditions are met:

- Redistributions of source code must retain the above copyright notice,

this list of conditions and the following disclaimer.

- Redistributions in binary form must reproduce the above copyright notice,

this list of conditions and the following disclaimer in the documentation

and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE

LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

POSSIBILITY OF SUCH DAMAGE.

#include "Adafruit_GFX.h"

#include "glcdfont.c"

#ifdef __AVR__

#include <avr/pgmspace.h>

#elif defined(ESP8266) || defined(ESP32)

#include <pgmspace.h>

#endif

// Many (but maybe not all) non-AVR board installs define macros

// for compatibility with existing PROGMEM-reading AVR code.

// Do our own checks and defines here for good measure...

#ifndef pgm_read_byte

#define pgm_read_byte(addr) (*(const unsigned char *)(addr))

#endif

#ifndef pgm_read_word

#define pgm_read_word(addr) (*(const unsigned short *)(addr))

#endif

#ifndef pgm_read_dword

#define pgm_read_dword(addr) (*(const unsigned long *)(addr))

#endif

// Pointers are a peculiar case...typically 16-bit on AVR boards,

// 32 bits elsewhere. Try to accommodate both...

#if !defined(__INT_MAX__) || (__INT_MAX__ > 0xFFFF)

#define pgm_read_pointer(addr) ((void *)pgm_read_dword(addr))

#else

#define pgm_read_pointer(addr) ((void *)pgm_read_word(addr))

#endif

inline GFXglyph *pgm_read_glyph_ptr(const GFXfont *gfxFont, uint8_t c) {

#ifdef __AVR__

return &(((GFXglyph *)pgm_read_pointer(&gfxFont->glyph))[c]);

#else

// expression in __AVR__ section may generate "dereferencing type-punned

// pointer will break strict-aliasing rules" warning In fact, on other

// platforms (such as STM32) there is no need to do this pointer magic as

// program memory may be read in a usual way So expression may be simplified

return gfxFont->glyph + c;

#endif //__AVR__

}

inline uint8_t *pgm_read_bitmap_ptr(const GFXfont *gfxFont) {

#ifdef __AVR__

return (uint8_t *)pgm_read_pointer(&gfxFont->bitmap);

#else

// expression in __AVR__ section generates "dereferencing type-punned pointer

// will break strict-aliasing rules" warning In fact, on other platforms (such

// as STM32) there is no need to do this pointer magic as program memory may

// be read in a usual way So expression may be simplified

return gfxFont->bitmap;

#endif //__AVR__

}

#ifndef min

#define min(a, b) (((a) < (b)) ? (a) : (b))

#endif

#ifndef _swap_int16_t

#define _swap_int16_t(a, b) \

{ \

int16_t t = a; \

a = b; \

b = t; \

}

#endif

/**************************************************************************/

/*!

@brief Instatiate a GFX context for graphics! Can only be done by a

superclass

@param w Display width, in pixels

@param h Display height, in pixels

/**************************************************************************/

Adafruit_GFX::Adafruit_GFX(int16_t w, int16_t h) : WIDTH(w), HEIGHT(h) {

_width = WIDTH;

_height = HEIGHT;

rotation = 0;

cursor_y = cursor_x = 0;

textsize_x = textsize_y = 1;

textcolor = textbgcolor = 0xFFFF;

wrap = true;

_cp437 = false;

gfxFont = NULL;

}

/**************************************************************************/

/*!

@brief Write a line. Bresenham's algorithm - thx wikpedia

@param x0 Start point x coordinate

@param y0 Start point y coordinate

@param x1 End point x coordinate

@param y1 End point y coordinate

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::writeLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1,

uint16_t color) {

#if defined(ESP8266)

yield();

#endif

int16_t steep = abs(y1 - y0) > abs(x1 - x0);

if (steep) {

_swap_int16_t(x0, y0);

_swap_int16_t(x1, y1);

}

if (x0 > x1) {

_swap_int16_t(x0, x1);

_swap_int16_t(y0, y1);

}

int16_t dx, dy;

dx = x1 - x0;

dy = abs(y1 - y0);

int16_t err = dx / 2;

int16_t ystep;

if (y0 < y1) {

ystep = 1;

} else {

ystep = -1;

}

for (; x0 <= x1; x0++) {

if (steep) {

writePixel(y0, x0, color);

} else {

writePixel(x0, y0, color);

}

err -= dy;

if (err < 0) {

y0 += ystep;

err += dx;

}

/**************************************************************************/

/*!

@brief Start a display-writing routine, overwrite in subclasses.

/**************************************************************************/

void Adafruit_GFX::startWrite() {}

/**************************************************************************/

/*!

@brief Write a pixel, overwrite in subclasses if startWrite is defined!

@param x x coordinate

@param y y coordinate

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::writePixel(int16_t x, int16_t y, uint16_t color) {

drawPixel(x, y, color);

}

/**************************************************************************/

/*!

@brief Write a perfectly vertical line, overwrite in subclasses if

startWrite is defined!

@param x Top-most x coordinate

@param y Top-most y coordinate

@param h Height in pixels

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::writeFastVLine(int16_t x, int16_t y, int16_t h,

uint16_t color) {

// Overwrite in subclasses if startWrite is defined!

// Can be just writeLine(x, y, x, y+h-1, color);

// or writeFillRect(x, y, 1, h, color);

drawFastVLine(x, y, h, color);

}

/**************************************************************************/

/*!

@brief Write a perfectly horizontal line, overwrite in subclasses if

startWrite is defined!

@param x Left-most x coordinate

@param y Left-most y coordinate

@param w Width in pixels

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::writeFastHLine(int16_t x, int16_t y, int16_t w,

uint16_t color) {

// Overwrite in subclasses if startWrite is defined!

// Example: writeLine(x, y, x+w-1, y, color);

// or writeFillRect(x, y, w, 1, color);

drawFastHLine(x, y, w, color);

}

/**************************************************************************/

/*!

@brief Write a rectangle completely with one color, overwrite in

subclasses if startWrite is defined!

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param w Width in pixels

@param h Height in pixels

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::writeFillRect(int16_t x, int16_t y, int16_t w, int16_t h,

uint16_t color) {

// Overwrite in subclasses if desired!

fillRect(x, y, w, h, color);

}

/**************************************************************************/

/*!

@brief End a display-writing routine, overwrite in subclasses if

startWrite is defined!

/**************************************************************************/

void Adafruit_GFX::endWrite() {}

/**************************************************************************/

/*!

@brief Draw a perfectly vertical line (this is often optimized in a

subclass!)

@param x Top-most x coordinate

@param y Top-most y coordinate

@param h Height in pixels

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::drawFastVLine(int16_t x, int16_t y, int16_t h,

uint16_t color) {

startWrite();

writeLine(x, y, x, y + h - 1, color);

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a perfectly horizontal line (this is often optimized in a

subclass!)

@param x Left-most x coordinate

@param y Left-most y coordinate

@param w Width in pixels

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::drawFastHLine(int16_t x, int16_t y, int16_t w,

uint16_t color) {

startWrite();

writeLine(x, y, x + w - 1, y, color);

endWrite();

}

/**************************************************************************/

/*!

@brief Fill a rectangle completely with one color. Update in subclasses if

desired!

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param w Width in pixels

@param h Height in pixels

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::fillRect(int16_t x, int16_t y, int16_t w, int16_t h,

uint16_t color) {

startWrite();

for (int16_t i = x; i < x + w; i++) {

writeFastVLine(i, y, h, color);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Fill the screen completely with one color. Update in subclasses if

desired!

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::fillScreen(uint16_t color) {

fillRect(0, 0, _width, _height, color);

}

/**************************************************************************/

/*!

@brief Draw a line

@param x0 Start point x coordinate

@param y0 Start point y coordinate

@param x1 End point x coordinate

@param y1 End point y coordinate

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1,

uint16_t color) {

// Update in subclasses if desired!

if (x0 == x1) {

if (y0 > y1)

_swap_int16_t(y0, y1);

drawFastVLine(x0, y0, y1 - y0 + 1, color);

} else if (y0 == y1) {

if (x0 > x1)

_swap_int16_t(x0, x1);

drawFastHLine(x0, y0, x1 - x0 + 1, color);

} else {

startWrite();

writeLine(x0, y0, x1, y1, color);

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a circle outline

@param x0 Center-point x coordinate

@param y0 Center-point y coordinate

@param r Radius of circle

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::drawCircle(int16_t x0, int16_t y0, int16_t r,

uint16_t color) {

#if defined(ESP8266)

yield();

#endif

int16_t f = 1 - r;

int16_t ddF_x = 1;

int16_t ddF_y = -2 * r;

int16_t x = 0;

int16_t y = r;

startWrite();

writePixel(x0, y0 + r, color);

writePixel(x0, y0 - r, color);

writePixel(x0 + r, y0, color);

writePixel(x0 - r, y0, color);

while (x < y) {

if (f >= 0) {

y--;

ddF_y += 2;

f += ddF_y;

}

x++;

ddF_x += 2;

f += ddF_x;

writePixel(x0 + x, y0 + y, color);

writePixel(x0 - x, y0 + y, color);

writePixel(x0 + x, y0 - y, color);

writePixel(x0 - x, y0 - y, color);

writePixel(x0 + y, y0 + x, color);

writePixel(x0 - y, y0 + x, color);

writePixel(x0 + y, y0 - x, color);

writePixel(x0 - y, y0 - x, color);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Quarter-circle drawer, used to do circles and roundrects

@param x0 Center-point x coordinate

@param y0 Center-point y coordinate

@param r Radius of circle

@param cornername Mask bit #1 or bit #2 to indicate which quarters of

the circle we're doing

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::drawCircleHelper(int16_t x0, int16_t y0, int16_t r,

uint8_t cornername, uint16_t color) {

int16_t f = 1 - r;

int16_t ddF_x = 1;

int16_t ddF_y = -2 * r;

int16_t x = 0;

int16_t y = r;

while (x < y) {

if (f >= 0) {

y--;

ddF_y += 2;

f += ddF_y;

}

x++;

ddF_x += 2;

f += ddF_x;

if (cornername & 0x4) {

writePixel(x0 + x, y0 + y, color);

writePixel(x0 + y, y0 + x, color);

}

if (cornername & 0x2) {

writePixel(x0 + x, y0 - y, color);

writePixel(x0 + y, y0 - x, color);

}

if (cornername & 0x8) {

writePixel(x0 - y, y0 + x, color);

writePixel(x0 - x, y0 + y, color);

}

if (cornername & 0x1) {

writePixel(x0 - y, y0 - x, color);

writePixel(x0 - x, y0 - y, color);

}

/**************************************************************************/

/*!

@brief Draw a circle with filled color

@param x0 Center-point x coordinate

@param y0 Center-point y coordinate

@param r Radius of circle

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::fillCircle(int16_t x0, int16_t y0, int16_t r,

uint16_t color) {

startWrite();

writeFastVLine(x0, y0 - r, 2 * r + 1, color);

fillCircleHelper(x0, y0, r, 3, 0, color);

endWrite();

}

/**************************************************************************/

/*!

@brief Quarter-circle drawer with fill, used for circles and roundrects

@param x0 Center-point x coordinate

@param y0 Center-point y coordinate

@param r Radius of circle

@param corners Mask bits indicating which quarters we're doing

@param delta Offset from center-point, used for round-rects

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void Adafruit_GFX::fillCircleHelper(int16_t x0, int16_t y0, int16_t r,

uint8_t corners, int16_t delta,

uint16_t color) {

int16_t f = 1 - r;

int16_t ddF_x = 1;

int16_t ddF_y = -2 * r;

int16_t x = 0;

int16_t y = r;

int16_t px = x;

int16_t py = y;

delta++; // Avoid some +1's in the loop

while (x < y) {

if (f >= 0) {

y--;

ddF_y += 2;

f += ddF_y;

}

x++;

ddF_x += 2;

f += ddF_x;

// These checks avoid double-drawing certain lines, important

// for the SSD1306 library which has an INVERT drawing mode.

if (x < (y + 1)) {

if (corners & 1)

writeFastVLine(x0 + x, y0 - y, 2 * y + delta, color);

if (corners & 2)

writeFastVLine(x0 - x, y0 - y, 2 * y + delta, color);

}

if (y != py) {

if (corners & 1)

writeFastVLine(x0 + py, y0 - px, 2 * px + delta, color);

if (corners & 2)

writeFastVLine(x0 - py, y0 - px, 2 * px + delta, color);

py = y;

}

px = x;

}

/**************************************************************************/

/*!

@brief Draw a rectangle with no fill color

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param w Width in pixels

@param h Height in pixels

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::drawRect(int16_t x, int16_t y, int16_t w, int16_t h,

uint16_t color) {

startWrite();

writeFastHLine(x, y, w, color);

writeFastHLine(x, y + h - 1, w, color);

writeFastVLine(x, y, h, color);

writeFastVLine(x + w - 1, y, h, color);

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a rounded rectangle with no fill color

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param w Width in pixels

@param h Height in pixels

@param r Radius of corner rounding

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::drawRoundRect(int16_t x, int16_t y, int16_t w, int16_t h,

int16_t r, uint16_t color) {

int16_t max_radius = ((w < h) ? w : h) / 2; // 1/2 minor axis

if (r > max_radius)

r = max_radius;

// smarter version

startWrite();

writeFastHLine(x + r, y, w - 2 * r, color); // Top

writeFastHLine(x + r, y + h - 1, w - 2 * r, color); // Bottom

writeFastVLine(x, y + r, h - 2 * r, color); // Left

writeFastVLine(x + w - 1, y + r, h - 2 * r, color); // Right

// draw four corners

drawCircleHelper(x + r, y + r, r, 1, color);

drawCircleHelper(x + w - r - 1, y + r, r, 2, color);

drawCircleHelper(x + w - r - 1, y + h - r - 1, r, 4, color);

drawCircleHelper(x + r, y + h - r - 1, r, 8, color);

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a rounded rectangle with fill color

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param w Width in pixels

@param h Height in pixels

@param r Radius of corner rounding

@param color 16-bit 5-6-5 Color to draw/fill with

/**************************************************************************/

void Adafruit_GFX::fillRoundRect(int16_t x, int16_t y, int16_t w, int16_t h,

int16_t r, uint16_t color) {

int16_t max_radius = ((w < h) ? w : h) / 2; // 1/2 minor axis

if (r > max_radius)

r = max_radius;

// smarter version

startWrite();

writeFillRect(x + r, y, w - 2 * r, h, color);

// draw four corners

fillCircleHelper(x + w - r - 1, y + r, r, 1, h - 2 * r - 1, color);

fillCircleHelper(x + r, y + r, r, 2, h - 2 * r - 1, color);

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a triangle with no fill color

@param x0 Vertex #0 x coordinate

@param y0 Vertex #0 y coordinate

@param x1 Vertex #1 x coordinate

@param y1 Vertex #1 y coordinate

@param x2 Vertex #2 x coordinate

@param y2 Vertex #2 y coordinate

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::drawTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1,

int16_t x2, int16_t y2, uint16_t color) {

drawLine(x0, y0, x1, y1, color);

drawLine(x1, y1, x2, y2, color);

drawLine(x2, y2, x0, y0, color);

}

/**************************************************************************/

/*!

@brief Draw a triangle with color-fill

@param x0 Vertex #0 x coordinate

@param y0 Vertex #0 y coordinate

@param x1 Vertex #1 x coordinate

@param y1 Vertex #1 y coordinate

@param x2 Vertex #2 x coordinate

@param y2 Vertex #2 y coordinate

@param color 16-bit 5-6-5 Color to fill/draw with

/**************************************************************************/

void Adafruit_GFX::fillTriangle(int16_t x0, int16_t y0, int16_t x1, int16_t y1,

int16_t x2, int16_t y2, uint16_t color) {

int16_t a, b, y, last;

// Sort coordinates by Y order (y2 >= y1 >= y0)

if (y0 > y1) {

_swap_int16_t(y0, y1);

_swap_int16_t(x0, x1);

}

if (y1 > y2) {

_swap_int16_t(y2, y1);

_swap_int16_t(x2, x1);

}

if (y0 > y1) {

_swap_int16_t(y0, y1);

_swap_int16_t(x0, x1);

}

startWrite();

if (y0 == y2) { // Handle awkward all-on-same-line case as its own thing

a = b = x0;

if (x1 < a)

a = x1;

else if (x1 > b)

b = x1;

if (x2 < a)

a = x2;

else if (x2 > b)

b = x2;

writeFastHLine(a, y0, b - a + 1, color);

endWrite();

return;

}

int16_t dx01 = x1 - x0, dy01 = y1 - y0, dx02 = x2 - x0, dy02 = y2 - y0,

dx12 = x2 - x1, dy12 = y2 - y1;

int32_t sa = 0, sb = 0;

// For upper part of triangle, find scanline crossings for segments

// 0-1 and 0-2. If y1=y2 (flat-bottomed triangle), the scanline y1

// is included here (and second loop will be skipped, avoiding a /0

// error there), otherwise scanline y1 is skipped here and handled

// in the second loop...which also avoids a /0 error here if y0=y1

// (flat-topped triangle).

if (y1 == y2)

last = y1; // Include y1 scanline

else

last = y1 - 1; // Skip it

for (y = y0; y <= last; y++) {

a = x0 + sa / dy01;

b = x0 + sb / dy02;

sa += dx01;

sb += dx02;

/* longhand:

a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);

b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);

if (a > b)

_swap_int16_t(a, b);

writeFastHLine(a, y, b - a + 1, color);

}

// For lower part of triangle, find scanline crossings for segments

// 0-2 and 1-2. This loop is skipped if y1=y2.

sa = (int32_t)dx12 * (y - y1);

sb = (int32_t)dx02 * (y - y0);

for (; y <= y2; y++) {

a = x1 + sa / dy12;

b = x0 + sb / dy02;

sa += dx12;

sb += dx02;

/* longhand:

a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);

b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);

if (a > b)

_swap_int16_t(a, b);

writeFastHLine(a, y, b - a + 1, color);

}

endWrite();

}

// BITMAP / XBITMAP / GRAYSCALE / RGB BITMAP FUNCTIONS ---------------------

/**************************************************************************/

/*!

@brief Draw a PROGMEM-resident 1-bit image at the specified (x,y)

position, using the specified foreground color (unset bits are transparent).

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with monochrome bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, const uint8_t bitmap[],

int16_t w, int16_t h, uint16_t color) {

int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte <<= 1;

else

byte = pgm_read_byte(&bitmap[j * byteWidth + i / 8]);

if (byte & 0x80)

writePixel(x + i, y, color);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a PROGMEM-resident 1-bit image at the specified (x,y)

position, using the specified foreground (for set bits) and background (unset

bits) colors.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with monochrome bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

@param color 16-bit 5-6-5 Color to draw pixels with

@param bg 16-bit 5-6-5 Color to draw background with

/**************************************************************************/

void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, const uint8_t bitmap[],

int16_t w, int16_t h, uint16_t color,

uint16_t bg) {

int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte <<= 1;

else

byte = pgm_read_byte(&bitmap[j * byteWidth + i / 8]);

writePixel(x + i, y, (byte & 0x80) ? color : bg);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a RAM-resident 1-bit image at the specified (x,y) position,

using the specified foreground color (unset bits are transparent).

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with monochrome bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

@param color 16-bit 5-6-5 Color to draw with

/**************************************************************************/

void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, uint8_t *bitmap, int16_t w,

int16_t h, uint16_t color) {

int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte <<= 1;

else

byte = bitmap[j * byteWidth + i / 8];

if (byte & 0x80)

writePixel(x + i, y, color);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a RAM-resident 1-bit image at the specified (x,y) position,

using the specified foreground (for set bits) and background (unset bits)

colors.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with monochrome bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

@param color 16-bit 5-6-5 Color to draw pixels with

@param bg 16-bit 5-6-5 Color to draw background with

/**************************************************************************/

void Adafruit_GFX::drawBitmap(int16_t x, int16_t y, uint8_t *bitmap, int16_t w,

int16_t h, uint16_t color, uint16_t bg) {

int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte <<= 1;

else

byte = bitmap[j * byteWidth + i / 8];

writePixel(x + i, y, (byte & 0x80) ? color : bg);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw PROGMEM-resident XBitMap Files (*.xbm), exported from GIMP.

Usage: Export from GIMP to *.xbm, rename *.xbm to *.c and open in editor.

C Array can be directly used with this function.

There is no RAM-resident version of this function; if generating bitmaps

in RAM, use the format defined by drawBitmap() and call that instead.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with monochrome bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

@param color 16-bit 5-6-5 Color to draw pixels with

/**************************************************************************/

void Adafruit_GFX::drawXBitmap(int16_t x, int16_t y, const uint8_t bitmap[],

int16_t w, int16_t h, uint16_t color) {

int16_t byteWidth = (w + 7) / 8; // Bitmap scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte >>= 1;

else

byte = pgm_read_byte(&bitmap[j * byteWidth + i / 8]);

// Nearly identical to drawBitmap(), only the bit order

// is reversed here (left-to-right = LSB to MSB):

if (byte & 0x01)

writePixel(x + i, y, color);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a PROGMEM-resident 8-bit image (grayscale) at the specified

(x,y) pos. Specifically for 8-bit display devices such as IS31FL3731; no

color reduction/expansion is performed.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with grayscale bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

/**************************************************************************/

void Adafruit_GFX::drawGrayscaleBitmap(int16_t x, int16_t y,

const uint8_t bitmap[], int16_t w,

int16_t h) {

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

writePixel(x + i, y, (uint8_t)pgm_read_byte(&bitmap[j * w + i]));

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a RAM-resident 8-bit image (grayscale) at the specified (x,y)

pos. Specifically for 8-bit display devices such as IS31FL3731; no color

reduction/expansion is performed.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with grayscale bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

/**************************************************************************/

void Adafruit_GFX::drawGrayscaleBitmap(int16_t x, int16_t y, uint8_t *bitmap,

int16_t w, int16_t h) {

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

writePixel(x + i, y, bitmap[j * w + i]);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a PROGMEM-resident 8-bit image (grayscale) with a 1-bit mask

(set bits = opaque, unset bits = clear) at the specified (x,y) position.

BOTH buffers (grayscale and mask) must be PROGMEM-resident.

Specifically for 8-bit display devices such as IS31FL3731; no color

reduction/expansion is performed.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with grayscale bitmap

@param mask byte array with mask bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

/**************************************************************************/

void Adafruit_GFX::drawGrayscaleBitmap(int16_t x, int16_t y,

const uint8_t bitmap[],

const uint8_t mask[], int16_t w,

int16_t h) {

int16_t bw = (w + 7) / 8; // Bitmask scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte <<= 1;

else

byte = pgm_read_byte(&mask[j * bw + i / 8]);

if (byte & 0x80) {

writePixel(x + i, y, (uint8_t)pgm_read_byte(&bitmap[j * w + i]));

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a RAM-resident 8-bit image (grayscale) with a 1-bit mask

(set bits = opaque, unset bits = clear) at the specified (x,y) position.

BOTH buffers (grayscale and mask) must be RAM-residentt, no mix-and-match

Specifically for 8-bit display devices such as IS31FL3731; no color

reduction/expansion is performed.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with grayscale bitmap

@param mask byte array with mask bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

/**************************************************************************/

void Adafruit_GFX::drawGrayscaleBitmap(int16_t x, int16_t y, uint8_t *bitmap,

uint8_t *mask, int16_t w, int16_t h) {

int16_t bw = (w + 7) / 8; // Bitmask scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte <<= 1;

else

byte = mask[j * bw + i / 8];

if (byte & 0x80) {

writePixel(x + i, y, bitmap[j * w + i]);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a PROGMEM-resident 16-bit image (RGB 5/6/5) at the specified

(x,y) position. For 16-bit display devices; no color reduction performed.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with 16-bit color bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

/**************************************************************************/

void Adafruit_GFX::drawRGBBitmap(int16_t x, int16_t y, const uint16_t bitmap[],

int16_t w, int16_t h) {

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

writePixel(x + i, y, pgm_read_word(&bitmap[j * w + i]));

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a RAM-resident 16-bit image (RGB 5/6/5) at the specified (x,y)

position. For 16-bit display devices; no color reduction performed.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with 16-bit color bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

/**************************************************************************/

void Adafruit_GFX::drawRGBBitmap(int16_t x, int16_t y, uint16_t *bitmap,

int16_t w, int16_t h) {

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

writePixel(x + i, y, bitmap[j * w + i]);

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a PROGMEM-resident 16-bit image (RGB 5/6/5) with a 1-bit mask

(set bits = opaque, unset bits = clear) at the specified (x,y) position. BOTH

buffers (color and mask) must be PROGMEM-resident. For 16-bit display

devices; no color reduction performed.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with 16-bit color bitmap

@param mask byte array with monochrome mask bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

/**************************************************************************/

void Adafruit_GFX::drawRGBBitmap(int16_t x, int16_t y, const uint16_t bitmap[],

const uint8_t mask[], int16_t w, int16_t h) {

int16_t bw = (w + 7) / 8; // Bitmask scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte <<= 1;

else

byte = pgm_read_byte(&mask[j * bw + i / 8]);

if (byte & 0x80) {

writePixel(x + i, y, pgm_read_word(&bitmap[j * w + i]));

}

endWrite();

}

/**************************************************************************/

/*!

@brief Draw a RAM-resident 16-bit image (RGB 5/6/5) with a 1-bit mask (set

bits = opaque, unset bits = clear) at the specified (x,y) position. BOTH

buffers (color and mask) must be RAM-resident. For 16-bit display devices; no

color reduction performed.

@param x Top left corner x coordinate

@param y Top left corner y coordinate

@param bitmap byte array with 16-bit color bitmap

@param mask byte array with monochrome mask bitmap

@param w Width of bitmap in pixels

@param h Height of bitmap in pixels

/**************************************************************************/

void Adafruit_GFX::drawRGBBitmap(int16_t x, int16_t y, uint16_t *bitmap,

uint8_t *mask, int16_t w, int16_t h) {

int16_t bw = (w + 7) / 8; // Bitmask scanline pad = whole byte

uint8_t byte = 0;

startWrite();

for (int16_t j = 0; j < h; j++, y++) {

for (int16_t i = 0; i < w; i++) {

if (i & 7)

byte <<= 1;

else

byte = mask[j * bw + i / 8];

if (byte & 0x80) {

writePixel(x + i, y, bitmap[j * w + i]);

}

endWrite();

}

// TEXT- AND CHARACTER-HANDLING FUNCTIONS ----------------------------------

// Draw a character

/**************************************************************************/

/*!

@brief Draw a single character

@param x Bottom left corner x coordinate

@param y Bottom left corner y coordinate

@param c The 8-bit font-indexed character (likely ascii)

@param color 16-bit 5-6-5 Color to draw chraracter with

@param bg 16-bit 5-6-5 Color to fill background with (if same as color,

no background)

@param size Font magnification level, 1 is 'original' size

/**************************************************************************/

void Adafruit_GFX::drawChar(int16_t x, int16_t y, unsigned char c,

uint16_t color, uint16_t bg, uint8_t size) {

drawChar(x, y, c, color, bg, size, size);

}

// Draw a character

/**************************************************************************/

/*!

@brief Draw a single character

@param x Bottom left corner x coordinate

@param y Bottom left corner y coordinate

@param c The 8-bit font-indexed character (likely ascii)

@param color 16-bit 5-6-5 Color to draw chraracter with

@param bg 16-bit 5-6-5 Color to fill background with (if same as color,

no background)

@param size_x Font magnification level in X-axis, 1 is 'original' size

@param size_y Font magnification level in Y-axis, 1 is 'original' size

/**************************************************************************/

void Adafruit_GFX::drawChar(int16_t x, int16_t y, unsigned char c,

uint16_t color, uint16_t bg, uint8_t size_x,

uint8_t size_y) {

if (!gfxFont) { // 'Classic' built-in font

if ((x >= _width) || // Clip right

(y >= _height) || // Clip bottom

((x + 6 * size_x - 1) < 0) || // Clip left

((y + 8 * size_y - 1) < 0)) // Clip top

return;

if (!_cp437 && (c >= 176))

c++; // Handle 'classic' charset behavior

startWrite();

for (int8_t i = 0; i < 5; i++) { // Char bitmap = 5 columns

uint8_t line = pgm_read_byte(&font[c * 5 + i]);

for (int8_t j = 0; j < 8; j++, line >>= 1) {

if (line & 1) {

if (size_x == 1 && size_y == 1)

writePixel(x + i, y + j, color);

else

writeFillRect(x + i * size_x, y + j * size_y, size_x, size_y,

color);

} else if (bg != color) {

if (size_x == 1 && size_y == 1)

writePixel(x + i, y + j, bg);

else

writeFillRect(x + i * size_x, y + j * size_y, size_x, size_y, bg);

}

if (bg != color) { // If opaque, draw vertical line for last column

if (size_x == 1 && size_y == 1)

writeFastVLine(x + 5, y, 8, bg);

else

writeFillRect(x + 5 * size_x, y, size_x, 8 * size_y, bg);

}

endWrite();

} else { // Custom font

// Character is assumed previously filtered by write() to eliminate

// newlines, returns, non-printable characters, etc. Calling

// drawChar() directly with 'bad' characters of font may cause mayhem!

c -= (uint8_t)pgm_read_byte(&gfxFont->first);

GFXglyph *glyph = pgm_read_glyph_ptr(gfxFont, c);

uint8_t *bitmap = pgm_read_bitmap_ptr(gfxFont);

uint16_t bo = pgm_read_word(&glyph->bitmapOffset);

uint8_t w = pgm_read_byte(&glyph->width), h = pgm_read_byte(&glyph->height);

int8_t xo = pgm_read_byte(&glyph->xOffset),

yo = pgm_read_byte(&glyph->yOffset);

uint8_t xx, yy, bits = 0, bit = 0;

int16_t xo16 = 0, yo16 = 0;

if (size_x > 1 || size_y > 1) {

xo16 = xo;

yo16 = yo;

}

// Todo: Add character clipping here

// NOTE: THERE IS NO 'BACKGROUND' COLOR OPTION ON CUSTOM FONTS.

// THIS IS ON PURPOSE AND BY DESIGN. The background color feature

// has typically been used with the 'classic' font to overwrite old

// screen contents with new data. This ONLY works because the

// characters are a uniform size; it's not a sensible thing to do with

// proportionally-spaced fonts with glyphs of varying sizes (and that

// may overlap). To replace previously-drawn text when using a custom

// font, use the getTextBounds() function to determine the smallest

// rectangle encompassing a string, erase the area with fillRect(),

// then draw new text. This WILL infortunately 'blink' the text, but

// is unavoidable. Drawing 'background' pixels will NOT fix this,

// only creates a new set of problems. Have an idea to work around

// this (a canvas object type for MCUs that can afford the RAM and

// displays supporting setAddrWindow() and pushColors()), but haven't

// implemented this yet.

startWrite();

for (yy = 0; yy < h; yy++) {

for (xx = 0; xx < w; xx++) {

if (!(bit++ & 7)) {

bits = pgm_read_byte(&bitmap[bo++]);

}

if (bits & 0x80) {

if (size_x == 1 && size_y == 1) {

writePixel(x + xo + xx, y + yo + yy, color);

} else {

writeFillRect(x + (xo16 + xx) * size_x, y + (yo16 + yy) * size_y,

size_x, size_y, color);

}

bits <<= 1;

}

endWrite();

} // End classic vs custom font

}

/**************************************************************************/

/*!

@brief Print one byte/character of data, used to support print()

@param c The 8-bit ascii character to write

/**************************************************************************/

size_t Adafruit_GFX::write(uint8_t c) {

if (!gfxFont) { // 'Classic' built-in font

if (c == '\n') { // Newline?

cursor_x = 0; // Reset x to zero,

cursor_y += textsize_y * 8; // advance y one line

} else if (c != '\r') { // Ignore carriage returns

if (wrap && ((cursor_x + textsize_x * 6) > _width)) { // Off right?

cursor_x = 0; // Reset x to zero,

cursor_y += textsize_y * 8; // advance y one line

}

drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize_x,

textsize_y);

cursor_x += textsize_x * 6; // Advance x one char

}

} else { // Custom font

if (c == '\n') {

cursor_x = 0;

cursor_y +=

(int16_t)textsize_y * (uint8_t)pgm_read_byte(&gfxFont->yAdvance);

} else if (c != '\r') {

uint8_t first = pgm_read_byte(&gfxFont->first);

if ((c >= first) && (c <= (uint8_t)pgm_read_byte(&gfxFont->last))) {

GFXglyph *glyph = pgm_read_glyph_ptr(gfxFont, c - first);

uint8_t w = pgm_read_byte(&glyph->width),

h = pgm_read_byte(&glyph->height);

if ((w > 0) && (h > 0)) { // Is there an associated bitmap?

int16_t xo = (int8_t)pgm_read_byte(&glyph->xOffset); // sic

if (wrap && ((cursor_x + textsize_x * (xo + w)) > _width)) {

cursor_x = 0;

cursor_y += (int16_t)textsize_y *

(uint8_t)pgm_read_byte(&gfxFont->yAdvance);

}

drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize_x,

textsize_y);

}

cursor_x +=

(uint8_t)pgm_read_byte(&glyph->xAdvance) * (int16_t)textsize_x;

}

return 1;

}

/**************************************************************************/

/*!

@brief Set text 'magnification' size. Each increase in s makes 1 pixel

that much bigger.

@param s Desired text size. 1 is default 6x8, 2 is 12x16, 3 is 18x24, etc

/**************************************************************************/

void Adafruit_GFX::setTextSize(uint8_t s) { setTextSize(s, s); }

/**************************************************************************/

/*!

@brief Set text 'magnification' size. Each increase in s makes 1 pixel

that much bigger.

@param s_x Desired text width magnification level in X-axis. 1 is default

@param s_y Desired text width magnification level in Y-axis. 1 is default

/**************************************************************************/

void Adafruit_GFX::setTextSize(uint8_t s_x, uint8_t s_y) {

textsize_x = (s_x > 0) ? s_x : 1;

textsize_y = (s_y > 0) ? s_y : 1;

}

/**************************************************************************/

/*!

@brief Set rotation setting for display

@param x 0 thru 3 corresponding to 4 cardinal rotations

/**************************************************************************/

void Adafruit_GFX::setRotation(uint8_t x) {

rotation = (x & 3);

switch (rotation) {

case 0:

case 2:

_width = WIDTH;

_height = HEIGHT;

break;

case 1:

case 3:

_width = HEIGHT;

_height = WIDTH;

break;

}

/**************************************************************************/

/*!

@brief Set the font to display when print()ing, either custom or default

@param f The GFXfont object, if NULL use built in 6x8 font

/**************************************************************************/

void Adafruit_GFX::setFont(const GFXfont *f) {

if (f) { // Font struct pointer passed in?

if (!gfxFont) { // And no current font struct?

// Switching from classic to new font behavior.

// Move cursor pos down 6 pixels so it's on baseline.

cursor_y += 6;

}

} else if (gfxFont) { // NULL passed. Current font struct defined?

// Switching from new to classic font behavior.

// Move cursor pos up 6 pixels so it's at top-left of char.

cursor_y -= 6;

}

gfxFont = (GFXfont *)f;

}

/**************************************************************************/

/*!

@brief Helper to determine size of a character with current font/size.

Broke this out as it's used by both the PROGMEM- and RAM-resident

getTextBounds() functions.

@param c The ascii character in question

@param x Pointer to x location of character

@param y Pointer to y location of character

@param minx Minimum clipping value for X

@param miny Minimum clipping value for Y

@param maxx Maximum clipping value for X

@param maxy Maximum clipping value for Y

/**************************************************************************/

void Adafruit_GFX::charBounds(unsigned char c, int16_t *x, int16_t *y,

int16_t *minx, int16_t *miny, int16_t *maxx,

int16_t *maxy) {

if (gfxFont) {

if (c == '\n') { // Newline?

*x = 0; // Reset x to zero, advance y by one line

*y += textsize_y * (uint8_t)pgm_read_byte(&gfxFont->yAdvance);

} else if (c != '\r') { // Not a carriage return; is normal char

uint8_t first = pgm_read_byte(&gfxFont->first),

last = pgm_read_byte(&gfxFont->last);

if ((c >= first) && (c <= last)) { // Char present in this font?

GFXglyph *glyph = pgm_read_glyph_ptr(gfxFont, c - first);

uint8_t gw = pgm_read_byte(&glyph->width),

gh = pgm_read_byte(&glyph->height),

xa = pgm_read_byte(&glyph->xAdvance);

int8_t xo = pgm_read_byte(&glyph->xOffset),

yo = pgm_read_byte(&glyph->yOffset);

if (wrap && ((*x + (((int16_t)xo + gw) * textsize_x)) > _width)) {

*x = 0; // Reset x to zero, advance y by one line

*y += textsize_y * (uint8_t)pgm_read_byte(&gfxFont->yAdvance);

}

int16_t tsx = (int16_t)textsize_x, tsy = (int16_t)textsize_y,

x1 = *x + xo * tsx, y1 = *y + yo * tsy, x2 = x1 + gw * tsx - 1,

y2 = y1 + gh * tsy - 1;

if (x1 < *minx)

*minx = x1;

if (y1 < *miny)

*miny = y1;

if (x2 > *maxx)

*maxx = x2;

if (y2 > *maxy)

*maxy = y2;

*x += xa * tsx;

}

} else { // Default font

if (c == '\n') { // Newline?

*x = 0; // Reset x to zero,

*y += textsize_y * 8; // advance y one line

// min/max x/y unchaged -- that waits for next 'normal' character

} else if (c != '\r') { // Normal char; ignore carriage returns

if (wrap && ((*x + textsize_x * 6) > _width)) { // Off right?

*x = 0; // Reset x to zero,

*y += textsize_y * 8; // advance y one line

}

int x2 = *x + textsize_x * 6 - 1, // Lower-right pixel of char

y2 = *y + textsize_y * 8 - 1;

if (x2 > *maxx)

*maxx = x2; // Track max x, y

if (y2 > *maxy)

*maxy = y2;

if (*x < *minx)

*minx = *x; // Track min x, y

if (*y < *miny)

*miny = *y;

*x += textsize_x * 6; // Advance x one char

}

/**************************************************************************/

/*!

@brief Helper to determine size of a string with current font/size. Pass

string and a cursor position, returns UL corner and W,H.

@param str The ascii string to measure

@param x The current cursor X

@param y The current cursor Y

@param x1 The boundary X coordinate, set by function

@param y1 The boundary Y coordinate, set by function

@param w The boundary width, set by function

@param h The boundary height, set by function

/**************************************************************************/

void Adafruit_GFX::getTextBounds(const char *str, int16_t x, int16_t y,

int16_t *x1, int16_t *y1, uint16_t *w,

uint16_t *h) {

uint8_t c; // Current character

*x1 = x;

*y1 = y;

*w = *h = 0;

int16_t minx = _width, miny = _height, maxx = -1, maxy = -1;

while ((c = *str++))

charBounds(c, &x, &y, &minx, &miny, &maxx, &maxy);

if (maxx >= minx) {

*x1 = minx;

*w = maxx - minx + 1;

}

if (maxy >= miny) {

*y1 = miny;

*h = maxy - miny + 1;

}

/**************************************************************************/

/*!

@brief Helper to determine size of a string with current font/size. Pass

string and a cursor position, returns UL corner and W,H.

@param str The ascii string to measure (as an arduino String() class)

@param x The current cursor X

@param y The current cursor Y

@param x1 The boundary X coordinate, set by function

@param y1 The boundary Y coordinate, set by function

@param w The boundary width, set by function

@param h The boundary height, set by function

/**************************************************************************/

void Adafruit_GFX::getTextBounds(const String &str, int16_t x, int16_t y,

int16_t *x1, int16_t *y1, uint16_t *w,

uint16_t *h) {

if (str.length() != 0) {

getTextBounds(const_cast<char *>(str.c_str()), x, y, x1, y1, w, h);

}

/**************************************************************************/

/*!

@brief Helper to determine size of a PROGMEM string with current

font/size. Pass string and a cursor position, returns UL corner and W,H.

@param str The flash-memory ascii string to measure

@param x The current cursor X

@param y The current cursor Y

@param x1 The boundary X coordinate, set by function

@param y1 The boundary Y coordinate, set by function

@param w The boundary width, set by function

@param h The boundary height, set by function

/**************************************************************************/

void Adafruit_GFX::getTextBounds(const __FlashStringHelper *str, int16_t x,

int16_t y, int16_t *x1, int16_t *y1,

uint16_t *w, uint16_t *h) {

uint8_t *s = (uint8_t *)str, c;

*x1 = x;

*y1 = y;

*w = *h = 0;

int16_t minx = _width, miny = _height, maxx = -1, maxy = -1;

while ((c = pgm_read_byte(s++)))

charBounds(c, &x, &y, &minx, &miny, &maxx, &maxy);

if (maxx >= minx) {

*x1 = minx;

*w = maxx - minx + 1;

}

if (maxy >= miny) {

*y1 = miny;

*h = maxy - miny + 1;

}

/**************************************************************************/

/*!

@brief Invert the display (ideally using built-in hardware command)

@param i True if you want to invert, false to make 'normal'

/**************************************************************************/

void Adafruit_GFX::invertDisplay(boolean i) {

// Do nothing, must be subclassed if supported by hardware

}

/***************************************************************************/

/**************************************************************************/

/*!

@brief Create a simple drawn button UI element

/**************************************************************************/

Adafruit_GFX_Button::Adafruit_GFX_Button(void) { _gfx = 0; }

/**************************************************************************/

/*!

@brief Initialize button with our desired color/size/settings

@param gfx Pointer to our display so we can draw to it!

@param x The X coordinate of the center of the button

@param y The Y coordinate of the center of the button

@param w Width of the buttton

@param h Height of the buttton

@param outline Color of the outline (16-bit 5-6-5 standard)

@param fill Color of the button fill (16-bit 5-6-5 standard)

@param textcolor Color of the button label (16-bit 5-6-5 standard)

@param label Ascii string of the text inside the button

@param textsize The font magnification of the label text

/**************************************************************************/

// Classic initButton() function: pass center & size

void Adafruit_GFX_Button::initButton(Adafruit_GFX *gfx, int16_t x, int16_t y,

uint16_t w, uint16_t h, uint16_t outline,

uint16_t fill, uint16_t textcolor,

char *label, uint8_t textsize) {

// Tweak arguments and pass to the newer initButtonUL() function...

initButtonUL(gfx, x - (w / 2), y - (h / 2), w, h, outline, fill, textcolor,

label, textsize);

}

/**************************************************************************/

/*!

@brief Initialize button with our desired color/size/settings

@param gfx Pointer to our display so we can draw to it!

@param x The X coordinate of the center of the button

@param y The Y coordinate of the center of the button

@param w Width of the buttton

@param h Height of the buttton

@param outline Color of the outline (16-bit 5-6-5 standard)

@param fill Color of the button fill (16-bit 5-6-5 standard)

@param textcolor Color of the button label (16-bit 5-6-5 standard)

@param label Ascii string of the text inside the button

@param textsize_x The font magnification in X-axis of the label text

@param textsize_y The font magnification in Y-axis of the label text

/**************************************************************************/

// Classic initButton() function: pass center & size

void Adafruit_GFX_Button::initButton(Adafruit_GFX *gfx, int16_t x, int16_t y,

uint16_t w, uint16_t h, uint16_t outline,

uint16_t fill, uint16_t textcolor,

char *label, uint8_t textsize_x,

uint8_t textsize_y) {

// Tweak arguments and pass to the newer initButtonUL() function...

initButtonUL(gfx, x - (w / 2), y - (h / 2), w, h, outline, fill, textcolor,

label, textsize_x, textsize_y);

}

/**************************************************************************/

/*!

@brief Initialize button with our desired color/size/settings, with

upper-left coordinates

@param gfx Pointer to our display so we can draw to it!

@param x1 The X coordinate of the Upper-Left corner of the button

@param y1 The Y coordinate of the Upper-Left corner of the button

@param w Width of the buttton

@param h Height of the buttton

@param outline Color of the outline (16-bit 5-6-5 standard)

@param fill Color of the button fill (16-bit 5-6-5 standard)

@param textcolor Color of the button label (16-bit 5-6-5 standard)

@param label Ascii string of the text inside the button

@param textsize The font magnification of the label text

/**************************************************************************/

void Adafruit_GFX_Button::initButtonUL(Adafruit_GFX *gfx, int16_t x1,

int16_t y1, uint16_t w, uint16_t h,

uint16_t outline, uint16_t fill,

uint16_t textcolor, char *label,

uint8_t textsize) {

initButtonUL(gfx, x1, y1, w, h, outline, fill, textcolor, label, textsize,

textsize);

}

/**************************************************************************/

/*!

@brief Initialize button with our desired color/size/settings, with

upper-left coordinates

@param gfx Pointer to our display so we can draw to it!

@param x1 The X coordinate of the Upper-Left corner of the button

@param y1 The Y coordinate of the Upper-Left corner of the button

@param w Width of the buttton

@param h Height of the buttton

@param outline Color of the outline (16-bit 5-6-5 standard)

@param fill Color of the button fill (16-bit 5-6-5 standard)

@param textcolor Color of the button label (16-bit 5-6-5 standard)

@param label Ascii string of the text inside the button

@param textsize_x The font magnification in X-axis of the label text

@param textsize_y The font magnification in Y-axis of the label text

/**************************************************************************/

void Adafruit_GFX_Button::initButtonUL(Adafruit_GFX *gfx, int16_t x1,

int16_t y1, uint16_t w, uint16_t h,

uint16_t outline, uint16_t fill,

uint16_t textcolor, char *label,

uint8_t textsize_x, uint8_t textsize_y) {

_x1 = x1;

_y1 = y1;

_w = w;

_h = h;

_outlinecolor = outline;

_fillcolor = fill;

_textcolor = textcolor;

_textsize_x = textsize_x;

_textsize_y = textsize_y;

_gfx = gfx;

strncpy(_label, label, 9);

}

/**************************************************************************/

/*!

@brief Draw the button on the screen

@param inverted Whether to draw with fill/text swapped to indicate

'pressed'

/**************************************************************************/

void Adafruit_GFX_Button::drawButton(boolean inverted) {

uint16_t fill, outline, text;

if (!inverted) {

fill = _fillcolor;

outline = _outlinecolor;

text = _textcolor;

} else {

fill = _textcolor;

outline = _outlinecolor;

text = _fillcolor;

}

uint8_t r = min(_w, _h) / 4; // Corner radius

_gfx->fillRoundRect(_x1, _y1, _w, _h, r, fill);

_gfx->drawRoundRect(_x1, _y1, _w, _h, r, outline);

_gfx->setCursor(_x1 + (_w / 2) - (strlen(_label) * 3 * _textsize_x),

_y1 + (_h / 2) - (4 * _textsize_y));

_gfx->setTextColor(text);

_gfx->setTextSize(_textsize_x, _textsize_y);

_gfx->print(_label);

}

/**************************************************************************/

/*!

@brief Helper to let us know if a coordinate is within the bounds of the

button

@param x The X coordinate to check

@param y The Y coordinate to check

@returns True if within button graphics outline

/**************************************************************************/

boolean Adafruit_GFX_Button::contains(int16_t x, int16_t y) {

return ((x >= _x1) && (x < (int16_t)(_x1 + _w)) && (y >= _y1) &&

(y < (int16_t)(_y1 + _h)));

}

/**************************************************************************/

/*!

@brief Query whether the button was pressed since we last checked state

@returns True if was not-pressed before, now is.

/**************************************************************************/

boolean Adafruit_GFX_Button::justPressed() { return (currstate && !laststate); }

/**************************************************************************/

/*!

@brief Query whether the button was released since we last checked state

@returns True if was pressed before, now is not.

/**************************************************************************/

boolean Adafruit_GFX_Button::justReleased() {

return (!currstate && laststate);

}

// -------------------------------------------------------------------------

// GFXcanvas1, GFXcanvas8 and GFXcanvas16 (currently a WIP, don't get too

// comfy with the implementation) provide 1-, 8- and 16-bit offscreen

// canvases, the address of which can be passed to drawBitmap() or

// pushColors() (the latter appears only in a couple of GFX-subclassed TFT

// libraries at this time). This is here mostly to help with the recently-

// added proportionally-spaced fonts; adds a way to refresh a section of the

// screen without a massive flickering clear-and-redraw...but maybe you'll

// find other uses too. VERY RAM-intensive, since the buffer is in MCU

// memory and not the display driver...GXFcanvas1 might be minimally useful

// on an Uno-class board, but this and the others are much more likely to

// require at least a Mega or various recent ARM-type boards (recommended,

// as the text+bitmap draw can be pokey). GFXcanvas1 requires 1 bit per

// pixel (rounded up to nearest byte per scanline), GFXcanvas8 is 1 byte

// per pixel (no scanline pad), and GFXcanvas16 uses 2 bytes per pixel (no

// scanline pad).

// NOT EXTENSIVELY TESTED YET. MAY CONTAIN WORST BUGS KNOWN TO HUMANKIND.

/**************************************************************************/

/*!

@brief Instatiate a GFX 1-bit canvas context for graphics

@param w Display width, in pixels

@param h Display height, in pixels

/**************************************************************************/

GFXcanvas1::GFXcanvas1(uint16_t w, uint16_t h) : Adafruit_GFX(w, h) {

uint16_t bytes = ((w + 7) / 8) * h;

if ((buffer = (uint8_t *)malloc(bytes))) {

memset(buffer, 0, bytes);

}

/**************************************************************************/

/*!

@brief Delete the canvas, free memory

/**************************************************************************/

GFXcanvas1::~GFXcanvas1(void) {

if (buffer)

free(buffer);

}

/**************************************************************************/

/*!

@brief Draw a pixel to the canvas framebuffer

@param x x coordinate

@param y y coordinate

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void GFXcanvas1::drawPixel(int16_t x, int16_t y, uint16_t color) {

#ifdef __AVR__

// Bitmask tables of 0x80>>X and ~(0x80>>X), because X>>Y is slow on AVR

static const uint8_t PROGMEM GFXsetBit[] = {0x80, 0x40, 0x20, 0x10,

0x08, 0x04, 0x02, 0x01},

GFXclrBit[] = {0x7F, 0xBF, 0xDF, 0xEF,

0xF7, 0xFB, 0xFD, 0xFE};

#endif

if (buffer) {

if ((x < 0) || (y < 0) || (x >= _width) || (y >= _height))

return;

int16_t t;

switch (rotation) {

case 1:

t = x;

x = WIDTH - 1 - y;

y = t;

break;

case 2:

x = WIDTH - 1 - x;

y = HEIGHT - 1 - y;

break;

case 3:

t = x;

x = y;

y = HEIGHT - 1 - t;

break;

}

uint8_t *ptr = &buffer[(x / 8) + y * ((WIDTH + 7) / 8)];

#ifdef __AVR__

if (color)

*ptr |= pgm_read_byte(&GFXsetBit[x & 7]);

else

*ptr &= pgm_read_byte(&GFXclrBit[x & 7]);

#else

if (color)

*ptr |= 0x80 >> (x & 7);

else

*ptr &= ~(0x80 >> (x & 7));

#endif

}

/**************************************************************************/

/*!

@brief Fill the framebuffer completely with one color

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void GFXcanvas1::fillScreen(uint16_t color) {

if (buffer) {

uint16_t bytes = ((WIDTH + 7) / 8) * HEIGHT;

memset(buffer, color ? 0xFF : 0x00, bytes);

}

/**************************************************************************/

/*!

@brief Instatiate a GFX 8-bit canvas context for graphics

@param w Display width, in pixels

@param h Display height, in pixels

/**************************************************************************/

GFXcanvas8::GFXcanvas8(uint16_t w, uint16_t h) : Adafruit_GFX(w, h) {

uint32_t bytes = w * h;

if ((buffer = (uint8_t *)malloc(bytes))) {

memset(buffer, 0, bytes);

}

/**************************************************************************/

/*!

@brief Delete the canvas, free memory

/**************************************************************************/

GFXcanvas8::~GFXcanvas8(void) {

if (buffer)

free(buffer);

}

/**************************************************************************/

/*!

@brief Draw a pixel to the canvas framebuffer

@param x x coordinate

@param y y coordinate

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void GFXcanvas8::drawPixel(int16_t x, int16_t y, uint16_t color) {

if (buffer) {

if ((x < 0) || (y < 0) || (x >= _width) || (y >= _height))

return;

int16_t t;

switch (rotation) {

case 1:

t = x;

x = WIDTH - 1 - y;

y = t;

break;

case 2:

x = WIDTH - 1 - x;

y = HEIGHT - 1 - y;

break;

case 3:

t = x;

x = y;

y = HEIGHT - 1 - t;

break;

}

buffer[x + y * WIDTH] = color;

}

/**************************************************************************/

/*!

@brief Fill the framebuffer completely with one color

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void GFXcanvas8::fillScreen(uint16_t color) {

if (buffer) {

memset(buffer, color, WIDTH * HEIGHT);

}

void GFXcanvas8::writeFastHLine(int16_t x, int16_t y, int16_t w,

uint16_t color) {

if ((x >= _width) || (y < 0) || (y >= _height))

return;

int16_t x2 = x + w - 1;

if (x2 < 0)

return;

// Clip left/right

if (x < 0) {

x = 0;

w = x2 + 1;

}

if (x2 >= _width)

w = _width - x;

int16_t t;

switch (rotation) {

case 1:

t = x;

x = WIDTH - 1 - y;

y = t;

break;

case 2:

x = WIDTH - 1 - x;

y = HEIGHT - 1 - y;

break;

case 3:

t = x;

x = y;

y = HEIGHT - 1 - t;

break;

}

memset(buffer + y * WIDTH + x, color, w);

}

/**************************************************************************/

/*!

@brief Instatiate a GFX 16-bit canvas context for graphics

@param w Display width, in pixels

@param h Display height, in pixels

/**************************************************************************/

GFXcanvas16::GFXcanvas16(uint16_t w, uint16_t h) : Adafruit_GFX(w, h) {

uint32_t bytes = w * h * 2;

if ((buffer = (uint16_t *)malloc(bytes))) {

memset(buffer, 0, bytes);

}

/**************************************************************************/

/*!

@brief Delete the canvas, free memory

/**************************************************************************/

GFXcanvas16::~GFXcanvas16(void) {

if (buffer)

free(buffer);

}

/**************************************************************************/

/*!

@brief Draw a pixel to the canvas framebuffer

@param x x coordinate

@param y y coordinate

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void GFXcanvas16::drawPixel(int16_t x, int16_t y, uint16_t color) {

if (buffer) {

if ((x < 0) || (y < 0) || (x >= _width) || (y >= _height))

return;

int16_t t;

switch (rotation) {

case 1:

t = x;

x = WIDTH - 1 - y;

y = t;

break;

case 2:

x = WIDTH - 1 - x;

y = HEIGHT - 1 - y;

break;

case 3:

t = x;

x = y;

y = HEIGHT - 1 - t;

break;

}

buffer[x + y * WIDTH] = color;

}

/**************************************************************************/

/*!

@brief Fill the framebuffer completely with one color

@param color 16-bit 5-6-5 Color to fill with

/**************************************************************************/

void GFXcanvas16::fillScreen(uint16_t color) {

if (buffer) {

uint8_t hi = color >> 8, lo = color & 0xFF;

if (hi == lo) {

memset(buffer, lo, WIDTH * HEIGHT * 2);

} else {

uint32_t i, pixels = WIDTH * HEIGHT;

for (i = 0; i < pixels; i++)

buffer[i] = color;

}

/**************************************************************************/

/*!

@brief Reverses the "endian-ness" of each 16-bit pixel within the

canvas; little-endian to big-endian, or big-endian to little.

Most microcontrollers (such as SAMD) are little-endian, while

most displays tend toward big-endianness. All the drawing

functions (including RGB bitmap drawing) take care of this

automatically, but some specialized code (usually involving

DMA) can benefit from having pixel data already in the

display-native order. Note that this does NOT convert to a

SPECIFIC endian-ness, it just flips the bytes within each word.

/**************************************************************************/

void GFXcanvas16::byteSwap(void) {

if (buffer) {

uint32_t i, pixels = WIDTH * HEIGHT;

for (i = 0; i < pixels; i++)

buffer[i] = __builtin_bswap16(buffer[i]);

}

Stay Tuned,

Frank

Caplet Dispenser

1 Reply

Posted 13 July 2021

Being an old fart, I have unfortunately accreted a number of meds that I must take on a daily basis; the current count is four different meds – two in caplet style, and to in pill style. Each night I have to remove the cap from four different bottles, extract just one pill/caplet, and close the cap, all without losing any. After the umpteenth time that I either lost a pill on the floor, or had some very expensive caplets literally go down the drain, I said “there must be a better way…”.

So, I started designing a mechanism that would dispense just one pill/caplet on each cycle. I started out by creating TinkerCad models of the pills and caplets, as shown below:

Meds to be dispensed. Large caplet is about 7 x 19 mm

Then I started working on a dispensing mechanism, and wound up with the following design.

Caplet is loaded in upper image, dispensed in lower one

The mechanism consists of a sliding drawer and a collar with a caplet-sized slot. In the ‘load’ position (upper image above), the caplet falls through the slot into a caplet-sized bay in the drawer. In the ‘dispense’ position, the caplet falls through the open bottom of the drawer bay. Each time the drawer is moved back to the ‘load’ position, another caplet falls into the dispense bay, and is dispensed when the drawer is moved back to the ‘dispense’ position.

The above design worked very well, but there were two significant problems; the drawer wouldn’t stay in the ‘load’ position, so care had to be taken to avoid dispensing several caplets at a time, and it was possible for two caplets to fit vertically into the slot, jamming the mechanism, as shown below:

Two caplets oriented vertically, jamming the drawer mechanism

To keep the drawer in the ‘load’ position, a small rubber band was attached to the collar (the red part above) and around the right end of the drawer, using a hot-glue gun. This keeps the drawer in the ‘load’ position until actively pushed against the rubber band tension to the ‘dispense’ position. The issue with two caplets jamming the drawer was solved by placing a ‘flap’ over the slot in the collar, reducing the hole size such that only one caplet at a time can fit. A caplet goes through the hole vertically, and then slides down into the drawer bay, winding up horizontally in the bay, as shown below:

final design showing reduced-size collar slot allowing only one caplet at a time into bay

The next step in the project was to design and fabricate an adaptor piece to connect the dispensing mechanism to the pill bottle. TinkerCad doesn’t really support morphing from one shape to another, so I had to find a different way. I tried Blender, and while it did work, I had no experience with the product and so stumbled around a lot. Next I tried Open SCAD and discovered the ‘hull()’ feature, which does pretty much exactly what I want. After playing around with this a while, I came up with the following OSCAD script to do what I wanted:

/*07/12/21 Caplet dispenser adaptor for bottles 
  with various size caps. The cap fits into the top,
  and the center of the cap is cut away to allow the
  pills to fall down into the funnel.
  This part gets mated with the actual dispenser
  mechanism in TinkerCad
  
  07/13/21 - increased wall thickness at square/cone transition
           - added flap to ensure only one cap at time into dispensing cavity
           - increased the height of the pill bottle mating collar
*/

rect_w = 12; //rectangle width
rect_l = 28; //rectangle height
th = 2; //wall thickness of the completed object
cyl_id = 56; //id to match pillbottle lid
cyl_od = cyl_id + 2*th; //outside diameter
ht = 16; //total height of morphed element
rect_h = 6; //height of just the rectangular solid
rim_ht = 12; //height of the collar above the morphed element
fn = 60; //number of sides making up cylinders


module cube_cyl(w,l,rect_h,dia, ht)
{
  dh = 0.5;
  echo(w = w, l = l, h = ht);
  echo(rect_l = rect_l, rect_w = rect_w, rect_h = rect_h); 
  
    hull()
  {
    cube([w,l, dh], center=true);
    translate ([0,0,ht]) cylinder (d=dia, h=0.5, $fn=fn);
  }

  translate ([0,0,-rect_h/2])cube([w,l,rect_h], center=true);
  translate ([0,0,ht+rim_ht/2])cylinder(h=rim_ht, d=dia, center=true, $fn=fn);
}

difference () 
{
  cube_cyl(rect_w,rect_l, rect_h, cyl_od,ht);
  color("green")
//  translate ([0,0,-0.5])cube_cyl(rect_w-th,rect_l-th,rect_h+th, cyl_id,ht+1);
  translate ([0,0,0])cube_cyl(rect_w-th,rect_l-th,rect_h+th, cyl_id,ht+1);
}

//07/13/21 flap ensures only one cap at a time into dispensing cavity
translate ([0,-rect_l/6,0])cube([rect_w,2*rect_l/3,1], center=true);

/*07/12/21 Caplet dispenser adaptor for bottles

with various size caps. The cap fits into the top,

and the center of the cap is cut away to allow the

pills to fall down into the funnel.

This part gets mated with the actual dispenser

mechanism in TinkerCad

07/13/21 - increased wall thickness at square/cone transition

- added flap to ensure only one cap at time into dispensing cavity

- increased the height of the pill bottle mating collar

rect_w = 12; //rectangle width

rect_l = 28; //rectangle height

th = 2; //wall thickness of the completed object

cyl_id = 56; //id to match pillbottle lid

cyl_od = cyl_id + 2*th; //outside diameter

ht = 16; //total height of morphed element

rect_h = 6; //height of just the rectangular solid

rim_ht = 12; //height of the collar above the morphed element

fn = 60; //number of sides making up cylinders

module cube_cyl(w,l,rect_h,dia, ht)

{

dh = 0.5;

echo(w = w, l = l, h = ht);

echo(rect_l = rect_l, rect_w = rect_w, rect_h = rect_h);

hull()

{

cube([w,l, dh], center=true);

translate ([0,0,ht]) cylinder (d=dia, h=0.5, $fn=fn);

}

translate ([0,0,-rect_h/2])cube([w,l,rect_h], center=true);

translate ([0,0,ht+rim_ht/2])cylinder(h=rim_ht, d=dia, center=true, $fn=fn);

}

difference ()

{

cube_cyl(rect_w,rect_l, rect_h, cyl_od,ht);

color("green")

// translate ([0,0,-0.5])cube_cyl(rect_w-th,rect_l-th,rect_h+th, cyl_id,ht+1);

translate ([0,0,0])cube_cyl(rect_w-th,rect_l-th,rect_h+th, cyl_id,ht+1);

}

//07/13/21 flap ensures only one cap at a time into dispensing cavity

translate ([0,-rect_l/6,0])cube([rect_w,2*rect_l/3,1], center=true);

The above code and parameter set produced the following model:

The cylindrical shape at the top just accepts a 56mm diameter bottle cap. The above model was converted to an STL file and then imported into TinkerCad, where it was mated with the dispensing drawer. Then the entire thing was printed in one go using my Prusa MK3S 3D printer, as shown below:

Prusa Slicer 2.3.0 showing ‘sliced’ model with supports, ready to print. This print takes about 3.5 hours.

After the print finishes, the support material between the drawer collar and the drawer itself has to be removed manually with an Exacto knife. This is a bit of a PITA, but worth it to have the entire thing printed as a single piece. The photo below shows the finished product.

30 August 2021 Update:

I just recently acquired a Flashforge Creator Pro 2 dual-independent extruder printer to replace the MakerGear M3-ID I sold on eBay for about a third of what I paid for it. Even after a year and a half of diligent work, I could NOT get the M3-ID to print with dissolvable filament worth a damn. I tried everything, including a brand-new roll of PVA filament and adding a BuilTak removable build plate system (see this post). I even ordered the 3D upFitters enclosure for the M3-ID, but came to my senses before I assembled and installed it. After some more web research, I came across the FlashForge Creator Pro 2 IDEX (FFCP2) system, available for about 1/4 the price of the M3-ID, and this printer came with glowing YouTube recommendations from many reputable 3D printer enthusiasts. After receiving my FFCP2 and setting it up, I was able to print the above pill/caplet dispenser design using PLA for the structure and PVA dissolvable filament for the support material. The result was a very high-quality build and the support material dissolved out after just a few hours in warm water – YAY!

Stay tuned,

Frank

Another Try at Wall Offset Tracking, Part II

26 June 2021 Update:

I modified my test program to just report the rear, center, and front VL53L0X distances, plus the steering value and the computed off-parallel angle, using the -0.0625 slope value obtained from the above plots. Unfortunately, the results were wildly unrealistic, leading me to believe something was badly wrong somewhere along the line. So, I redid the plots, thinking maybe the left side VL53L0X sensors were different enough to make that much of a difference. When I plotted the steering value vs off-parallel angle for 10, 20, 30 & 40cm wall offsets, I got a significantly different plot, as shown below:

As can be seen in the above plot, the slope of steering values to off-parallel angles is nice and linear, and also quite constant over the range of wall offset distances from 10 to 40 cm; this is quite a bit different than the behavior of the right-side sensor array, but it is what it is. In any case, it appears that the slope is close to 1.4/80 = 0.0175, or almost twice the right-side slope derived from the previous right-side plots.

Using the value of 0.0175 in my GetSteeringAngle() function results, and comparing the calculated off-parallel angle with the actual measured angle, I get the following Excel plot.

As can be seen in the above plot, the agreement between measured and calculated off-parallel angles is quite good, using the average slope value of 0.0175.

The above plot shows the raw values that produced the first plot.

So, back to my ‘FourWD_WallTrackTest’ program to develop Wall-E2’s ability to capture and then track a particular desired offset. In my first iteration, I always started with Wall-E2 parallel to the wall, and calculated an intercept angle based on the difference between the robot’s actual and desired offsets from the near wall. This worked very nicely, but didn’t address what happens if the robot doesn’t start in a parallel orientation. However, when I tried to use the data from a previous post, the results were wildly off. Now it is time to try this trick again, using the above data instead. Because the measured steering value/off angle slopes for all selected wall offsets were essentially identical, I can eliminate the intermediate step of calculating the appropriate slope value based on the current wall offset distance.

So, I modified my ‘getSteeringAngle()’ function to drop the ‘ctr_dist_mm’ parameter and to use a constant 0.0175 slope value, as shown below:

double getSteeringAngle(double steerval)
{
	//Purpose:  Convert a steering angle into the equivalent off-parallel orientation in degrees
	//Inputs:
	//	steerval = double value denoting current steering value
	//	Notes:
	//	06/27/21 now steering_val/off_angle slope is a constant 0.0175
	const double slopeval = 0.0175;
	double offparalleldeg = steerval / slopeval; //e.g. for steerval = -0.187 @ 10cm offset, returns +21deg
	//mySerial.printf("getSteeringAngle(%d, %2.4f): slopeval = %2.4f, res %2.2f\n", ctrdist_cm, steerval, slopeval, offparalleldeg);
	return offparalleldeg;
}

double getSteeringAngle(double steerval)

{

//Purpose: Convert a steering angle into the equivalent off-parallel orientation in degrees

//Inputs:

// steerval = double value denoting current steering value

// Notes:

// 06/27/21 now steering_val/off_angle slope is a constant 0.0175

const double slopeval = 0.0175;

double offparalleldeg = steerval / slopeval; //e.g. for steerval = -0.187 @ 10cm offset, returns +21deg

//mySerial.printf("getSteeringAngle(%d, %2.4f): slopeval = %2.4f, res %2.2f\n", ctrdist_cm, steerval, slopeval, offparalleldeg);

return offparalleldeg;

}

With this modification, I was able to get pretty decent results; Wall-E2 successfully captured and tracked the desired offsets from three different ‘inside’ (robot closer to wall than the desired offset) orientations, as shown in the following short videos:

‘Inside’ capture, with initial orientation angle > desired approach angle

‘Inside’ capture with initial orientation angle < desired approach angle

‘Inside’ capture with negative initial orientation angle

04 July 2021 Update:

After succeeding with the ‘inside’ cases, I started working on the ‘outside’ ones. This turned out to be considerably more difficult, as the larger distances from the wall caused considerable variation in the VL53L0X measurements (lower SNR?), which in turn produced more variation in the starting and ‘cut’ angles. However, the result does seem to be reasonably reliable, as shown in the following videos.

‘outside’ capture with initial outward angle

‘outside’ capture with initial inward angle

‘outside’ capture with small outward angle.

05 July Update:

After getting the left-side tracking algorithm working reasonably well, I ported the ‘TrackLeftWallOffset()’ functionality to ‘TrackRightWallOffset()’. After making (and mostly correcting) the usual number of mistakes, I got it going reasonably well, as shown in the following short videos:

Right wall tracking, starting inside desired offset, oriented toward wall

Right wall tracking, starting inside desired offset, oriented away from wall

Right wall tracking, starting outside desired offset, oriented toward wall

Right wall tracking, starting outside desired offset, oriented away from wall

Here is the complete code for my wall capture/track test program:

/*
    Name:       FourWD_WallTrackTest.ino
    Created:	6/18/2021 7:20:58 PM
    Author:     FRANKNEWXPS15\Frank
*/

/*
		Name:       FourWD_PulseTurnRateTest.ino
		Created:	5/24/2021 4:49:52 PM
		Author:     FRANKNEWXPS15\Frank
*/

#pragma region INCLUDES
#include <PID_v2.h>
#include <elapsedMillis.h>
#include <PrintEx.h> //allows printf-style printout syntax
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/05/19
#include "I2Cdev.h" //02/19/19: this includes SBWire.h
#include "I2C_Anything.h" //needed for sending float data over I2C
#pragma endregion INCLUDES

#pragma region DEFINES
#define MPU6050_I2C_ADDR 0x69
//#define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop
#pragma endregion DEFINES

#pragma region PRE_SETUP
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
MPU6050 mpu(MPU6050_I2C_ADDR); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0)
volatile int frontdistval = 0; //10/10/20 chg to volatile 
volatile double frontvar = 0; //08/11/20 now updated in timer1 ISR
const int MAX_FRONT_DISTANCE_CM = 400;
const int FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19
const int FRONT_DIST_ARRAY_SIZE = 50; //11/22/20 doubled to acct for 10Hz update rate
uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte

//11/03/18 added for new incremental variance calc
double Front_Dist_PrevVar = 0;
double Front_Dist_PrevVMean = 0;
#pragma endregion PRE_SETUP

#pragma region MOTOR_PINS
//09/11/20 Now using two Adafruit DRV8871 drivers for all 4 motors
const int In1_Left = 10;
const int In2_Left = 11;
const int In1_Right = 8;
const int In2_Right = 9;
#pragma endregion Motor Pin Assignments

#pragma region MISC_PIN_ASSIGNMENTS
const int RED_LASER_DIODE_PIN = 7;
const int LIDAR_MODE_PIN = 4; //mvd here 01/10/18
#pragma endregion MISC_PIN_ASSIGNMENTS

#pragma region MOTOR_PARAMETERS
//drive wheel speed parameters
const int MOTOR_SPEED_FULL = 200; //range is 0-255
const int MOTOR_SPEED_MAX = 255; //range is 0-255
const int MOTOR_SPEED_HALF = 127; //range is 0-255
const int MOTOR_SPEED_QTR = 75; //added 09/25/20
const int MOTOR_SPEED_LOW = 50; //added 01/22/15
const int MOTOR_SPEED_OFF = 0; //range is 0-255

//drive wheel direction constants
const boolean FWD_DIR = true;
const boolean REV_DIR = !FWD_DIR;
const bool TURNDIR_CCW = true;
const bool TURNDIR_CW = false;

//Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;
bool bIsForwardDir = true; //default is foward direction

#pragma endregion Motor Parameters

#pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS
elapsedMillis MsecSinceLastTurnRateUpdate;

const int MAX_PULSE_WIDTH_MSEC = 100;
const int MIN_PULSE_WIDTH_MSEC = 0;

const int MAX_SPIN_MOTOR_SPEED = 250;
const int MIN_SPIN_MOTOR_SPEED = 50;

const int TURN_RATE_UPDATE_INTERVAL_MSEC = 30;

double Prev_HdgDeg = 0;
float tgt_deg;
float timeout_sec;

//05/31/21 latest & greatest PID values
double TurnRate_Kp = 5.0;
double TurnRate_Ki = 1;
double TurnRate_Kd = 0.5;

double TurnRatePIDSetPointDPS, TurnRatePIDOutput;
double TurnRatePIDInput = 15.f;
PID TurnRatePID(&TurnRatePIDInput, &TurnRatePIDOutput, &TurnRatePIDSetPointDPS, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, DIRECT);

const float HDG_NEAR_MATCH_VAL = 0.8; //slow the turn down here
const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 05/17/20
const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started

#pragma endregion HEADING_AND_RATE_BASED_TURN_PARAMETERS

const double RATIO_TURN_MATCH_THRESHOLD = 0.1;

#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

						// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

//MPU6050 status flags
bool bMPU6050Ready = true;
bool dmpReady = false;  // set true if DMP init was successful
volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR
const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param
bool bFirstTime = true;
#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19
#pragma endregion MPU6050 Support

#pragma region WALL_FOLLOW_SUPPORT
const int WALL_OFFSET_TRACK_Kp = 100;
const int WALL_OFFSET_TRACK_Ki = 0;
const int WALL_OFFSET_TRACK_Kd = 0;
//const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.3;
//const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.4;
//const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.5;
//const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.15;
const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.25;
double WallTrackSteerVal, WallTrackOutput, WallTrackSetPoint;
const int CHG_CONNECT_LED_PIN = 50; //12/16/20 added for debug
const int WALL_OFFSET_TGTDIST_CM = 30;
double LeftSteeringVal, RightSteeringVal; //added 08/06/20
const int WALL_TRACK_UPDATE_INTERVAL_MSEC = 50;
//const int WALL_TRACK_UPDATE_INTERVAL_MSEC = 30;
#pragma endregion WALL_FOLLOW_SUPPORT

#pragma region VL53L0X LIDAR SUPPORT
//VL53L0X Sensor data values
//11/05/20 revised to make all volatile
volatile int Lidar_RightFront;
volatile int Lidar_RightCenter;
volatile int Lidar_RightRear;
volatile int Lidar_LeftFront;
volatile int Lidar_LeftCenter;
volatile int Lidar_LeftRear;
volatile int Lidar_Rear; //added 10/24/20

bool bVL53L0X_TeensyReady = false; //11/10/20 added to prevent bad data reads during Teensy setup()

enum VL53L0X_REQUEST
{
	VL53L0X_READYCHECK, //added 11/10/20 to prevent bad reads during Teensy setup()
	VL53L0X_CENTERS_ONLY,
	VL53L0X_RIGHT,
	VL53L0X_LEFT,
	VL53L0X_ALL, //added 08/06/20, replaced VL53L0X_BOTH 10/31/20
	VL53L0X_REAR_ONLY //added 10/24/20
};

const int VL53L0X_I2C_SLAVE_ADDRESS = 0x20; ////Teensy 3.5 VL53L0X ToF LIDAR controller

#pragma endregion VL53L0X LIDAR SUPPORT

//************************************************************************************************************************************
//*********************************************************** TIMER ISR **************************************************************
//************************************************************************************************************************************

#pragma region TIMER_ISR
#define TIMER_INT_OUTPUT_PIN 53 //scope monitor pin

bool GetRequestedVL53l0xValues(VL53L0X_REQUEST which);
volatile bool bTimeForNavUpdate = false;
const int TIMER5_INTERVAL_MSEC = 100; //05/15/21 added to eliminate magic number
int GetFrontDistCm();

ISR(TIMER5_COMPA_vect) //timer5 interrupt 5Hz
{
	digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);//so I can monitor interrupt timing
	delayMicroseconds(30); //leave in - so can see even with NO_STUCK defined

	bTimeForNavUpdate = true;

	//11/2/20 have to re-enable interrupts for I2C comms to work
	interrupts();
	GetRequestedVL53l0xValues(VL53L0X_ALL); //11/2/20 update all distances every 100mSec
	noInterrupts();

	frontdistval = GetFrontDistCm();
	digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);//so I can monitor interrupt timing
}
#pragma endregion TIMER_ISR

void setup()
{
	Serial.begin(115200);

#pragma region MPU6050
	mySerial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);
	mpu.initialize();

	// verify connection
	Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

	float StartSec = 0; //used to time MPU6050 init
	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// make sure it worked (returns 0 if successful)
	if (devStatus == 0)
	{
		// turn on the DMP, now that it's ready
		Serial.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

		// set our DMP Ready flag so the main loop() function knows it's okay to use it
		Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
		bMPU6050Ready = true;
		StartSec = millis() / 1000.f;
		mySerial.printf("MPU6050 Ready at %2.2f Sec\n", StartSec);
	}
	else
	{
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		Serial.print(F("DMP Initialization failed (code "));
		Serial.print(devStatus);
		Serial.println(F(")"));

		bMPU6050Ready = false;
	}
#pragma endregion MPU6050

#pragma region PIN_INITIALIZATION
	pinMode(LIDAR_MODE_PIN, INPUT); // Set LIDAR input monitor pin

#pragma endregion PIN_INITIALIZATION


#pragma region TIMER_INTERRUPT
	//09/18/20 can't use Timer1 cuz doing so conflicts with PWM on pins 10-12
	cli();//stop interrupts
	TCCR5A = 0;// set entire TCCR5A register to 0
	TCCR5B = 0;// same for TCCR5B
	TCNT5 = 0;//initialize counter value to 0

	// 06/15/21 rewritten to use defined interval constant rather than magic number
	//10/11/20 chg timer interval to 10Hz vs 5
	//OCR5A = 1562;// = (16*10^6) / (10*1024) - 1 (must be <65536)
	int ocr5a = (16 * 1e6) / ((1000 / TIMER5_INTERVAL_MSEC) * 1024) - 1;
	mySerial.printf("ocr5a = %d\n", ocr5a);
	OCR5A = ocr5a;
	TCCR5B |= (1 << WGM52);// turn on CTC mode
	TCCR5B |= (1 << CS52) | (1 << CS50);// Set CS10 and CS12 bits for 1024 prescaler
	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt
	sei();//allow interrupts
	pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);
#pragma endregion TIMER_INTERRUPT


	//02/08/21 mvd below ISR enable so this section can use ISR-generated VL53L0X distances
#pragma region VL53L0X_TEENSY

	mySerial.printf("Checking for Teensy 3.5 VL53L0X Controller at I2C addr 0x%x\n", VL53L0X_I2C_SLAVE_ADDRESS);
	while (!bVL53L0X_TeensyReady)
	{
		Wire.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);
		bVL53L0X_TeensyReady = (Wire.endTransmission() == 0);

		//mySerial.printf("%lu: VL53L0X Teensy Not Avail...\n", millis());
		delay(100);
	}

	mySerial.printf("Teensy available at %lu with bVL53L0X_TeensyReady = %d. Waiting for Teensy setup() to finish\n", millis(), bVL53L0X_TeensyReady);

	WaitForVL53L0XTeensy();

	//mySerial.printf("VL53L0X Teensy Ready at %lu\n\n", millis());
#pragma endregion VL53L0X_TEENSY

	//11/14/18 added this section for distance printout only
	//08/06/20 revised for VL53L0X support
#ifdef DISTANCES_ONLY
	TIMSK5 = 0;
	digitalWrite(RED_LASER_DIODE_PIN, HIGH); //enable the front laser dot
	mySerial.printf("\n------------ DISTANCES ONLY MODE!!! -----------------\n\n");

	int i = 0; //added 09/20/20 for in-line header display
	mySerial.printf("Msec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\tRear\n");
	while (true)
	{
		//if (bTimeForNavUpdate)
		//{
		//	bTimeForNavUpdate = false;

			//09/20/20 re-display the column headers
			//if (++i % 20 == 0)
			//{
			//	mySerial.printf("\nMsec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\tFront\tRear\n");
			//}

			//commented out 02/02/21 - now done in ISR
			GetRequestedVL53l0xValues(VL53L0X_ALL);

			mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
				millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
				Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, frontdistval, Lidar_Rear);

		//}

			delay(50);
	}
	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

#endif // DISTANCES_ONLY

#pragma region L/R/FRONT DISTANCE ARRAYS
	//09/20/20 have to do this for parallel finding to go the right way
	Serial.println(F("Initializing Left, Right, Front Distance Arrays..."));
#ifndef NO_PINGS
	//03/30/21 moved FrontDistArray init ahead of left/right dist init to prevent inadvertent 'stuck' detection
	Serial.println(F("Initializing Front Distance Array"));
	InitFrontDistArray(); //08/12/20 code extracted to fcn so can call elsewhere

	//Serial.println(F("Updating Left\tRight Distance Arrays"));
	//for (size_t i = 0; i < LR_DIST_ARRAY_SIZE; i++)
	//{
	//	delay(100); //ensure enough time for ISR to update distances
	//	mySerial.printf("%d\t%d\n", Lidar_LeftCenter, Lidar_RightCenter);
	//	UpdateLRDistanceArrays(Lidar_LeftCenter, Lidar_RightCenter);
	//}

	//Serial.println(F("Updating Rear Distance Arrays"));
	//InitRearDistArray();
	//mySerial.printf("Initial rear prev_mean, prev_var = %2.2f, %2.2f\n",
	//	Rear_Dist_PrevMean, Rear_Dist_PrevVar);
#else
	Serial.println(F("Distance Sensors Disabled"));
#endif // NO_PINGS
#pragma endregion L/R/FRONT DISTANCE ARRAYS

#pragma region WALL_TRACK_TEST
	mySerial.printf("End of test - Stopping!\n");
	//while (true)
	//{
	//	CheckForUserInput();
	//}
	StopBothMotors();
	Serial.println(F("Setting Kp value"));
	const int bufflen = 80;
	char buff[bufflen];
	memset(buff, 0, bufflen);
	float OffsetCm;
	float Kp, Ki, Kd;
	const char s[2] = ",";
	char* token;


	//consume CR/LF chars
	while (Serial.available() > 0)
	{
		int byte = Serial.read();
		mySerial.printf("consumed %d\n", byte);
	}

	while (!Serial.available())
	{
	}

	Serial.readBytes(buff, bufflen);
	mySerial.printf("%s\n", buff);

	/* extract Kp */
	token = strtok(buff, s);
	Kp = atof(token);

	/* extract Ki */
	token = strtok(NULL, s);
	Ki = atof(token);

	/* extract Kd */
	token = strtok(NULL, s);
	Kd = atof(token);

	/* extract Offset in cm */
	token = strtok(NULL, s);
	OffsetCm = atof(token);

	//mySerial.printf("Kp,Ki,Kd,OffsetCm =  %2.2f, %2.2f, %2.2f, %2.2f\n",
	//	Kp, Ki, Kd, OffsetCm);

	//TrackLeftWallOffset(Kp, Ki, Kd, OffsetCm);
	TrackRightWallOffset(Kp, Ki, Kd, OffsetCm);
	//mySerial.printf("Rdist\tCdist\tFdist\tsteer\toffang\n");
	//while (true)
	//{
	//	//double offangdeg = GetSteeringAngle(Lidar_LeftCenter / 10, LeftSteeringVal);
	//	double offangdeg = GetSteeringAngle(LeftSteeringVal); //06/25/21 chg to mm vs cm
	//	mySerial.printf("%d\t%d\t%d\t%2.4f\t%2.2f\n", Lidar_LeftRear, Lidar_LeftCenter, Lidar_LeftFront, LeftSteeringVal, offangdeg);
	//	delay(1000);
	//}
#pragma endregion WALL_TRACK_TEST
}

void loop()
{


}

void CheckForUserInput()
{
	const int bufflen = 80;
	char buff[bufflen];
	memset(buff, 0, bufflen);
	float OffsetCm;
	float Kp, Ki, Kd;
	const char s[2] = ",";
	char* token;

	if (Serial.available() > 0)
	{
		// read the incoming byte:
		int incomingByte = Serial.read();

		// say what you got:
		//Serial.print("I received: ");
		//Serial.println(incomingByte, HEX); //chg to HEX 02/18/20

		//02/18/20 experiment with multiple commands
		switch (incomingByte)
		{
		case 0x43: //ASCII 'C'
		case 0x63: //ASCII 'c'
#pragma region MANUALCONTROL
			//enter infinite loop for direct remote control
			Serial.println(F("ENTERING COMMAND MODE:"));
			Serial.println(F("0 = 180 deg CCW Turn"));
			Serial.println(F("1 = 180 deg CW Turn"));
			Serial.println(F("A = Back to Auto Mode"));
			Serial.println(F("S = Stop"));
			Serial.println(F("F = Forward"));
			Serial.println(F("R = Reverse"));
			Serial.println(F(""));
			Serial.println(F("       Faster"));
			Serial.println(F("\t8"));
			Serial.println(F("Left 4\t5  6 Right"));
			Serial.println(F("\t2"));
			Serial.println(F("       Slower"));

			StopBothMotors();
			int speed = 0;

			bool bAutoMode = false;
			while (!bAutoMode)
			{
				if (Serial.available() > 0)
				{
					// read the incoming byte:
					int incomingByte = Serial.read();

					//mySerial.printf("Got %x\n", incomingByte);

					switch (incomingByte)
					{
					case 0x54: //ASCII 'T'
					case 0x74: //ASCII 't'
						Serial.println(F("Toggle TIMER5 Enable/Disable"));
						if (TIMSK5 == 0)
						{
							Serial.println(F("Enable TIMER5"));
							//TIMSK5 |= OCIE5A;
							TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt
							mySerial.printf("TIMSK5 = %x\n", TIMSK5);
						}
						else
						{
							Serial.println(F("Disable TIMER5"));
							TIMSK5 = 0;
							mySerial.printf("TIMSK5 = %x\n", TIMSK5);
						}
						break;
					case 0x44: //ASCII 'D'
					case 0x64: //ASCII 'd'
						Serial.println(F("Display left/right distances"));
						if (TIMSK5 == 0)
						{
							Serial.println(F("Enable TIMER5"));
							//TIMSK5 |= OCIE5A;
							TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt
							mySerial.printf("TIMSK5 = %x\n", TIMSK5);
						}
						mySerial.printf("Msec\tLF\tLC\tLR\tRF\tRC\tRR\n");
						while (true)
						{
							if (bTimeForNavUpdate)
							{
								bTimeForNavUpdate = false;
								mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(),
									Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);
							}
						}
						break;
					case 0x30: //Dec '0'
						Serial.println(F("CCW 180 deg Turn"));
						//RollingTurn(true, bIsForwardDir, 180);
						//SpinTurn(true, 180, 45);
						SpinTurn(true, 180, 90);
						MoveAhead(speed, speed);
						break;
					case 0x31: //Dec '1'
						Serial.println(F("CW 180 deg Turn"));
						//RollingTurn(false, bIsForwardDir, 180);
						SpinTurn(false, 180, 45);
						//MoveAhead(speed, speed);
						break;
					case 0x34: //Turn left 10 deg
						Serial.println(F("CCW 10 deg Turn"));
						//MoveAhead(speed, speed);
						//RollingTurn(true, bIsForwardDir, 10);
						SpinTurn(true, 10, 30);
						//SpinTurn(true, 30, 30);

						//added 05/03/20
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x36: //Turn right 10 deg'
						mySerial.print("CW 10 deg Turn\n");
						MoveAhead(speed, speed);
						//RollingTurn(false, bIsForwardDir, 10);
						SpinTurn(false, 10, 30);
						//SpinTurn(false, 45, 30);

						//added 05/03/20
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x38: //Speed up 
						speed += 50;
						speed = (speed >= MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed;
						mySerial.printf("Speeding up: speed now %d\n", speed);
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x32: //Slow down 
						speed -= 50;
						speed = (speed < 0) ? 0 : speed;
						mySerial.printf("Slowing down: speed now %d\n", speed);
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x35: //05/07/20 changed to use '5' vs 'S'
						Serial.println(F("Stopping Motors!"));
						StopBothMotors();
						speed = 0;
						break;
					case 0x41: //ASCII 'A'
					case 0x61: //ASCII 'a'
						StopBothMotors();
						Serial.println(F("Re-entering AUTO mode"));
						bAutoMode = true;
						break;
					case 0x52: //ASCII 'R'
					case 0x72: //ASCII 'r'
						Serial.println(F("Setting both motors to reverse"));
						bIsForwardDir = false;
						MoveReverse(speed, speed);
						break;
					case 0x46: //ASCII 'F'
					case 0x66: //ASCII 'f'
						Serial.println(F("Setting both motors to forward"));
						bIsForwardDir = true;
						MoveAhead(speed, speed);
#pragma endregion MANUALCONTROL
						break;

#pragma region	WALL OFFSET TRACKING CASES
						//left side wall tracking
					case 0x4C: //ASCII 'L'
					case 0x6C: //ASCII 'l'
						StopBothMotors();
						Serial.println(F("Setting Kp value"));

						//consume CR/LF chars
						while (Serial.available() > 0)
						{
							int byte = Serial.read();
							mySerial.printf("consumed %d\n", byte);
						}

						while (!Serial.available())
						{
						}

						Serial.readBytes(buff, bufflen);
						mySerial.printf("%s\n", buff);

						/* extract Kp */
						token = strtok(buff, s);
						Kp = atof(token);

						/* extract Ki */
						token = strtok(NULL, s);
						Ki = atof(token);

						/* extract Kd */
						token = strtok(NULL, s);
						Kd = atof(token);

						/* extract Offset in cm */
						token = strtok(NULL, s);
						OffsetCm = atof(token);

						mySerial.printf("Kp,Ki,Kd,OffsetCm =  %2.2f, %2.2f, %2.2f, %2.2f\n",
							Kp, Ki, Kd, OffsetCm);

						TrackLeftWallOffset(Kp, Ki, Kd, OffsetCm);
						break;
					case 0x4D: //ASCII 'M'
					case 0x6D: //ASCII 'm'
						StopBothMotors();
						Serial.println(F("Setting Kp value"));

						//consume CR/LF chars
						while (Serial.available() > 0)
						{
							int byte = Serial.read();
							mySerial.printf("consumed %d\n", byte);
						}

						while (!Serial.available())
						{
						}

						Serial.readBytes(buff, bufflen);
						mySerial.printf("%s\n", buff);

						/* extract Kp */
						token = strtok(buff, s);
						Kp = atof(token);

						/* extract Ki */
						token = strtok(NULL, s);
						Ki = atof(token);

						/* extract Kd */
						token = strtok(NULL, s);
						Kd = atof(token);

						/* extract Offset in cm */
						token = strtok(NULL, s);
						OffsetCm = atof(token);

						mySerial.printf("Kp,Ki,Kd,OffsetCm =  %2.2f, %2.2f, %2.2f, %2.2f\n",
							Kp, Ki, Kd, OffsetCm);

						TrackRightWallOffset(Kp, Ki, Kd, OffsetCm);
						break;
#pragma endregion	WALL OFFSET TRACKING CASES
						Default:
						Serial.println(F("In Default Case: Stopping Motors!"));
						MoveAhead(0, 0);
						while (true)
						{

						}
					}
				}
			}
		}
	}
}

void TrackLeftWallOffset(double kp, double ki, double kd, double offsetCm)
{
	//Notes:
	//	06/21/21 modified to do a turn, then straight, then track 0 steer val

	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;
	bool bFirstTime = true;
	double spinRateDPS = 45;

	GetRequestedVL53l0xValues(VL53L0X_LEFT); //update LeftSteeringVal

	mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

	//06/21/21 modified to do a turn, then straight, then track 0 steer val
	double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_LeftCenter / 10.f);//positive inside, negative outside desired offset

	//07/05/21 implement 15deg min cut angle
	if (cutAngleDeg < 0 && abs(cutAngleDeg) < 15)
	{
		cutAngleDeg = -15;
	}
	else if (cutAngleDeg > 0 && cutAngleDeg < 15 )
	{
		cutAngleDeg = 15;
	}

	double SteerAngleDeg = GetSteeringAngle(LeftSteeringVal);

	mySerial.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n", 
		Lidar_LeftRear, Lidar_LeftCenter, Lidar_LeftFront, LeftSteeringVal, SteerAngleDeg, cutAngleDeg);

	double adjCutAngleDeg = 0;
	uint16_t fudgeFactorMM = 0; //07/05/21 added so can change for inside vs outside starting condx
	if (cutAngleDeg > 0) //robot inside desired offset distance
	{
		if (SteerAngleDeg > cutAngleDeg)  // --> SteerAngleDeg also > 0
		{
			mySerial.printf("if (SteerAngleDeg > cutAngleDeg)\n");
			SpinTurn(true, SteerAngleDeg - cutAngleDeg, spinRateDPS); //
		}
		else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg could be < 0
		{
			if (SteerAngleDeg <= 0 ) //cutAngleDeg > 0, SteerAngleDeg <= 0
			{
				mySerial.printf("if (SteerAngleDeg <= 0)\n");
				SpinTurn(false,cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg
			}
			else //SteerAngleDeg < cutAngleDeg but still > 0
			{
				mySerial.printf("else\n");
				SpinTurn(false,cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg
			}
		}

		fudgeFactorMM = -100; //07/05/21 don't need fudge factor for inside start condx
	}
	else // cutAngleDeg < 0 --> robot outside desired offset distance
	{
		if (SteerAngleDeg > cutAngleDeg)  // --> SteerAngleDeg may also be > 0
		{
			mySerial.printf("if (SteerAngleDeg > cutAngleDeg)\n");
			SpinTurn(true, SteerAngleDeg - cutAngleDeg, spinRateDPS); //
		}
		else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg must also be < 0
		{
			if (SteerAngleDeg <= 0) //cutAngleDeg > 0, SteerAngleDeg <= 0
			{
				mySerial.printf("if (SteerAngleDeg <= 0)\n");
				SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg
			}
			else //SteerAngleDeg < cutAngleDeg but still > 0
			{
				mySerial.printf("else\n");
				SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg
			}
		}

		fudgeFactorMM = 100; //07/05/21 need fudge factor for outside start condx
	}

	delay(1000);

	//adjust so offset capture occurs at correct perpendicular offset
	//double adjfactor = cos(PI * avgAppAngle / 180.f);
	double adjfactor = cos(PI * cutAngleDeg / 180.f);
	int adjOffsetMM = (int)(10 * (double)WALL_OFFSET_TGTDIST_CM / adjfactor);
	//adjOffsetMM += 100; //fudge factor for distance measurements lagging behind robot's travel. 
	adjOffsetMM += fudgeFactorMM; //fudge factor for distance measurements lagging behind robot's travel. 
	//mySerial.printf("at approach start: steerval = %2.3f, adj cut angle = %2.2f, adjfactor = %2.3f, tgt dist = %d\n", avgSteerVal, avgAppAngle, adjfactor, adjOffsetMM);
	mySerial.printf("at approach start: cut angle = %2.3f, adjfactor = %2.3f, tgt dist = %d\n", cutAngleDeg, adjfactor, adjOffsetMM);

	//long int prev_res = Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM; //neg for inside going out, pos for outside going in
	long int prev_res = Lidar_LeftFront - adjOffsetMM; //neg for inside going out, pos for outside going in
	long int res = prev_res;
	mySerial.printf("At start - prev_res = %ld\n", prev_res);

	mySerial.printf("Msec\tLFront\tLCtr\tLRear\tFront\tRear\tRes\tP_res\n");

	//TIMSK5 = 0; //turn off TIMER5 interrupts
	while (prev_res * res > 0) //sign change makes this < 0
	{
		
		prev_res = res;

		MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);
		while (!bTimeForNavUpdate)
		{

		}
		bTimeForNavUpdate = false;
		frontdistval = GetFrontDistCm();
		res = Lidar_LeftFront - adjOffsetMM; //06/29/21 now adj for slant dist vs perp dist
		mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",
			millis(),
			Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, frontdistval, Lidar_Rear, res, prev_res);
		CheckForUserInput();
	}

	//now turn back the same (unadjusted) amount
	mySerial.printf("At end - prev_res*res = %ld\n", prev_res*res);
	mySerial.printf("correct back to parallel\n");
	SpinTurn(!(cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

	//sinceLastComputeTime = 0;
		WallTrackSetPoint = 0; //moved here 6/22/21
		//TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

	mySerial.printf("Msec\tFdir\tCdir\tRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

	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);

				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);
			}
		}
	}
}

void TrackRightWallOffset(double kp, double ki, double kd, double offsetCm)
{
	//Notes:
	//	06/21/21 modified to do a turn, then straight, then track 0 steer val

	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;
	bool bFirstTime = true;
	double spinRateDPS = 45;

	GetRequestedVL53l0xValues(VL53L0X_RIGHT); //update RightSteeringVal = (Front - Rear)/100

	mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

	//06/21/21 modified to do a turn, then straight, then track 0 steer val
	double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_RightCenter / 10.f);//positive inside, negative outside desired offset

	//07/05/21 implement 15deg min cut angle
	if (cutAngleDeg < 0 && abs(cutAngleDeg) < 15)
	{
		cutAngleDeg = -15;
	}
	else if (cutAngleDeg > 0 && cutAngleDeg < 15)
	{
		cutAngleDeg = 15;
	}

	double SteerAngleDeg = GetSteeringAngle(RightSteeringVal);

	mySerial.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",
		Lidar_RightRear, Lidar_RightCenter, Lidar_RightFront, RightSteeringVal, SteerAngleDeg, cutAngleDeg);

	double adjCutAngleDeg = 0;
	uint16_t fudgeFactorMM = 0; //07/05/21 added so can change for inside vs outside starting condx
	if (cutAngleDeg > 0) //robot inside desired offset distance
	{
		if (SteerAngleDeg > cutAngleDeg)  // --> SteerAngleDeg also > 0 (pointing away from wall)
		{
			mySerial.printf("if (SteerAngleDeg > cutAngleDeg)\n");
			SpinTurn(false, SteerAngleDeg - cutAngleDeg, spinRateDPS); //
		}
		else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg could be < 0
		{
			if (SteerAngleDeg <= 0) //cutAngleDeg > 0, SteerAngleDeg <= 0
			{
				mySerial.printf("if (SteerAngleDeg <= 0)\n");
				//SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg
				SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg
			}
			else //SteerAngleDeg < cutAngleDeg but still > 0
			{
				mySerial.printf("else\n");
				//SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg
				SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg
			}
		}

		fudgeFactorMM = -100; //07/05/21 don't need fudge factor for inside start condx
	}
	else // cutAngleDeg < 0 --> robot outside desired offset distance
	{
		if (SteerAngleDeg > cutAngleDeg)  // --> SteerAngleDeg may also be > 0
		{
			mySerial.printf("if (SteerAngleDeg > cutAngleDeg)\n");
			SpinTurn(false, SteerAngleDeg - cutAngleDeg, spinRateDPS); //
		}
		else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg must also be < 0
		{
			if (SteerAngleDeg <= 0) //cutAngleDeg > 0, SteerAngleDeg <= 0
			{
				mySerial.printf("if (SteerAngleDeg <= 0)\n");
				//SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg
				SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg
			}
			else //SteerAngleDeg < cutAngleDeg but still > 0
			{
				mySerial.printf("else\n");
				//SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg
				SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg
			}
		}

		fudgeFactorMM = 100; //07/05/21 need fudge factor for outside start condx
	}
	//DEBUG!!
		//mySerial.printf("Msec\tFront\tCtr\ttRear\tSteer\tAngle\n");
		//double sumOfSteerVals = 0;
		//for (size_t i = 0; i < 10; i++)
		//{
		//	while (!bTimeForNavUpdate)
		//	{

		//	}
		//	bTimeForNavUpdate = false;
		//	//GetRequestedVL53l0xValues(VL53L0X_LEFT); //refresh steering value
		//	mySerial.printf("%lu\t%d\t%d\t%d\t%2.3f\t%2.2f\n",millis(), 
		//		Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, RightSteeringVal, GetSteeringAngle(RightSteeringVal));
		//	sumOfSteerVals += RightSteeringVal;
		//	//delay(100);
		//}

		//double avgSteerVal = sumOfSteerVals / 10.f;
		//double avgAppAngle = GetSteeringAngle(avgSteerVal);
		//mySerial.printf("avg steerval = %2.3f, avg angle = %2.3f\n", avgSteerVal, avgAppAngle);
	//DEBUG!!

	delay(1000);

	//adjust so offset capture occurs at correct perpendicular offset
	//double adjfactor = cos(PI * avgAppAngle / 180.f);
	double adjfactor = cos(PI * cutAngleDeg / 180.f);
	int adjOffsetMM = (int)(10 * (double)WALL_OFFSET_TGTDIST_CM / adjfactor);
	//adjOffsetMM += 100; //fudge factor for distance measurements lagging behind robot's travel. 
	adjOffsetMM += fudgeFactorMM; //fudge factor for distance measurements lagging behind robot's travel. 
	//mySerial.printf("at approach start: steerval = %2.3f, adj cut angle = %2.2f, adjfactor = %2.3f, tgt dist = %d\n", avgSteerVal, avgAppAngle, adjfactor, adjOffsetMM);
	mySerial.printf("at approach start: cut angle = %2.3f, adjfactor = %2.3f, tgt dist = %d\n", cutAngleDeg, adjfactor, adjOffsetMM);

	//long int prev_res = Lidar_RightFront - 10 * WALL_OFFSET_TGTDIST_CM; //neg for inside going out, pos for outside going in
	long int prev_res = Lidar_RightFront - adjOffsetMM; //neg for inside going out, pos for outside going in
	long int res = prev_res;
	mySerial.printf("At start - prev_res = %ld\n", prev_res);

	mySerial.printf("Msec\tLFront\tLCtr\tLRear\tFront\tRear\tRes\tP_res\n");

	//TIMSK5 = 0; //turn off TIMER5 interrupts
	while (prev_res * res > 0) //sign change makes this < 0
	{

		prev_res = res;

		MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);
		while (!bTimeForNavUpdate)
		{

		}
		bTimeForNavUpdate = false;
		frontdistval = GetFrontDistCm();
		res = Lidar_RightFront - adjOffsetMM; //06/29/21 now adj for slant dist vs perp dist
		mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",
			millis(),
			Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, frontdistval, Lidar_Rear, res, prev_res);
		CheckForUserInput();
	}

	//now turn back the same (unadjusted) amount
	mySerial.printf("At end - prev_res*res = %ld\n", prev_res * res);
	mySerial.printf("correct back to parallel\n");
	//SpinTurn(!(cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg
	SpinTurn((cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

	//sinceLastComputeTime = 0;
	WallTrackSetPoint = 0; //moved here 6/22/21
	//TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

	mySerial.printf("Msec\tFdir\tCdir\tRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

	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 = RightSteeringVal + (Lidar_RightCenter - 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, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
					kp, ki, kd, WallTrackOutput);

				int speed = 0;

				//07/05/21 reverse signs for right side tracking 
				//int leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;
				//int rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;
				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_RightFront, Lidar_RightCenter, Lidar_RightRear,
				//	WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,
				//	kp * lastError, ki * lastIval, kd * lastDerror,
				//	WallTrackOutput, 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_RightFront, Lidar_RightCenter, Lidar_RightRear,
					-WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,
					kp * lastError, ki * lastIval, kd * lastDerror,
					WallTrackOutput, leftspeednum, rightspeednum);
			}
		}
	}
}

#pragma region MOTOR SUPPORT
//09/08/20 modified for DRV8871 motor driver
void MoveReverse(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move in reverse direction continuously - companion to MoveAhead()
	//ProvEnA_Pinnce: G. Frank Paynter 09/08/18
	//Inputs:  
	//    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
	//    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
	//Outputs: both drive Motors energized with the specified speed
	//Plan:
	//    Step 1: Set reverse direction for both wheels
	//    Step 2: Run both Motors at specified speeds
	//Notes:
	//	01/22/20 now using Adafruit DRV8871 drivers

	//Step 1: Set reverse direction and speed for both wheels
	SetLeftMotorDirAndSpeed(REV_DIR, leftspeednum);
	SetRightMotorDirAndSpeed(REV_DIR, rightspeednum);
}

//09/08/20 modified for DRV8871 motor driver
void MoveAhead(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move ahead continuously
	//ProvEnA_Pinnce: G. Frank Paynter and Danny Frank 01/24/2014
	//Inputs:  
	//    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
	//    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
	//Outputs: both drive Motors energized with the specified speed
	//Plan:
	//    Step 1: Set forward direction for both wheels
	//    Step 2: Run both Motors at specified speeds
	//Notes:
	//	01/22/20 now using Adafruit DRV8871 drivers

	//mySerial.printf("InMoveAhead(%d,%d)\n", leftspeednum, rightspeednum);

	//Step 1: Set forward direction and speed for both wheels
	SetLeftMotorDirAndSpeed(true, leftspeednum);
	SetRightMotorDirAndSpeed(true, rightspeednum);
}

//09/08/10 modified for DRV8871 motor driver
void StopLeftMotors()
{
	analogWrite(In1_Left, MOTOR_SPEED_OFF);
	analogWrite(In2_Left, MOTOR_SPEED_OFF);
}

void StopRightMotors()
{
	analogWrite(In1_Right, MOTOR_SPEED_OFF);
	analogWrite(In2_Right, MOTOR_SPEED_OFF);
}

//09/08/20 added bool bisFwd param for DRV8871 motor driver
void RunBothMotors(bool bisFwd, int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors at left/rightspeednum speeds
	//Inputs:
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//    Step 1: Apply drive to both wheels
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//    01/24/15 - revised for continuous run - no timing
	//  01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()
	//	12/07/15 - chg args from &int to int
	//Step 1: Apply drive to both wheels

//DEBUG!!
	//mySerial.printf("In RunBothMotors(%s, %d,%d)\n", bisFwd? "FWD":"REV", leftspeednum, rightspeednum);
//DEBUG!!

	SetLeftMotorDirAndSpeed(bisFwd, leftspeednum);
	SetRightMotorDirAndSpeed(bisFwd, rightspeednum);
}

void RunBothMotorsBidirectional(int leftspeed, int rightspeed)
{
	//Purpose:  Accommodate the need for independent bidirectional wheel motion
	//Inputs:
	//	leftspeed - integer denoting left wheel speed.  Positive value is fwd, negative is rev
	//	rightspeed - integer denoting right wheel speed.  Positive value is fwd, negative is rev
	//Outputs:
	//	left/right wheel motors direction and speed set as appropriate
	//Plan:
	//	Step1: Set left wheel direction and speed
	//	Step2: Set right wheel direction and speed

//Step1: Set left wheel direction and speed

	//DEBUG!!
	//mySerial.printf("In RunBothMotorsBidirectional(%d, %d)\n", leftspeed, rightspeed);
	if (leftspeed < 0)
	{
		SetLeftMotorDirAndSpeed(false, -leftspeed); //neg value ==> reverse
	}
	else
	{
		SetLeftMotorDirAndSpeed(true, leftspeed); //pos or zero value ==> fwd
	}

	//Step2: Set right wheel direction and speed
	if (rightspeed < 0)
	{
		SetRightMotorDirAndSpeed(false, -rightspeed); //neg value ==> reverse
	}
	else
	{
		SetRightMotorDirAndSpeed(true, rightspeed); //pos or zero value ==> fwd
	}
}

//09/08/20 added bool bisFwd param for DRV8871 motor driver
void RunBothMotorsMsec(bool bisFwd, int timeMsec, int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors for timesec seconds at speednum speed
	//Inputs:
	//    timesec = time in seconds to run the Motors
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//    Step 1: Apply drive to both wheels
	//    Step 2: Delay timsec seconds
	//    Step 3: Remove drive from both wheels.
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//	11/25/15 - rev to use motor driver class object
	//	09/08/20 added bool bisFwd param for DRV8871 motor driver

	RunBothMotors(bisFwd, leftspeednum, rightspeednum);

	//Step 2: Delay timsec seconds
	delay(timeMsec);

	//Step3: Stop motors added 04/12/21
	StopBothMotors();
}

//11/25/15 added for symmetry ;-).
void StopBothMotors()
{
	StopLeftMotors();
	StopRightMotors();
}

void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)
{
	//mySerial.printf("SetLeftMotorDirAndSpeed(%d,%d)\n", bIsFwd, speed);

#ifndef NO_MOTORS
	if (bIsFwd)
	{
		digitalWrite(In1_Left, LOW);
		analogWrite(In2_Left, speed);
		//mySerial.printf("In TRUE block of SetLeftMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
	}
	else
	{
		//mySerial.printf("In FALSE block of SetLeftMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
		digitalWrite(In2_Left, LOW);
		analogWrite(In1_Left, speed);
	}
#endif // !NO_MOTORS
}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)
{
	//mySerial.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",
	//	(bIsFwd == true) ? "true" : "false", speed);

#ifndef NO_MOTORS

	if (bIsFwd)
	{
		//mySerial.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
		digitalWrite(In1_Right, LOW);
		analogWrite(In2_Right, speed);
	}
	else
	{
		//mySerial.printf("In FALSE block of SetRightMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
		digitalWrite(In2_Right, LOW);
		analogWrite(In1_Right, speed);
	}

#endif // !NO_MOTORS
}

#pragma endregion Motor Support Functions

#pragma region MPU5060 Support
float UpdateIMUHdgValDeg()
{
	//Purpose: Get latest yaw (heading) value from IMU
	//Inputs: None.  This function should only be called after mpu.dmpPacketAvailable() returns TRUE
	//Outputs: 
	//	returns true if successful, otherwise false
	//	IMUHdgValDeg updated on success
	//Plan:
	//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet
	//Step2: read all available packets to get to latest data
	//Step3: update IMUHdgValDeg with latest value
	//Notes:
	//	10/08/19 changed return type to boolean
	//	10/08/19 no longer need mpuIntStatus
	//	10/21/19 completely rewritten to use Homer's algorithm
	//	05/05/20 changed return type to float vs bool.

	bool retval = false;

	int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

	if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow
	{
		// display Euler angles in degrees
		mpu.dmpGetQuaternion(&q, fifoBuffer);
		mpu.dmpGetGravity(&gravity, &q);
		mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

		//compute the yaw value
		IMUHdgValDeg = ypr[0] * 180 / M_PI;
		retval = true;
	}

	//return retval;
	return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience
}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)
{
	mpu.resetFIFO();
	delay(1);
	//int countloop = 0;

	fifoCount = mpu.getFIFOCount();
	GetPacketLoopCount = 0;

	//mySerial.printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);
	while (fifoCount < packetSize && GetPacketLoopCount < max_loops)
	{
		GetPacketLoopCount++;
		fifoCount = mpu.getFIFOCount();
		delay(2);
	}

	//mySerial.printf("In GetCurrentFIFOPacket: after loop fifoC = %d, loop count = %d\n", fifoCount, GetPacketLoopCount);

	if (GetPacketLoopCount >= max_loops)
	{
		return 0;
	}

	//if we get to here, there should be exactly one packet in the FIFO
	mpu.getFIFOBytes(data, packetSize);
	return 1;
}
#pragma endregion MPU5060 Support

#pragma region HDG_BASED_TURN_SUPPORT

float GetHdgMatchVal(float tgt_deg, float cur_deg)
{
	//Purpose:  Compute the match ratio between two compass headings
	//Inputs:
	//	tgt_deg = float representing target heading in +/-180 range
	//	IMUHdgValDeg = float representing sensor yaw value in +/-180 deg range
	//Outputs:
	//	returns result of 1 - abs(Tgt_deg - Hdg_deg)/180, all angles in 0-360 deg range
	//Plan:
	//	Step1: convert both inputs to 0-360 deg range
	//	Step2: compute match ratio
	//Notes:
	//	formula from https://gis.stackexchange.com/questions/129954/comparing-compass-bearings

//Step1: convert both inputs to 0-360 deg range
	float tgthdg = (tgt_deg < 0) ? tgt_deg + 360 : tgt_deg;
	float curhdg = (cur_deg < 0) ? cur_deg + 360 : cur_deg;

	//Step2: compute match ratio
	float match_ratio = 1 - abs(tgthdg - curhdg) / 180;

	//DEBUG!!
		//mySerial.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio);
	//DEBUG!!
	return abs(match_ratio);
}

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)
{
	//Purpose: Make a numDeg CW or CCW 'spin' turn
	//Inputs:
	//	b_ccw - True if turn is to be ccw, false otherwise
	//	numDeg - angle to be swept out in the turn
	//	ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg
	//	IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR
	//	degPerSec = float value denoting desired turn rate
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Disable TIMER5 interrupts
	//	Step3: Compute new target value & timeout value
	//	Step4: Run motors until target reached, using inline PID algorithm to control turn rate
	//	Step5: Re-enable TIMER5 interrupts
	//Notes:
	//	06/06/21 we-written to remove PID library - now uses custom 'PIDCalcs()' function
	//	06/06/21 added re-try for 180.00 return from IMU - could be bad value
	//	06/11/21 added code to correct dHdg errors due to 179/-179 transition & bad IMU values
	//	06/12/21 cleaned up & commented out debug code

	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point

	//DEBUG!!
	mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);
	//DEBUG!!
	//mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",
	//	TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

	//no need to continue if the IMU isn't available
	if (!dmpReady)
	{
		return false;
	}

	//Step1: Get current heading as starting point
		//06/06/21 it is possible for IMU to return 180.00 on failure
		//so try again.  If it really IS 180, then 
		//it will eventually time out and go on
	int retries = 0;
	if (IMUHdgValDeg == 180.f && retries < 5)
	{
		//DEBUG!!
		mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");
		//DEBUG!!
		UpdateIMUHdgValDeg();
		retries++;
		delay(30);
	}

	//Step2: Compute new target value & timeout value
	timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

	//05/17/20 limit timeout_sec to 1 sec or more
	timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

	UpdateIMUHdgValDeg();

	//12/05/19 added #define back in to manage which direction increases yaw values
#ifdef MPU6050_CCW_INCREASES_YAWVAL
	tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;
#else
	tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

	//correct for -180/180 transition
	if (tgt_deg < -180)
	{
		tgt_deg += 360;
	}

	//07/29/19 bugfix
	if (tgt_deg > 180)
	{
		tgt_deg -= 360;
	}

	//DEBUG!!
	mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f  deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",
		IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);
	//DEBUG!!

	double curHdgMatchVal = 0;

	//09/08/18 added to bolster end-of-turn detection
	float prevHdgMatchVal = 0;
	float matchSlope = 0;


	//Step2: Disable TIMER5 interrupts
	TIMSK5 = 0; //disable timer compare interrupt

	//Step3: Run motors until target reached, using PID algorithm to control turn rate
	Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

	//DEBUG!!
	//mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg);

	//mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n");
	//DEBUG!!

	elapsedMillis sinceLastTimeCheck = 0;
	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;
	bool bFirstIMUHdg = true;

	while (!bDoneTurning && !bTimedOut)
	{
		//11/06/20 now just loops between ISR hits
		CheckForUserInput();

		//runs every 30mSec
		if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)
		{
			sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

			UpdateIMUHdgValDeg(); //update IMUHdgValDeg

			double dHdg = IMUHdgValDeg - Prev_HdgDeg;
			if (dHdg > 180)
			{
				dHdg -= 360;
				mySerial.printf("dHdg > 180 (%2.2f - %2.2f) - subtracting 360\n", IMUHdgValDeg, Prev_HdgDeg);
			}
			else if (dHdg < -180)
			{
				dHdg += 360;
				mySerial.printf("dHdg < -180 (%2.2f - %2.2f) - adding 360\n", IMUHdgValDeg, Prev_HdgDeg);
			}

			//watch for turn rates that are wildly off
			double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);
			if (rate > 3 * degPersec)
			{
				//DEBUG!!
				mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec);
				//DEBUG!!
				rate = degPersec;
			}

			//06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet
			if (bFirstIMUHdg)
			{
				bFirstIMUHdg = false;
				Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time
			}
			else
			{
				PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

				int speed = 0;

				(TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput;
				(TurnRatePIDOutput <= MOTOR_SPEED_LOW) ? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput;

				SetLeftMotorDirAndSpeed(!b_ccw, speed);
				SetRightMotorDirAndSpeed(b_ccw, speed);

				//check for nearly there and all the way there
				curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);
				matchSlope = curHdgMatchVal - prevHdgMatchVal;
				prevHdgMatchVal = curHdgMatchVal;

				//DEBUG!!
				//mySerial.printf("%lu\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%2.2f\t%d\n", 
				//	millis(), 
				//	IMUHdgValDeg,
				//	Prev_HdgDeg,
				//	dHdg,
				//	rate,
				//	degPersec, 
				//	lastError, 
				//	TurnRate_Kp * lastError,
				//	TurnRate_Ki * lastIval,
				//	TurnRate_Kd * lastDerror,
				//	speed);
				//DEBUG!!

				Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

				//look for full match
				bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL
					|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start
			}
		}

		bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

		if (bTimedOut)
		{
			//DEBUG!!
			mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);
			//DEBUG!!

			bResult = false;
		}
	}

	//DEBUG!!
	mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg);
	//DEBUG!!

//Step4: Re-enable TIMER5 interrupts
	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

	StopBothMotors();
	//delay(1000); //added 04/27/21 for debug
	return bResult;
}

//bool SpinTurnToSteerVal(bool b_ccw, float tgtSteerVal, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)
//{
//	//Purpose: Make a rate-controlled CW or CCW 'spin' turn to a desired steering value
//	//Inputs:
//	//	b_ccw - True if turn is to be ccw, false otherwise
//	//	steerval - steering value to be met
//	//	ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg
//	//	IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR
//	//	degPerSec = float value denoting desired turn rate
//	//Plan:
//	//	Step1: Get current heading as starting point
//	//	Step2: Disable TIMER5 interrupts
//	//	Step4: Run motors until target steering value reached, using inline PID algorithm to control turn rate
//	//	Step5: Re-enable TIMER5 interrupts
//	//Notes:
//	//	copied from SpinTurn() and modifed to terminate on steering value
//
//	//float tgt_deg;
//	float timeout_sec;
//	bool bDoneTurning = false;
//	bool bTimedOut = false;
//	bool bResult = true; //04/21/20 added so will be only one exit point
//
//	////DEBUG!!
//	//mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);
//	////DEBUG!!
//	mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",
//		TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);
//
//	//no need to continue if the IMU isn't available
//	if (!dmpReady)
//	{
//		return false;
//	}
//
//	//Step1: Get current heading as starting point
//		//06/06/21 it is possible for IMU to return 180.00 on failure
//		//so try again.  If it really IS 180, then 
//		//it will eventually time out and go on
//	int retries = 0;
//	if (IMUHdgValDeg == 180.f && retries < 5)
//	{
//		//DEBUG!!
//		//mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");
//		//DEBUG!!
//		UpdateIMUHdgValDeg();
//		retries++;
//		delay(30);
//	}
//
//	////Step2: Compute new target value & timeout value
//	//timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter
//
//	////05/17/20 limit timeout_sec to 1 sec or more
//	//timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;
//
//	UpdateIMUHdgValDeg();
//
////	//12/05/19 added #define back in to manage which direction increases yaw values
////#ifdef MPU6050_CCW_INCREASES_YAWVAL
////	tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;
////#else
////	tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;
////
////#endif // MPU6050_CCW_INCREASES_YAWVAL
////
////	//correct for -180/180 transition
////	if (tgt_deg < -180)
////	{
////		tgt_deg += 360;
////	}
////
////	//07/29/19 bugfix
////	if (tgt_deg > 180)
////	{
////		tgt_deg -= 360;
////	}
//
//	//DEBUG!!
//	//mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f  deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",
//	//	IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);
//	//DEBUG!!
//
//	//float curHdgMatchVal = 0;
//
//	////09/08/18 added to bolster end-of-turn detection
//	//float prevHdgMatchVal = 0;
//	//float matchSlope = 0;
//
//
//	//Step2: Disable TIMER5 interrupts
//	TIMSK5 = 0; //disable timer compare interrupt
//
//	//Step3: Run motors until target reached, using PID algorithm to control turn rate
//	Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop
//
//	//DEBUG!!
//	//mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg);
//
//	//mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n");
//	//DEBUG!!
//
//	elapsedMillis sinceLastTimeCheck = 0;
//	elapsedMillis sinceLastComputeTime = 0;
//	double lastError = 0;
//	double lastInput = 0;
//	double lastIval = 0;
//	double lastDerror = 0;
//	bool bFirstIMUHdg = true;
//
//	//while (!bDoneTurning && !bTimedOut)
//	while (!bDoneTurning)
//	{
//		//11/06/20 now just loops between ISR hits
//		CheckForUserInput();
//
//		//runs every 30mSec
//		if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)
//		{
//			sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;
//
//			UpdateIMUHdgValDeg(); //update IMUHdgValDeg
//
//			double dHdg = IMUHdgValDeg - Prev_HdgDeg;
//			if (dHdg > 180)
//			{
//				dHdg -= 360;
//				mySerial.printf("dHdg > 180 - subtracting 360\n");
//			}
//			else if (dHdg < -180)
//			{
//				dHdg += 360;
//				mySerial.printf("dHdg < -180 - adding 360\n");
//			}
//
//			//watch for turn rates that are wildly off
//			double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);
//			if (rate > 3 * degPersec)
//			{
//				//DEBUG!!
//				//mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec);
//				//DEBUG!!
//				rate = degPersec;
//			}
//
//			//06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet
//			if (bFirstIMUHdg)
//			{
//				bFirstIMUHdg = false;
//				Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time
//			}
//			else
//			{
//				PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);
//
//				int speed = 0;
//
//				(TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput;
//				(TurnRatePIDOutput <= MOTOR_SPEED_LOW) ? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput;
//
//				SetLeftMotorDirAndSpeed(!b_ccw, speed);
//				SetRightMotorDirAndSpeed(b_ccw, speed);
//
//				//check for nearly there and all the way there
//				//curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);
//				//matchSlope = curHdgMatchVal - prevHdgMatchVal;
//				//prevHdgMatchVal = curHdgMatchVal;
//
//				//DEBUG!!
//				mySerial.printf("%lu\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%2.2f\t%d\n", 
//					millis(), 
//					IMUHdgValDeg,
//					Prev_HdgDeg,
//					dHdg,
//					rate,
//					degPersec, 
//					lastError, 
//					TurnRate_Kp * lastError,
//					TurnRate_Ki * lastIval,
//					TurnRate_Kd * lastDerror,
//					speed);
//				//DEBUG!!
//
//				Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time
//
//				//look for full match
//				GetRequestedVL53l0xValues(VL53L0X_ALL);
//				bDoneTurning = (abs(LeftSteeringVal - tgtSteerVal) <= RATIO_TURN_MATCH_THRESHOLD);
//			}
//		}
//
//		//bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);
//
//		//if (bTimedOut)
//		//{
//		//	//DEBUG!!
//		//	//mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);
//		//	//DEBUG!!
//
//		//	bResult = false;
//		//}
//	}
//
//	//DEBUG!!
//	//mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg);
//	//DEBUG!!
//
////Step4: Re-enable TIMER5 interrupts
//	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt
//
//	StopBothMotors();
//	delay(1000); //added 04/27/21 for debug
//	return bResult;
//}

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)
{
	//Purpose:  Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR
	//Inputs:
	//	input = double denoting current input value (turn rate, speed, whatever)
	//	setpoint = double denoting desired setpoint in same units as input
	//	sampleTime = int denoting sample time to be used in calcs.
	//	lastError = ref to double denoting error saved from prev calc
	//	lastInput = ref to double denoting input saved from prev calc
	//	Kp/Ki/Kd = doubles denoting PID values to be used for calcs
	//	Output = ref to double denoting output from calc

	double error = setpoint - input;
	double dInput = (input - lastInput);
	lastIval += (error);
	double dErr = (error - lastError);

	//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr,
	//	Kp,Ki,Kd);

	/*Compute PID Output*/
	output = Kp * error + Ki * lastIval + Kd * dErr;

	/*Remember some variables for next time*/
	lastError = error;
	lastInput = input;
	lastDerror = dErr;
}
#pragma endregion HDG_BASED_TURN_SUPPORT

bool GetRequestedVL53l0xValues(VL53L0X_REQUEST which)
{
	//Purpose: Obtain VL53L0X ToF sensor data from Teensy sensor handler
	//Inputs: 
	//	which = VL53L0X_REQUEST value denoting which combination of value to retrieve
	//		VL53L0X_CENTERS_ONLY -> Just the left/right center sensor values
	//		VL53L0X_RIGHT -> All three right sensor values, in front/center/rear order
	//		VL53L0X_LEFT -> All three left sensor values, in front/center/rear order
	//		VL53L0X_ALL -> All seven sensor values, in left/right front/center/rear/rear order
	//		VL53L0X_REAR_ONLY -> added 10/24/20 Just the rear sensor reading

	//Outputs: 
	//	Requested sensor values, obtained via I2C from the VL53L0X sensor handler
	//	Returns TRUE if data retrieval successful, otherwise FALSE
	//Plan:
	//	Step1: Send request to VL53L0X handler
	//	Step2: get the requested data
	//Notes:
	// Copied from FourWD_WallE2_V4.ino's IsIRBeamAvail() and adapted
		// 08/05/20 added a VL53L0X_ALL request type
		// 01/24/21 added error detection/reporting

	//Step1: Send request to VL53L0X handler
	//DEBUG!!
		//mySerial.printf("Sending %d to slave\n", which);
	//DEBUG!!

	//mySerial.printf("In GetRequestedVL53l0xValues(%d)\n", (int)which);

	Wire.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);
	I2C_writeAnything((uint8_t)which);
	Wire.endTransmission();


	//Step2: get the requested data
	int readResult = 0;
	int data_size = 0;
	switch (which)
	{
	case VL53L0X_READYCHECK: //11/10/20 added to prevent bad reads during Teensy setup()
		Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(sizeof(bVL53L0X_TeensyReady)));
		readResult = I2C_readAnything(bVL53L0X_TeensyReady);
		break;

	case VL53L0X_CENTERS_ONLY:
		//just two data values needed here
		data_size = 2 * sizeof(int);
		Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(2 * sizeof(Lidar_RightCenter)));
		readResult = I2C_readAnything(Lidar_RightCenter);

		if (readResult > 0)
		{
			I2C_readAnything(Lidar_LeftCenter);
		}

		//DEBUG!!
				//mySerial.printf("VL53L0X_CENTERS_ONLY case: Got LC/RC = %d, %d\n", Lidar_LeftCenter, Lidar_RightCenter);
		//DEBUG!!

		break;
	case VL53L0X_RIGHT:
		//four data values needed here
		data_size = 3 * sizeof(int) + sizeof(float);

		//DEBUG!!
				//mySerial.printf("data_size = %d\n", data_size);
		//DEBUG!!

		Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);
		readResult = I2C_readAnything(Lidar_RightFront);

		if (readResult > 0)
		{
			readResult = I2C_readAnything(Lidar_RightCenter);
		}
		if (readResult > 0)
		{
			readResult = I2C_readAnything(Lidar_RightRear);
		}
		if (readResult > 0)
		{
			readResult = I2C_readAnything(RightSteeringVal);
		}

		//DEBUG!!
				//mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",
				//	Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, ToFSteeringVal);
		//DEBUG!!

		break;
	case VL53L0X_LEFT:
		//four data values needed here
		data_size = 3 * sizeof(int) + sizeof(float);

		Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);
		readResult = I2C_readAnything(Lidar_LeftFront);

		if (readResult > 0)
		{
			readResult = I2C_readAnything(Lidar_LeftCenter);
		}
		if (readResult > 0)
		{
			readResult = I2C_readAnything(Lidar_LeftRear);
		}
		if (readResult > 0)
		{
			readResult = I2C_readAnything(LeftSteeringVal);
		}

		//DEBUG!!
				//mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",
				//	Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal);
		//DEBUG!!

		break;
	case VL53L0X_ALL: //added 08/05/20, chg to VL53L0X_ALL 10/31/20
		//nine data values needed here - 7 ints and 2 floats
		data_size = 7 * sizeof(int) + 2 * sizeof(float); //10/31/20 added rear distance

		//mySerial.printf("In VL53L0X_ALL case with data_size = %d\n", data_size);

		Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

		//Lidar_LeftFront
		readResult = I2C_readAnything(Lidar_LeftFront);
		if (readResult != sizeof(Lidar_LeftFront))
		{
			mySerial.printf("Error reading Lidar_LeftFront\n");
		}

		//Lidar_LeftCenter
		readResult = I2C_readAnything(Lidar_LeftCenter);
		if (readResult != sizeof(Lidar_LeftCenter))
		{
			mySerial.printf("Error reading Lidar_LeftCenter\n");
		}

		//Lidar_LeftRear
		readResult = I2C_readAnything(Lidar_LeftRear);
		if (readResult != sizeof(Lidar_LeftRear))
		{
			mySerial.printf("Error reading Lidar_LeftRear\n");
		}

		//LeftSteeringVal
		readResult = I2C_readAnything(LeftSteeringVal);
		if (readResult != sizeof(LeftSteeringVal))
		{
			mySerial.printf("Error reading LeftSteeringVal\n");
		}

		//Lidar_RightFront
		readResult = I2C_readAnything(Lidar_RightFront);
		if (readResult != sizeof(Lidar_RightFront))
		{
			mySerial.printf("Error reading Lidar_RightFront\n");
		}

		//Lidar_RightCenter
		readResult = I2C_readAnything(Lidar_RightCenter);
		if (readResult != sizeof(Lidar_RightCenter))
		{
			mySerial.printf("Error reading Lidar_RightCenter\n");
		}

		//Lidar_RightRear
		readResult = I2C_readAnything(Lidar_RightRear);
		if (readResult != sizeof(Lidar_RightRear))
		{
			mySerial.printf("Error reading Lidar_RightRear\n");
		}

		//RightSteeringVal
		readResult = I2C_readAnything(RightSteeringVal);
		if (readResult != sizeof(RightSteeringVal))
		{
			mySerial.printf("Error reading LeftSteeringVal\n");
		}

		//Lidar_Rear
		readResult = I2C_readAnything(Lidar_Rear);
		if (readResult != sizeof(Lidar_Rear))
		{
			mySerial.printf("Error reading Lidar_Rear\n");
		}

		//mySerial.printf("%lu: VL53l0x - %d, %d, %d, %d, %d, %d, %d\n",
		//	millis(), 
		//	Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
		//	Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear,
		//	Lidar_Rear);
		break; //10/31/20 bugfix

	case VL53L0X_REAR_ONLY:
		//just ONE data value needed here
		data_size = sizeof(int);
		Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(sizeof(Lidar_Rear)));
		readResult = I2C_readAnything(Lidar_Rear);

		//DEBUG!!
				//mySerial.printf("VL53L0X_REAR_ONLY case: Got Rear = %d\n", Lidar_Rear);
		//DEBUG!!

		break;

	default:
		break;
	}

	return readResult > 0; //this is true only if all reads succeed
}

void WaitForVL53L0XTeensy()
{
	bVL53L0X_TeensyReady = false;
	while (!bVL53L0X_TeensyReady)
	{
		GetRequestedVL53l0xValues(VL53L0X_READYCHECK); //this updates bVL53L0X_TeensyReady
		//mySerial.printf("%lu: got %d from VL53L0X Teensy\n", millis(), bVL53L0X_TeensyReady);
		delay(100);
	}

	Serial.print(F("Teensy setup() finished at ")); Serial.printf("%lu mSec\n", millis());

	//now try to get a VL53L0X measurement
	//11/08/20 rev to loop until all distance sensors provide valid data
	//mySerial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\n");

	//mySerial.printf("%lu: %d\t%d\t%d\t%d\t%d\t%d\t%d\n",
	//	millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
	//	Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, Lidar_Rear);

	while (Lidar_LeftFront <= 0 || Lidar_LeftCenter <= 0 || Lidar_LeftRear <= 0
		|| Lidar_LeftFront <= 0 || Lidar_LeftCenter <= 0 || Lidar_LeftRear <= 0
		|| Lidar_Rear <= 0)
	{
		////values updated 10 times/sec in ISR
		mySerial.printf("%lu: %d\t%d\t%d\t%d\t%d\t%d\t%d\n",
			millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
			Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, Lidar_Rear);

		delay(100);
	}

	mySerial.printf("VL53L0X Teensy Ready at %lu\n\n", millis());
}

double GetSteeringAngle(double steerval)
{
	//Purpose:  Convert a steering angle into the equivalent off-parallel orientation in degrees
	//Inputs:
	//	steerval = double value denoting current steering value
	//	Notes:
	//	06/27/21 now steering_val/off_angle slope is a constant 0.0175
	const double slopeval = 0.0175;
	double offparalleldeg = steerval / slopeval; //e.g. for steerval = -0.187 @ 10cm offset, returns +21deg
	//mySerial.printf("GetSteeringAngle(%d, %2.4f): slopeval = %2.4f, res %2.2f\n", ctrdist_cm, steerval, slopeval, offparalleldeg);
	return offparalleldeg;
}

//11/05/15 added to get LIDAR measurement
int GetFrontDistCm()
{
	//Notes:
	//	12/05/15 chg to MODE line vs I2C
	//	01/06/16 rev to return avg of prev distances on error 

#ifndef NO_LIDAR
	unsigned long pulse_width;
	int LIDARdistCm;

	pulse_width = pulseIn(LIDAR_MODE_PIN, HIGH); // Count how long the pulse is high in microseconds
	LIDARdistCm = pulse_width / 10; // 10usec = 1 cm of distance for LIDAR-Lite

	//chk for erroneous reading
	if (LIDARdistCm == 0)
	{
		//replace with average of last three readings from aFrontDist
		int avgdist = GetAvgFrontDistCm();
		mySerial.printf("%lu: Error in GetFrontDistCm() - %d replaced with %d\n", millis(), LIDARdistCm, avgdist);
		LIDARdistCm = avgdist;
	}

	//04/30/17 added limit detection/correction
	LIDARdistCm = (LIDARdistCm > 0) ? LIDARdistCm : MAX_FRONT_DISTANCE_CM;

	return LIDARdistCm;
#else
	return 10; //safe number, I hope
#endif
}

//04/25/21 rewritten to use aFrontDist[] values
float GetAvgFrontDistCm()
{
	int avgdist = 0;
	for (int i = 0; i < FRONT_DIST_AVG_WINDOW_SIZE; i++)
	{
		//DEBUG!!
		//mySerial.printf("frontdist[%d] = %d\n", FRONT_DIST_ARRAY_SIZE - 1 - i, aFrontDist[FRONT_DIST_ARRAY_SIZE - 1 - i]);
		//DEBUG!!
		avgdist += aFrontDist[FRONT_DIST_ARRAY_SIZE - 1 - i];
	}
	//DEBUG!!
	//mySerial.printf("avgdisttot = %d\n", avgdist);
	//DEBUG!!

	avgdist = (int)((float)avgdist / (float)FRONT_DIST_AVG_WINDOW_SIZE);
	//DEBUG!!
	//mySerial.printf("avgdist = %d\n", avgdist);
	//DEBUG!!
	return avgdist;
}

//08/12/20 Extracted inline FRONT_DIST_ARRAY init code so can be called from anywhere
void InitFrontDistArray()
{
	//04/01/15 initialize 'stuck detection' arrays
	//06/17/20 re-wrote for better readability
	//to ensure var > STUCK_FRONT_VARIANCE_THRESHOLD for first FRONT_DIST_ARRAY_SIZE loops
	//array is initialized with sawtooth from 0 to MAX_FRONT_DISTANCE_CM
	int newval = 0;
	int bumpval = MAX_FRONT_DISTANCE_CM / FRONT_DIST_ARRAY_SIZE;
	bool bgoingUp = true;

	for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++)
	{
		aFrontDist[i] = newval;
		//DEBUG!!
		//mySerial.printf("i = %d, newval = %d, aFrontdist[%d] = %d\n", i, newval, i, aFrontDist[i]);
		//DEBUG!!
		if (bgoingUp)
		{

			if (newval < MAX_FRONT_DISTANCE_CM - bumpval) //don't want newval > MAX_FRONT_DISTANCE_CM
			{
				newval += bumpval;
			}
			else
			{
				bgoingUp = false;
			}
		}
		else
		{
			if (newval > bumpval) //don't want newval < 0
			{
				newval -= bumpval;
			}
			else
			{
				bgoingUp = true;
			}
		}
	}

	//04/19/19 init Front_Dist_PrevVMean  & Front_Dist_PrevVar to mean/var respectively
	long sum = 0;
	for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++)
	{
		sum += aFrontDist[i]; //adds in rest of values
	}
	Front_Dist_PrevVMean = (float)sum / (float)FRONT_DIST_ARRAY_SIZE;

	//	Step2: calc new 'brute force' variance
	float sumsquares = 0;
	for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++)
	{
		sumsquares += (aFrontDist[i] - Front_Dist_PrevVMean) * (aFrontDist[i] - Front_Dist_PrevVMean);
	}

	Front_Dist_PrevVar = sumsquares / FRONT_DIST_ARRAY_SIZE;
	mySerial.printf("%lu: aFrontDist Init: Front_Dist_PrevVMean = %3.2f, Front_Dist_PrevVar = %3.2f\n", millis(), Front_Dist_PrevVMean, Front_Dist_PrevVar);
	frontvar = Front_Dist_PrevVar; //added 03/30/21 to prevent 'stuck' at startup
}

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

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

751

752

753

754

755

756

757

758

759

760

761

762

763

764

765

766

767

768

769

770

771

772

773

774

775

776

777

778

779

780

781

782

783

784

785

786

787

788

789

790

791

792

793

794

795

796

797

798

799

800

801

802

803

804

805

806

807

808

809

810

811

812

813

814

815

816

817

818

819

820

821

822

823

824

825

826

827

828

829

830

831

832

833

834

835

836

837

838

839

840

841

842

843

844

845

846

847

848

849

850

851

852

853

854

855

856

857

858

859

860

861

862

863

864

865

866

867

868

869

870

871

872

873

874

875

876

877

878

879

880

881

882

883

884

885

886

887

888

889

890

891

892

893

894

895

896

897

898

899

900

901

902

903

904

905

906

907

908

909

910

911

912

913

914

915

916

917

918

919

920

921

922

923

924

925

926

927

928

929

930

931

932

933

934

935

936

937

938

939

940

941

942

943

944

945

946

947

948

949

950

951

952

953

954

955

956

957

958

959

960

961

962

963

964

965

966

967

968

969

970

971

972

973

974

975

976

977

978

979

980

981

982

983

984

985

986

987

988

989

990

991

992

993

994

995

996

997

998

999

1000

1001

1002

1003

1004

1005

1006

1007

1008

1009

1010

1011

1012

1013

1014

1015

1016

1017

1018

1019

1020

1021

1022

1023

1024

1025

1026

1027

1028

1029

1030

1031

1032

1033

1034

1035

1036

1037

1038

1039

1040

1041

1042

1043

1044

1045

1046

1047

1048

1049

1050

1051

1052

1053

1054

1055

1056

1057

1058

1059

1060

1061

1062

1063

1064

1065

1066

1067

1068

1069

1070

1071

1072

1073

1074

1075

1076

1077

1078

1079

1080

1081

1082

1083

1084

1085

1086

1087

1088

1089

1090

1091

1092

1093

1094

1095

1096

1097

1098

1099

1100

1101

1102

1103

1104

1105

1106

1107

1108

1109

1110

1111

1112

1113

1114

1115

1116

1117

1118

1119

1120

1121

1122

1123

1124

1125

1126

1127

1128

1129

1130

1131

1132

1133

1134

1135

1136

1137

1138

1139

1140

1141

1142

1143

1144

1145

1146

1147

1148

1149

1150

1151

1152

1153

1154

1155

1156

1157

1158

1159

1160

1161

1162

1163

1164

1165

1166

1167

1168

1169

1170

1171

1172

1173

1174

1175

1176

1177

1178

1179

1180

1181

1182

1183

1184

1185

1186

1187

1188

1189

1190

1191

1192

1193

1194

1195

1196

1197

1198

1199

1200

1201

1202

1203

1204

1205

1206

1207

1208

1209

1210

1211

1212

1213

1214

1215

1216

1217

1218

1219

1220

1221

1222

1223

1224

1225

1226

1227

1228

1229

1230

1231

1232

1233

1234

1235

1236

1237

1238

1239

1240

1241

1242

1243

1244

1245

1246

1247

1248

1249

1250

1251

1252

1253

1254

1255

1256

1257

1258

1259

1260

1261

1262

1263

1264

1265

1266

1267

1268

1269

1270

1271

1272

1273

1274

1275

1276

1277

1278

1279

1280

1281

1282

1283

1284

1285

1286

1287

1288

1289

1290

1291

1292

1293

1294

1295

1296

1297

1298

1299

1300

1301

1302

1303

1304

1305

1306

1307

1308

1309

1310

1311

1312

1313

1314

1315

1316

1317

1318

1319

1320

1321

1322

1323

1324

1325

1326

1327

1328

1329

1330

1331

1332

1333

1334

1335

1336

1337

1338

1339

1340

1341

1342

1343

1344

1345

1346

1347

1348

1349

1350

1351

1352

1353

1354

1355

1356

1357

1358

1359

1360

1361

1362

1363

1364

1365

1366

1367

1368

1369

1370

1371

1372

1373

1374

1375

1376

1377

1378

1379

1380

1381

1382

1383

1384

1385

1386

1387

1388

1389

1390

1391

1392

1393

1394

1395

1396

1397

1398

1399

1400

1401

1402

1403

1404

1405

1406

1407

1408

1409

1410

1411

1412

1413

1414

1415

1416

1417

1418

1419

1420

1421

1422

1423

1424

1425

1426

1427

1428

1429

1430

1431

1432

1433

1434

1435

1436

1437

1438

1439

1440

1441

1442

1443

1444

1445

1446

1447

1448

1449

1450

1451

1452

1453

1454

1455

1456

1457

1458

1459

1460

1461

1462

1463

1464

1465

1466

1467

1468

1469

1470

1471

1472

1473

1474

1475

1476

1477

1478

1479

1480

1481

1482

1483

1484

1485

1486

1487

1488

1489

1490

1491

1492

1493

1494

1495

1496

1497

1498

1499

1500

1501

1502

1503

1504

1505

1506

1507

1508

1509

1510

1511

1512

1513

1514

1515

1516

1517

1518

1519

1520

1521

1522

1523

1524

1525

1526

1527

1528

1529

1530

1531

1532

1533

1534

1535

1536

1537

1538

1539

1540

1541

1542

1543

1544

1545

1546

1547

1548

1549

1550

1551

1552

1553

1554

1555

1556

1557

1558

1559

1560

1561

1562

1563

1564

1565

1566

1567

1568

1569

1570

1571

1572

1573

1574

1575

1576

1577

1578

1579

1580

1581

1582

1583

1584

1585

1586

1587

1588

1589

1590

1591

1592

1593

1594

1595

1596

1597

1598

1599

1600

1601

1602

1603

1604

1605

1606

1607

1608

1609

1610

1611

1612

1613

1614

1615

1616

1617

1618

1619

1620

1621

1622

1623

1624

1625

1626

1627

1628

1629

1630

1631

1632

1633

1634

1635

1636

1637

1638

1639

1640

1641

1642

1643

1644

1645

1646

1647

1648

1649

1650

1651

1652

1653

1654

1655

1656

1657

1658

1659

1660

1661

1662

1663

1664

1665

1666

1667

1668

1669

1670

1671

1672

1673

1674

1675

1676

1677

1678

1679

1680

1681

1682

1683

1684

1685

1686

1687

1688

1689

1690

1691

1692

1693

1694

1695

1696

1697

1698

1699

1700

1701

1702

1703

1704

1705

1706

1707

1708

1709

1710

1711

1712

1713

1714

1715

1716

1717

1718

1719

1720

1721

1722

1723

1724

1725

1726

1727

1728

1729

1730

1731

1732

1733

1734

1735

1736

1737

1738

1739

1740

1741

1742

1743

1744

1745

1746

1747

1748

1749

1750

1751

1752

1753

1754

1755

1756

1757

1758

1759

1760

1761

1762

1763

1764

1765

1766

1767

1768

1769

1770

1771

1772

1773

1774

1775

1776

1777

1778

1779

1780

1781

1782

1783

1784

1785

1786

1787

1788

1789

1790

1791

1792

1793

1794

1795

1796

1797

1798

1799

1800

1801

1802

1803

1804

1805

1806

1807

1808

1809

1810

1811

1812

1813

1814

1815

1816

1817

1818

1819

1820

1821

1822

1823

1824

1825

1826

1827

1828

1829

1830

1831

1832

1833

1834

1835

1836

1837

1838

1839

1840

1841

1842

1843

1844

1845

1846

1847

1848

1849

1850

1851

1852

1853

1854

1855

1856

1857

1858

1859

1860

1861

1862

1863

1864

1865

1866

1867

1868

1869

1870

1871

1872

1873

1874

1875

1876

1877

1878

1879

1880

1881

1882

1883

1884

1885

1886

1887

1888

1889

1890

1891

1892

1893

1894

1895

1896

1897

1898

1899

1900

1901

1902

1903

1904

1905

1906

1907

1908

1909

1910

1911

1912

1913

1914

1915

1916

1917

1918

1919

1920

1921

1922

1923

1924

1925

1926

1927

1928

1929

1930

1931

1932

1933

1934

1935

1936

1937

1938

1939

1940

1941

1942

1943

1944

1945

1946

1947

1948

1949

1950

1951

1952

1953

1954

1955

1956

1957

1958

1959

1960

1961

1962

1963

1964

1965

1966

1967

1968

1969

1970

1971

1972

1973

1974

1975

1976

1977

1978

1979

1980

1981

1982

1983

1984

1985

1986

1987

1988

1989

1990

1991

1992

1993

1994

1995

1996

1997

1998

1999

2000

2001

2002

2003

2004

2005

2006

2007

2008

2009

2010

2011

2012

2013

2014

2015

2016

2017

2018

2019

2020

2021

2022

2023

2024

2025

2026

2027

2028

2029

2030

2031

2032

2033

2034

2035

2036

2037

2038

2039

2040

2041

2042

2043

2044

2045

2046

2047

2048

2049

2050

2051

2052

2053

2054

2055

2056

2057

2058

2059

2060

2061

2062

2063

2064

2065

2066

2067

2068

2069

2070

2071

2072

2073

2074

2075

2076

2077

2078

2079

2080

2081

2082

2083

2084

2085

2086

2087

2088

2089

2090

2091

2092

2093

2094

2095

2096

2097

2098

2099

2100

2101

2102

2103

2104

2105

2106

2107

2108

2109

2110

2111

2112

2113

2114

2115

2116

2117

2118

2119

2120

2121

2122

2123

2124

2125

2126

2127

2128

2129

2130

2131

2132

2133

2134

2135

2136

2137

2138

2139

2140

2141

2142

2143

2144

2145

2146

2147

2148

2149

2150

2151

2152

2153

2154

2155

2156

2157

2158

2159

2160

2161

2162

2163

2164

2165

2166

2167

2168

2169

2170

2171

2172

2173

2174

2175

2176

2177

2178

2179

2180

2181

2182

2183

2184

2185

2186

2187

2188

2189

2190

2191

2192

2193

2194

2195

2196

2197

2198

2199

2200

2201

2202

2203

2204

2205

2206

2207

2208

2209

2210

2211

2212

2213

2214

2215

2216

2217

2218

2219

2220

2221

2222

2223

2224

2225

2226

2227

2228

2229

2230

2231

2232

2233

2234

2235

2236

2237

2238

2239

2240

2241

2242

2243

2244

2245

2246

2247

2248

2249

Name: FourWD_WallTrackTest.ino

Created: 6/18/2021 7:20:58 PM

Author: FRANKNEWXPS15\Frank

Name: FourWD_PulseTurnRateTest.ino

Created: 5/24/2021 4:49:52 PM

Author: FRANKNEWXPS15\Frank

#pragma region INCLUDES

#include <PID_v2.h>

#include <elapsedMillis.h>

#include <PrintEx.h> //allows printf-style printout syntax

#include "MPU6050_6Axis_MotionApps_V6_12.h" //changed to this version 10/05/19

#include "I2Cdev.h" //02/19/19: this includes SBWire.h

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

#pragma endregion INCLUDES

#pragma region DEFINES

#define MPU6050_I2C_ADDR 0x69

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

#pragma endregion DEFINES

#pragma region PRE_SETUP

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

MPU6050 mpu(MPU6050_I2C_ADDR); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0)

volatile int frontdistval = 0; //10/10/20 chg to volatile

volatile double frontvar = 0; //08/11/20 now updated in timer1 ISR

const int MAX_FRONT_DISTANCE_CM = 400;

const int FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19

const int FRONT_DIST_ARRAY_SIZE = 50; //11/22/20 doubled to acct for 10Hz update rate

uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte

//11/03/18 added for new incremental variance calc

double Front_Dist_PrevVar = 0;

double Front_Dist_PrevVMean = 0;

#pragma endregion PRE_SETUP

#pragma region MOTOR_PINS

//09/11/20 Now using two Adafruit DRV8871 drivers for all 4 motors

const int In1_Left = 10;

const int In2_Left = 11;

const int In1_Right = 8;

const int In2_Right = 9;

#pragma endregion Motor Pin Assignments

#pragma region MISC_PIN_ASSIGNMENTS

const int RED_LASER_DIODE_PIN = 7;

const int LIDAR_MODE_PIN = 4; //mvd here 01/10/18

#pragma endregion MISC_PIN_ASSIGNMENTS

#pragma region MOTOR_PARAMETERS

//drive wheel speed parameters

const int MOTOR_SPEED_FULL = 200; //range is 0-255

const int MOTOR_SPEED_MAX = 255; //range is 0-255

const int MOTOR_SPEED_HALF = 127; //range is 0-255

const int MOTOR_SPEED_QTR = 75; //added 09/25/20

const int MOTOR_SPEED_LOW = 50; //added 01/22/15

const int MOTOR_SPEED_OFF = 0; //range is 0-255

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

const bool TURNDIR_CCW = true;

const bool TURNDIR_CW = false;

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

bool bIsForwardDir = true; //default is foward direction

#pragma endregion Motor Parameters

#pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS

elapsedMillis MsecSinceLastTurnRateUpdate;

const int MAX_PULSE_WIDTH_MSEC = 100;

const int MIN_PULSE_WIDTH_MSEC = 0;

const int MAX_SPIN_MOTOR_SPEED = 250;

const int MIN_SPIN_MOTOR_SPEED = 50;

const int TURN_RATE_UPDATE_INTERVAL_MSEC = 30;

double Prev_HdgDeg = 0;

float tgt_deg;

float timeout_sec;

//05/31/21 latest & greatest PID values

double TurnRate_Kp = 5.0;

double TurnRate_Ki = 1;

double TurnRate_Kd = 0.5;

double TurnRatePIDSetPointDPS, TurnRatePIDOutput;

double TurnRatePIDInput = 15.f;

PID TurnRatePID(&TurnRatePIDInput, &TurnRatePIDOutput, &TurnRatePIDSetPointDPS, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, DIRECT);

const float HDG_NEAR_MATCH_VAL = 0.8; //slow the turn down here

const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 05/17/20

const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started

#pragma endregion HEADING_AND_RATE_BASED_TURN_PARAMETERS

const double RATIO_TURN_MATCH_THRESHOLD = 0.1;

#pragma region MPU6050_SUPPORT

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float euler[3]; // [psi, theta, phi] Euler angle container

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

int GetPacketLoopCount = 0;

int OuterGetPacketLoopCount = 0;

//MPU6050 status flags

bool bMPU6050Ready = true;

bool dmpReady = false; // set true if DMP init was successful

volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR

const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param

bool bFirstTime = true;

#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19

#pragma endregion MPU6050 Support

#pragma region WALL_FOLLOW_SUPPORT

const int WALL_OFFSET_TRACK_Kp = 100;

const int WALL_OFFSET_TRACK_Ki = 0;

const int WALL_OFFSET_TRACK_Kd = 0;

//const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.3;

//const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.4;

//const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.5;

//const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.15;

const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.25;

double WallTrackSteerVal, WallTrackOutput, WallTrackSetPoint;

const int CHG_CONNECT_LED_PIN = 50; //12/16/20 added for debug

const int WALL_OFFSET_TGTDIST_CM = 30;

double LeftSteeringVal, RightSteeringVal; //added 08/06/20

const int WALL_TRACK_UPDATE_INTERVAL_MSEC = 50;

//const int WALL_TRACK_UPDATE_INTERVAL_MSEC = 30;

#pragma endregion WALL_FOLLOW_SUPPORT

#pragma region VL53L0X LIDAR SUPPORT

//VL53L0X Sensor data values

//11/05/20 revised to make all volatile

volatile int Lidar_RightFront;

volatile int Lidar_RightCenter;

volatile int Lidar_RightRear;

volatile int Lidar_LeftFront;

volatile int Lidar_LeftCenter;

volatile int Lidar_LeftRear;

volatile int Lidar_Rear; //added 10/24/20

bool bVL53L0X_TeensyReady = false; //11/10/20 added to prevent bad data reads during Teensy setup()

enum VL53L0X_REQUEST

{

VL53L0X_READYCHECK, //added 11/10/20 to prevent bad reads during Teensy setup()

VL53L0X_CENTERS_ONLY,

VL53L0X_RIGHT,

VL53L0X_LEFT,

VL53L0X_ALL, //added 08/06/20, replaced VL53L0X_BOTH 10/31/20

VL53L0X_REAR_ONLY //added 10/24/20

};

const int VL53L0X_I2C_SLAVE_ADDRESS = 0x20; ////Teensy 3.5 VL53L0X ToF LIDAR controller

#pragma endregion VL53L0X LIDAR SUPPORT

//************************************************************************************************************************************

//*********************************************************** TIMER ISR **************************************************************

//************************************************************************************************************************************

#pragma region TIMER_ISR

#define TIMER_INT_OUTPUT_PIN 53 //scope monitor pin

bool GetRequestedVL53l0xValues(VL53L0X_REQUEST which);

volatile bool bTimeForNavUpdate = false;

const int TIMER5_INTERVAL_MSEC = 100; //05/15/21 added to eliminate magic number

int GetFrontDistCm();

ISR(TIMER5_COMPA_vect) //timer5 interrupt 5Hz

{

digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);//so I can monitor interrupt timing

delayMicroseconds(30); //leave in - so can see even with NO_STUCK defined

bTimeForNavUpdate = true;

//11/2/20 have to re-enable interrupts for I2C comms to work

interrupts();

GetRequestedVL53l0xValues(VL53L0X_ALL); //11/2/20 update all distances every 100mSec

noInterrupts();

frontdistval = GetFrontDistCm();

digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);//so I can monitor interrupt timing

}

#pragma endregion TIMER_ISR

void setup()

{

Serial.begin(115200);

#pragma region MPU6050

mySerial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);

mpu.initialize();

// verify connection

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

float StartSec = 0; //used to time MPU6050 init

Serial.println(F("Initializing DMP..."));

devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if successful)

if (devStatus == 0)

{

// turn on the DMP, now that it's ready

Serial.println(F("Enabling DMP..."));

mpu.setDMPEnabled(true);

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

bMPU6050Ready = true;

StartSec = millis() / 1000.f;

mySerial.printf("MPU6050 Ready at %2.2f Sec\n", StartSec);

}

else

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

Serial.print(F("DMP Initialization failed (code "));

Serial.print(devStatus);

Serial.println(F(")"));

bMPU6050Ready = false;

}

#pragma endregion MPU6050

#pragma region PIN_INITIALIZATION

pinMode(LIDAR_MODE_PIN, INPUT); // Set LIDAR input monitor pin

#pragma endregion PIN_INITIALIZATION

#pragma region TIMER_INTERRUPT

//09/18/20 can't use Timer1 cuz doing so conflicts with PWM on pins 10-12

cli();//stop interrupts

TCCR5A = 0;// set entire TCCR5A register to 0

TCCR5B = 0;// same for TCCR5B

TCNT5 = 0;//initialize counter value to 0

// 06/15/21 rewritten to use defined interval constant rather than magic number

//10/11/20 chg timer interval to 10Hz vs 5

//OCR5A = 1562;// = (16*10^6) / (10*1024) - 1 (must be <65536)

int ocr5a = (16 * 1e6) / ((1000 / TIMER5_INTERVAL_MSEC) * 1024) - 1;

mySerial.printf("ocr5a = %d\n", ocr5a);

OCR5A = ocr5a;

TCCR5B |= (1 << WGM52);// turn on CTC mode

TCCR5B |= (1 << CS52) | (1 << CS50);// Set CS10 and CS12 bits for 1024 prescaler

TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

sei();//allow interrupts

pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);

#pragma endregion TIMER_INTERRUPT

//02/08/21 mvd below ISR enable so this section can use ISR-generated VL53L0X distances

#pragma region VL53L0X_TEENSY

mySerial.printf("Checking for Teensy 3.5 VL53L0X Controller at I2C addr 0x%x\n", VL53L0X_I2C_SLAVE_ADDRESS);

while (!bVL53L0X_TeensyReady)

{

Wire.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

bVL53L0X_TeensyReady = (Wire.endTransmission() == 0);

//mySerial.printf("%lu: VL53L0X Teensy Not Avail...\n", millis());

delay(100);

}

mySerial.printf("Teensy available at %lu with bVL53L0X_TeensyReady = %d. Waiting for Teensy setup() to finish\n", millis(), bVL53L0X_TeensyReady);

WaitForVL53L0XTeensy();

//mySerial.printf("VL53L0X Teensy Ready at %lu\n\n", millis());

#pragma endregion VL53L0X_TEENSY

//11/14/18 added this section for distance printout only

//08/06/20 revised for VL53L0X support

#ifdef DISTANCES_ONLY

TIMSK5 = 0;

digitalWrite(RED_LASER_DIODE_PIN, HIGH); //enable the front laser dot

mySerial.printf("\n------------ DISTANCES ONLY MODE!!! -----------------\n\n");

int i = 0; //added 09/20/20 for in-line header display

mySerial.printf("Msec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\tRear\n");

while (true)

{

//if (bTimeForNavUpdate)

//{

// bTimeForNavUpdate = false;

//09/20/20 re-display the column headers

//if (++i % 20 == 0)

//{

// mySerial.printf("\nMsec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\tFront\tRear\n");

//}

//commented out 02/02/21 - now done in ISR

GetRequestedVL53l0xValues(VL53L0X_ALL);

mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",

millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, frontdistval, Lidar_Rear);

//}

delay(50);

}

TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

#endif // DISTANCES_ONLY

#pragma region L/R/FRONT DISTANCE ARRAYS

//09/20/20 have to do this for parallel finding to go the right way

Serial.println(F("Initializing Left, Right, Front Distance Arrays..."));

#ifndef NO_PINGS

//03/30/21 moved FrontDistArray init ahead of left/right dist init to prevent inadvertent 'stuck' detection

Serial.println(F("Initializing Front Distance Array"));

InitFrontDistArray(); //08/12/20 code extracted to fcn so can call elsewhere

//Serial.println(F("Updating Left\tRight Distance Arrays"));

//for (size_t i = 0; i < LR_DIST_ARRAY_SIZE; i++)

//{

// delay(100); //ensure enough time for ISR to update distances

// mySerial.printf("%d\t%d\n", Lidar_LeftCenter, Lidar_RightCenter);

// UpdateLRDistanceArrays(Lidar_LeftCenter, Lidar_RightCenter);

//}

//Serial.println(F("Updating Rear Distance Arrays"));

//InitRearDistArray();

//mySerial.printf("Initial rear prev_mean, prev_var = %2.2f, %2.2f\n",

// Rear_Dist_PrevMean, Rear_Dist_PrevVar);

#else

Serial.println(F("Distance Sensors Disabled"));

#endif // NO_PINGS

#pragma endregion L/R/FRONT DISTANCE ARRAYS

#pragma region WALL_TRACK_TEST

mySerial.printf("End of test - Stopping!\n");

//while (true)

//{

// CheckForUserInput();

//}

StopBothMotors();

Serial.println(F("Setting Kp value"));

const int bufflen = 80;

char buff[bufflen];

memset(buff, 0, bufflen);

float OffsetCm;

float Kp, Ki, Kd;

const char s[2] = ",";

char* token;

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

mySerial.printf("consumed %d\n", byte);

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

mySerial.printf("%s\n", buff);

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract Offset in cm */

token = strtok(NULL, s);

OffsetCm = atof(token);

//mySerial.printf("Kp,Ki,Kd,OffsetCm = %2.2f, %2.2f, %2.2f, %2.2f\n",

// Kp, Ki, Kd, OffsetCm);

//TrackLeftWallOffset(Kp, Ki, Kd, OffsetCm);

TrackRightWallOffset(Kp, Ki, Kd, OffsetCm);

//mySerial.printf("Rdist\tCdist\tFdist\tsteer\toffang\n");

//while (true)

//{

// //double offangdeg = GetSteeringAngle(Lidar_LeftCenter / 10, LeftSteeringVal);

// double offangdeg = GetSteeringAngle(LeftSteeringVal); //06/25/21 chg to mm vs cm

// mySerial.printf("%d\t%d\t%d\t%2.4f\t%2.2f\n", Lidar_LeftRear, Lidar_LeftCenter, Lidar_LeftFront, LeftSteeringVal, offangdeg);

// delay(1000);

//}

#pragma endregion WALL_TRACK_TEST

}

void loop()

{

}

void CheckForUserInput()

{

const int bufflen = 80;

char buff[bufflen];

memset(buff, 0, bufflen);

float OffsetCm;

float Kp, Ki, Kd;

const char s[2] = ",";

char* token;

if (Serial.available() > 0)

{

// read the incoming byte:

int incomingByte = Serial.read();

// say what you got:

//Serial.print("I received: ");

//Serial.println(incomingByte, HEX); //chg to HEX 02/18/20

//02/18/20 experiment with multiple commands

switch (incomingByte)

{

case 0x43: //ASCII 'C'

case 0x63: //ASCII 'c'

#pragma region MANUALCONTROL

//enter infinite loop for direct remote control

Serial.println(F("ENTERING COMMAND MODE:"));

Serial.println(F("0 = 180 deg CCW Turn"));

Serial.println(F("1 = 180 deg CW Turn"));

Serial.println(F("A = Back to Auto Mode"));

Serial.println(F("S = Stop"));

Serial.println(F("F = Forward"));

Serial.println(F("R = Reverse"));

Serial.println(F(""));

Serial.println(F(" Faster"));

Serial.println(F("\t8"));

Serial.println(F("Left 4\t5 6 Right"));

Serial.println(F("\t2"));

Serial.println(F(" Slower"));

StopBothMotors();

int speed = 0;

bool bAutoMode = false;

while (!bAutoMode)

{

if (Serial.available() > 0)

{

// read the incoming byte:

int incomingByte = Serial.read();

//mySerial.printf("Got %x\n", incomingByte);

switch (incomingByte)

{

case 0x54: //ASCII 'T'

case 0x74: //ASCII 't'

Serial.println(F("Toggle TIMER5 Enable/Disable"));

if (TIMSK5 == 0)

{

Serial.println(F("Enable TIMER5"));

//TIMSK5 |= OCIE5A;

TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

mySerial.printf("TIMSK5 = %x\n", TIMSK5);

}

else

{

Serial.println(F("Disable TIMER5"));

TIMSK5 = 0;

mySerial.printf("TIMSK5 = %x\n", TIMSK5);

}

break;

case 0x44: //ASCII 'D'

case 0x64: //ASCII 'd'

Serial.println(F("Display left/right distances"));

if (TIMSK5 == 0)

{

Serial.println(F("Enable TIMER5"));

//TIMSK5 |= OCIE5A;

TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

mySerial.printf("TIMSK5 = %x\n", TIMSK5);

}

mySerial.printf("Msec\tLF\tLC\tLR\tRF\tRC\tRR\n");

while (true)

{

if (bTimeForNavUpdate)

{

bTimeForNavUpdate = false;

mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);

}

break;

case 0x30: //Dec '0'

Serial.println(F("CCW 180 deg Turn"));

//RollingTurn(true, bIsForwardDir, 180);

//SpinTurn(true, 180, 45);

SpinTurn(true, 180, 90);

MoveAhead(speed, speed);

break;

case 0x31: //Dec '1'

Serial.println(F("CW 180 deg Turn"));

//RollingTurn(false, bIsForwardDir, 180);

SpinTurn(false, 180, 45);

//MoveAhead(speed, speed);

break;

case 0x34: //Turn left 10 deg

Serial.println(F("CCW 10 deg Turn"));

//MoveAhead(speed, speed);

//RollingTurn(true, bIsForwardDir, 10);

SpinTurn(true, 10, 30);

//SpinTurn(true, 30, 30);

//added 05/03/20

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x36: //Turn right 10 deg'

mySerial.print("CW 10 deg Turn\n");

MoveAhead(speed, speed);

//RollingTurn(false, bIsForwardDir, 10);

SpinTurn(false, 10, 30);

//SpinTurn(false, 45, 30);

//added 05/03/20

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x38: //Speed up

speed += 50;

speed = (speed >= MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed;

mySerial.printf("Speeding up: speed now %d\n", speed);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x32: //Slow down

speed -= 50;

speed = (speed < 0) ? 0 : speed;

mySerial.printf("Slowing down: speed now %d\n", speed);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x35: //05/07/20 changed to use '5' vs 'S'

Serial.println(F("Stopping Motors!"));

StopBothMotors();

speed = 0;

break;

case 0x41: //ASCII 'A'

case 0x61: //ASCII 'a'

StopBothMotors();

Serial.println(F("Re-entering AUTO mode"));

bAutoMode = true;

break;

case 0x52: //ASCII 'R'

case 0x72: //ASCII 'r'

Serial.println(F("Setting both motors to reverse"));

bIsForwardDir = false;

MoveReverse(speed, speed);

break;

case 0x46: //ASCII 'F'

case 0x66: //ASCII 'f'

Serial.println(F("Setting both motors to forward"));

bIsForwardDir = true;

MoveAhead(speed, speed);

#pragma endregion MANUALCONTROL

break;

#pragma region WALL OFFSET TRACKING CASES

//left side wall tracking

case 0x4C: //ASCII 'L'

case 0x6C: //ASCII 'l'

StopBothMotors();

Serial.println(F("Setting Kp value"));

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

mySerial.printf("consumed %d\n", byte);

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

mySerial.printf("%s\n", buff);

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract Offset in cm */

token = strtok(NULL, s);

OffsetCm = atof(token);

mySerial.printf("Kp,Ki,Kd,OffsetCm = %2.2f, %2.2f, %2.2f, %2.2f\n",

Kp, Ki, Kd, OffsetCm);

TrackLeftWallOffset(Kp, Ki, Kd, OffsetCm);

break;

case 0x4D: //ASCII 'M'

case 0x6D: //ASCII 'm'

StopBothMotors();

Serial.println(F("Setting Kp value"));

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

mySerial.printf("consumed %d\n", byte);

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

mySerial.printf("%s\n", buff);

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract Offset in cm */

token = strtok(NULL, s);

OffsetCm = atof(token);

mySerial.printf("Kp,Ki,Kd,OffsetCm = %2.2f, %2.2f, %2.2f, %2.2f\n",

Kp, Ki, Kd, OffsetCm);

TrackRightWallOffset(Kp, Ki, Kd, OffsetCm);

break;

#pragma endregion WALL OFFSET TRACKING CASES

Default:

Serial.println(F("In Default Case: Stopping Motors!"));

MoveAhead(0, 0);

while (true)

{

}

void TrackLeftWallOffset(double kp, double ki, double kd, double offsetCm)

{

//Notes:

// 06/21/21 modified to do a turn, then straight, then track 0 steer val

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

bool bFirstTime = true;

double spinRateDPS = 45;

GetRequestedVL53l0xValues(VL53L0X_LEFT); //update LeftSteeringVal

mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

//06/21/21 modified to do a turn, then straight, then track 0 steer val

double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_LeftCenter / 10.f);//positive inside, negative outside desired offset

//07/05/21 implement 15deg min cut angle

if (cutAngleDeg < 0 && abs(cutAngleDeg) < 15)

{

cutAngleDeg = -15;

}

else if (cutAngleDeg > 0 && cutAngleDeg < 15 )

{

cutAngleDeg = 15;

}

double SteerAngleDeg = GetSteeringAngle(LeftSteeringVal);

mySerial.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",

Lidar_LeftRear, Lidar_LeftCenter, Lidar_LeftFront, LeftSteeringVal, SteerAngleDeg, cutAngleDeg);

double adjCutAngleDeg = 0;

uint16_t fudgeFactorMM = 0; //07/05/21 added so can change for inside vs outside starting condx

if (cutAngleDeg > 0) //robot inside desired offset distance

{

if (SteerAngleDeg > cutAngleDeg) // --> SteerAngleDeg also > 0

{

mySerial.printf("if (SteerAngleDeg > cutAngleDeg)\n");

SpinTurn(true, SteerAngleDeg - cutAngleDeg, spinRateDPS); //

}

else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg could be < 0

{

if (SteerAngleDeg <= 0 ) //cutAngleDeg > 0, SteerAngleDeg <= 0

{

mySerial.printf("if (SteerAngleDeg <= 0)\n");

SpinTurn(false,cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg

}

else //SteerAngleDeg < cutAngleDeg but still > 0

{

mySerial.printf("else\n");

SpinTurn(false,cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg

}

fudgeFactorMM = -100; //07/05/21 don't need fudge factor for inside start condx

}

else // cutAngleDeg < 0 --> robot outside desired offset distance

{

if (SteerAngleDeg > cutAngleDeg) // --> SteerAngleDeg may also be > 0

{

mySerial.printf("if (SteerAngleDeg > cutAngleDeg)\n");

SpinTurn(true, SteerAngleDeg - cutAngleDeg, spinRateDPS); //

}

else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg must also be < 0

{

if (SteerAngleDeg <= 0) //cutAngleDeg > 0, SteerAngleDeg <= 0

{

mySerial.printf("if (SteerAngleDeg <= 0)\n");

SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg

}

else //SteerAngleDeg < cutAngleDeg but still > 0

{

mySerial.printf("else\n");

SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg

}

fudgeFactorMM = 100; //07/05/21 need fudge factor for outside start condx

}

delay(1000);

//adjust so offset capture occurs at correct perpendicular offset

//double adjfactor = cos(PI * avgAppAngle / 180.f);

double adjfactor = cos(PI * cutAngleDeg / 180.f);

int adjOffsetMM = (int)(10 * (double)WALL_OFFSET_TGTDIST_CM / adjfactor);

//adjOffsetMM += 100; //fudge factor for distance measurements lagging behind robot's travel.

adjOffsetMM += fudgeFactorMM; //fudge factor for distance measurements lagging behind robot's travel.

//mySerial.printf("at approach start: steerval = %2.3f, adj cut angle = %2.2f, adjfactor = %2.3f, tgt dist = %d\n", avgSteerVal, avgAppAngle, adjfactor, adjOffsetMM);

mySerial.printf("at approach start: cut angle = %2.3f, adjfactor = %2.3f, tgt dist = %d\n", cutAngleDeg, adjfactor, adjOffsetMM);

//long int prev_res = Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM; //neg for inside going out, pos for outside going in

long int prev_res = Lidar_LeftFront - adjOffsetMM; //neg for inside going out, pos for outside going in

long int res = prev_res;

mySerial.printf("At start - prev_res = %ld\n", prev_res);

mySerial.printf("Msec\tLFront\tLCtr\tLRear\tFront\tRear\tRes\tP_res\n");

//TIMSK5 = 0; //turn off TIMER5 interrupts

while (prev_res * res > 0) //sign change makes this < 0

{

prev_res = res;

MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);

while (!bTimeForNavUpdate)

{

}

bTimeForNavUpdate = false;

frontdistval = GetFrontDistCm();

res = Lidar_LeftFront - adjOffsetMM; //06/29/21 now adj for slant dist vs perp dist

mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",

millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, frontdistval, Lidar_Rear, res, prev_res);

CheckForUserInput();

}

//now turn back the same (unadjusted) amount

mySerial.printf("At end - prev_res*res = %ld\n", prev_res*res);

mySerial.printf("correct back to parallel\n");

SpinTurn(!(cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

//sinceLastComputeTime = 0;

WallTrackSetPoint = 0; //moved here 6/22/21

//TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

mySerial.printf("Msec\tFdir\tCdir\tRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

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);

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);

}

void TrackRightWallOffset(double kp, double ki, double kd, double offsetCm)

{

//Notes:

// 06/21/21 modified to do a turn, then straight, then track 0 steer val

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

bool bFirstTime = true;

double spinRateDPS = 45;

GetRequestedVL53l0xValues(VL53L0X_RIGHT); //update RightSteeringVal = (Front - Rear)/100

mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

//06/21/21 modified to do a turn, then straight, then track 0 steer val

double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_RightCenter / 10.f);//positive inside, negative outside desired offset

//07/05/21 implement 15deg min cut angle

if (cutAngleDeg < 0 && abs(cutAngleDeg) < 15)

{

cutAngleDeg = -15;

}

else if (cutAngleDeg > 0 && cutAngleDeg < 15)

{

cutAngleDeg = 15;

}

double SteerAngleDeg = GetSteeringAngle(RightSteeringVal);

mySerial.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",

Lidar_RightRear, Lidar_RightCenter, Lidar_RightFront, RightSteeringVal, SteerAngleDeg, cutAngleDeg);

double adjCutAngleDeg = 0;

uint16_t fudgeFactorMM = 0; //07/05/21 added so can change for inside vs outside starting condx

if (cutAngleDeg > 0) //robot inside desired offset distance

{

if (SteerAngleDeg > cutAngleDeg) // --> SteerAngleDeg also > 0 (pointing away from wall)

{

mySerial.printf("if (SteerAngleDeg > cutAngleDeg)\n");

SpinTurn(false, SteerAngleDeg - cutAngleDeg, spinRateDPS); //

}

else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg could be < 0

{

if (SteerAngleDeg <= 0) //cutAngleDeg > 0, SteerAngleDeg <= 0

{

mySerial.printf("if (SteerAngleDeg <= 0)\n");

//SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg

SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg

}

else //SteerAngleDeg < cutAngleDeg but still > 0

{

mySerial.printf("else\n");

//SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg

SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg

}

fudgeFactorMM = -100; //07/05/21 don't need fudge factor for inside start condx

}

else // cutAngleDeg < 0 --> robot outside desired offset distance

{

if (SteerAngleDeg > cutAngleDeg) // --> SteerAngleDeg may also be > 0

{

mySerial.printf("if (SteerAngleDeg > cutAngleDeg)\n");

SpinTurn(false, SteerAngleDeg - cutAngleDeg, spinRateDPS); //

}

else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg must also be < 0

{

if (SteerAngleDeg <= 0) //cutAngleDeg > 0, SteerAngleDeg <= 0

{

mySerial.printf("if (SteerAngleDeg <= 0)\n");

//SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg

SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg

}

else //SteerAngleDeg < cutAngleDeg but still > 0

{

mySerial.printf("else\n");

//SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg

SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg

}

fudgeFactorMM = 100; //07/05/21 need fudge factor for outside start condx

}

//DEBUG!!

//mySerial.printf("Msec\tFront\tCtr\ttRear\tSteer\tAngle\n");

//double sumOfSteerVals = 0;

//for (size_t i = 0; i < 10; i++)

//{

// while (!bTimeForNavUpdate)

// {

// }

// bTimeForNavUpdate = false;

// //GetRequestedVL53l0xValues(VL53L0X_LEFT); //refresh steering value

// mySerial.printf("%lu\t%d\t%d\t%d\t%2.3f\t%2.2f\n",millis(),

// Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, RightSteeringVal, GetSteeringAngle(RightSteeringVal));

// sumOfSteerVals += RightSteeringVal;

// //delay(100);

//}

//double avgSteerVal = sumOfSteerVals / 10.f;

//double avgAppAngle = GetSteeringAngle(avgSteerVal);

//mySerial.printf("avg steerval = %2.3f, avg angle = %2.3f\n", avgSteerVal, avgAppAngle);

//DEBUG!!

delay(1000);

//adjust so offset capture occurs at correct perpendicular offset

//double adjfactor = cos(PI * avgAppAngle / 180.f);

double adjfactor = cos(PI * cutAngleDeg / 180.f);

int adjOffsetMM = (int)(10 * (double)WALL_OFFSET_TGTDIST_CM / adjfactor);

//adjOffsetMM += 100; //fudge factor for distance measurements lagging behind robot's travel.

adjOffsetMM += fudgeFactorMM; //fudge factor for distance measurements lagging behind robot's travel.

//mySerial.printf("at approach start: steerval = %2.3f, adj cut angle = %2.2f, adjfactor = %2.3f, tgt dist = %d\n", avgSteerVal, avgAppAngle, adjfactor, adjOffsetMM);

mySerial.printf("at approach start: cut angle = %2.3f, adjfactor = %2.3f, tgt dist = %d\n", cutAngleDeg, adjfactor, adjOffsetMM);

//long int prev_res = Lidar_RightFront - 10 * WALL_OFFSET_TGTDIST_CM; //neg for inside going out, pos for outside going in

long int prev_res = Lidar_RightFront - adjOffsetMM; //neg for inside going out, pos for outside going in

long int res = prev_res;

mySerial.printf("At start - prev_res = %ld\n", prev_res);

mySerial.printf("Msec\tLFront\tLCtr\tLRear\tFront\tRear\tRes\tP_res\n");

//TIMSK5 = 0; //turn off TIMER5 interrupts

while (prev_res * res > 0) //sign change makes this < 0

{

prev_res = res;

MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);

while (!bTimeForNavUpdate)

{

}

bTimeForNavUpdate = false;

frontdistval = GetFrontDistCm();

res = Lidar_RightFront - adjOffsetMM; //06/29/21 now adj for slant dist vs perp dist

mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",

millis(),

Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, frontdistval, Lidar_Rear, res, prev_res);

CheckForUserInput();

}

//now turn back the same (unadjusted) amount

mySerial.printf("At end - prev_res*res = %ld\n", prev_res * res);

mySerial.printf("correct back to parallel\n");

//SpinTurn(!(cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

SpinTurn((cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

//sinceLastComputeTime = 0;

WallTrackSetPoint = 0; //moved here 6/22/21

//TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

mySerial.printf("Msec\tFdir\tCdir\tRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

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 = RightSteeringVal + (Lidar_RightCenter - 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, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

kp, ki, kd, WallTrackOutput);

int speed = 0;

//07/05/21 reverse signs for right side tracking

//int leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

//int rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

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_RightFront, Lidar_RightCenter, Lidar_RightRear,

// WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,

// kp * lastError, ki * lastIval, kd * lastDerror,

// WallTrackOutput, 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_RightFront, Lidar_RightCenter, Lidar_RightRear,

-WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,

kp * lastError, ki * lastIval, kd * lastDerror,

WallTrackOutput, leftspeednum, rightspeednum);

}

#pragma region MOTOR SUPPORT

//09/08/20 modified for DRV8871 motor driver

void MoveReverse(int leftspeednum, int rightspeednum)

{

//Purpose: Move in reverse direction continuously - companion to MoveAhead()

//ProvEnA_Pinnce: G. Frank Paynter 09/08/18

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Plan:

// Step 1: Set reverse direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

// 01/22/20 now using Adafruit DRV8871 drivers

//Step 1: Set reverse direction and speed for both wheels

SetLeftMotorDirAndSpeed(REV_DIR, leftspeednum);

SetRightMotorDirAndSpeed(REV_DIR, rightspeednum);

}

//09/08/20 modified for DRV8871 motor driver

void MoveAhead(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

//ProvEnA_Pinnce: G. Frank Paynter and Danny Frank 01/24/2014

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Plan:

// Step 1: Set forward direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

// 01/22/20 now using Adafruit DRV8871 drivers

//mySerial.printf("InMoveAhead(%d,%d)\n", leftspeednum, rightspeednum);

//Step 1: Set forward direction and speed for both wheels

SetLeftMotorDirAndSpeed(true, leftspeednum);

SetRightMotorDirAndSpeed(true, rightspeednum);

}

//09/08/10 modified for DRV8871 motor driver

void StopLeftMotors()

{

analogWrite(In1_Left, MOTOR_SPEED_OFF);

analogWrite(In2_Left, MOTOR_SPEED_OFF);

}

void StopRightMotors()

{

analogWrite(In1_Right, MOTOR_SPEED_OFF);

analogWrite(In2_Right, MOTOR_SPEED_OFF);

}

//09/08/20 added bool bisFwd param for DRV8871 motor driver

void RunBothMotors(bool bisFwd, int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors at left/rightspeednum speeds

//Inputs:

// speednum = speed value (0 = OFF, 255 = full speed)

//Outputs: Both Motors run for timesec seconds at speednum speed

//Plan:

// Step 1: Apply drive to both wheels

//Notes:

// 01/14/15 - added left/right speed offset for straightness compensation

// 01/22/15 - added code to restrict fast/slow values

// 01/24/15 - revised for continuous run - no timing

// 01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()

// 12/07/15 - chg args from &int to int

//Step 1: Apply drive to both wheels

//DEBUG!!

//mySerial.printf("In RunBothMotors(%s, %d,%d)\n", bisFwd? "FWD":"REV", leftspeednum, rightspeednum);

//DEBUG!!

SetLeftMotorDirAndSpeed(bisFwd, leftspeednum);

SetRightMotorDirAndSpeed(bisFwd, rightspeednum);

}

void RunBothMotorsBidirectional(int leftspeed, int rightspeed)

{

//Purpose: Accommodate the need for independent bidirectional wheel motion

//Inputs:

// leftspeed - integer denoting left wheel speed. Positive value is fwd, negative is rev

// rightspeed - integer denoting right wheel speed. Positive value is fwd, negative is rev

//Outputs:

// left/right wheel motors direction and speed set as appropriate

//Plan:

// Step1: Set left wheel direction and speed

// Step2: Set right wheel direction and speed

//Step1: Set left wheel direction and speed

//DEBUG!!

//mySerial.printf("In RunBothMotorsBidirectional(%d, %d)\n", leftspeed, rightspeed);

if (leftspeed < 0)

{

SetLeftMotorDirAndSpeed(false, -leftspeed); //neg value ==> reverse

}

else

{

SetLeftMotorDirAndSpeed(true, leftspeed); //pos or zero value ==> fwd

}

//Step2: Set right wheel direction and speed

if (rightspeed < 0)

{

SetRightMotorDirAndSpeed(false, -rightspeed); //neg value ==> reverse

}

else

{

SetRightMotorDirAndSpeed(true, rightspeed); //pos or zero value ==> fwd

}

//09/08/20 added bool bisFwd param for DRV8871 motor driver

void RunBothMotorsMsec(bool bisFwd, int timeMsec, int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors for timesec seconds at speednum speed

//Inputs:

// timesec = time in seconds to run the Motors

// speednum = speed value (0 = OFF, 255 = full speed)

//Outputs: Both Motors run for timesec seconds at speednum speed

//Plan:

// Step 1: Apply drive to both wheels

// Step 2: Delay timsec seconds

// Step 3: Remove drive from both wheels.

//Notes:

// 01/14/15 - added left/right speed offset for straightness compensation

// 01/22/15 - added code to restrict fast/slow values

// 11/25/15 - rev to use motor driver class object

// 09/08/20 added bool bisFwd param for DRV8871 motor driver

RunBothMotors(bisFwd, leftspeednum, rightspeednum);

//Step 2: Delay timsec seconds

delay(timeMsec);

//Step3: Stop motors added 04/12/21

StopBothMotors();

}

//11/25/15 added for symmetry ;-).

void StopBothMotors()

{

StopLeftMotors();

StopRightMotors();

}

void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)

{

//mySerial.printf("SetLeftMotorDirAndSpeed(%d,%d)\n", bIsFwd, speed);

#ifndef NO_MOTORS

if (bIsFwd)

{

digitalWrite(In1_Left, LOW);

analogWrite(In2_Left, speed);

//mySerial.printf("In TRUE block of SetLeftMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

}

else

{

//mySerial.printf("In FALSE block of SetLeftMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

digitalWrite(In2_Left, LOW);

analogWrite(In1_Left, speed);

}

#endif // !NO_MOTORS

}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)

{

//mySerial.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

#ifndef NO_MOTORS

if (bIsFwd)

{

//mySerial.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

digitalWrite(In1_Right, LOW);

analogWrite(In2_Right, speed);

}

else

{

//mySerial.printf("In FALSE block of SetRightMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

digitalWrite(In2_Right, LOW);

analogWrite(In1_Right, speed);

}

#endif // !NO_MOTORS

}

#pragma endregion Motor Support Functions

#pragma region MPU5060 Support

float UpdateIMUHdgValDeg()

{

//Purpose: Get latest yaw (heading) value from IMU

//Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE

//Outputs:

// returns true if successful, otherwise false

// IMUHdgValDeg updated on success

//Plan:

//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet

//Step2: read all available packets to get to latest data

//Step3: update IMUHdgValDeg with latest value

//Notes:

// 10/08/19 changed return type to boolean

// 10/08/19 no longer need mpuIntStatus

// 10/21/19 completely rewritten to use Homer's algorithm

// 05/05/20 changed return type to float vs bool.

bool retval = false;

int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow

{

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

IMUHdgValDeg = ypr[0] * 180 / M_PI;

retval = true;

}

//return retval;

return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience

}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)

{

mpu.resetFIFO();

delay(1);

//int countloop = 0;

fifoCount = mpu.getFIFOCount();

GetPacketLoopCount = 0;

//mySerial.printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);

while (fifoCount < packetSize && GetPacketLoopCount < max_loops)

{

GetPacketLoopCount++;

fifoCount = mpu.getFIFOCount();

delay(2);

}

//mySerial.printf("In GetCurrentFIFOPacket: after loop fifoC = %d, loop count = %d\n", fifoCount, GetPacketLoopCount);

if (GetPacketLoopCount >= max_loops)

{

return 0;

}

//if we get to here, there should be exactly one packet in the FIFO

mpu.getFIFOBytes(data, packetSize);

return 1;

}

#pragma endregion MPU5060 Support

#pragma region HDG_BASED_TURN_SUPPORT

float GetHdgMatchVal(float tgt_deg, float cur_deg)

{

//Purpose: Compute the match ratio between two compass headings

//Inputs:

// tgt_deg = float representing target heading in +/-180 range

// IMUHdgValDeg = float representing sensor yaw value in +/-180 deg range

//Outputs:

// returns result of 1 - abs(Tgt_deg - Hdg_deg)/180, all angles in 0-360 deg range

//Plan:

// Step1: convert both inputs to 0-360 deg range

// Step2: compute match ratio

//Notes:

// formula from https://gis.stackexchange.com/questions/129954/comparing-compass-bearings

//Step1: convert both inputs to 0-360 deg range

float tgthdg = (tgt_deg < 0) ? tgt_deg + 360 : tgt_deg;

float curhdg = (cur_deg < 0) ? cur_deg + 360 : cur_deg;

//Step2: compute match ratio

float match_ratio = 1 - abs(tgthdg - curhdg) / 180;

//DEBUG!!

//mySerial.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio);

//DEBUG!!

return abs(match_ratio);

}

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)

{

//Purpose: Make a numDeg CW or CCW 'spin' turn

//Inputs:

// b_ccw - True if turn is to be ccw, false otherwise

// numDeg - angle to be swept out in the turn

// ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg

// IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR

// degPerSec = float value denoting desired turn rate

//Plan:

// Step1: Get current heading as starting point

// Step2: Disable TIMER5 interrupts

// Step3: Compute new target value & timeout value

// Step4: Run motors until target reached, using inline PID algorithm to control turn rate

// Step5: Re-enable TIMER5 interrupts

//Notes:

// 06/06/21 we-written to remove PID library - now uses custom 'PIDCalcs()' function

// 06/06/21 added re-try for 180.00 return from IMU - could be bad value

// 06/11/21 added code to correct dHdg errors due to 179/-179 transition & bad IMU values

// 06/12/21 cleaned up & commented out debug code

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

//DEBUG!!

mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);

//DEBUG!!

//mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",

// TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

//no need to continue if the IMU isn't available

if (!dmpReady)

{

return false;

}

//Step1: Get current heading as starting point

//06/06/21 it is possible for IMU to return 180.00 on failure

//so try again. If it really IS 180, then

//it will eventually time out and go on

int retries = 0;

if (IMUHdgValDeg == 180.f && retries < 5)

{

//DEBUG!!

mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");

//DEBUG!!

UpdateIMUHdgValDeg();

retries++;

delay(30);

}

//Step2: Compute new target value & timeout value

timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

//05/17/20 limit timeout_sec to 1 sec or more

timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

UpdateIMUHdgValDeg();

//12/05/19 added #define back in to manage which direction increases yaw values

#ifdef MPU6050_CCW_INCREASES_YAWVAL

tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;

#else

tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

//correct for -180/180 transition

if (tgt_deg < -180)

{

tgt_deg += 360;

}

//07/29/19 bugfix

if (tgt_deg > 180)

{

tgt_deg -= 360;

}

//DEBUG!!

mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",

IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);

//DEBUG!!

double curHdgMatchVal = 0;

//09/08/18 added to bolster end-of-turn detection

float prevHdgMatchVal = 0;

float matchSlope = 0;

//Step2: Disable TIMER5 interrupts

TIMSK5 = 0; //disable timer compare interrupt

//Step3: Run motors until target reached, using PID algorithm to control turn rate

Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

//DEBUG!!

//mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg);

//mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n");

//DEBUG!!

elapsedMillis sinceLastTimeCheck = 0;

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

bool bFirstIMUHdg = true;

while (!bDoneTurning && !bTimedOut)

{

//11/06/20 now just loops between ISR hits

CheckForUserInput();

//runs every 30mSec

if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg(); //update IMUHdgValDeg

double dHdg = IMUHdgValDeg - Prev_HdgDeg;

if (dHdg > 180)

{

dHdg -= 360;

mySerial.printf("dHdg > 180 (%2.2f - %2.2f) - subtracting 360\n", IMUHdgValDeg, Prev_HdgDeg);

}

else if (dHdg < -180)

{

dHdg += 360;

mySerial.printf("dHdg < -180 (%2.2f - %2.2f) - adding 360\n", IMUHdgValDeg, Prev_HdgDeg);

}

//watch for turn rates that are wildly off

double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);

if (rate > 3 * degPersec)

{

//DEBUG!!

mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec);

//DEBUG!!

rate = degPersec;

}

//06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet

if (bFirstIMUHdg)

{

bFirstIMUHdg = false;

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

}

else

{

PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

int speed = 0;

(TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput;

(TurnRatePIDOutput <= MOTOR_SPEED_LOW) ? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput;

SetLeftMotorDirAndSpeed(!b_ccw, speed);

SetRightMotorDirAndSpeed(b_ccw, speed);

//check for nearly there and all the way there

curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);

matchSlope = curHdgMatchVal - prevHdgMatchVal;

prevHdgMatchVal = curHdgMatchVal;

//DEBUG!!

//mySerial.printf("%lu\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%2.2f\t%d\n",

// millis(),

// IMUHdgValDeg,

// Prev_HdgDeg,

// dHdg,

// rate,

// degPersec,

// lastError,

// TurnRate_Kp * lastError,

// TurnRate_Ki * lastIval,

// TurnRate_Kd * lastDerror,

// speed);

//DEBUG!!

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start

}

bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

if (bTimedOut)

{

//DEBUG!!

mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

//DEBUG!!

bResult = false;

}

//DEBUG!!

mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg);

//DEBUG!!

//Step4: Re-enable TIMER5 interrupts

TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

StopBothMotors();

//delay(1000); //added 04/27/21 for debug

return bResult;

}

//bool SpinTurnToSteerVal(bool b_ccw, float tgtSteerVal, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)

//{

// //Purpose: Make a rate-controlled CW or CCW 'spin' turn to a desired steering value

// //Inputs:

// // b_ccw - True if turn is to be ccw, false otherwise

// // steerval - steering value to be met

// // ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg

// // IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR

// // degPerSec = float value denoting desired turn rate

// //Plan:

// // Step1: Get current heading as starting point

// // Step2: Disable TIMER5 interrupts

// // Step4: Run motors until target steering value reached, using inline PID algorithm to control turn rate

// // Step5: Re-enable TIMER5 interrupts

// //Notes:

// // copied from SpinTurn() and modifed to terminate on steering value

// //float tgt_deg;

// float timeout_sec;

// bool bDoneTurning = false;

// bool bTimedOut = false;

// bool bResult = true; //04/21/20 added so will be only one exit point

// ////DEBUG!!

// //mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);

// ////DEBUG!!

// mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",

// TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

// //no need to continue if the IMU isn't available

// if (!dmpReady)

// {

// return false;

// }

// //Step1: Get current heading as starting point

// //06/06/21 it is possible for IMU to return 180.00 on failure

// //so try again. If it really IS 180, then

// //it will eventually time out and go on

// int retries = 0;

// if (IMUHdgValDeg == 180.f && retries < 5)

// {

// //DEBUG!!

// //mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");

// //DEBUG!!

// UpdateIMUHdgValDeg();

// retries++;

// delay(30);

// }

// ////Step2: Compute new target value & timeout value

// //timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

// ////05/17/20 limit timeout_sec to 1 sec or more

// //timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

// UpdateIMUHdgValDeg();

//// //12/05/19 added #define back in to manage which direction increases yaw values

////#ifdef MPU6050_CCW_INCREASES_YAWVAL

//// tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;

////#else

//// tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

////

////#endif // MPU6050_CCW_INCREASES_YAWVAL

////

//// //correct for -180/180 transition

//// if (tgt_deg < -180)

//// {

//// tgt_deg += 360;

//// }

////

//// //07/29/19 bugfix

//// if (tgt_deg > 180)

//// {

//// tgt_deg -= 360;

//// }

// //DEBUG!!

// //mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",

// // IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);

// //DEBUG!!

// //float curHdgMatchVal = 0;

// ////09/08/18 added to bolster end-of-turn detection

// //float prevHdgMatchVal = 0;

// //float matchSlope = 0;

// //Step2: Disable TIMER5 interrupts

// TIMSK5 = 0; //disable timer compare interrupt

// //Step3: Run motors until target reached, using PID algorithm to control turn rate

// Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

// //DEBUG!!

// //mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg);

// //mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n");

// //DEBUG!!

// elapsedMillis sinceLastTimeCheck = 0;

// elapsedMillis sinceLastComputeTime = 0;

// double lastError = 0;

// double lastInput = 0;

// double lastIval = 0;

// double lastDerror = 0;

// bool bFirstIMUHdg = true;

// //while (!bDoneTurning && !bTimedOut)

// while (!bDoneTurning)

// {

// //11/06/20 now just loops between ISR hits

// CheckForUserInput();

// //runs every 30mSec

// if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)

// {

// sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

// UpdateIMUHdgValDeg(); //update IMUHdgValDeg

// double dHdg = IMUHdgValDeg - Prev_HdgDeg;

// if (dHdg > 180)

// {

// dHdg -= 360;

// mySerial.printf("dHdg > 180 - subtracting 360\n");

// }

// else if (dHdg < -180)

// {

// dHdg += 360;

// mySerial.printf("dHdg < -180 - adding 360\n");

// }

// //watch for turn rates that are wildly off

// double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);

// if (rate > 3 * degPersec)

// {

// //DEBUG!!

// //mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec);

// //DEBUG!!

// rate = degPersec;

// }

// //06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet

// if (bFirstIMUHdg)

// {

// bFirstIMUHdg = false;

// Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

// }

// else

// {

// PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

// int speed = 0;

// (TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput;

// (TurnRatePIDOutput <= MOTOR_SPEED_LOW) ? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput;

// SetLeftMotorDirAndSpeed(!b_ccw, speed);

// SetRightMotorDirAndSpeed(b_ccw, speed);

// //check for nearly there and all the way there

// //curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);

// //matchSlope = curHdgMatchVal - prevHdgMatchVal;

// //prevHdgMatchVal = curHdgMatchVal;

// //DEBUG!!

// mySerial.printf("%lu\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%2.2f\t%d\n",

// millis(),

// IMUHdgValDeg,

// Prev_HdgDeg,

// dHdg,

// rate,

// degPersec,

// lastError,

// TurnRate_Kp * lastError,

// TurnRate_Ki * lastIval,

// TurnRate_Kd * lastDerror,

// speed);

// //DEBUG!!

// Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

// //look for full match

// GetRequestedVL53l0xValues(VL53L0X_ALL);

// bDoneTurning = (abs(LeftSteeringVal - tgtSteerVal) <= RATIO_TURN_MATCH_THRESHOLD);

// }

// //bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

// //if (bTimedOut)

// //{

// // //DEBUG!!

// // //mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

// // //DEBUG!!

// // bResult = false;

// //}

// }

// //DEBUG!!

// //mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg);

// //DEBUG!!

////Step4: Re-enable TIMER5 interrupts

// TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

// StopBothMotors();

// delay(1000); //added 04/27/21 for debug

// return bResult;

//}

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)

{

//Purpose: Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR

//Inputs:

// input = double denoting current input value (turn rate, speed, whatever)

// setpoint = double denoting desired setpoint in same units as input

// sampleTime = int denoting sample time to be used in calcs.

// lastError = ref to double denoting error saved from prev calc

// lastInput = ref to double denoting input saved from prev calc

// Kp/Ki/Kd = doubles denoting PID values to be used for calcs

// Output = ref to double denoting output from calc

double error = setpoint - input;

double dInput = (input - lastInput);

lastIval += (error);

double dErr = (error - lastError);

//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr,

// Kp,Ki,Kd);

/*Compute PID Output*/

output = Kp * error + Ki * lastIval + Kd * dErr;

/*Remember some variables for next time*/

lastError = error;

lastInput = input;

lastDerror = dErr;

}

#pragma endregion HDG_BASED_TURN_SUPPORT

bool GetRequestedVL53l0xValues(VL53L0X_REQUEST which)

{

//Purpose: Obtain VL53L0X ToF sensor data from Teensy sensor handler

//Inputs:

// which = VL53L0X_REQUEST value denoting which combination of value to retrieve

// VL53L0X_CENTERS_ONLY -> Just the left/right center sensor values

// VL53L0X_RIGHT -> All three right sensor values, in front/center/rear order

// VL53L0X_LEFT -> All three left sensor values, in front/center/rear order

// VL53L0X_ALL -> All seven sensor values, in left/right front/center/rear/rear order

// VL53L0X_REAR_ONLY -> added 10/24/20 Just the rear sensor reading

//Outputs:

// Requested sensor values, obtained via I2C from the VL53L0X sensor handler

// Returns TRUE if data retrieval successful, otherwise FALSE

//Plan:

// Step1: Send request to VL53L0X handler

// Step2: get the requested data

//Notes:

// Copied from FourWD_WallE2_V4.ino's IsIRBeamAvail() and adapted

// 08/05/20 added a VL53L0X_ALL request type

// 01/24/21 added error detection/reporting

//Step1: Send request to VL53L0X handler

//DEBUG!!

//mySerial.printf("Sending %d to slave\n", which);

//DEBUG!!

//mySerial.printf("In GetRequestedVL53l0xValues(%d)\n", (int)which);

Wire.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

I2C_writeAnything((uint8_t)which);

Wire.endTransmission();

//Step2: get the requested data

int readResult = 0;

int data_size = 0;

switch (which)

{

case VL53L0X_READYCHECK: //11/10/20 added to prevent bad reads during Teensy setup()

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(sizeof(bVL53L0X_TeensyReady)));

readResult = I2C_readAnything(bVL53L0X_TeensyReady);

break;

case VL53L0X_CENTERS_ONLY:

//just two data values needed here

data_size = 2 * sizeof(int);

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(2 * sizeof(Lidar_RightCenter)));

readResult = I2C_readAnything(Lidar_RightCenter);

if (readResult > 0)

{

I2C_readAnything(Lidar_LeftCenter);

}

//DEBUG!!

//mySerial.printf("VL53L0X_CENTERS_ONLY case: Got LC/RC = %d, %d\n", Lidar_LeftCenter, Lidar_RightCenter);

//DEBUG!!

break;

case VL53L0X_RIGHT:

//four data values needed here

data_size = 3 * sizeof(int) + sizeof(float);

//DEBUG!!

//mySerial.printf("data_size = %d\n", data_size);

//DEBUG!!

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

readResult = I2C_readAnything(Lidar_RightFront);

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_RightCenter);

}

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_RightRear);

}

if (readResult > 0)

{

readResult = I2C_readAnything(RightSteeringVal);

}

//DEBUG!!

//mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",

// Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, ToFSteeringVal);

//DEBUG!!

break;

case VL53L0X_LEFT:

//four data values needed here

data_size = 3 * sizeof(int) + sizeof(float);

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

readResult = I2C_readAnything(Lidar_LeftFront);

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_LeftCenter);

}

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_LeftRear);

}

if (readResult > 0)

{

readResult = I2C_readAnything(LeftSteeringVal);

}

//DEBUG!!

//mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",

// Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal);

//DEBUG!!

break;

case VL53L0X_ALL: //added 08/05/20, chg to VL53L0X_ALL 10/31/20

//nine data values needed here - 7 ints and 2 floats

data_size = 7 * sizeof(int) + 2 * sizeof(float); //10/31/20 added rear distance

//mySerial.printf("In VL53L0X_ALL case with data_size = %d\n", data_size);

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

//Lidar_LeftFront

readResult = I2C_readAnything(Lidar_LeftFront);

if (readResult != sizeof(Lidar_LeftFront))

{

mySerial.printf("Error reading Lidar_LeftFront\n");

}

//Lidar_LeftCenter

readResult = I2C_readAnything(Lidar_LeftCenter);

if (readResult != sizeof(Lidar_LeftCenter))

{

mySerial.printf("Error reading Lidar_LeftCenter\n");

}

//Lidar_LeftRear

readResult = I2C_readAnything(Lidar_LeftRear);

if (readResult != sizeof(Lidar_LeftRear))

{

mySerial.printf("Error reading Lidar_LeftRear\n");

}

//LeftSteeringVal

readResult = I2C_readAnything(LeftSteeringVal);

if (readResult != sizeof(LeftSteeringVal))

{

mySerial.printf("Error reading LeftSteeringVal\n");

}

//Lidar_RightFront

readResult = I2C_readAnything(Lidar_RightFront);

if (readResult != sizeof(Lidar_RightFront))

{

mySerial.printf("Error reading Lidar_RightFront\n");

}

//Lidar_RightCenter

readResult = I2C_readAnything(Lidar_RightCenter);

if (readResult != sizeof(Lidar_RightCenter))

{

mySerial.printf("Error reading Lidar_RightCenter\n");

}

//Lidar_RightRear

readResult = I2C_readAnything(Lidar_RightRear);

if (readResult != sizeof(Lidar_RightRear))

{

mySerial.printf("Error reading Lidar_RightRear\n");

}

//RightSteeringVal

readResult = I2C_readAnything(RightSteeringVal);

if (readResult != sizeof(RightSteeringVal))

{

mySerial.printf("Error reading LeftSteeringVal\n");

}

//Lidar_Rear

readResult = I2C_readAnything(Lidar_Rear);

if (readResult != sizeof(Lidar_Rear))

{

mySerial.printf("Error reading Lidar_Rear\n");

}

//mySerial.printf("%lu: VL53l0x - %d, %d, %d, %d, %d, %d, %d\n",

// millis(),

// Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

// Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear,

// Lidar_Rear);

break; //10/31/20 bugfix

case VL53L0X_REAR_ONLY:

//just ONE data value needed here

data_size = sizeof(int);

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(sizeof(Lidar_Rear)));

readResult = I2C_readAnything(Lidar_Rear);

//DEBUG!!

//mySerial.printf("VL53L0X_REAR_ONLY case: Got Rear = %d\n", Lidar_Rear);

//DEBUG!!

break;

default:

break;

}

return readResult > 0; //this is true only if all reads succeed

}

void WaitForVL53L0XTeensy()

{

bVL53L0X_TeensyReady = false;

while (!bVL53L0X_TeensyReady)

{

GetRequestedVL53l0xValues(VL53L0X_READYCHECK); //this updates bVL53L0X_TeensyReady

//mySerial.printf("%lu: got %d from VL53L0X Teensy\n", millis(), bVL53L0X_TeensyReady);

delay(100);

}

Serial.print(F("Teensy setup() finished at ")); Serial.printf("%lu mSec\n", millis());

//now try to get a VL53L0X measurement

//11/08/20 rev to loop until all distance sensors provide valid data

//mySerial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\n");

//mySerial.printf("%lu: %d\t%d\t%d\t%d\t%d\t%d\t%d\n",

// millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

// Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, Lidar_Rear);

while (Lidar_LeftFront <= 0 || Lidar_LeftCenter <= 0 || Lidar_LeftRear <= 0

|| Lidar_LeftFront <= 0 || Lidar_LeftCenter <= 0 || Lidar_LeftRear <= 0

|| Lidar_Rear <= 0)

{

////values updated 10 times/sec in ISR

mySerial.printf("%lu: %d\t%d\t%d\t%d\t%d\t%d\t%d\n",

millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, Lidar_Rear);

delay(100);

}

mySerial.printf("VL53L0X Teensy Ready at %lu\n\n", millis());

}

double GetSteeringAngle(double steerval)

{

//Purpose: Convert a steering angle into the equivalent off-parallel orientation in degrees

//Inputs:

// steerval = double value denoting current steering value

// Notes:

// 06/27/21 now steering_val/off_angle slope is a constant 0.0175

const double slopeval = 0.0175;

double offparalleldeg = steerval / slopeval; //e.g. for steerval = -0.187 @ 10cm offset, returns +21deg

//mySerial.printf("GetSteeringAngle(%d, %2.4f): slopeval = %2.4f, res %2.2f\n", ctrdist_cm, steerval, slopeval, offparalleldeg);

return offparalleldeg;

}

//11/05/15 added to get LIDAR measurement

int GetFrontDistCm()

{

//Notes:

// 12/05/15 chg to MODE line vs I2C

// 01/06/16 rev to return avg of prev distances on error

#ifndef NO_LIDAR

unsigned long pulse_width;

int LIDARdistCm;

pulse_width = pulseIn(LIDAR_MODE_PIN, HIGH); // Count how long the pulse is high in microseconds

LIDARdistCm = pulse_width / 10; // 10usec = 1 cm of distance for LIDAR-Lite

//chk for erroneous reading

if (LIDARdistCm == 0)

{

//replace with average of last three readings from aFrontDist

int avgdist = GetAvgFrontDistCm();

mySerial.printf("%lu: Error in GetFrontDistCm() - %d replaced with %d\n", millis(), LIDARdistCm, avgdist);

LIDARdistCm = avgdist;

}

//04/30/17 added limit detection/correction

LIDARdistCm = (LIDARdistCm > 0) ? LIDARdistCm : MAX_FRONT_DISTANCE_CM;

return LIDARdistCm;

#else

return 10; //safe number, I hope

#endif

}

//04/25/21 rewritten to use aFrontDist[] values

float GetAvgFrontDistCm()

{

int avgdist = 0;

for (int i = 0; i < FRONT_DIST_AVG_WINDOW_SIZE; i++)

{

//DEBUG!!

//mySerial.printf("frontdist[%d] = %d\n", FRONT_DIST_ARRAY_SIZE - 1 - i, aFrontDist[FRONT_DIST_ARRAY_SIZE - 1 - i]);

//DEBUG!!

avgdist += aFrontDist[FRONT_DIST_ARRAY_SIZE - 1 - i];

}

//DEBUG!!

//mySerial.printf("avgdisttot = %d\n", avgdist);

//DEBUG!!

avgdist = (int)((float)avgdist / (float)FRONT_DIST_AVG_WINDOW_SIZE);

//DEBUG!!

//mySerial.printf("avgdist = %d\n", avgdist);

//DEBUG!!

return avgdist;

}

//08/12/20 Extracted inline FRONT_DIST_ARRAY init code so can be called from anywhere

void InitFrontDistArray()

{

//04/01/15 initialize 'stuck detection' arrays

//06/17/20 re-wrote for better readability

//to ensure var > STUCK_FRONT_VARIANCE_THRESHOLD for first FRONT_DIST_ARRAY_SIZE loops

//array is initialized with sawtooth from 0 to MAX_FRONT_DISTANCE_CM

int newval = 0;

int bumpval = MAX_FRONT_DISTANCE_CM / FRONT_DIST_ARRAY_SIZE;

bool bgoingUp = true;

for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++)

{

aFrontDist[i] = newval;

//DEBUG!!

//mySerial.printf("i = %d, newval = %d, aFrontdist[%d] = %d\n", i, newval, i, aFrontDist[i]);

//DEBUG!!

if (bgoingUp)

{

if (newval < MAX_FRONT_DISTANCE_CM - bumpval) //don't want newval > MAX_FRONT_DISTANCE_CM

{

newval += bumpval;

}

else

{

bgoingUp = false;

}

else

{

if (newval > bumpval) //don't want newval < 0

{

newval -= bumpval;

}

else

{

bgoingUp = true;

}

//04/19/19 init Front_Dist_PrevVMean & Front_Dist_PrevVar to mean/var respectively

long sum = 0;

for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++)

{

sum += aFrontDist[i]; //adds in rest of values

}

Front_Dist_PrevVMean = (float)sum / (float)FRONT_DIST_ARRAY_SIZE;

// Step2: calc new 'brute force' variance

float sumsquares = 0;

for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++)

{

sumsquares += (aFrontDist[i] - Front_Dist_PrevVMean) * (aFrontDist[i] - Front_Dist_PrevVMean);

}

Front_Dist_PrevVar = sumsquares / FRONT_DIST_ARRAY_SIZE;

mySerial.printf("%lu: aFrontDist Init: Front_Dist_PrevVMean = %3.2f, Front_Dist_PrevVar = %3.2f\n", millis(), Front_Dist_PrevVMean, Front_Dist_PrevVar);

frontvar = Front_Dist_PrevVar; //added 03/30/21 to prevent 'stuck' at startup

}

Here is a link to the above file, plus all required library & ancillary files.

Stay tuned,

Frank

Another Try at Wall Offset Tracking

1 Reply

Posted 22 June 2021

About nine months ago (October 2020) I made a run at getting offset tracking to work (see here and here). This post describes yet another attempt at getting this right, taking advantage of recent work on controlled-rate turns. I constructed a short single-task program to do just the wall tracking task, hopefully simplifying things to the point where I can understand what is happening.

One of the big issues that arose in previous work was the inability to synch my TIMER5 ISR with the PID library’s ‘Compute()’ function. The PID library insists on managing the update timing internally, which meant there was no way to ensure that Compute() would be called every time the ISR ran. I eventually came to the conclusion that I simply could not use the PID library version, and instead wrote my own small function that did the Compute() function, but with the timing value passed in as an argument rather than being managed internally. This forces the PID calculations to actually update in synch with the TIMER5 interrupt. Here’s the new PIDCalcs() function:

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)
{
	//Purpose:  Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR
	//Inputs:
	//	input = double denoting current input value (turn rate, speed, whatever)
	//	setpoint = double denoting desired setpoint in same units as input
	//	sampleTime = int denoting sample time to be used in calcs.
	//	lastError = ref to double denoting error saved from prev calc
	//	lastInput = ref to double denoting input saved from prev calc
	//	Kp/Ki/Kd = doubles denoting PID values to be used for calcs
	//	Output = ref to double denoting output from calc

	double error = setpoint - input;
	double dInput = (input - lastInput);
	lastIval += (error);
	double dErr = (error - lastError);

	//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr,
	//	Kp,Ki,Kd);

	/*Compute PID Output*/
	output = Kp * error + Ki * lastIval + Kd * dErr;

	/*Remember some variables for next time*/
	lastError = error;
	lastInput = input;
	lastDerror = dErr;
}

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)

{

//Purpose: Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR

//Inputs:

// input = double denoting current input value (turn rate, speed, whatever)

// setpoint = double denoting desired setpoint in same units as input

// sampleTime = int denoting sample time to be used in calcs.

// lastError = ref to double denoting error saved from prev calc

// lastInput = ref to double denoting input saved from prev calc

// Kp/Ki/Kd = doubles denoting PID values to be used for calcs

// Output = ref to double denoting output from calc

double error = setpoint - input;

double dInput = (input - lastInput);

lastIval += (error);

double dErr = (error - lastError);

//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr,

// Kp,Ki,Kd);

/*Compute PID Output*/

output = Kp * error + Ki * lastIval + Kd * dErr;

/*Remember some variables for next time*/

lastError = error;

lastInput = input;

lastDerror = dErr;

}

As can be seen from the above, this is a very simple routine that just does one thing, and doesn’t incorporate any of the improvements (windup suppression, sample time changes, etc) available in the PID library. The calling function has to manage the persistent parameters, but that’s a small price to pay for clarity and the assurance that the output value will indeed be updated every time the function is called.

With this function in hand, I worked on getting the robot to reliably track a specified offset, but was initially stymied because while I could get it to control the motors so that the robot stayed parallel to the nearest wall, I still couldn’t get it to track a specific offset. I solved this problem by leveraging my new-found ability to make accurate, rate-controlled turns; the robot first turns by an amount proportional to it’s distance from the desired offset, moves straight ahead until the offset is met, and then turns the same number of degrees in the other direction. Assuming the robot started parallel to the wall, this results in it facing the direction of travel, at the desired offset and parallel to the nearest wall. Here is the code for this algorithm.

void TrackLeftWallOffset(double kp, double ki, double kd, double offsetCm)
{
	//Notes:
	//	06/21/21 modified to do a turn, then straight, then track 0 steer val

	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;
	bool bFirstTime = true;

	WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5

	//at 30mm from tgt offset, setpoint will be +/-0.3
	WallTrackSetPoint = (double)(10 * WALL_OFFSET_TGTDIST_CM - Lidar_LeftCenter) / 100.f; //10/04/20 positive value drives robot toward wall

	//limit approach angle to reasonable values
	if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;
	if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

	mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);
	mySerial.printf("Msec\tFdir\tCdir\ttRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

	//06/21/21 modified to do a turn, then straight, then track 0 steer val
	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_LeftCenter / 10.f);
	mySerial.printf("dist = %d, cutAngleDeg = %2.2f\n", Lidar_LeftCenter, cutAngleDeg);
	SpinTurn((cutAngleDeg < 0), cutAngleDeg, 30);

	while (abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM) > 10)
	{
		MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);
		GetRequestedVL53l0xValues(VL53L0X_LEFT);
		mySerial.printf("%lu \t%d\t%d\t%d\t%d\n",
			millis(),
			Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
			abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM));
		delay(100);
	}

	//now turn back the same amount
	SpinTurn(!(cutAngleDeg < 0), cutAngleDeg, 30);

	//sinceLastComputeTime = 0;
		WallTrackSetPoint = 0; //moved here 6/22/21

	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);
			}
		}
	}
}

void TrackLeftWallOffset(double kp, double ki, double kd, double offsetCm)

{

//Notes:

// 06/21/21 modified to do a turn, then straight, then track 0 steer val

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

bool bFirstTime = true;

WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5

//at 30mm from tgt offset, setpoint will be +/-0.3

WallTrackSetPoint = (double)(10 * WALL_OFFSET_TGTDIST_CM - Lidar_LeftCenter) / 100.f; //10/04/20 positive value drives robot toward wall

//limit approach angle to reasonable values

if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;

if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

mySerial.printf("Msec\tFdir\tCdir\ttRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

//06/21/21 modified to do a turn, then straight, then track 0 steer val

GetRequestedVL53l0xValues(VL53L0X_LEFT);

double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_LeftCenter / 10.f);

mySerial.printf("dist = %d, cutAngleDeg = %2.2f\n", Lidar_LeftCenter, cutAngleDeg);

SpinTurn((cutAngleDeg < 0), cutAngleDeg, 30);

while (abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM) > 10)

{

MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);

GetRequestedVL53l0xValues(VL53L0X_LEFT);

mySerial.printf("%lu \t%d\t%d\t%d\t%d\n",

millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM));

delay(100);

}

//now turn back the same amount

SpinTurn(!(cutAngleDeg < 0), cutAngleDeg, 30);

//sinceLastComputeTime = 0;

WallTrackSetPoint = 0; //moved here 6/22/21

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);

}

Here are a couple of videos showing the ability to capture and track a desired offset from either side.

In both of the above videos, the desired wall offset is 30 cm.

Stay Tuned,

Frank

Turn Rate PID Tuning, Part IV

1 Reply

Posted 10 June 2021,

In my last post on this issue, I described using a small test program to explore an in-line version of the PID (Proportional-Integral-Differential) algorithm for turn rate control with Wall-E2, my autonomous wall-following robot. This post describes some follow-on work on this same subject.

The fundamental problem with all the available Arduino PID libraries is they all require the user to wait in a loop for the PID::Compute() function to decide when to actually produce a new output value, and since this computation is inside the function, it is difficult or impossible to synchronize any other related timed element with the PID function. In my case where I want to control the turn rate of a robot, the input to the PID engine is, obviously, the turn rate, in degrees/sec. But, calculation of the turn rate is necessarily a timed function, i.e. (curent_heading – last_heading) / elapsed_time, where the ‘elapsed_time’ parameter is usually a constant. But, all the Arduino PID libraries use an internal private class member that defines the measurement period (in milliseconds), and this value isn’t available externally (well, it is, but only because the user can set the sample time – it can’t be read). So, the best one can do with the current libraries is to use the same constant for PID::SetSampleTime() and for any external time-based calculations, and hope there aren’t any synchronization issues. With this setup, it would be quite possible (and inevitable IMHO) for the PID::Compute() function to skip a step, or to be ‘phase-locked’ to producing an output that is one time constant off from the input.

The solution to this problem is to not use a PID library at all, and instead place the PID algorithm in-line with the rest of the code. This ensures that the PID calculation any related time-based calculations are operating on the same schedule. The downside of this arrangement is loss of generality; all the cool enhancements described by Brett Beauregard in his wonderful PID tutorial go away or have to be implemented in-line as well. From Brett’s tutorial, here’s ‘the beginner’s PID algorithm’:

The difficulty with all the current PID libraries is the ‘dt’ parameter in the above expression; the implementation becomes much easier if ‘dt’ is a constant – i.e. the time between calculations is a constant. However, this constraint also requires that the library, not the user program, controls the timing. This just doesn’t work when the ‘Input’ parameter above also requires a constant time interval for calculation. In my case of turn rate control, the turn rate calculation requires knowledge of the time interval between calculations, and the calculation itself should be done immediately after the turn rate is determined, using the same time interval. So, the turn rate is calculated and then PID::Compute() is called, but Compute() may or may not generate a new output value, because it can return without action if it’s internal time duration criteria isn’t met; see the problem? It may, or even might, generate a new output value each time, but there is no way to ensure that it will!

After figuring this out the hard way (by trying and failing to make the library work), I finally decided to forget the library – at least for my turn rate problem, and in-line all the needed code. Once I had it all running, I abstracted just the PID algorithm to its own function so I could use it elsewhere. This function is shown below:

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)
{
	//Purpose:  Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR
	//Inputs:
	//	input = double denoting current input value (turn rate, speed, whatever)
	//	setpoint = double denoting desired setpoint in same units as input
	//	sampleTime = int denoting sample time to be used in calcs.
	//	lastError = ref to double denoting error saved from prev calc
	//	lastInput = ref to double denoting input saved from prev calc
	//	Kp/Ki/Kd = doubles denoting PID values to be used for calcs
	//	Output = ref to double denoting output from calc


	double error = setpoint - input;
	double dInput = (input - lastInput);
	lastIval += (error);
	double dErr = (error - lastError);

	//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr = %2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr);

	/*Compute PID Output*/
	output = Kp * error + Ki * lastIval + Kd * dErr;

	/*Remember some variables for next time*/
	lastError = error;
	lastInput = input;
	lastDerror = dErr;
}

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)

{

//Purpose: Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR

//Inputs:

// input = double denoting current input value (turn rate, speed, whatever)

// setpoint = double denoting desired setpoint in same units as input

// sampleTime = int denoting sample time to be used in calcs.

// lastError = ref to double denoting error saved from prev calc

// lastInput = ref to double denoting input saved from prev calc

// Kp/Ki/Kd = doubles denoting PID values to be used for calcs

// Output = ref to double denoting output from calc

double error = setpoint - input;

double dInput = (input - lastInput);

lastIval += (error);

double dErr = (error - lastError);

//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr = %2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr);

/*Compute PID Output*/

output = Kp * error + Ki * lastIval + Kd * dErr;

/*Remember some variables for next time*/

lastError = error;

lastInput = input;

lastDerror = dErr;

}

As you can see, the explanatory comments are much bigger than the function itself, which is really just eight lines long. Also, it has a huge number of arguments, five of which are references that are updated by the function. This function wouldn’t win any awards for good design, as it has too many arguments (wide coupling), but it does have high cohesion (does just one thing), and the coupling is at least ‘data’ coupling only.

Once this function was implemented, the calling function (in this case ‘SpinTurn()’ looks like this:

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)
{
	//Purpose: Make a numDeg CW or CCW 'spin' turn
	//Inputs:
	//	b_ccw - True if turn is to be ccw, false otherwise
	//	numDeg - angle to be swept out in the turn
	//	ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg
	//	IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR
	//	degPerSec = float value denoting desired turn rate
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Compute new target value
	//	Step3: 11/14/20 activate turn rate PID
	//	Step4: Set motor speeds based on b_ccw
	//	Step5: Run motors until target reached, using PID to control turn rate
	//Notes:
	//	06/27/20 copied from RollingTurn & adapted
	//	09/02/20 experiment with adjustable turn rate
	//	11/02/20 rev to use ISR-updated IMUHdgValDeg vs call to UpdateIMUHdgValDeg
	//	11/06/20 rev to use bTimeForNavUpdate flag to space out hdg/dist updates
	//	11/06/20 rev to make target turn rate variable for slowdown portion of spin
	//	11/14/20 rev to use PID for turn rate managaement
	//	04/05/21 rewritten to use ISR for timing and simplified
	//	04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)
	//	05/29/21 replaced TurnRatePID with SetTurnRateMotorSpeeds()
	//	06/06/21 added re-try for 180.00 return from IMU - could be bad value

	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point
	//unsigned long prev_uSec; //added 09/02/20

	//DEBUG!!
	mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);
	//DEBUG!!
	mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",
		TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

	//no need to continue if the IMU isn't available
	if (!dmpReady)
	{
		return false;
	}

	UpdateIMUHdgValDeg(); //refreshes IMHHdgValDeg

	//06/06/21 it is possible for IMU to return 180.00 on failure
	//so try one more time in this case.  If it really IS 180, then 
	//the second try should confirm this.
	if (IMUHdgValDeg == 180.f)
	{
		mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");
		UpdateIMUHdgValDeg();
	}

	//Step2: Compute new target value & timeout value
	timeout_sec = 2 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

	//05/17/20 limit timeout_sec to 1 sec or more
	timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

	//12/05/19 added #define back in to manage which direction increases yaw values
#ifdef MPU6050_CCW_INCREASES_YAWVAL
	tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;
#else
	tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

	//correct for -180/180 transition
	if (tgt_deg < -180)
	{
		tgt_deg += 360;
	}

	//07/29/19 bugfix
	if (tgt_deg > 180)
	{
		tgt_deg -= 360;
	}

	//DEBUG!!
	mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f  deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",
		IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);
	//DEBUG!!

	float curHdgMatchVal = 0;

	//09/08/18 added to bolster end-of-turn detection
	float prevHdgMatchVal = 0;
	float matchSlope = 0;
	TIMSK5 = 0; //disable timer compare interrupt

	//Step5: Run motors until target reached, using PID to control turn rate
	Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

	elapsedMillis sinceLastTimeCheck = 0;
	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;

	while (!bDoneTurning && !bTimedOut)
	{
		//11/06/20 now just loops between ISR hits
		CheckForUserInput();
		UpdateIMUHdgValDeg(); //now Prev_HdgDeg & IMUHdgValDeg can be different (but probably aren't the first time through)

			//runs every 30mSec
		if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)
		{
			sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

			//here Prev_HdgDeg & IMUHdgValDeg should be different, as TURN_RATE_UPDATE_INTERVAL_MSEC should have elapsed since last synch
			double dHdg = IMUHdgValDeg - Prev_HdgDeg;
			double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);
			Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

			PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

			int speed = 0;

			if (TurnRatePIDOutput > MOTOR_SPEED_MAX) speed = MOTOR_SPEED_MAX;
			else if (TurnRatePIDOutput <= MOTOR_SPEED_LOW) speed = MOTOR_SPEED_LOW;
			else speed = (int)TurnRatePIDOutput;

			SetLeftMotorDirAndSpeed(!b_ccw, speed);
			SetRightMotorDirAndSpeed(b_ccw, speed);

			//07/31/19 now running as fast as we can get valid hdgs from IMU
			//check for nearly there and all the way there
			curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);
			matchSlope = curHdgMatchVal - prevHdgMatchVal;
			prevHdgMatchVal = curHdgMatchVal;

			//look for full match
			bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL
				|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start
		}

		bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

		if (bTimedOut)
		{
			//DEBUG!!
			mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);
			//DEBUG!!

			bResult = false;

		}
	}

	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

	//04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding
	StopBothMotors();
	delay(1000); //added 04/27/21 for debug
	
	return bResult;
}

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

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)

{

//Purpose: Make a numDeg CW or CCW 'spin' turn

//Inputs:

// b_ccw - True if turn is to be ccw, false otherwise

// numDeg - angle to be swept out in the turn

// ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg

// IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR

// degPerSec = float value denoting desired turn rate

//Plan:

// Step1: Get current heading as starting point

// Step2: Compute new target value

// Step3: 11/14/20 activate turn rate PID

// Step4: Set motor speeds based on b_ccw

// Step5: Run motors until target reached, using PID to control turn rate

//Notes:

// 06/27/20 copied from RollingTurn & adapted

// 09/02/20 experiment with adjustable turn rate

// 11/02/20 rev to use ISR-updated IMUHdgValDeg vs call to UpdateIMUHdgValDeg

// 11/06/20 rev to use bTimeForNavUpdate flag to space out hdg/dist updates

// 11/06/20 rev to make target turn rate variable for slowdown portion of spin

// 11/14/20 rev to use PID for turn rate managaement

// 04/05/21 rewritten to use ISR for timing and simplified

// 04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)

// 05/29/21 replaced TurnRatePID with SetTurnRateMotorSpeeds()

// 06/06/21 added re-try for 180.00 return from IMU - could be bad value

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

//unsigned long prev_uSec; //added 09/02/20

//DEBUG!!

mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);

//DEBUG!!

mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",

TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

//no need to continue if the IMU isn't available

if (!dmpReady)

{

return false;

}

UpdateIMUHdgValDeg(); //refreshes IMHHdgValDeg

//06/06/21 it is possible for IMU to return 180.00 on failure

//so try one more time in this case. If it really IS 180, then

//the second try should confirm this.

if (IMUHdgValDeg == 180.f)

{

mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");

UpdateIMUHdgValDeg();

}

//Step2: Compute new target value & timeout value

timeout_sec = 2 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

//05/17/20 limit timeout_sec to 1 sec or more

timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

//12/05/19 added #define back in to manage which direction increases yaw values

#ifdef MPU6050_CCW_INCREASES_YAWVAL

tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;

#else

tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

//correct for -180/180 transition

if (tgt_deg < -180)

{

tgt_deg += 360;

}

//07/29/19 bugfix

if (tgt_deg > 180)

{

tgt_deg -= 360;

}

//DEBUG!!

mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",

IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);

//DEBUG!!

float curHdgMatchVal = 0;

//09/08/18 added to bolster end-of-turn detection

float prevHdgMatchVal = 0;

float matchSlope = 0;

TIMSK5 = 0; //disable timer compare interrupt

//Step5: Run motors until target reached, using PID to control turn rate

Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

elapsedMillis sinceLastTimeCheck = 0;

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

while (!bDoneTurning && !bTimedOut)

{

//11/06/20 now just loops between ISR hits

CheckForUserInput();

UpdateIMUHdgValDeg(); //now Prev_HdgDeg & IMUHdgValDeg can be different (but probably aren't the first time through)

//runs every 30mSec

if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

//here Prev_HdgDeg & IMUHdgValDeg should be different, as TURN_RATE_UPDATE_INTERVAL_MSEC should have elapsed since last synch

double dHdg = IMUHdgValDeg - Prev_HdgDeg;

double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

int speed = 0;

if (TurnRatePIDOutput > MOTOR_SPEED_MAX) speed = MOTOR_SPEED_MAX;

else if (TurnRatePIDOutput <= MOTOR_SPEED_LOW) speed = MOTOR_SPEED_LOW;

else speed = (int)TurnRatePIDOutput;

SetLeftMotorDirAndSpeed(!b_ccw, speed);

SetRightMotorDirAndSpeed(b_ccw, speed);

//07/31/19 now running as fast as we can get valid hdgs from IMU

//check for nearly there and all the way there

curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);

matchSlope = curHdgMatchVal - prevHdgMatchVal;

prevHdgMatchVal = curHdgMatchVal;

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start

}

bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

if (bTimedOut)

{

//DEBUG!!

mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

//DEBUG!!

bResult = false;

}

TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

//04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding

StopBothMotors();

delay(1000); //added 04/27/21 for debug

return bResult;

}

In the above code, the lines dealing with ‘TIMSK5’ are there to disable and then re-enable the TIMER5 interrupt I have set up to update external sensor values every 100 mSec. I’m not really sure that this HAS to be done, but once I learned how to do it I figured it wouldn’t hurt, either ☺

Now that I have this ‘PIDCalcs()’ function working properly, I plan to use it in several other places where I currently use the PID library; it’s just so much simpler now, and because all the relevant parameters are visible to the calling program, debugging is now a piece of cake where before it was just an opaque black box.

12 June 2021 Update:

After chasing down and eliminating a number of bugs and edge-case issues, I think I now have a pretty stable/working version of the ‘SpinTurn’ function, as shown below:

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)
{
	//Purpose: Make a numDeg CW or CCW 'spin' turn
	//Inputs:
	//	b_ccw - True if turn is to be ccw, false otherwise
	//	numDeg - angle to be swept out in the turn
	//	ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg
	//	IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR
	//	degPerSec = float value denoting desired turn rate
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Disable TIMER5 interrupts
	//	Step3: Compute new target value & timeout value
	//	Step4: Run motors until target reached, using inline PID algorithm to control turn rate
	//	Step5: Re-enable TIMER5 interrupts
	//Notes:
	//	06/06/21 we-written to remove PID library - now uses custom 'PIDCalcs()' function
	//	06/06/21 added re-try for 180.00 return from IMU - could be bad value
	//	06/11/21 added code to correct dHdg errors due to 179/-179 transition & bad IMU values
	//	06/12/21 cleaned up & commented out debug code

	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point

	////DEBUG!!
	//mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);
	////DEBUG!!
	//mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",
	//	TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

	//no need to continue if the IMU isn't available
	if (!dmpReady)
	{
		return false;
	}

//Step1: Get current heading as starting point
	//06/06/21 it is possible for IMU to return 180.00 on failure
	//so try again.  If it really IS 180, then 
	//it will eventually time out and go on
	int retries = 0;
	if (IMUHdgValDeg == 180.f && retries < 5)
	{
		//DEBUG!!
		//mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");
		//DEBUG!!
		UpdateIMUHdgValDeg();
		retries++;
		delay(30);
	}

//Step2: Compute new target value & timeout value
	timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

	//05/17/20 limit timeout_sec to 1 sec or more
	timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

	UpdateIMUHdgValDeg();

	//12/05/19 added #define back in to manage which direction increases yaw values
#ifdef MPU6050_CCW_INCREASES_YAWVAL
	tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;
#else
	tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

	//correct for -180/180 transition
	if (tgt_deg < -180)
	{
		tgt_deg += 360;
	}

	//07/29/19 bugfix
	if (tgt_deg > 180)
	{
		tgt_deg -= 360;
	}

	//DEBUG!!
	//mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f  deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",
	//	IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);
	//DEBUG!!

	float curHdgMatchVal = 0;

	//09/08/18 added to bolster end-of-turn detection
	float prevHdgMatchVal = 0;
	float matchSlope = 0;


//Step2: Disable TIMER5 interrupts
	TIMSK5 = 0; //disable timer compare interrupt

	//Step3: Run motors until target reached, using PID algorithm to control turn rate
	Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

	//DEBUG!!
	//mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg);

	//mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n");
	//DEBUG!!

	elapsedMillis sinceLastTimeCheck = 0;
	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;
	bool bFirstIMUHdg = true;

	while (!bDoneTurning && !bTimedOut)
	{
		//11/06/20 now just loops between ISR hits
		CheckForUserInput();

			//runs every 30mSec
		if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)
		{
			sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

			UpdateIMUHdgValDeg(); //update IMUHdgValDeg

			double dHdg = IMUHdgValDeg - Prev_HdgDeg;
			if (dHdg > 180)
			{
				dHdg -= 360;
				mySerial.printf("dHdg > 180 - subtracting 360\n");
			}
			else if (dHdg < -180)
			{
				dHdg += 360;
				mySerial.printf("dHdg < -180 - adding 360\n");
			}

			//watch for turn rates that are wildly off
			double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);
			if (rate > 3*degPersec)
			{
				//DEBUG!!
				//mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec);
				//DEBUG!!
				rate = degPersec;
			}

			//06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet
			if (bFirstIMUHdg)
			{
				bFirstIMUHdg = false;
				Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time
			}
			else
			{
				PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

				int speed = 0;

				(TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput;
				(TurnRatePIDOutput <= MOTOR_SPEED_LOW)? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput;

				SetLeftMotorDirAndSpeed(!b_ccw, speed);
				SetRightMotorDirAndSpeed(b_ccw, speed);

				//check for nearly there and all the way there
				curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);
				matchSlope = curHdgMatchVal - prevHdgMatchVal;
				prevHdgMatchVal = curHdgMatchVal;

				//DEBUG!!
				//mySerial.printf("%lu\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%2.2f\t%d\n", 
				//	millis(), 
				//	IMUHdgValDeg,
				//	Prev_HdgDeg,
				//	dHdg,
				//	rate,
				//	degPersec, 
				//	lastError, 
				//	TurnRate_Kp * lastError,
				//	TurnRate_Ki * lastIval,
				//	TurnRate_Kd * lastDerror,
				//	speed);
				//DEBUG!!

				Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

				//look for full match
				bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL
					|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start
			}
		}

		bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

		if (bTimedOut)
		{
			//DEBUG!!
			//mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);
			//DEBUG!!

			bResult = false;
		}
	}

	//DEBUG!!
	//mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg);
	//DEBUG!!

//Step4: Re-enable TIMER5 interrupts
	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

	StopBothMotors();
	delay(1000); //added 04/27/21 for debug
	return bResult;
}

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

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)

{

//Purpose: Make a numDeg CW or CCW 'spin' turn

//Inputs:

// b_ccw - True if turn is to be ccw, false otherwise

// numDeg - angle to be swept out in the turn

// ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg

// IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR

// degPerSec = float value denoting desired turn rate

//Plan:

// Step1: Get current heading as starting point

// Step2: Disable TIMER5 interrupts

// Step3: Compute new target value & timeout value

// Step4: Run motors until target reached, using inline PID algorithm to control turn rate

// Step5: Re-enable TIMER5 interrupts

//Notes:

// 06/06/21 we-written to remove PID library - now uses custom 'PIDCalcs()' function

// 06/06/21 added re-try for 180.00 return from IMU - could be bad value

// 06/11/21 added code to correct dHdg errors due to 179/-179 transition & bad IMU values

// 06/12/21 cleaned up & commented out debug code

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

////DEBUG!!

//mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);

////DEBUG!!

//mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",

// TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

//no need to continue if the IMU isn't available

if (!dmpReady)

{

return false;

}

//Step1: Get current heading as starting point

//06/06/21 it is possible for IMU to return 180.00 on failure

//so try again. If it really IS 180, then

//it will eventually time out and go on

int retries = 0;

if (IMUHdgValDeg == 180.f && retries < 5)

{

//DEBUG!!

//mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");

//DEBUG!!

UpdateIMUHdgValDeg();

retries++;

delay(30);

}

//Step2: Compute new target value & timeout value

timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

//05/17/20 limit timeout_sec to 1 sec or more

timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

UpdateIMUHdgValDeg();

//12/05/19 added #define back in to manage which direction increases yaw values

#ifdef MPU6050_CCW_INCREASES_YAWVAL

tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;

#else

tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

//correct for -180/180 transition

if (tgt_deg < -180)

{

tgt_deg += 360;

}

//07/29/19 bugfix

if (tgt_deg > 180)

{

tgt_deg -= 360;

}

//DEBUG!!

//mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",

// IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);

//DEBUG!!

float curHdgMatchVal = 0;

//09/08/18 added to bolster end-of-turn detection

float prevHdgMatchVal = 0;

float matchSlope = 0;

//Step2: Disable TIMER5 interrupts

TIMSK5 = 0; //disable timer compare interrupt

//Step3: Run motors until target reached, using PID algorithm to control turn rate

Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

//DEBUG!!

//mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg);

//mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n");

//DEBUG!!

elapsedMillis sinceLastTimeCheck = 0;

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

bool bFirstIMUHdg = true;

while (!bDoneTurning && !bTimedOut)

{

//11/06/20 now just loops between ISR hits

CheckForUserInput();

//runs every 30mSec

if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg(); //update IMUHdgValDeg

double dHdg = IMUHdgValDeg - Prev_HdgDeg;

if (dHdg > 180)

{

dHdg -= 360;

mySerial.printf("dHdg > 180 - subtracting 360\n");

}

else if (dHdg < -180)

{

dHdg += 360;

mySerial.printf("dHdg < -180 - adding 360\n");

}

//watch for turn rates that are wildly off

double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);

if (rate > 3*degPersec)

{

//DEBUG!!

//mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec);

//DEBUG!!

rate = degPersec;

}

//06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet

if (bFirstIMUHdg)

{

bFirstIMUHdg = false;

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

}

else

{

PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

int speed = 0;

(TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput;

(TurnRatePIDOutput <= MOTOR_SPEED_LOW)? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput;

SetLeftMotorDirAndSpeed(!b_ccw, speed);

SetRightMotorDirAndSpeed(b_ccw, speed);

//check for nearly there and all the way there

curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);

matchSlope = curHdgMatchVal - prevHdgMatchVal;

prevHdgMatchVal = curHdgMatchVal;

//DEBUG!!

//mySerial.printf("%lu\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%2.2f\t%d\n",

// millis(),

// IMUHdgValDeg,

// Prev_HdgDeg,

// dHdg,

// rate,

// degPersec,

// lastError,

// TurnRate_Kp * lastError,

// TurnRate_Ki * lastIval,

// TurnRate_Kd * lastDerror,

// speed);

//DEBUG!!

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start

}

bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

if (bTimedOut)

{

//DEBUG!!

//mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

//DEBUG!!

bResult = false;

}

//DEBUG!!

//mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg);

//DEBUG!!

//Step4: Re-enable TIMER5 interrupts

TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

StopBothMotors();

delay(1000); //added 04/27/21 for debug

return bResult;

}

With this code in place, I made some 180º turns at 45 & 90 deg/sec, both on my benchtop and on carpet, as shown in the plots and video below:

Stay Tuned!

Frank

Turn Rate PID Tuning, Part III

1 Reply

In my previous post on this subject, I described my efforts to control the turn rate (in deg/sec) of my two-wheel robot, in preparation for doing the same thing on Wall-E2, four wheel drive autonomous wall following robot.

As noted previously, I have a TIMER5 Interrupt Service Routine (ISR) set up on my four wheel robot to provide updates to the various sensor values every 100 mSec, but was unable to figure out a robust way of synchronizing the PID library’s Compute() timing with the ISR timing. So, I decided to bag the PID library entirely, at least for turn rate control, and insert the PID algorithm directly into the turn rate control, and removing the extraneous stuff that caused divide-by-zero errors when the setSampleTime() function was modified to accept a zero value.

To facilitate more rapid test cycles, I created a new program that contained just enough code to initialize and read the MP6050 IMU module, and a routine called ‘SpinTurnForever() that accepts PID parameters and causes the robot to ‘spin’ turn forever (or at least until I stop it with a keyboard command. Here’s the entire program.

/*
    Name:       FourWD_PulseTurnRateTest.ino
    Created:	5/24/2021 4:49:52 PM
    Author:     FRANKNEWXPS15\Frank
*/

#pragma region INCLUDES
#include <elapsedMillis.h>
#include <PrintEx.h> //allows printf-style printout syntax
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/05/19
#include "I2Cdev.h" //02/19/19: this includes SBWire.h
#include "I2C_Anything.h" //needed for sending float data over I2C
#pragma endregion INCLUDES

#pragma region DEFINES
#define MPU6050_I2C_ADDR 0x69
#pragma endregion DEFINES

#pragma region PRE_SETUP
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
MPU6050 mpu(MPU6050_I2C_ADDR); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0)
#pragma endregion PRE_SETUP

#pragma region MOTOR_PINS
//09/11/20 Now using two Adafruit DRV8871 drivers for all 4 motors
const int In1_Left = 10;
const int In2_Left = 11;

const int In1_Right = 8;
const int In2_Right = 9;
#pragma endregion Motor Pin Assignments

#pragma region MOTOR_PARAMETERS
//drive wheel speed parameters
const int MOTOR_SPEED_FULL = 200; //range is 0-255
const int MOTOR_SPEED_MAX = 255; //range is 0-255
const int MOTOR_SPEED_HALF = 127; //range is 0-255
const int MOTOR_SPEED_QTR = 75; //added 09/25/20
const int MOTOR_SPEED_LOW = 50; //added 01/22/15
const int MOTOR_SPEED_OFF = 0; //range is 0-255
//drive wheel direction constants
const boolean FWD_DIR = true;
const boolean REV_DIR = !FWD_DIR;

//Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;
bool bIsForwardDir = true; //default is foward direction
#pragma endregion Motor Parameters

#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

            // orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

//MPU6050 status flags
bool bMPU6050Ready = true;
bool dmpReady = false;  // set true if DMP init was successful
volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR
const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param
bool bFirstTime = true;
#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19
#pragma endregion MPU6050 Support

const int CHG_CONNECT_LED_PIN = 50; //12/16/20 added for debug

elapsedMillis MsecSinceLastTurnRateUpdate;

//const int MAX_PULSE_WIDTH_MSEC = 100;
const int MAX_PULSE_WIDTH_MSEC = 50;
const int MIN_PULSE_WIDTH_MSEC = 0;

//const int MAX_SPIN_MOTOR_SPEED = 200;
const int MAX_SPIN_MOTOR_SPEED = 250;
const int MIN_SPIN_MOTOR_SPEED = 50;

const int TURN_RATE_UPDATE_INTERVAL_MSEC = 20;
//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 100;

double TurnRateOutput;
double TurnRateSetPoint_DPS;
double Ival = 0; double lastErr = 0;
double lastTurnRateVal_DPS;
double TurnRateVal_DPS;


void setup()
{
	Serial.begin(115200);

#pragma region MPU6050
	mySerial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);
	mpu.initialize();

	// verify connection
	Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

	float StartSec = 0; //used to time MPU6050 init
	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// make sure it worked (returns 0 if successful)
	if (devStatus == 0)
	{
		// turn on the DMP, now that it's ready
		Serial.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

		// set our DMP Ready flag so the main loop() function knows it's okay to use it
		Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
		bMPU6050Ready = true;
		StartSec = millis() / 1000.f;
		mySerial.printf("MPU6050 Ready at %2.2f Sec\n", StartSec);
	}
	else
	{
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		Serial.print(F("DMP Initialization failed (code "));
		Serial.print(devStatus);
		Serial.println(F(")"));

		bMPU6050Ready = false;
	}
#pragma endregion MPU6050

	mySerial.printf("End of test - Stopping!\n");
	while (true)
	{
		CheckForUserInput();
	}
}

void loop()
{


}

void CheckForUserInput()
{
	const int bufflen = 80;
	char buff[bufflen];
	memset(buff, 0, bufflen);
	float degPerSec;
	float Kp, Ki, Kd;
	const char s[2] = ",";
	char* token;

	if (Serial.available() > 0)
	{
		// read the incoming byte:
		int incomingByte = Serial.read();

		// say what you got:
		Serial.print("I received: ");
		//Serial.println(incomingByte, DEC);
		Serial.println(incomingByte, HEX); //chg to HEX 02/18/20

		//02/18/20 experiment with multiple commands
		switch (incomingByte)
		{
		case 0x50: //ASCII 'P'
		case 0x70: //ASCII 'p'
			StopBothMotors();
			Serial.println(F("Setting Kp value"));

			//consume CR/LF chars
			while (Serial.available() > 0)
			{
				int byte = Serial.read();
				mySerial.printf("consumed %d\n", byte);
			}

			while (!Serial.available())
			{
			}

			Serial.readBytes(buff, bufflen);
			mySerial.printf("%s\n", buff);

			/* extract Kp */
			token = strtok(buff, s);
			Kp = atof(token);

			/* extract Ki */
			token = strtok(NULL, s);
			Ki = atof(token);

			/* extract Kd */
			token = strtok(NULL, s);
			Kd = atof(token);

			/* extract degPerSec */
			token = strtok(NULL, s);
			degPerSec = atof(token);

			mySerial.printf("Kp,Ki,Kd,degPerSec =  %2.2f, %2.2f, %2.2f, %2.2f\n",
				Kp, Ki, Kd, degPerSec);

			PulseSpinTurnForever(true, Kp, Ki, Kd, degPerSec);

			break;
		case 0x51: //ASCII 'Q'
		case 0x71: //ASCII 'q'
			StopBothMotors();
			Serial.println(F("Setting Kp value"));

			//consume CR/LF chars
			while (Serial.available() > 0)
			{
				int byte = Serial.read();
				mySerial.printf("consumed %d\n", byte);
			}

			while (!Serial.available())
			{
			}

			Serial.readBytes(buff, bufflen);
			mySerial.printf("%s\n", buff);

			/* extract Kp */
			token = strtok(buff, s);
			Kp = atof(token);

			/* extract Ki */
			token = strtok(NULL, s);
			Ki = atof(token);

			/* extract Kd */
			token = strtok(NULL, s);
			Kd = atof(token);

			/* extract degPerSec */
			token = strtok(NULL, s);
			degPerSec = atof(token);

			mySerial.printf("Kp,Ki,Kd,degPerSec =  %2.2f, %2.2f, %2.2f, %2.2f\n",
				Kp, Ki, Kd, degPerSec);

			SpinTurnForever(true, Kp, Ki, Kd, degPerSec);

			break;
		Default:
						Serial.println(F("In Default Case: Stopping Motors!"));
						MoveAhead(0, 0);
						while (true)
						{

						}
					}
				}
			}

void PulseSpinTurnForever(bool b_ccw, float Kp, float Ki, float Kd, float degPersec)
{
	float tgt_deg;
	double prev_hdg = 0;

	//DEBUG!!
	mySerial.printf("PulseTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",
		Kp, Ki, Kd, degPersec);
	//DEBUG!!


	mySerial.printf("Msec\tHdg\tDhdg\tRate\tSet\terror\tDerr\tIval\tKp*err\tKi*Ival\tKd*Derr\tOut\n");

	//init some stuff
	TurnRateOutput = 0;
	TurnRateSetPoint_DPS = degPersec;
	Ival = 0; lastErr = 0;
	lastTurnRateVal_DPS;
	TurnRateVal_DPS;

	UpdateIMUHdgValDeg();
	prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR
	MsecSinceLastTurnRateUpdate = 0;

	//05/03/21 - back to the PWM technique
	//Step4: Pulse the motors to get things started
	digitalWrite(CHG_CONNECT_LED_PIN, HIGH);
	SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);
	SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);
	delay(50);
	StopBothMotors();
	digitalWrite(CHG_CONNECT_LED_PIN, LOW);

	while (true)
	{
		//11/06/20 now just loops between ISR hits
		CheckForUserInput();

		//if (bTimeForNavUpdate) //set true in ISR
		//{
		//	bTimeForNavUpdate = false;
		if (MsecSinceLastTurnRateUpdate >= 100)
		{
			MsecSinceLastTurnRateUpdate -= 100;

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;

			//11/14/20 need to handle -179 to +179 transition
			deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

			//05/20/21 gfp can occasionally get one or two bad readings
			while (abs(deltaDeg) > TurnRateSetPoint_DPS)
			{
				mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",
					IMUHdgValDeg, prev_hdg, deltaDeg);

				prev_hdg = IMUHdgValDeg; //05/23/21 bugfix
				CheckForUserInput();

				deltaDeg = IMUHdgValDeg - prev_hdg;

				deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
				deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

				prev_hdg = IMUHdgValDeg;
				UpdateIMUHdgValDeg();
			}

			TurnRateVal_DPS = 10 * abs(deltaDeg); //05/23/21 4wd uses fixed 100mSec TIMER5 interval

			double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;
			double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);
			Ival += (error);
			double dErr = (error - lastErr);

			/*Compute PID Output*/
			TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

			/*Remember some variables for next time*/
			lastErr = error;
			lastTurnRateVal_DPS = TurnRateVal_DPS;

			if (TurnRateOutput > MAX_PULSE_WIDTH_MSEC)
				TurnRateOutput = MAX_PULSE_WIDTH_MSEC;
			else if (TurnRateOutput < MIN_PULSE_WIDTH_MSEC)
				TurnRateOutput = MIN_PULSE_WIDTH_MSEC;

			//05/03/21 - back to the PWM technique
			//Step4: Pulse the motors to full speed for the duration specified by the PID output
			digitalWrite(CHG_CONNECT_LED_PIN, HIGH);
			SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_MAX);
			SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_MAX);
			//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);
			//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);
			//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);
			//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);
			delay(TurnRateOutput);
			StopBothMotors();
			digitalWrite(CHG_CONNECT_LED_PIN, LOW);


			//DEBUG!!
			mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",
				millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,
				Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);
			//DEBUG

			prev_hdg = IMUHdgValDeg;
		}
	}
}

#pragma region MOTOR SUPPORT
//09/08/20 modified for DRV8871 motor driver
void MoveReverse(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move in reverse direction continuously - companion to MoveAhead()
	//ProvEnA_Pinnce: G. Frank Paynter 09/08/18
	//Inputs:  
	//    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
	//    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
	//Outputs: both drive Motors energized with the specified speed
	//Plan:
	//    Step 1: Set reverse direction for both wheels
	//    Step 2: Run both Motors at specified speeds
	//Notes:
	//	01/22/20 now using Adafruit DRV8871 drivers

	//Step 1: Set reverse direction and speed for both wheels
	SetLeftMotorDirAndSpeed(REV_DIR, leftspeednum);
	SetRightMotorDirAndSpeed(REV_DIR, rightspeednum);
}

//09/08/20 modified for DRV8871 motor driver
void MoveAhead(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move ahead continuously
	//ProvEnA_Pinnce: G. Frank Paynter and Danny Frank 01/24/2014
	//Inputs:  
	//    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
	//    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
	//Outputs: both drive Motors energized with the specified speed
	//Plan:
	//    Step 1: Set forward direction for both wheels
	//    Step 2: Run both Motors at specified speeds
	//Notes:
	//	01/22/20 now using Adafruit DRV8871 drivers

	//mySerial.printf("InMoveAhead(%d,%d)\n", leftspeednum, rightspeednum);

	//Step 1: Set forward direction and speed for both wheels
	SetLeftMotorDirAndSpeed(true, leftspeednum);
	SetRightMotorDirAndSpeed(true, rightspeednum);
}

//09/08/10 modified for DRV8871 motor driver
void StopLeftMotors()
{
	analogWrite(In1_Left, MOTOR_SPEED_OFF);
	analogWrite(In2_Left, MOTOR_SPEED_OFF);
}

void StopRightMotors()
{
	analogWrite(In1_Right, MOTOR_SPEED_OFF);
	analogWrite(In2_Right, MOTOR_SPEED_OFF);
}

//09/08/20 added bool bisFwd param for DRV8871 motor driver
void RunBothMotors(bool bisFwd, int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors at left/rightspeednum speeds
	//Inputs:
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//    Step 1: Apply drive to both wheels
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//    01/24/15 - revised for continuous run - no timing
	//  01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()
	//	12/07/15 - chg args from &int to int
	//Step 1: Apply drive to both wheels

//DEBUG!!
	//mySerial.printf("In RunBothMotors(%s, %d,%d)\n", bisFwd? "FWD":"REV", leftspeednum, rightspeednum);
//DEBUG!!

	SetLeftMotorDirAndSpeed(bisFwd, leftspeednum);
	SetRightMotorDirAndSpeed(bisFwd, rightspeednum);
}

void RunBothMotorsBidirectional(int leftspeed, int rightspeed)
{
	//Purpose:  Accommodate the need for independent bidirectional wheel motion
	//Inputs:
	//	leftspeed - integer denoting left wheel speed.  Positive value is fwd, negative is rev
	//	rightspeed - integer denoting right wheel speed.  Positive value is fwd, negative is rev
	//Outputs:
	//	left/right wheel motors direction and speed set as appropriate
	//Plan:
	//	Step1: Set left wheel direction and speed
	//	Step2: Set right wheel direction and speed

//Step1: Set left wheel direction and speed

	//DEBUG!!
	//mySerial.printf("In RunBothMotorsBidirectional(%d, %d)\n", leftspeed, rightspeed);
	if (leftspeed < 0)
	{
		SetLeftMotorDirAndSpeed(false, -leftspeed); //neg value ==> reverse
	}
	else
	{
		SetLeftMotorDirAndSpeed(true, leftspeed); //pos or zero value ==> fwd
	}

	//Step2: Set right wheel direction and speed
	if (rightspeed < 0)
	{
		SetRightMotorDirAndSpeed(false, -rightspeed); //neg value ==> reverse
	}
	else
	{
		SetRightMotorDirAndSpeed(true, rightspeed); //pos or zero value ==> fwd
	}
}

//09/08/20 added bool bisFwd param for DRV8871 motor driver
void RunBothMotorsMsec(bool bisFwd, int timeMsec, int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors for timesec seconds at speednum speed
	//Inputs:
	//    timesec = time in seconds to run the Motors
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//    Step 1: Apply drive to both wheels
	//    Step 2: Delay timsec seconds
	//    Step 3: Remove drive from both wheels.
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//	11/25/15 - rev to use motor driver class object
	//	09/08/20 added bool bisFwd param for DRV8871 motor driver

	RunBothMotors(bisFwd, leftspeednum, rightspeednum);

	//Step 2: Delay timsec seconds
	delay(timeMsec);

	//Step3: Stop motors added 04/12/21
	StopBothMotors();
}

//11/25/15 added for symmetry ;-).
void StopBothMotors()
{
	StopLeftMotors();
	StopRightMotors();
}

void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)
{
	//mySerial.printf("SetLeftMotorDirAndSpeed(%d,%d)\n", bIsFwd, speed);

#ifndef NO_MOTORS
	if (bIsFwd)
	{
		digitalWrite(In1_Left, LOW);
		analogWrite(In2_Left, speed);
		//mySerial.printf("In TRUE block of SetLeftMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
	}
	else
	{
		//mySerial.printf("In FALSE block of SetLeftMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
		digitalWrite(In2_Left, LOW);
		analogWrite(In1_Left, speed);
	}
#endif // !NO_MOTORS
}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)
{
	//mySerial.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",
	//	(bIsFwd == true) ? "true" : "false", speed);

#ifndef NO_MOTORS

	if (bIsFwd)
	{
		//mySerial.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
		digitalWrite(In1_Right, LOW);
		analogWrite(In2_Right, speed);
	}
	else
	{
		//mySerial.printf("In FALSE block of SetRightMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
		digitalWrite(In2_Right, LOW);
		analogWrite(In1_Right, speed);
	}

#endif // !NO_MOTORS
}
#pragma endregion Motor Support Functions

#pragma region MPU5060 Support
float UpdateIMUHdgValDeg()
{
	//Purpose: Get latest yaw (heading) value from IMU
	//Inputs: None.  This function should only be called after mpu.dmpPacketAvailable() returns TRUE
	//Outputs: 
	//	returns true if successful, otherwise false
	//	IMUHdgValDeg updated on success
	//Plan:
	//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet
	//Step2: read all available packets to get to latest data
	//Step3: update IMUHdgValDeg with latest value
	//Notes:
	//	10/08/19 changed return type to boolean
	//	10/08/19 no longer need mpuIntStatus
	//	10/21/19 completely rewritten to use Homer's algorithm
	//	05/05/20 changed return type to float vs bool.

	bool retval = false;

	int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

	if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow
	{
		// display Euler angles in degrees
		mpu.dmpGetQuaternion(&q, fifoBuffer);
		mpu.dmpGetGravity(&gravity, &q);
		mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

		//compute the yaw value
		IMUHdgValDeg = ypr[0] * 180 / M_PI;
		retval = true;
	}

	//return retval;
	return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience
}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)
{
	mpu.resetFIFO();
	delay(1);
	//int countloop = 0;

	fifoCount = mpu.getFIFOCount();
	GetPacketLoopCount = 0;

	//mySerial.printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);
	while (fifoCount < packetSize && GetPacketLoopCount < max_loops)
	{
		GetPacketLoopCount++;
		fifoCount = mpu.getFIFOCount();
		delay(2);
	}

	//mySerial.printf("In GetCurrentFIFOPacket: after loop fifoC = %d, loop count = %d\n", fifoCount, GetPacketLoopCount);

	if (GetPacketLoopCount >= max_loops)
	{
		return 0;
	}

	//if we get to here, there should be exactly one packet in the FIFO
	mpu.getFIFOBytes(data, packetSize);
	return 1;
}
#pragma endregion MPU5060 Support

void SpinTurnForever(bool b_ccw, float Kp, float Ki, float Kd, float degPersec)
{
	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point
	double prev_hdg = 0;
	unsigned long prev_uSec; //added 09/02/20

	//DEBUG!!
	mySerial.printf("SpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",
		Kp, Ki, Kd, degPersec);
	//DEBUG!!



	UpdateIMUHdgValDeg();//get new heading information from the MPU6050
	prev_hdg = IMUHdgValDeg;

	mySerial.printf("Msec\tHdg\tDhdg\tRate\tSet\terror\tDerr\tIval\tKp*err\tKi*Ival\tKd*Derr\tOut\n");

	//init some stuff
	TurnRateOutput = 0;
	TurnRateSetPoint_DPS = degPersec;
	MsecSinceLastTurnRateUpdate = 0;
	Ival = 0; lastErr = 0;


	SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);
	SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

	while (true)
	{
		CheckForUserInput();

		//5/12/21 now time interval is constant at ~100mSec
		if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)
		{
			MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;

			//11/14/20 need to handle -179 to +179 transition
			deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

			//05/20/21 gfp can occasionally get one or two bad readings
			while (abs(deltaDeg) > TurnRateSetPoint_DPS)
			{
				mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",
					IMUHdgValDeg, prev_hdg, deltaDeg);

				UpdateIMUHdgValDeg();//get new heading information from the MPU6050
				deltaDeg = IMUHdgValDeg - prev_hdg;
				prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

				deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
				deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;
			}

			TurnRateVal_DPS = (1000 / TURN_RATE_UPDATE_INTERVAL_MSEC) * abs(deltaDeg);

			double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;
			double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);
			Ival += (error);
			double dErr = (error - lastErr);

			/*Compute PID Output*/
			TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

			/*Remember some variables for next time*/
			lastErr = error;
			lastTurnRateVal_DPS = TurnRateVal_DPS;

			if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)
				TurnRateOutput = MAX_SPIN_MOTOR_SPEED;
			else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)
				TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

			SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);
			SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

			//DEBUG!!
			mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",
				millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,
				Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);
			//DEBUG

			prev_hdg = IMUHdgValDeg;
		}
	}
}

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

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

Name: FourWD_PulseTurnRateTest.ino

Created: 5/24/2021 4:49:52 PM

Author: FRANKNEWXPS15\Frank

#pragma region INCLUDES

#include <elapsedMillis.h>

#include <PrintEx.h> //allows printf-style printout syntax

#include "MPU6050_6Axis_MotionApps_V6_12.h" //changed to this version 10/05/19

#include "I2Cdev.h" //02/19/19: this includes SBWire.h

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

#pragma endregion INCLUDES

#pragma region DEFINES

#define MPU6050_I2C_ADDR 0x69

#pragma endregion DEFINES

#pragma region PRE_SETUP

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

MPU6050 mpu(MPU6050_I2C_ADDR); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0)

#pragma endregion PRE_SETUP

#pragma region MOTOR_PINS

//09/11/20 Now using two Adafruit DRV8871 drivers for all 4 motors

const int In1_Left = 10;

const int In2_Left = 11;

const int In1_Right = 8;

const int In2_Right = 9;

#pragma endregion Motor Pin Assignments

#pragma region MOTOR_PARAMETERS

//drive wheel speed parameters

const int MOTOR_SPEED_FULL = 200; //range is 0-255

const int MOTOR_SPEED_MAX = 255; //range is 0-255

const int MOTOR_SPEED_HALF = 127; //range is 0-255

const int MOTOR_SPEED_QTR = 75; //added 09/25/20

const int MOTOR_SPEED_LOW = 50; //added 01/22/15

const int MOTOR_SPEED_OFF = 0; //range is 0-255

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

bool bIsForwardDir = true; //default is foward direction

#pragma endregion Motor Parameters

#pragma region MPU6050_SUPPORT

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float euler[3]; // [psi, theta, phi] Euler angle container

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

int GetPacketLoopCount = 0;

int OuterGetPacketLoopCount = 0;

//MPU6050 status flags

bool bMPU6050Ready = true;

bool dmpReady = false; // set true if DMP init was successful

volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR

const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param

bool bFirstTime = true;

#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19

#pragma endregion MPU6050 Support

const int CHG_CONNECT_LED_PIN = 50; //12/16/20 added for debug

elapsedMillis MsecSinceLastTurnRateUpdate;

//const int MAX_PULSE_WIDTH_MSEC = 100;

const int MAX_PULSE_WIDTH_MSEC = 50;

const int MIN_PULSE_WIDTH_MSEC = 0;

//const int MAX_SPIN_MOTOR_SPEED = 200;

const int MAX_SPIN_MOTOR_SPEED = 250;

const int MIN_SPIN_MOTOR_SPEED = 50;

const int TURN_RATE_UPDATE_INTERVAL_MSEC = 20;

//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 100;

double TurnRateOutput;

double TurnRateSetPoint_DPS;

double Ival = 0; double lastErr = 0;

double lastTurnRateVal_DPS;

double TurnRateVal_DPS;

void setup()

{

Serial.begin(115200);

#pragma region MPU6050

mySerial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);

mpu.initialize();

// verify connection

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

float StartSec = 0; //used to time MPU6050 init

Serial.println(F("Initializing DMP..."));

devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if successful)

if (devStatus == 0)

{

// turn on the DMP, now that it's ready

Serial.println(F("Enabling DMP..."));

mpu.setDMPEnabled(true);

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

bMPU6050Ready = true;

StartSec = millis() / 1000.f;

mySerial.printf("MPU6050 Ready at %2.2f Sec\n", StartSec);

}

else

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

Serial.print(F("DMP Initialization failed (code "));

Serial.print(devStatus);

Serial.println(F(")"));

bMPU6050Ready = false;

}

#pragma endregion MPU6050

mySerial.printf("End of test - Stopping!\n");

while (true)

{

CheckForUserInput();

}

void loop()

{

}

void CheckForUserInput()

{

const int bufflen = 80;

char buff[bufflen];

memset(buff, 0, bufflen);

float degPerSec;

float Kp, Ki, Kd;

const char s[2] = ",";

char* token;

if (Serial.available() > 0)

{

// read the incoming byte:

int incomingByte = Serial.read();

// say what you got:

Serial.print("I received: ");

//Serial.println(incomingByte, DEC);

Serial.println(incomingByte, HEX); //chg to HEX 02/18/20

//02/18/20 experiment with multiple commands

switch (incomingByte)

{

case 0x50: //ASCII 'P'

case 0x70: //ASCII 'p'

StopBothMotors();

Serial.println(F("Setting Kp value"));

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

mySerial.printf("consumed %d\n", byte);

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

mySerial.printf("%s\n", buff);

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract degPerSec */

token = strtok(NULL, s);

degPerSec = atof(token);

mySerial.printf("Kp,Ki,Kd,degPerSec = %2.2f, %2.2f, %2.2f, %2.2f\n",

Kp, Ki, Kd, degPerSec);

PulseSpinTurnForever(true, Kp, Ki, Kd, degPerSec);

break;

case 0x51: //ASCII 'Q'

case 0x71: //ASCII 'q'

StopBothMotors();

Serial.println(F("Setting Kp value"));

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

mySerial.printf("consumed %d\n", byte);

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

mySerial.printf("%s\n", buff);

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract degPerSec */

token = strtok(NULL, s);

degPerSec = atof(token);

mySerial.printf("Kp,Ki,Kd,degPerSec = %2.2f, %2.2f, %2.2f, %2.2f\n",

Kp, Ki, Kd, degPerSec);

SpinTurnForever(true, Kp, Ki, Kd, degPerSec);

break;

Default:

Serial.println(F("In Default Case: Stopping Motors!"));

MoveAhead(0, 0);

while (true)

{

}

void PulseSpinTurnForever(bool b_ccw, float Kp, float Ki, float Kd, float degPersec)

{

float tgt_deg;

double prev_hdg = 0;

//DEBUG!!

mySerial.printf("PulseTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",

Kp, Ki, Kd, degPersec);

//DEBUG!!

mySerial.printf("Msec\tHdg\tDhdg\tRate\tSet\terror\tDerr\tIval\tKp*err\tKi*Ival\tKd*Derr\tOut\n");

//init some stuff

TurnRateOutput = 0;

TurnRateSetPoint_DPS = degPersec;

Ival = 0; lastErr = 0;

lastTurnRateVal_DPS;

TurnRateVal_DPS;

UpdateIMUHdgValDeg();

prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

MsecSinceLastTurnRateUpdate = 0;

//05/03/21 - back to the PWM technique

//Step4: Pulse the motors to get things started

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

delay(50);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

while (true)

{

//11/06/20 now just loops between ISR hits

CheckForUserInput();

//if (bTimeForNavUpdate) //set true in ISR

//{

// bTimeForNavUpdate = false;

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

//05/20/21 gfp can occasionally get one or two bad readings

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",

IMUHdgValDeg, prev_hdg, deltaDeg);

prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

CheckForUserInput();

deltaDeg = IMUHdgValDeg - prev_hdg;

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

prev_hdg = IMUHdgValDeg;

UpdateIMUHdgValDeg();

}

TurnRateVal_DPS = 10 * abs(deltaDeg); //05/23/21 4wd uses fixed 100mSec TIMER5 interval

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_PULSE_WIDTH_MSEC)

TurnRateOutput = MAX_PULSE_WIDTH_MSEC;

else if (TurnRateOutput < MIN_PULSE_WIDTH_MSEC)

TurnRateOutput = MIN_PULSE_WIDTH_MSEC;

//05/03/21 - back to the PWM technique

//Step4: Pulse the motors to full speed for the duration specified by the PID output

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_MAX);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_MAX);

//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);

//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);

delay(TurnRateOutput);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

//DEBUG!!

mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,

Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);

//DEBUG

prev_hdg = IMUHdgValDeg;

}

#pragma region MOTOR SUPPORT

//09/08/20 modified for DRV8871 motor driver

void MoveReverse(int leftspeednum, int rightspeednum)

{

//Purpose: Move in reverse direction continuously - companion to MoveAhead()

//ProvEnA_Pinnce: G. Frank Paynter 09/08/18

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Plan:

// Step 1: Set reverse direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

// 01/22/20 now using Adafruit DRV8871 drivers

//Step 1: Set reverse direction and speed for both wheels

SetLeftMotorDirAndSpeed(REV_DIR, leftspeednum);

SetRightMotorDirAndSpeed(REV_DIR, rightspeednum);

}

//09/08/20 modified for DRV8871 motor driver

void MoveAhead(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

//ProvEnA_Pinnce: G. Frank Paynter and Danny Frank 01/24/2014

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Plan:

// Step 1: Set forward direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

// 01/22/20 now using Adafruit DRV8871 drivers

//mySerial.printf("InMoveAhead(%d,%d)\n", leftspeednum, rightspeednum);

//Step 1: Set forward direction and speed for both wheels

SetLeftMotorDirAndSpeed(true, leftspeednum);

SetRightMotorDirAndSpeed(true, rightspeednum);

}

//09/08/10 modified for DRV8871 motor driver

void StopLeftMotors()

{

analogWrite(In1_Left, MOTOR_SPEED_OFF);

analogWrite(In2_Left, MOTOR_SPEED_OFF);

}

void StopRightMotors()

{

analogWrite(In1_Right, MOTOR_SPEED_OFF);

analogWrite(In2_Right, MOTOR_SPEED_OFF);

}

//09/08/20 added bool bisFwd param for DRV8871 motor driver

void RunBothMotors(bool bisFwd, int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors at left/rightspeednum speeds

//Inputs:

// speednum = speed value (0 = OFF, 255 = full speed)

//Outputs: Both Motors run for timesec seconds at speednum speed

//Plan:

// Step 1: Apply drive to both wheels

//Notes:

// 01/14/15 - added left/right speed offset for straightness compensation

// 01/22/15 - added code to restrict fast/slow values

// 01/24/15 - revised for continuous run - no timing

// 01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()

// 12/07/15 - chg args from &int to int

//Step 1: Apply drive to both wheels

//DEBUG!!

//mySerial.printf("In RunBothMotors(%s, %d,%d)\n", bisFwd? "FWD":"REV", leftspeednum, rightspeednum);

//DEBUG!!

SetLeftMotorDirAndSpeed(bisFwd, leftspeednum);

SetRightMotorDirAndSpeed(bisFwd, rightspeednum);

}

void RunBothMotorsBidirectional(int leftspeed, int rightspeed)

{

//Purpose: Accommodate the need for independent bidirectional wheel motion

//Inputs:

// leftspeed - integer denoting left wheel speed. Positive value is fwd, negative is rev

// rightspeed - integer denoting right wheel speed. Positive value is fwd, negative is rev

//Outputs:

// left/right wheel motors direction and speed set as appropriate

//Plan:

// Step1: Set left wheel direction and speed

// Step2: Set right wheel direction and speed

//Step1: Set left wheel direction and speed

//DEBUG!!

//mySerial.printf("In RunBothMotorsBidirectional(%d, %d)\n", leftspeed, rightspeed);

if (leftspeed < 0)

{

SetLeftMotorDirAndSpeed(false, -leftspeed); //neg value ==> reverse

}

else

{

SetLeftMotorDirAndSpeed(true, leftspeed); //pos or zero value ==> fwd

}

//Step2: Set right wheel direction and speed

if (rightspeed < 0)

{

SetRightMotorDirAndSpeed(false, -rightspeed); //neg value ==> reverse

}

else

{

SetRightMotorDirAndSpeed(true, rightspeed); //pos or zero value ==> fwd

}

//09/08/20 added bool bisFwd param for DRV8871 motor driver

void RunBothMotorsMsec(bool bisFwd, int timeMsec, int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors for timesec seconds at speednum speed

//Inputs:

// timesec = time in seconds to run the Motors

// speednum = speed value (0 = OFF, 255 = full speed)

//Outputs: Both Motors run for timesec seconds at speednum speed

//Plan:

// Step 1: Apply drive to both wheels

// Step 2: Delay timsec seconds

// Step 3: Remove drive from both wheels.

//Notes:

// 01/14/15 - added left/right speed offset for straightness compensation

// 01/22/15 - added code to restrict fast/slow values

// 11/25/15 - rev to use motor driver class object

// 09/08/20 added bool bisFwd param for DRV8871 motor driver

RunBothMotors(bisFwd, leftspeednum, rightspeednum);

//Step 2: Delay timsec seconds

delay(timeMsec);

//Step3: Stop motors added 04/12/21

StopBothMotors();

}

//11/25/15 added for symmetry ;-).

void StopBothMotors()

{

StopLeftMotors();

StopRightMotors();

}

void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)

{

//mySerial.printf("SetLeftMotorDirAndSpeed(%d,%d)\n", bIsFwd, speed);

#ifndef NO_MOTORS

if (bIsFwd)

{

digitalWrite(In1_Left, LOW);

analogWrite(In2_Left, speed);

//mySerial.printf("In TRUE block of SetLeftMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

}

else

{

//mySerial.printf("In FALSE block of SetLeftMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

digitalWrite(In2_Left, LOW);

analogWrite(In1_Left, speed);

}

#endif // !NO_MOTORS

}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)

{

//mySerial.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

#ifndef NO_MOTORS

if (bIsFwd)

{

//mySerial.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

digitalWrite(In1_Right, LOW);

analogWrite(In2_Right, speed);

}

else

{

//mySerial.printf("In FALSE block of SetRightMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

digitalWrite(In2_Right, LOW);

analogWrite(In1_Right, speed);

}

#endif // !NO_MOTORS

}

#pragma endregion Motor Support Functions

#pragma region MPU5060 Support

float UpdateIMUHdgValDeg()

{

//Purpose: Get latest yaw (heading) value from IMU

//Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE

//Outputs:

// returns true if successful, otherwise false

// IMUHdgValDeg updated on success

//Plan:

//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet

//Step2: read all available packets to get to latest data

//Step3: update IMUHdgValDeg with latest value

//Notes:

// 10/08/19 changed return type to boolean

// 10/08/19 no longer need mpuIntStatus

// 10/21/19 completely rewritten to use Homer's algorithm

// 05/05/20 changed return type to float vs bool.

bool retval = false;

int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow

{

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

IMUHdgValDeg = ypr[0] * 180 / M_PI;

retval = true;

}

//return retval;

return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience

}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)

{

mpu.resetFIFO();

delay(1);

//int countloop = 0;

fifoCount = mpu.getFIFOCount();

GetPacketLoopCount = 0;

//mySerial.printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);

while (fifoCount < packetSize && GetPacketLoopCount < max_loops)

{

GetPacketLoopCount++;

fifoCount = mpu.getFIFOCount();

delay(2);

}

//mySerial.printf("In GetCurrentFIFOPacket: after loop fifoC = %d, loop count = %d\n", fifoCount, GetPacketLoopCount);

if (GetPacketLoopCount >= max_loops)

{

return 0;

}

//if we get to here, there should be exactly one packet in the FIFO

mpu.getFIFOBytes(data, packetSize);

return 1;

}

#pragma endregion MPU5060 Support

void SpinTurnForever(bool b_ccw, float Kp, float Ki, float Kd, float degPersec)

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

double prev_hdg = 0;

unsigned long prev_uSec; //added 09/02/20

//DEBUG!!

mySerial.printf("SpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",

Kp, Ki, Kd, degPersec);

//DEBUG!!

UpdateIMUHdgValDeg();//get new heading information from the MPU6050

prev_hdg = IMUHdgValDeg;

mySerial.printf("Msec\tHdg\tDhdg\tRate\tSet\terror\tDerr\tIval\tKp*err\tKi*Ival\tKd*Derr\tOut\n");

//init some stuff

TurnRateOutput = 0;

TurnRateSetPoint_DPS = degPersec;

MsecSinceLastTurnRateUpdate = 0;

Ival = 0; lastErr = 0;

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

while (true)

{

CheckForUserInput();

//5/12/21 now time interval is constant at ~100mSec

if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

//05/20/21 gfp can occasionally get one or two bad readings

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",

IMUHdgValDeg, prev_hdg, deltaDeg);

UpdateIMUHdgValDeg();//get new heading information from the MPU6050

deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

}

TurnRateVal_DPS = (1000 / TURN_RATE_UPDATE_INTERVAL_MSEC) * abs(deltaDeg);

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)

TurnRateOutput = MAX_SPIN_MOTOR_SPEED;

else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)

TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,

Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);

//DEBUG

prev_hdg = IMUHdgValDeg;

}

This program includes a function called ‘CheckForUserInput()’ that, curiously enough, monitors the serial port for user input, and uses a ‘switch’ statement to execute different commands. One of these commands (‘q’ or ‘Q’) causes ‘SpinTurnForever()’ to execute, which in turn accepts a 4-paremeter input that specifies the three PID parameters plus the desired turn rage, in deg/sec. This routine then starts and manages a CCW turn ‘forever’, in the ‘while()’ block shown below:

while (true)
{
	CheckForUserInput();

	//5/12/21 now time interval is constant at ~100mSec
	if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)
	{
		MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

		UpdateIMUHdgValDeg();
		float deltaDeg = IMUHdgValDeg - prev_hdg;

		//11/14/20 need to handle -179 to +179 transition
		deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
		deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

		//05/20/21 gfp can occasionally get one or two bad readings
		while (abs(deltaDeg) > TurnRateSetPoint_DPS)
		{
			mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",
				IMUHdgValDeg, prev_hdg, deltaDeg);

			UpdateIMUHdgValDeg();//get new heading information from the MPU6050
			deltaDeg = IMUHdgValDeg - prev_hdg;
			prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

			deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;
		}

		TurnRateVal_DPS = (1000 / TURN_RATE_UPDATE_INTERVAL_MSEC) * abs(deltaDeg);

		double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;
		double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);
		Ival += (error);
		double dErr = (error - lastErr);

		/*Compute PID Output*/
		TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

		/*Remember some variables for next time*/
		lastErr = error;
		lastTurnRateVal_DPS = TurnRateVal_DPS;

		if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)
			TurnRateOutput = MAX_SPIN_MOTOR_SPEED;
		else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)
			TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

		SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);
		SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

		//DEBUG!!
		mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",
			millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,
			Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);
		//DEBUG

		prev_hdg = IMUHdgValDeg;
	}
}

while (true)

{

CheckForUserInput();

//5/12/21 now time interval is constant at ~100mSec

if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

//05/20/21 gfp can occasionally get one or two bad readings

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",

IMUHdgValDeg, prev_hdg, deltaDeg);

UpdateIMUHdgValDeg();//get new heading information from the MPU6050

deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

}

TurnRateVal_DPS = (1000 / TURN_RATE_UPDATE_INTERVAL_MSEC) * abs(deltaDeg);

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)

TurnRateOutput = MAX_SPIN_MOTOR_SPEED;

else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)

TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,

Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);

//DEBUG

prev_hdg = IMUHdgValDeg;

}

This routine mimics the PID library computations without suffering from library’s synchronization problems, and also allows me to fully instrument the contribution of each PID term to the output. This program also allows me to vary the computational interval independently of the rest of the program, bounded only by the ability of the MPU6050 to produce reliable readings.

After a number of trials, I started getting some reasonable results on my benchtop (hard surface with a thin electrostatic mat), as shown below:

As can be seen in the above plot, the turn rate is controlled pretty well around the 90 deg/sec turn rate, with an average turn rate of 89.6 deg/sec.

The plot below shows the same parameter set, but run on carpet rather than my bench.

Comparing these two plots it is obvious that a lot more motor current is required to make the robot turn on carpet, due to the much higher sideways friction on the wheels.

The next step was to see if the PID parameters for 90 deg/sec would also handle different turn rates. Here are the plots for 45 deg/sec on my benchtop and on carpet:

And then 30 deg/sec on benchtop and carpet

It is clear from the above plots that the PID values (5,0.8,0.1) do fairly well for the four wheel robot, both on hard surfaces and carpet.

Having this kind of control over turn rate is pretty nice. I might even be able to do turns by setting the turn rate appropriately and just timing the turn, or even vary the turn rate during the turn. For a long turn (say 180 deg) I could do the first 90-120 at 90 deg/sec, and then do the last 90-60 at 30 deg/sec; might make for a much more precise turn.

All of the above tests were done with a 20 mSec time interval, which is 5x smaller than the current 100mSec time interval used for the master timer in Wall-E2. So, my next set of tests will keep the turn rate constant and slowly increase the time interval to see if I can get back to 100 mSec without any major sacrifice in performance.

28 May 2021 Update:

I went back through the tests using a 100 mSec interval instead of 20 mSec, and was gratified to see that there was very little degradation in performance. The turn performance was a bit more ‘jerky’ than with a 20 mSec interval, but still quite acceptable, and very well controlled, both on the benchtop and carpet surfaces – Yay! Here are some plots to show the performance.

31 May 2021 Update:

I made some additional runs on benchtop and carpet, thinking I might be able to reduce the turn-rate oscillations a bit. I found that by reducing the time interval back to 20 mSec and increase the ‘D’ (differential) parameter. After some tweaking back and forth, I wound up with a PID set of (5, 0.8, 3). Using these parameters, I got the following performance plots.

PID = (5,0.8,3), 20mSec interval, 90 deg/sec

As can be seen in the Excel plot and the movie, the turn performance is much smoother – yay!

Stay tuned!

Frank

New Batteries for Wall-E2

Turn Rate PID Tuning, Part II

1 Reply

Posted 14 May 2021,

In my previous post on this topic, I described my efforts to use the Arduino PID library to manage turns with Wall-E2, my autonomous wall following robot. This post talks about a problem I encountered with the PID library when used in a system that uses an external timing source, like the TIMER5 ISR in my system and a PID input that depends on accurate timing, such as my turn-rate input.

In my autonomous wall-following robot project, I use TIMER5 on the Arduino Mega 2560 to generate an interrupt ever 100 mSec, and update all time-sensitive parameters in the ISR. These include results from all seven VL53L0X ToF distance sensors, the front-mounted LIDAR, and heading information from a MP6050 IMU. This simplifies the software immensely, as now the latest information is available throughout the code, and encapsulates all sensor-related calls to a single routine.

In my initial efforts at turn-rate tuning using the Arduino PID library, I computed the turn rate in the ISR by simply using

turn_rate = 10*curr_hdg - prev_hdg //appropriately adjusted for 0-180 transitions, etc.

1	turn_rate = 10*curr_hdg - prev_hdg //appropriately adjusted for 0-180 transitions, etc.

This actually worked because, the ISR frequency and the PID::Compute() frequency were more or less the same. However, since the two time intervals are independent of each other there could be a phase shift, which might drift slowly over time. Also, if either timer interval is changed sometime down the road, the system behavior could change dramatically. I thought I had figured out how to handle this issue by moving the turn-rate computation inside the PID::Compute() function block, as shown below

In a typical PID use case, you see code like the following:

if(myPID.Compute())
{
    //compute the new turn rate
   //use the PID output term to control something
   ...
   ...
}

if(myPID.Compute())

{

//compute the new turn rate

//use the PID output term to control something

...

}

After making the above change, I started getting really weird behavior, and all my efforts at PID tuning failed miserably. After a LOT of troubleshooting and head-scratching, I finally figured out what was happening. In the above code configuration, the PID generates a new output value BEFORE the new turn rate is computed, so the PID is always operating on information that is at least 100mSec old – not a good way to run a railroad!

Some of the PID documentation I researched said (or at least implied) that by setting the PID’s sample time to zero using PID::SetSampleTime(0), that Compute() would actually produce a new output value every time it was called. This meant that I could do something like the following:

if (bTimeForNavUpdate) //set true in ISR
{
	bTimeForNavUpdate = false;

	//4/28/21 now time interval is constant at ~100mSec
	//11/14/20 need to handle -179 to +179 transition
	float deltaDeg = IMUHdgValDeg - prev_hdg;
	deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;
	deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;
	TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

	TurnRatePID.Compute();//04/10/21 SampleTime == 0 so now this updates every time

	SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);
	SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);
	prev_hdg = IMUHdgValDeg;
}

if (bTimeForNavUpdate) //set true in ISR

{

bTimeForNavUpdate = false;

//4/28/21 now time interval is constant at ~100mSec

//11/14/20 need to handle -179 to +179 transition

float deltaDeg = IMUHdgValDeg - prev_hdg;

deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;

TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

TurnRatePID.Compute();//04/10/21 SampleTime == 0 so now this updates every time

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);

prev_hdg = IMUHdgValDeg;

}

Great idea, but it didn’t work! After some more troubleshooting and head-scratching, I finally realized that the PID::SetSampleTime() function specifically disallows a value of zero, as it would cause the ‘D’ term to go to infinity – oops! Here’s the relevant code

void PID::SetSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;
   }
}

void PID::SetSampleTime(int NewSampleTime)

{

if (NewSampleTime > 0)

{

double ratio = (double)NewSampleTime

/ (double)SampleTime;

ki *= ratio;

kd /= ratio;

SampleTime = (unsigned long)NewSampleTime;

}

As can be seen from the above, an argument of zero is simply ignored, and the sample time remains unchanged. When I pointed this out to the developer, he said this was by design, as the ‘ratio’ calculation above would be undefined for an input argument of zero. This is certainly a valid point, but makes it impossible to synch the PID to an external master clock – bummer!

After some more thought, I modified my copy of PID.cpp as follows:

void PID::SetSampleTime(int NewSampleTime)
{
  Serial.println("In PID::SetSampleTime with NewSampleTime = "); Serial.println(NewSampleTime);
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      //SampleTime = (unsigned long)NewSampleTime;
   }

    SampleTime = (unsigned long)NewSampleTime;
}

void PID::SetSampleTime(int NewSampleTime)

{

Serial.println("In PID::SetSampleTime with NewSampleTime = "); Serial.println(NewSampleTime);

if (NewSampleTime > 0)

{

double ratio = (double)NewSampleTime

/ (double)SampleTime;

ki *= ratio;

kd /= ratio;

//SampleTime = (unsigned long)NewSampleTime;

}

SampleTime = (unsigned long)NewSampleTime;

}

By moving the SampleTime = (unsigned long)NewSampleTime; line out of the ‘if’ block, I can now set the sample time to zero without causing problems with the value of ‘ratio’. Now PID::Compute() will generate a new output value every time it is called, which synchs the PID engine with the program’s master timing source – yay!

I tried out a slightly modified version of this technique on my small 2-wheel robot. The two-wheeler uses an Arduino Uno instead of a Mega, so I didn’t use a TIMER interrupt. Instead I used the ‘elapsedMillisecond’ library and set up an elapsed time of 100 mSec, and also modified the program to turn indefinitely at the desired turn rate in deg/sec.

I experimented with two different methods for controlling the turn rate – a ‘PWM’ method where the wheel motors are pulsed at full speed for a variable pulse width, and a ‘direct’ method where the wheel motor speeds are varied directly to achieve the desired turn rate. I thought the PWM method might work better on a heavier robot for smaller angle turns as there is quite a bit of inertia to overcome, but the ‘direct’ method might be more accurate.

Here’s the code for the ‘direct’ method, where the wheel speeds are varied with

SetLeftMotorDirAndSpeed

1	SetLeftMotorDirAndSpeed

void SpinTurnForever(bool b_ccw, float kp, float ki, float kd, float degPersec)
{
	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point
	double prev_hdg = 0;
	unsigned long prev_uSec; //added 09/02/20

	//DEBUG!!
	mySerial.printf("SpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",
		kp, ki, kd, degPersec);
	//DEBUG!!

//Step3: 11/14/20 activate turn rate PID
	TurnRateOutput = 0;
	TurnRatePID.SetMode(AUTOMATIC);
	TurnRatePID.SetTunings(kp, ki, kd);
	TurnRateSetPoint = degPersec;

	//set the limits
	//TurnRatePID.SetOutputLimits(10, 100);
	TurnRatePID.SetOutputLimits(MOTOR_SPEED_LOW, MOTOR_SPEED_MAX);
	TurnRatePID.SetSampleTime(0); //Compute() runs every time it is called

	UpdateIMUHdgValDeg();
	prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

	mySerial.printf("Msec\tHdg\tPrevHdg\tRate\tSet\tOut\n");

	MsecSinceLastTurnRateUpdate = 0;

	while (true)
	{
		CheckForUserInput();

		//5/12/21 now time interval is constant at ~100mSec
		if (MsecSinceLastTurnRateUpdate >= 100)
		{
			MsecSinceLastTurnRateUpdate -= 100;

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;
			prev_hdg = IMUHdgValDeg;

			//11/14/20 need to handle -179 to +179 transition
			deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;
			TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

			TurnRatePID.Compute();  //this is where everything is computed

			SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);
			SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

			//DEBUG!!
			mySerial.printf("%lu\t%4.2f\t%4.2f\t%2.1f\t%2.1f\t%2.1f\n",
				millis(), IMUHdgValDeg, prev_hdg, TurnRateVal, degPersec, TurnRateOutput);
			//DEBUG

			prev_hdg = IMUHdgValDeg;
		}
	}
}

void SpinTurnForever(bool b_ccw, float kp, float ki, float kd, float degPersec)

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

double prev_hdg = 0;

unsigned long prev_uSec; //added 09/02/20

//DEBUG!!

mySerial.printf("SpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",

kp, ki, kd, degPersec);

//DEBUG!!

//Step3: 11/14/20 activate turn rate PID

TurnRateOutput = 0;

TurnRatePID.SetMode(AUTOMATIC);

TurnRatePID.SetTunings(kp, ki, kd);

TurnRateSetPoint = degPersec;

//set the limits

//TurnRatePID.SetOutputLimits(10, 100);

TurnRatePID.SetOutputLimits(MOTOR_SPEED_LOW, MOTOR_SPEED_MAX);

TurnRatePID.SetSampleTime(0); //Compute() runs every time it is called

UpdateIMUHdgValDeg();

prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

mySerial.printf("Msec\tHdg\tPrevHdg\tRate\tSet\tOut\n");

MsecSinceLastTurnRateUpdate = 0;

while (true)

{

CheckForUserInput();

//5/12/21 now time interval is constant at ~100mSec

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;

TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

TurnRatePID.Compute(); //this is where everything is computed

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

mySerial.printf("%lu\t%4.2f\t%4.2f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, prev_hdg, TurnRateVal, degPersec, TurnRateOutput);

//DEBUG

prev_hdg = IMUHdgValDeg;

}

Here’s the code for the PWM method: the only difference is that is the duration of the pulse that is varied, not the wheel speed.

void PulseSpinTurnForever(bool b_ccw, float kp, float ki, float kd, float degPersec)
{
	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point
	double prev_hdg = 0;
	unsigned long prev_uSec; //added 09/02/20

	//DEBUG!!
	mySerial.printf("PulseSpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",
		kp, ki, kd, degPersec);
	//DEBUG!!

//Step3: 11/14/20 activate turn rate PID
	TurnRateOutput = 0;
	TurnRatePID.SetMode(AUTOMATIC);
	TurnRatePID.SetTunings(kp, ki, kd);
	TurnRateSetPoint = degPersec;

	//set the limits
	TurnRatePID.SetOutputLimits(10, 100);
	TurnRatePID.SetSampleTime(0); //Compute() runs every time it is called

	UpdateIMUHdgValDeg();
	prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

	mySerial.printf("Msec\tHdg\tPrevHdg\tRate\tSet\tOut\n");
	uint32_t prevUsec = micros();//initialize
	uint32_t nowUsec = prevUsec;

	MsecSinceLastTurnRateUpdate = 0;

	while (true)
	{
		CheckForUserInput();

		//5/12/21 now time interval is constant at ~100mSec
		if (MsecSinceLastTurnRateUpdate >= 100)
		{
			MsecSinceLastTurnRateUpdate -= 100;

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;
			prev_hdg = IMUHdgValDeg;

			//11/14/20 need to handle -179 to +179 transition
			deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;
			TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

			TurnRatePID.Compute();  //this is where everything is computed

			//05/03/21 - back to the PWM technique
		//Step4: Pulse the motors to full speed for the duration specified by the PID output
			digitalWrite(CHG_CONNECT_LED_PIN, HIGH);
			SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);
			SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);
			delay(TurnRateOutput);
			StopBothMotors();
			digitalWrite(CHG_CONNECT_LED_PIN, LOW);

			//DEBUG!!
			mySerial.printf("%lu\t%4.2f\t%4.2f\t%2.1f\t%2.1f\t%2.1f\n",
				millis(), IMUHdgValDeg, prev_hdg, TurnRateVal, degPersec, TurnRateOutput);
			//DEBUG

			StopBothMotors();

			prev_hdg = IMUHdgValDeg;
		}
	}
}

void PulseSpinTurnForever(bool b_ccw, float kp, float ki, float kd, float degPersec)

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

double prev_hdg = 0;

unsigned long prev_uSec; //added 09/02/20

//DEBUG!!

mySerial.printf("PulseSpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",

kp, ki, kd, degPersec);

//DEBUG!!

//Step3: 11/14/20 activate turn rate PID

TurnRateOutput = 0;

TurnRatePID.SetMode(AUTOMATIC);

TurnRatePID.SetTunings(kp, ki, kd);

TurnRateSetPoint = degPersec;

//set the limits

TurnRatePID.SetOutputLimits(10, 100);

TurnRatePID.SetSampleTime(0); //Compute() runs every time it is called

UpdateIMUHdgValDeg();

prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

mySerial.printf("Msec\tHdg\tPrevHdg\tRate\tSet\tOut\n");

uint32_t prevUsec = micros();//initialize

uint32_t nowUsec = prevUsec;

MsecSinceLastTurnRateUpdate = 0;

while (true)

{

CheckForUserInput();

//5/12/21 now time interval is constant at ~100mSec

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;

TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

TurnRatePID.Compute(); //this is where everything is computed

//05/03/21 - back to the PWM technique

//Step4: Pulse the motors to full speed for the duration specified by the PID output

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);

delay(TurnRateOutput);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

//DEBUG!!

mySerial.printf("%lu\t%4.2f\t%4.2f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, prev_hdg, TurnRateVal, degPersec, TurnRateOutput);

//DEBUG

StopBothMotors();

prev_hdg = IMUHdgValDeg;

}

Here’s a short video showing the two-wheel robot doing a spin turn using the PWM technique with a desired turn rate of 90 deg/sec, using PID = (1,0.5,0).

The average turn rate for the entire run was about 85 deg/sec.

Here’s another run, this time on carpet:

Average turn rate for the entire run was about 85 deg/sec

Here’s some data from the ‘direct’ method, on hard flooring

And on carpet

So, it appears that either the PWM or ‘direct’ methods are effective in controlling the turn rate, and I don’t really see any huge difference between them. I guess the PWM method might be a little more effective with the 4-wheel robot caused by the wheels having to slide sideways while turning.

Stay Tuned!

Frank

Wall Parallel Find PID Tuning

1 Reply

Posted 10 April 2021

In addition to using PID for homing to its charging station and for turn rate control, Wall-E2 also uses PID for finding the parallel orientation to a nearby wall. After successfully tuning the turn rate and IR Homing PID controllers using the Ziegler-Nichols method for PID tuning, I decided to see what I could do with the PID controller for parallel orientation finding

Wall-E2 uses two 3-element VL53L0X Time-of-Flight distance sensors for parallel orientation finding. The idea is that when all three sensors report the same distance, then the robot must be oriented parallel to the wall. The Teensy 3.5 Array Controller MCU calculates a ‘steering value’ using the expression (shown for the left side array):

  LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

1	LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

This value is fed to the PID engine, which drives the motors to zero it out – thus arriving at a parallel orientation. Originally I just basically ‘winged it’ in choosing PID Kp, Ki & Kd values, arriving emperically at Kp = 200, Ki = 50, Kd = 0. However, after going through the K-N process with Wall-E2’s other two PID control setups, I decided to try it with this one as well.

The first step is to determine Kc, the Kp value for which the system oscillates in a reasonably stable fashion. To accomplish this I started with Kp = 20 and worked my way up in stages, plotting the ‘steering value’ each time. The last three trials (as shown in the following plots) were for Kp = 400, Kp = 500 and Kp = 600:

Looking at the above plots, it looks like Kp = 600 will work for Kc. Using the K-N formula, we get

Kp = 0.5Kc = 300
Ki = 0.45Kc = 270
Kd = 0.6Kc = 360

Kp = 0.5Kc = 300

Ki = 0.45Kc = 270

Kd = 0.6Kc = 360

Using the above values for the Parallel Find PID, we get the following plot:

Which is not exactly what I thought it would be – it looks like my guess for Kc must be off. Trying again with a Kc = 400 –> PID = 200,180,240, we get:

which, to my eye at least, seems a bit better.

To test how this worked with ‘real’ parallel finding, I incorporated these parameter values into my ‘RotateToParallelOrientation()’ routine and ran a couple of tests. Here’s one where Wall-E2 starts in the ‘toed-out’ position:

And here’s the Excel plot from this same run

As can be seen, the robot takes less than two seconds to converge on a pretty decent parallel orientation, starting from a 30-40º angle to the near wall.

Here’s another run where the robot starts in the ‘toed-in’ orientation.

And here’s the Excel plot for the run

Again, the robot gets to a pretty decent parallel orientation within 2 seconds of the start of the run. The only concern I have with this run is that it winds up pretty close to the wall.

Serial Bridge over WiFi using ESP32 DevKitC

3 Replies

Posted 09 April 2021

I have been wanting to upgrade my robot’s brains from the old Arduino Mega 2560 to a more modern Teensy 3.x or 4.x MCU for some time now, but have been stymied by the lack of Over-The-Air (OTA) programming support. My current Mega-based setup uses a pair of Pololu Wixels to form a transparent wireless bi-directional serial link between my PC and the robot. My Microsoft Visual Studio IDE sees the link as just another COM port, and the Mega thinks it’s Tx/Rx0 lines are connected directly to the PC; couldn’t be simpler and more effective. The range of the Wixel connection is on the order of a few tens of meters, but that’s all I need for managing my autonomous wall-following robot.

Unfortunately, the Teensy world seems a bit short on effective wireless OTA solutions, or at least my searches have come up mostly dry. Some users report they have been able to use a Raspberry Pi Zero-W for this purpose, but that seems like a major overkill

After yet another Google search, I started seeing some posts about the ability to form a wireless serial data bridge using an ESP32 wireless-enabled development module, and I since I happened to have a ESP32 DevKitC hanging around Idecided to try my hand at that, maybe as a first step to achieving OTA nirvana with a Teensy 3/4.x.

The starting point for this project was this tutorial (the bottom ‘Serial Bridge Using ESP8266 (Simpler)’ part), based on this module sold on the AliExpress site. The AliExpress module wasn’t exactly the same as the DevKitC I had on hand, but it was close enough and AFAICT the pinouts are identical.

The first step was getting the Arduino/VS2019 IDE to recognize the ESP32 hardware. This was a semi-major PITA, but I eventually found some posts showing how the board information could be added to the system.

The next step was getting PuTTY downloaded and installed on my Win10 system. This went fairly easily.

Next I downloaded the ESP32 serial bridge software from the GitHub site and set it up as a project in my VS2019/Visual Micro IDE. This all seemed to work, and I was able to upload the program to my DevKitC module

I also happened to have an old CKdevices FTDI module, so I was able to connect to the required serial lines on the ESP32 module. However, I discovered that the tutorial didn’t use the standard Tx/Rx lines shown in the DevKitC pinout diagram; evidently they had to be moved to allow all three UART/USB modules to be connected at the same time. The actual pinouts used by the tutorial are:

here is the wiring diagram recomendation: https://raw.githubusercontent.com/AlphaLima/ESP32-Serial-Bridge/master/ESP32-SerialBridge.jpg
Pinning
COM0 Rx <-> GPIO21
COM0 Tx <-> GPIO01
COM1 Rx <-> GPIO16
COM1 Tx <-> GPIO17
COM2 Rx <-> GPIO15
COM2 Tx <-> GPIO04

NOTE: The PIN assignment has changed and may not look straigt forward (other PINs are marke as Rx/Tx), but this assignment allows to flash via USB also with hooked MAX3232 serial drivers.

here is the wiring diagram recomendation: https://raw.githubusercontent.com/AlphaLima/ESP32-Serial-Bridge/master/ESP32-SerialBridge.jpg

Pinning

COM0 Rx <-> GPIO21

COM0 Tx <-> GPIO01

COM1 Rx <-> GPIO16

COM1 Tx <-> GPIO17

COM2 Rx <-> GPIO15

COM2 Tx <-> GPIO04

NOTE: The PIN assignment has changed and may not look straigt forward (other PINs are marke as Rx/Tx), but this assignment allows to flash via USB also with hooked MAX3232 serial drivers.

After some fumbling around, I finally realized that the ‘GPIOxx’ labels in the above pinout list is the same as the numbers printed on the actual module, i.e. ‘GPIO21′ <==> ’21’ on the module PCB (the lone exception to this is ‘GPIO1’, which corresponds to ‘TX’ on the module). Here’s a picture from the GitHub documentation showing the actual layout expected by the sofware.

After working my way though these problems, here’s the physical setup I wound up with:

CkDevices FTDI UART/USB connected to COM5 on USB side, Tx2/Rx2 on ESP32 side

Detail showing the pin connections to ESP32 module. Yellow is connected to ESP32 Rx2, Green to Tx2

With the above hardware setup, I was able to pass serial data from one PuTTY terminal connected to the 192.168.4.1/8882 port, and another connected to COM5 on the PC (corresponding to COM2 on the ESP32).

Having accomplished this, I’m still unsure how to go about using this capability to program a Teensy 3/4.x using the wireless link. More study required!

Frank