Tag Archives: i2c_t3.h

Wall-E3 Replacing Mega 2560 With Teensy 3.5 Part V

Posted 15 January 2022,

In my last post on this subject I described my effort to get IR Homing functional on my new Wall-E3 robot. This post is intended to document the process of getting the ‘second deck’ from Wall-E2 ported over to Wall-E3. The second deck from Wall-E2 houses the forward-looking PulsedLight (acquired by Garmin in 2015) LIDAR system, the two side-looking VL53L0X arrays, and a rear-looking VL53L0X, as shown in the photo below

The second deck connects to the main system via the 16-pin Amp connector visible in the bottom left-hand corner of the photo above.

Looking at the code, the only pin assignments associated with ‘second deck’ functionality are:

const int RED_LASER_DIODE_PIN = 5;//Laser pointer
const int LIDAR_MODE_PIN = 2; //LIDAR MODE pin (continuous mode)
const int VL53L0X_TEENSY_RESET_PIN = 4; //pulled low for 1 mSec in Setup()

To start the process, I ported the following sections from From FourWD_WAllE2_V12.ino::setup() to T35_WallE3::setup():

The code to reset the VL53L0X Teensy on the second deck
The code to initialize the above output pins.
The entire #pragma region VL53L0X_TEENSY section
The entire #ifdef DISTANCES_ONLY section
pragma region L/R/FRONT DISTANCE ARRAYS

That should take care of all new declarations and initializations associated with the second deck. And wonder of wonders, the T35_WallE3_V5 project compiled without errors! – time to quit for the night! 🙂

16 January 2022:

Well, of course when I connected up the second deck – nothing worked, so back to basic troubleshooting. First, I connected a USB cable directly to the T3.5 running the VL53L0X array, and determined that I can, in fact, see valid distance values from all seven sensors – yay! Now to determine why I can’t see them from the main processor.

Next, I loaded a basic I2C scanner program onto the main processor to see if the main processor could see the VL53L0X process on Wire1. The I2C scanner reported it could find the MPU6050 IMU module and the Teensy 3.2 IR homing beacon detection processor, but nothing else. After a few more seconds (and yet another face palm!) I realized that the I2C scanner program wasn’t finding the VL53L0X processor because it was only checking the Wire bus, not Wire1 or Wire2 – oops!

So, I modified the basic scanner so it would optionally check Wire1 & Wire2 in addition to Wire1, as shown below:

/*
    Name:       T35_BasicI2CScanner.ino
    Created:	1/6/2022 12:11:46 PM
    Author:     NEWXPS15\paynt

    01/16/22 Added code to scan all three I2C busses
*/

// -------------------------------------------------------------------------------------------
// I2C Bus Scanner
// -------------------------------------------------------------------------------------------
//
// This creates an I2C master device which will scan the address space and report all
// devices which ACK.  It does not attempt to transfer data, it only reports which devices
// ACK their address.
//
// Pull the control pin low to initiate the scan.  Result will output to Serial.
//
// This example code is in the public domain.
// -------------------------------------------------------------------------------------------

#include <i2c_t3.h>

#define WIRE1_AVAILABLE
#define WIRE2_AVAILABLE

uint8_t found, target, all;

void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);    // LED
  pinMode(12, INPUT_PULLUP);       // pull pin 12 low to show ACK only results
  pinMode(11, INPUT_PULLUP);       // pull pin 11 low for a more verbose result (shows both ACK and NACK)

  // Setup for Master mode, pins 18/19, external pullups, 400kHz, 10ms default timeout
  Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);  //using internal pullups also works!
  Wire.setDefaultTimeout(10000); // 10ms

#ifdef WIRE1_AVAILABLE
  Wire1.begin(I2C_MASTER, 0x00, I2C_PINS_37_38 , I2C_PULLUP_INT, 400000);  //using internal pullups also works!
  Wire1.setDefaultTimeout(10000); // 10ms
#endif

#ifdef WIRE2_AVAILABLE
  Wire2.begin(I2C_MASTER, 0x00, I2C_PINS_3_4 , I2C_PULLUP_INT, 400000);  //using internal pullups also works!
  Wire2.setDefaultTimeout(10000); // 10ms
#endif


  Serial.begin(115200);
  delay(2000);
  Serial.printf("Teensy Basic I2C Scanner\npull pin 12 low to show ACK only results\npull pin 11 low for a more verbose result (shows both ACK and NACK)\n");
}

void loop()
{
  // Scan I2C addresses
  //
  if (digitalRead(12) == LOW || digitalRead(11) == LOW)
  {
    all = (digitalRead(11) == LOW);
    found = 0;

    Serial.print("---------------------------------------------------\n");
    Serial.print("Starting scan on Wire bus...\n");
    digitalWrite(LED_BUILTIN, HIGH); // LED on
    for (target = 1; target <= 0x7F; target++) // sweep addr, skip general call
    {
      Wire.beginTransmission(target);       // slave addr
      Wire.endTransmission();               // no data, just addr
      //print_scan_status(target, all);
      print_scan_status(Wire, target, all);
    }
    digitalWrite(LED_BUILTIN, LOW); // LED off

    if (!found) Serial.print("No devices found.\n");

    delay(500); // delay to space out tests

#ifdef WIRE1_AVAILABLE
    Serial.print("---------------------------------------------------\n");
    Serial.print("Starting scan on Wire1 bus...\n");
    found = 0;
    digitalWrite(LED_BUILTIN, HIGH); // LED on
    for (target = 1; target <= 0x7F; target++) // sweep addr, skip general call
    {
      Wire1.beginTransmission(target);       // slave addr
      Wire1.endTransmission();               // no data, just addr
      //print_scan_status(target, all);
      print_scan_status(Wire1, target, all);
    }
    digitalWrite(LED_BUILTIN, LOW); // LED off

    if (!found) Serial.print("No devices found.\n");
#endif

#ifdef WIRE2_AVAILABLE
    Serial.print("---------------------------------------------------\n");
    Serial.print("Starting scan on Wire2 bus...\n");
    found = 0;
    digitalWrite(LED_BUILTIN, HIGH); // LED on
    for (target = 1; target <= 0x7F; target++) // sweep addr, skip general call
    {
      Wire2.beginTransmission(target);       // slave addr
      Wire2.endTransmission();               // no data, just addr
      //print_scan_status(target, all);
      print_scan_status(Wire2, target, all);
    }
    digitalWrite(LED_BUILTIN, LOW); // LED off

    if (!found) Serial.print("No devices found.\n");
#endif
    delay(500); // delay to space out tests
  }
}

//01/16/22 rev to take wire object ref as an argument
void print_scan_status(i2c_t3& wire, uint8_t target, uint8_t all)
{
  //switch (Wire.status())
  switch (wire.status())
  {
  case I2C_WAITING:
    Serial.printf("Addr: 0x%02X ACK\n", target);
    found = 1;
    break;
  case I2C_ADDR_NAK:
    if (all) Serial.printf("Addr: 0x%02X\n", target);
    break;
  default:
    break;
  }
}

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

Name: T35_BasicI2CScanner.ino

Created: 1/6/2022 12:11:46 PM

Author: NEWXPS15\paynt

01/16/22 Added code to scan all three I2C busses

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

// I2C Bus Scanner

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

// This creates an I2C master device which will scan the address space and report all

// devices which ACK. It does not attempt to transfer data, it only reports which devices

// ACK their address.

// Pull the control pin low to initiate the scan. Result will output to Serial.

// This example code is in the public domain.

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

#include <i2c_t3.h>

#define WIRE1_AVAILABLE

#define WIRE2_AVAILABLE

uint8_t found, target, all;

void setup()

{

pinMode(LED_BUILTIN, OUTPUT); // LED

pinMode(12, INPUT_PULLUP); // pull pin 12 low to show ACK only results

pinMode(11, INPUT_PULLUP); // pull pin 11 low for a more verbose result (shows both ACK and NACK)

// Setup for Master mode, pins 18/19, external pullups, 400kHz, 10ms default timeout

Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000); //using internal pullups also works!

Wire.setDefaultTimeout(10000); // 10ms

#ifdef WIRE1_AVAILABLE

Wire1.begin(I2C_MASTER, 0x00, I2C_PINS_37_38 , I2C_PULLUP_INT, 400000); //using internal pullups also works!

Wire1.setDefaultTimeout(10000); // 10ms

#endif

#ifdef WIRE2_AVAILABLE

Wire2.begin(I2C_MASTER, 0x00, I2C_PINS_3_4 , I2C_PULLUP_INT, 400000); //using internal pullups also works!

Wire2.setDefaultTimeout(10000); // 10ms

#endif

Serial.begin(115200);

delay(2000);

Serial.printf("Teensy Basic I2C Scanner\npull pin 12 low to show ACK only results\npull pin 11 low for a more verbose result (shows both ACK and NACK)\n");

}

void loop()

{

// Scan I2C addresses

if (digitalRead(12) == LOW || digitalRead(11) == LOW)

{

all = (digitalRead(11) == LOW);

found = 0;

Serial.print("---------------------------------------------------\n");

Serial.print("Starting scan on Wire bus...\n");

digitalWrite(LED_BUILTIN, HIGH); // LED on

for (target = 1; target <= 0x7F; target++) // sweep addr, skip general call

{

Wire.beginTransmission(target); // slave addr

Wire.endTransmission(); // no data, just addr

//print_scan_status(target, all);

print_scan_status(Wire, target, all);

}

digitalWrite(LED_BUILTIN, LOW); // LED off

if (!found) Serial.print("No devices found.\n");

delay(500); // delay to space out tests

#ifdef WIRE1_AVAILABLE

Serial.print("---------------------------------------------------\n");

Serial.print("Starting scan on Wire1 bus...\n");

found = 0;

digitalWrite(LED_BUILTIN, HIGH); // LED on

for (target = 1; target <= 0x7F; target++) // sweep addr, skip general call

{

Wire1.beginTransmission(target); // slave addr

Wire1.endTransmission(); // no data, just addr

//print_scan_status(target, all);

print_scan_status(Wire1, target, all);

}

digitalWrite(LED_BUILTIN, LOW); // LED off

if (!found) Serial.print("No devices found.\n");

#endif

#ifdef WIRE2_AVAILABLE

Serial.print("---------------------------------------------------\n");

Serial.print("Starting scan on Wire2 bus...\n");

found = 0;

digitalWrite(LED_BUILTIN, HIGH); // LED on

for (target = 1; target <= 0x7F; target++) // sweep addr, skip general call

{

Wire2.beginTransmission(target); // slave addr

Wire2.endTransmission(); // no data, just addr

//print_scan_status(target, all);

print_scan_status(Wire2, target, all);

}

digitalWrite(LED_BUILTIN, LOW); // LED off

if (!found) Serial.print("No devices found.\n");

#endif

delay(500); // delay to space out tests

}

//01/16/22 rev to take wire object ref as an argument

void print_scan_status(i2c_t3& wire, uint8_t target, uint8_t all)

{

//switch (Wire.status())

switch (wire.status())

{

case I2C_WAITING:

Serial.printf("Addr: 0x%02X ACK\n", target);

found = 1;

break;

case I2C_ADDR_NAK:

if (all) Serial.printf("Addr: 0x%02X\n", target);

break;

default:

break;

}

And here’s the output with all three I2C busses enabled.

Opening port
Port open
Teensy Basic I2C Scanner
pull pin 12 low to show ACK only results
pull pin 11 low for a more verbose result (shows both ACK and NACK)
---------------------------------------------------
Starting scan on Wire bus...
Addr: 0x08 ACK   <<<< IR Homing Teensy
Addr: 0x68 ACK   <<<< MPU6050 IMU
---------------------------------------------------
Starting scan on Wire1 bus...
Addr: 0x20 ACK   <<<< VL53L0X Array Teensy
---------------------------------------------------
Starting scan on Wire2 bus...
No devices found.

Opening port

Port open

Teensy Basic I2C Scanner

pull pin 12 low to show ACK only results

pull pin 11 low for a more verbose result (shows both ACK and NACK)

---------------------------------------------------

Starting scan on Wire bus...

Addr: 0x08 ACK <<<< IR Homing Teensy

Addr: 0x68 ACK <<<< MPU6050 IMU

---------------------------------------------------

Starting scan on Wire1 bus...

Addr: 0x20 ACK <<<< VL53L0X Array Teensy

---------------------------------------------------

Starting scan on Wire2 bus...

No devices found.

So now that I know that the main processor can ‘see’ the VL53L0X Teensy on Wire1, I have to figure out why it’s not working properly. As usual, this turned out to be pretty simple once I knew what I was looking for. The entire solution was to change this line:

Wire.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

1	Wire.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

To this line:

Wire1.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

1	Wire1.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

Wall-E2 used a Mega 2560 processor with only one I2C bus, so everything had to be on that one bus. However, when I changed to the Teensy 3.5, I had more busses available, so I chose to move the VL53L0X array manager to Wire1, but forgot to change the initialization code to use Wire1 vs Wire – oops!

18 January 2022 Update:

Or,….. Maybe not. When I tried to compile the above changes I started running into ‘#include file hell’. I couldn’t figure out whether to use #include <i2c_t3.h> or #include <Wire.h> and every time I changed the include in one file, it seemed to conflict with an earlier change in another – argggghhhhh!

So, I took my troubles to the Teensy forum and asked what the difference was between the ‘i2c_t3.h’ and ‘Wire’ libraries, particularly with respect to multiple I2C bus support. The answer I got from ‘defragster’ (a very experienced Teensy forum contributor) was:

Wire.h is the base i2c supplied and supported by PJRC. It covers all WIRE#’s on various Teensy models: See {local install}\hardware\teensy\avr\libraries\Wire\WireKinetis.h
i2c_t3.h is an alternative library that can be used to instead of WIRE.h when it works or offers some added feature or alternate method.

I took his post to say “use Wire.h” unless you have some specific reason why the i2c_t3.h library offers a needed feature that Wire.h doesn’t offer.

So, now I have to go back through all the code that I have dicked with over the years to make work (like ‘I2C_Anything’, for instance) with multiple I2C buses, and see what needs to be done to, as much as possible, use ‘Wire.h’ in lieu of ‘i2c_t3.h’

The main file for this project has the following #include’s that may require work:

#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
#include "I2C_Anything1.h" //01/16/22 added for Wire1 bus

#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

#include "I2C_Anything1.h" //01/16/22 added for Wire1 bus

MPU6050_6Axis_MotionApps_V6_12.h: Right away I run into problems; the first thing this file does is #include “I2Cdev.h”, which has the following code:

#ifdef ARDUINO
    #if ARDUINO < 100
        #include "WProgram.h"
    #else
        #include "Arduino.h"
    #endif
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        //#include <Wire.h>
        #include <i2c_t3.h>
#endif
    #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
        #include <I2C.h>
    #endif
	#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
		#include "SBWire.h"
	#endif
#endif

#ifdef ARDUINO

#if ARDUINO < 100

#include "WProgram.h"

#else

#include "Arduino.h"

#endif

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

//#include <Wire.h>

#include <i2c_t3.h>

#endif

#if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY

#include <I2C.h>

#endif

#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE

#include "SBWire.h"

#endif

Which I modified sometime in the distant past to use #include <i2c_t3.h> instead of #include <Wire.h>. At the time I think I was convinced that I had to use i2c_t3.h to get access to multiple I2C buses, but now I know that isn’t necessary. Fortunately this is a ‘local’ file, so changing this back shouldn’t (fingers crossed!) break other programs.

Aside: I did a search on MPU6050_6Axis_MotionApps_V6_12.h and I2Cdev.h in the Arduino folder, and got 48 different hits for MPU6050_6Axis_MotionApps_V6_12.h and 108 hits for ‘I2Cdev.h – ouch!! Not going to worry about this now, but I’m sure I’ll be dealing with this problem forever – if not longer 🙁

Aside2: The ‘I2Cdev.h’ file gets specialized to different hardware in I2CDevLib. For ‘Arduino’ hardware it is this file:

// I2Cdev library collection - Main I2C device class header file
// Abstracts bit and byte I2C R/W functions into a convenient class
// 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
//      2021-09-28 - allow custom Wire object as transaction function argument
//      2020-01-20 - hardija : complete support for Teensy 3.x
//      2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1
//      2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
//      2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
//      2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
//                 - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
//      2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
//      2011-10-03 - added automatic Arduino version detection for ease of use
//      2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
//      2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
//      2011-08-03 - added optional timeout parameter to read* methods to easily change from default
//      2011-08-02 - added support for 16-bit registers
//                 - fixed incorrect Doxygen comments on some methods
//                 - added timeout value for read operations (thanks mem @ Arduino forums)
//      2011-07-30 - changed read/write function structures to return success or byte counts
//                 - made all methods static for multi-device memory savings
//      2011-07-28 - initial release

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2013 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

#ifndef _I2CDEV_H_
#define _I2CDEV_H_

// -----------------------------------------------------------------------------
// Enable deprecated pgmspace typedefs in avr-libc
// -----------------------------------------------------------------------------
#define __PROG_TYPES_COMPAT__

// -----------------------------------------------------------------------------
// I2C interface implementation setting
// -----------------------------------------------------------------------------
#ifndef I2CDEV_IMPLEMENTATION
//#define I2CDEV_IMPLEMENTATION       I2CDEV_ARDUINO_WIRE
#define I2CDEV_IMPLEMENTATION       I2CDEV_TEENSY_3X_WIRE
//#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_SBWIRE
//#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_FASTWIRE
#endif // I2CDEV_IMPLEMENTATION

// comment this out if you are using a non-optimal IDE/implementation setting
// but want the compiler to shut up about it
#define I2CDEV_IMPLEMENTATION_WARNINGS

// -----------------------------------------------------------------------------
// I2C interface implementation options
// -----------------------------------------------------------------------------
#define I2CDEV_ARDUINO_WIRE         1 // Wire object from Arduino
#define I2CDEV_BUILTIN_NBWIRE       2 // Tweaked Wire object from Gene Knight's NBWire project
                                      // ^^^ NBWire implementation is still buggy w/some interrupts!
#define I2CDEV_BUILTIN_FASTWIRE     3 // FastWire object from Francesco Ferrara's project
#define I2CDEV_I2CMASTER_LIBRARY    4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library
#define I2CDEV_BUILTIN_SBWIRE	    5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire 
#define I2CDEV_TEENSY_3X_WIRE       6 // Teensy 3.x support using i2c_t3 library

// -----------------------------------------------------------------------------
// Arduino-style "Serial.print" debug constant (uncomment to enable)
// -----------------------------------------------------------------------------
//#define I2CDEV_SERIAL_DEBUG

#ifdef ARDUINO
    #if ARDUINO < 100
        #include "WProgram.h"
    #else
        #include "Arduino.h"
    #endif
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        #include <Wire.h>
    #endif
    #if I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE
        #include <i2c_t3.h>
    #endif
    #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
        #include <I2C.h>
    #endif
    #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
        #include "SBWire.h"
    #endif
#endif

100

101

102

103

104

105

// I2Cdev library collection - Main I2C device class header file

// Abstracts bit and byte I2C R/W functions into a convenient class

// 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>

// Changelog:

// 2021-09-28 - allow custom Wire object as transaction function argument

// 2020-01-20 - hardija : complete support for Teensy 3.x

// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1

// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications

// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)

// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire

// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation

// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)

// 2011-10-03 - added automatic Arduino version detection for ease of use

// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications

// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)

// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default

// 2011-08-02 - added support for 16-bit registers

// - fixed incorrect Doxygen comments on some methods

// - added timeout value for read operations (thanks mem @ Arduino forums)

// 2011-07-30 - changed read/write function structures to return success or byte counts

// - made all methods static for multi-device memory savings

// 2011-07-28 - initial release

/* ============================================

I2Cdev device library code is placed under the MIT license

Permission is hereby granted, free of charge, to any person obtaining a copy

of this software and associated documentation files (the "Software"), to deal

in the Software without restriction, including without limitation the rights

to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

copies of the Software, and to permit persons to whom the Software is

furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in

all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

THE SOFTWARE.

===============================================

#ifndef _I2CDEV_H_

#define _I2CDEV_H_

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

// Enable deprecated pgmspace typedefs in avr-libc

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

#define __PROG_TYPES_COMPAT__

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

// I2C interface implementation setting

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

#ifndef I2CDEV_IMPLEMENTATION

//#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE

#define I2CDEV_IMPLEMENTATION I2CDEV_TEENSY_3X_WIRE

//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE

//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE

#endif // I2CDEV_IMPLEMENTATION

// comment this out if you are using a non-optimal IDE/implementation setting

// but want the compiler to shut up about it

#define I2CDEV_IMPLEMENTATION_WARNINGS

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

// I2C interface implementation options

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

#define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino

#define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project

// ^^^ NBWire implementation is still buggy w/some interrupts!

#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project

#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library

#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire

#define I2CDEV_TEENSY_3X_WIRE 6 // Teensy 3.x support using i2c_t3 library

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

// Arduino-style "Serial.print" debug constant (uncomment to enable)

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

//#define I2CDEV_SERIAL_DEBUG

#ifdef ARDUINO

#if ARDUINO < 100

#include "WProgram.h"

#else

#include "Arduino.h"

#endif

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include <Wire.h>

#endif

#if I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE

#include <i2c_t3.h>

#endif

#if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY

#include <I2C.h>

#endif

#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE

#include "SBWire.h"

#endif

Where it can be seen that sometime in the distant past, I modified the original library file to select ‘I2CDEV_TEENSY_3X_WIRE’, which has the effect of #include <i2c_t3.h> instead of #include <Wire.h> – oops! So, any project that uses the library version of this file will always #include <i2c_t3.h> instead of #include <Wire.h> – double, triple, and quadruple oops! In addition, I discovered that the version of i2cdev.h I am using for this project is at least 6 years out of date – wonderful (at least it looks like the MPU6050_6Axis_MotionApps_V6_12.h is reasonably current (in fact, it is essentially identical to the library version).

To start with, I made sure T35_WallE3_V5 compiles for T3.5 ‘as is’, and then modified the includes to use the library version of MPU6050_6Axis_MotionApps_V6_12.h. This actually worked (yay!), although I discovered I had to use the “file.h” format rather than <file.h> – don’t know why.

23 January 2022 Update:

As usual, there were a few detours on the way to full ‘second-deck’ functionality. To start with, I discovered/realized that I was using the wrong I2C library (i2c_t3.h) for working with multiple I2C busses. The i2c_t3.h library does a lot of nice things, including multiple I2C bus support, but unfortunately isn’t compatible with a lot of the hardware driver libraries I use, most of which assume you are using the ‘Wire.h’ library. I had ‘sort-of’ solved this problem with many of my Teensy projects by hacking the needed hardware driver libraries to use i2c_t3.h instead of Wire.h, but this got old pretty quickly, and even older when I started trying to use multiple I2C bus enabled hardware driver libraries (like Kurt E’s wonderful multiple bus MPU6050 driver).

So, I wound up spending way too many hours tracking down the differences and similarities between i2c_t3.h and the Teensy version of Wire.h See this post for all the gory details. Along the way I learned how to simplify access to deeply buried library files using the Windows 10 flavor of symlinks, which was kinda cool, but I could probably have spent the time more wisely elsewhere. Also, I ran headlong into yet another multiple bus problem with one of my favorite libraries – Nick Gammon’s wonderful ‘I2C_Anything’ library that does just two things – it reads/rights ‘anything’ – ints, floats, unsigned long ints, doubles, whatever – across an I2C connection – no more worrying about how to do that, or being forced to transmit ASCII across the bus and construct/deconstruct as necessary on both ends. Unfortunately, this library is unabashedly single-bus – it expects to be used in a Wire.h environment and that is that. The good news is, by the time I was done the question of ‘why use i2c_t3.h as opposed to Wire.h?’, I was able to intelligently (I hope) modify I2C_Anything.h to accommodate multiple I2C busses (though it still assuming a ‘Wire.h’ environment). At the end, I made a pull request to the I2C_Anything github repo, so maybe others can use this as well.

After all this was done, I still hadn’t even started on getting the second-deck VL53L0X sensors talking to the main Teensy 3.5 processor, but at least now I (kinda) knew what I was doing.

Anyhoo, once I got back to trying to get connected to the VL53L0X array, I was able to reasonably quickly revise both the main Teensy 3.5 processor program and the VL53L0X array management program to use Wire.h vs i2c_t3.h, and to instantiate/initialize the multiple busses required for both processors (the main processor talks to the VL53L0X array manager via Wire1 on its end to Wire0 on the array end, and the array manager talks to the seven VL53L0X modules via its Wire1 & Wire2 busses). And with my newly modified I2C_Anything library, I was ready to get them talking to each other.

Here’s the working Array manager code (pardon the extra comment lines):

/*
    Name:       Teensy_7VL53L0X_I2C_Slave_V3.ino
    Created:	1/24/2021 4:03:35 PM
    Author:     FRANKNEWXPS15\Frank

    This is the 'final' version of the Teensy 3.5 project to manage all 
    seven VL53L0X lidar distance sensors, three on left, three on right, 
    and now including a rear distance sensor.  As of this date it was an
    identical copy of the TeensyHexVL53L0XHighSpeedDemo project, just
    renamed for clarity

    01/23/22 - revised to work properly with Teensy 3.5 main processor
*/

/*
    Name:       TeensyHexVL53L0XHighSpeedDemo.ino
    Created:	10/1/2020 3:49:01 PM
    Author:     FRANKNEWXPS15\Frank

    This demo program combines the code from my Teensy_VL53L0X_I2C_Slave_V2
    project with the VL53L0X 'high speed' code from the Pololu support
    forum post at https://forum.pololu.com/t/high-speed-with-vl53l0x/16585/3

*/


/*
    Name:       Teensy_VL53L0X_I2C_Slave_V2.ino
    Created:	8/4/2020 7:42:22 PM
    Author:     FRANKNEWXPS15\Frank

    This project merges KurtE's multi-wire capable version of Pololu's VL53L0X library
    with my previous Teensy_VL53L0X_I2C_Slave project, which used the Adafruit VL53L0X
    library.  This (hopefully) will be the version uploaded to the Teensy on the 2nd
    deck of Wall-E2 and which controls the left & right 3-VL53L0X sensor arrays

    01/17/22:  Copied VL53L0X.h/cpp from C:\Users\paynt\Documents\Arduino\Libraries\vl53l0x-arduino-multi_wire
    so I won't have to guess which library folder these files came from

    01/17/22: if you have a local file.h same as a library\xx\file.h, then if VMicro's 'deep search'option 
     is enabled, must use #include "file.h" instead of #include <file.h>.  This is the reason for using
     #include "VL53L0X.h" and #include "I2C_Anything.h"  below 

    01/17/22 the local version of "I2C_Anything.h" uses '#include <i2c_t3.h>' vice <Wire.h> for multiple
    I2C buss availability.

    01/18/22 removed I2C_Anything.h & VL53L0X.h from project folder so can use library versions
    01/22/22 added #ifdef NO_VL53L0X and assoc #ifndef blocks for debug

*/
#include <Wire.h> //01/18/22 Wire.h has multiple I2C bus access capability
#include <VL53L0X.h>
#include <I2C_Anything.h> 
#include "elapsedMillis.h"

//#define NO_VL53L0X

volatile uint8_t request_type;

const int SETUP_DURATION_OUTPUT_PIN = 33;
const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;
bool bTeensyReady = false;
const int MSEC_BETWEEN_LED_TOGGLES = 100;
elapsedMillis mSecBetweenLEDToggle;


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/05/20, chg 'BOTH' to 'ALL' 10/31/20
  VL53L0X_REAR_ONLY //added 10/24/20
};

//right side array
uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;

//left side array
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;
uint16_t Rear_Dist_mm;

//float RightSteeringVal;
//float LeftSteeringVal;
double RightSteeringVal;
double LeftSteeringVal;

const int SLAVE_ADDRESS = 0x20; //just a guess for now
const int DEFAULT_VL53L0X_ADDR = 0x29;

//XSHUT required for I2C address init 
//right side array
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;

//left side array
const int LF_XSHUT_PIN = 5;
const int LC_XSHUT_PIN = 6;
const int LR_XSHUT_PIN = 7;
const int Rear_XSHUT_PIN = 8;

const int MAX_LIDAR_RANGE_MM = 1000; //make it obvious when nearest object is out of range

#ifndef NO_VL53L0X

//right side array
VL53L0X lidar_RF(&Wire1);
VL53L0X lidar_RC(&Wire1);
VL53L0X lidar_RR(&Wire1);

//left side array
VL53L0X lidar_LF(&Wire2);
VL53L0X lidar_LC(&Wire2);
VL53L0X lidar_LR(&Wire2);

VL53L0X lidar_Rear(&Wire2); //added to Wire2 daisy-chain

#endif // !NO_VL53L0X

void setup()
{
  bTeensyReady = false; //11/10/20 added to prevent bad data reads by main controller

  pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);
  pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);
  Serial.begin(115200);

  // wait until serial port opens ... For 5 seconds max
  while (!Serial && millis() < 5000);

  Wire.begin(SLAVE_ADDRESS); //set Teensy Wire0 port up as slave
  Wire.onRequest(requestEvent);  //ISR for I2C requests from master (Mega 2560)
  Wire.onReceive(receiveEvent); //ISR for I2C data from master (Mega 2560)

  Wire1.begin();
  Wire2.begin();

  Serial.printf("Teensy_7VL53L0X_I2C_Slave_V3\n");

#pragma region VL53L0X_INIT

#ifndef NO_VL53L0X
  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);

  pinMode(LF_XSHUT_PIN, OUTPUT);
  pinMode(LC_XSHUT_PIN, OUTPUT);
  pinMode(LR_XSHUT_PIN, OUTPUT);

  //Put all sensors in reset mode by pulling XSHUT low
  digitalWrite(RF_XSHUT_PIN, LOW);
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  digitalWrite(LF_XSHUT_PIN, LOW);
  digitalWrite(LC_XSHUT_PIN, LOW);
  digitalWrite(LR_XSHUT_PIN, LOW);

  //added 10/23/20 for rear sensor
  pinMode(Rear_XSHUT_PIN, OUTPUT);
  digitalWrite(Rear_XSHUT_PIN, LOW);


  //now bring lidar_RF only out of reset and set it's address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RF_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_RF.init())
  {
    Serial.println("Failed to detect and initialize lidar_RF!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RF.setMeasurementTimingBudget(20000);
  lidar_RF.startContinuous();


  lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);
  Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

  //now bring lidar_RC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RC_XSHUT_PIN, INPUT);

  delay(10);
  if (!lidar_RC.init())
  {
    Serial.println("Failed to detect and initialize lidar_RC!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RC.setMeasurementTimingBudget(20000);
  lidar_RC.startContinuous();

  lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);
  Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

  //now bring lidar_RR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RR_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_RR.init())
  {
    Serial.println("Failed to detect and initialize lidar_RR!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RR.setMeasurementTimingBudget(20000);
  lidar_RR.startContinuous();

  lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);
  Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

  //now bring lidar_LF only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LF_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LF.init())
  {
    Serial.println("Failed to detect and initialize lidar_LF!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_LF.setMeasurementTimingBudget(20000);
  lidar_LF.startContinuous();

  lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);
  Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

  //now bring lidar_LC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LC_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LC.init())
  {
    Serial.println("Failed to detect and initialize lidar_LC!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_LC.setMeasurementTimingBudget(20000);
  lidar_LC.startContinuous();

  lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);
  Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

  //now bring lidar_LR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LR_XSHUT_PIN, INPUT);

  if (!lidar_LR.init())
  {
    Serial.println("Failed to detect and initialize lidar_LR!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_LR.setMeasurementTimingBudget(20000);
  lidar_LR.startContinuous();

  lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);
  Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

  //added 10/23/20 for rear sensor
  //now bring lidar_Rear only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(Rear_XSHUT_PIN, INPUT);

  if (!lidar_Rear.init())
  {
    Serial.println("Failed to detect and initialize lidar_Rear!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_Rear.setMeasurementTimingBudget(20000);
  lidar_Rear.startContinuous();

  lidar_Rear.setAddress(DEFAULT_VL53L0X_ADDR + 7);
  Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());
#pragma endregion VL53L0X_INIT

  //Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tLSteer\tRSteer\n");
  Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\tLSteer\tRSteer\n");
#endif // !NO_VL53L0X

  digitalWrite(SETUP_DURATION_OUTPUT_PIN, LOW);
  bTeensyReady = true; //11/10/20 added to prevent bad data reads by main controller
  mSecBetweenLEDToggle = 0;
}

void loop()
{
  if (mSecBetweenLEDToggle <= MSEC_BETWEEN_LED_TOGGLES)
  {
    mSecBetweenLEDToggle -= MSEC_BETWEEN_LED_TOGGLES;
    digitalToggle(LED_BUILTIN);
    Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\n",
      millis(), LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm,
      LeftSteeringVal, RightSteeringVal);
  }

#ifndef NO_VL53L0X
  //from Pololu forum post
  RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();
  RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();
  RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

  RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

  //from Pololu forum post
  LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();
  LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();
  LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();

  LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

  //added 10/23/20 for rear sensor
  Rear_Dist_mm = lidar_Rear.readRangeContinuousMillimeters();
#endif // !NO_VL53L0X
}

// function that executes whenever data is requested by master
// this function is registered as an event, see setup()
void requestEvent()
{
  //Purpose: Send requested sensor data to the Mega controller via main I2C bus
  //Inputs: 
  //  request_type = uint8_t value denoting type of data requested (from receiveEvent())
  //      0 = left & right center distances only
  //      1 = right side front, center and rear distances, plus steering value
  //      2 = left side front, center and rear distances, plus steering value
  //      3 = both side front, center and rear distances, plus both steering values
//Outputs:
  //  Requested data sent to master
  //Notes:
  //  08/05/20 added VL53L0X_ALL request type to get both sides at once
  //  10/24/20 added VL53L0X_REAR_ONLY request type
  //  11/09/20 added write to pin for O'scope monitoring

  int data_size = 0;

//DEBUG!!
    //Serial.printf("RequestEvent() with request_type = %d: VL53L0X Front/Center/Rear distances = %d, %d, %d\n", 
    //    request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);
//DEBUG!!

  digitalWrite(LED_BUILTIN, HIGH);//added 01/22/22
  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);

  switch (request_type)
  {
  case 	VL53L0X_READYCHECK: //added 11/10/20 to prevent bad reads during Teensy setup()
    Serial.printf("in VL53L0X_READYCHECK case at %lu with bTeensyReady = %d\n", millis(), bTeensyReady);
    I2C_writeAnything(bTeensyReady);
    break;

  case VL53L0X_CENTERS_ONLY:

    //DEBUG!!
      //data_size = 2*sizeof(uint16_t);
      //Serial.printf("Sending %d bytes LC_Dist_mm = %d, RC_Dist_mm = %d to master\n", data_size, LC_Dist_mm, RC_Dist_mm);
    //DEBUG!!

    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    break;

  case VL53L0X_RIGHT:

    //DEBUG!!
            //data_size = 3 * sizeof(uint16_t) + sizeof(float);
            //Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",
            //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);
    //DEBUG!!

    I2C_writeAnything(RF_Dist_mm);
    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(RR_Dist_mm);
    I2C_writeAnything(RightSteeringVal);
    break;

  case VL53L0X_LEFT:

    //DEBUG!!
            //data_size = 3 * sizeof(uint16_t) + sizeof(float);
            //Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",
            //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);
    //DEBUG!!

    I2C_writeAnything(LF_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    I2C_writeAnything(LR_Dist_mm);
    I2C_writeAnything(LeftSteeringVal);
    break;

  case VL53L0X_ALL:
    //Serial.printf("In VL53L0X_ALL case\n");
    //added 08/05/20 to get data from both sides at once
    //10/31/20 chg to VL53L0X_ALL & report all 7 sensor values

//DEBUG!!
    //data_size = 3 * sizeof(uint16_t) + sizeof(float);
    data_size = 7 * sizeof(uint16_t) + 2 * sizeof(float);
    //Serial.printf("Sending %d bytes to master\n", data_size);
    //Serial.printf("%d bytes: %d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%d\t%3.2f\n",
    //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal,
    //  RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm, RightSteeringVal);
//DEBUG!!


    //left side  
    I2C_writeAnything(LF_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    I2C_writeAnything(LR_Dist_mm);
    I2C_writeAnything(LeftSteeringVal);


    //DEBUG!!
      //data_size = 3 * sizeof(uint16_t) + sizeof(float);
      //Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",
      //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, LeftSteeringVal);

      //Serial.printf(" right %d\t%d\t%d\t%d\t%2.3f\n",
      //RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);
    //DEBUG!!

       //right side
    I2C_writeAnything(RF_Dist_mm);
    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(RR_Dist_mm);
    I2C_writeAnything(Rear_Dist_mm);
    I2C_writeAnything(RightSteeringVal);

    break;

  case VL53L0X_REAR_ONLY:

    //DEBUG!!
      //data_size = sizeof(uint16_t);
      //Serial.printf("Sending %d bytes Rear_Dist_mm = %d to master\n", data_size, Rear_Dist_mm);
    //DEBUG!!

    I2C_writeAnything(Rear_Dist_mm);
    break;

  default:
    break;
  }

  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);
  digitalWrite(LED_BUILTIN, LOW);//added 01/22/22
}

//
// handle Rx Event (incoming I2C data)
//
void receiveEvent(int numBytes)
{
  //Purpose: capture data request type from Mega I2C master
  //Inputs:
  //  numBytes = int value denoting number of bytes received from master
  //Outputs:
  //  uint8_t request_type filled with request type value from master


//DEBUG!!
    //Serial.printf("receiveEvent(%d)\n", numBytes);
//DEBUG!!

  I2C_readAnything(request_type);

  //DEBUG!!
      //Serial.printf("receiveEvent: Got %d from master\n", request_type);
  //DEBUG!!
}

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

Name: Teensy_7VL53L0X_I2C_Slave_V3.ino

Created: 1/24/2021 4:03:35 PM

Author: FRANKNEWXPS15\Frank

This is the 'final' version of the Teensy 3.5 project to manage all

seven VL53L0X lidar distance sensors, three on left, three on right,

and now including a rear distance sensor. As of this date it was an

identical copy of the TeensyHexVL53L0XHighSpeedDemo project, just

renamed for clarity

01/23/22 - revised to work properly with Teensy 3.5 main processor

Name: TeensyHexVL53L0XHighSpeedDemo.ino

Created: 10/1/2020 3:49:01 PM

Author: FRANKNEWXPS15\Frank

This demo program combines the code from my Teensy_VL53L0X_I2C_Slave_V2

project with the VL53L0X 'high speed' code from the Pololu support

forum post at https://forum.pololu.com/t/high-speed-with-vl53l0x/16585/3

Name: Teensy_VL53L0X_I2C_Slave_V2.ino

Created: 8/4/2020 7:42:22 PM

Author: FRANKNEWXPS15\Frank

This project merges KurtE's multi-wire capable version of Pololu's VL53L0X library

with my previous Teensy_VL53L0X_I2C_Slave project, which used the Adafruit VL53L0X

library. This (hopefully) will be the version uploaded to the Teensy on the 2nd

deck of Wall-E2 and which controls the left & right 3-VL53L0X sensor arrays

01/17/22: Copied VL53L0X.h/cpp from C:\Users\paynt\Documents\Arduino\Libraries\vl53l0x-arduino-multi_wire

so I won't have to guess which library folder these files came from

01/17/22: if you have a local file.h same as a library\xx\file.h, then if VMicro's 'deep search'option

is enabled, must use #include "file.h" instead of #include <file.h>. This is the reason for using

#include "VL53L0X.h" and #include "I2C_Anything.h" below

01/17/22 the local version of "I2C_Anything.h" uses '#include <i2c_t3.h>' vice <Wire.h> for multiple

I2C buss availability.

01/18/22 removed I2C_Anything.h & VL53L0X.h from project folder so can use library versions

01/22/22 added #ifdef NO_VL53L0X and assoc #ifndef blocks for debug

#include <Wire.h> //01/18/22 Wire.h has multiple I2C bus access capability

#include <VL53L0X.h>

#include <I2C_Anything.h>

#include "elapsedMillis.h"

//#define NO_VL53L0X

volatile uint8_t request_type;

const int SETUP_DURATION_OUTPUT_PIN = 33;

const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;

bool bTeensyReady = false;

const int MSEC_BETWEEN_LED_TOGGLES = 100;

elapsedMillis mSecBetweenLEDToggle;

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/05/20, chg 'BOTH' to 'ALL' 10/31/20

VL53L0X_REAR_ONLY //added 10/24/20

};

//right side array

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

//left side array

uint16_t LF_Dist_mm;

uint16_t LC_Dist_mm;

uint16_t LR_Dist_mm;

uint16_t Rear_Dist_mm;

//float RightSteeringVal;

//float LeftSteeringVal;

double RightSteeringVal;

double LeftSteeringVal;

const int SLAVE_ADDRESS = 0x20; //just a guess for now

const int DEFAULT_VL53L0X_ADDR = 0x29;

//XSHUT required for I2C address init

//right side array

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

//left side array

const int LF_XSHUT_PIN = 5;

const int LC_XSHUT_PIN = 6;

const int LR_XSHUT_PIN = 7;

const int Rear_XSHUT_PIN = 8;

const int MAX_LIDAR_RANGE_MM = 1000; //make it obvious when nearest object is out of range

#ifndef NO_VL53L0X

//right side array

VL53L0X lidar_RF(&Wire1);

VL53L0X lidar_RC(&Wire1);

VL53L0X lidar_RR(&Wire1);

//left side array

VL53L0X lidar_LF(&Wire2);

VL53L0X lidar_LC(&Wire2);

VL53L0X lidar_LR(&Wire2);

VL53L0X lidar_Rear(&Wire2); //added to Wire2 daisy-chain

#endif // !NO_VL53L0X

void setup()

{

bTeensyReady = false; //11/10/20 added to prevent bad data reads by main controller

pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);

pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);

pinMode(LED_BUILTIN, OUTPUT);

digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);

Serial.begin(115200);

// wait until serial port opens ... For 5 seconds max

while (!Serial && millis() < 5000);

Wire.begin(SLAVE_ADDRESS); //set Teensy Wire0 port up as slave

Wire.onRequest(requestEvent); //ISR for I2C requests from master (Mega 2560)

Wire.onReceive(receiveEvent); //ISR for I2C data from master (Mega 2560)

Wire1.begin();

Wire2.begin();

Serial.printf("Teensy_7VL53L0X_I2C_Slave_V3\n");

#pragma region VL53L0X_INIT

#ifndef NO_VL53L0X

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(LF_XSHUT_PIN, OUTPUT);

pinMode(LC_XSHUT_PIN, OUTPUT);

pinMode(LR_XSHUT_PIN, OUTPUT);

//Put all sensors in reset mode by pulling XSHUT low

digitalWrite(RF_XSHUT_PIN, LOW);

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

//added 10/23/20 for rear sensor

pinMode(Rear_XSHUT_PIN, OUTPUT);

digitalWrite(Rear_XSHUT_PIN, LOW);

//now bring lidar_RF only out of reset and set it's address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RF.init())

{

Serial.println("Failed to detect and initialize lidar_RF!");

while (1) {}

}

//from Pololu forum post

lidar_RF.setMeasurementTimingBudget(20000);

lidar_RF.startContinuous();

lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);

Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

//now bring lidar_RC only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RC.init())

{

Serial.println("Failed to detect and initialize lidar_RC!");

while (1) {}

}

//from Pololu forum post

lidar_RC.setMeasurementTimingBudget(20000);

lidar_RC.startContinuous();

lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);

Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

//now bring lidar_RR only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RR_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RR.init())

{

Serial.println("Failed to detect and initialize lidar_RR!");

while (1) {}

}

//from Pololu forum post

lidar_RR.setMeasurementTimingBudget(20000);

lidar_RR.startContinuous();

lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);

Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

//now bring lidar_LF only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LF.init())

{

Serial.println("Failed to detect and initialize lidar_LF!");

while (1) {}

}

//from Pololu forum post

lidar_LF.setMeasurementTimingBudget(20000);

lidar_LF.startContinuous();

lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);

Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

//now bring lidar_LC only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LC.init())

{

Serial.println("Failed to detect and initialize lidar_LC!");

while (1) {}

}

//from Pololu forum post

lidar_LC.setMeasurementTimingBudget(20000);

lidar_LC.startContinuous();

lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);

Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

//now bring lidar_LR only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LR_XSHUT_PIN, INPUT);

if (!lidar_LR.init())

{

Serial.println("Failed to detect and initialize lidar_LR!");

while (1) {}

}

//from Pololu forum post

lidar_LR.setMeasurementTimingBudget(20000);

lidar_LR.startContinuous();

lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);

Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

//added 10/23/20 for rear sensor

//now bring lidar_Rear only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(Rear_XSHUT_PIN, INPUT);

if (!lidar_Rear.init())

{

Serial.println("Failed to detect and initialize lidar_Rear!");

while (1) {}

}

//from Pololu forum post

lidar_Rear.setMeasurementTimingBudget(20000);

lidar_Rear.startContinuous();

lidar_Rear.setAddress(DEFAULT_VL53L0X_ADDR + 7);

Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());

#pragma endregion VL53L0X_INIT

//Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tLSteer\tRSteer\n");

Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\tLSteer\tRSteer\n");

#endif // !NO_VL53L0X

digitalWrite(SETUP_DURATION_OUTPUT_PIN, LOW);

bTeensyReady = true; //11/10/20 added to prevent bad data reads by main controller

mSecBetweenLEDToggle = 0;

}

void loop()

{

if (mSecBetweenLEDToggle <= MSEC_BETWEEN_LED_TOGGLES)

{

mSecBetweenLEDToggle -= MSEC_BETWEEN_LED_TOGGLES;

digitalToggle(LED_BUILTIN);

Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\n",

millis(), LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm,

LeftSteeringVal, RightSteeringVal);

}

#ifndef NO_VL53L0X

//from Pololu forum post

RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();

RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();

RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

//from Pololu forum post

LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();

LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();

LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();

LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

//added 10/23/20 for rear sensor

Rear_Dist_mm = lidar_Rear.readRangeContinuousMillimeters();

#endif // !NO_VL53L0X

}

// function that executes whenever data is requested by master

// this function is registered as an event, see setup()

void requestEvent()

{

//Purpose: Send requested sensor data to the Mega controller via main I2C bus

//Inputs:

// request_type = uint8_t value denoting type of data requested (from receiveEvent())

// 0 = left & right center distances only

// 1 = right side front, center and rear distances, plus steering value

// 2 = left side front, center and rear distances, plus steering value

// 3 = both side front, center and rear distances, plus both steering values

//Outputs:

// Requested data sent to master

//Notes:

// 08/05/20 added VL53L0X_ALL request type to get both sides at once

// 10/24/20 added VL53L0X_REAR_ONLY request type

// 11/09/20 added write to pin for O'scope monitoring

int data_size = 0;

//DEBUG!!

//Serial.printf("RequestEvent() with request_type = %d: VL53L0X Front/Center/Rear distances = %d, %d, %d\n",

// request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//DEBUG!!

digitalWrite(LED_BUILTIN, HIGH);//added 01/22/22

digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);

switch (request_type)

{

case VL53L0X_READYCHECK: //added 11/10/20 to prevent bad reads during Teensy setup()

Serial.printf("in VL53L0X_READYCHECK case at %lu with bTeensyReady = %d\n", millis(), bTeensyReady);

I2C_writeAnything(bTeensyReady);

break;

case VL53L0X_CENTERS_ONLY:

//DEBUG!!

//data_size = 2*sizeof(uint16_t);

//Serial.printf("Sending %d bytes LC_Dist_mm = %d, RC_Dist_mm = %d to master\n", data_size, LC_Dist_mm, RC_Dist_mm);

//DEBUG!!

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

break;

case VL53L0X_RIGHT:

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",

// data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);

//DEBUG!!

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(RightSteeringVal);

break;

case VL53L0X_LEFT:

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",

// data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

//DEBUG!!

I2C_writeAnything(LF_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

I2C_writeAnything(LR_Dist_mm);

I2C_writeAnything(LeftSteeringVal);

break;

case VL53L0X_ALL:

//Serial.printf("In VL53L0X_ALL case\n");

//added 08/05/20 to get data from both sides at once

//10/31/20 chg to VL53L0X_ALL & report all 7 sensor values

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

data_size = 7 * sizeof(uint16_t) + 2 * sizeof(float);

//Serial.printf("Sending %d bytes to master\n", data_size);

//Serial.printf("%d bytes: %d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%d\t%3.2f\n",

// data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal,

// RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm, RightSteeringVal);

//DEBUG!!

//left side

I2C_writeAnything(LF_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

I2C_writeAnything(LR_Dist_mm);

I2C_writeAnything(LeftSteeringVal);

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",

// data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, LeftSteeringVal);

//Serial.printf(" right %d\t%d\t%d\t%d\t%2.3f\n",

//RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//DEBUG!!

//right side

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(Rear_Dist_mm);

I2C_writeAnything(RightSteeringVal);

break;

case VL53L0X_REAR_ONLY:

//DEBUG!!

//data_size = sizeof(uint16_t);

//Serial.printf("Sending %d bytes Rear_Dist_mm = %d to master\n", data_size, Rear_Dist_mm);

//DEBUG!!

I2C_writeAnything(Rear_Dist_mm);

break;

default:

break;

}

digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);

digitalWrite(LED_BUILTIN, LOW);//added 01/22/22

}

// handle Rx Event (incoming I2C data)

void receiveEvent(int numBytes)

{

//Purpose: capture data request type from Mega I2C master

//Inputs:

// numBytes = int value denoting number of bytes received from master

//Outputs:

// uint8_t request_type filled with request type value from master

//DEBUG!!

//Serial.printf("receiveEvent(%d)\n", numBytes);

//DEBUG!!

I2C_readAnything(request_type);

//DEBUG!!

//Serial.printf("receiveEvent: Got %d from master\n", request_type);

//DEBUG!!

}

And here’s the working main processor code. Note that the code as it is presented here has the ‘DISTANCES_ONLY’ define enabled, and all non-existent hardware modules ‘#defined’ out so I could concentrate on just the VL53L0X array connection.

/*
    Name:       T35_WallE3_V5.ino
    Created:	1/15/2022 8:08:22 PM
    Author:     NEWXPS15\paynt

    01/15/22 - This version is intended to add the second deck functionality, and start integrating wall-following
    01/20/22 - Now incorporating lessons learned from 
    https://www.fpaynter.com/2022/01/fixing-the-mess-i-made-with-the-i2cdevlib-mpu6050-libraries/
    01/22/22 - replaced I2C_read/writeAnything() calls with new multiple-I2C-bus capable version
    01/23/22 - now working properly with Teensy 3.5 VL53L0X array manager
*/
/*
    Name:       T35_WallE3_V4.ino
    Created:	1/3/2022 3:44:09 PM
    Author:     NEWXPS15\paynt

    - added GetOpMode() and the associated loop() switch and case code.
    - added the IR Homing code so I could test the IR demod hardware.
    - added #include "TeePrint.h" to incorporate dual-port printing in one line - see
      (https://forum.pjrc.com/threads/69146-print-same-string-to-two-different-serial-ports)
      for details
     - replaced all 'Serialx.printf' instances with 'TeePrint.printf'
*/
/*
    Name:       T35_WallE3_V3.ino
    Created:	11/26/2021 8:06:22 PM
    Author:     FRANKNEWXPS15\Frank

    12/18/21: This version uses a TeraTerm script 'TeensyOTA1.ttl' in conjunction with Visual Micro's 'board.txt' feature
    to accomplish OTA updates.  Note that a 'regular' (F7) build does not trigger an OTA update,
    but a DEBUG (F5) build does.

    Implementation of OTA update requires that Joe Pasquariello's 'FlasherX'
    code be added to the Teensy sketch as it is below, and some code to allow
    an external serial communications program to trigger the update process.
*/

#pragma region INCLUDES
#include <Wire.h>
#include "FlashTxx.h"		// TLC/T3x/T4x flash primitives
#include <elapsedMillis.h>
#include "MPU6050_6Axis_MotionApps612.h"  //01/18/22 changed to use the \I2CDevLib\Arduino\MPU6050\ version
#include "I2C_Anything.h" //needed for sending float data over I2C
#include <PID_v2.h> //new version 05/16/21 
#include "timelib.h" //added 01/01/22 for charge monitoring support
#include "TeePrint.h" //added 01/04/22 to print to both ports in one line
#pragma endregion Includes

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

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

#pragma region PRE_SETUP
MPU6050 mpu(MPU6050_I2C_ADDR);
elapsedMillis MsecSinceLastLEDToggle; //used for LED blink timer
elapsedMillis MsecSinceLastOpModeChk; //01/07/22 used for OpMode check
elapsedMillis MsecSinceLastIRHomingAdj; //01/07/22 used for #ifdef IR_HOMING_ONLY block
elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops
const int MSEC_PER_IR_HOMING_ADJ = 200; //01/07/22
const int MSEC_PER_OP_MODE_CHK = 200; //01/07/22
const int MSEC_PER_LOOP = 200;
const int MSEC_PER_DIST_UPDATE = 200; //01/07/22
TeePrint myTeePrint(Serial, Serial1);

#pragma region ENUMS
//01/05/16 enum types cannot be used as arguments or return types for functions due to Arduino pre-processor quirk
//12/20/15 added for navigation support
//01/15/16 revised to be consistent wtih nav modes identified in https://fpaynter.com/2016/01/making-wall-e2-smarter-using-karnaugh-maps/
enum NavCases
{
  NAV_NONE = 0,
  NAV_WALLTRK,
  NAV_OBSTACLE,
  NAV_STEPTURN,
  NAV_STUCK,
  NAV_OPENCNR
};

//04/10/20 Experiment with porting heading based tracking capability from two wheel robot
enum TrackingState
{
  TRK_RIGHT_NONE,
  TRK_RIGHT_CAPTURE,
  TRK_RIGHT_MAINTAIN,
  TRK_RIGHT_BACKUP_AND_TURN,
  TRK_RIGHT_STEP_TURN
};
const char* NavStrArray[] = { "WallTrack", "Obstacle", "StepTurn", "Stuck", "OpenCorner" };

//03/08/17 added for new mode/state definitions; see https://fpaynter.com/2017/03/wall-e2-operating-mode-review/
enum OpModes
{
  MODE_NONE = 0, //04/04/17 chg from MODE_DEFAULT and moved to top (zero) position
  MODE_CHARGING,
  MODE_IRHOMING,
  MODE_WALLFOLLOW,
  MODE_WALLCAPTURE, //added 07/16/21 to manage wall offset capture operations
  MODE_DEADBATTERY, //added 01/16/18 to handle dead battery case
  MODE_DISCHARGE //added 03/04/20 to safely discharge the battery
};

const char* ModeStrArray[] = { "None", "Charge", "Home", "Wall", "Capture", "DeadBatt", "Discharge" };
const char* LongModeStrArray[] = { "None", "Charging", "IR Homing", "Wall Following", "Wall Capture", "Dead Battery", "Discharging" };

enum WallTrackingCases
{
  TRACKING_NONE = 0,
  TRACKING_LEFT,
  TRACKING_RIGHT,
  TRACKING_NEITHER
};
const char* TrkStrArray[] = { "None", "Left", "Right", "Neither" };

#pragma endregion Tracking and Nav Case Enums

#pragma region ADC CONSTANTS
const double ADC_REF_VOLTS = 3.3; //teensy default for analog inputs
const int MAX_AD_COUNT = 1023;
const double AmpsPerVolt = 1.00; //default 10K Rs
const double VoltsPerCount = ADC_REF_VOLTS / MAX_AD_COUNT;
const float VOLTAGE_TO_CURRENT_RATIO = 1.f; //Used for both 'Total' and 'Run' sensors
#pragma endregion ADC CONSTANTS

#pragma region TELEMETRYSTRINGS
//const String IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD";
const String IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD\tRearD";
const char* LeftWallFollowTelemStr = "Msec\tLFront\tLCtr\tLRear\tFront\tFrontVar\tRear\tSteer\tOutput\tSetPt\tAdjDist\tLSpd\tRSpd";
const char* RightWallFollowTelemStr = "Msec\tRFront\tRCtr\tRRear\tFront\tFVar\tSteer\tOutput\tSetPt\tAdjDist\tLSpd\tRSpd";
//const String ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\n"; //rev 05/02/20
const String ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tRearD\n"; //rev 01/30/21
#pragma endregion Mode-specific telemetry header strings

#pragma region BATTCONSTS
//03/10/15 added for battery charge level monitoring
//const int LOW_BATT_THRESH_VOLTS = 7.4; //50% chg per http://batteryuniversity.com/learn/article/lithium_based_batteries
//const int LOW_BATT_THRESH_VOLTS = 8.4; //07/10/20 temp debug settingr//50% chg per http://batteryuniversity.com/learn/article/lithium_based_batteries
const float LOW_BATT_THRESH_VOLTS = 8.4; //12/18/20 chg to float to fix bug
//const long BATT_CHG_TIMEOUT_SEC = 36000; //10 HRS
const long BATT_CHG_TIMEOUT_SEC = 3600; //12/28/20 for test only 
const float DEAD_BATT_THRESH_VOLTS = 6; //added 01/24/17
const float FULL_BATT_VOLTS = 8.4; //added 03/17/18. Chg to 8.4 03/05/20
const int MINIMUM_CHARGE_TIME_SEC = 10; //added 04/01/18
const float FULL_BATT_CURRENT_THRESHOLD = 0.5; //amps chg to 0.5A 03/02/19 per https://www.fpaynter.com/2019/03/better-battery-charging-for-wall-e2/
//const int CURRENT_AVERAGE_NUMBER = 10; //added 03/01/19
//const int VOLTS_AVERAGE_NUMBER = 5; //added 03/01/19
const int IR_HOMING_TELEMETRY_SPACING_MSEC = 200; //added 04/23/20

//battery fuel guage constants
const float _20PCT_BATT_VOLTS = DEAD_BATT_THRESH_VOLTS + 0.2f * (FULL_BATT_VOLTS - DEAD_BATT_THRESH_VOLTS);
const float _40PCT_BATT_VOLTS = DEAD_BATT_THRESH_VOLTS + 0.4f * (FULL_BATT_VOLTS - DEAD_BATT_THRESH_VOLTS);
const float _60PCT_BATT_VOLTS = DEAD_BATT_THRESH_VOLTS + 0.6f * (FULL_BATT_VOLTS - DEAD_BATT_THRESH_VOLTS);
const float _80PCT_BATT_VOLTS = DEAD_BATT_THRESH_VOLTS + 0.8f * (FULL_BATT_VOLTS - DEAD_BATT_THRESH_VOLTS);
const float ZENER_VOLTAGE_OFFSET = 5.84; //measured zener voltage
#pragma endregion Battery Constants

#pragma region DISTANCE_MEASUREMENT_SUPPORT
//misc LIDAR and Ping sensor parameters
const int MIN_FRONT_OBSTACLE_DIST_CM = 20; //rev 04/28/17 for better obstacle handling
const int CHG_STN_AVOIDANCE_DIST_CM = 40; //added 03/11/17 for charge stn avoidance
const int STEPTURN_DIST_CM = 50; //rev 06/29/20 to temporarily disable
const int MAX_FRONT_DISTANCE_CM = 400;
const int MAX_LR_DISTANCE_CM = 200;  //04/19/15 now using sep parameters for front and side sensors
const int CHG_STN_FINAL_APPR_DIST_CM = 20; //added 03/11/17 for charge stn avoidance

//04/01/2015 added for 'stuck' detection support
const int FRONT_DIST_ARRAY_SIZE = 50; //11/22/20 doubled to acct for 10Hz update rate
const int FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19
const int LR_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg
const int REAR_DIST_ARRAY_SIZE = 5; //04/13/21 added for running avg calc
const int LR_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ
const int STUCK_FRONT_VARIANCE_THRESHOLD = 50; //chg to 50 04/28/17
const int STUCK_REAR_VARIANCE_THRESHOLD = 50; //added 01/08/22
const int NO_LIDAR_FRONT_VAR_VAL = 10 * STUCK_FRONT_VARIANCE_THRESHOLD; //01/16/19
uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte

//04/28/19 added to reinstate l/r dist running avg
//06/28/20 chg to uint_16 to accommodate change from cm to mm
uint16_t aLeftDistMM[LR_DIST_ARRAY_SIZE];
uint16_t aRightDistMM[LR_DIST_ARRAY_SIZE];
uint16_t aRearDistMM[REAR_DIST_ARRAY_SIZE];
double Rear_Dist_PrevVar = 0;
double Rear_Dist_PrevMean = 0;

//float RearDistPrevAvg = 0;

int curMinObstacleDistance = MIN_FRONT_OBSTACLE_DIST_CM;//added 03/11/17 for chg stn avoidance

//04/13/20 moved distance vars up here so can be initialized just before loop()
//01/08/22 no longer using ISR
int glFrontDistVal = 0;
int glRearDistVal = 0;
double glFrontvar = 0;
double glRearvar = 0;

//added 10/24/20
//const int STUCK_BACKUP_DISTANCE_CM = 25;
const int STUCK_BACKUP_DISTANCE_CM = 20; //11/21/28 shortened slightly
const int STUCK_FORWARD_DISTANCE_CM = 15;
const int MAX_REAR_DISTANCE_CM = 100;
const int MIN_REAR_OBSTACLE_DIST_CM = 10;
const int STUCK_BACKUP_TIME_MSEC = 1000;
const int STUCK_FORWARD_TIME_MSEC = 1000;
#pragma endregion Distance Measurement Support

#pragma region PIN ASSIGNMENTS

//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//
//===================  START PIN ASSIGNMENTS ===================//
//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//

#pragma region MOTOR PINS
//11/04/21 Now using Pololu VNH5019 drivers for all 4 motors
const int InA_Left = 22;
const int InB_Left = 21;
const int Spd_Left = 23;

const int InA_Right = 34;
const int InB_Right = 33;
const int Spd_Right = 35;
#pragma endregion MOTOR PINS

#pragma region CHG SUPP PINS
//12/19/21 updated here, schematic, and spreadsheet
const int CHG_CONNECT_PIN = A0; //output of photo resistor looking at TP5100 CHG LED
const int BATT_MON_PIN = A1; //connected to 5VLDO regulator module
const int TOT_CURR_PIN = A2; //connected to 1NA619 between charge plug and battery
const int RUN_CURR_PIN = A3; //connected to 1NA619 between battery and rest of robot
const int CHG_CONNECT_LED_PIN = 30; //illuminates when chg cable connected

//state-of-charge indicator LEDs
const int FIN_LED_PIN = 27;
const int _80PCT_LED_PIN = 26;
const int _60PCT_LED_PIN = 25;
const int _40PCT_LED_PIN = 24;
const int _20PCT_LED_PIN = 12;
const int CHG_LED_PIN = 11;
#pragma endregion CHG SUPP PINS

//Second Deck Pins
const int RED_LASER_DIODE_PIN = 5;//Laser pointer
const int LIDAR_MODE_PIN = 2; //LIDAR MODE pin (continuous mode)
const int VL53L0X_TEENSY_RESET_PIN = 4; //pulled low for 1 mSec in Setup()

//Miscellaneous pins
const int HOMING_PID_COMPUTE_CALL_PIN = CHG_CONNECT_LED_PIN; //using same LED for two indications
//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//
//===================  END PIN ASSIGNMENTS ===================//
//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//
#pragma endregion PIN ASSIGNMENTS

//04/25/21 moved here from MoveToDesiredFront/BackDist() functions
#pragma region Front/Back Offset Motion PID
const double OffsetDistKp = 5.f;
const double OffsetDistKi = 3.f;
const double OffsetDistKd = 0.0f;
double OffsetDistSetpointCm, OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive
double OffsetDistVal = 0; //has to be 'double'

PID OffsetDistPID(&OffsetDistVal, &OffsetDistOutput, &OffsetDistSetpointCm,
  OffsetDistKp, OffsetDistKi, OffsetDistKd, DIRECT);//12/14/20 re-doing this from scratch
#pragma endregion

#pragma region CHG SUPP PARAMETERS
//02/06/21 added for charge connection debounce commented out 12/25/21
//const int CHG_CONNECT_PIN_READING_ARRAY_SIZE = 5;
//uint16_t aChgConnectReadings[CHG_CONNECT_PIN_READING_ARRAY_SIZE];
//const float CHG_CONNECTED_AVG_THRESHOLD = 0.8;
//const float CHG_DISCONNECTED_AVG_THRESHOLD = 0.2;

//12/25/21 now using analog input, so threshold scaled by analog ref voltage
const uint16_t CHG_CONNECTED_AVG_THRESHOLD = 0.2 * MAX_AD_COUNT; //integer truncation OK
const uint16_t CHG_DISCONNECTED_AVG_THRESHOLD = 0.8 * MAX_AD_COUNT;//integer truncation OK
//uint16_t ChgConnectReadingTotal;//this is the 'oldTot' in 'IsChargerConnected(bChgConnect, oldTot)'
long chgStartMsec;//added 02/24/17
//bool bChgConnect = false; //updated every MSEC_PER_LOOP Msec
float TotalAmps; //moved here from loop() 01/02/22
float RunAmps; //moved here from loop() 01/02/22
#pragma endregion CHG SUPP PARAMETERS		

#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
const int MOTOR_SPEED_CAPTURE_OFFSET = 75; //added 06/21/20 for offset capture
const int TURN_START_SPEED = MOTOR_SPEED_QTR; //added 11/14/21

//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
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 commented out 11/22/21 as now the MPU6050 module is mounted 'Z-up'
#pragma endregion MPU6050 Support

#pragma region VL53L0X_TOF_LIDAR_SUPPORT
const int ToFArray_PARALLEL_FIND_Kp = 200;   //0.50*Kp(crit)
const int ToFArray_PARALLEL_FIND_Ki = 0; //0.45*Kp(crit)
const int ToFArray_PARALLEL_FIND_Kd = 0;//0.60*Kp(crit)
const float ToFArray_PARALLEL_FIND_SETPOINT = 0.01; //09/22/20 moved here

const int ToFArray_OFFSET_CAPTURE_Kp = 200;
const int ToFArray_OFFSET_CAPTURE_Ki = 50;
const int ToFArray_OFFSET_CAPTURE_Kd = 50;
const int ToFArray_OFFSET_TRACK_Kp = 10;
const int ToFArray_OFFSET_TRACK_Kd = 0;
double LeftSteeringVal, RightSteeringVal; //added 08/06/20
double ToFSetpoint, ToFSteeringVal, ToFOutput;//10/06/17 chg input variable name to something more descriptive
PID ToFArrayPID(&ToFSteeringVal, &ToFOutput, &ToFSetpoint, ToFArray_PARALLEL_FIND_Kp, 0.0, 0.0, DIRECT);//06/19/20 use this for now
const int ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC = 200;
const float PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD = 0.1; //rev 06/21/20 - now using (F-R)/100
const float CAPTURE_APPROACH_STEERING_VALUE = 0.4; //rev 06/28/20 - now using (F-R)/100
const float WALL_OFFSET_CAPTURE_WINDOW_CM = 2.0; //just a guess
const int VL53L0X_I2C_SLAVE_ADDRESS = 0x20; ////Teensy 3.5 VL53L0X ToF LIDAR controller

//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
uint16_t Lidar_RightFront;
uint16_t Lidar_RightCenter;
uint16_t Lidar_RightRear;
uint16_t Lidar_LeftFront;
uint16_t Lidar_LeftCenter;
uint16_t Lidar_LeftRear;
uint16_t Lidar_Rear; //added 10/24/20

bool bVL53L0X_TeensyReady = false; //11/10/20 added to prevent bad data reads during Teensy setup()

elapsedMillis lastToFArrayTelemetryMsec; //used to space out telemetry prints

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
};
#pragma endregion VL53L0X ToF LIDAR Support

#pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS
elapsedMillis MsecSinceLastTurnRateUpdate;

const int MAX_SPIN_MOTOR_SPEED = 250;
const int MIN_SPIN_MOTOR_SPEED = 50;
const int TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go

double TurnRateOutput;
double TurnRateSetPoint_DPS;
double Ival = 0;
double lastErr = 0;
double lastTurnRateVal_DPS;
double TurnRateVal_DPS;
double Prev_HdgDeg = 0;
float tgt_deg;
float timeout_sec;

//11/16/21 almost perfect for 45 deg/sec, 100mSec
//11/17/21 and pretty good for 90 deg/sec too!
double TurnRate_Kp = 1.8;
double TurnRate_Ki = 1.0;
double TurnRate_Kd = 0.4;

//ported from FourWD_PulseTurnRateTest.ino
double TurnRatePIDOutput;

//06/02/
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 06/01/21
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

#pragma region OTA UPDATE
//******************************************************************************
// hex_info_t	struct for hex record and hex file info
//******************************************************************************
typedef struct {	// 
  char* data;		// pointer to array allocated elsewhere
  unsigned int addr;	// address in intel hex record
  unsigned int code;	// intel hex record type (0=data, etc.)
  unsigned int num;	// number of data bytes in intel hex record

  uint32_t base;	// base address to be added to intel hex 16-bit addr
  uint32_t min;		// min address in hex file
  uint32_t max;		// max address in hex file

  int eof;		// set true on intel hex EOF (code = 1)
  int lines;		// number of hex records received  
} hex_info_t;

void read_ascii_line(Stream* serial, char* line, int maxbytes);
int  parse_hex_line(const char* theline, char* bytes,
  unsigned int* addr, unsigned int* num, unsigned int* code);
int  process_hex_record(hex_info_t* hex);
void update_firmware(Stream* serial, uint32_t buffer_addr, uint32_t buffer_size);

uint32_t buffer_addr, buffer_size; //09/20/21 copied from FlasherX - loop()
#pragma endregion OTA UPDATE

#pragma region IR_HOMING_SUPPORT
//01/07/22 no longer using ISR
//01/10/22 in Teensy land, better to use 'float' everywhere
uint8_t IR_HOMING_MODULE_SLAVE_ADDR = 8;  //uint8_t type reqd here for Wire.requestFrom() call
double IRHomingSetpoint, IRHomingOutput;//10/06/17 chg input variable name to something more descriptive
double IRHomingLRSteeringVal = 0;

//these values all come from IR Homing Teensy via I2C calls
unsigned long IRFinalValue1 = 0;
unsigned long IRFinalValue2 = 0;
unsigned long IRHomingValTotalAvg = 0;

//04/05/21 from Z-N method, see https://www.fpaynter.com/2021/04/another-try-at-charging-station-homing-pid-tuning/
//const double IRHomingKp = 100.f;
//const double IRHomingKi = 90.f;
//const double IRHomingKd = 120.f;

//01/11/22 started over again with WallE3 and home-grown PID algorithm
//const double IRHomingKp = 100.f;
//const double IRHomingKp = 200.f; //one or two oscillations at start
//const double IRHomingKp = 250.f; //one or two oscillations at start
//const double IRHomingKp = 300.f; //defining this value as Kc for Z-N method
//const double IRHomingKi = 0.f;
//const double IRHomingKd = 0.f;
// 
// 01/15/22 The Z-N values did not work very well - LOTS of oscillation/overshoot!
//const double IRHomingKp = 150.f; //0.5Kc
//const double IRHomingKi = 135.f; //0.45Kc
//const double IRHomingKd = 150.f; //0.5Kc

//01/15/22 starting with Kp = 200, then increasing Ki by increments of 50
//const double IRHomingKp = 200.f;
////const double IRHomingKi = 50.f;
//const double IRHomingKi = 100.f; 
////const double IRHomingKd = 25.f; //too much
//const double IRHomingKd = 10.f; //better

//01/15/22 reduce Kp to 150, keeping others the same
const double IRHomingKp = 150.f;
const double IRHomingKi = 100.f;
const double IRHomingKd = 10.f; //better


//PID IRHomingPID(&IRHomingLRSteeringVal, &IRHomingOutput, &IRHomingSetpoint,
//  IRHomingKp, IRHomingKi, IRHomingKd, REVERSE);//12/14/20 re-doing this from scratch
const long IR_BEAM_DETECTION_CHANNEL_MAX = 2621440;
const long IR_BEAM_DETECTION_THRESHOLD = 15000;
const double IR_HOMING_STEERING_VAL_CAPTURE_THRESHOLD = 0.3; //added 01/30/21
const double IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD = 0.8; //added 03/30/21
const int IRHOMING_VALUE_ARRAY_SIZE = 3; //added 03/16/21 for value average support
long int aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE];//added 03/16/21 for value average support
const int IRHOMING_IAP_OFFSET_LOW_THRESHOLD_PCT = 75; //03/21/21 if wall offset is less than this relative to IAP offset, then adjust
const int IRHOMING_IAP_OFFSET_HIGH_THRESHOLD_PCT = 110; //03/23/21 if wall offset is more than this relative to IAP offset, then adjust
const double IRHOMING_IAP_FINE_TUNE_STEERING_VALUE_THRESHOLD = 0.2; //03/21/21 fine-tune if more off-boresight than this
#pragma endregion IR_HOMING_SUPPORT

#pragma region LOOP_VARS
int leftspeednum = MOTOR_SPEED_HALF;
int rightspeednum = MOTOR_SPEED_HALF;

NavCases NavCase = NAV_WALLTRK;
WallTrackingCases TrackingCase = TRACKING_NEITHER; //added 01/05/16
WallTrackingCases PrevTrackingCase = TRACKING_LEFT; //only used decide which way to turn in the TRACKING_NEITHER case  
OpModes PrevOpMode = MODE_NONE; //added 03/08/17, rev to MODE_NONE 04/04/17
OpModes CurrentOpMode = MODE_NONE; //added 10/13/17 so can use in motor speed setting routines

//04/10/20 added for experiment to port heading based wall tracking from two wheel robot
TrackingState CurrentTrackingState = TRK_RIGHT_NONE;
TrackingState PrevTrackingState = TRK_RIGHT_NONE;

//02/13/16 added for 'pause' debug
int m_FinalLeftSpeed = 0;
int m_FinalRightSpeed = 0;

//11/03/18 added for new incremental variance calc
double Front_Dist_PrevVar = 0;
double Front_Dist_PrevVMean = 0;
elapsedMillis lastHomingTelemetryMsec; //used to space out telemetry prints
#pragma endregion Loop Variables

#pragma endregion PRE_SETUP

void setup()
{
#pragma region PIN INITIALIZATION

  //init ALL pins to OUTPUT_LOW, then change as necessary
  //01/08/22 - THIS MUST BE FIRST STEP IN SETUP() (ask me how i know!)
  //InitAllPins(); 

  pinMode(LED_BUILTIN, OUTPUT);

#pragma region Teensy VL53L0X Array Init
  //11/15/20 force VL53L0X Teensy to reset whenever main controller does
  pinMode(VL53L0X_TEENSY_RESET_PIN, OUTPUT);
  digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW);
  delay(50);
  digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH);
#pragma endregion Teensy VL53L0X Array Init

#pragma region Second Deck Pins
  pinMode(RED_LASER_DIODE_PIN, OUTPUT);
  pinMode(LIDAR_MODE_PIN, INPUT);
  pinMode(VL53L0X_TEENSY_RESET_PIN, OUTPUT);
#pragma endregion Second Deck Pins

#pragma region Charge_Support_Pins
  //current sensor pins
  pinMode(RUN_CURR_PIN, INPUT); //02/24/19 now connected to 'Run Current' 1NA619 charge current sensor
  digitalWrite(RUN_CURR_PIN, LOW); //turn off the internal pullup resistor

  pinMode(TOT_CURR_PIN, INPUT);//02/24/19 now connected to 'Total Current' 1NA619 charge current sensor
  digitalWrite(TOT_CURR_PIN, LOW); //turn off the internal pullup resistor

  //Battery Voltage Monitor pin
  pinMode(BATT_MON_PIN, INPUT);
  digitalWrite(BATT_MON_PIN, LOW); //turn off the internal pullup resistor

  //charge connect
  pinMode(CHG_CONNECT_PIN, INPUT_PULLUP);  //goes LOW when chg cable connected
  digitalWrite(CHG_CONNECT_PIN, HIGH); //01/09/22 needed now that InitAllPins() forces it LOW

  //charge connect status display pin (this will eventually be one of the LEDs on the rear LED panel)
  pinMode(CHG_CONNECT_LED_PIN, OUTPUT);  //12/16/20 lights LED when chg cable connected
  digitalWrite(CHG_CONNECT_LED_PIN, HIGH); //01/09/22 needed now that InitAllPins() forces it LOW

  //Charge status LED en/dis pins
  pinMode(_60PCT_LED_PIN, OUTPUT);
  pinMode(FIN_LED_PIN, OUTPUT);
  pinMode(_80PCT_LED_PIN, OUTPUT);
  pinMode(_40PCT_LED_PIN, OUTPUT);
  pinMode(_20PCT_LED_PIN, OUTPUT);
#pragma endregion Charge_Support_Pins

#pragma region Motor_Pins
  //motor pins
  pinMode(InA_Left, OUTPUT);
  pinMode(InB_Left, OUTPUT);
  pinMode(Spd_Left, OUTPUT);

  pinMode(InA_Right, OUTPUT);
  pinMode(InB_Right, OUTPUT);
  pinMode(Spd_Right, OUTPUT);
#pragma endregion Motor_Pins

#pragma endregion PIN INITIALIZATION

#pragma region SERIAL_PORTS
  Serial.begin(115200);
  delay(2000); //10/06/21 - just use fixed delay instead

  Serial1.begin(115200); //used HC-05 'AT' commands to set this speed
  delay(2000); //11/20/21 use fixed delay instead of waiting

  //I2C bus
  //Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT); //MPU6050,IR_DET. MPU6050 has 4.7K pullups
  //Wire1.begin(I2C_MASTER, 0x00, I2C_PINS_37_38, I2C_PULLUP_EXT, 400000);//VL53L0X Array
  Wire.begin();
  Wire1.begin();
  

  pinMode(LED_BUILTIN, OUTPUT);

  //DEBUG!!
  if (Serial)
  {
    Serial.printf("Serial = %p, Serial1 = %p\n", &Serial, &Serial1);
  }
  //DEBUG!!

  //DEBUG!!
  if (Serial1)
  {
    Serial1.printf("Serial = %p, Serial1 = %p\n", &Serial, &Serial1);
  }
  //DEBUG!!

#pragma endregion SERIAL_PORTS

  CheckForUserInput(); //01/13/22 - here so OTA procedure can maybe start faster

#pragma region MPU6050
#ifndef NO_MPU6050
  myTeePrint.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);
  myTeePrint.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  mpu.initialize();

  // verify connection

  float StartSec = 0; //used to time MPU6050 init
  myTeePrint.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
    myTeePrint.printf(F("Enabling DMP...\n"));
    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..."));
    Serial1.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();

    myTeePrint.printf(F("Calibrating...Retrieving Calibration Values\n\n"));
    mpu.CalibrateGyro(); //using default value of 15
    mpu.PrintActiveOffsets();

    //loop heading display until stabilized
    myTeePrint.printf(F("\nMsec\tHdg\n"));

    UpdateIMUHdgValDeg();
    Prev_HdgDeg = IMUHdgValDeg;
    delay(100);
    UpdateIMUHdgValDeg();

    myTeePrint.printf("%lu\t%2.3f\t%2.3f\n", millis(), IMUHdgValDeg, Prev_HdgDeg);
    while (abs(IMUHdgValDeg - Prev_HdgDeg) > 0.1f)
    {
      myTeePrint.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg);
      Prev_HdgDeg = IMUHdgValDeg;
      delay(100);
      UpdateIMUHdgValDeg();
    }

    StartSec = millis() / 1000.f;
    myTeePrint.printf("MPU6050 Ready at %2.2f Sec with delta = %2.3f\n", StartSec, IMUHdgValDeg - Prev_HdgDeg);
    bMPU6050Ready = true;

    delay(1000);
  }
  else //MPU6050 Init failed for some reason
  {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    myTeePrint.printf("DMP Initialization failed with code %d", devStatus);

    //08/29/21 print out battery voltage on failure
    float batV = GetBattVoltage();
    myTeePrint.printf("Battery Voltage = %2.2f\n", batV);

    bMPU6050Ready = false;
  }
#endif // !NO_MPU6050
#pragma endregion MPU6050

  CheckForUserInput(); //01/13/22 - here so OTA procedure can maybe start faster

#pragma region VL53L0X_TEENSY

  myTeePrint.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);
    Wire1.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);
    //bVL53L0X_TeensyReady = (Wire.endTransmission() == 0);
    bVL53L0X_TeensyReady = (Wire1.endTransmission() == 0);

    //mySerial.printf("%lu: VL53L0X Teensy Not Avail...\n", millis());
    delay(100);
  }

  myTeePrint.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

#ifdef DISTANCES_ONLY
  digitalWrite(RED_LASER_DIODE_PIN, HIGH); //enable the front laser dot
  myTeePrint.printf("\n------------ DISTANCES ONLY MODE!!! -----------------\n\n");

  int i = 0; //added 09/20/20 for in-line header display
  myTeePrint.printf("Msec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\tRear\n");
  while (true)
  {
    if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)
    {

      //just in case there is a long delay somewhere that allows MsecSinceLastDistUpdate to build up
      while (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)
      {
        MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;
      }

      //09/20/20 re-display the column headers
      if (++i % 20 == 0)
      {
        myTeePrint.printf("\nMsec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\tRear\n");
      }

      GetRequestedVL53l0xValues(VL53L0X_ALL);

      myTeePrint.printf("%lu\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, Lidar_Rear);
    }
  }
#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
  myTeePrint.println(F("Initializing Left, Right, Front Distance Arrays..."));
#ifndef NO_VL53L0X
  //03/30/21 moved FrontDistArray init ahead of left/right dist init to prevent inadvertent 'stuck' detection
  myTeePrint.println(F("Initializing Front Distance Array"));
  InitFrontDistArray(); //08/12/20 code extracted to fcn so can call elsewhere

  myTeePrint.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
    myTeePrint.printf("%d\t%d\n", Lidar_LeftCenter, Lidar_RightCenter);
    UpdateLRDistanceArrays(Lidar_LeftCenter, Lidar_RightCenter);
  }

  myTeePrint.println(F("Updating Rear Distance Arrays"));
  InitRearDistArray();
  myTeePrint.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_VL53L0X
#pragma endregion L/R/FRONT DISTANCE ARRAYS

#pragma region IRDET_TEENSY
#ifndef NO_IRDET
  bool IRDET_TeensyOK = false;
  myTeePrint.printf("\nChecking for Teensy 3.2 IRDET Controller at I2C addr 0x%x\n", IR_HOMING_MODULE_SLAVE_ADDR);
  while (!IRDET_TeensyOK)
  {
    Wire.beginTransmission(IR_HOMING_MODULE_SLAVE_ADDR);
    IRDET_TeensyOK = (Wire.endTransmission() == 0);

    myTeePrint.printf("%lu: IRDET Teensy Not Avail...\n", millis());
    delay(100);
  }

  myTeePrint.printf("IRDET Teensy Ready at %lu\n", millis());

  //get initial values
  long Fin1, Fin2;//04/05/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)
  float SteeringValue;

  Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, sizeof(Fin1) + sizeof(Fin2) + sizeof(SteeringValue));

  I2C_readAnything(Fin1);
  I2C_readAnything(Fin2);
  I2C_readAnything(SteeringValue);

  myTeePrint.printf("Fin1/Fin2/SteeringVal = %ld\t%ld\t%2.4f\n", Fin1, Fin2, SteeringValue);
#endif // !NO_IRDET
#pragma endregion IRDET_TEENSY

  CheckForUserInput(); //01/13/22 - here so OTA procedure can maybe start faster

#ifdef IR_HOMING_ONLY
  //myTeePrint.printf("Msec\tValAvg\tSteer\t\tRRear\tRCtr\tRFront\tFront\n");
  myTeePrint.printf("Msec\tValAvg\tSteer\n");
  MsecSinceLastIRHomingAdj = 0;

  while (true)
  {
    CheckForUserInput();

    if (MsecSinceLastIRHomingAdj >= MSEC_PER_IR_HOMING_ADJ)
    {
      MsecSinceLastIRHomingAdj -= MSEC_PER_IR_HOMING_ADJ;
      UpdateIRHomingValues();
      //myTeePrint.printf("%lu\t%lu\t%2.2f\t\t%d\t%d\t%d\t%d\n",
      //  millis(), IRHomingValTotalAvg, IRHomingLRSteeringVal, Lidar_RightRear, Lidar_RightCenter, Lidar_RightFront, glFrontDistVal);
      myTeePrint.printf("%lu\t%lu\t%lu\t%lu\t%2.2f\n",
        millis(), IRFinalValue1, IRFinalValue2, IRHomingValTotalAvg, IRHomingLRSteeringVal);
    }

    IRHomeToChgStnNoPings(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
  }

#endif

#pragma region IR_BEAM_VALUE_AVG_ARRAY
  //03/17/21 added for IR beam value averaging array init
  Serial.printf("%lu: Initializing IR Beam Value Averaging Array...\n", millis());
  for (size_t i = 0; i < IRHOMING_VALUE_ARRAY_SIZE; i++)
  {
    aIRHOMINGVALTOTALS[i] = 0;
  }
  IRHomingValTotalAvg = 0;

#pragma endregion IR_BEAM_VALUE_AVG_ARRAY

  analogReadAveraging(8); //applies to all analogRead() operations?

  myTeePrint.printf("Msec\tBattV\tTotAmps\tRunAmps\tHdgDeg\tChgConn\n");
  MsecSinceLastOpModeChk = 0; //main opMode update timer
}

void loop()
{
  if (Serial1.available() || Serial.available())
  {
    CheckForUserInput();
  }

  if (MsecSinceLastOpModeChk >= MSEC_PER_OP_MODE_CHK)
  {
    MsecSinceLastOpModeChk -= MSEC_PER_OP_MODE_CHK;
    digitalToggle(LED_BUILTIN);
    digitalWrite(RED_LASER_DIODE_PIN, HIGH); //fire the laser pointer

  //Step1: Determine current operating mode
    CurrentOpMode = (OpModes)GetOpMode();
    String trackstr = "Unknown"; //used for telemetry printouts

  //Step2: Switch to appropriate operating case block
#pragma region OP_MODE SWITCH
    switch (CurrentOpMode)
    {
    case MODE_DEADBATTERY:
      break;
    case MODE_DISCHARGE:
      break;
#pragma region MODE_CHARGING
    case MODE_CHARGING:
      digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
      chgStartMsec = millis(); //used for charge duration telemetry
      myTeePrint.print(F("Starting Charge Mode at ")); Serial.println(chgStartMsec);
      StopBothMotors(); //added 03/13/17

      //04/27/21
#ifndef NO_VL53L0X
      myTeePrint.printf("Reset VL53L0X Teensy to reduce current consumption\n");
      digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption
#endif
      MonitorChargeUntilDone();
#ifndef NO_VL53L0X
      myTeePrint.printf("Restart VL53L0X Teensy...\n");
      digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy
      WaitForVL53L0XTeensy(); //wait for it to come back up.
#endif
      ExecDisconManeuver();
      PrevOpMode = MODE_NONE; //04/27/21: this will force wall offset recapture
      //myTeePrint.printf("%lu: just after ExecDisconManeuver() in TRACKING_RIGHT case block\n", millis());
      break;
#pragma endregion MODE_CHARGING
#pragma region MODE_IRHOMING
    case MODE_IRHOMING:
      digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
      { //brackets required to set the scope of the new variable (float batVoltage)
        float batVoltage = GetBattVoltage();
        myTeePrint.printf("IR Beam Detected with Battery Voltage = %1.2f\n", batVoltage);
        //PrevOpMode = MODE_IRHOMING;

        if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry
        {
          Serial.println("Not Hungry - Avoiding Charger");

          //04/27/21 too complicated - just have robot reverse course
          int leftdist = GetAvgLeftDistCm();
          int rightdist = GetAvgRightDistCm();
          SpinTurn(leftdist > rightdist, 180, 45);
          PrevOpMode = MODE_NONE;
        }
        else //hungry - try to connect
        {
          Serial.println("Low battery - homing to Charger");
          //IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
        }
      } //brackets required to set the scope of the new variable (float batVoltage)      
      break;
#pragma endregion MODE_IRHOMING

#pragma region MODE_WALLCAPTURE
    case MODE_WALLCAPTURE:
      break;
#pragma endregion MODE_WALLCAPTURE

#pragma region MODE_WALLFOLLOW
    case MODE_WALLFOLLOW:
      break;
#pragma ENDregion MODE_WALLFOLLOW
    case MODE_NONE:
      break;
    }

    //digitalWrite(CHG_CONNECT_LED_PIN, !bChgConnect); //LOW output illuminates LED

    //if (bChgConnect)
    //{
    //  MonitorChargeUntilDone();
    //  MsecSinceLastLEDToggle = 0; //reset so don't get hundreds of lines of output after long charge

    //  //01/02/22 have to re-print the headings with leading blank line after charging completes
    //  myTeePrint.printf("\nMsec\tBattV\tTotAmps\tRunAmps\tHdgDeg\tChgConn\n");
    //}

    ////Get battery voltage
    //float calc_volts = GetBattVoltage();

    ////get current heading
    //UpdateIMUHdgValDeg(); //updates IMUHdgValDeg

    ////update IR homing/steering values
    ////Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, sizeof(IRFinalValue1) + sizeof(IRFinalValue2) + sizeof(IRHomingLRSteeringVal));
    //Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, (size_t)(sizeof(IRFinalValue1) + sizeof(IRFinalValue2) + sizeof(IRHomingLRSteeringVal)));
    //I2C_readAnything(IRFinalValue1);
    //I2C_readAnything(IRFinalValue2);
    //I2C_readAnything(IRHomingLRSteeringVal);
    //IRHomingValTotalAvg = UpdateIRHomingValueTotalAverage(IRFinalValue1, IRFinalValue2, IRHomingValTotalAvg);

    ////print out battery voltage, currents, and chgr connection state
    //myTeePrint.printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\n", millis(), calc_volts, TotalAmps, RunAmps, IMUHdgValDeg, bChgConnect);
  }
}

#pragma region CHARGE SUPPORT FUNCTIONS
float GetAmps(int pin_number)
{
  //Purpose:  Get current in amps
  //Inputs: 
  // pin_number = integer denoting analog pin to be used for measurement
  //	VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio
  // MAX_AD_COUNT = int denoting max A/D reading value
  // VOLTAGE_TO_CURRENT_RATIO = int denoting conversion ratio
  //Outputs:
  //	returns total robot current (chg current plus running current)
  //Notes:
  //	02/28/18 chg name from GetBattChgAmps() to GetTotalAmps()
  //  11/24/21 chg name, add pin_number param so can use for both Itot & Irun

  int reading = analogRead(pin_number); //range is 0-1023
  float volts = ((float)reading / (float)MAX_AD_COUNT) * ADC_REF_VOLTS;
  float amps = volts * VOLTAGE_TO_CURRENT_RATIO;

  //DEBUG!!
    //myTeePrint.printf("GetAmps(): reading, volts, amps = %d, %3.2f, %3.2f\n", 
    //  reading, volts, amps);
  //DEBUG!!

  return amps;
}

bool IsStillCharging()
{
  //Purpose:  Determine battery charge status
  //Inputs: 
  //	Battry charging current in amps from GetBattChgAmps()
  //	Battery voltage from GetBattV()
  //Outputs:
  //	returns TRUE if battery voltage is below full charge voltage threshold 
  //	AND charging current is above full charge current threshold.  Otherwise returns FALSE

  float BattV = GetBattVoltage();
  float TotI = GetAmps(TOT_CURR_PIN);
  float RunI = GetAmps(RUN_CURR_PIN);

  //DEBUG!!
    //myTeePrint.printf("IsStillCharging(): BattV = %2.3f, TotI = %2.3f, RunI = %2.3f\n", BattV, TotI, RunI); 

  //DEBUG!!

  return (BattV < FULL_BATT_VOLTS&& TotI - RunI > FULL_BATT_CURRENT_THRESHOLD);
}

bool IsChargerConnected(bool curState)
{
  //Purpose: Determine if robot has connected to charger
  //Inputs:  
    //oldTot = previous readings total
    //curState = current value of bChgConnected
  //Outputs:
  //	true when array total <= CHG_CONNECTED_AVG_THRESHOLD
  //	false when array total >= CHG_DISCONNECTED_AVG_THRESHOLD
  //	otherwise maintains previous state
  //Notes:
  //  12/25/21 - now pin is connected to voltage divider.  Goes LOW when charging
  //  12/25/21 - now using AnalogReadAveraging(8) everywhere, so no need to do running avg

  bool retStatus = curState;
  uint16_t  adval = (uint16_t)analogRead(CHG_CONNECT_PIN);

  if (adval <= CHG_CONNECTED_AVG_THRESHOLD) //low means 'connected'
  {
    retStatus = true;
  }
  else if (adval >= CHG_DISCONNECTED_AVG_THRESHOLD)
  {
    retStatus = false;
  }

  //myTeePrint.printf("%lu: curState = %d, adval = %d, ConThresh = %d, disConThresh = %d retsatus = %d\n",
  //  millis(), curState, adval, CHG_CONNECTED_AVG_THRESHOLD, CHG_DISCONNECTED_AVG_THRESHOLD, retStatus);
  //else return previous conn/disconnect state
  return retStatus;
}

bool MonitorChargeUntilDone()
{
  //Purpose:  Monitor charging status until charge is complete
  //Inputs: startMsec = millis() at the time of the function call
  //Outputs: 
  //	returns TRUE if charging completes successfully, FALSE otherwise
  //	provides mode-specific telemetry to PC via Wixel
  //Plan:
  //	Step1: Blink charger display LEDs
  //	Step1: Get current time check for sufficiently elapsed time
  //	Step2: Get charger status signals, and echo them to display LEDs
  //	Step2: Send telemetry to PC via Serial port (Wixel)
  //	Step2: Check for end-of-charge or failure (don't know what this would be yet...)
  //Notes:
  // 03/11/17 for testing, rev to return as soon as connection dropped
  // 05/21/17 rev to xmit telemetry before loop & then delay a bit before entering loop
  // 05/21/17 abstracted status reporting code to separate function
  // 10/16/17 removed startMsec from call sig
  //	03/15/18 revised for TP5100 charger module
  //	04/01/18 rev to always stay on charge for at least MINIMUM_CHARGE_TIME_SEC sec
  //	02/24/19 rev to use new 1NA169 current sensor output
  //	02/28/19 moved ChargeTelemetryString printout here from MODE_CHARGING case
  //	01/30/21 added rear distance readout for debugging zero distance problem
  //	02/06/21 repl bChgConn with ISR-managed bChgConnect
  //	04/02/21 added code to blink LED associated with current charge level
  //  01/02/22 ported to Wall-E3

//Step1: Get current time & check for sufficient elapsed time
  int ElapsedChgTimeSec = 0;
  float ElapsedChgTimeMin = 0; //added 05/02/20

  myTeePrint.println(ChargingTelemStr);  //moved here from main loop MODE_CHARGING case

  bool bStillCharging = true;
  bool bChgConnect = true; //01/08/22 no longer using ISR
  //EnableAllRearLEDs(false);

  while (ElapsedChgTimeSec < MINIMUM_CHARGE_TIME_SEC ||
    (bStillCharging
      && ElapsedChgTimeSec < BATT_CHG_TIMEOUT_SEC
      && bChgConnect)
    )
  {
    //04/02/21 moved to 'fast' part of loop
    float BattV = GetBattVoltage();
    float TotI = GetAmps(TOT_CURR_PIN);
    float RunI = GetAmps(RUN_CURR_PIN);
    //UpdateChgStatusLEDs(BattV, bStillCharging); //updates 'fuel guage' LEDs 04/22/20 added bStillCharging to sig

    //05/02/20 rev to only print out 10 times/min
    if (ElapsedChgTimeSec % 6 == 0)
    {
      ElapsedChgTimeMin = (float)ElapsedChgTimeSec / 60.f;

      myTeePrint.printf("%3.1f\t%2.4f\t%2.4f\t%2.4f\t%2.4f\n",
        ElapsedChgTimeMin, BattV, TotI, RunI, TotI - RunI); //rev 02/24/19 for 1Na169 sensor
    }

    CheckForUserInput(); //added 04/02/21
    delay(1000); //one-second loop
    ElapsedChgTimeSec++;
    bStillCharging = IsStillCharging(); //02/24/19 - now using 1NA169 current sensor
    bChgConnect = IsChargerConnected(bChgConnect); //01/02/22 - wasn't being checked
  }

  //Step2: Check for end-of-charge or failure (don't know what this would be yet...)
    //if charging ran over time, something went wrong

  time_t t = now();
  if (!bChgConnect) //charger unplugged
  {
    Serial.printf("Charge connection dropped after %2.2f minutes at %d:%d:%d elapsed time\n", (float)(ElapsedChgTimeSec / 60.), hour(t), minute(t), second(t));
    myTeePrint.printf("Charge connection dropped after %2.2f minutes at %d:%d:%d elapsed time\n", (float)(ElapsedChgTimeSec / 60.), hour(t), minute(t), second(t));
    return false;
  }
  else if (ElapsedChgTimeSec < BATT_CHG_TIMEOUT_SEC)
  {
    myTeePrint.printf("Charging Completed Successfully in %2.2f minutes at %d:%d:%d elapsed time\n", (float)(ElapsedChgTimeSec / 60.), hour(t), minute(t), second(t));
    return true;
  }
  else
  {
    myTeePrint.printf("Charging timout value of %2.2f minutes expired at\n", (float)(BATT_CHG_TIMEOUT_SEC / 60.), hour(t), minute(t), second(t));
    return false;
  }
}

float GetBattVoltage()
{
  //02/18/17 get corrected battery voltage.  Voltage reading is 1/3 actual Vbatt value
  int analog_batt_reading = analogRead(BATT_MON_PIN);//analogReadAveraging(8) in setup() does internal averaging
  float calc_volts = ZENER_VOLTAGE_OFFSET + ADC_REF_VOLTS * (double)analog_batt_reading / (double)MAX_AD_COUNT;

  //DEBUG!!
    //myTeePrint.printf("a/d = %d, calc = %2.2f\n", analog_batt_reading,calc_volts);
  //DEBUG!!

  return calc_volts;
}

bool ExecDisconManeuver()
{
  //Purpose:  Disconnect from charging station
  //Inputs: Call from Charging Mode case block
  //Outputs: Robot disconnects from charging station, backs up, and turns 90 away from near wall
  //Plan:
  //	Step1: Turn OFF charger status LEDs (added 04/28/17)
  //	Step2: Determine which side wall is closer
  //	Step3: Back straight up for long enough to clear side rails
  //	Step4: Turn 90 away from near side wall
  //Notes:
  //	02/15/18 rev to use full speed to disengage, and new rolling turn routines
  //	03/27/18 rev for TP5100 charging module

  float batv = GetBattVoltage();
  myTeePrint.printf("in ExecDisconManeuver() with BattV = %2.4f\n", batv);


  //Step1: Turn OFF charger status LEDs (added 04/28/17)
    //chg status LEDs are all enabled via a LOW digital output
    //03/15/18 rev for TP5100 
  digitalWrite(FIN_LED_PIN, HIGH); //output is LOW (active) when Pw1_IN is HIGH/TRUE
  digitalWrite(_80PCT_LED_PIN, HIGH); //output is LOW (active) when Pw2_IN is HIGH/TRUE
  digitalWrite(_60PCT_LED_PIN, HIGH); //output is LOW (active) when Chg1_IN is LOW/FALSE
  digitalWrite(_40PCT_LED_PIN, HIGH); //output is LOW (active) when Chg2_IN is LOW/FALSE
  digitalWrite(_20PCT_LED_PIN, HIGH); //output is LOW (active) when Fin1_IN is LOW/FALSE
  digitalWrite(CHG_LED_PIN, HIGH); //output is LOW (active) when Fin2_IN is LOW/FALSE

//Step2: Determine which side wall is closer.  Ping sensors on 2nd deck can see over charger side rails
  int leftdist = GetAvgLeftDistCm();
  int rightdist = GetAvgRightDistCm();
  Serial.print("leftdist = "); Serial.print(leftdist); Serial.print(", ");
  Serial.print("rightdist = "); Serial.println(rightdist);

  //Step3: Back straight up for long enough to clear side rails
#ifndef NO_VL53L0X
  MoveToDesiredFrontDistCm(70); //70cm is plenty to clear the guide rails
#endif

  StopBothMotors();
  delay(1000);

  //Step4: Turn around and go the other way
  //SpinTurn(true, 180, 30); //slightly higher rate than default 20dps
  SpinTurn(leftdist > rightdist, 180, 30); //slightly higher rate than default 20dps

  return true; //can't think of anything else at the moment
}
#pragma endregion CHARGE SUPPORT FUNCTIONS

#pragma region DISTANCE_MEASUREMENT_SUPPORT

//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!!
    //myTeePrint.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;
  myTeePrint.printf("%lu: aFrontDist Init: Front_Dist_PrevVMean = %3.2f, Front_Dist_PrevVar = %3.2f\n", millis(), Front_Dist_PrevVMean, Front_Dist_PrevVar);
  glFrontvar = Front_Dist_PrevVar; //added 03/30/21 to prevent 'stuck' at startup
}

//08/12/20 Extracted inline FRONT_DIST_ARRAY init code so can be called from anywhere
void InitRearDistArray()
{
  //Purpose:  initialize rear distance array and initial values for mean and variance
  //Inputs:
  // aRearDistMM = array of rear distance values
  // REAR_DIST_ARRAY_SIZE = number of rear distance values stored
  //Outputs: 
  //	aRearDistMM = array of rear distance values
  //	Rear_Dist_PrevMean = previous mean value initialized
  //	Rear_Dist_PrevVar = previous variance value initialized
  //Notes:
  //  01/08/22 no longer using ISR

  int idx = 0;
  while (idx < REAR_DIST_ARRAY_SIZE)
  {
    GetRequestedVL53l0xValues(VL53L0X_REAR_ONLY);
    aRearDistMM[idx] = Lidar_Rear;
    idx++;
    delay(100);
  }

  //04/19/19 init Rear_Dist_PrevMean  & Rear_Dist_PrevVar to mean/var respectively
  long sum = 0;
  for (int i = 0; i < REAR_DIST_ARRAY_SIZE; i++)
  {
    sum += aRearDistMM[i]; //adds in rest of values
  }
  Rear_Dist_PrevMean = (float)sum / (float)REAR_DIST_ARRAY_SIZE;

  //	Step2: calc new 'brute force' variance
  float sumsquares = 0;
  for (int i = 0; i < REAR_DIST_ARRAY_SIZE; i++)
  {
    sumsquares += (aRearDistMM[i] - Rear_Dist_PrevMean) * (aRearDistMM[i] - Rear_Dist_PrevMean);
  }

  Rear_Dist_PrevVar = sumsquares / REAR_DIST_ARRAY_SIZE;
  myTeePrint.printf("%lu: aRearDistMM Init: Rear_Dist_PrevMean = %3.2f, Rear_Dist_PrevVar = %3.2f\n", millis(), Rear_Dist_PrevMean, Rear_Dist_PrevVar);
}

//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!!
    //myTeePrint.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!!
  //myTeePrint.printf("avgdisttot = %d\n", avgdist);
  //DEBUG!!

  avgdist = (int)((float)avgdist / (float)FRONT_DIST_AVG_WINDOW_SIZE);
  //DEBUG!!
  //myTeePrint.printf("avgdist = %d\n", avgdist);
  //DEBUG!!
  return avgdist;
}

float GetAvgRightDistCm()
{
  //Notes:
  //	04/09/20 revised to compute proper running average of
  //		latest LR_AVG_WINDOW_SIZE ping measurements
  //	10/08/20 aRight/LeftDist arrays now contain mm vs cm.  rev to convert mm to cm on exit

  //int rightavgdist_cm = 0;
  int rightavgdist_mm = 0;
  for (int validx = 0; validx < LR_AVG_WINDOW_SIZE; validx++)
  {
    //rightavgdist_cm += aRightDistMM[LR_DIST_ARRAY_SIZE - 1 - validx];
    rightavgdist_mm += aRightDistMM[LR_DIST_ARRAY_SIZE - 1 - validx];
  }

  //float avg = (float)rightavgdist_cm / (float)LR_AVG_WINDOW_SIZE;
  float avg = (float)rightavgdist_mm / (float)LR_AVG_WINDOW_SIZE;
  //return avg;
  return avg / 10.f;
}

float GetAvgLeftDistCm()
{
  //Notes:
  //	04/09/20 revised to compute proper running average of
  //		latest LR_AVG_WINDOW_SIZE ping measurements

  //int leftavgdist_cm = 0;
  int leftavgdist_mm = 0;
  for (int validx = 0; validx < LR_AVG_WINDOW_SIZE; validx++)
  {
    //leftavgdist_cm += aLeftDistMM[LR_DIST_ARRAY_SIZE - 1 - validx];
    leftavgdist_mm += aLeftDistMM[LR_DIST_ARRAY_SIZE - 1 - validx];
  }

  //float avg = (float)leftavgdist_cm / (float)LR_AVG_WINDOW_SIZE;
  float avg = (float)leftavgdist_mm / (float)LR_AVG_WINDOW_SIZE;
  //return avg;
  return avg / 10.f;
}

//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();
    myTeePrint.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
}

double  CalcFrontDistArrayVariance(unsigned long newdistval)
{
  //Purpose:  Calculate Variance of input array
  //Inputs: aDistArray = FRONT_DIST_ARRAY_SIZE array of integers representing left/right/front distances
  //Outputs: Variance of selected array
  //Plan:
  //	Step1: Calculate mean for array
  //	Step2: Sum up squared deviation of each array item from mean
  //	Step3: Divide squared deviation sum by number of array elements
  //Notes:
  //	11/01/18 this function takes about 1.8mSec - small compared to 200mSec loop interval
  //	11/02/18 added distval to sig to facilitate incremental calc algorithm
  //	11/12/18 re-wrote incr alg 
  //		see C:\Users\Frank\Documents\Arduino\FourWD_WallE2_V1\Variance.xlsm
  //		and C:\Users\Frank\Documents\Arduino\VarianceCalcTest.ino
  //	01/16/19 added 'return inc_var'
  //	04/21/19 copied number overflow corrections from VarianceCalcTest.ino
  //	04/28/19 commented out the 'brute force' sections - now using incr var exclusively


  //unsigned long funcStartMicrosec = micros();

  //11/03/18 update distance array, saving oldest for later use in incremental calcs
  unsigned long oldestDistVal = aFrontDist[0];
  for (int i = 0; i < FRONT_DIST_ARRAY_SIZE - 1; i++)
  {
    aFrontDist[i] = aFrontDist[i + 1];
  }
  aFrontDist[FRONT_DIST_ARRAY_SIZE - 1] = newdistval;

  //11/02/18 now re-do the calculation using the incremental method, and compare the times
  //mu_t = mu_(t-1) - dist_(t-N)/N + dist_t/N
  //Example: mu_7 = mu_(6) - dist_(2)/N + dist_7/N

  //var^2_t = var^2_(t-1) + dist^2_(t) - dist^2_(t-N) + mu^2_(t-1) - mu^2_t
  //Example: var^2_7 = var^2_(6) + dist^2_(7) - dist^2_(t-N) + mu^2_(6) - mu^2_7

//DEBUG!!
  //for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++)
  //{
  //	Serial.print("aDistArray["); Serial.print(i); Serial.print("] = "); Serial.println(aDistArray[i]);
  //}
//DEBUG!!

  double inc_mean = Front_Dist_PrevVMean - (double)oldestDistVal / (double)FRONT_DIST_ARRAY_SIZE + (double)newdistval / (double)FRONT_DIST_ARRAY_SIZE;

  unsigned long olddist_squared = oldestDistVal * oldestDistVal;
  unsigned long newdist_squared = newdistval * newdistval;

  double last_incmean_squared = Front_Dist_PrevVMean * Front_Dist_PrevVMean;
  double inc_mean_squared = inc_mean * inc_mean;

  double inc_var = Front_Dist_PrevVar + ((double)newdist_squared / FRONT_DIST_ARRAY_SIZE) - ((double)olddist_squared / FRONT_DIST_ARRAY_SIZE)
    + last_incmean_squared - inc_mean_squared;

  //long uSecI = micros() - funcStartMicrosec - uSecB;

//DEBUG!!
  //display results:
  //myTeePrint.printf("%lu\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\n", millis(),
  //	newdistval, oldestDistVal, Front_Dist_PrevVMean, Front_Dist_PrevVar, inc_var);
//DEBUG!!

  Front_Dist_PrevVar = inc_var; //save for next time
  Front_Dist_PrevVMean = inc_mean; //save for next time
  return inc_var; //added  01/16/19
}

double  CalcRearDistArrayVariance(unsigned long newdistval)
{
  //Purpose:  Calculate Variance of input array
  //Inputs: aDistArray = FRONT_DIST_ARRAY_SIZE array of integers representing left/right/front distances
  //Outputs: Variance of selected array
  //Plan:
  //	Step1: Calculate mean for array
  //	Step2: Sum up squared deviation of each array item from mean
  //	Step3: Divide squared deviation sum by number of array elements
  //Notes:
  //	11/01/18 this function takes about 1.8mSec - small compared to 200mSec loop interval
  //	11/02/18 added distval to sig to facilitate incremental calc algorithm
  //	11/12/18 re-wrote incr alg 
  //		see C:\Users\Frank\Documents\Arduino\FourWD_WallE2_V1\Variance.xlsm
  //		and C:\Users\Frank\Documents\Arduino\VarianceCalcTest.ino
  //	01/16/19 added 'return inc_var'
  //	04/21/19 copied number overflow corrections from VarianceCalcTest.ino
  //	04/28/19 commented out the 'brute force' sections - now using incr var exclusively


  //unsigned long funcStartMicrosec = micros();

//DEBUG
  //myTeePrint.printf("in CalcRearDistArryVariance(%d, %2.2f, %2.2f)\n", newdistval, Rear_Dist_PrevMean, Rear_Dist_PrevVar);
//DEBUG

  //11/03/18 update distance array, saving oldest for later use in incremental calcs
  unsigned long oldestDistVal = aRearDistMM[0];
  for (int i = 0; i < REAR_DIST_ARRAY_SIZE - 1; i++)
  {
    aRearDistMM[i] = aRearDistMM[i + 1];
  }
  aRearDistMM[REAR_DIST_ARRAY_SIZE - 1] = newdistval;

  //11/02/18 now re-do the calculation using the incremental method, and compare the times
  //mu_t = mu_(t-1) - dist_(t-N)/N + dist_t/N
  //Example: mu_7 = mu_(6) - dist_(2)/N + dist_7/N

  //var^2_t = var^2_(t-1) + dist^2_(t) - dist^2_(t-N) + mu^2_(t-1) - mu^2_t
  //Example: var^2_7 = var^2_(6) + dist^2_(7) - dist^2_(t-N) + mu^2_(6) - mu^2_7

//DEBUG!!
  //for (int i = 0; i < REAR_DIST_ARRAY_SIZE; i++)
  //{
  //	Serial.print("aRearDistMM["); Serial.print(i); Serial.print("] = "); Serial.println(aRearDistMM[i]);
  //}
//DEBUG!!

  double inc_mean = Rear_Dist_PrevMean - (double)oldestDistVal / (double)REAR_DIST_ARRAY_SIZE + (double)newdistval / (double)REAR_DIST_ARRAY_SIZE;

  //DEBUG
    //myTeePrint.printf("inc_mean = %2.2f, ", inc_mean);
  //DEBUG

  unsigned long olddist_squared = oldestDistVal * oldestDistVal;
  unsigned long newdist_squared = newdistval * newdistval;

  double last_incmean_squared = Rear_Dist_PrevMean * Rear_Dist_PrevMean;
  double inc_mean_squared = inc_mean * inc_mean;

  //myTeePrint.printf("last_incmean_squared = %2.2f\n", last_incmean_squared);
  //myTeePrint.printf("inc_mean_squared = %2.2f\n", inc_mean_squared);
  //myTeePrint.printf("olddist_squared = %lu\n", olddist_squared);
  //myTeePrint.printf("newdist_squared = %lu\n", newdist_squared);
  //myTeePrint.printf("Rear_Dist_PrevVar = %2.2f\n", Rear_Dist_PrevVar);
  double inc_var = Rear_Dist_PrevVar + ((double)newdist_squared / REAR_DIST_ARRAY_SIZE) - ((double)olddist_squared / REAR_DIST_ARRAY_SIZE)
    + last_incmean_squared - inc_mean_squared;


  //DEBUG
    //myTeePrint.printf("inc_var = %2.2f\n", inc_var);
  //DEBUG

    //long uSecI = micros() - funcStartMicrosec - uSecB;

  //DEBUG!!
    //display results:
    //myTeePrint.printf("%lu\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\n", millis(),
    //	newdistval, oldestDistVal, Rear_Dist_PrevVMean, Rear_Dist_PrevVar, inc_var);
  //DEBUG!!

  Rear_Dist_PrevVar = inc_var; //save for next time
  Rear_Dist_PrevMean = inc_mean; //save for next time
  return inc_var; //added  01/16/19
}

//04/28/18 added to update left/right dist arrays, so can reinstate incr l/r dist avg
void UpdateLRDistanceArrays(int leftdistval, int rightdistval)
{
  //Purpose: Update the L/R distance arrays with the latest values, shifting all other values down 1
  //Inputs:
  //	Latest left/right values from sensors
  //Outputs:
  //	latest value placed at Array[LR_DIST_ARRAY_SIZE - 1], all other values moved down one
  //Plan:
  //	Step 1: For each array, shift all values down one (the 0th value drops into the bit bucket)
  //	Step 2: Place the latest reading at [LR_DIST_ARRAY_SIZE - 1].
  //Notes:

//DEBUG!!
  //myTeePrint.printf("UpdateLRDistanceArrays(left = %d, right = %d)", leftdistval, rightdistval);
//DEBUG!!


    //Step 1: For each array, shift all values down one (the 0th value drops into the bit bucket)
  for (int i = 0; i < LR_DIST_ARRAY_SIZE - 1; i++)
    //for (int i = 0; i < DIST_ARRAY_SIZE; i++)
  {
    aRightDistMM[i] = aRightDistMM[i + 1];
    aLeftDistMM[i] = aLeftDistMM[i + 1];
  }

  //Step 2: Place the latest reading at [DIST_ARRAY_SIZE - 1].
  aRightDistMM[LR_DIST_ARRAY_SIZE - 1] = rightdistval;
  aLeftDistMM[LR_DIST_ARRAY_SIZE - 1] = leftdistval;
}

uint16_t CalcRearDistAvgMM(int cur_val, double& prev_avg)
{
  //Purposes: compute 5-point moving average 
  //Inputs:  
  //	cur_val = Most recent value to be averaged
  //	prev_avg = previous average
  //	aRearDistMM = float array containing previous N values

  uint16_t old_val = aRearDistMM[0];
  for (size_t i = 0; i < REAR_DIST_ARRAY_SIZE - 1; i++)
  {
    aRearDistMM[i] = aRearDistMM[i + 1];
  }

  aRearDistMM[REAR_DIST_ARRAY_SIZE - 1] = cur_val;

  //DEBUG!!
    //myTeePrint.printf("In CalcRearDistAvgMM(%2.2f, %2.2f)\n", cur_val, prev_avg);
    //for (size_t i = 0; i < REAR_DIST_ARRAY_SIZE-1; i++)
    //{
    //	myTeePrint.printf("%d, ",aRearDistMM[i]);
    //}
    //myTeePrint.printf("%d\n", aRearDistMM[REAR_DIST_ARRAY_SIZE - 1]);

  float new_avg = (prev_avg * REAR_DIST_ARRAY_SIZE + cur_val - old_val) / REAR_DIST_ARRAY_SIZE;

  prev_avg = new_avg;

  return new_avg;
}
#pragma endregion Distance Measurement Support

#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

  //myTeePrint.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/20 modified for DRV8871 motor driver
//11/04/21 modified for Pololu VNH5019 motor driver
void StopLeftMotors()
{
  digitalWrite(InA_Left, LOW);
  digitalWrite(InB_Left, LOW);
  analogWrite(Spd_Left, MOTOR_SPEED_OFF);
}

//11/04/21 modified for Pololu VNH5019 motor driver
void StopRightMotors()
{
  digitalWrite(InA_Right, LOW);
  digitalWrite(InB_Right, LOW);
  analogWrite(Spd_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!!
  //myTeePrint.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!!
  //myTeePrint.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)
{
  //DEBUG!!
    //myTeePrint.printf("In SetLeftMotorDirAndSpeed(%s, %d)\n",
    //  (bIsFwd == true) ? "true" : "false", speed);
  //DEBUG!!

    //11/04/21 fwd for right motors is CCW when looking at shaft
#ifndef NO_MOTORS
  if (bIsFwd)
  {
    digitalWrite(InA_Left, LOW);
    digitalWrite(InB_Left, HIGH);
    analogWrite(Spd_Left, speed);

    //DEBUG!!
        //myTeePrint.printf("In TRUE block of SetLeftMotorDirAndSpeed(%s, %d)\n",
        //  (bIsFwd == true) ? "true" : "false", speed);
    //DEBUG!!
  }
  else
  {
    //DEBUG!!
        //myTeePrint.printf("In FALSE block of SetLeftMotorDirAndSpeed(%s, %d)\n",
        //  (bIsFwd == true) ? "true" : "false", speed);
    //DEBUG!!

    digitalWrite(InA_Left, HIGH);
    digitalWrite(InB_Left, LOW);
    analogWrite(Spd_Left, speed);
  }
#endif // !NO_MOTORS
}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)
{
  //DEBUG!!
    //myTeePrint.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",
    //  (bIsFwd == true) ? "true" : "false", speed);
  //DEBUG!!

    //11/04/21 fwd for right motors is CW when looking at shaft
#ifndef NO_MOTORS

  if (bIsFwd)
  {
    //DEBUG!!
        //myTeePrint.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n",
        //  (bIsFwd == true) ? "true" : "false", speed);
    //DEBUG!!

    digitalWrite(InA_Right, HIGH);
    digitalWrite(InB_Right, LOW);
    analogWrite(Spd_Right, speed);
  }
  else
  {
    //DEBUG!!
        //myTeePrint.printf("In FALSE block of SetRightMotorDirAndSpeed(%s, %d)\n",
        //  (bIsFwd == true) ? "true" : "false", speed);
        //DEBUG!!

    digitalWrite(InA_Right, LOW);
    digitalWrite(InB_Right, HIGH);
    analogWrite(Spd_Right, speed);
  }

#endif // !NO_MOTORS
}
#pragma endregion Motor Support Functions

#pragma region HDG_BASED_TURN_SUPPORT

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
  //	11/14/21 removed 'first time skip' block; added motor start before entering loop

  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!!
  myTeePrint.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);
  //Serial.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);
  //DEBUG!!

  //no need to continue if the IMU isn't available
  if (!dmpReady)
  {
    Serial.printf("DMP Failure - returning FALSE\n");
    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

  //08/26/21 re-wrote using 3-value array to make sure initial heading is a steady value
  UpdateIMUHdgValDeg();

  int retries = 0;
  if ((IMUHdgValDeg == 180.f || IMUHdgValDeg == 0.f) && retries < 5)
  {
    //DEBUG!!
    myTeePrint.printf("Got 180.00 or 0.00 exactly (%2.3f) from IMU - retrying %d...\n", IMUHdgValDeg, retries);
    //DEBUG!!
    UpdateIMUHdgValDeg();
    retries++;
    delay(100);
  }

  //Step2: Compute new target value & timeout value
  //timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter
  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!!
  myTeePrint.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;

  //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

  //11/14/21 start motors before entering loop.  For CCW turn, left motors go backwards, right ones forward
  int startSpd = (degPersec <= 45) ? TURN_START_SPEED : 2 * TURN_START_SPEED;
  SetLeftMotorDirAndSpeed(!b_ccw, startSpd);
  SetRightMotorDirAndSpeed(b_ccw, startSpd);



  elapsedMillis sinceLastTimeCheck = 0;
  elapsedMillis sinceLastComputeTime = 0;
  double lastError = 0;
  double lastInput = 0;
  double lastIval = 0;
  double lastDerror = 0;
  //bool bFirstIMUHdg = true;

  //DEBUG!!
  myTeePrint.printf("Msec\tHdg\tPrvHdg\tdHdg\tRate\ttgtDPS\terr\tKp*err\tIval\tKd*Derr\tspeed\tMatch\tSlope\n");
  //DEBUG!!

  while (!bDoneTurning && !bTimedOut)
  {
    //11/06/20 now just loops between PID calcs
    CheckForUserInput();

    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;
        //Serial.printf("dHdg > 180 - subtracting 360\n");
      }
      else if (dHdg < -180)
      {
        dHdg += 360;
        //Serial.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!!
      //	Serial.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;
      //}

      TurnRatePIDOutput = PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd);

      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;

      myTeePrint.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\t%2.2f\t%2.2f\n",
        millis(),
        IMUHdgValDeg,
        Prev_HdgDeg,
        dHdg,
        rate,
        degPersec,
        lastError,
        TurnRate_Kp * lastError,
        lastIval,
        TurnRate_Kd * lastDerror,
        speed,
        curHdgMatchVal,
        matchSlope);
      //DEBUG!!

      Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

      //look for full match
      bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL
        || (prevHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope <= -0.01)); //have to use < vs <= as slope == 0 at start

      //Serial.printf("curHdgMatchVal = %2.2f, prevHdgMatchVal = %2.2f, matchslope = %2.2f, bDoneTurning = %d\n",
      //	curHdgMatchVal,
      //	prevHdgMatchVal,
      //	matchSlope,
      //	bDoneTurning);

      prevHdgMatchVal = curHdgMatchVal; //07/31/21 moved below bDoneTurning chk so can use prevHdgMatchVal vs curHdgMatchVal in slope check

      bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

      if (bTimedOut)
      {
        //DEBUG!!
        myTeePrint.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);
        //DEBUG!!

        bResult = false;
        break;
      }

      if (bDoneTurning)
      {
        myTeePrint.printf("Completed turn with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

        bResult = true;
        break;
      }
    }
  }

  StopBothMotors();
  delay(1000); //added 04/27/21 for debug
  return bResult;
}

double PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd)
{
  //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
  //  Notes:
  //    01/13/22 sampleTime input parameter is never used.  This is OK as long as PIDCalcs
  //      is called at regular intervals.

  double error = setpoint - input;
  lastIval += (Ki * error);

  //11/16/21 rev to clamp integral term
  lastIval = (lastIval > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : lastIval;
  lastIval = (lastIval < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : lastIval;
  double dErr = (error - lastError);

  //myTeePrint.printf("PIDCalcs: error/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, lastIval, dErr,
  //	Kp,Ki,Kd);

  /*Compute PID Output*/
  //11/16/21 rev to subtract differential term rather than add.
  //double output = Kp * error + Ki * lastIval + Kd * dErr;
  double output = Kp * error + lastIval - Kd * dErr;
  //myTeePrint.printf("Kp * error = %2.2f, lastIval = %2.2f, Kd * dErr = %2.2f, output = %2.2f\n", 
  //  Kp * error, lastIval, Kd * dErr, output);

  /*Remember some variables for next time*/
  lastError = error;
  lastInput = input;
  lastDerror = dErr;

  return output; //added 09/03/21
}

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!!
    //myTeePrint.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio);
  //DEBUG!!
  return abs(match_ratio);
}

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.

  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;
  }

  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;

  //myTeePrint.printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);
  while (fifoCount < packetSize && GetPacketLoopCount < max_loops)
  {
    GetPacketLoopCount++;
    fifoCount = mpu.getFIFOCount();
    delay(2);
  }

  //myTeePrint.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 HDG_BASED_TURN_SUPPORT

#pragma region FLASHER_X

//09/20/21 copied from FlasherX
//******************************************************************************
// update_firmware()	read hex file and write new firmware to program flash
//******************************************************************************
void update_firmware(Stream* serial, uint32_t buffer_addr, uint32_t buffer_size)
{
  static char line[96];					// buffer for hex lines
  static char data[32] __attribute__((aligned(8)));	// buffer for hex data
  hex_info_t hex = {					// intel hex info struct
    data, 0, 0, 0,					//   data,addr,num,code
    0, 0xFFFFFFFF, 0, 					//   base,min,max,
    0, 0						//   eof,lines
  };

  serial->printf("waiting for hex lines...\n");

  // read and process intel hex lines until EOF or error
//DEBUG!!
  //int line_num = 0;
//DEBUG!!
  long int start_msec = 0;
  bool bFirstTime = true;
  while (!hex.eof) {

    read_ascii_line(serial, line, sizeof(line));

    if (bFirstTime)
    {
      start_msec = millis();
      bFirstTime = false;
    }

    //DEBUG!!
        //serial->printf("%d: %s\n", line_num, line);
        //line_num++;
    //DEBUG!!

        //10/02/21 gfp added for visual xfer confirmation
    if (hex.lines % 10 == 0)
    {
      digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN));
    }

    // reliability of transfer via USB is improved by this printf/flush
    if (serial == (Stream*)&Serial)
    {
      serial->printf("%s\n", line);
      serial->flush();
    }

    if (parse_hex_line((const char*)line, hex.data, &hex.addr, &hex.num, &hex.code) == 0) {
      serial->printf("abort - bad hex line %s\n", line);
      return;
    }
    else if (process_hex_record(&hex) != 0) { // error on bad hex code
      serial->printf("abort - invalid hex code %d\n", hex.code);
      return;
    }
    else if (hex.code == 0) { // if data record
      uint32_t addr = buffer_addr + hex.base + hex.addr - FLASH_BASE_ADDR;
      if (hex.max > (FLASH_BASE_ADDR + buffer_size)) {
        serial->printf("abort - max address %08lX too large\n", hex.max);
        return;
      }
      else if (!IN_FLASH(buffer_addr)) {
        memcpy((void*)addr, (void*)hex.data, hex.num);
      }
      else if (IN_FLASH(buffer_addr)) {
        int error = flash_write_block(addr, hex.data, hex.num);
        if (error) {
          serial->printf("abort - error %02X in flash_write_block()\n", error);
          return;
        }
      }
    }
    hex.lines++;
  }

  //serial->printf("\nhex file: %1d lines %1lu bytes (%08lX - %08lX)\n",
  //  hex.lines, hex.max - hex.min, hex.min, hex.max);
  serial->printf("\nhex file: %1d lines %1lu bytes (%08lX - %08lX), %lu Msec\n",
    hex.lines, hex.max - hex.min, hex.min, hex.max, millis() - start_msec);

  // check FSEC value in new code -- abort if incorrect
#if defined(KINETISK) || defined(KINETISL)
  uint32_t value = *(uint32_t*)(0x40C + buffer_addr);
  if (value == 0xfffff9de) {
    serial->printf("new code contains correct FSEC value %08lX\n", value);
  }
  else {
    serial->printf("abort - FSEC value %08lX should be FFFFF9DE\n", value);
    return;
  }
#endif

  // check FLASH_ID in new code - abort if not found
  if (check_flash_id(buffer_addr, hex.max - hex.min)) {
    serial->printf("new code contains correct target ID %s\n", FLASH_ID);
  }
  else {
    serial->printf("abort - new code missing string %s\n", FLASH_ID);
    return;
  }

  // get user input to write to flash or abort
  int user_lines = -1;
  while (user_lines != hex.lines && user_lines != 0) {
    serial->printf("enter %d to flash or 0 to abort\n", hex.lines);
    read_ascii_line(serial, line, sizeof(line));
    sscanf(line, "%d", &user_lines);
  }

  if (user_lines == 0) {
    serial->printf("abort - user entered 0 lines\n");
    return;
  }

  // move new program from buffer to flash, free buffer, and reboot
  //serial->printf("user entered %d lines\n", user_lines);
  flash_move(FLASH_BASE_ADDR, buffer_addr, hex.max - hex.min);

  // should not return from flash_move(), but put REBOOT here as reminder
  REBOOT;
}

//******************************************************************************
// read_ascii_line()	read ascii characters until '\n', '\r', or max bytes
//******************************************************************************
void read_ascii_line(Stream* serial, char* line, int maxbytes)
{
  int c = 0, nchar = 0;
  while (nchar < maxbytes && !(c == '\n' || c == '\r')) {
    if (serial->available()) {
      c = serial->read();
      line[nchar++] = c;
    }
  }
  line[nchar - 1] = 0;	// null-terminate
}

//******************************************************************************
// process_hex_record()		process record and return okay (0) or error (1)
//******************************************************************************
int process_hex_record(hex_info_t* hex)
{
  if (hex->code == 0) { // data -- update min/max address so far
    if (hex->base + hex->addr + hex->num > hex->max)
      hex->max = hex->base + hex->addr + hex->num;
    if (hex->base + hex->addr < hex->min)
      hex->min = hex->base + hex->addr;
  }
  else if (hex->code == 1) { // EOF (:flash command not received yet)
    hex->eof = 1;
  }
  else if (hex->code == 2) { // extended segment address (top 16 of 24-bit addr)
    hex->base = ((hex->data[0] << 8) | hex->data[1]) << 4;
  }
  else if (hex->code == 3) { // start segment address (80x86 real mode only)
    return 1;
  }
  else if (hex->code == 4) { // extended linear address (top 16 of 32-bit addr)
    hex->base = ((hex->data[0] << 8) | hex->data[1]) << 16;
  }
  else if (hex->code == 5) { // start linear address (32-bit big endian addr)
    hex->base = (hex->data[0] << 24) | (hex->data[1] << 16)
      | (hex->data[2] << 8) | (hex->data[3] << 0);
  }
  else {
    return 1;
  }

  return 0;
}

//******************************************************************************
// Intel Hex record foramt:
//
// Start code:  one character, ASCII colon ':'.
// Byte count:  two hex digits, number of bytes (hex digit pairs) in data field.
// Address:     four hex digits
// Record type: two hex digits, 00 to 05, defining the meaning of the data field.
// Data:        n bytes of data represented by 2n hex digits.
// Checksum:    two hex digits, computed value used to verify record has no errors.
//
// Examples:
//  :10 9D30 00 711F0000AD38000005390000F5460000 35
//  :04 9D40 00 01480000 D6
//  :00 0000 01 FF
//******************************************************************************

/* Intel HEX read/write functions, Paul Stoffregen, paul@ece.orst.edu */
/* This code is in the public domain.  Please retain my name and */
/* email address in distributed copies, and let me know about any bugs */

/* I, Paul Stoffregen, give no warranty, expressed or implied for */
/* this software and/or documentation provided, including, without */
/* limitation, warranty of merchantability and fitness for a */
/* particular purpose. */

// type modifications by Jon Zeeff

/* parses a line of intel hex code, stores the data in bytes[] */
/* and the beginning address in addr, and returns a 1 if the */
/* line was valid, or a 0 if an error occured.  The variable */
/* num gets the number of bytes that were stored into bytes[] */

#include <stdio.h>		// sscanf(), etc.
#include <string.h>		// strlen(), etc.

int parse_hex_line(const char* theline, char* bytes,
  unsigned int* addr, unsigned int* num, unsigned int* code)
{
  unsigned sum, len, cksum;
  const char* ptr;
  int temp;

  *num = 0;
  if (theline[0] != ':')
    return 0;
  if (strlen(theline) < 11)
    return 0;
  ptr = theline + 1;
  if (!sscanf(ptr, "%02x", &len))
    return 0;
  ptr += 2;
  if (strlen(theline) < (11 + (len * 2)))
    return 0;
  if (!sscanf(ptr, "%04x", (unsigned int*)addr))
    return 0;
  ptr += 4;
  /* myTeePrint.printf("Line: length=%d Addr=%d\n", len, *addr); */
  if (!sscanf(ptr, "%02x", code))
    return 0;
  ptr += 2;
  sum = (len & 255) + ((*addr >> 8) & 255) + (*addr & 255) + (*code & 255);
  while (*num != len)
  {
    if (!sscanf(ptr, "%02x", &temp))
      return 0;
    bytes[*num] = temp;
    ptr += 2;
    sum += bytes[*num] & 255;
    (*num)++;
    if (*num >= 256)
      return 0;
  }
  if (!sscanf(ptr, "%02x", &cksum))
    return 0;

  if (((sum & 255) + (cksum & 255)) & 255)
    return 0;     /* checksum error */
  return 1;
}
#pragma endregion FLASHER_X

#pragma region IR_HOMING_SUPPORT
//bool IRHomeToChgStn(int avoidancedistCm, int initleftspeed, int initrightspeed)
//{
//  //Purpose:  Home in to charging station with optional avoidance manuever
//  //Inputs:
//  //	avoidancedistCm = int denoting how far away to start avoidance maneuver
//  //Outputs:
//  //	either connected to charging station or turn away at avoidancedistCm
//  //Plan: 
//  //	Step1: Navigate to IAP
//  //	Step2: Initialize PID for homing
//  //	Step3: If front distance < avoidancedistCm, turn 90 deg away from near wall
//  //		   otherwise continue homing until connected or stuck
//  //Notes:
//  //	03/19/17 rev to add initial left/right speed vals to calling sig
//  //	08/09/20 added IsStuck() call, limited to 5 calls/sec to avoid false positives
//  //	08/10/20 now using timer ISR for this so only need to check bIsStuck state
//  //	11/08/20 moved I2C comms into timer ISR
//  //	01/23/21 added code to re-try docking if the robot gets stuck
//  //	02/07/21 rev to use ISR-managed bChgConnect vs local bChgConn
//  //	02/21/21 added new nav-to-IAP algorithm
//  //	04/03/21 added IRHomingPID.SetSampleTime(0) to make sure sample interval controlled by ISR
//
//  bool result = true; //added 01/16/19 to supress warning
//
////Step2: Initialize PID for homing
//  IRHomingSetpoint = 0; //01/14/21 
//  IRHomingPID.SetMode(AUTOMATIC);
//  IRHomingPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
//  //IRHomingPID.SetSampleTime(0); //04/03/21 zero sample time so .Compute() interval controlled by ISR
//
//  //int bChgConn = LOW; //for testing
//
//  lastHomingTelemetryMsec = 0; //used to space out telemetry prints
//
//  myTeePrint.printf("IRHomeToChgStn with detection value = %lu, Steering = %2.2f & IRHOMING Kp,Ki,Kd = (%2.0f,%2.0f,%2.0f)\n",
//    IRHomingValTotalAvg, IRHomingLRSteeringVal, IRHomingPID.GetKp(), IRHomingPID.GetKi(), IRHomingPID.GetKd());
//
//  //Step3: If front distance < avoidancedistCm, turn 90 deg away from near wall
//  //	   otherwise continue homing until connected or stuck
//  myTeePrint.printf("front dist = %d, lastHomingTelemetryMsec = ", glFrontDistVal);
//  myTeePrint.println(lastHomingTelemetryMsec);
//  myTeePrint.println(IRHomingTelemStr); //header for chg telemetry data
//
//  //08/10/20 now using ISR for bChgConnect  and front distance
//  //01/08/22 no longer using ISR
//  //while (bChgConnect == LOW && glFrontDistVal > avoidancedistCm) //01/23/21 removed bIsStuck - handled inside while()
//  bool bChgConnect = false; //01/08/22 no longer using ISR, so have to generate locally
//
//  //while (bChgConnect == LOW ) //01/08/22 removed avoidancedistCm check
//  while (!bChgConnect) //01/08/22 removed avoidancedistCm check
//  {
//    //05/02/20 turn on Laser
//    digitalWrite(RED_LASER_DIODE_PIN, HIGH);
//
//    //01/30/17 added to kill motors remotely using Wixel & serial port
//    CheckForUserInput();
//
//    if (MsecSinceLastIRHomingAdj >= MSEC_PER_IR_HOMING_ADJ)
//    {
//      MsecSinceLastIRHomingAdj -= MSEC_PER_IR_HOMING_ADJ;
//      GetRequestedVL53l0xValues(VL53L0X_ALL);
//      
//      if (!isnan(IRHomingLRSteeringVal))//skip bad values
//      {
//        if (IRHomingPID.Compute()) //runs every 100mSec, or every MSEC_PER_IR_HOMING_ADJ whichever is longer
//        {
//          ////DEBUG!!
//          			digitalWrite(HOMING_PID_COMPUTE_CALL_PIN, HIGH);
//          			delay(1);
//          			digitalWrite(HOMING_PID_COMPUTE_CALL_PIN, LOW);
//          ////DEBUG!!
//        }
//        leftspeednum = initleftspeed + IRHomingOutput;
//        rightspeednum = initrightspeed - IRHomingOutput;
//
//        //DEBUG!!
//        //print out telemetry every 400 msec or so
//        //if (lastHomingTelemetryMsec > IR_HOMING_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics
//        //{
//        //  lastHomingTelemetryMsec -= IR_HOMING_TELEMETRY_SPACING_MSEC;
//          myTeePrint.printf("%lu\t%2.2f\t%lu\t%lu\t%2.2f\t%2.2f\t\t%d\t%d\t%d\t%d\n",
//            millis(), GetBattVoltage(), IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal, IRHomingOutput,
//            leftspeednum, rightspeednum, glFrontDistVal, Lidar_Rear);
//        //}
//
//        UpdateRearLEDPanel(leftspeednum, rightspeednum);
//
//        //04/24/20 experiment with going to full speed when near charge plug
//        //01/30/21 bugfix.  Robot must also be somewhere near centerline alignment, to avoid
//        //sensing the charging station beacon while approaching a wall and 'accelerating to
//        //contact' before actually turning to home in on the beacon
//        if (glFrontDistVal <= CHG_STN_FINAL_APPR_DIST_CM && abs(IRHomingLRSteeringVal) <= IR_HOMING_STEERING_VAL_CAPTURE_THRESHOLD)
//        {
//          leftspeednum = rightspeednum = MOTOR_SPEED_MAX;
//          myTeePrint.printf("Accelerating to Contact with frontdist = %d\n", glFrontDistVal);
//        }
//
//        MoveAhead(leftspeednum, rightspeednum);
//      }
//
//      bChgConnect = IsChargerConnected(bChgConnect);
//    }
//
//    //01/23/21 moved new 'bIsStuck' section into main while() loop to support multiple docking attempts
//      //	01/23/21 added code to handle 'stuck' condition in charging station funnel
//    glFrontDistVal = GetFrontDistCm();
//    glFrontvar = CalcFrontDistArrayVariance(glFrontDistVal);
//
//    glRearDistVal = GetRequestedVL53l0xValues(VL53L0X_REAR_ONLY);
//    glRearvar = CalcRearDistArrayVariance(glRearDistVal);
//    bool bIsStuck = glFrontvar > STUCK_FRONT_VARIANCE_THRESHOLD || glRearvar > STUCK_REAR_VARIANCE_THRESHOLD;
//    if (bIsStuck)
//    {
//      myTeePrint.printf("Stuck in IRHomeToChargingStation() - Attempting to recover\n");
//      myTeePrint.printf("Msec\tLF\tLC\tLR\tRF\tRC\tRR\tIR\n");
//
//      myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%2.3f\n", millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
//        Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, IRHomingLRSteeringVal);
//
//      //reverse for 0.5 sec, then pause for 2 sec
//      MoveReverse(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
//      delay(500);
//      StopBothMotors();
//      delay(2000);
//
//      while (abs(IRHomingLRSteeringVal) > 0.1f)
//      {
//        if (!isnan(IRHomingLRSteeringVal))
//        {
//          //11/08/20  now this executes every time, with interval controlled by timer ISR
//          IRHomingPID.Compute();
//          leftspeednum = initleftspeed - IRHomingOutput;
//          rightspeednum = initrightspeed + IRHomingOutput;
//
//          //DEBUG!!
//          //print out telemetry every 400 msec or so
//          if (lastHomingTelemetryMsec > IR_HOMING_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics
//          {
//            lastHomingTelemetryMsec -= IR_HOMING_TELEMETRY_SPACING_MSEC;
//            myTeePrint.printf("%lu\t%2.2f\t%lu\t%lu\t%2.2f\t%2.2f\t\t%d\t%d\t%d\n",
//              millis(), GetBattVoltage(), IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal, IRHomingOutput,
//              leftspeednum, rightspeednum, glFrontDistVal);
//          }
//          //DEBUG!!
//
//          MoveReverse(leftspeednum, rightspeednum);
//        }
//      }
//
//      myTeePrint.printf("Succeeded!  Robot should now be lined up again!  Stopping motors\n");
//      StopBothMotors();
//      delay(1000);
//      myTeePrint.printf("Trying again to dock with Charging Station\n");
//      delay(1000);
//
//      //08/10/20 now using ISR for bIsStuck  and front distances
//      while (bChgConnect == LOW && !bIsStuck && glFrontDistVal > avoidancedistCm)
//      {
//        //05/02/20 turn on Laser
//        digitalWrite(RED_LASER_DIODE_PIN, HIGH);
//
//        //01/30/17 added to kill motors remotely using Wixel & serial port
//        CheckForUserInput();
//
//        //skip bad values
//        if (!isnan(IRHomingLRSteeringVal))
//        {
//          //11/08/20  now this executes every time, with interval controlled by timer ISR
//          IRHomingPID.Compute();
//          leftspeednum = initleftspeed + IRHomingOutput;
//          rightspeednum = initrightspeed - IRHomingOutput;
//
//          //DEBUG!!
//          //print out telemetry every 400 msec or so
//          if (lastHomingTelemetryMsec > IR_HOMING_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics
//          {
//            lastHomingTelemetryMsec -= IR_HOMING_TELEMETRY_SPACING_MSEC;
//            myTeePrint.printf("%lu\t%2.2f\t%lu\t%lu\t%2.2f\t%2.2f\t\t%d\t%d\t%d\n",
//              millis(), GetBattVoltage(), IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal, IRHomingOutput,
//              leftspeednum, rightspeednum, glFrontDistVal);
//          }
//          //DEBUG!!
//
//          //04/24/20 experiment with going to full speed when near charge plug
//          if (glFrontDistVal <= CHG_STN_FINAL_APPR_DIST_CM)
//          {
//            leftspeednum = rightspeednum = MOTOR_SPEED_MAX;
//            myTeePrint.printf("Accelerating to Contact with frontdist = %d\n", glFrontDistVal);
//          }
//
//          MoveAhead(leftspeednum, rightspeednum);
//        }
//
//        //05/02/20 turn off Laser
//        digitalWrite(RED_LASER_DIODE_PIN, LOW);
//      }//	while (bChgConn == LOW && !bIsStuck && glFrontDistVal > avoidancedistCm)
//    }
//  }
//
//  //if (bChgConnect == HIGH)
//  if (bChgConnect)
//  {
//    Serial.print("Charger Connected at "); Serial.println(millis());
//    result = true; //added 01/16/19 to supress warning
//  }
//  else //added 12/28/20 for debug.
//  {
//    myTeePrint.printf("In IRHomeToChgStation: This code block should never execute! Stopping Program!");
//    StopBothMotors();
//    while (true)
//    {
//
//    }
//  }
//
//  //11/21/20 added to force PID re-init next time
//  IRHomingPID.SetMode(MANUAL);
//
//  return result; //added 01/16/19 to supress warning
//}

bool IRHomeToChgStnNoPings(int initleftspeed, int initrightspeed)
{
  //Purpose:  Home in to charging station without distance information
  //Inputs:
  //Outputs:
  //	robot homes to charging station
  //Plan: 
  //	Step2: Initialize PID for homing
  //Notes:
  //	03/19/17 rev to add initial left/right speed vals to calling sig
  //  01/14/22 rev to use PIDCalcs vs PID library

  bool result = true; //added 01/16/19 to suppress  warning

//Step2: Initialize PID for homing
  IRHomingSetpoint = 0; //01/14/21 
  double lastError = 0;
  double lastInput = 0;
  double lastIval = 0;
  double lastDerror = 0;

  myTeePrint.printf("IRHomeToChgStnNoPings with detection value = %lu, Steering = %2.2f & IRHOMING Kp,Ki,Kd = (%2.0f,%2.0f,%2.0f)\n",
    IRHomingValTotalAvg, IRHomingLRSteeringVal, IRHomingKp, IRHomingKi, IRHomingKd);

  myTeePrint.println(IRHomingTelemStr); //header for chg telemetry data

  bool bChgConnect = false; //01/08/22 no longer using ISR, so have to generate locally

  while (!bChgConnect) //01/08/22 removed avoidancedistCm check
  {
    //05/02/20 turn on Laser
    digitalWrite(RED_LASER_DIODE_PIN, HIGH);

    //01/30/17 added to kill motors remotely using Wixel & serial port
    CheckForUserInput();

    if (MsecSinceLastIRHomingAdj >= MSEC_PER_IR_HOMING_ADJ)
    {
      //01/15/22 added to measure time required to do a PID computation loop (w/o pings)
     //DEBUG!!
      digitalWrite(HOMING_PID_COMPUTE_CALL_PIN, HIGH);
     //DEBUG!!
        
      //01/15/22 when changing from 'remote manual control' to 'auto' mode
      //it is possible for MsecSinceLastIRHomingAdj to grow very large
      //MsecSinceLastIRHomingAdj -= MSEC_PER_IR_HOMING_ADJ;
      while (MsecSinceLastIRHomingAdj >= MSEC_PER_IR_HOMING_ADJ)
      {
        MsecSinceLastIRHomingAdj -= MSEC_PER_IR_HOMING_ADJ;
      }
      UpdateIRHomingValues();

      if (!isnan(IRHomingLRSteeringVal))//skip bad values
      {
        //myTeePrint.printf("IRHomingLRSteeringVal = %2.2f\n", IRHomingLRSteeringVal);
        IRHomingOutput = PIDCalcs(IRHomingLRSteeringVal, 0, MSEC_PER_IR_HOMING_ADJ, lastError, lastInput, lastIval,
          lastDerror, IRHomingKp, IRHomingKi, IRHomingKd);

        //reversed 01/11/22 - 01/14/22 - reversed it back again
        leftspeednum = initleftspeed + IRHomingOutput;
        rightspeednum = initrightspeed - IRHomingOutput;

        //limit wheel speeds to valid range (0-255)
        leftspeednum = (leftspeednum > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : leftspeednum;
        leftspeednum = (leftspeednum < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : leftspeednum;
        rightspeednum = (rightspeednum > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : rightspeednum;
        rightspeednum = (rightspeednum < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : rightspeednum;

        //DEBUG!!
        myTeePrint.printf("%lu\t%2.2f\t%lu\t%lu\t%2.2f\t%2.2f\t\t%d\t%d\n",
          millis(), GetBattVoltage(), IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal, IRHomingOutput,
          leftspeednum, rightspeednum);

        UpdateRearLEDPanel(leftspeednum, rightspeednum);

        MoveAhead(leftspeednum, rightspeednum);
      }

      bChgConnect = IsChargerConnected(bChgConnect);

      //01/15/22 added to measure time required to do a PID computation loop (w/o pings)
      //DEBUG!!
      digitalWrite(HOMING_PID_COMPUTE_CALL_PIN, LOW);
      //DEBUG!!
    }
  }

  if (bChgConnect)
  {
    Serial.print("Charger Connected at "); Serial.println(millis());
    result = true; //added 01/16/19 to supress warning
  }
  else //added 12/28/20 for debug.
  {
    myTeePrint.printf("In IRHomeToChgStation: This code block should never execute! Stopping Program!");
    StopBothMotors();
    while (true)
    {

    }
  }

  return result; //added 01/16/19 to supress warning
}

void UpdateIRHomingValues()
{
  //Purpose: Update all the IR Homing related values
  //Provenance:  Ported from FourWD_WallE2_V12 01/07/22
  //Inputs:
  //  IRHomingValTotalAvg = long int denoting previous average value 
  //Outputs: 
  //  IRFinalValue1/2 = long ints denoting left/right computed demodulator outputs
  //  IRHomingLRSteeringVal = double denoting left/right steering error
  //  IRHomingValueTotalAverage array & IRHomingValTotalAvg updated

  //myTeePrint.printf("Requesting %d, %d, %d bytes from IRDet\n", sizeof(IRFinalValue1), sizeof(IRFinalValue2), sizeof(IRHomingLRSteeringVal));
  Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, sizeof(IRFinalValue1) + sizeof(IRFinalValue2) + sizeof(IRHomingLRSteeringVal));
  I2C_readAnything(IRFinalValue1);
  I2C_readAnything(IRFinalValue2);
  I2C_readAnything(IRHomingLRSteeringVal);
  //myTeePrint.printf("Got %lu, %lu, %2.2f from IR Detector module\n", IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal);
  IRHomingValTotalAvg = UpdateIRHomingValueTotalAverage(IRFinalValue1, IRFinalValue2, IRHomingValTotalAvg);
}

unsigned long UpdateIRHomingValueTotalAverage(unsigned long FinVal1, unsigned long FinVal2, unsigned long prev_avg)
{
  //Purpose: Update L/R IR Homing average total signal strength
  // Provenance 03/16/21
  // Inputs: 
  //	FinVal1, FinVal2 = latest values from IR Homing Teensy
  //	prev_avg = previous left/right total average
  // Plan:
  // Step1: Add new total to top of array, bump everything down one, oldest value to bit-bucket
  // Step2: Calc new average as new_avg = old_avg + (new_val - old_val)/SIZE

//Step1: Add new total to top of array, bump everything down one, oldest value to bit-bucket
//Step2: Calc new average as new_avg = old_avg + (new_val - old_val)/SIZE
  unsigned long newValTotal = FinVal1 + FinVal2;
  unsigned long oldValTotal = aIRHOMINGVALTOTALS[0];
  for (int i = 0; i < IRHOMING_VALUE_ARRAY_SIZE - 1; i++)
  {
    aIRHOMINGVALTOTALS[i] = aIRHOMINGVALTOTALS[i + 1];
    //myTeePrint.printf("%d\t%lu\n", i, aIRHOMINGVALTOTALS[i]);
  }
  aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE - 1] = newValTotal; //new total value
  //myTeePrint.printf("%d\t%lu\n", IRHOMING_VALUE_ARRAY_SIZE - 1, aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE - 1]);

//Step2: Calc new average as new_avg = old_avg + (new_val - old_val)/SIZE

  unsigned long new_val_avg = prev_avg + ((long int)newValTotal - (long int)oldValTotal) / IRHOMING_VALUE_ARRAY_SIZE;

  //myTeePrint.printf("FinVal1/2 = %lu/%lu, prev_avg = %lu, old_tot = %lu, new_avg = %lu\n",
    //FinVal1,FinVal2,prev_avg, oldValTotal, new_val_avg);

  return (unsigned long)new_val_avg;
}

bool MoveToDesiredRearDistCm(int offsetCm)
{
  //Purpose: Move forward or backward to a specified rear offset distance
  // Inputs:
  //	offsetCm - int denoting desired rear distance offset
  // Outputs:
  //	Robot moves forward or backward to the desired offset, under PID control

  PID OffsetDistPID(&OffsetDistVal, &OffsetDistOutput, &OffsetDistSetpointCm,
    OffsetDistKp, OffsetDistKi, OffsetDistKd, DIRECT);//12/14/20 re-doing this from scratch

  OffsetDistPID.SetMode(AUTOMATIC);
  OffsetDistPID.SetSampleTime(0);
  OffsetDistPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

  StopBothMotors();
  int speed;
  //OffsetDistSetpointCm = 30;
  OffsetDistSetpointCm = offsetCm;
  myTeePrint.printf("Msec\tDist\tdiff\tspeed\tvar\tavg\n");
  uint16_t avg = CalcRearDistAvgMM(Lidar_Rear, Rear_Dist_PrevMean);
  MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore
  while (abs(Lidar_Rear - 10 * OffsetDistSetpointCm) > 10 || abs(avg - 10 * OffsetDistSetpointCm) > 10)
  {
    CheckForUserInput();
    if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)
    {
      MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;
      OffsetDistVal = Lidar_Rear / 10;
      OffsetDistPID.Compute();
      speed = (int)OffsetDistOutput;
      uint16_t rear_var = CalcRearDistArrayVariance((unsigned long)Lidar_Rear);
      avg = CalcRearDistAvgMM(Lidar_Rear, Rear_Dist_PrevMean);
      myTeePrint.printf("%lu\t%2.2f\t%2.2f\t%d\t%d\t%d\n", millis(), OffsetDistVal, OffsetDistOutput, speed, rear_var, avg);
      if (speed >= 0)
      {
        RunBothMotors(true, speed, speed);
      }
      else
      {
        RunBothMotors(false, -speed, -speed);
      }
    }
  }

  myTeePrint.printf("Stopped at %2.2f cm with avg = %d\n", (float)Lidar_Rear / 10, avg);
  //StopBothMotors();
  return true;
}

bool MoveToDesiredFrontDistCm(int offsetCm)
{
  //Purpose: Move forward or backward to a specified front offset distance
  // Inputs:
  //	offsetCm - int denoting desired forward distance offset
  //	Kp,Ki,Kd set at top of program (lines 85-87)
  // Outputs:
  //	Robot moves forward or backward to the desired offset, under PID control
  //Notes:
  //	04/25/21 the Kp/Ki/Kd values are much lower than prescribed by Z-N theory
  //		but I like the slower, more deliberate action.

  //DEBUG!!
  myTeePrint.printf("MoveToDesiredFrontDistCm: Kp/Ki/Kd = %2.1f/%2.1f/%2.1f\n", OffsetDistPID.GetKp(), OffsetDistPID.GetKi(), OffsetDistPID.GetKd());
  OffsetDistPID.SetMode(AUTOMATIC);
  OffsetDistPID.SetSampleTime(0);
  OffsetDistPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

  StopBothMotors();
  int speed;
  OffsetDistSetpointCm = offsetCm;
  myTeePrint.printf("Msec\tDist\tdiff\tspeed\tavg\n");
  uint16_t avg = GetAvgFrontDistCm();
  MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore

  while (abs(glFrontDistVal - OffsetDistSetpointCm) > 1 || abs(avg - OffsetDistSetpointCm) > 1)
  {
    CheckForUserInput();
    if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)
    {
      MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;
      OffsetDistVal = glFrontDistVal;
      OffsetDistPID.Compute();
      speed = -(int)OffsetDistOutput;
      avg = GetAvgFrontDistCm();
      myTeePrint.printf("%lu\t%2.2f\t%2.2f\t%d\t%d\n", millis(), OffsetDistVal, OffsetDistOutput, speed, avg);
      RunBothMotorsBidirectional(speed, speed);
    }
  }

  myTeePrint.printf("Stopped at %d cm with avg = %d\n", glFrontDistVal, avg);
  StopBothMotors();
  return true;
}

void UpdateRearLEDPanel(int leftspeed, int rightspeed) //added 05/02/17
{
  //Purpose:  Update LED enable/disable states on rear LED panel
  //Provenance: Created 05/03/17 gfp
  //Inputs:
  //	leftspeed, rightspeed = integers representing left/right motor speeds
  //Outputs:
  //	Rear LED panel updated to reflect left/right motor speeds
  //Plan:
  //	Step1:  Disable all LEDs to blank the display
  //	Step2:  Enable the appropriate LEDs on each side

  //Step1:  Disable all LEDs to blank the display
  //DisableAllRearPanelLEDs();
  EnableAllRearLEDs(false); //repl DisableAllRearPanelLEDs() 04/02/21

  //Step2:  Enable the appropriate LEDs on each side

  //left side
  if (leftspeed >= MOTOR_SPEED_LOW)
  {
    digitalWrite(_40PCT_LED_PIN, LOW);
  }
  if (leftspeed >= MOTOR_SPEED_HALF)
  {
    digitalWrite(_20PCT_LED_PIN, LOW);
  }
  if (leftspeed >= MOTOR_SPEED_FULL)
  {
    digitalWrite(CHG_LED_PIN, LOW);
  }

  //right side
  if (rightspeed >= MOTOR_SPEED_LOW)
  {
    digitalWrite(_60PCT_LED_PIN, LOW);
  }
  if (rightspeed >= MOTOR_SPEED_HALF)
  {
    digitalWrite(_80PCT_LED_PIN, LOW);
  }
  if (rightspeed >= MOTOR_SPEED_FULL)
  {
    digitalWrite(FIN_LED_PIN, LOW);
  }
}
#pragma endregion IR_HOMING_SUPPORT

#pragma region MISCELLANEOUS
int GetOpMode() //04/28/19 added batv to sig 01/07/22 removed - now call GetBatt
{
  //Purpose:  Determine operating mode based on sensor inputs
  //Inputs: none
  //Outputs: Integer denoting current op mode (CHARGING = 1, IRHOMING = 2, WALLFOLLOW = 3)
  //Plan:
  //	Step1: If batt voltage too low, return MODE_DEADBATTERY (5)
  //	Step2: If Charger is connected, return MODE_CHARGING (1)
  //	Step3: Else If IR Homing beam detected, return MODE_IRHOMING (2)
  //	Step4: Else return MODE_WALLFOLLOW (3)
  //Notes:
  //	01/16/18 added MODE_DEADBATTERY
  //	03/28/18 now looking for either IsChargerConnected OR low on TP5100 Chg line
  //	09/03/18 reorganized so DEADBATTERY is on top, and added new TURNING_TO_HDG mode
  //	02/24/19 now using 1NA619 charge current sensor
  //	04/28/19 added batV to function sig
  //	04/27/20 rewrote to simplify MODE_CHARGING code and to have only one exit point
  //	05/02/20 bugfix - IsChargerConnected() block has to come before DEAD_BATTERY check
  //  07/16/21 added MODE_WALLCAPTURE (4)
  //  01/07/22 removed battv from sig - now done internally
  // 
  myTeePrint.printf("%lu: At top of GetOpMode() with mode = %s (%d), prev_mode = %s (%d)\n",
    millis(), ModeStrArray[CurrentOpMode], CurrentOpMode, ModeStrArray[PrevOpMode], PrevOpMode);

  int mode = MODE_NONE; //04/27/20 added so function has only one exit point

  //01/08/22 updates added here - no longer using ISR strategy
  float batv = GetBattVoltage();
  UpdateIRHomingValues();
  bool bIRBeamAvail = (IRHomingValTotalAvg > IR_BEAM_DETECTION_THRESHOLD
    && abs(IRHomingLRSteeringVal) < IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD);
  bool bChgConnect = true;
  bChgConnect = IsChargerConnected(bChgConnect);

  //Step1: If Charger is connected, return MODE_CHARGING (1)
  if (bChgConnect) //12/16/20 rev to use ISR-generated value 
  {
    myTeePrint.printf("Robot has successfully connected to charger!\n");
    mode = MODE_CHARGING;
  }

  //Step2: Else If IR Homing beam detected, return MODE_IRHOMING (2)
  else if (bIRBeamAvail)
  {
    myTeePrint.printf("IR beam detected with value = %lu\n", IRFinalValue1 + IRFinalValue2);
    mode = MODE_IRHOMING; //this handles 'hungry/not-hungry' cases internally.
  }

  //Step3: If batt voltage too low, return MODE_DEADBATTERY (4)
  else if (batv <= DEAD_BATT_THRESH_VOLTS)
  {
    myTeePrint.printf("in MODE_DEADBATTERY block with batV = %2.3f\n", batv);
    mode = MODE_DEADBATTERY;
  }

  //01/08/22 - revised to explicitly add default MODE_WALLFOLLOW assignment
  else if (PrevOpMode != MODE_WALLFOLLOW)
  {
    mode = MODE_WALLCAPTURE;
    PrevOpMode = MODE_WALLCAPTURE;
    //myTeePrint.printf("%lu: After PrevOpMode != MODE_WALLFOLLOW with mode = %s (%d), prev_mode = %s (%d)\n", 
    //  millis(), ModeStrArray[CurrentOpMode], CurrentOpMode, ModeStrArray[PrevOpMode], PrevOpMode);
  }

  else mode = MODE_WALLFOLLOW;

  //myTeePrint.printf("%lu: At bottom of GetOpMode() with mode = %d\n", millis(), CurrentOpMode);
  return mode;
}

void CheckForUserInput()
{
  //Notes:
  //  11/21/21 rev to accomodate cmd input from either serial, and output to both
  //  12/25/21 another try at using pointer for Serial object

  //DEBUG!!
  //myTeePrint.printf("In CheckForUserInput().  Serial.available() = %d, Serial1.available() = %d\n",
  //  Serial.available(), Serial1.available());

  const int bufflen = 3;
  char buff[bufflen];
  memset(buff, 0, bufflen);
  byte incomingByte = 0; //moved here 11/21/21
  int numchars = 0;
  Stream* pStream = 0;

  if (Serial.available() > 0)
  {
    // read the incoming byte:
    numchars = Serial.readBytesUntil('\n', buff, sizeof(buff));
    incomingByte = buff[0];

    //Set pStream to Serial
    pStream = (Stream*)&Serial;

    // say what you got:
    pStream->printf("I received %d chars, first char 0X%X on USB Serial\n", numchars, incomingByte);
  }

  //11/21/21 now input can come from Serial1 too
  else if (Serial1.available() > 0)
  {
    // read the incoming byte:
    numchars = Serial1.readBytesUntil('\n', buff, sizeof(buff));
    incomingByte = buff[0];

    //Set pStream to Serial1
    pStream = (Stream*)&Serial1;

    // say what you got:
    pStream->printf("I received %d chars, first char 0X%X on BT Serial1\n", numchars, incomingByte);
  }

  //11/21/21 incomingByte can come from either serial input
  if (incomingByte != 0)
  {
    //02/18/20 experiment with multiple commands
    switch (incomingByte)
    {
    case 0x55: //ASCII 'U'
    case 0x75: //ASCII 'u'
#pragma region FIRMWARE UPDATE
      StopBothMotors();
      pStream->println(F("Start Program Update - Send new HEX file!"));

      //09/20/21 copied from FlasherX - loop()
      if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
      {
        pStream->printf("unable to create buffer\n"); Serial.flush();

        for (;;) {}
      }

      pStream->printf("buffer = %1luK %s (%08lX - %08lX)\n",
        buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",
        buffer_addr, buffer_addr + buffer_size);

      //09/20/21 clear the serial buffer
      while (pStream->available())
      {
        pStream->read();
      }

      // receive hex file via serial, write new firmware to flash, clean up, reboot
      update_firmware(&Serial1, buffer_addr, buffer_size); // no return if success

      // return from update_firmware() means error or user abort, so clean up and
      // reboot to ensure that static vars get boot-up initialized before retry
      pStream->printf("erase FLASH buffer / free RAM buffer...\n");
      firmware_buffer_free(buffer_addr, buffer_size);
      Serial1.flush();
      REBOOT;
      break;
#pragma endregion FIRMWARE UPDATE
    case 0x43: //ASCII 'C'
    case 0x63: //ASCII 'c'
#pragma region MANUALCONTROL
      pStream->println(F("ENTERING COMMAND MODE:"));
      pStream->println(F("0 = 180 deg CCW Turn"));
      pStream->println(F("1 = 180 deg CW Turn"));
      pStream->println(F("A = Back to Auto Mode"));
      pStream->println(F("S = Stop"));
      pStream->println(F("F = Forward"));
      pStream->println(F("R = Reverse"));
      pStream->println(F(""));
      pStream->println(F("       Faster"));
      pStream->println(F("\t8"));
      pStream->println(F("Left 4\t5  6 Right"));
      pStream->println(F("\t2"));
      pStream->println(F("       Slower"));

      StopBothMotors();
      int speed = 0;
      bool bAutoMode = false;

      while (pStream->available())
      {
        incomingByte = pStream->read();
        pStream->printf("I removed 0X%X from Serial1\n", incomingByte);
      }

      incomingByte = 0;

      while (!bAutoMode)
      {
        //12/25/21 now using Stream* for serial
        if (pStream->available() > 0)
        {
          // read the incoming bytes:
          numchars = pStream->readBytesUntil('\n', buff, sizeof(buff));
          incomingByte = buff[0];

          // say what you got:
          pStream->printf("I received %d chars, first char 0X%X on Serial1\n", numchars, incomingByte);

          while (pStream->available())
          {
            pStream->read();
            pStream->printf("I removed 0X%X from Serial1\n", incomingByte);
          }
        }

        //11/21/21 incomingByte can come from either serial input
        if (incomingByte != 0)
        {
          switch (incomingByte)
          {
          case 0x30: //Dec '0'
            pStream->println(F("CCW 180 deg Turn"));
            SpinTurn(true, 180, 90);
            MoveAhead(speed, speed);
            break;
          case 0x31: //Dec '1'
            pStream->println(F("CW 180 deg Turn"));
            SpinTurn(false, 180, 45);
            break;
          case 0x34: //Turn left 10 deg and keep moving
            pStream->println(F("CCW 10 deg Turn"));
            SpinTurn(true, 10, 30);

            if (bIsForwardDir)
            {
              MoveAhead(speed, speed);
            }
            else
            {
              MoveReverse(speed, speed);
            }
            break;
          case 0x36: //Turn right 10 deg and keep moving
            pStream->print("CW 10 deg Turn\n");
            SpinTurn(false, 10, 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;
            pStream->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;
            pStream->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'
            pStream->println(F("Stopping Motors!"));
            StopBothMotors();
            speed = 0;
            break;
          case 0x41: //ASCII 'A'
          case 0x61: //ASCII 'a'
            StopBothMotors();
            pStream->println(F("Re-entering AUTO mode"));
            bAutoMode = true;
            break;
          case 0x52: //ASCII 'R'
          case 0x72: //ASCII 'r'
            pStream->println(F("Setting both motors to reverse"));
            bIsForwardDir = false;
            MoveReverse(speed, speed);
            break;
          case 0x46: //ASCII 'F'
          case 0x66: //ASCII 'f'
            pStream->println(F("Setting both motors to forward"));
            bIsForwardDir = true;
            MoveAhead(speed, speed);
#pragma endregion MANUALCONTROL
            break;

            //01/11/22 copied here from 'automatic' mode to allow firmware updates
            //even when in 'command' mode
          case 0x55: //ASCII 'U'
          case 0x75: //ASCII 'u'
#pragma region FIRMWARE UPDATE
            StopBothMotors();
            pStream->println(F("Start Program Update - Send new HEX file!"));

            //09/20/21 copied from FlasherX - loop()
            if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
            {
              pStream->printf("unable to create buffer\n"); Serial.flush();

              for (;;) {}
            }

            pStream->printf("buffer = %1luK %s (%08lX - %08lX)\n",
              buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",
              buffer_addr, buffer_addr + buffer_size);

            //09/20/21 clear the serial buffer
            while (pStream->available())
            {
              pStream->read();
            }

            // receive hex file via serial, write new firmware to flash, clean up, reboot
            update_firmware(&Serial1, buffer_addr, buffer_size); // no return if success

            // return from update_firmware() means error or user abort, so clean up and
            // reboot to ensure that static vars get boot-up initialized before retry
            pStream->printf("erase FLASH buffer / free RAM buffer...\n");
            firmware_buffer_free(buffer_addr, buffer_size);
            Serial1.flush();
            REBOOT;
            break;
#pragma endregion FIRMWARE UPDATE
          default:
            pStream->println(F("In Default Case: Stopping Motors!"));
            StopBothMotors();
            //while (true)
            //{

            //}
          }
          incomingByte = 0;
        }
      }
    }
  }
}

void InitAllPins()
{
  for (uint8_t i = 0; i < CORE_NUM_DIGITAL; i++)
  {
    pinMode(i, OUTPUT);
    digitalWrite(i, LOW);
  }
}

void EnableAllRearLEDs(bool bEnable)
{
  //Purpose:  Turns all 4 LEDs ON or OFF (LOW is ON)
  //Provenance: Created 05/02/17 gfp

  if (bEnable)
  {
    digitalWrite(CHG_LED_PIN, LOW);
    digitalWrite(_20PCT_LED_PIN, LOW);
    digitalWrite(_40PCT_LED_PIN, LOW);
    digitalWrite(_60PCT_LED_PIN, LOW);
    digitalWrite(_80PCT_LED_PIN, LOW);
    digitalWrite(FIN_LED_PIN, LOW);
  }
  else
  {
    digitalWrite(CHG_LED_PIN, HIGH);
    digitalWrite(_20PCT_LED_PIN, HIGH);
    digitalWrite(_40PCT_LED_PIN, HIGH);
    digitalWrite(_60PCT_LED_PIN, HIGH);
    digitalWrite(_80PCT_LED_PIN, HIGH);
    digitalWrite(FIN_LED_PIN, HIGH);
  }
}
#pragma endregion MISCELLANEOUS

#pragma region VL53L0X Sensor Support
void WaitForVL53L0XTeensy()
{
  bVL53L0X_TeensyReady = false;
  while (!bVL53L0X_TeensyReady)
  {
    GetRequestedVL53l0xValues(VL53L0X_READYCHECK); //this updates bVL53L0X_TeensyReady
    myTeePrint.printf("%lu: got %d from VL53L0X Teensy\n", millis(), bVL53L0X_TeensyReady);
    delay(100);
  }

  myTeePrint.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

  myTeePrint.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\n");

  GetRequestedVL53l0xValues(VL53L0X_ALL);
  myTeePrint.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)
  {
    GetRequestedVL53l0xValues(VL53L0X_ALL);
    myTeePrint.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);
  }

  myTeePrint.printf("VL53L0X Teensy Ready at %lu\n\n", millis());
}

//01/24/21 revised to implement error reporting
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
    // 01/16/22 rev to use I2C_Anything1 functions for Wire1

  //Step1: Send request to VL53L0X handler
  //DEBUG!!
    //myTeePrint.printf("Sending %d to slave\n", which);
  //DEBUG!!

  //myTeePrint.printf("In GetRequestedVL53l0xValues(%d)\n", (int)which);

  Wire1.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);
  //I2C_writeAnything1((uint8_t)which);
  I2C_writeAnything((uint8_t)which, &Wire1);
  Wire1.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()
    Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(sizeof(bVL53L0X_TeensyReady)));
    //readResult = I2C_readAnything1(bVL53L0X_TeensyReady);
    readResult = I2C_readAnything(bVL53L0X_TeensyReady, &Wire1);
    break;

  case VL53L0X_CENTERS_ONLY:
    //just two data values needed here
    data_size = 2 * sizeof(int);
    Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(2 * sizeof(Lidar_RightCenter)));
    readResult = I2C_readAnything(Lidar_RightCenter, &Wire1);

    if (readResult > 0)
    {
      I2C_readAnything(Lidar_LeftCenter, &Wire1);
    }

    //DEBUG!!
        //myTeePrint.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!!
        //myTeePrint.printf("data_size = %d\n", data_size);
    //DEBUG!!

    Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);
    readResult = I2C_readAnything(Lidar_RightFront, &Wire1);

    if (readResult > 0)
    {
      readResult = I2C_readAnything(Lidar_RightCenter, &Wire1);
    }
    if (readResult > 0)
    {
      readResult = I2C_readAnything(Lidar_RightRear, &Wire1);
    }
    if (readResult > 0)
    {
      readResult = I2C_readAnything(RightSteeringVal, &Wire1);
    }

    //DEBUG!!
        //myTeePrint.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);

    Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);
    readResult = I2C_readAnything(Lidar_LeftFront, &Wire1);

    if (readResult > 0)
    {
      readResult = I2C_readAnything(Lidar_LeftCenter, &Wire1);
    }
    if (readResult > 0)
    {
      readResult = I2C_readAnything(Lidar_LeftRear, &Wire1);
    }
    if (readResult > 0)
    {
      readResult = I2C_readAnything(LeftSteeringVal, &Wire1);
    }

    //DEBUG!!
        //myTeePrint.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
    data_size = 7 * sizeof(uint16_t) + 2 * sizeof(double); //10/31/20 added rear distance

    //myTeePrint.printf("In VL53L0X_ALL case with data_size = %d\n", data_size);

    Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

    //Lidar_LeftFront
    readResult = I2C_readAnything(Lidar_LeftFront, &Wire1);
    if (readResult != sizeof(Lidar_LeftFront))
    {
      myTeePrint.printf("Error reading Lidar_LeftFront\n");
    }

    //Lidar_LeftCenter
    readResult = I2C_readAnything(Lidar_LeftCenter, &Wire1);
    if (readResult != sizeof(Lidar_LeftCenter))
    {
      myTeePrint.printf("Error reading Lidar_LeftCenter\n");
    }

    //Lidar_LeftRear
    readResult = I2C_readAnything(Lidar_LeftRear, &Wire1);
    if (readResult != sizeof(Lidar_LeftRear))
    {
      myTeePrint.printf("Error reading Lidar_LeftRear\n");
    }

    //LeftSteeringVal
    readResult = I2C_readAnything(LeftSteeringVal, &Wire1);
    if (readResult != sizeof(LeftSteeringVal))
    {
      myTeePrint.printf("Error reading LeftSteeringVal\n");
    }

    //Lidar_RightFront
    readResult = I2C_readAnything(Lidar_RightFront, &Wire1);
    if (readResult != sizeof(Lidar_RightFront))
    {
      myTeePrint.printf("Error reading Lidar_RightFront\n");
    }

    //Lidar_RightCenter
    readResult = I2C_readAnything(Lidar_RightCenter, &Wire1);
    if (readResult != sizeof(Lidar_RightCenter))
    {
      myTeePrint.printf("Error reading Lidar_RightCenter\n");
    }

    //Lidar_RightRear
    readResult = I2C_readAnything(Lidar_RightRear, &Wire1);
    if (readResult != sizeof(Lidar_RightRear))
    {
      myTeePrint.printf("Error reading Lidar_RightRear\n");
    }

    //Lidar_Rear
    readResult = I2C_readAnything(Lidar_Rear, &Wire1);
    if (readResult != sizeof(Lidar_Rear))
    {
      myTeePrint.printf("Error reading Lidar_Rear\n");
    }

    //RightSteeringVal
    readResult = I2C_readAnything(RightSteeringVal, &Wire1);
    if (readResult != sizeof(RightSteeringVal))
    {
      myTeePrint.printf("Error reading LeftSteeringVal\n");
    }

    //myTeePrint.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);
    Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(sizeof(Lidar_Rear)));
    readResult = I2C_readAnything(Lidar_Rear, &Wire1);

    //DEBUG!!
        //myTeePrint.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
}
#pragma endregion VL53L0X Support

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

2250

2251

2252

2253

2254

2255

2256

2257

2258

2259

2260

2261

2262

2263

2264

2265

2266

2267

2268

2269

2270

2271

2272

2273

2274

2275

2276

2277

2278

2279

2280

2281

2282

2283

2284

2285

2286

2287

2288

2289

2290

2291

2292

2293

2294

2295

2296

2297

2298

2299

2300

2301

2302

2303

2304

2305

2306

2307

2308

2309

2310

2311

2312

2313

2314

2315

2316

2317

2318

2319

2320

2321

2322

2323

2324

2325

2326

2327

2328

2329

2330

2331

2332

2333

2334

2335

2336

2337

2338

2339

2340

2341

2342

2343

2344

2345

2346

2347

2348

2349

2350

2351

2352

2353

2354

2355

2356

2357

2358

2359

2360

2361

2362

2363

2364

2365

2366

2367

2368

2369

2370

2371

2372

2373

2374

2375

2376

2377

2378

2379

2380

2381

2382

2383

2384

2385

2386

2387

2388

2389

2390

2391

2392

2393

2394

2395

2396

2397

2398

2399

2400

2401

2402

2403

2404

2405

2406

2407

2408

2409

2410

2411

2412

2413

2414

2415

2416

2417

2418

2419

2420

2421

2422

2423

2424

2425

2426

2427

2428

2429

2430

2431

2432

2433

2434

2435

2436

2437

2438

2439

2440

2441

2442

2443

2444

2445

2446

2447

2448

2449

2450

2451

2452

2453

2454

2455

2456

2457

2458

2459

2460

2461

2462

2463

2464

2465

2466

2467

2468

2469

2470

2471

2472

2473

2474

2475

2476

2477

2478

2479

2480

2481

2482

2483

2484

2485

2486

2487

2488

2489

2490

2491

2492

2493

2494

2495

2496

2497

2498

2499

2500

2501

2502

2503

2504

2505

2506

2507

2508

2509

2510

2511

2512

2513

2514

2515

2516

2517

2518

2519

2520

2521

2522

2523

2524

2525

2526

2527

2528

2529

2530

2531

2532

2533

2534

2535

2536

2537

2538

2539

2540

2541

2542

2543

2544

2545

2546

2547

2548

2549

2550

2551

2552

2553

2554

2555

2556

2557

2558

2559

2560

2561

2562

2563

2564

2565

2566

2567

2568

2569

2570

2571

2572

2573

2574

2575

2576

2577

2578

2579

2580

2581

2582

2583

2584

2585

2586

2587

2588

2589

2590

2591

2592

2593

2594

2595

2596

2597

2598

2599

2600

2601

2602

2603

2604

2605

2606

2607

2608

2609

2610

2611

2612

2613

2614

2615

2616

2617

2618

2619

2620

2621

2622

2623

2624

2625

2626

2627

2628

2629

2630

2631

2632

2633

2634

2635

2636

2637

2638

2639

2640

2641

2642

2643

2644

2645

2646

2647

2648

2649

2650

2651

2652

2653

2654

2655

2656

2657

2658

2659

2660

2661

2662

2663

2664

2665

2666

2667

2668

2669

2670

2671

2672

2673

2674

2675

2676

2677

2678

2679

2680

2681

2682

2683

2684

2685

2686

2687

2688

2689

2690

2691

2692

2693

2694

2695

2696

2697

2698

2699

2700

2701

2702

2703

2704

2705

2706

2707

2708

2709

2710

2711

2712

2713

2714

2715

2716

2717

2718

2719

2720

2721

2722

2723

2724

2725

2726

2727

2728

2729

2730

2731

2732

2733

2734

2735

2736

2737

2738

2739

2740

2741

2742

2743

2744

2745

2746

2747

2748

2749

2750

2751

2752

2753

2754

2755

2756

2757

2758

2759

2760

2761

2762

2763

2764

2765

2766

2767

2768

2769

2770

2771

2772

2773

2774

2775

2776

2777

2778

2779

2780

2781

2782

2783

2784

2785

2786

2787

2788

2789

2790

2791

2792

2793

2794

2795

2796

2797

2798

2799

2800

2801

2802

2803

2804

2805

2806

2807

2808

2809

2810

2811

2812

2813

2814

2815

2816

2817

2818

2819

2820

2821

2822

2823

2824

2825

2826

2827

2828

2829

2830

2831

2832

2833

2834

2835

2836

2837

2838

2839

2840

2841

2842

2843

2844

2845

2846

2847

2848

2849

2850

2851

2852

2853

2854

2855

2856

2857

2858

2859

2860

2861

2862

2863

2864

2865

2866

2867

2868

2869

2870

2871

2872

2873

2874

2875

2876

2877

2878

2879

2880

2881

2882

2883

2884

2885

2886

2887

2888

2889

2890

2891

2892

2893

2894

2895

2896

2897

2898

2899

2900

2901

2902

2903

2904

2905

2906

2907

2908

2909

2910

2911

2912

2913

2914

2915

2916

2917

2918

2919

2920

2921

2922

2923

2924

2925

2926

2927

2928

2929

2930

2931

2932

2933

2934

2935

2936

2937

2938

2939

2940

2941

2942

2943

2944

2945

2946

2947

2948

2949

2950

2951

2952

2953

2954

2955

2956

2957

2958

2959

2960

2961

2962

2963

2964

2965

2966

2967

2968

2969

2970

2971

2972

2973

2974

2975

2976

2977

2978

2979

2980

2981

2982

2983

2984

2985

2986

2987

2988

2989

2990

2991

2992

2993

2994

2995

2996

2997

2998

2999

3000

3001

3002

3003

3004

3005

3006

3007

3008

3009

3010

3011

3012

3013

3014

3015

3016

3017

3018

3019

3020

3021

3022

3023

3024

3025

3026

3027

3028

3029

3030

3031

3032

3033

3034

3035

3036

3037

3038

3039

3040

3041

3042

3043

3044

3045

3046

3047

3048

3049

3050

3051

3052

3053

3054

3055

3056

3057

3058

3059

3060

3061

3062

3063

3064

3065

3066

3067

3068

3069

3070

3071

3072

3073

3074

3075

3076

3077

3078

3079

3080

3081

3082

3083

3084

3085

3086

3087

3088

3089

3090

3091

3092

3093

3094

3095

3096

3097

3098

3099

3100

3101

3102

3103

3104

3105

3106

3107

3108

3109

3110

3111

3112

3113

3114

3115

3116

3117

3118

3119

3120

3121

3122

3123

3124

3125

3126

3127

3128

3129

3130

3131

3132

3133

3134

3135

3136

3137

3138

3139

3140

3141

3142

3143

3144

3145

3146

3147

3148

3149

3150

3151

3152

3153

3154

3155

3156

3157

3158

3159

3160

3161

3162

3163

3164

3165

3166

3167

3168

3169

3170

3171

3172

3173

3174

3175

3176

3177

3178

3179

3180

3181

3182

3183

3184

3185

3186

3187

3188

3189

3190

3191

3192

3193

3194

3195

3196

3197

3198

3199

3200

3201

3202

3203

3204

3205

3206

3207

3208

3209

3210

3211

3212

3213

3214

3215

3216

3217

3218

3219

3220

3221

3222

3223

3224

3225

3226

3227

3228

3229

3230

3231

3232

3233

3234

3235

3236

3237

3238

3239

3240

3241

3242

3243

3244

3245

3246

3247

3248

3249

3250

3251

3252

3253

3254

3255

3256

3257

3258

3259

3260

3261

3262

3263

3264

3265

3266

3267

3268

3269

3270

3271

3272

3273

3274

3275

3276

3277

3278

3279

3280

3281

3282

3283

3284

3285

3286

3287

3288

3289

3290

3291

3292

3293

3294

3295

3296

3297

3298

3299

3300

3301

3302

3303

3304

3305

3306

3307

3308

3309

3310

3311

3312

3313

3314

3315

3316

3317

3318

3319

3320

3321

3322

3323

3324

3325

3326

3327

3328

3329

3330

3331

3332

3333

3334

3335

3336

3337

3338

3339

3340

3341

3342

3343

3344

3345

3346

3347

3348

3349

3350

3351

3352

3353

3354

3355

3356

3357

3358

3359

3360

3361

3362

3363

3364

3365

3366

3367

3368

3369

3370

3371

3372

3373

3374

3375

3376

3377

3378

3379

3380

3381

3382

3383

3384

3385

3386

3387

3388

3389

3390

3391

3392

3393

3394

3395

3396

3397

3398

3399

3400

3401

3402

3403

3404

3405

3406

3407

3408

3409

3410

3411

3412

3413

3414

3415

3416

3417

3418

3419

3420

3421

3422

3423

3424

3425

3426

3427

3428

3429

3430

3431

3432

3433

3434

3435

3436

3437

3438

3439

3440

3441

3442

3443

3444

3445

3446

3447

3448

3449

3450

3451

3452

3453

3454

3455

3456

3457

3458

3459

3460

3461

3462

3463

3464

3465

3466

3467

3468

3469

3470

3471

3472

3473

3474

3475

3476

3477

3478

3479

3480

3481

3482

3483

3484

3485

3486

3487

3488

3489

3490

3491

3492

3493

3494

3495

3496

3497

3498

3499

3500

3501

3502

3503

3504

3505

3506

3507

3508

3509

3510

3511

3512

3513

3514

3515

3516

3517

3518

3519

3520

3521

3522

3523

3524

3525

3526

3527

3528

3529

3530

3531

3532

3533

3534

3535

3536

3537

3538

3539

3540

3541

3542

3543

3544

3545

3546

3547

3548

3549

3550

3551

3552

3553

3554

3555

3556

3557

3558

3559

3560

3561

3562

3563

3564

3565

3566

3567

3568

3569

3570

3571

3572

3573

3574

3575

3576

3577

3578

3579

3580

3581

3582

3583

3584

3585

3586

3587

3588

3589

3590

3591

3592

3593

3594

3595

3596

3597

3598

3599

3600

3601

3602

3603

3604

3605

3606

3607

3608

3609

3610

3611

3612

3613

3614

3615

3616

3617

3618

3619

3620

3621

3622

3623

3624

3625

3626

3627

3628

Name: T35_WallE3_V5.ino

Created: 1/15/2022 8:08:22 PM

Author: NEWXPS15\paynt

01/15/22 - This version is intended to add the second deck functionality, and start integrating wall-following

01/20/22 - Now incorporating lessons learned from

https://www.fpaynter.com/2022/01/fixing-the-mess-i-made-with-the-i2cdevlib-mpu6050-libraries/

01/22/22 - replaced I2C_read/writeAnything() calls with new multiple-I2C-bus capable version

01/23/22 - now working properly with Teensy 3.5 VL53L0X array manager

Name: T35_WallE3_V4.ino

Created: 1/3/2022 3:44:09 PM

Author: NEWXPS15\paynt

- added GetOpMode() and the associated loop() switch and case code.

- added the IR Homing code so I could test the IR demod hardware.

- added #include "TeePrint.h" to incorporate dual-port printing in one line - see

(https://forum.pjrc.com/threads/69146-print-same-string-to-two-different-serial-ports)

for details

- replaced all 'Serialx.printf' instances with 'TeePrint.printf'

Name: T35_WallE3_V3.ino

Created: 11/26/2021 8:06:22 PM

Author: FRANKNEWXPS15\Frank

12/18/21: This version uses a TeraTerm script 'TeensyOTA1.ttl' in conjunction with Visual Micro's 'board.txt' feature

to accomplish OTA updates. Note that a 'regular' (F7) build does not trigger an OTA update,

but a DEBUG (F5) build does.

Implementation of OTA update requires that Joe Pasquariello's 'FlasherX'

code be added to the Teensy sketch as it is below, and some code to allow

an external serial communications program to trigger the update process.

#pragma region INCLUDES

#include <Wire.h>

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

#include <elapsedMillis.h>

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

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

#include <PID_v2.h> //new version 05/16/21

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

#include "TeePrint.h" //added 01/04/22 to print to both ports in one line

#pragma endregion Includes

#pragma region DEFINES

//02/29/16 hardware defines

#define NO_MOTORS

#define NO_MPU6050 //added 01/23/22

//#define IR_HOMING_ONLY

//#define NO_LIDAR

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

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

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

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

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

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

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

#define IRDET_I2C_ADDR 0x08

#define MPU6050_I2C_ADDR 0x68

//#define IR_HOMING_ONLY

#pragma endregion Program #Defines

#pragma region PRE_SETUP

MPU6050 mpu(MPU6050_I2C_ADDR);

elapsedMillis MsecSinceLastLEDToggle; //used for LED blink timer

elapsedMillis MsecSinceLastOpModeChk; //01/07/22 used for OpMode check

elapsedMillis MsecSinceLastIRHomingAdj; //01/07/22 used for #ifdef IR_HOMING_ONLY block

elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops

const int MSEC_PER_IR_HOMING_ADJ = 200; //01/07/22

const int MSEC_PER_OP_MODE_CHK = 200; //01/07/22

const int MSEC_PER_LOOP = 200;

const int MSEC_PER_DIST_UPDATE = 200; //01/07/22

TeePrint myTeePrint(Serial, Serial1);

#pragma region ENUMS

//01/05/16 enum types cannot be used as arguments or return types for functions due to Arduino pre-processor quirk

//12/20/15 added for navigation support

//01/15/16 revised to be consistent wtih nav modes identified in https://fpaynter.com/2016/01/making-wall-e2-smarter-using-karnaugh-maps/

enum NavCases

{

NAV_NONE = 0,

NAV_WALLTRK,

NAV_OBSTACLE,

NAV_STEPTURN,

NAV_STUCK,

NAV_OPENCNR

};

//04/10/20 Experiment with porting heading based tracking capability from two wheel robot

enum TrackingState

{

TRK_RIGHT_NONE,

TRK_RIGHT_CAPTURE,

TRK_RIGHT_MAINTAIN,

TRK_RIGHT_BACKUP_AND_TURN,

TRK_RIGHT_STEP_TURN

};

const char* NavStrArray[] = { "WallTrack", "Obstacle", "StepTurn", "Stuck", "OpenCorner" };

//03/08/17 added for new mode/state definitions; see https://fpaynter.com/2017/03/wall-e2-operating-mode-review/

enum OpModes

{

MODE_NONE = 0, //04/04/17 chg from MODE_DEFAULT and moved to top (zero) position

MODE_CHARGING,

MODE_IRHOMING,

MODE_WALLFOLLOW,

MODE_WALLCAPTURE, //added 07/16/21 to manage wall offset capture operations

MODE_DEADBATTERY, //added 01/16/18 to handle dead battery case

MODE_DISCHARGE //added 03/04/20 to safely discharge the battery

};

const char* ModeStrArray[] = { "None", "Charge", "Home", "Wall", "Capture", "DeadBatt", "Discharge" };

const char* LongModeStrArray[] = { "None", "Charging", "IR Homing", "Wall Following", "Wall Capture", "Dead Battery", "Discharging" };

enum WallTrackingCases

{

TRACKING_NONE = 0,

TRACKING_LEFT,

TRACKING_RIGHT,

TRACKING_NEITHER

};

const char* TrkStrArray[] = { "None", "Left", "Right", "Neither" };

#pragma endregion Tracking and Nav Case Enums

#pragma region ADC CONSTANTS

const double ADC_REF_VOLTS = 3.3; //teensy default for analog inputs

const int MAX_AD_COUNT = 1023;

const double AmpsPerVolt = 1.00; //default 10K Rs

const double VoltsPerCount = ADC_REF_VOLTS / MAX_AD_COUNT;

const float VOLTAGE_TO_CURRENT_RATIO = 1.f; //Used for both 'Total' and 'Run' sensors

#pragma endregion ADC CONSTANTS

#pragma region TELEMETRYSTRINGS

//const String IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD";

const String IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD\tRearD";

const char* LeftWallFollowTelemStr = "Msec\tLFront\tLCtr\tLRear\tFront\tFrontVar\tRear\tSteer\tOutput\tSetPt\tAdjDist\tLSpd\tRSpd";

const char* RightWallFollowTelemStr = "Msec\tRFront\tRCtr\tRRear\tFront\tFVar\tSteer\tOutput\tSetPt\tAdjDist\tLSpd\tRSpd";

//const String ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\n"; //rev 05/02/20

const String ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tRearD\n"; //rev 01/30/21

#pragma endregion Mode-specific telemetry header strings

#pragma region BATTCONSTS

//03/10/15 added for battery charge level monitoring

//const int LOW_BATT_THRESH_VOLTS = 7.4; //50% chg per http://batteryuniversity.com/learn/article/lithium_based_batteries

//const int LOW_BATT_THRESH_VOLTS = 8.4; //07/10/20 temp debug settingr//50% chg per http://batteryuniversity.com/learn/article/lithium_based_batteries

const float LOW_BATT_THRESH_VOLTS = 8.4; //12/18/20 chg to float to fix bug

//const long BATT_CHG_TIMEOUT_SEC = 36000; //10 HRS

const long BATT_CHG_TIMEOUT_SEC = 3600; //12/28/20 for test only

const float DEAD_BATT_THRESH_VOLTS = 6; //added 01/24/17

const float FULL_BATT_VOLTS = 8.4; //added 03/17/18. Chg to 8.4 03/05/20

const int MINIMUM_CHARGE_TIME_SEC = 10; //added 04/01/18

const float FULL_BATT_CURRENT_THRESHOLD = 0.5; //amps chg to 0.5A 03/02/19 per https://www.fpaynter.com/2019/03/better-battery-charging-for-wall-e2/

//const int CURRENT_AVERAGE_NUMBER = 10; //added 03/01/19

//const int VOLTS_AVERAGE_NUMBER = 5; //added 03/01/19

const int IR_HOMING_TELEMETRY_SPACING_MSEC = 200; //added 04/23/20

//battery fuel guage constants

const float _20PCT_BATT_VOLTS = DEAD_BATT_THRESH_VOLTS + 0.2f * (FULL_BATT_VOLTS - DEAD_BATT_THRESH_VOLTS);

const float _40PCT_BATT_VOLTS = DEAD_BATT_THRESH_VOLTS + 0.4f * (FULL_BATT_VOLTS - DEAD_BATT_THRESH_VOLTS);

const float _60PCT_BATT_VOLTS = DEAD_BATT_THRESH_VOLTS + 0.6f * (FULL_BATT_VOLTS - DEAD_BATT_THRESH_VOLTS);

const float _80PCT_BATT_VOLTS = DEAD_BATT_THRESH_VOLTS + 0.8f * (FULL_BATT_VOLTS - DEAD_BATT_THRESH_VOLTS);

const float ZENER_VOLTAGE_OFFSET = 5.84; //measured zener voltage

#pragma endregion Battery Constants

#pragma region DISTANCE_MEASUREMENT_SUPPORT

//misc LIDAR and Ping sensor parameters

const int MIN_FRONT_OBSTACLE_DIST_CM = 20; //rev 04/28/17 for better obstacle handling

const int CHG_STN_AVOIDANCE_DIST_CM = 40; //added 03/11/17 for charge stn avoidance

const int STEPTURN_DIST_CM = 50; //rev 06/29/20 to temporarily disable

const int MAX_FRONT_DISTANCE_CM = 400;

const int MAX_LR_DISTANCE_CM = 200; //04/19/15 now using sep parameters for front and side sensors

const int CHG_STN_FINAL_APPR_DIST_CM = 20; //added 03/11/17 for charge stn avoidance

//04/01/2015 added for 'stuck' detection support

const int FRONT_DIST_ARRAY_SIZE = 50; //11/22/20 doubled to acct for 10Hz update rate

const int FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19

const int LR_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg

const int REAR_DIST_ARRAY_SIZE = 5; //04/13/21 added for running avg calc

const int LR_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ

const int STUCK_FRONT_VARIANCE_THRESHOLD = 50; //chg to 50 04/28/17

const int STUCK_REAR_VARIANCE_THRESHOLD = 50; //added 01/08/22

const int NO_LIDAR_FRONT_VAR_VAL = 10 * STUCK_FRONT_VARIANCE_THRESHOLD; //01/16/19

uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte

//04/28/19 added to reinstate l/r dist running avg

//06/28/20 chg to uint_16 to accommodate change from cm to mm

uint16_t aLeftDistMM[LR_DIST_ARRAY_SIZE];

uint16_t aRightDistMM[LR_DIST_ARRAY_SIZE];

uint16_t aRearDistMM[REAR_DIST_ARRAY_SIZE];

double Rear_Dist_PrevVar = 0;

double Rear_Dist_PrevMean = 0;

//float RearDistPrevAvg = 0;

int curMinObstacleDistance = MIN_FRONT_OBSTACLE_DIST_CM;//added 03/11/17 for chg stn avoidance

//04/13/20 moved distance vars up here so can be initialized just before loop()

//01/08/22 no longer using ISR

int glFrontDistVal = 0;

int glRearDistVal = 0;

double glFrontvar = 0;

double glRearvar = 0;

//added 10/24/20

//const int STUCK_BACKUP_DISTANCE_CM = 25;

const int STUCK_BACKUP_DISTANCE_CM = 20; //11/21/28 shortened slightly

const int STUCK_FORWARD_DISTANCE_CM = 15;

const int MAX_REAR_DISTANCE_CM = 100;

const int MIN_REAR_OBSTACLE_DIST_CM = 10;

const int STUCK_BACKUP_TIME_MSEC = 1000;

const int STUCK_FORWARD_TIME_MSEC = 1000;

#pragma endregion Distance Measurement Support

#pragma region PIN ASSIGNMENTS

//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//

//=================== START PIN ASSIGNMENTS ===================//

//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//

#pragma region MOTOR PINS

//11/04/21 Now using Pololu VNH5019 drivers for all 4 motors

const int InA_Left = 22;

const int InB_Left = 21;

const int Spd_Left = 23;

const int InA_Right = 34;

const int InB_Right = 33;

const int Spd_Right = 35;

#pragma endregion MOTOR PINS

#pragma region CHG SUPP PINS

//12/19/21 updated here, schematic, and spreadsheet

const int CHG_CONNECT_PIN = A0; //output of photo resistor looking at TP5100 CHG LED

const int BATT_MON_PIN = A1; //connected to 5VLDO regulator module

const int TOT_CURR_PIN = A2; //connected to 1NA619 between charge plug and battery

const int RUN_CURR_PIN = A3; //connected to 1NA619 between battery and rest of robot

const int CHG_CONNECT_LED_PIN = 30; //illuminates when chg cable connected

//state-of-charge indicator LEDs

const int FIN_LED_PIN = 27;

const int _80PCT_LED_PIN = 26;

const int _60PCT_LED_PIN = 25;

const int _40PCT_LED_PIN = 24;

const int _20PCT_LED_PIN = 12;

const int CHG_LED_PIN = 11;

#pragma endregion CHG SUPP PINS

//Second Deck Pins

const int RED_LASER_DIODE_PIN = 5;//Laser pointer

const int LIDAR_MODE_PIN = 2; //LIDAR MODE pin (continuous mode)

const int VL53L0X_TEENSY_RESET_PIN = 4; //pulled low for 1 mSec in Setup()

//Miscellaneous pins

const int HOMING_PID_COMPUTE_CALL_PIN = CHG_CONNECT_LED_PIN; //using same LED for two indications

//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//

//=================== END PIN ASSIGNMENTS ===================//

//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//

#pragma endregion PIN ASSIGNMENTS

//04/25/21 moved here from MoveToDesiredFront/BackDist() functions

#pragma region Front/Back Offset Motion PID

const double OffsetDistKp = 5.f;

const double OffsetDistKi = 3.f;

const double OffsetDistKd = 0.0f;

double OffsetDistSetpointCm, OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive

double OffsetDistVal = 0; //has to be 'double'

PID OffsetDistPID(&OffsetDistVal, &OffsetDistOutput, &OffsetDistSetpointCm,

OffsetDistKp, OffsetDistKi, OffsetDistKd, DIRECT);//12/14/20 re-doing this from scratch

#pragma endregion

#pragma region CHG SUPP PARAMETERS

//02/06/21 added for charge connection debounce commented out 12/25/21

//const int CHG_CONNECT_PIN_READING_ARRAY_SIZE = 5;

//uint16_t aChgConnectReadings[CHG_CONNECT_PIN_READING_ARRAY_SIZE];

//const float CHG_CONNECTED_AVG_THRESHOLD = 0.8;

//const float CHG_DISCONNECTED_AVG_THRESHOLD = 0.2;

//12/25/21 now using analog input, so threshold scaled by analog ref voltage

const uint16_t CHG_CONNECTED_AVG_THRESHOLD = 0.2 * MAX_AD_COUNT; //integer truncation OK

const uint16_t CHG_DISCONNECTED_AVG_THRESHOLD = 0.8 * MAX_AD_COUNT;//integer truncation OK

//uint16_t ChgConnectReadingTotal;//this is the 'oldTot' in 'IsChargerConnected(bChgConnect, oldTot)'

long chgStartMsec;//added 02/24/17

//bool bChgConnect = false; //updated every MSEC_PER_LOOP Msec

float TotalAmps; //moved here from loop() 01/02/22

float RunAmps; //moved here from loop() 01/02/22

#pragma endregion CHG SUPP PARAMETERS

#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

const int MOTOR_SPEED_CAPTURE_OFFSET = 75; //added 06/21/20 for offset capture

const int TURN_START_SPEED = MOTOR_SPEED_QTR; //added 11/14/21

//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

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 commented out 11/22/21 as now the MPU6050 module is mounted 'Z-up'

#pragma endregion MPU6050 Support

#pragma region VL53L0X_TOF_LIDAR_SUPPORT

const int ToFArray_PARALLEL_FIND_Kp = 200; //0.50*Kp(crit)

const int ToFArray_PARALLEL_FIND_Ki = 0; //0.45*Kp(crit)

const int ToFArray_PARALLEL_FIND_Kd = 0;//0.60*Kp(crit)

const float ToFArray_PARALLEL_FIND_SETPOINT = 0.01; //09/22/20 moved here

const int ToFArray_OFFSET_CAPTURE_Kp = 200;

const int ToFArray_OFFSET_CAPTURE_Ki = 50;

const int ToFArray_OFFSET_CAPTURE_Kd = 50;

const int ToFArray_OFFSET_TRACK_Kp = 10;

const int ToFArray_OFFSET_TRACK_Kd = 0;

double LeftSteeringVal, RightSteeringVal; //added 08/06/20

double ToFSetpoint, ToFSteeringVal, ToFOutput;//10/06/17 chg input variable name to something more descriptive

PID ToFArrayPID(&ToFSteeringVal, &ToFOutput, &ToFSetpoint, ToFArray_PARALLEL_FIND_Kp, 0.0, 0.0, DIRECT);//06/19/20 use this for now

const int ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC = 200;

const float PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD = 0.1; //rev 06/21/20 - now using (F-R)/100

const float CAPTURE_APPROACH_STEERING_VALUE = 0.4; //rev 06/28/20 - now using (F-R)/100

const float WALL_OFFSET_CAPTURE_WINDOW_CM = 2.0; //just a guess

const int VL53L0X_I2C_SLAVE_ADDRESS = 0x20; ////Teensy 3.5 VL53L0X ToF LIDAR controller

//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

uint16_t Lidar_RightFront;

uint16_t Lidar_RightCenter;

uint16_t Lidar_RightRear;

uint16_t Lidar_LeftFront;

uint16_t Lidar_LeftCenter;

uint16_t Lidar_LeftRear;

uint16_t Lidar_Rear; //added 10/24/20

bool bVL53L0X_TeensyReady = false; //11/10/20 added to prevent bad data reads during Teensy setup()

elapsedMillis lastToFArrayTelemetryMsec; //used to space out telemetry prints

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

};

#pragma endregion VL53L0X ToF LIDAR Support

#pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS

elapsedMillis MsecSinceLastTurnRateUpdate;

const int MAX_SPIN_MOTOR_SPEED = 250;

const int MIN_SPIN_MOTOR_SPEED = 50;

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

double TurnRateOutput;

double TurnRateSetPoint_DPS;

double Ival = 0;

double lastErr = 0;

double lastTurnRateVal_DPS;

double TurnRateVal_DPS;

double Prev_HdgDeg = 0;

float tgt_deg;

float timeout_sec;

//11/16/21 almost perfect for 45 deg/sec, 100mSec

//11/17/21 and pretty good for 90 deg/sec too!

double TurnRate_Kp = 1.8;

double TurnRate_Ki = 1.0;

double TurnRate_Kd = 0.4;

//ported from FourWD_PulseTurnRateTest.ino

double TurnRatePIDOutput;

//06/02/

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 06/01/21

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

#pragma region OTA UPDATE

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

// hex_info_t struct for hex record and hex file info

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

typedef struct { //

char* data; // pointer to array allocated elsewhere

unsigned int addr; // address in intel hex record

unsigned int code; // intel hex record type (0=data, etc.)

unsigned int num; // number of data bytes in intel hex record

uint32_t base; // base address to be added to intel hex 16-bit addr

uint32_t min; // min address in hex file

uint32_t max; // max address in hex file

int eof; // set true on intel hex EOF (code = 1)

int lines; // number of hex records received

} hex_info_t;

void read_ascii_line(Stream* serial, char* line, int maxbytes);

int parse_hex_line(const char* theline, char* bytes,

unsigned int* addr, unsigned int* num, unsigned int* code);

int process_hex_record(hex_info_t* hex);

void update_firmware(Stream* serial, uint32_t buffer_addr, uint32_t buffer_size);

uint32_t buffer_addr, buffer_size; //09/20/21 copied from FlasherX - loop()

#pragma endregion OTA UPDATE

#pragma region IR_HOMING_SUPPORT

//01/07/22 no longer using ISR

//01/10/22 in Teensy land, better to use 'float' everywhere

uint8_t IR_HOMING_MODULE_SLAVE_ADDR = 8; //uint8_t type reqd here for Wire.requestFrom() call

double IRHomingSetpoint, IRHomingOutput;//10/06/17 chg input variable name to something more descriptive

double IRHomingLRSteeringVal = 0;

//these values all come from IR Homing Teensy via I2C calls

unsigned long IRFinalValue1 = 0;

unsigned long IRFinalValue2 = 0;

unsigned long IRHomingValTotalAvg = 0;

//04/05/21 from Z-N method, see https://www.fpaynter.com/2021/04/another-try-at-charging-station-homing-pid-tuning/

//const double IRHomingKp = 100.f;

//const double IRHomingKi = 90.f;

//const double IRHomingKd = 120.f;

//01/11/22 started over again with WallE3 and home-grown PID algorithm

//const double IRHomingKp = 100.f;

//const double IRHomingKp = 200.f; //one or two oscillations at start

//const double IRHomingKp = 250.f; //one or two oscillations at start

//const double IRHomingKp = 300.f; //defining this value as Kc for Z-N method

//const double IRHomingKi = 0.f;

//const double IRHomingKd = 0.f;

// 01/15/22 The Z-N values did not work very well - LOTS of oscillation/overshoot!

//const double IRHomingKp = 150.f; //0.5Kc

//const double IRHomingKi = 135.f; //0.45Kc

//const double IRHomingKd = 150.f; //0.5Kc

//01/15/22 starting with Kp = 200, then increasing Ki by increments of 50

//const double IRHomingKp = 200.f;

////const double IRHomingKi = 50.f;

//const double IRHomingKi = 100.f;

////const double IRHomingKd = 25.f; //too much

//const double IRHomingKd = 10.f; //better

//01/15/22 reduce Kp to 150, keeping others the same

const double IRHomingKp = 150.f;

const double IRHomingKi = 100.f;

const double IRHomingKd = 10.f; //better

//PID IRHomingPID(&IRHomingLRSteeringVal, &IRHomingOutput, &IRHomingSetpoint,

// IRHomingKp, IRHomingKi, IRHomingKd, REVERSE);//12/14/20 re-doing this from scratch

const long IR_BEAM_DETECTION_CHANNEL_MAX = 2621440;

const long IR_BEAM_DETECTION_THRESHOLD = 15000;

const double IR_HOMING_STEERING_VAL_CAPTURE_THRESHOLD = 0.3; //added 01/30/21

const double IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD = 0.8; //added 03/30/21

const int IRHOMING_VALUE_ARRAY_SIZE = 3; //added 03/16/21 for value average support

long int aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE];//added 03/16/21 for value average support

const int IRHOMING_IAP_OFFSET_LOW_THRESHOLD_PCT = 75; //03/21/21 if wall offset is less than this relative to IAP offset, then adjust

const int IRHOMING_IAP_OFFSET_HIGH_THRESHOLD_PCT = 110; //03/23/21 if wall offset is more than this relative to IAP offset, then adjust

const double IRHOMING_IAP_FINE_TUNE_STEERING_VALUE_THRESHOLD = 0.2; //03/21/21 fine-tune if more off-boresight than this

#pragma endregion IR_HOMING_SUPPORT

#pragma region LOOP_VARS

int leftspeednum = MOTOR_SPEED_HALF;

int rightspeednum = MOTOR_SPEED_HALF;

NavCases NavCase = NAV_WALLTRK;

WallTrackingCases TrackingCase = TRACKING_NEITHER; //added 01/05/16

WallTrackingCases PrevTrackingCase = TRACKING_LEFT; //only used decide which way to turn in the TRACKING_NEITHER case

OpModes PrevOpMode = MODE_NONE; //added 03/08/17, rev to MODE_NONE 04/04/17

OpModes CurrentOpMode = MODE_NONE; //added 10/13/17 so can use in motor speed setting routines

//04/10/20 added for experiment to port heading based wall tracking from two wheel robot

TrackingState CurrentTrackingState = TRK_RIGHT_NONE;

TrackingState PrevTrackingState = TRK_RIGHT_NONE;

//02/13/16 added for 'pause' debug

int m_FinalLeftSpeed = 0;

int m_FinalRightSpeed = 0;

//11/03/18 added for new incremental variance calc

double Front_Dist_PrevVar = 0;

double Front_Dist_PrevVMean = 0;

elapsedMillis lastHomingTelemetryMsec; //used to space out telemetry prints

#pragma endregion Loop Variables

#pragma endregion PRE_SETUP

void setup()

{

#pragma region PIN INITIALIZATION

//init ALL pins to OUTPUT_LOW, then change as necessary

//01/08/22 - THIS MUST BE FIRST STEP IN SETUP() (ask me how i know!)

//InitAllPins();

pinMode(LED_BUILTIN, OUTPUT);

#pragma region Teensy VL53L0X Array Init

//11/15/20 force VL53L0X Teensy to reset whenever main controller does

pinMode(VL53L0X_TEENSY_RESET_PIN, OUTPUT);

digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW);

delay(50);

digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH);

#pragma endregion Teensy VL53L0X Array Init

#pragma region Second Deck Pins

pinMode(RED_LASER_DIODE_PIN, OUTPUT);

pinMode(LIDAR_MODE_PIN, INPUT);

pinMode(VL53L0X_TEENSY_RESET_PIN, OUTPUT);

#pragma endregion Second Deck Pins

#pragma region Charge_Support_Pins

//current sensor pins

pinMode(RUN_CURR_PIN, INPUT); //02/24/19 now connected to 'Run Current' 1NA619 charge current sensor

digitalWrite(RUN_CURR_PIN, LOW); //turn off the internal pullup resistor

pinMode(TOT_CURR_PIN, INPUT);//02/24/19 now connected to 'Total Current' 1NA619 charge current sensor

digitalWrite(TOT_CURR_PIN, LOW); //turn off the internal pullup resistor

//Battery Voltage Monitor pin

pinMode(BATT_MON_PIN, INPUT);

digitalWrite(BATT_MON_PIN, LOW); //turn off the internal pullup resistor

//charge connect

pinMode(CHG_CONNECT_PIN, INPUT_PULLUP); //goes LOW when chg cable connected

digitalWrite(CHG_CONNECT_PIN, HIGH); //01/09/22 needed now that InitAllPins() forces it LOW

//charge connect status display pin (this will eventually be one of the LEDs on the rear LED panel)

pinMode(CHG_CONNECT_LED_PIN, OUTPUT); //12/16/20 lights LED when chg cable connected

digitalWrite(CHG_CONNECT_LED_PIN, HIGH); //01/09/22 needed now that InitAllPins() forces it LOW

//Charge status LED en/dis pins

pinMode(_60PCT_LED_PIN, OUTPUT);

pinMode(FIN_LED_PIN, OUTPUT);

pinMode(_80PCT_LED_PIN, OUTPUT);

pinMode(_40PCT_LED_PIN, OUTPUT);

pinMode(_20PCT_LED_PIN, OUTPUT);

#pragma endregion Charge_Support_Pins

#pragma region Motor_Pins

//motor pins

pinMode(InA_Left, OUTPUT);

pinMode(InB_Left, OUTPUT);

pinMode(Spd_Left, OUTPUT);

pinMode(InA_Right, OUTPUT);

pinMode(InB_Right, OUTPUT);

pinMode(Spd_Right, OUTPUT);

#pragma endregion Motor_Pins

#pragma endregion PIN INITIALIZATION

#pragma region SERIAL_PORTS

Serial.begin(115200);

delay(2000); //10/06/21 - just use fixed delay instead

Serial1.begin(115200); //used HC-05 'AT' commands to set this speed

delay(2000); //11/20/21 use fixed delay instead of waiting

//I2C bus

//Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT); //MPU6050,IR_DET. MPU6050 has 4.7K pullups

//Wire1.begin(I2C_MASTER, 0x00, I2C_PINS_37_38, I2C_PULLUP_EXT, 400000);//VL53L0X Array

Wire.begin();

Wire1.begin();

pinMode(LED_BUILTIN, OUTPUT);

//DEBUG!!

if (Serial)

{

Serial.printf("Serial = %p, Serial1 = %p\n", &Serial, &Serial1);

}

//DEBUG!!

if (Serial1)

{

Serial1.printf("Serial = %p, Serial1 = %p\n", &Serial, &Serial1);

}

//DEBUG!!

#pragma endregion SERIAL_PORTS

CheckForUserInput(); //01/13/22 - here so OTA procedure can maybe start faster

#pragma region MPU6050

#ifndef NO_MPU6050

myTeePrint.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);

myTeePrint.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

mpu.initialize();

// verify connection

float StartSec = 0; //used to time MPU6050 init

myTeePrint.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

myTeePrint.printf(F("Enabling DMP...\n"));

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..."));

Serial1.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

myTeePrint.printf(F("Calibrating...Retrieving Calibration Values\n\n"));

mpu.CalibrateGyro(); //using default value of 15

mpu.PrintActiveOffsets();

//loop heading display until stabilized

myTeePrint.printf(F("\nMsec\tHdg\n"));

UpdateIMUHdgValDeg();

Prev_HdgDeg = IMUHdgValDeg;

delay(100);

UpdateIMUHdgValDeg();

myTeePrint.printf("%lu\t%2.3f\t%2.3f\n", millis(), IMUHdgValDeg, Prev_HdgDeg);

while (abs(IMUHdgValDeg - Prev_HdgDeg) > 0.1f)

{

myTeePrint.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg);

Prev_HdgDeg = IMUHdgValDeg;

delay(100);

UpdateIMUHdgValDeg();

}

StartSec = millis() / 1000.f;

myTeePrint.printf("MPU6050 Ready at %2.2f Sec with delta = %2.3f\n", StartSec, IMUHdgValDeg - Prev_HdgDeg);

bMPU6050Ready = true;

delay(1000);

}

else //MPU6050 Init failed for some reason

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

myTeePrint.printf("DMP Initialization failed with code %d", devStatus);

//08/29/21 print out battery voltage on failure

float batV = GetBattVoltage();

myTeePrint.printf("Battery Voltage = %2.2f\n", batV);

bMPU6050Ready = false;

}

#endif // !NO_MPU6050

#pragma endregion MPU6050

CheckForUserInput(); //01/13/22 - here so OTA procedure can maybe start faster

#pragma region VL53L0X_TEENSY

myTeePrint.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);

Wire1.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

//bVL53L0X_TeensyReady = (Wire.endTransmission() == 0);

bVL53L0X_TeensyReady = (Wire1.endTransmission() == 0);

//mySerial.printf("%lu: VL53L0X Teensy Not Avail...\n", millis());

delay(100);

}

myTeePrint.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

#ifdef DISTANCES_ONLY

digitalWrite(RED_LASER_DIODE_PIN, HIGH); //enable the front laser dot

myTeePrint.printf("\n------------ DISTANCES ONLY MODE!!! -----------------\n\n");

int i = 0; //added 09/20/20 for in-line header display

myTeePrint.printf("Msec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\tRear\n");

while (true)

{

if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)

{

//just in case there is a long delay somewhere that allows MsecSinceLastDistUpdate to build up

while (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)

{

MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;

}

//09/20/20 re-display the column headers

if (++i % 20 == 0)

{

myTeePrint.printf("\nMsec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\tRear\n");

}

GetRequestedVL53l0xValues(VL53L0X_ALL);

myTeePrint.printf("%lu\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, Lidar_Rear);

}

#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

myTeePrint.println(F("Initializing Left, Right, Front Distance Arrays..."));

#ifndef NO_VL53L0X

//03/30/21 moved FrontDistArray init ahead of left/right dist init to prevent inadvertent 'stuck' detection

myTeePrint.println(F("Initializing Front Distance Array"));

InitFrontDistArray(); //08/12/20 code extracted to fcn so can call elsewhere

myTeePrint.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

myTeePrint.printf("%d\t%d\n", Lidar_LeftCenter, Lidar_RightCenter);

UpdateLRDistanceArrays(Lidar_LeftCenter, Lidar_RightCenter);

}

myTeePrint.println(F("Updating Rear Distance Arrays"));

InitRearDistArray();

myTeePrint.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_VL53L0X

#pragma endregion L/R/FRONT DISTANCE ARRAYS

#pragma region IRDET_TEENSY

#ifndef NO_IRDET

bool IRDET_TeensyOK = false;

myTeePrint.printf("\nChecking for Teensy 3.2 IRDET Controller at I2C addr 0x%x\n", IR_HOMING_MODULE_SLAVE_ADDR);

while (!IRDET_TeensyOK)

{

Wire.beginTransmission(IR_HOMING_MODULE_SLAVE_ADDR);

IRDET_TeensyOK = (Wire.endTransmission() == 0);

myTeePrint.printf("%lu: IRDET Teensy Not Avail...\n", millis());

delay(100);

}

myTeePrint.printf("IRDET Teensy Ready at %lu\n", millis());

//get initial values

long Fin1, Fin2;//04/05/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)

float SteeringValue;

Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, sizeof(Fin1) + sizeof(Fin2) + sizeof(SteeringValue));

I2C_readAnything(Fin1);

I2C_readAnything(Fin2);

I2C_readAnything(SteeringValue);

myTeePrint.printf("Fin1/Fin2/SteeringVal = %ld\t%ld\t%2.4f\n", Fin1, Fin2, SteeringValue);

#endif // !NO_IRDET

#pragma endregion IRDET_TEENSY

CheckForUserInput(); //01/13/22 - here so OTA procedure can maybe start faster

#ifdef IR_HOMING_ONLY

//myTeePrint.printf("Msec\tValAvg\tSteer\t\tRRear\tRCtr\tRFront\tFront\n");

myTeePrint.printf("Msec\tValAvg\tSteer\n");

MsecSinceLastIRHomingAdj = 0;

while (true)

{

CheckForUserInput();

if (MsecSinceLastIRHomingAdj >= MSEC_PER_IR_HOMING_ADJ)

{

MsecSinceLastIRHomingAdj -= MSEC_PER_IR_HOMING_ADJ;

UpdateIRHomingValues();

//myTeePrint.printf("%lu\t%lu\t%2.2f\t\t%d\t%d\t%d\t%d\n",

// millis(), IRHomingValTotalAvg, IRHomingLRSteeringVal, Lidar_RightRear, Lidar_RightCenter, Lidar_RightFront, glFrontDistVal);

myTeePrint.printf("%lu\t%lu\t%lu\t%lu\t%2.2f\n",

millis(), IRFinalValue1, IRFinalValue2, IRHomingValTotalAvg, IRHomingLRSteeringVal);

}

IRHomeToChgStnNoPings(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

}

#endif

#pragma region IR_BEAM_VALUE_AVG_ARRAY

//03/17/21 added for IR beam value averaging array init

Serial.printf("%lu: Initializing IR Beam Value Averaging Array...\n", millis());

for (size_t i = 0; i < IRHOMING_VALUE_ARRAY_SIZE; i++)

{

aIRHOMINGVALTOTALS[i] = 0;

}

IRHomingValTotalAvg = 0;

#pragma endregion IR_BEAM_VALUE_AVG_ARRAY

analogReadAveraging(8); //applies to all analogRead() operations?

myTeePrint.printf("Msec\tBattV\tTotAmps\tRunAmps\tHdgDeg\tChgConn\n");

MsecSinceLastOpModeChk = 0; //main opMode update timer

}

void loop()

{

if (Serial1.available() || Serial.available())

{

CheckForUserInput();

}

if (MsecSinceLastOpModeChk >= MSEC_PER_OP_MODE_CHK)

{

MsecSinceLastOpModeChk -= MSEC_PER_OP_MODE_CHK;

digitalToggle(LED_BUILTIN);

digitalWrite(RED_LASER_DIODE_PIN, HIGH); //fire the laser pointer

//Step1: Determine current operating mode

CurrentOpMode = (OpModes)GetOpMode();

String trackstr = "Unknown"; //used for telemetry printouts

//Step2: Switch to appropriate operating case block

#pragma region OP_MODE SWITCH

switch (CurrentOpMode)

{

case MODE_DEADBATTERY:

break;

case MODE_DISCHARGE:

break;

#pragma region MODE_CHARGING

case MODE_CHARGING:

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

chgStartMsec = millis(); //used for charge duration telemetry

myTeePrint.print(F("Starting Charge Mode at ")); Serial.println(chgStartMsec);

StopBothMotors(); //added 03/13/17

//04/27/21

#ifndef NO_VL53L0X

myTeePrint.printf("Reset VL53L0X Teensy to reduce current consumption\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption

#endif

MonitorChargeUntilDone();

#ifndef NO_VL53L0X

myTeePrint.printf("Restart VL53L0X Teensy...\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy

WaitForVL53L0XTeensy(); //wait for it to come back up.

#endif

ExecDisconManeuver();

PrevOpMode = MODE_NONE; //04/27/21: this will force wall offset recapture

//myTeePrint.printf("%lu: just after ExecDisconManeuver() in TRACKING_RIGHT case block\n", millis());

break;

#pragma endregion MODE_CHARGING

#pragma region MODE_IRHOMING

case MODE_IRHOMING:

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

{ //brackets required to set the scope of the new variable (float batVoltage)

float batVoltage = GetBattVoltage();

myTeePrint.printf("IR Beam Detected with Battery Voltage = %1.2f\n", batVoltage);

//PrevOpMode = MODE_IRHOMING;

if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry

{

Serial.println("Not Hungry - Avoiding Charger");

//04/27/21 too complicated - just have robot reverse course

int leftdist = GetAvgLeftDistCm();

int rightdist = GetAvgRightDistCm();

SpinTurn(leftdist > rightdist, 180, 45);

PrevOpMode = MODE_NONE;

}

else //hungry - try to connect

{

Serial.println("Low battery - homing to Charger");

//IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

}

} //brackets required to set the scope of the new variable (float batVoltage)

break;

#pragma endregion MODE_IRHOMING

#pragma region MODE_WALLCAPTURE

case MODE_WALLCAPTURE:

break;

#pragma endregion MODE_WALLCAPTURE

#pragma region MODE_WALLFOLLOW

case MODE_WALLFOLLOW:

break;

#pragma ENDregion MODE_WALLFOLLOW

case MODE_NONE:

break;

}

//digitalWrite(CHG_CONNECT_LED_PIN, !bChgConnect); //LOW output illuminates LED

//if (bChgConnect)

//{

// MonitorChargeUntilDone();

// MsecSinceLastLEDToggle = 0; //reset so don't get hundreds of lines of output after long charge

// //01/02/22 have to re-print the headings with leading blank line after charging completes

// myTeePrint.printf("\nMsec\tBattV\tTotAmps\tRunAmps\tHdgDeg\tChgConn\n");

//}

////Get battery voltage

//float calc_volts = GetBattVoltage();

////get current heading

//UpdateIMUHdgValDeg(); //updates IMUHdgValDeg

////update IR homing/steering values

////Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, sizeof(IRFinalValue1) + sizeof(IRFinalValue2) + sizeof(IRHomingLRSteeringVal));

//Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, (size_t)(sizeof(IRFinalValue1) + sizeof(IRFinalValue2) + sizeof(IRHomingLRSteeringVal)));

//I2C_readAnything(IRFinalValue1);

//I2C_readAnything(IRFinalValue2);

//I2C_readAnything(IRHomingLRSteeringVal);

//IRHomingValTotalAvg = UpdateIRHomingValueTotalAverage(IRFinalValue1, IRFinalValue2, IRHomingValTotalAvg);

////print out battery voltage, currents, and chgr connection state

//myTeePrint.printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\n", millis(), calc_volts, TotalAmps, RunAmps, IMUHdgValDeg, bChgConnect);

}

#pragma region CHARGE SUPPORT FUNCTIONS

float GetAmps(int pin_number)

{

//Purpose: Get current in amps

//Inputs:

// pin_number = integer denoting analog pin to be used for measurement

// VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio

// MAX_AD_COUNT = int denoting max A/D reading value

// VOLTAGE_TO_CURRENT_RATIO = int denoting conversion ratio

//Outputs:

// returns total robot current (chg current plus running current)

//Notes:

// 02/28/18 chg name from GetBattChgAmps() to GetTotalAmps()

// 11/24/21 chg name, add pin_number param so can use for both Itot & Irun

int reading = analogRead(pin_number); //range is 0-1023

float volts = ((float)reading / (float)MAX_AD_COUNT) * ADC_REF_VOLTS;

float amps = volts * VOLTAGE_TO_CURRENT_RATIO;

//DEBUG!!

//myTeePrint.printf("GetAmps(): reading, volts, amps = %d, %3.2f, %3.2f\n",

// reading, volts, amps);

//DEBUG!!

return amps;

}

bool IsStillCharging()

{

//Purpose: Determine battery charge status

//Inputs:

// Battry charging current in amps from GetBattChgAmps()

// Battery voltage from GetBattV()

//Outputs:

// returns TRUE if battery voltage is below full charge voltage threshold

// AND charging current is above full charge current threshold. Otherwise returns FALSE

float BattV = GetBattVoltage();

float TotI = GetAmps(TOT_CURR_PIN);

float RunI = GetAmps(RUN_CURR_PIN);

//DEBUG!!

//myTeePrint.printf("IsStillCharging(): BattV = %2.3f, TotI = %2.3f, RunI = %2.3f\n", BattV, TotI, RunI);

//DEBUG!!

return (BattV < FULL_BATT_VOLTS&& TotI - RunI > FULL_BATT_CURRENT_THRESHOLD);

}

bool IsChargerConnected(bool curState)

{

//Purpose: Determine if robot has connected to charger

//Inputs:

//oldTot = previous readings total

//curState = current value of bChgConnected

//Outputs:

// true when array total <= CHG_CONNECTED_AVG_THRESHOLD

// false when array total >= CHG_DISCONNECTED_AVG_THRESHOLD

// otherwise maintains previous state

//Notes:

// 12/25/21 - now pin is connected to voltage divider. Goes LOW when charging

// 12/25/21 - now using AnalogReadAveraging(8) everywhere, so no need to do running avg

bool retStatus = curState;

uint16_t adval = (uint16_t)analogRead(CHG_CONNECT_PIN);

if (adval <= CHG_CONNECTED_AVG_THRESHOLD) //low means 'connected'

{

retStatus = true;

}

else if (adval >= CHG_DISCONNECTED_AVG_THRESHOLD)

{

retStatus = false;

}

//myTeePrint.printf("%lu: curState = %d, adval = %d, ConThresh = %d, disConThresh = %d retsatus = %d\n",

// millis(), curState, adval, CHG_CONNECTED_AVG_THRESHOLD, CHG_DISCONNECTED_AVG_THRESHOLD, retStatus);

//else return previous conn/disconnect state

return retStatus;

}

bool MonitorChargeUntilDone()

{

//Purpose: Monitor charging status until charge is complete

//Inputs: startMsec = millis() at the time of the function call

//Outputs:

// returns TRUE if charging completes successfully, FALSE otherwise

// provides mode-specific telemetry to PC via Wixel

//Plan:

// Step1: Blink charger display LEDs

// Step1: Get current time check for sufficiently elapsed time

// Step2: Get charger status signals, and echo them to display LEDs

// Step2: Send telemetry to PC via Serial port (Wixel)

// Step2: Check for end-of-charge or failure (don't know what this would be yet...)

//Notes:

// 03/11/17 for testing, rev to return as soon as connection dropped

// 05/21/17 rev to xmit telemetry before loop & then delay a bit before entering loop

// 05/21/17 abstracted status reporting code to separate function

// 10/16/17 removed startMsec from call sig

// 03/15/18 revised for TP5100 charger module

// 04/01/18 rev to always stay on charge for at least MINIMUM_CHARGE_TIME_SEC sec

// 02/24/19 rev to use new 1NA169 current sensor output

// 02/28/19 moved ChargeTelemetryString printout here from MODE_CHARGING case

// 01/30/21 added rear distance readout for debugging zero distance problem

// 02/06/21 repl bChgConn with ISR-managed bChgConnect

// 04/02/21 added code to blink LED associated with current charge level

// 01/02/22 ported to Wall-E3

//Step1: Get current time & check for sufficient elapsed time

int ElapsedChgTimeSec = 0;

float ElapsedChgTimeMin = 0; //added 05/02/20

myTeePrint.println(ChargingTelemStr); //moved here from main loop MODE_CHARGING case

bool bStillCharging = true;

bool bChgConnect = true; //01/08/22 no longer using ISR

//EnableAllRearLEDs(false);

while (ElapsedChgTimeSec < MINIMUM_CHARGE_TIME_SEC ||

(bStillCharging

&& ElapsedChgTimeSec < BATT_CHG_TIMEOUT_SEC

&& bChgConnect)

)

{

//04/02/21 moved to 'fast' part of loop

float BattV = GetBattVoltage();

float TotI = GetAmps(TOT_CURR_PIN);

float RunI = GetAmps(RUN_CURR_PIN);

//UpdateChgStatusLEDs(BattV, bStillCharging); //updates 'fuel guage' LEDs 04/22/20 added bStillCharging to sig

//05/02/20 rev to only print out 10 times/min

if (ElapsedChgTimeSec % 6 == 0)

{

ElapsedChgTimeMin = (float)ElapsedChgTimeSec / 60.f;

myTeePrint.printf("%3.1f\t%2.4f\t%2.4f\t%2.4f\t%2.4f\n",

ElapsedChgTimeMin, BattV, TotI, RunI, TotI - RunI); //rev 02/24/19 for 1Na169 sensor

}

CheckForUserInput(); //added 04/02/21

delay(1000); //one-second loop

ElapsedChgTimeSec++;

bStillCharging = IsStillCharging(); //02/24/19 - now using 1NA169 current sensor

bChgConnect = IsChargerConnected(bChgConnect); //01/02/22 - wasn't being checked

}

//Step2: Check for end-of-charge or failure (don't know what this would be yet...)

//if charging ran over time, something went wrong

time_t t = now();

if (!bChgConnect) //charger unplugged

{

Serial.printf("Charge connection dropped after %2.2f minutes at %d:%d:%d elapsed time\n", (float)(ElapsedChgTimeSec / 60.), hour(t), minute(t), second(t));

myTeePrint.printf("Charge connection dropped after %2.2f minutes at %d:%d:%d elapsed time\n", (float)(ElapsedChgTimeSec / 60.), hour(t), minute(t), second(t));

return false;

}

else if (ElapsedChgTimeSec < BATT_CHG_TIMEOUT_SEC)

{

myTeePrint.printf("Charging Completed Successfully in %2.2f minutes at %d:%d:%d elapsed time\n", (float)(ElapsedChgTimeSec / 60.), hour(t), minute(t), second(t));

return true;

}

else

{

myTeePrint.printf("Charging timout value of %2.2f minutes expired at\n", (float)(BATT_CHG_TIMEOUT_SEC / 60.), hour(t), minute(t), second(t));

return false;

}

float GetBattVoltage()

{

//02/18/17 get corrected battery voltage. Voltage reading is 1/3 actual Vbatt value

int analog_batt_reading = analogRead(BATT_MON_PIN);//analogReadAveraging(8) in setup() does internal averaging

float calc_volts = ZENER_VOLTAGE_OFFSET + ADC_REF_VOLTS * (double)analog_batt_reading / (double)MAX_AD_COUNT;

//DEBUG!!

//myTeePrint.printf("a/d = %d, calc = %2.2f\n", analog_batt_reading,calc_volts);

//DEBUG!!

return calc_volts;

}

bool ExecDisconManeuver()

{

//Purpose: Disconnect from charging station

//Inputs: Call from Charging Mode case block

//Outputs: Robot disconnects from charging station, backs up, and turns 90 away from near wall

//Plan:

// Step1: Turn OFF charger status LEDs (added 04/28/17)

// Step2: Determine which side wall is closer

// Step3: Back straight up for long enough to clear side rails

// Step4: Turn 90 away from near side wall

//Notes:

// 02/15/18 rev to use full speed to disengage, and new rolling turn routines

// 03/27/18 rev for TP5100 charging module

float batv = GetBattVoltage();

myTeePrint.printf("in ExecDisconManeuver() with BattV = %2.4f\n", batv);

//Step1: Turn OFF charger status LEDs (added 04/28/17)

//chg status LEDs are all enabled via a LOW digital output

//03/15/18 rev for TP5100

digitalWrite(FIN_LED_PIN, HIGH); //output is LOW (active) when Pw1_IN is HIGH/TRUE

digitalWrite(_80PCT_LED_PIN, HIGH); //output is LOW (active) when Pw2_IN is HIGH/TRUE

digitalWrite(_60PCT_LED_PIN, HIGH); //output is LOW (active) when Chg1_IN is LOW/FALSE

digitalWrite(_40PCT_LED_PIN, HIGH); //output is LOW (active) when Chg2_IN is LOW/FALSE

digitalWrite(_20PCT_LED_PIN, HIGH); //output is LOW (active) when Fin1_IN is LOW/FALSE

digitalWrite(CHG_LED_PIN, HIGH); //output is LOW (active) when Fin2_IN is LOW/FALSE

//Step2: Determine which side wall is closer. Ping sensors on 2nd deck can see over charger side rails

int leftdist = GetAvgLeftDistCm();

int rightdist = GetAvgRightDistCm();

Serial.print("leftdist = "); Serial.print(leftdist); Serial.print(", ");

Serial.print("rightdist = "); Serial.println(rightdist);

//Step3: Back straight up for long enough to clear side rails

#ifndef NO_VL53L0X

MoveToDesiredFrontDistCm(70); //70cm is plenty to clear the guide rails

#endif

StopBothMotors();

delay(1000);

//Step4: Turn around and go the other way

//SpinTurn(true, 180, 30); //slightly higher rate than default 20dps

SpinTurn(leftdist > rightdist, 180, 30); //slightly higher rate than default 20dps

return true; //can't think of anything else at the moment

}

#pragma endregion CHARGE SUPPORT FUNCTIONS

#pragma region DISTANCE_MEASUREMENT_SUPPORT

//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!!

//myTeePrint.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;

myTeePrint.printf("%lu: aFrontDist Init: Front_Dist_PrevVMean = %3.2f, Front_Dist_PrevVar = %3.2f\n", millis(), Front_Dist_PrevVMean, Front_Dist_PrevVar);

glFrontvar = Front_Dist_PrevVar; //added 03/30/21 to prevent 'stuck' at startup

}

//08/12/20 Extracted inline FRONT_DIST_ARRAY init code so can be called from anywhere

void InitRearDistArray()

{

//Purpose: initialize rear distance array and initial values for mean and variance

//Inputs:

// aRearDistMM = array of rear distance values

// REAR_DIST_ARRAY_SIZE = number of rear distance values stored

//Outputs:

// aRearDistMM = array of rear distance values

// Rear_Dist_PrevMean = previous mean value initialized

// Rear_Dist_PrevVar = previous variance value initialized

//Notes:

// 01/08/22 no longer using ISR

int idx = 0;

while (idx < REAR_DIST_ARRAY_SIZE)

{

GetRequestedVL53l0xValues(VL53L0X_REAR_ONLY);

aRearDistMM[idx] = Lidar_Rear;

idx++;

delay(100);

}

//04/19/19 init Rear_Dist_PrevMean & Rear_Dist_PrevVar to mean/var respectively

long sum = 0;

for (int i = 0; i < REAR_DIST_ARRAY_SIZE; i++)

{

sum += aRearDistMM[i]; //adds in rest of values

}

Rear_Dist_PrevMean = (float)sum / (float)REAR_DIST_ARRAY_SIZE;

// Step2: calc new 'brute force' variance

float sumsquares = 0;

for (int i = 0; i < REAR_DIST_ARRAY_SIZE; i++)

{

sumsquares += (aRearDistMM[i] - Rear_Dist_PrevMean) * (aRearDistMM[i] - Rear_Dist_PrevMean);

}

Rear_Dist_PrevVar = sumsquares / REAR_DIST_ARRAY_SIZE;

myTeePrint.printf("%lu: aRearDistMM Init: Rear_Dist_PrevMean = %3.2f, Rear_Dist_PrevVar = %3.2f\n", millis(), Rear_Dist_PrevMean, Rear_Dist_PrevVar);

}

//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!!

//myTeePrint.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!!

//myTeePrint.printf("avgdisttot = %d\n", avgdist);

//DEBUG!!

avgdist = (int)((float)avgdist / (float)FRONT_DIST_AVG_WINDOW_SIZE);

//DEBUG!!

//myTeePrint.printf("avgdist = %d\n", avgdist);

//DEBUG!!

return avgdist;

}

float GetAvgRightDistCm()

{

//Notes:

// 04/09/20 revised to compute proper running average of

// latest LR_AVG_WINDOW_SIZE ping measurements

// 10/08/20 aRight/LeftDist arrays now contain mm vs cm. rev to convert mm to cm on exit

//int rightavgdist_cm = 0;

int rightavgdist_mm = 0;

for (int validx = 0; validx < LR_AVG_WINDOW_SIZE; validx++)

{

//rightavgdist_cm += aRightDistMM[LR_DIST_ARRAY_SIZE - 1 - validx];

rightavgdist_mm += aRightDistMM[LR_DIST_ARRAY_SIZE - 1 - validx];

}

//float avg = (float)rightavgdist_cm / (float)LR_AVG_WINDOW_SIZE;

float avg = (float)rightavgdist_mm / (float)LR_AVG_WINDOW_SIZE;

//return avg;

return avg / 10.f;

}

float GetAvgLeftDistCm()

{

//Notes:

// 04/09/20 revised to compute proper running average of

// latest LR_AVG_WINDOW_SIZE ping measurements

//int leftavgdist_cm = 0;

int leftavgdist_mm = 0;

for (int validx = 0; validx < LR_AVG_WINDOW_SIZE; validx++)

{

//leftavgdist_cm += aLeftDistMM[LR_DIST_ARRAY_SIZE - 1 - validx];

leftavgdist_mm += aLeftDistMM[LR_DIST_ARRAY_SIZE - 1 - validx];

}

//float avg = (float)leftavgdist_cm / (float)LR_AVG_WINDOW_SIZE;

float avg = (float)leftavgdist_mm / (float)LR_AVG_WINDOW_SIZE;

//return avg;

return avg / 10.f;

}

//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();

myTeePrint.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

}

double CalcFrontDistArrayVariance(unsigned long newdistval)

{

//Purpose: Calculate Variance of input array

//Inputs: aDistArray = FRONT_DIST_ARRAY_SIZE array of integers representing left/right/front distances

//Outputs: Variance of selected array

//Plan:

// Step1: Calculate mean for array

// Step2: Sum up squared deviation of each array item from mean

// Step3: Divide squared deviation sum by number of array elements

//Notes:

// 11/01/18 this function takes about 1.8mSec - small compared to 200mSec loop interval

// 11/02/18 added distval to sig to facilitate incremental calc algorithm

// 11/12/18 re-wrote incr alg

// see C:\Users\Frank\Documents\Arduino\FourWD_WallE2_V1\Variance.xlsm

// and C:\Users\Frank\Documents\Arduino\VarianceCalcTest.ino

// 01/16/19 added 'return inc_var'

// 04/21/19 copied number overflow corrections from VarianceCalcTest.ino

// 04/28/19 commented out the 'brute force' sections - now using incr var exclusively

//unsigned long funcStartMicrosec = micros();

//11/03/18 update distance array, saving oldest for later use in incremental calcs

unsigned long oldestDistVal = aFrontDist[0];

for (int i = 0; i < FRONT_DIST_ARRAY_SIZE - 1; i++)

{

aFrontDist[i] = aFrontDist[i + 1];

}

aFrontDist[FRONT_DIST_ARRAY_SIZE - 1] = newdistval;

//11/02/18 now re-do the calculation using the incremental method, and compare the times

//mu_t = mu_(t-1) - dist_(t-N)/N + dist_t/N

//Example: mu_7 = mu_(6) - dist_(2)/N + dist_7/N

//var^2_t = var^2_(t-1) + dist^2_(t) - dist^2_(t-N) + mu^2_(t-1) - mu^2_t

//Example: var^2_7 = var^2_(6) + dist^2_(7) - dist^2_(t-N) + mu^2_(6) - mu^2_7

//DEBUG!!

//for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++)

//{

// Serial.print("aDistArray["); Serial.print(i); Serial.print("] = "); Serial.println(aDistArray[i]);

//}

//DEBUG!!

double inc_mean = Front_Dist_PrevVMean - (double)oldestDistVal / (double)FRONT_DIST_ARRAY_SIZE + (double)newdistval / (double)FRONT_DIST_ARRAY_SIZE;

unsigned long olddist_squared = oldestDistVal * oldestDistVal;

unsigned long newdist_squared = newdistval * newdistval;

double last_incmean_squared = Front_Dist_PrevVMean * Front_Dist_PrevVMean;

double inc_mean_squared = inc_mean * inc_mean;

double inc_var = Front_Dist_PrevVar + ((double)newdist_squared / FRONT_DIST_ARRAY_SIZE) - ((double)olddist_squared / FRONT_DIST_ARRAY_SIZE)

+ last_incmean_squared - inc_mean_squared;

//long uSecI = micros() - funcStartMicrosec - uSecB;

//DEBUG!!

//display results:

//myTeePrint.printf("%lu\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\n", millis(),

// newdistval, oldestDistVal, Front_Dist_PrevVMean, Front_Dist_PrevVar, inc_var);

//DEBUG!!

Front_Dist_PrevVar = inc_var; //save for next time

Front_Dist_PrevVMean = inc_mean; //save for next time

return inc_var; //added 01/16/19

}

double CalcRearDistArrayVariance(unsigned long newdistval)

{

//Purpose: Calculate Variance of input array

//Inputs: aDistArray = FRONT_DIST_ARRAY_SIZE array of integers representing left/right/front distances

//Outputs: Variance of selected array

//Plan:

// Step1: Calculate mean for array

// Step2: Sum up squared deviation of each array item from mean

// Step3: Divide squared deviation sum by number of array elements

//Notes:

// 11/01/18 this function takes about 1.8mSec - small compared to 200mSec loop interval

// 11/02/18 added distval to sig to facilitate incremental calc algorithm

// 11/12/18 re-wrote incr alg

// see C:\Users\Frank\Documents\Arduino\FourWD_WallE2_V1\Variance.xlsm

// and C:\Users\Frank\Documents\Arduino\VarianceCalcTest.ino

// 01/16/19 added 'return inc_var'

// 04/21/19 copied number overflow corrections from VarianceCalcTest.ino

// 04/28/19 commented out the 'brute force' sections - now using incr var exclusively

//unsigned long funcStartMicrosec = micros();

//DEBUG

//myTeePrint.printf("in CalcRearDistArryVariance(%d, %2.2f, %2.2f)\n", newdistval, Rear_Dist_PrevMean, Rear_Dist_PrevVar);

//DEBUG

//11/03/18 update distance array, saving oldest for later use in incremental calcs

unsigned long oldestDistVal = aRearDistMM[0];

for (int i = 0; i < REAR_DIST_ARRAY_SIZE - 1; i++)

{

aRearDistMM[i] = aRearDistMM[i + 1];

}

aRearDistMM[REAR_DIST_ARRAY_SIZE - 1] = newdistval;

//11/02/18 now re-do the calculation using the incremental method, and compare the times

//mu_t = mu_(t-1) - dist_(t-N)/N + dist_t/N

//Example: mu_7 = mu_(6) - dist_(2)/N + dist_7/N

//var^2_t = var^2_(t-1) + dist^2_(t) - dist^2_(t-N) + mu^2_(t-1) - mu^2_t

//Example: var^2_7 = var^2_(6) + dist^2_(7) - dist^2_(t-N) + mu^2_(6) - mu^2_7

//DEBUG!!

//for (int i = 0; i < REAR_DIST_ARRAY_SIZE; i++)

//{

// Serial.print("aRearDistMM["); Serial.print(i); Serial.print("] = "); Serial.println(aRearDistMM[i]);

//}

//DEBUG!!

double inc_mean = Rear_Dist_PrevMean - (double)oldestDistVal / (double)REAR_DIST_ARRAY_SIZE + (double)newdistval / (double)REAR_DIST_ARRAY_SIZE;

//DEBUG

//myTeePrint.printf("inc_mean = %2.2f, ", inc_mean);

//DEBUG

unsigned long olddist_squared = oldestDistVal * oldestDistVal;

unsigned long newdist_squared = newdistval * newdistval;

double last_incmean_squared = Rear_Dist_PrevMean * Rear_Dist_PrevMean;

double inc_mean_squared = inc_mean * inc_mean;

//myTeePrint.printf("last_incmean_squared = %2.2f\n", last_incmean_squared);

//myTeePrint.printf("inc_mean_squared = %2.2f\n", inc_mean_squared);

//myTeePrint.printf("olddist_squared = %lu\n", olddist_squared);

//myTeePrint.printf("newdist_squared = %lu\n", newdist_squared);

//myTeePrint.printf("Rear_Dist_PrevVar = %2.2f\n", Rear_Dist_PrevVar);

double inc_var = Rear_Dist_PrevVar + ((double)newdist_squared / REAR_DIST_ARRAY_SIZE) - ((double)olddist_squared / REAR_DIST_ARRAY_SIZE)

+ last_incmean_squared - inc_mean_squared;

//DEBUG

//myTeePrint.printf("inc_var = %2.2f\n", inc_var);

//DEBUG

//long uSecI = micros() - funcStartMicrosec - uSecB;

//DEBUG!!

//display results:

//myTeePrint.printf("%lu\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\n", millis(),

// newdistval, oldestDistVal, Rear_Dist_PrevVMean, Rear_Dist_PrevVar, inc_var);

//DEBUG!!

Rear_Dist_PrevVar = inc_var; //save for next time

Rear_Dist_PrevMean = inc_mean; //save for next time

return inc_var; //added 01/16/19

}

//04/28/18 added to update left/right dist arrays, so can reinstate incr l/r dist avg

void UpdateLRDistanceArrays(int leftdistval, int rightdistval)

{

//Purpose: Update the L/R distance arrays with the latest values, shifting all other values down 1

//Inputs:

// Latest left/right values from sensors

//Outputs:

// latest value placed at Array[LR_DIST_ARRAY_SIZE - 1], all other values moved down one

//Plan:

// Step 1: For each array, shift all values down one (the 0th value drops into the bit bucket)

// Step 2: Place the latest reading at [LR_DIST_ARRAY_SIZE - 1].

//Notes:

//DEBUG!!

//myTeePrint.printf("UpdateLRDistanceArrays(left = %d, right = %d)", leftdistval, rightdistval);

//DEBUG!!

//Step 1: For each array, shift all values down one (the 0th value drops into the bit bucket)

for (int i = 0; i < LR_DIST_ARRAY_SIZE - 1; i++)

//for (int i = 0; i < DIST_ARRAY_SIZE; i++)

{

aRightDistMM[i] = aRightDistMM[i + 1];

aLeftDistMM[i] = aLeftDistMM[i + 1];

}

//Step 2: Place the latest reading at [DIST_ARRAY_SIZE - 1].

aRightDistMM[LR_DIST_ARRAY_SIZE - 1] = rightdistval;

aLeftDistMM[LR_DIST_ARRAY_SIZE - 1] = leftdistval;

}

uint16_t CalcRearDistAvgMM(int cur_val, double& prev_avg)

{

//Purposes: compute 5-point moving average

//Inputs:

// cur_val = Most recent value to be averaged

// prev_avg = previous average

// aRearDistMM = float array containing previous N values

uint16_t old_val = aRearDistMM[0];

for (size_t i = 0; i < REAR_DIST_ARRAY_SIZE - 1; i++)

{

aRearDistMM[i] = aRearDistMM[i + 1];

}

aRearDistMM[REAR_DIST_ARRAY_SIZE - 1] = cur_val;

//DEBUG!!

//myTeePrint.printf("In CalcRearDistAvgMM(%2.2f, %2.2f)\n", cur_val, prev_avg);

//for (size_t i = 0; i < REAR_DIST_ARRAY_SIZE-1; i++)

//{

// myTeePrint.printf("%d, ",aRearDistMM[i]);

//}

//myTeePrint.printf("%d\n", aRearDistMM[REAR_DIST_ARRAY_SIZE - 1]);

float new_avg = (prev_avg * REAR_DIST_ARRAY_SIZE + cur_val - old_val) / REAR_DIST_ARRAY_SIZE;

prev_avg = new_avg;

return new_avg;

}

#pragma endregion Distance Measurement Support

#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

//myTeePrint.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/20 modified for DRV8871 motor driver

//11/04/21 modified for Pololu VNH5019 motor driver

void StopLeftMotors()

{

digitalWrite(InA_Left, LOW);

digitalWrite(InB_Left, LOW);

analogWrite(Spd_Left, MOTOR_SPEED_OFF);

}

//11/04/21 modified for Pololu VNH5019 motor driver

void StopRightMotors()

{

digitalWrite(InA_Right, LOW);

digitalWrite(InB_Right, LOW);

analogWrite(Spd_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!!

//myTeePrint.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!!

//myTeePrint.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)

{

//DEBUG!!

//myTeePrint.printf("In SetLeftMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

//DEBUG!!

//11/04/21 fwd for right motors is CCW when looking at shaft

#ifndef NO_MOTORS

if (bIsFwd)

{

digitalWrite(InA_Left, LOW);

digitalWrite(InB_Left, HIGH);

analogWrite(Spd_Left, speed);

//DEBUG!!

//myTeePrint.printf("In TRUE block of SetLeftMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

//DEBUG!!

}

else

{

//DEBUG!!

//myTeePrint.printf("In FALSE block of SetLeftMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

//DEBUG!!

digitalWrite(InA_Left, HIGH);

digitalWrite(InB_Left, LOW);

analogWrite(Spd_Left, speed);

}

#endif // !NO_MOTORS

}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)

{

//DEBUG!!

//myTeePrint.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

//DEBUG!!

//11/04/21 fwd for right motors is CW when looking at shaft

#ifndef NO_MOTORS

if (bIsFwd)

{

//DEBUG!!

//myTeePrint.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

//DEBUG!!

digitalWrite(InA_Right, HIGH);

digitalWrite(InB_Right, LOW);

analogWrite(Spd_Right, speed);

}

else

{

//DEBUG!!

//myTeePrint.printf("In FALSE block of SetRightMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

//DEBUG!!

digitalWrite(InA_Right, LOW);

digitalWrite(InB_Right, HIGH);

analogWrite(Spd_Right, speed);

}

#endif // !NO_MOTORS

}

#pragma endregion Motor Support Functions

#pragma region HDG_BASED_TURN_SUPPORT

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

// 11/14/21 removed 'first time skip' block; added motor start before entering loop

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!!

myTeePrint.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);

//Serial.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);

//DEBUG!!

//no need to continue if the IMU isn't available

if (!dmpReady)

{

Serial.printf("DMP Failure - returning FALSE\n");

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

//08/26/21 re-wrote using 3-value array to make sure initial heading is a steady value

UpdateIMUHdgValDeg();

int retries = 0;

if ((IMUHdgValDeg == 180.f || IMUHdgValDeg == 0.f) && retries < 5)

{

//DEBUG!!

myTeePrint.printf("Got 180.00 or 0.00 exactly (%2.3f) from IMU - retrying %d...\n", IMUHdgValDeg, retries);

//DEBUG!!

UpdateIMUHdgValDeg();

retries++;

delay(100);

}

//Step2: Compute new target value & timeout value

//timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

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!!

myTeePrint.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;

//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

//11/14/21 start motors before entering loop. For CCW turn, left motors go backwards, right ones forward

int startSpd = (degPersec <= 45) ? TURN_START_SPEED : 2 * TURN_START_SPEED;

SetLeftMotorDirAndSpeed(!b_ccw, startSpd);

SetRightMotorDirAndSpeed(b_ccw, startSpd);

elapsedMillis sinceLastTimeCheck = 0;

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

//bool bFirstIMUHdg = true;

//DEBUG!!

myTeePrint.printf("Msec\tHdg\tPrvHdg\tdHdg\tRate\ttgtDPS\terr\tKp*err\tIval\tKd*Derr\tspeed\tMatch\tSlope\n");

//DEBUG!!

while (!bDoneTurning && !bTimedOut)

{

//11/06/20 now just loops between PID calcs

CheckForUserInput();

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;

//Serial.printf("dHdg > 180 - subtracting 360\n");

}

else if (dHdg < -180)

{

dHdg += 360;

//Serial.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!!

// Serial.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;

//}

TurnRatePIDOutput = PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd);

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;

myTeePrint.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\t%2.2f\t%2.2f\n",

millis(),

IMUHdgValDeg,

Prev_HdgDeg,

dHdg,

rate,

degPersec,

lastError,

TurnRate_Kp * lastError,

lastIval,

TurnRate_Kd * lastDerror,

speed,

curHdgMatchVal,

matchSlope);

//DEBUG!!

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

|| (prevHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope <= -0.01)); //have to use < vs <= as slope == 0 at start

//Serial.printf("curHdgMatchVal = %2.2f, prevHdgMatchVal = %2.2f, matchslope = %2.2f, bDoneTurning = %d\n",

// curHdgMatchVal,

// prevHdgMatchVal,

// matchSlope,

// bDoneTurning);

prevHdgMatchVal = curHdgMatchVal; //07/31/21 moved below bDoneTurning chk so can use prevHdgMatchVal vs curHdgMatchVal in slope check

bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

if (bTimedOut)

{

//DEBUG!!

myTeePrint.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

//DEBUG!!

bResult = false;

break;

}

if (bDoneTurning)

{

myTeePrint.printf("Completed turn with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

bResult = true;

break;

}

StopBothMotors();

delay(1000); //added 04/27/21 for debug

return bResult;

}

double PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd)

{

//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

// Notes:

// 01/13/22 sampleTime input parameter is never used. This is OK as long as PIDCalcs

// is called at regular intervals.

double error = setpoint - input;

lastIval += (Ki * error);

//11/16/21 rev to clamp integral term

lastIval = (lastIval > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : lastIval;

lastIval = (lastIval < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : lastIval;

double dErr = (error - lastError);

//myTeePrint.printf("PIDCalcs: error/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, lastIval, dErr,

// Kp,Ki,Kd);

/*Compute PID Output*/

//11/16/21 rev to subtract differential term rather than add.

//double output = Kp * error + Ki * lastIval + Kd * dErr;

double output = Kp * error + lastIval - Kd * dErr;

//myTeePrint.printf("Kp * error = %2.2f, lastIval = %2.2f, Kd * dErr = %2.2f, output = %2.2f\n",

// Kp * error, lastIval, Kd * dErr, output);

/*Remember some variables for next time*/

lastError = error;

lastInput = input;

lastDerror = dErr;

return output; //added 09/03/21

}

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!!

//myTeePrint.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio);

//DEBUG!!

return abs(match_ratio);

}

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.

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;

}

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;

//myTeePrint.printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);

while (fifoCount < packetSize && GetPacketLoopCount < max_loops)

{

GetPacketLoopCount++;

fifoCount = mpu.getFIFOCount();

delay(2);

}

//myTeePrint.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 HDG_BASED_TURN_SUPPORT

#pragma region FLASHER_X

//09/20/21 copied from FlasherX

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

// update_firmware() read hex file and write new firmware to program flash

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

void update_firmware(Stream* serial, uint32_t buffer_addr, uint32_t buffer_size)

{

static char line[96]; // buffer for hex lines

static char data[32] __attribute__((aligned(8))); // buffer for hex data

hex_info_t hex = { // intel hex info struct

data, 0, 0, 0, // data,addr,num,code

0, 0xFFFFFFFF, 0, // base,min,max,

0, 0 // eof,lines

};

serial->printf("waiting for hex lines...\n");

// read and process intel hex lines until EOF or error

//DEBUG!!

//int line_num = 0;

//DEBUG!!

long int start_msec = 0;

bool bFirstTime = true;

while (!hex.eof) {

read_ascii_line(serial, line, sizeof(line));

if (bFirstTime)

{

start_msec = millis();

bFirstTime = false;

}

//DEBUG!!

//serial->printf("%d: %s\n", line_num, line);

//line_num++;

//DEBUG!!

//10/02/21 gfp added for visual xfer confirmation

if (hex.lines % 10 == 0)

{

digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN));

}

// reliability of transfer via USB is improved by this printf/flush

if (serial == (Stream*)&Serial)

{

serial->printf("%s\n", line);

serial->flush();

}

if (parse_hex_line((const char*)line, hex.data, &hex.addr, &hex.num, &hex.code) == 0) {

serial->printf("abort - bad hex line %s\n", line);

return;

}

else if (process_hex_record(&hex) != 0) { // error on bad hex code

serial->printf("abort - invalid hex code %d\n", hex.code);

return;

}

else if (hex.code == 0) { // if data record

uint32_t addr = buffer_addr + hex.base + hex.addr - FLASH_BASE_ADDR;

if (hex.max > (FLASH_BASE_ADDR + buffer_size)) {

serial->printf("abort - max address %08lX too large\n", hex.max);

return;

}

else if (!IN_FLASH(buffer_addr)) {

memcpy((void*)addr, (void*)hex.data, hex.num);

}

else if (IN_FLASH(buffer_addr)) {

int error = flash_write_block(addr, hex.data, hex.num);

if (error) {

serial->printf("abort - error %02X in flash_write_block()\n", error);

return;

}

hex.lines++;

}

//serial->printf("\nhex file: %1d lines %1lu bytes (%08lX - %08lX)\n",

// hex.lines, hex.max - hex.min, hex.min, hex.max);

serial->printf("\nhex file: %1d lines %1lu bytes (%08lX - %08lX), %lu Msec\n",

hex.lines, hex.max - hex.min, hex.min, hex.max, millis() - start_msec);

// check FSEC value in new code -- abort if incorrect

#if defined(KINETISK) || defined(KINETISL)

uint32_t value = *(uint32_t*)(0x40C + buffer_addr);

if (value == 0xfffff9de) {

serial->printf("new code contains correct FSEC value %08lX\n", value);

}

else {

serial->printf("abort - FSEC value %08lX should be FFFFF9DE\n", value);

return;

}

#endif

// check FLASH_ID in new code - abort if not found

if (check_flash_id(buffer_addr, hex.max - hex.min)) {

serial->printf("new code contains correct target ID %s\n", FLASH_ID);

}

else {

serial->printf("abort - new code missing string %s\n", FLASH_ID);

return;

}

// get user input to write to flash or abort

int user_lines = -1;

while (user_lines != hex.lines && user_lines != 0) {

serial->printf("enter %d to flash or 0 to abort\n", hex.lines);

read_ascii_line(serial, line, sizeof(line));

sscanf(line, "%d", &user_lines);

}

if (user_lines == 0) {

serial->printf("abort - user entered 0 lines\n");

return;

}

// move new program from buffer to flash, free buffer, and reboot

//serial->printf("user entered %d lines\n", user_lines);

flash_move(FLASH_BASE_ADDR, buffer_addr, hex.max - hex.min);

// should not return from flash_move(), but put REBOOT here as reminder

REBOOT;

}

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

// read_ascii_line() read ascii characters until '\n', '\r', or max bytes

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

void read_ascii_line(Stream* serial, char* line, int maxbytes)

{

int c = 0, nchar = 0;

while (nchar < maxbytes && !(c == '\n' || c == '\r')) {

if (serial->available()) {

c = serial->read();

line[nchar++] = c;

}

line[nchar - 1] = 0; // null-terminate

}

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

// process_hex_record() process record and return okay (0) or error (1)

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

int process_hex_record(hex_info_t* hex)

{

if (hex->code == 0) { // data -- update min/max address so far

if (hex->base + hex->addr + hex->num > hex->max)

hex->max = hex->base + hex->addr + hex->num;

if (hex->base + hex->addr < hex->min)

hex->min = hex->base + hex->addr;

}

else if (hex->code == 1) { // EOF (:flash command not received yet)

hex->eof = 1;

}

else if (hex->code == 2) { // extended segment address (top 16 of 24-bit addr)

hex->base = ((hex->data[0] << 8) | hex->data[1]) << 4;

}

else if (hex->code == 3) { // start segment address (80x86 real mode only)

return 1;

}

else if (hex->code == 4) { // extended linear address (top 16 of 32-bit addr)

hex->base = ((hex->data[0] << 8) | hex->data[1]) << 16;

}

else if (hex->code == 5) { // start linear address (32-bit big endian addr)

hex->base = (hex->data[0] << 24) | (hex->data[1] << 16)

| (hex->data[2] << 8) | (hex->data[3] << 0);

}

else {

return 1;

}

return 0;

}

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

// Intel Hex record foramt:

// Start code: one character, ASCII colon ':'.

// Byte count: two hex digits, number of bytes (hex digit pairs) in data field.

// Address: four hex digits

// Record type: two hex digits, 00 to 05, defining the meaning of the data field.

// Data: n bytes of data represented by 2n hex digits.

// Checksum: two hex digits, computed value used to verify record has no errors.

// Examples:

// :10 9D30 00 711F0000AD38000005390000F5460000 35

// :04 9D40 00 01480000 D6

// :00 0000 01 FF

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

/* Intel HEX read/write functions, Paul Stoffregen, paul@ece.orst.edu */

/* This code is in the public domain. Please retain my name and */

/* email address in distributed copies, and let me know about any bugs */

/* I, Paul Stoffregen, give no warranty, expressed or implied for */

/* this software and/or documentation provided, including, without */

/* limitation, warranty of merchantability and fitness for a */

/* particular purpose. */

// type modifications by Jon Zeeff

/* parses a line of intel hex code, stores the data in bytes[] */

/* and the beginning address in addr, and returns a 1 if the */

/* line was valid, or a 0 if an error occured. The variable */

/* num gets the number of bytes that were stored into bytes[] */

#include <stdio.h> // sscanf(), etc.

#include <string.h> // strlen(), etc.

int parse_hex_line(const char* theline, char* bytes,

unsigned int* addr, unsigned int* num, unsigned int* code)

{

unsigned sum, len, cksum;

const char* ptr;

int temp;

*num = 0;

if (theline[0] != ':')

return 0;

if (strlen(theline) < 11)

return 0;

ptr = theline + 1;

if (!sscanf(ptr, "%02x", &len))

return 0;

ptr += 2;

if (strlen(theline) < (11 + (len * 2)))

return 0;

if (!sscanf(ptr, "%04x", (unsigned int*)addr))

return 0;

ptr += 4;

/* myTeePrint.printf("Line: length=%d Addr=%d\n", len, *addr); */

if (!sscanf(ptr, "%02x", code))

return 0;

ptr += 2;

sum = (len & 255) + ((*addr >> 8) & 255) + (*addr & 255) + (*code & 255);

while (*num != len)

{

if (!sscanf(ptr, "%02x", &temp))

return 0;

bytes[*num] = temp;

ptr += 2;

sum += bytes[*num] & 255;

(*num)++;

if (*num >= 256)

return 0;

}

if (!sscanf(ptr, "%02x", &cksum))

return 0;

if (((sum & 255) + (cksum & 255)) & 255)

return 0; /* checksum error */

return 1;

}

#pragma endregion FLASHER_X

#pragma region IR_HOMING_SUPPORT

//bool IRHomeToChgStn(int avoidancedistCm, int initleftspeed, int initrightspeed)

//{

// //Purpose: Home in to charging station with optional avoidance manuever

// //Inputs:

// // avoidancedistCm = int denoting how far away to start avoidance maneuver

// //Outputs:

// // either connected to charging station or turn away at avoidancedistCm

// //Plan:

// // Step1: Navigate to IAP

// // Step2: Initialize PID for homing

// // Step3: If front distance < avoidancedistCm, turn 90 deg away from near wall

// // otherwise continue homing until connected or stuck

// //Notes:

// // 03/19/17 rev to add initial left/right speed vals to calling sig

// // 08/09/20 added IsStuck() call, limited to 5 calls/sec to avoid false positives

// // 08/10/20 now using timer ISR for this so only need to check bIsStuck state

// // 11/08/20 moved I2C comms into timer ISR

// // 01/23/21 added code to re-try docking if the robot gets stuck

// // 02/07/21 rev to use ISR-managed bChgConnect vs local bChgConn

// // 02/21/21 added new nav-to-IAP algorithm

// // 04/03/21 added IRHomingPID.SetSampleTime(0) to make sure sample interval controlled by ISR

// bool result = true; //added 01/16/19 to supress warning

////Step2: Initialize PID for homing

// IRHomingSetpoint = 0; //01/14/21

// IRHomingPID.SetMode(AUTOMATIC);

// IRHomingPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

// //IRHomingPID.SetSampleTime(0); //04/03/21 zero sample time so .Compute() interval controlled by ISR

// //int bChgConn = LOW; //for testing

// lastHomingTelemetryMsec = 0; //used to space out telemetry prints

// myTeePrint.printf("IRHomeToChgStn with detection value = %lu, Steering = %2.2f & IRHOMING Kp,Ki,Kd = (%2.0f,%2.0f,%2.0f)\n",

// IRHomingValTotalAvg, IRHomingLRSteeringVal, IRHomingPID.GetKp(), IRHomingPID.GetKi(), IRHomingPID.GetKd());

// //Step3: If front distance < avoidancedistCm, turn 90 deg away from near wall

// // otherwise continue homing until connected or stuck

// myTeePrint.printf("front dist = %d, lastHomingTelemetryMsec = ", glFrontDistVal);

// myTeePrint.println(lastHomingTelemetryMsec);

// myTeePrint.println(IRHomingTelemStr); //header for chg telemetry data

// //08/10/20 now using ISR for bChgConnect and front distance

// //01/08/22 no longer using ISR

// //while (bChgConnect == LOW && glFrontDistVal > avoidancedistCm) //01/23/21 removed bIsStuck - handled inside while()

// bool bChgConnect = false; //01/08/22 no longer using ISR, so have to generate locally

// //while (bChgConnect == LOW ) //01/08/22 removed avoidancedistCm check

// while (!bChgConnect) //01/08/22 removed avoidancedistCm check

// {

// //05/02/20 turn on Laser

// digitalWrite(RED_LASER_DIODE_PIN, HIGH);

// //01/30/17 added to kill motors remotely using Wixel & serial port

// CheckForUserInput();

// if (MsecSinceLastIRHomingAdj >= MSEC_PER_IR_HOMING_ADJ)

// {

// MsecSinceLastIRHomingAdj -= MSEC_PER_IR_HOMING_ADJ;

// GetRequestedVL53l0xValues(VL53L0X_ALL);

// if (!isnan(IRHomingLRSteeringVal))//skip bad values

// {

// if (IRHomingPID.Compute()) //runs every 100mSec, or every MSEC_PER_IR_HOMING_ADJ whichever is longer

// {

// ////DEBUG!!

// digitalWrite(HOMING_PID_COMPUTE_CALL_PIN, HIGH);

// delay(1);

// digitalWrite(HOMING_PID_COMPUTE_CALL_PIN, LOW);

// ////DEBUG!!

// }

// leftspeednum = initleftspeed + IRHomingOutput;

// rightspeednum = initrightspeed - IRHomingOutput;

// //DEBUG!!

// //print out telemetry every 400 msec or so

// //if (lastHomingTelemetryMsec > IR_HOMING_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics

// //{

// // lastHomingTelemetryMsec -= IR_HOMING_TELEMETRY_SPACING_MSEC;

// myTeePrint.printf("%lu\t%2.2f\t%lu\t%lu\t%2.2f\t%2.2f\t\t%d\t%d\t%d\t%d\n",

// millis(), GetBattVoltage(), IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal, IRHomingOutput,

// leftspeednum, rightspeednum, glFrontDistVal, Lidar_Rear);

// //}

// UpdateRearLEDPanel(leftspeednum, rightspeednum);

// //04/24/20 experiment with going to full speed when near charge plug

// //01/30/21 bugfix. Robot must also be somewhere near centerline alignment, to avoid

// //sensing the charging station beacon while approaching a wall and 'accelerating to

// //contact' before actually turning to home in on the beacon

// if (glFrontDistVal <= CHG_STN_FINAL_APPR_DIST_CM && abs(IRHomingLRSteeringVal) <= IR_HOMING_STEERING_VAL_CAPTURE_THRESHOLD)

// {

// leftspeednum = rightspeednum = MOTOR_SPEED_MAX;

// myTeePrint.printf("Accelerating to Contact with frontdist = %d\n", glFrontDistVal);

// }

// MoveAhead(leftspeednum, rightspeednum);

// }

// bChgConnect = IsChargerConnected(bChgConnect);

// }

// //01/23/21 moved new 'bIsStuck' section into main while() loop to support multiple docking attempts

// // 01/23/21 added code to handle 'stuck' condition in charging station funnel

// glFrontDistVal = GetFrontDistCm();

// glFrontvar = CalcFrontDistArrayVariance(glFrontDistVal);

// glRearDistVal = GetRequestedVL53l0xValues(VL53L0X_REAR_ONLY);

// glRearvar = CalcRearDistArrayVariance(glRearDistVal);

// bool bIsStuck = glFrontvar > STUCK_FRONT_VARIANCE_THRESHOLD || glRearvar > STUCK_REAR_VARIANCE_THRESHOLD;

// if (bIsStuck)

// {

// myTeePrint.printf("Stuck in IRHomeToChargingStation() - Attempting to recover\n");

// myTeePrint.printf("Msec\tLF\tLC\tLR\tRF\tRC\tRR\tIR\n");

// myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%2.3f\n", millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

// Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, IRHomingLRSteeringVal);

// //reverse for 0.5 sec, then pause for 2 sec

// MoveReverse(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

// delay(500);

// StopBothMotors();

// delay(2000);

// while (abs(IRHomingLRSteeringVal) > 0.1f)

// {

// if (!isnan(IRHomingLRSteeringVal))

// {

// //11/08/20 now this executes every time, with interval controlled by timer ISR

// IRHomingPID.Compute();

// leftspeednum = initleftspeed - IRHomingOutput;

// rightspeednum = initrightspeed + IRHomingOutput;

// //DEBUG!!

// //print out telemetry every 400 msec or so

// if (lastHomingTelemetryMsec > IR_HOMING_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics

// {

// lastHomingTelemetryMsec -= IR_HOMING_TELEMETRY_SPACING_MSEC;

// myTeePrint.printf("%lu\t%2.2f\t%lu\t%lu\t%2.2f\t%2.2f\t\t%d\t%d\t%d\n",

// millis(), GetBattVoltage(), IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal, IRHomingOutput,

// leftspeednum, rightspeednum, glFrontDistVal);

// }

// //DEBUG!!

// MoveReverse(leftspeednum, rightspeednum);

// }

// myTeePrint.printf("Succeeded! Robot should now be lined up again! Stopping motors\n");

// StopBothMotors();

// delay(1000);

// myTeePrint.printf("Trying again to dock with Charging Station\n");

// delay(1000);

// //08/10/20 now using ISR for bIsStuck and front distances

// while (bChgConnect == LOW && !bIsStuck && glFrontDistVal > avoidancedistCm)

// {

// //05/02/20 turn on Laser

// digitalWrite(RED_LASER_DIODE_PIN, HIGH);

// //01/30/17 added to kill motors remotely using Wixel & serial port

// CheckForUserInput();

// //skip bad values

// if (!isnan(IRHomingLRSteeringVal))

// {

// //11/08/20 now this executes every time, with interval controlled by timer ISR

// IRHomingPID.Compute();

// leftspeednum = initleftspeed + IRHomingOutput;

// rightspeednum = initrightspeed - IRHomingOutput;

// //DEBUG!!

// //print out telemetry every 400 msec or so

// if (lastHomingTelemetryMsec > IR_HOMING_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics

// {

// lastHomingTelemetryMsec -= IR_HOMING_TELEMETRY_SPACING_MSEC;

// myTeePrint.printf("%lu\t%2.2f\t%lu\t%lu\t%2.2f\t%2.2f\t\t%d\t%d\t%d\n",

// millis(), GetBattVoltage(), IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal, IRHomingOutput,

// leftspeednum, rightspeednum, glFrontDistVal);

// }

// //DEBUG!!

// //04/24/20 experiment with going to full speed when near charge plug

// if (glFrontDistVal <= CHG_STN_FINAL_APPR_DIST_CM)

// {

// leftspeednum = rightspeednum = MOTOR_SPEED_MAX;

// myTeePrint.printf("Accelerating to Contact with frontdist = %d\n", glFrontDistVal);

// }

// MoveAhead(leftspeednum, rightspeednum);

// }

// //05/02/20 turn off Laser

// digitalWrite(RED_LASER_DIODE_PIN, LOW);

// }// while (bChgConn == LOW && !bIsStuck && glFrontDistVal > avoidancedistCm)

// }

// //if (bChgConnect == HIGH)

// if (bChgConnect)

// {

// Serial.print("Charger Connected at "); Serial.println(millis());

// result = true; //added 01/16/19 to supress warning

// }

// else //added 12/28/20 for debug.

// {

// myTeePrint.printf("In IRHomeToChgStation: This code block should never execute! Stopping Program!");

// StopBothMotors();

// while (true)

// {

// }

// //11/21/20 added to force PID re-init next time

// IRHomingPID.SetMode(MANUAL);

// return result; //added 01/16/19 to supress warning

//}

bool IRHomeToChgStnNoPings(int initleftspeed, int initrightspeed)

{

//Purpose: Home in to charging station without distance information

//Inputs:

//Outputs:

// robot homes to charging station

//Plan:

// Step2: Initialize PID for homing

//Notes:

// 03/19/17 rev to add initial left/right speed vals to calling sig

// 01/14/22 rev to use PIDCalcs vs PID library

bool result = true; //added 01/16/19 to suppress warning

//Step2: Initialize PID for homing

IRHomingSetpoint = 0; //01/14/21

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

myTeePrint.printf("IRHomeToChgStnNoPings with detection value = %lu, Steering = %2.2f & IRHOMING Kp,Ki,Kd = (%2.0f,%2.0f,%2.0f)\n",

IRHomingValTotalAvg, IRHomingLRSteeringVal, IRHomingKp, IRHomingKi, IRHomingKd);

myTeePrint.println(IRHomingTelemStr); //header for chg telemetry data

bool bChgConnect = false; //01/08/22 no longer using ISR, so have to generate locally

while (!bChgConnect) //01/08/22 removed avoidancedistCm check

{

//05/02/20 turn on Laser

digitalWrite(RED_LASER_DIODE_PIN, HIGH);

//01/30/17 added to kill motors remotely using Wixel & serial port

CheckForUserInput();

if (MsecSinceLastIRHomingAdj >= MSEC_PER_IR_HOMING_ADJ)

{

//01/15/22 added to measure time required to do a PID computation loop (w/o pings)

//DEBUG!!

digitalWrite(HOMING_PID_COMPUTE_CALL_PIN, HIGH);

//DEBUG!!

//01/15/22 when changing from 'remote manual control' to 'auto' mode

//it is possible for MsecSinceLastIRHomingAdj to grow very large

//MsecSinceLastIRHomingAdj -= MSEC_PER_IR_HOMING_ADJ;

while (MsecSinceLastIRHomingAdj >= MSEC_PER_IR_HOMING_ADJ)

{

MsecSinceLastIRHomingAdj -= MSEC_PER_IR_HOMING_ADJ;

}

UpdateIRHomingValues();

if (!isnan(IRHomingLRSteeringVal))//skip bad values

{

//myTeePrint.printf("IRHomingLRSteeringVal = %2.2f\n", IRHomingLRSteeringVal);

IRHomingOutput = PIDCalcs(IRHomingLRSteeringVal, 0, MSEC_PER_IR_HOMING_ADJ, lastError, lastInput, lastIval,

lastDerror, IRHomingKp, IRHomingKi, IRHomingKd);

//reversed 01/11/22 - 01/14/22 - reversed it back again

leftspeednum = initleftspeed + IRHomingOutput;

rightspeednum = initrightspeed - IRHomingOutput;

//limit wheel speeds to valid range (0-255)

leftspeednum = (leftspeednum > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : leftspeednum;

leftspeednum = (leftspeednum < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : leftspeednum;

rightspeednum = (rightspeednum > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : rightspeednum;

rightspeednum = (rightspeednum < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : rightspeednum;

//DEBUG!!

myTeePrint.printf("%lu\t%2.2f\t%lu\t%lu\t%2.2f\t%2.2f\t\t%d\t%d\n",

millis(), GetBattVoltage(), IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal, IRHomingOutput,

leftspeednum, rightspeednum);

UpdateRearLEDPanel(leftspeednum, rightspeednum);

MoveAhead(leftspeednum, rightspeednum);

}

bChgConnect = IsChargerConnected(bChgConnect);

//01/15/22 added to measure time required to do a PID computation loop (w/o pings)

//DEBUG!!

digitalWrite(HOMING_PID_COMPUTE_CALL_PIN, LOW);

//DEBUG!!

}

if (bChgConnect)

{

Serial.print("Charger Connected at "); Serial.println(millis());

result = true; //added 01/16/19 to supress warning

}

else //added 12/28/20 for debug.

{

myTeePrint.printf("In IRHomeToChgStation: This code block should never execute! Stopping Program!");

StopBothMotors();

while (true)

{

}

return result; //added 01/16/19 to supress warning

}

void UpdateIRHomingValues()

{

//Purpose: Update all the IR Homing related values

//Provenance: Ported from FourWD_WallE2_V12 01/07/22

//Inputs:

// IRHomingValTotalAvg = long int denoting previous average value

//Outputs:

// IRFinalValue1/2 = long ints denoting left/right computed demodulator outputs

// IRHomingLRSteeringVal = double denoting left/right steering error

// IRHomingValueTotalAverage array & IRHomingValTotalAvg updated

//myTeePrint.printf("Requesting %d, %d, %d bytes from IRDet\n", sizeof(IRFinalValue1), sizeof(IRFinalValue2), sizeof(IRHomingLRSteeringVal));

Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, sizeof(IRFinalValue1) + sizeof(IRFinalValue2) + sizeof(IRHomingLRSteeringVal));

I2C_readAnything(IRFinalValue1);

I2C_readAnything(IRFinalValue2);

I2C_readAnything(IRHomingLRSteeringVal);

//myTeePrint.printf("Got %lu, %lu, %2.2f from IR Detector module\n", IRFinalValue1, IRFinalValue2, IRHomingLRSteeringVal);

IRHomingValTotalAvg = UpdateIRHomingValueTotalAverage(IRFinalValue1, IRFinalValue2, IRHomingValTotalAvg);

}

unsigned long UpdateIRHomingValueTotalAverage(unsigned long FinVal1, unsigned long FinVal2, unsigned long prev_avg)

{

//Purpose: Update L/R IR Homing average total signal strength

// Provenance 03/16/21

// Inputs:

// FinVal1, FinVal2 = latest values from IR Homing Teensy

// prev_avg = previous left/right total average

// Plan:

// Step1: Add new total to top of array, bump everything down one, oldest value to bit-bucket

// Step2: Calc new average as new_avg = old_avg + (new_val - old_val)/SIZE

//Step1: Add new total to top of array, bump everything down one, oldest value to bit-bucket

//Step2: Calc new average as new_avg = old_avg + (new_val - old_val)/SIZE

unsigned long newValTotal = FinVal1 + FinVal2;

unsigned long oldValTotal = aIRHOMINGVALTOTALS[0];

for (int i = 0; i < IRHOMING_VALUE_ARRAY_SIZE - 1; i++)

{

aIRHOMINGVALTOTALS[i] = aIRHOMINGVALTOTALS[i + 1];

//myTeePrint.printf("%d\t%lu\n", i, aIRHOMINGVALTOTALS[i]);

}

aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE - 1] = newValTotal; //new total value

//myTeePrint.printf("%d\t%lu\n", IRHOMING_VALUE_ARRAY_SIZE - 1, aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE - 1]);

//Step2: Calc new average as new_avg = old_avg + (new_val - old_val)/SIZE

unsigned long new_val_avg = prev_avg + ((long int)newValTotal - (long int)oldValTotal) / IRHOMING_VALUE_ARRAY_SIZE;

//myTeePrint.printf("FinVal1/2 = %lu/%lu, prev_avg = %lu, old_tot = %lu, new_avg = %lu\n",

//FinVal1,FinVal2,prev_avg, oldValTotal, new_val_avg);

return (unsigned long)new_val_avg;

}

bool MoveToDesiredRearDistCm(int offsetCm)

{

//Purpose: Move forward or backward to a specified rear offset distance

// Inputs:

// offsetCm - int denoting desired rear distance offset

// Outputs:

// Robot moves forward or backward to the desired offset, under PID control

PID OffsetDistPID(&OffsetDistVal, &OffsetDistOutput, &OffsetDistSetpointCm,

OffsetDistKp, OffsetDistKi, OffsetDistKd, DIRECT);//12/14/20 re-doing this from scratch

OffsetDistPID.SetMode(AUTOMATIC);

OffsetDistPID.SetSampleTime(0);

OffsetDistPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

StopBothMotors();

int speed;

//OffsetDistSetpointCm = 30;

OffsetDistSetpointCm = offsetCm;

myTeePrint.printf("Msec\tDist\tdiff\tspeed\tvar\tavg\n");

uint16_t avg = CalcRearDistAvgMM(Lidar_Rear, Rear_Dist_PrevMean);

MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore

while (abs(Lidar_Rear - 10 * OffsetDistSetpointCm) > 10 || abs(avg - 10 * OffsetDistSetpointCm) > 10)

{

CheckForUserInput();

if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)

{

MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;

OffsetDistVal = Lidar_Rear / 10;

OffsetDistPID.Compute();

speed = (int)OffsetDistOutput;

uint16_t rear_var = CalcRearDistArrayVariance((unsigned long)Lidar_Rear);

avg = CalcRearDistAvgMM(Lidar_Rear, Rear_Dist_PrevMean);

myTeePrint.printf("%lu\t%2.2f\t%2.2f\t%d\t%d\t%d\n", millis(), OffsetDistVal, OffsetDistOutput, speed, rear_var, avg);

if (speed >= 0)

{

RunBothMotors(true, speed, speed);

}

else

{

RunBothMotors(false, -speed, -speed);

}

myTeePrint.printf("Stopped at %2.2f cm with avg = %d\n", (float)Lidar_Rear / 10, avg);

//StopBothMotors();

return true;

}

bool MoveToDesiredFrontDistCm(int offsetCm)

{

//Purpose: Move forward or backward to a specified front offset distance

// Inputs:

// offsetCm - int denoting desired forward distance offset

// Kp,Ki,Kd set at top of program (lines 85-87)

// Outputs:

// Robot moves forward or backward to the desired offset, under PID control

//Notes:

// 04/25/21 the Kp/Ki/Kd values are much lower than prescribed by Z-N theory

// but I like the slower, more deliberate action.

//DEBUG!!

myTeePrint.printf("MoveToDesiredFrontDistCm: Kp/Ki/Kd = %2.1f/%2.1f/%2.1f\n", OffsetDistPID.GetKp(), OffsetDistPID.GetKi(), OffsetDistPID.GetKd());

OffsetDistPID.SetMode(AUTOMATIC);

OffsetDistPID.SetSampleTime(0);

OffsetDistPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

StopBothMotors();

int speed;

OffsetDistSetpointCm = offsetCm;

myTeePrint.printf("Msec\tDist\tdiff\tspeed\tavg\n");

uint16_t avg = GetAvgFrontDistCm();

MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore

while (abs(glFrontDistVal - OffsetDistSetpointCm) > 1 || abs(avg - OffsetDistSetpointCm) > 1)

{

CheckForUserInput();

if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)

{

MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;

OffsetDistVal = glFrontDistVal;

OffsetDistPID.Compute();

speed = -(int)OffsetDistOutput;

avg = GetAvgFrontDistCm();

myTeePrint.printf("%lu\t%2.2f\t%2.2f\t%d\t%d\n", millis(), OffsetDistVal, OffsetDistOutput, speed, avg);

RunBothMotorsBidirectional(speed, speed);

}

myTeePrint.printf("Stopped at %d cm with avg = %d\n", glFrontDistVal, avg);

StopBothMotors();

return true;

}

void UpdateRearLEDPanel(int leftspeed, int rightspeed) //added 05/02/17

{

//Purpose: Update LED enable/disable states on rear LED panel

//Provenance: Created 05/03/17 gfp

//Inputs:

// leftspeed, rightspeed = integers representing left/right motor speeds

//Outputs:

// Rear LED panel updated to reflect left/right motor speeds

//Plan:

// Step1: Disable all LEDs to blank the display

// Step2: Enable the appropriate LEDs on each side

//Step1: Disable all LEDs to blank the display

//DisableAllRearPanelLEDs();

EnableAllRearLEDs(false); //repl DisableAllRearPanelLEDs() 04/02/21

//Step2: Enable the appropriate LEDs on each side

//left side

if (leftspeed >= MOTOR_SPEED_LOW)

{

digitalWrite(_40PCT_LED_PIN, LOW);

}

if (leftspeed >= MOTOR_SPEED_HALF)

{

digitalWrite(_20PCT_LED_PIN, LOW);

}

if (leftspeed >= MOTOR_SPEED_FULL)

{

digitalWrite(CHG_LED_PIN, LOW);

}

//right side

if (rightspeed >= MOTOR_SPEED_LOW)

{

digitalWrite(_60PCT_LED_PIN, LOW);

}

if (rightspeed >= MOTOR_SPEED_HALF)

{

digitalWrite(_80PCT_LED_PIN, LOW);

}

if (rightspeed >= MOTOR_SPEED_FULL)

{

digitalWrite(FIN_LED_PIN, LOW);

}

#pragma endregion IR_HOMING_SUPPORT

#pragma region MISCELLANEOUS

int GetOpMode() //04/28/19 added batv to sig 01/07/22 removed - now call GetBatt

{

//Purpose: Determine operating mode based on sensor inputs

//Inputs: none

//Outputs: Integer denoting current op mode (CHARGING = 1, IRHOMING = 2, WALLFOLLOW = 3)

//Plan:

// Step1: If batt voltage too low, return MODE_DEADBATTERY (5)

// Step2: If Charger is connected, return MODE_CHARGING (1)

// Step3: Else If IR Homing beam detected, return MODE_IRHOMING (2)

// Step4: Else return MODE_WALLFOLLOW (3)

//Notes:

// 01/16/18 added MODE_DEADBATTERY

// 03/28/18 now looking for either IsChargerConnected OR low on TP5100 Chg line

// 09/03/18 reorganized so DEADBATTERY is on top, and added new TURNING_TO_HDG mode

// 02/24/19 now using 1NA619 charge current sensor

// 04/28/19 added batV to function sig

// 04/27/20 rewrote to simplify MODE_CHARGING code and to have only one exit point

// 05/02/20 bugfix - IsChargerConnected() block has to come before DEAD_BATTERY check

// 07/16/21 added MODE_WALLCAPTURE (4)

// 01/07/22 removed battv from sig - now done internally

myTeePrint.printf("%lu: At top of GetOpMode() with mode = %s (%d), prev_mode = %s (%d)\n",

millis(), ModeStrArray[CurrentOpMode], CurrentOpMode, ModeStrArray[PrevOpMode], PrevOpMode);

int mode = MODE_NONE; //04/27/20 added so function has only one exit point

//01/08/22 updates added here - no longer using ISR strategy

float batv = GetBattVoltage();

UpdateIRHomingValues();

bool bIRBeamAvail = (IRHomingValTotalAvg > IR_BEAM_DETECTION_THRESHOLD

&& abs(IRHomingLRSteeringVal) < IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD);

bool bChgConnect = true;

bChgConnect = IsChargerConnected(bChgConnect);

//Step1: If Charger is connected, return MODE_CHARGING (1)

if (bChgConnect) //12/16/20 rev to use ISR-generated value

{

myTeePrint.printf("Robot has successfully connected to charger!\n");

mode = MODE_CHARGING;

}

//Step2: Else If IR Homing beam detected, return MODE_IRHOMING (2)

else if (bIRBeamAvail)

{

myTeePrint.printf("IR beam detected with value = %lu\n", IRFinalValue1 + IRFinalValue2);

mode = MODE_IRHOMING; //this handles 'hungry/not-hungry' cases internally.

}

//Step3: If batt voltage too low, return MODE_DEADBATTERY (4)

else if (batv <= DEAD_BATT_THRESH_VOLTS)

{

myTeePrint.printf("in MODE_DEADBATTERY block with batV = %2.3f\n", batv);

mode = MODE_DEADBATTERY;

}

//01/08/22 - revised to explicitly add default MODE_WALLFOLLOW assignment

else if (PrevOpMode != MODE_WALLFOLLOW)

{

mode = MODE_WALLCAPTURE;

PrevOpMode = MODE_WALLCAPTURE;

//myTeePrint.printf("%lu: After PrevOpMode != MODE_WALLFOLLOW with mode = %s (%d), prev_mode = %s (%d)\n",

// millis(), ModeStrArray[CurrentOpMode], CurrentOpMode, ModeStrArray[PrevOpMode], PrevOpMode);

}

else mode = MODE_WALLFOLLOW;

//myTeePrint.printf("%lu: At bottom of GetOpMode() with mode = %d\n", millis(), CurrentOpMode);

return mode;

}

void CheckForUserInput()

{

//Notes:

// 11/21/21 rev to accomodate cmd input from either serial, and output to both

// 12/25/21 another try at using pointer for Serial object

//DEBUG!!

//myTeePrint.printf("In CheckForUserInput(). Serial.available() = %d, Serial1.available() = %d\n",

// Serial.available(), Serial1.available());

const int bufflen = 3;

char buff[bufflen];

memset(buff, 0, bufflen);

byte incomingByte = 0; //moved here 11/21/21

int numchars = 0;

Stream* pStream = 0;

if (Serial.available() > 0)

{

// read the incoming byte:

numchars = Serial.readBytesUntil('\n', buff, sizeof(buff));

incomingByte = buff[0];

//Set pStream to Serial

pStream = (Stream*)&Serial;

// say what you got:

pStream->printf("I received %d chars, first char 0X%X on USB Serial\n", numchars, incomingByte);

}

//11/21/21 now input can come from Serial1 too

else if (Serial1.available() > 0)

{

// read the incoming byte:

numchars = Serial1.readBytesUntil('\n', buff, sizeof(buff));

incomingByte = buff[0];

//Set pStream to Serial1

pStream = (Stream*)&Serial1;

// say what you got:

pStream->printf("I received %d chars, first char 0X%X on BT Serial1\n", numchars, incomingByte);

}

//11/21/21 incomingByte can come from either serial input

if (incomingByte != 0)

{

//02/18/20 experiment with multiple commands

switch (incomingByte)

{

case 0x55: //ASCII 'U'

case 0x75: //ASCII 'u'

#pragma region FIRMWARE UPDATE

StopBothMotors();

pStream->println(F("Start Program Update - Send new HEX file!"));

//09/20/21 copied from FlasherX - loop()

if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)

{

pStream->printf("unable to create buffer\n"); Serial.flush();

for (;;) {}

}

pStream->printf("buffer = %1luK %s (%08lX - %08lX)\n",

buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",

buffer_addr, buffer_addr + buffer_size);

//09/20/21 clear the serial buffer

while (pStream->available())

{

pStream->read();

}

// receive hex file via serial, write new firmware to flash, clean up, reboot

update_firmware(&Serial1, buffer_addr, buffer_size); // no return if success

// return from update_firmware() means error or user abort, so clean up and

// reboot to ensure that static vars get boot-up initialized before retry

pStream->printf("erase FLASH buffer / free RAM buffer...\n");

firmware_buffer_free(buffer_addr, buffer_size);

Serial1.flush();

REBOOT;

break;

#pragma endregion FIRMWARE UPDATE

case 0x43: //ASCII 'C'

case 0x63: //ASCII 'c'

#pragma region MANUALCONTROL

pStream->println(F("ENTERING COMMAND MODE:"));

pStream->println(F("0 = 180 deg CCW Turn"));

pStream->println(F("1 = 180 deg CW Turn"));

pStream->println(F("A = Back to Auto Mode"));

pStream->println(F("S = Stop"));

pStream->println(F("F = Forward"));

pStream->println(F("R = Reverse"));

pStream->println(F(""));

pStream->println(F(" Faster"));

pStream->println(F("\t8"));

pStream->println(F("Left 4\t5 6 Right"));

pStream->println(F("\t2"));

pStream->println(F(" Slower"));

StopBothMotors();

int speed = 0;

bool bAutoMode = false;

while (pStream->available())

{

incomingByte = pStream->read();

pStream->printf("I removed 0X%X from Serial1\n", incomingByte);

}

incomingByte = 0;

while (!bAutoMode)

{

//12/25/21 now using Stream* for serial

if (pStream->available() > 0)

{

// read the incoming bytes:

numchars = pStream->readBytesUntil('\n', buff, sizeof(buff));

incomingByte = buff[0];

// say what you got:

pStream->printf("I received %d chars, first char 0X%X on Serial1\n", numchars, incomingByte);

while (pStream->available())

{

pStream->read();

pStream->printf("I removed 0X%X from Serial1\n", incomingByte);

}

//11/21/21 incomingByte can come from either serial input

if (incomingByte != 0)

{

switch (incomingByte)

{

case 0x30: //Dec '0'

pStream->println(F("CCW 180 deg Turn"));

SpinTurn(true, 180, 90);

MoveAhead(speed, speed);

break;

case 0x31: //Dec '1'

pStream->println(F("CW 180 deg Turn"));

SpinTurn(false, 180, 45);

break;

case 0x34: //Turn left 10 deg and keep moving

pStream->println(F("CCW 10 deg Turn"));

SpinTurn(true, 10, 30);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x36: //Turn right 10 deg and keep moving

pStream->print("CW 10 deg Turn\n");

SpinTurn(false, 10, 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;

pStream->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;

pStream->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'

pStream->println(F("Stopping Motors!"));

StopBothMotors();

speed = 0;

break;

case 0x41: //ASCII 'A'

case 0x61: //ASCII 'a'

StopBothMotors();

pStream->println(F("Re-entering AUTO mode"));

bAutoMode = true;

break;

case 0x52: //ASCII 'R'

case 0x72: //ASCII 'r'

pStream->println(F("Setting both motors to reverse"));

bIsForwardDir = false;

MoveReverse(speed, speed);

break;

case 0x46: //ASCII 'F'

case 0x66: //ASCII 'f'

pStream->println(F("Setting both motors to forward"));

bIsForwardDir = true;

MoveAhead(speed, speed);

#pragma endregion MANUALCONTROL

break;

//01/11/22 copied here from 'automatic' mode to allow firmware updates

//even when in 'command' mode

case 0x55: //ASCII 'U'

case 0x75: //ASCII 'u'

#pragma region FIRMWARE UPDATE

StopBothMotors();

pStream->println(F("Start Program Update - Send new HEX file!"));

//09/20/21 copied from FlasherX - loop()

if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)

{

pStream->printf("unable to create buffer\n"); Serial.flush();

for (;;) {}

}

pStream->printf("buffer = %1luK %s (%08lX - %08lX)\n",

buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",

buffer_addr, buffer_addr + buffer_size);

//09/20/21 clear the serial buffer

while (pStream->available())

{

pStream->read();

}

// receive hex file via serial, write new firmware to flash, clean up, reboot

update_firmware(&Serial1, buffer_addr, buffer_size); // no return if success

// return from update_firmware() means error or user abort, so clean up and

// reboot to ensure that static vars get boot-up initialized before retry

pStream->printf("erase FLASH buffer / free RAM buffer...\n");

firmware_buffer_free(buffer_addr, buffer_size);

Serial1.flush();

REBOOT;

break;

#pragma endregion FIRMWARE UPDATE

default:

pStream->println(F("In Default Case: Stopping Motors!"));

StopBothMotors();

//while (true)

//{

//}

}

incomingByte = 0;

}

void InitAllPins()

{

for (uint8_t i = 0; i < CORE_NUM_DIGITAL; i++)

{

pinMode(i, OUTPUT);

digitalWrite(i, LOW);

}

void EnableAllRearLEDs(bool bEnable)

{

//Purpose: Turns all 4 LEDs ON or OFF (LOW is ON)

//Provenance: Created 05/02/17 gfp

if (bEnable)

{

digitalWrite(CHG_LED_PIN, LOW);

digitalWrite(_20PCT_LED_PIN, LOW);

digitalWrite(_40PCT_LED_PIN, LOW);

digitalWrite(_60PCT_LED_PIN, LOW);

digitalWrite(_80PCT_LED_PIN, LOW);

digitalWrite(FIN_LED_PIN, LOW);

}

else

{

digitalWrite(CHG_LED_PIN, HIGH);

digitalWrite(_20PCT_LED_PIN, HIGH);

digitalWrite(_40PCT_LED_PIN, HIGH);

digitalWrite(_60PCT_LED_PIN, HIGH);

digitalWrite(_80PCT_LED_PIN, HIGH);

digitalWrite(FIN_LED_PIN, HIGH);

}

#pragma endregion MISCELLANEOUS

#pragma region VL53L0X Sensor Support

void WaitForVL53L0XTeensy()

{

bVL53L0X_TeensyReady = false;

while (!bVL53L0X_TeensyReady)

{

GetRequestedVL53l0xValues(VL53L0X_READYCHECK); //this updates bVL53L0X_TeensyReady

myTeePrint.printf("%lu: got %d from VL53L0X Teensy\n", millis(), bVL53L0X_TeensyReady);

delay(100);

}

myTeePrint.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

myTeePrint.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\n");

GetRequestedVL53l0xValues(VL53L0X_ALL);

myTeePrint.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)

{

GetRequestedVL53l0xValues(VL53L0X_ALL);

myTeePrint.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);

}

myTeePrint.printf("VL53L0X Teensy Ready at %lu\n\n", millis());

}

//01/24/21 revised to implement error reporting

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

// 01/16/22 rev to use I2C_Anything1 functions for Wire1

//Step1: Send request to VL53L0X handler

//DEBUG!!

//myTeePrint.printf("Sending %d to slave\n", which);

//DEBUG!!

//myTeePrint.printf("In GetRequestedVL53l0xValues(%d)\n", (int)which);

Wire1.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

//I2C_writeAnything1((uint8_t)which);

I2C_writeAnything((uint8_t)which, &Wire1);

Wire1.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()

Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(sizeof(bVL53L0X_TeensyReady)));

//readResult = I2C_readAnything1(bVL53L0X_TeensyReady);

readResult = I2C_readAnything(bVL53L0X_TeensyReady, &Wire1);

break;

case VL53L0X_CENTERS_ONLY:

//just two data values needed here

data_size = 2 * sizeof(int);

Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(2 * sizeof(Lidar_RightCenter)));

readResult = I2C_readAnything(Lidar_RightCenter, &Wire1);

if (readResult > 0)

{

I2C_readAnything(Lidar_LeftCenter, &Wire1);

}

//DEBUG!!

//myTeePrint.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!!

//myTeePrint.printf("data_size = %d\n", data_size);

//DEBUG!!

Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

readResult = I2C_readAnything(Lidar_RightFront, &Wire1);

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_RightCenter, &Wire1);

}

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_RightRear, &Wire1);

}

if (readResult > 0)

{

readResult = I2C_readAnything(RightSteeringVal, &Wire1);

}

//DEBUG!!

//myTeePrint.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);

Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

readResult = I2C_readAnything(Lidar_LeftFront, &Wire1);

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_LeftCenter, &Wire1);

}

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_LeftRear, &Wire1);

}

if (readResult > 0)

{

readResult = I2C_readAnything(LeftSteeringVal, &Wire1);

}

//DEBUG!!

//myTeePrint.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

data_size = 7 * sizeof(uint16_t) + 2 * sizeof(double); //10/31/20 added rear distance

//myTeePrint.printf("In VL53L0X_ALL case with data_size = %d\n", data_size);

Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

//Lidar_LeftFront

readResult = I2C_readAnything(Lidar_LeftFront, &Wire1);

if (readResult != sizeof(Lidar_LeftFront))

{

myTeePrint.printf("Error reading Lidar_LeftFront\n");

}

//Lidar_LeftCenter

readResult = I2C_readAnything(Lidar_LeftCenter, &Wire1);

if (readResult != sizeof(Lidar_LeftCenter))

{

myTeePrint.printf("Error reading Lidar_LeftCenter\n");

}

//Lidar_LeftRear

readResult = I2C_readAnything(Lidar_LeftRear, &Wire1);

if (readResult != sizeof(Lidar_LeftRear))

{

myTeePrint.printf("Error reading Lidar_LeftRear\n");

}

//LeftSteeringVal

readResult = I2C_readAnything(LeftSteeringVal, &Wire1);

if (readResult != sizeof(LeftSteeringVal))

{

myTeePrint.printf("Error reading LeftSteeringVal\n");

}

//Lidar_RightFront

readResult = I2C_readAnything(Lidar_RightFront, &Wire1);

if (readResult != sizeof(Lidar_RightFront))

{

myTeePrint.printf("Error reading Lidar_RightFront\n");

}

//Lidar_RightCenter

readResult = I2C_readAnything(Lidar_RightCenter, &Wire1);

if (readResult != sizeof(Lidar_RightCenter))

{

myTeePrint.printf("Error reading Lidar_RightCenter\n");

}

//Lidar_RightRear

readResult = I2C_readAnything(Lidar_RightRear, &Wire1);

if (readResult != sizeof(Lidar_RightRear))

{

myTeePrint.printf("Error reading Lidar_RightRear\n");

}

//Lidar_Rear

readResult = I2C_readAnything(Lidar_Rear, &Wire1);

if (readResult != sizeof(Lidar_Rear))

{

myTeePrint.printf("Error reading Lidar_Rear\n");

}

//RightSteeringVal

readResult = I2C_readAnything(RightSteeringVal, &Wire1);

if (readResult != sizeof(RightSteeringVal))

{

myTeePrint.printf("Error reading LeftSteeringVal\n");

}

//myTeePrint.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);

Wire1.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(sizeof(Lidar_Rear)));

readResult = I2C_readAnything(Lidar_Rear, &Wire1);

//DEBUG!!

//myTeePrint.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

}

#pragma endregion VL53L0X Support

And here is the ‘multiple I2C bus’ version of I2C_Anything.h, just in case you come across this post before Nick updates the Github repo (assuming he likes what I have done):

// Written by Nick Gammon
// May 2012
// Jan 2022 Rev by Frank Paynter to accomodate multiple I2C busses

#include <Arduino.h>
#include <Wire.h> //07/06/20 chg back for I2C hangup testing

template <typename T> unsigned int I2C_writeAnything (const T& value, void *wireObj = 0)
  {

  //01/20/22 allow use of Wire1, Wire2, etc
  TwoWire* useWire = &Wire; //default is 'original' Wire
  if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-zero addr was supplied
  const byte * p = (const byte*) &value;
    unsigned int i;
    for (i = 0; i < sizeof value; i++)
      useWire->write(*p++);

    return i;
  }  // end of I2C_writeAnything

template <typename T> unsigned int I2C_readAnything(T& value, TwoWire* wireObj = 0)
  {
  //01/20/22 allow use of Wire1, Wire2, etc
  TwoWire* useWire = &Wire; //default is 'original' Wire
  if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-zero addr was supplied

    byte * p = (byte*) &value;
    unsigned int i;
    for (i = 0; i < sizeof value; i++)
          *p++ = useWire->read();

    return i;
  }  // end of I2C_readAnything

// Written by Nick Gammon

// May 2012

// Jan 2022 Rev by Frank Paynter to accomodate multiple I2C busses

#include <Arduino.h>

#include <Wire.h> //07/06/20 chg back for I2C hangup testing

template <typename T> unsigned int I2C_writeAnything (const T& value, void *wireObj = 0)

{

//01/20/22 allow use of Wire1, Wire2, etc

TwoWire* useWire = &Wire; //default is 'original' Wire

if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-zero addr was supplied

const byte * p = (const byte*) &value;

unsigned int i;

for (i = 0; i < sizeof value; i++)

useWire->write(*p++);

return i;

} // end of I2C_writeAnything

template <typename T> unsigned int I2C_readAnything(T& value, TwoWire* wireObj = 0)

{

//01/20/22 allow use of Wire1, Wire2, etc

TwoWire* useWire = &Wire; //default is 'original' Wire

if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-zero addr was supplied

byte * p = (byte*) &value;

unsigned int i;

for (i = 0; i < sizeof value; i++)

*p++ = useWire->read();

return i;

} // end of I2C_readAnything

Here’s the the test setup:

Main Teensy 3.5 processor on plugboard, connected via I2C to the Teensy 3.5 VL53L0X array manager

Using Wire1 & Wire2 with the I2CDevLib & MPU6050 Libraries

2 Replies

While working on porting Wall-E2’s ‘second deck’ hardware (VL53L0X array and LIDAR) to Wall_E3, I started running into problems associated with using Wire1 on the main Teensy 3.5 master processor to communicate with the Teensy 3.5 slave processor that manages the seven VL53L0X time-of-flight distance sensors. As I worked to troubleshoot the issue, it soon became evident that “I wasn’t in Kansas anymore”, and in fact had once again gone down a rabbit hole into Wonderland, with nary a bread-crumb in sight. Basically, I have been trying to use both the ‘i2c_t3.h’ and ‘Wire.h’ Teensy to utilize multiple I2C buses (Wire1, Wire2, etc) over the last few years, without really understanding what I was doing. In the process I have created a spaghetti mess of conflicting I2C library file locations and configurations.

So, this post is my account of what went wrong, and the steps taken to get things working properly again.

The problem – utilizing multiple I2C buses:

Teensy 3.x processors provide for multiple I2C buses, a feature I used originally to manage the 7-element VL53L0X array located on the ‘second deck’ of Wall_E2, my autonomous wall following robot. With the addition of a Teensy 3.5 for the main robot processor, the multiple I2C bus problem is now relevant to the main processor as well. The main processor now uses Wire to talk to the second-deck VL53L0X array manager (Teensy 3.5), and Wire1 for the IR homing beacon detector/demodulator and the MPU6050 IMU. Consequently, the main processor must utilize (multi-wire) capable library functions.

The 7-element VL53L0X array manager (Teensy 3.5)

This worked great, even though I had tried my best to screw it up. The main project file has already been changed to use <Wire.h> and although it uses a local copy of I2C_Anything.h, that copy uses <Wire.h> as well. Also, VL53L0X.h (local folder copy) also uses <Wire.h>. So, I made the following changes:

Removed VL53L0X.h/cpp and I2C_Anything.h from project references and deleted the local file copies
Changed #include “file.h” to <file.h> for both (not sure this is necessarsy
Deleted the vl53l0x-arduino folder from the \Libraries folder so there would be only one version of VL53L0X.h/cpp available.
Re-scanned for libraries, did a File->SaveAll, and recompiled OK – yay!

So now at least the Teensy_7VL53L0X_Slave_V3 project has been cleaned up

Main Wall-E3 Processor (Teensy 3.5)

This is where all the trouble with multiple I2C busses started. The main processor has to talk to the VL53L0X array manager via I2C on Wire1, which means that not only does the main processor code need to utilize multi-bus functionality, but I2C_Anything (which internally uses Wire for bit-wise comms) does as well. In addition, interfacing to the MPU6050 requires the use of Jeff Rowberg’s I2CDevLib stuff, specifically MPU6050_6Axis_MotionApps_V6_12.h, I2CDev.h, I2C_Anything.h, and a Wire1 capable version of I2C_Anything.h. To make all this work, I made the following changes:

The #include for MPU6050_6Axis_MotionApps_V6_12.h, I2CDev.h is aimed at \Libraries\MPU6050\, which is a (old) copy of C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050\. A suggestion in Jeff Rowberg’s ReadME file regarding the use of symlinks instead of actual copies led me to this ‘how-to’ page on creating symlinks in Windows. So I deleted the C:\Users\paynt\Documents\Arduino\Libraries\MPU6050\ folder and instead created a ‘hard’ symlink from there to C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050\. Then I similarly deleted the C:\Users\paynt\Documents\Arduino\Libraries\I2Cdev\ folder and created a ‘hard’ symlink from there to C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev\.

Here are the cmdline commands for both operations:

C:\Users\paynt\Documents\Arduino>mklink /J C:\Users\paynt\Documents\Arduino\Libraries\MPU6050 C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050
Junction created for C:\Users\paynt\Documents\Arduino\Libraries\MPU6050 <<===>> C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050

C:\Users\paynt\Documents\Arduino>mklink /J C:\Users\paynt\Documents\Arduino\Libraries\I2Cdev C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev
Junction created for C:\Users\paynt\Documents\Arduino\Libraries\I2Cdev <<===>> C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev

C:\Users\paynt\Documents\Arduino>mklink /J C:\Users\paynt\Documents\Arduino\Libraries\MPU6050 C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050

Junction created for C:\Users\paynt\Documents\Arduino\Libraries\MPU6050 <<===>> C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050

C:\Users\paynt\Documents\Arduino>mklink /J C:\Users\paynt\Documents\Arduino\Libraries\I2Cdev C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev

Junction created for C:\Users\paynt\Documents\Arduino\Libraries\I2Cdev <<===>> C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev

and here is the result of dir /A in the C:\Users\paynt\Documents\Arduino\Libraries\ folder

01/18/2022  03:16 PM    <JUNCTION>     I2Cdev [C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev]
01/18/2022  02:58 PM    <JUNCTION>     MPU6050 [C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050]

1 2	01/18/2022 03:16 PM <JUNCTION> I2Cdev [C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev] 01/18/2022 02:58 PM <JUNCTION> MPU6050 [C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050]

showing that the hard links were actually created properly.

So now when I right-click on include “MPU6050_6Axis_MotionApps_V6_12.h”, the file opens properly, and the location is shown as in the C:\Users\paynt\Documents\Arduino\Libraries\MPU6050 folder even though the file actually resides in C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\MPU6050\. Similarly, for include “I2Cdev.h” the file opens properly and the location is shown as C:\Users\paynt\Documents\Arduino\Libraries\I2Cdev\ even though it is actually in the C:\Users\paynt\Documents\Arduino\Libraries\i2cdevlib\Arduino\I2Cdev\ folder

This all worked, except now I’m getting errors that say that ‘I2C_PINS_18_19’ (and all the other Teensy I2C-specific enums) can’t be found – argggggghhhhh! They don’t seem to be defined anywhere in the C:\Program Files (x86)\Arduino\hardware\teensy\avr\ folder tree either – I’m at a loss

Well, maybe not. I’m beginning to think that the enums are <i2c_t3.h>-specific, and a more basic style of initialization is used with <Wire.h>. I sent off a plea to the Teensy forum – we’ll see.

In the meantime, I tried a couple of simple experiments that determined pretty conclusively that the <Wire.h> style does indeed work, but in a different (more constrained?) way than with <i2c_t3.h>. I created a ‘Wire_Slave_Sender’ VS2022 project by copying the ‘slave_sender.ino’ example and loaded it onto a T3.2. Then I created a ‘Wire_Master_Reader’ VS2022 project by copying the ‘master_reader.ino’ example, and loaded it onto a T3.5. With this setup I was able to demonstrate that ‘Wire.begin()’ facilitated I2C comms on pins 18 & 19 (the default pinouts for Wire0) on the T3.5 (pins 18 & 19 on the slave didn’t change), and ‘Wire1.begin()’ facilitated the same thing, but this time using pins 37 & 38 (the default pinouts for Wire1). Here are the programs:

/*
    Name:       Wire_Master_Reader.ino
    Created:	1/18/2022 9:11:05 PM
    Author:     NEWXPS15\paynt

    First trial was with all default settings, and 
    connected to Wire_Slave_Sender.ino via pins 18,19
    on both ends. This worked as expected

    Second trial was to change all 'Wire.*' to 'Wire1.*'
    in this program, and physically move the SCL & SDA
    wires from 19,18 to 37,38 on a T3.5.  This also worked
*/

// Wire Master Reader
// by Nicholas Zambetti <http://www.zambetti.com>

// Demonstrates use of the Wire library
// Reads data from an I2C/TWI slave device
// Refer to the "Wire Slave Sender" example for use with this

// Created 29 March 2006

// This example code is in the public domain.


#include <Wire.h>

int led = LED_BUILTIN;

void setup()
{
  pinMode(led, OUTPUT);
  //Wire.begin();             // join i2c bus (address optional for master)
  Wire1.begin();             // join i2c bus (address optional for master)
  Serial.begin(9600);       // start serial for output
}

void loop()
{
  Serial.print("read: ");

  digitalWrite(led, HIGH);  // briefly flash the LED
  //Wire.requestFrom(8, 6);   // request 6 bytes from slave device #8
  Wire1.requestFrom(8, 6);   // request 6 bytes from slave device #8

  //while (Wire.available()) { // slave may send less than requested
  //  char c = Wire.read();   // receive a byte as character
  while (Wire1.available()) { // slave may send less than requested
    char c = Wire1.read();   // receive a byte as character
    Serial.print(c);        // print the character
  }

  Serial.println();
  digitalWrite(led, LOW);
  delay(500);
}

Name: Wire_Master_Reader.ino

Created: 1/18/2022 9:11:05 PM

Author: NEWXPS15\paynt

First trial was with all default settings, and

connected to Wire_Slave_Sender.ino via pins 18,19

on both ends. This worked as expected

Second trial was to change all 'Wire.*' to 'Wire1.*'

in this program, and physically move the SCL & SDA

wires from 19,18 to 37,38 on a T3.5. This also worked

// Wire Master Reader

// by Nicholas Zambetti <http://www.zambetti.com>

// Demonstrates use of the Wire library

// Reads data from an I2C/TWI slave device

// Refer to the "Wire Slave Sender" example for use with this

// Created 29 March 2006

// This example code is in the public domain.

#include <Wire.h>

int led = LED_BUILTIN;

void setup()

{

pinMode(led, OUTPUT);

//Wire.begin(); // join i2c bus (address optional for master)

Wire1.begin(); // join i2c bus (address optional for master)

Serial.begin(9600); // start serial for output

}

void loop()

{

Serial.print("read: ");

digitalWrite(led, HIGH); // briefly flash the LED

//Wire.requestFrom(8, 6); // request 6 bytes from slave device #8

Wire1.requestFrom(8, 6); // request 6 bytes from slave device #8

//while (Wire.available()) { // slave may send less than requested

// char c = Wire.read(); // receive a byte as character

while (Wire1.available()) { // slave may send less than requested

char c = Wire1.read(); // receive a byte as character

Serial.print(c); // print the character

}

Serial.println();

digitalWrite(led, LOW);

delay(500);

}

/*
 Name:		Wire_Slave_Sender.ino
 Created:	1/18/2022 9:08:53 PM
 Author:	paynt
*/
// Wire Slave Sender
// by Nicholas Zambetti <http://www.zambetti.com>

// Demonstrates use of the Wire library
// Sends data as an I2C/TWI slave device
// Refer to the "Wire Master Reader" example for use with this

// Created 29 March 2006

// This example code is in the public domain.


#include <Wire.h>

int led = LED_BUILTIN;

void setup()
{
  pinMode(led, OUTPUT);
  Wire.begin(8);                // join i2c bus with address #8
  Wire.onRequest(requestEvent); // register event
}

void loop()
{
  delay(100);
}

// function that executes whenever data is requested by master
// this function is registered as an event, see setup()
void requestEvent()
{
  digitalWrite(led, HIGH);  // briefly flash the LED
  Wire.write("hello ");     // respond with message of 6 bytes
                            // as expected by master
  digitalWrite(led, LOW);
}

Name: Wire_Slave_Sender.ino

Created: 1/18/2022 9:08:53 PM

Author: paynt

// Wire Slave Sender

// by Nicholas Zambetti <http://www.zambetti.com>

// Demonstrates use of the Wire library

// Sends data as an I2C/TWI slave device

// Refer to the "Wire Master Reader" example for use with this

// Created 29 March 2006

// This example code is in the public domain.

#include <Wire.h>

int led = LED_BUILTIN;

void setup()

{

pinMode(led, OUTPUT);

Wire.begin(8); // join i2c bus with address #8

Wire.onRequest(requestEvent); // register event

}

void loop()

{

delay(100);

}

// function that executes whenever data is requested by master

// this function is registered as an event, see setup()

void requestEvent()

{

digitalWrite(led, HIGH); // briefly flash the LED

Wire.write("hello "); // respond with message of 6 bytes

// as expected by master

digitalWrite(led, LOW);

}

and here’s a sampling of the output:

Opening port
Port open
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello

Opening port

Port open

read: hello

OK, now that I understand the <Wire.h> vs <i2c_t3.h> issues, I still have at least one more hurdle to clear. It appears that ” MPU6050_6Axis_MotionApps_V6_12.h ” is an older version of the DMP-enabled code for the MPU6050, and “MPU6050_6Axis_MotionApps612.h” (no ‘V’, no underscores in version number) is the latest and greatest. However, using this version also requires the correct version of i2cdev.h (I think). In any case, I was able to change the #includes on my T35_WallE3_V5 project to “MPU6050_6Axis_MotionApps612.h” and “I2Cdev.h” and get the program to compile for a T3.5 target. Whether or not it will actually behave as required is TBD.

To pursue this issue, I set up a simple T3.5 – MPU6050 plugboard configuration, and loaded an old MPU6050 test project – “Teensy_MPU6050_DMP6_V3”. This project uses the older “MPU6050_6Axis_MotionApps_V6_12.h” code (located in the project folder) along with I2Cdev.h/cpp, helper_3dmath.h, and MPU6050.h/cpp all in the project folder. It compiled right out of the box, and I was able to demonstrate successful interfacing with the MPU6050.

Next, I changed #include “MPU6050_6Axis_MotionApps_V6_12.h” to #include “MPU6050_6Axis_MotionApps612.h” whereupon it blew a whole bunch of compile errors. I was able to confirm that, due to the ‘hard’ symlink magic, the compiler thinks the “MPU6050_6Axis_MotionApps612.h” file is at …\Arduino\Libraries\MPU6050 rather than way down in the i2cdevlib tree – yay!

At this point, rather than continuing to modify the Teensy_MPU6050_DMP6_V3 project, I decided to create a Teensy_MPU6050_DMP6_V4 project and make all the changes there, so that when I screw up and get lost I can go back and start all over (ask me how I know to do this….). So I changed the include back to #include “MPU6050_6Axis_MotionApps_V6_12.h” and verified it still compiled OK, then I created a new project called Teensy_MPU6050_DMP6_V4 and copy/pasted the entire .ino file into it.

When I tried to compile the new project, it blew a bunch of errors about MPU6050_Base:: functions not being found. This sounds like the i2cdev.h/cpp that is being found is the later one, so I went ahead and changed #include “MPU6050_6Axis_MotionApps_V6_12.h” to #include “MPU6050_6Axis_MotionApps612.h” without doing anything else. This time it didn’t blow any errors about MPU6050_Base:: functions but did blow some siimilar to ‘I2C_PULLUP_EXT was not declared’. I think this is due to using <Wire.h> in the #include chain rather than <i2c_t3.h>, so I changed

Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);
Wire.setDefaultTimeout(200000); // 200ms

to 

Wire.begin()

Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);

Wire.setDefaultTimeout(200000); // 200ms

Wire.begin()

With just that one change, the project now compiles for a Teensy 3.5 target – woohoo! In addition, I didn’t have to add any references to the project, which I have had to do in almost every other case – double woohoo!

Then I uploaded this project to the T3.5, and it actually worked – the MPU6050 is generating valid azimuth values – triple woohoo!

So now I have a working program using the latest/greatest DMP-enabled driver for the MPU6050, and a much simpler #include file/reference setup. Compare this:

to this:

Just for grins, I modified the _V4 project to see if I can move the MPU6050 from Wire (pins 19,18) to Wire1 (pins 37,38). This requires changing MPU6050 mpu to MPU6050((uint8_t) 0x68, &Wire1) and Wire.begin to Wire1.begin(). This worked like a champ, so at this point I think I have figured out all I need to know on this subject.

20 January 2022 – One last piece of the puzzle:

I use Nick Gammon’s wonderful I2C_Anything library (just two template functions, but…) a lot, but it doesn’t support multiple I2C buses. So, I decided to see if I could modify his template functions to accept a default argument that if present, specifies which I2C bus to use. Here’s the modified file, temporarily renamed to ‘I2C_AnythingMultiWire.h’

// Written by Nick Gammon
// May 2012
// Jan 2022 Rev by Frank Paynter to accomodate multiple I2C busses

#include <Arduino.h>
#include <Wire.h> //07/06/20 chg back for I2C hangup testing

template <typename T> unsigned int I2C_writeAnything (const T& value, void *wireObj = 0)
  {

  //01/20/22 allow use of Wire1, Wire2, etc
  TwoWire* useWire = &Wire; //default is 'original' Wire
  if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-default addr was supplied
  const byte * p = (const byte*) &value;
    unsigned int i;
    for (i = 0; i < sizeof value; i++)
      useWire->write(*p++);

    return i;
  }  // end of I2C_writeAnything

template <typename T> unsigned int I2C_readAnything(T& value, TwoWire* wireObj = 0)
  {
  //01/20/22 allow use of Wire1, Wire2, etc
  TwoWire* useWire = &Wire; //default is 'original' Wire
  if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-default addr was supplied

    byte * p = (byte*) &value;
    unsigned int i;
    for (i = 0; i < sizeof value; i++)
          *p++ = useWire->read();

    return i;
  }  // end of I2C_readAnything

// Written by Nick Gammon

// May 2012

// Jan 2022 Rev by Frank Paynter to accomodate multiple I2C busses

#include <Arduino.h>

#include <Wire.h> //07/06/20 chg back for I2C hangup testing

template <typename T> unsigned int I2C_writeAnything (const T& value, void *wireObj = 0)

{

//01/20/22 allow use of Wire1, Wire2, etc

TwoWire* useWire = &Wire; //default is 'original' Wire

if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-default addr was supplied

const byte * p = (const byte*) &value;

unsigned int i;

for (i = 0; i < sizeof value; i++)

useWire->write(*p++);

return i;

} // end of I2C_writeAnything

template <typename T> unsigned int I2C_readAnything(T& value, TwoWire* wireObj = 0)

{

//01/20/22 allow use of Wire1, Wire2, etc

TwoWire* useWire = &Wire; //default is 'original' Wire

if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-default addr was supplied

byte * p = (byte*) &value;

unsigned int i;

for (i = 0; i < sizeof value; i++)

*p++ = useWire->read();

return i;

} // end of I2C_readAnything

I used my previously constructed ‘Wire_Slave_Sender.ino’ and ‘Wire_Master_Reader.ino’ projects to test this, and it seems to work just as advertised. In ‘Wire_Slave_Sender.ino’ I use:

  I2C_writeAnything(val);//2nd arg missing, so &Wire selected by default

1	I2C_writeAnything(val);//2nd arg missing, so &Wire selected by default

Which automagically uses ‘Wire’ because the 2nd argument is missing from the call, and in ‘Wire_Master_Reader.ino’ I use:

I2C_readAnything(val, &Wire1);

1	I2C_readAnything(val, &Wire1);

Which causes Wire1 to be used. Here are both demo programs in their entirety, along with the full ‘I2C_AnythingMultiWire.h’ file:

/*
 Name:		Wire_Slave_Sender.ino
 Created:	1/18/2022 9:08:53 PM
 Author:	paynt
*/
// Wire Slave Sender
// by Nicholas Zambetti <http://www.zambetti.com>

// Demonstrates use of the Wire library
// Sends data as an I2C/TWI slave device
// Refer to the "Wire Master Reader" example for use with this

// Created 29 March 2006

// This example code is in the public domain.
// 01/20/22 modified to use I2C_Anything to send a float value


#include <Wire.h>
//#include "I2C_Anything.h"
#include "I2C_AnythingMultiWire.h"



int led = LED_BUILTIN;

void setup()
{
  pinMode(led, OUTPUT);
  Wire.begin(8);                // join i2c bus with address #8
  Wire.onRequest(requestEvent); // register event
}

void loop()
{
  delay(100);
}

// function that executes whenever data is requested by master
// this function is registered as an event, see setup()
void requestEvent()
{
  digitalWrite(led, HIGH);  // briefly flash the LED
  //Wire.write("hello ");     // respond with message of 6 bytes
  // 
  // 01/20/22 rev to use I2C_Anything to write a double val
  double val = 3.14159;
  Serial.printf("sending %2.4f to master\n", val);
  I2C_writeAnything(val);//2nd arg missing, so &Wire selected by default
  //I2C_writeAnything(val, &Wire);
                            // as expected by master
  digitalWrite(led, LOW);
}

Name: Wire_Slave_Sender.ino

Created: 1/18/2022 9:08:53 PM

Author: paynt

// Wire Slave Sender

// by Nicholas Zambetti <http://www.zambetti.com>

// Demonstrates use of the Wire library

// Sends data as an I2C/TWI slave device

// Refer to the "Wire Master Reader" example for use with this

// Created 29 March 2006

// This example code is in the public domain.

// 01/20/22 modified to use I2C_Anything to send a float value

#include <Wire.h>

//#include "I2C_Anything.h"

#include "I2C_AnythingMultiWire.h"

int led = LED_BUILTIN;

void setup()

{

pinMode(led, OUTPUT);

Wire.begin(8); // join i2c bus with address #8

Wire.onRequest(requestEvent); // register event

}

void loop()

{

delay(100);

}

// function that executes whenever data is requested by master

// this function is registered as an event, see setup()

void requestEvent()

{

digitalWrite(led, HIGH); // briefly flash the LED

//Wire.write("hello "); // respond with message of 6 bytes

// 01/20/22 rev to use I2C_Anything to write a double val

double val = 3.14159;

Serial.printf("sending %2.4f to master\n", val);

I2C_writeAnything(val);//2nd arg missing, so &Wire selected by default

//I2C_writeAnything(val, &Wire);

// as expected by master

digitalWrite(led, LOW);

}

/*
    Name:       Wire_Master_Reader.ino
    Created:	1/18/2022 9:11:05 PM
    Author:     NEWXPS15\paynt

    First trial was with all default settings, and 
    connected to Wire_Slave_Sender.ino via pins 18,19
    on both ends. This worked as expected

    Second trial was to change all 'Wire.*' to 'Wire1.*'
    in this program, and physically move the SCL & SDA
    wires from 19,18 to 37,38 on a T3.5.  This also worked
*/

// Wire Master Reader
// by Nicholas Zambetti <http://www.zambetti.com>

// Demonstrates use of the Wire library
// Reads data from an I2C/TWI slave device
// Refer to the "Wire Slave Sender" example for use with this

// Created 29 March 2006

// This example code is in the public domain.
// 01/20/22 modified to use I2C_Anything to ask for a 'double' (4 byte) value


#include <Wire.h>
//#include "I2C_Anything1.h"
#include "I2C_AnythingMultiWire.h"


const int led = LED_BUILTIN;
const int SLAVE_ADDR = 0x8;
  double val = 0;

void setup()
{
  pinMode(led, OUTPUT);
  //Wire.begin();             // join i2c bus (address optional for master)
  Wire1.begin();             // join i2c bus (address optional for master)
  Serial.begin(9600);       // start serial for output
}

void loop()
{
  Serial.print("read: ");

  digitalWrite(led, HIGH);  // briefly flash the LED
  //Wire1.requestFrom(8, 6);   // request 6 bytes from slave device #8
  Wire1.requestFrom(8, sizeof(val));   // request 6 bytes from slave device #8

  //while (Wire.available()) { // slave may send less than requested
  //  char c = Wire.read();   // receive a byte as character

  //while (Wire1.available()) { // slave may send less than requested
  //  char c = Wire1.read();   // receive a byte as character
  //  Serial.print(c);        // print the character
  //}

  //01/20/22 now read double value using I2C_Anything
  //I2C_readAnything1(val);
  I2C_readAnything(val, &Wire1);
  Serial.printf("read %2.4f from Slave\n", val);

  //Serial.println();
  digitalWrite(led, LOW);
  delay(500);
}

Name: Wire_Master_Reader.ino

Created: 1/18/2022 9:11:05 PM

Author: NEWXPS15\paynt

First trial was with all default settings, and

connected to Wire_Slave_Sender.ino via pins 18,19

on both ends. This worked as expected

Second trial was to change all 'Wire.*' to 'Wire1.*'

in this program, and physically move the SCL & SDA

wires from 19,18 to 37,38 on a T3.5. This also worked

// Wire Master Reader

// by Nicholas Zambetti <http://www.zambetti.com>

// Demonstrates use of the Wire library

// Reads data from an I2C/TWI slave device

// Refer to the "Wire Slave Sender" example for use with this

// Created 29 March 2006

// This example code is in the public domain.

// 01/20/22 modified to use I2C_Anything to ask for a 'double' (4 byte) value

#include <Wire.h>

//#include "I2C_Anything1.h"

#include "I2C_AnythingMultiWire.h"

const int led = LED_BUILTIN;

const int SLAVE_ADDR = 0x8;

double val = 0;

void setup()

{

pinMode(led, OUTPUT);

//Wire.begin(); // join i2c bus (address optional for master)

Wire1.begin(); // join i2c bus (address optional for master)

Serial.begin(9600); // start serial for output

}

void loop()

{

Serial.print("read: ");

digitalWrite(led, HIGH); // briefly flash the LED

//Wire1.requestFrom(8, 6); // request 6 bytes from slave device #8

Wire1.requestFrom(8, sizeof(val)); // request 6 bytes from slave device #8

//while (Wire.available()) { // slave may send less than requested

// char c = Wire.read(); // receive a byte as character

//while (Wire1.available()) { // slave may send less than requested

// char c = Wire1.read(); // receive a byte as character

// Serial.print(c); // print the character

//}

//01/20/22 now read double value using I2C_Anything

//I2C_readAnything1(val);

I2C_readAnything(val, &Wire1);

Serial.printf("read %2.4f from Slave\n", val);

//Serial.println();

digitalWrite(led, LOW);

delay(500);

}

// Written by Nick Gammon
// May 2012
// Jan 2022 Rev by Frank Paynter to accomodate multiple I2C busses

#include <Arduino.h>
#include <Wire.h> //07/06/20 chg back for I2C hangup testing

template <typename T> unsigned int I2C_writeAnything (const T& value, void *wireObj = 0)
  {

  //01/20/22 allow use of Wire1, Wire2, etc
  TwoWire* useWire = &Wire; //default is 'original' Wire
  if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-default addr was supplied
  const byte * p = (const byte*) &value;
    unsigned int i;
    for (i = 0; i < sizeof value; i++)
      useWire->write(*p++);

    return i;
  }  // end of I2C_writeAnything

template <typename T> unsigned int I2C_readAnything(T& value, TwoWire* wireObj = 0)
  {
  //01/20/22 allow use of Wire1, Wire2, etc
  TwoWire* useWire = &Wire; //default is 'original' Wire
  if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-default addr was supplied

    byte * p = (byte*) &value;
    unsigned int i;
    for (i = 0; i < sizeof value; i++)
          *p++ = useWire->read();

    return i;
  }  // end of I2C_readAnything

// Written by Nick Gammon

// May 2012

// Jan 2022 Rev by Frank Paynter to accomodate multiple I2C busses

#include <Arduino.h>

#include <Wire.h> //07/06/20 chg back for I2C hangup testing

template <typename T> unsigned int I2C_writeAnything (const T& value, void *wireObj = 0)

{

//01/20/22 allow use of Wire1, Wire2, etc

TwoWire* useWire = &Wire; //default is 'original' Wire

if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-default addr was supplied

const byte * p = (const byte*) &value;

unsigned int i;

for (i = 0; i < sizeof value; i++)

useWire->write(*p++);

return i;

} // end of I2C_writeAnything

template <typename T> unsigned int I2C_readAnything(T& value, TwoWire* wireObj = 0)

{

//01/20/22 allow use of Wire1, Wire2, etc

TwoWire* useWire = &Wire; //default is 'original' Wire

if (wireObj) useWire = (TwoWire*)wireObj;//use this if a non-default addr was supplied

byte * p = (byte*) &value;

unsigned int i;

for (i = 0; i < sizeof value; i++)

*p++ = useWire->read();

return i;

} // end of I2C_readAnything

I made a ‘pull request’ to the I2C_Anything github repo so that everyone who uses it can benefit, but in the meantime feel free to use the I2C_AnythingMultiWire.h file.

25 January Update: Just one more ‘one last piece of the puzzle’

When I was using the ‘i2c_t3.h’ library, I noticed that I didn’t have to use external pullup resistors as long as I used the ‘I2C_PULLUP_INT’ treatment in the Wire.begin() call. This was somewhat contrary to the general run of the posts on the Teensy forum, so it was a bit disconcerting. However, I ran a series of experiments that clearly showed that I2C between two Teensy 3.5 processors didn’t need external pullups, and O’scope waveform analysis showed no difference between using external 2.2KΩ pullups and no external pullups with the ‘I2C_PULLUP_INT’ option.

However, when I switched to the ‘Wire.h’ library, all this changed. I had to use external 2.2KΩ pullups – and this was verified via O’scope analysis. This usually isn’t a big deal, but it turns out that in my case it’s going to be a real PITA to add the pullups. my hardware configuration uses all point-to-point jumpers and no PCB, so there just isn’t any easy way to do this.

You would think that, since there is obviously a way to enable the pullups on the I2C lines (obviously, because that is exactly what the i2c_t3.h library does), there must be a way to do the same thing with the Wire.h library. You would think that, but I’ll be darned if I can figure it out. I have posted this issue to the Teensy forum, but so far no luck finding a solution.

OK, I may have found a clue. Buried deep in i2c_t3.cpp is the following macro:

#define PIN_CONFIG_ALT(name,alt) uint32_t name = (pullup == I2C_PULLUP_EXT) ? (PORT_PCR_MUX(alt)|PORT_PCR_ODE|PORT_PCR_SRE|PORT_PCR_DSE) \
                                                                            : (PORT_PCR_MUX(alt)|PORT_PCR_PE|PORT_PCR_PS)

1 2	#define PIN_CONFIG_ALT(name,alt) uint32_t name = (pullup == I2C_PULLUP_EXT) ? (PORT_PCR_MUX(alt)\|PORT_PCR_ODE\|PORT_PCR_SRE\|PORT_PCR_DSE) \ : (PORT_PCR_MUX(alt)\|PORT_PCR_PE\|PORT_PCR_PS)

it is this macro that actually casts the magic spell over the currently defined SCL & SDA pins to enable (or disable) internal pullups.

29 January 2022 Update:

After a lot of forum and Google searching, I decided to try some small experiments regarding how to set an ‘open-drain’ or ‘input-pullup’ configuration on a Teensy 3.5 GPIO pin. Here’s the code:

/*
    Name:       T35_PortDiddle_V1.ino
    Created:	1/28/2022 9:31:05 PM
    Author:     DESKTOP-RNC43DB\Frank
*/

#define SDA1 38
#define SCL1 37
#define TEST1_PIN 12
#define TEST2_PIN 11

void setup()
{
  Serial.begin(115200);
  delay(1000);

  pinMode(TEST1_PIN, INPUT_PULLUP);
  pinMode(TEST2_PIN, INPUT_PULLUP);

  Serial.printf("Waiting for LOW on pin 12\n");
}

//CORE_PIN_CONCATENATE(14, DDRREG);
void loop()
{
  if (digitalRead(TEST1_PIN)==LOW)
  {
    Serial.printf("\n%lu\tsetting SDA1 to OUTPUT, LOW\n", millis());

    digitalWrite(LED_BUILTIN, HIGH);
    delay(500);
    digitalWrite(LED_BUILTIN, LOW);

    pinMode(SDA1, OUTPUT);
    digitalWrite(SDA1, LOW);
    delay(1000);

    Serial.printf("%lu\tsetting SDA1 to INPUT - i.e. open-drain\n", millis());
    digitalWrite(LED_BUILTIN, HIGH);
    delay(1000);
    digitalWrite(LED_BUILTIN, LOW);
    pinMode(SDA1, INPUT);
    Serial.printf("%lu\t test complete\n", millis());
  }

  if (digitalRead(TEST2_PIN)==LOW)
  {
    Serial.printf("\n%lu\tsetting SCL1 to OUTPUT, LOW\n", millis());

    digitalWrite(LED_BUILTIN, HIGH);
    delay(500);
    digitalWrite(LED_BUILTIN, LOW);

    pinMode(SCL1, OUTPUT);
    digitalWrite(SCL1, LOW);
    delay(1000);

    Serial.printf("%lu\tsetting SCL1 to INPUT_PULLUP\n", millis());
    digitalWrite(LED_BUILTIN, HIGH);
    delay(1000);
    digitalWrite(LED_BUILTIN, LOW);
    pinMode(SCL1, INPUT_PULLUP);
    Serial.printf("%lu\t test complete\n", millis());
  }
  while (digitalRead(TEST1_PIN)==HIGH && digitalRead(TEST2_PIN) == HIGH)
  {
    Serial.printf(".");
    delay(1000);
  }

}

Name: T35_PortDiddle_V1.ino

Created: 1/28/2022 9:31:05 PM

Author: DESKTOP-RNC43DB\Frank

#define SDA1 38

#define SCL1 37

#define TEST1_PIN 12

#define TEST2_PIN 11

void setup()

{

Serial.begin(115200);

delay(1000);

pinMode(TEST1_PIN, INPUT_PULLUP);

pinMode(TEST2_PIN, INPUT_PULLUP);

Serial.printf("Waiting for LOW on pin 12\n");

}

//CORE_PIN_CONCATENATE(14, DDRREG);

void loop()

{

if (digitalRead(TEST1_PIN)==LOW)

{

Serial.printf("\n%lu\tsetting SDA1 to OUTPUT, LOW\n", millis());

digitalWrite(LED_BUILTIN, HIGH);

delay(500);

digitalWrite(LED_BUILTIN, LOW);

pinMode(SDA1, OUTPUT);

digitalWrite(SDA1, LOW);

delay(1000);

Serial.printf("%lu\tsetting SDA1 to INPUT - i.e. open-drain\n", millis());

digitalWrite(LED_BUILTIN, HIGH);

delay(1000);

digitalWrite(LED_BUILTIN, LOW);

pinMode(SDA1, INPUT);

Serial.printf("%lu\t test complete\n", millis());

}

if (digitalRead(TEST2_PIN)==LOW)

{

Serial.printf("\n%lu\tsetting SCL1 to OUTPUT, LOW\n", millis());

digitalWrite(LED_BUILTIN, HIGH);

delay(500);

digitalWrite(LED_BUILTIN, LOW);

pinMode(SCL1, OUTPUT);

digitalWrite(SCL1, LOW);

delay(1000);

Serial.printf("%lu\tsetting SCL1 to INPUT_PULLUP\n", millis());

digitalWrite(LED_BUILTIN, HIGH);

delay(1000);

digitalWrite(LED_BUILTIN, LOW);

pinMode(SCL1, INPUT_PULLUP);

Serial.printf("%lu\t test complete\n", millis());

}

while (digitalRead(TEST1_PIN)==HIGH && digitalRead(TEST2_PIN) == HIGH)

{

Serial.printf(".");

delay(1000);

}

And here’s some of the output:

Port open
..............................
9564036	setting SDA1 to OUTPUT, LOW
9565536	setting SDA1 to INPUT - i.e. open-drain
9566536	 test complete
....
9570536	setting SCL1 to OUTPUT, LOW
9572036	setting SCL1 to INPUT_PULLUP
9573036	 test complete
.......

Port open

..............................

9564036 setting SDA1 to OUTPUT, LOW

9565536 setting SDA1 to INPUT - i.e. open-drain

9566536 test complete

....

9570536 setting SCL1 to OUTPUT, LOW

9572036 setting SCL1 to INPUT_PULLUP

9573036 test complete

.......

In particular, I was able to indirectly measure the pin pullup resistor value by tying the pin to GND through a 10KΩ resistor. The voltage at the junction was about 0.78V, so the voltage divider equation Vr2 = V *R2/(R1+R2) when solved for Vr2 = 0.78V and R2 = 10K gives R1 ~33K, which agrees well with the known pullup resistor value for the Teensy 3.5.

From this it seems that I should be able to initialize the appropriate pins for I2C with ‘wirex.begin()’ and then follow that with the code to set the pins for input_pullup. We’ll see.

I uploaded a very basic I2C ‘master’ sketch to my T3.5, as shown:

/*
    Name:       T35_I2C_PullupMaster.ino
    Created:	1/28/2022 7:38:01 PM
    Author:     DESKTOP-RNC43DB\Frank
*/
#include <elapsedMillis.h>
#include <Wire.h>


elapsedMillis mSecSinceLastLEDToggle;
#define LED_TOGGLE_MSEC 200
void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);
  Wire1.begin();             // join i2c bus (address optional for master)
  Serial.begin(115200);       // start serial for output


  mSecSinceLastLEDToggle = 0;
}

void loop()
{
  if (mSecSinceLastLEDToggle >= LED_TOGGLE_MSEC)
  {
    mSecSinceLastLEDToggle -= LED_TOGGLE_MSEC;
    digitalToggle(LED_BUILTIN);

    Serial.print("read: ");

    Wire1.requestFrom(8, 6);   // request 6 bytes from slave device #8

    while (Wire1.available()) { // slave may send less than requested
      char c = Wire1.read();   // receive a byte as character
      Serial.print(c);        // print the character
    }

    Serial.println();
  }

}

Name: T35_I2C_PullupMaster.ino

Created: 1/28/2022 7:38:01 PM

Author: DESKTOP-RNC43DB\Frank

#include <elapsedMillis.h>

#include <Wire.h>

elapsedMillis mSecSinceLastLEDToggle;

#define LED_TOGGLE_MSEC 200

void setup()

{

pinMode(LED_BUILTIN, OUTPUT);

Wire1.begin(); // join i2c bus (address optional for master)

Serial.begin(115200); // start serial for output

mSecSinceLastLEDToggle = 0;

}

void loop()

{

if (mSecSinceLastLEDToggle >= LED_TOGGLE_MSEC)

{

mSecSinceLastLEDToggle -= LED_TOGGLE_MSEC;

digitalToggle(LED_BUILTIN);

Serial.print("read: ");

Wire1.requestFrom(8, 6); // request 6 bytes from slave device #8

while (Wire1.available()) { // slave may send less than requested

char c = Wire1.read(); // receive a byte as character

Serial.print(c); // print the character

}

Serial.println();

}

And verified with an O’scope that the SDA & SCL pins were active, and that external pullup resistors were required.

When I added ‘pinMode(SCL1, INPUT_PULLUP);’ just after Wire1.begin(), the I2C activity was disabled – no signal at all on either line. I tried some other combinations of pinMode() and digitalWrite(), but nothing changed – clearly the use of pinMode() and/or digitalWrite() overwrites at least some of the required configuration for I2C output.

So, next I plan to try some direct port control and see if that will do the trick.

From Kurt E’s spreadsheet, SCL1 & SDA1 (pins 37/38) are PortC pin 10 & 11 respectively. So,

PORT_PCR_MUX(n): selects the ALTernate function for the pin in question. PORT_PCR_MUX(1) just selects the pin as a GPIO. For instance, to select T3.5 pin 37 as I2C1 SCL, we would use PORT_PCR_MUX(2). Thus, the code line might look like:

CORE_PIN37_CONFIG = PORT_PCR_ODE | PORT_PCR_MUX(2);

1	CORE_PIN37_CONFIG = PORT_PCR_ODE \| PORT_PCR_MUX(2);

where ‘PORT_PCR_ODE’ is the defined constant that selects the ‘Open-Drain-Enable’ bit in the Port Control Register for Port C, bit 10, which is connected to T3.5 pin 37. ‘PORT_PCR_ODE’ is defined in Kinetis.h as:

#define PORT_PCR_ODE			((uint32_t)0x00000020)		// Open Drain Enable

1	#define PORT_PCR_ODE ((uint32_t)0x00000020) // Open Drain Enable

The ‘0x00000020’ part, when translated to binary is: 0000 0000 0000 0000 0000 0000 0001 0000 << selects the 5th bit in the 32-bit Port Control Register.

PORT_PCR_MUX(n) is defined in Kinetis.h as:

#define PORT_PCR_MUX(n)			((uint32_t)(((n) & 7) << 8))	// Pin Mux Control

1	#define PORT_PCR_MUX(n) ((uint32_t)(((n) & 7) << 8)) // Pin Mux Control

so PORT_PCR_MUX(2) –> (uint32_t)(((2 & 7) << 8)) –> (uint32_t)(((0010 & 0111) << 8)) –> (uint32_t)(((0010) << 8)) –> (uint32_t)(0010 << 8) –> 0000 0000 0000 0000 0000 0000 0000 0010 << 8 –> 0000 0000 0000 0000 0000 0010 0000 0000 –> 0x200

Based on the above information, I thought that I might be able to accomplish my goal by using the following construct in setup():

Wire1.begin();             // join i2c bus (address optional for master)
CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);

Wire1.begin(); // join i2c bus (address optional for master)

CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);

CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);

Unfortunately, this did not work; I2C activity on pins 37/38 ‘flatlined’ and that was that. However, after letting my mind work on the problem while the rest of me slept, I had a new thought when I woke up this morning. Maybe the problem with the above construct is the ‘PORT_PCR_MUX(1)’ fragment. Maybe selecting the default GPIO port overwrites the prior I2C function selection?

So, I decided to try again this morning, using ‘PORT_PCR_MUX(2)’ (I2C function selected) instead. The code looks like this:

  Wire1.begin();             // join i2c bus (address optional for master)
  CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);
  CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

Wire1.begin(); // join i2c bus (address optional for master)

CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

And, “son of a gun” – it worked! pins 37/38 activity continued, and physical pullups aren’t required – YES!

Here’s the full program:

/*
    Name:       T35_I2C_PullupMaster.ino
    Created:	1/28/2022 7:38:01 PM
    Author:     DESKTOP-RNC43DB\Frank
*/
#include <elapsedMillis.h>
#include <Wire.h>

#define SCL1 37 //added to test pullup idea
#define SDA1 38 //added to test pullup idea


elapsedMillis mSecSinceLastLEDToggle;
#define LED_TOGGLE_MSEC 200
void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);
  Wire1.begin();             // join i2c bus (address optional for master)

  ////01/29/2022 this doesn't work - flatlines I2C outputs
  //pinMode(SCL1, OUTPUT);
  //digitalWrite(SCL1, LOW);
  //pinMode(SCL1, INPUT_PULLUP);

  //01/29/2022 this doesn't work either - flatlines I2C outputs
  //CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
  //CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);

  //01/30/2022 tried again with PORT_PCR_MUX(2) this worked!!!!
  CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);
  CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

  Serial.begin(115200);       // start serial for output


  mSecSinceLastLEDToggle = 0;
}

void loop()
{
  if (mSecSinceLastLEDToggle >= LED_TOGGLE_MSEC)
  {
    mSecSinceLastLEDToggle -= LED_TOGGLE_MSEC;
    digitalToggle(LED_BUILTIN);

    Serial.print("read: ");

    Wire1.requestFrom(8, 6);   // request 6 bytes from slave device #8

    while (Wire1.available()) { // slave may send less than requested
      char c = Wire1.read();   // receive a byte as character
      Serial.print(c);        // print the character
    }

    Serial.println();
  }

}

Name: T35_I2C_PullupMaster.ino

Created: 1/28/2022 7:38:01 PM

Author: DESKTOP-RNC43DB\Frank

#include <elapsedMillis.h>

#include <Wire.h>

#define SCL1 37 //added to test pullup idea

#define SDA1 38 //added to test pullup idea

elapsedMillis mSecSinceLastLEDToggle;

#define LED_TOGGLE_MSEC 200

void setup()

{

pinMode(LED_BUILTIN, OUTPUT);

Wire1.begin(); // join i2c bus (address optional for master)

////01/29/2022 this doesn't work - flatlines I2C outputs

//pinMode(SCL1, OUTPUT);

//digitalWrite(SCL1, LOW);

//pinMode(SCL1, INPUT_PULLUP);

//01/29/2022 this doesn't work either - flatlines I2C outputs

//CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);

//CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);

//01/30/2022 tried again with PORT_PCR_MUX(2) this worked!!!!

CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

Serial.begin(115200); // start serial for output

mSecSinceLastLEDToggle = 0;

}

void loop()

{

if (mSecSinceLastLEDToggle >= LED_TOGGLE_MSEC)

{

mSecSinceLastLEDToggle -= LED_TOGGLE_MSEC;

digitalToggle(LED_BUILTIN);

Serial.print("read: ");

Wire1.requestFrom(8, 6); // request 6 bytes from slave device #8

while (Wire1.available()) { // slave may send less than requested

char c = Wire1.read(); // receive a byte as character

Serial.print(c); // print the character

}

Serial.println();

}

I2C activity visible on scope with 2.2K pullup resistors (foreground under green wire jumper) disconnected

I went ahead and connected my plug-board ‘master’ with the Teensy 3.5 on my ‘second-deck’ plate to test end-end I2C comms without external 2.2KΩ pullups. I loaded the Wire library master_reader example on the plugboard Teensy 3.5, and the slave_sender example on the ‘second-deck’ Teensy 3.5. Then I connected Wire1 on the master to Wire (19/18) on the slave. I modified both the master and slave examples with the

CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

1	CORE_PIN37_CONFIG = PORT_PCR_PE \| PORT_PCR_PS \| PORT_PCR_MUX(2);

On the affected lines (37/38 on the master, 19/18 on the slave). When I ran the examples, I got the following output:

Port closed
Opening port
Port open
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello 
read: hello

Port closed

Opening port

Port open

read: hello

I ran the above experiment with both ends modified for internal pullups, just one end modified, and one or both modified with external 2.2K pullups. the link worked perfectly for all these cases. Her’s a photo of the setup:

master/slave I2C connection with 6″ jumpers. Note 2.2K resistors NOT used

Frank