Left Side Wall Tracking Success With VL53L0X Array

Posted 05 October 2020

This post describes the successful left-side wall-tracking performance of my re-motored, re-wheeled, and re-sensored robot. Back in January of this year I was able to demonstrate reasonable wall tracking performance with my two-wheel robot using the old HC-SR04 ‘Ping’ sensors. However, I still wasn’t able to consistently track and maintain a desired wall offset, the main goal in this project stage

Since January, I have made the following changes to my larger four-wheel robot:

Incorporated heading-based turns using the onboard MPU6050 IMU
Developed an effective algorithm for finding the parallel heading to the nearest wall
Replaced the two ‘ping’ sensors with two 3-element VL53L0X time-of-flight sensor arrays.
Added a Teensy 3.5 MCU to manage the two VL53L0X sensor arrays and report distance measurements and steering values to the MEGA main MCU via I2C.
Improved VL53L0X response by shifting to a 20 mSec measurement budget and ‘continuous’ vs ‘single’ measurements.
Replaced the L298N motor drivers with Adafruit DRV8871 drivers.
Replaced the wheels with custom 3D printed wheels and tires.
Added a TIMER5 interrupt at 5Hz rate for sensor update timing

With all the changes, I had kind of lost track of the ultimate goal, which is to have the robot follow the nearest wall at a specified offset distance. All of the above updates were intended, in one way or another, to facilitate that goal, but I hadn’t yet got the robot to actually perform to expectations.

To help clear away some of the fog, I created a new version of the operating software that was pared down to just what was required to track the left wall, and nothing else. The idea was to work out all the bugs for offset capture and subsequent wall tracking with just the minimum required software, and then incorporate the modified code back into the mainstream software.

At first I was working with a 4-stage process;

find the parallel heading to the selected wall
drive at an angle toward the desired offset distance
when the offset distance is obtained, turn parallel to the wall again
track the wall at the desired offset

However, I found that the when the robot started off outside the desired wall offset, the second ‘turn to parallel’ operation took up too much space, both in terms of wall offset distance, and distance along the wall. By the time the second ‘find parallel’ operation was completed, the robot was usually much too close to the wall for effective offset tracking, meaning the entire 4-step process would have to be repeated. So, I eliminated step 3 in the process (the second ‘turn to parallel’ operation) entirely, and modified the wall tracking algorithm to capture the desired wall offset and track it. Instead of using the distance sensor measurements directly, I generate a ‘steering value’ proportional to the difference between the front and rear sensor measurements, and a target ‘steering value’ proportional to the difference between the desired offset and the center sensor measurement and use a PID controller to match the measured steering value to the target steering value. The effect of this is that the robot will track toward the offset at an angle, and then turn parallel to the wall and continue to track, as shown in the video below:

Left-side offset capture and track demonstration

Here’s an Excel plot showing the wall offset distance versus time for the above demonstration run.

As can be seen in the above plot, the robot starts off at about 45 cm from the wall, tracks inward to capture the desired offset, and then continues to track the desired offset even when it goes around the 45-degree bend. The code that accomplished this is posted below:

/*
    Name:       FourWD_WallE2_V7.ino
    Created:	9/24/2020 11:03:59 AM
    Author:     FRANKNEWXPS15\Frank
*/



#define TIMER_INT_OUTPUT_PIN 51 //scope monitor pin


#pragma region INCLUDES
#include <avr/sleep.h> //needed for sleep_enable() & sleep_cpu() functions
#include <elapsedMillis.h>
#include <PrintEx.h> //allows printf-style printout syntax
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/05/19
#include "I2Cdev.h" //02/19/19: this includes SBWire.h
#include "I2C_Anything.h" //needed for sending float data over I2C
#include "Adafruit_FRAM_I2C.h"
#include "RTClib.h" //07/11/20 this MUST be the #include "file" form for the "Local files override.." option to work
#include <PID.h> //moved here 02/09/19 
#include <limits.h> //added 04/19/19
#pragma endregion Includes

#pragma region DEFINES
//02/29/16 hardware defines
//#define NO_MOTORS
//#define NO_LIDAR
//#define NO_PINGS
//#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_FRAM_TELEMETRY //added 11/14/18
//#define FORCE_RTC_TO_LAST_COMPILE_TIME //added 02/18/19. Forces RTC to last compile time
#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

#pragma endregion Program #Defines

#pragma region PRE_SETUP
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
Adafruit_FRAM_I2C fram = Adafruit_FRAM_I2C();  //I2C addr = 0x50 (default)

//FramPacket.h holds 'inline' definition of CFRAMStatePacket class 
/*this has to come after the following lines
	#include "Adafruit_FRAM_I2C.h"
	Adafruit_FRAM_I2C fram = Adafruit_FRAM_I2C();
	StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
*/
#include "FramPacket.h"

//tri-sensor board 
RTC_DS3231 rtc;  //I2C addr = 0x68
MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0)
CFRAMStatePacket FramPacket = CFRAMStatePacket(&fram); //this object is used for all FRAM transactions
const char daysOfTheWeek[7][12] = { "Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday" };//used for RTC date/time readouts


//added 01/30/17 for IR homing support
#pragma region IRHOMING
uint8_t IR_HOMING_MODULE_SLAVE_ADDR = 8;  //uint8_t type reqd here for Wire.requestFrom() call
double IRHomingSetpoint, IRHomingLRSteeringVal, IRHomingOutput;//10/06/17 chg input variable name to something more descriptive
PID IRHomingPID(&IRHomingLRSteeringVal, &IRHomingOutput, &IRHomingSetpoint, 1000, 0.0, 200.0, REVERSE);//10/15/17 'final' setup
const long IR_BEAM_DETECTION_CHANNEL_MAX = 2621440;
const long IR_BEAM_DETECTION_THRESHOLD = 10000;
#pragma endregion IR Homing Parameters

#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_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", "DeadBatt", "Discharge" };
const char* LongModeStrArray[] = { "None", "Charging", "IR Homing", "Wall Following", "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 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 int MAX_AD_VALUE = 1023;
const long BATT_CHG_TIMEOUT_SEC = 36000; //10 HRS, for test only
//const long BATT_CHG_TIMEOUT_MIN = 120; //2 hours.  Chg to min vs sec 01/09/18
const float DEAD_BATT_THRESH_VOLTS = 6; //added 01/24/17
//const float DEAD_BATT_THRESH_VOLTS = 6.2; //chg to 6.2 03/02/19 per https://www.fpaynter.com/2019/03/better-battery-charging-for-wall-e2/
//const float FULL_BATT_VOLTS = 8.2; //added 03/17/18. Chg to 8.2 02/24/19
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 VOLTAGE_TO_CURRENT_RATIO = 0.75f; //Volts/Amp rev 03/01/19. Used for both 'Total' and 'Run' sensors
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
const float ADC_REF_VOLTS = 3.3; //03/27/18 now using analogReference(EXTERNAL) with Teensy 3.3V connected to AREF 
#pragma endregion Battery Constants

#pragma region DISTANCE_MEASUREMENT_SUPPORT
	//misc LIDAR and Ping sensor parameters
const int MIN_OBS_DIST_CM = 20; //rev 04/28/17 for better obstacle handling
const int CHG_STN_AVOIDANCE_DIST_CM = 30; //added 03/11/17 for charge stn avoidance
//const int STEPTURN_DIST_CM = 80; //rev 04/28/17 to be indep of MIN_OBS_DIST_CM
//const int STEPTURN_DIST_CM = 0; //rev 05/16/20 to temporarily disable
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 MIN_PING_INTERVAL_MSEC = 200; //rev 03/12/16
//const int MIN_PING_INTERVAL_MSEC = 50; //rev 10/03/20
const int MIN_PING_INTERVAL_MSEC = 30; //rev 10/03/20
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 = 25; //04/28/19 - final value (I hope)
const int FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19
const int LR_PING_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg
const int LR_PING_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_FRONT_VARIANCE_THRESHOLD = 0; //chg to 0 09/15/18
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
//byte aLeftDist[LR_PING_DIST_ARRAY_SIZE];
//byte aRightDist[LR_PING_DIST_ARRAY_SIZE];
uint16_t aLeftDist[LR_PING_DIST_ARRAY_SIZE];
uint16_t aRightDist[LR_PING_DIST_ARRAY_SIZE];

//added 12/26/14 for wall following support
int prevleftdistval = 0;
int prevrightdistval = 0;
int prevfrontdistcm = 0;
int curMinObstacleDistance = MIN_OBS_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()
int leftdistval = 0;
int rightdistval = 0;
int frontdistval = 0;
//double frontvar = 0;
volatile double frontvar = 0; //08/11/20 now updated in timer1 ISR
#pragma endregion Distance Measurement Support

#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 MOTOR_SPEED_CAPTURE_OFFSET = 125; //added 06/28/20 for offset capture
const int MOTOR_SPEED_ADJ_FACTOR = 40; //chg to 40 at 5:55pm
const int LEFT_SPEED_COMP_VAL_FWD = 15; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_FWD = -LEFT_SPEED_COMP_VAL_FWD; //right speed compensation value
const int LEFT_SPEED_COMP_VAL_REV = 5; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_REV = -LEFT_SPEED_COMP_VAL_REV; //right speed compensation value

//drive wheel direction constants
const boolean FWD_DIR = true;
const boolean REV_DIR = !FWD_DIR;

//Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;
bool bIsForwardDir = true; //default is foward direction

#pragma endregion Motor Parameters

#pragma region HEADING_BASED_TURN_PARAMS
//09/11/18 new heading-based turn support constants
const int ESS_TURN_DEG = 45;
const int QUARTER_TURN_DEG = 90;
const float DEFAULT_HDG_JUMP_VAL = 20.f;
const int DEFAULT_TURN_DEG = QUARTER_TURN_DEG;
const float ROLLING_TURN_MAX_SEC_PER_DEG = 1 / 15.0; //used to limit time in rolling turns
//const int OFFSIDE_MOTOR_SPEED = MOTOR_SPEED_LOW;
//const int OFFSIDE_MOTOR_SPEED = 0; //03/04/20 debug
const int OFFSIDE_MOTOR_SPEED = 25; //03/04/20 turn debug - leave at this value for now
const int DRIVESIDE_MOTOR_SPEED_HIGH = MOTOR_SPEED_MAX;
const int DRIVESIDE_MOTOR_SPEED_HALF = MOTOR_SPEED_HALF;//added 02/16/20 for wall tracking support
const int DRIVESIDE_MOTOR_SPEED_LOW = MOTOR_SPEED_HALF;
const long MSEC_PER_HDG_CHECK = 100; //added 08/29/18
const float HDG_NEAR_MATCH_VAL = 0.9; //slow the turn down here
//const float HDG_FULL_MATCH_VAL = 0.98; //stop the turn here
const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 05/17/20
const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started
elapsedMillis sinceLastTimeCheck; //used for rolling turn timeout
#pragma endregion Heading-based turn support parameters

#pragma region VL53L0X_TOF_LIDAR_SUPPORT
//const int ToFArray_PARALLEL_FIND_Kp = 800;
//const int ToFArray_PARALLEL_FIND_Kp = 80;
//const int ToFArray_PARALLEL_FIND_Kp = 120;
const int ToFArray_PARALLEL_FIND_Kp = 200;
const int ToFArray_PARALLEL_FIND_Ki = 20; //added 09/22/20
const int ToFArray_PARALLEL_FIND_Kd = 0; //added 09/22/20
//const float ToFArray_PARALLEL_FIND_SETPOINT = 0.05; //09/22/20 moved here
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 = 0; //06/28/20
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;
//const int ToFArray_OFFSET_TRACK_Kd = 50;
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.2; //added 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 = 5.0; //just a guess
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
int Lidar_RightFront;
int Lidar_RightCenter;
int Lidar_RightRear;
int Lidar_LeftFront;
int Lidar_LeftCenter;
int Lidar_LeftRear;

elapsedMillis lastToFArrayTelemetryMsec; //used to space out telemetry prints

enum VL53L0X_REQUEST
{
	VL53L0X_CENTERS_ONLY,
	VL53L0X_RIGHT,
	VL53L0X_LEFT,
	VL53L0X_BOTH //added 08/06/20
};
#pragma endregion VL53L0X ToF LIDAR Support


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

#pragma region MOTOR_PINS
//09/11/20 Now using two Adafruit DRV8871 drivers for all 4 motors
const int In1_Left = 10;
const int In2_Left = 11;

const int In1_Right = 8;
const int In2_Right = 9;

#pragma endregion Motor Pin Assignments

//Laser pointer
const int RED_LASER_DIODE_PIN = 7;

//LIDAR MODE pin (continuous mode)
const int LIDAR_MODE_PIN = 4; //mvd here 01/10/18

//BATT Monitor and IR Detector pin assignments added 01/30/17
const int BATT_MON_PIN = A1;

#pragma region CHG_SUPP_PINS

//04/22/20 bugfix - RUN & TOT input definitions were reversed
//const int RUN_CURR_PIN = A8; //02/25/19 bugfix 02/28/19 name chg
//const int TOT_CURR_PIN = A9; //02/24/19 now connected to 1NA619 charge current sensor
const int RUN_CURR_PIN = A9; //connected to 1NA619 between battery and rest of robot
const int TOT_CURR_PIN = A8; //connected to 1NA619 between charge plug and battery
const int CHG_CONNECT_PIN = 23;  //goes HIGH when chg cable connected

//LED en/dis output pins
//03/15/18 revised for TP5100 module
const int FIN_LED_PIN = 35;
const int _80PCT_LED_PIN = 33;
const int _60PCT_LED_PIN = 31;
const int _40PCT_LED_PIN = 29;
const int _20PCT_LED_PIN = 27;
const int CHG_LED_PIN = 25; //pwr2 signal line repl by Batt2
long chgStartMsec;//added 02/24/17
#pragma endregion Charge Control/Status Pins

//05/03/17 moved below Chg supp LED defs so can re-use them
//03/15/18 revised for TP5100 module
#pragma region BACKUP_TURN_LED_PINS
//03/19/18 new plan - use 'full/half/qtr_left, full/half/qtr_right aliases
const int FULL_LEFT_LED_PIN = FIN_LED_PIN;
const int HALF_LEFT_LED_PIN = _80PCT_LED_PIN;
const int QTR_LEFT_LED_PIN = _60PCT_LED_PIN;
const int QTR_RIGHT_LED_PIN = _40PCT_LED_PIN;
const int HALF_RIGHT_LED_PIN = _20PCT_LED_PIN;
const int FULL_RIGHT_LED_PIN = CHG_LED_PIN;
#pragma endregion Rear Bumper Display LEDs

#pragma region SPEAKER_CONSTANTS
//const int SOS_PWM_PIN = 3;
const int SOS_PWM_PIN = 2; //chg to proper pin 09/13/2020
const int DOT_MS = 200;
const int DASH_MS = 800;
const int HIGHTONE = 1000;
const int LOWTONE = 500;
#pragma endregion Speaker Constants

//const int PWRDWN_INTERRUPT_PIN = 2; //added 03/27/18

//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^//
//=================== END PIN ASSIGNMENTS ================//
//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^//

//03/08/17 added Mode/state-specific telemetry header strings
#pragma region TELEMETRYSTRINGS
//const String IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_In\tPID_Out\tLSpd\tRSpd";
const String IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD";
//const String WallFollowTelemStr = "DateTime\tBatt\tMode\tTrack\tLeft\tRight\tFront\tVar\tLSpd\tRSpd";
//const char* WallFollowTelemStr = "DateTime\tBatt\tMode\tTrack\tLeft\tRight\tFront\tVar\tLSpd\tRSpd";
const char* WallFollowTelemStr = "Msec\tMode\tTrack\tFront\tCtr\tRear\tSteer\tOutput\tLSpd\tRSpd"; //09/01/20 PID tuning
//const String WallFollowTelemStr = "NewDist\tOldDist\tBmean\tBvar\tOldImean\tOldIvar\tOldD^2\tNewD^2\tImean\tIvar"; //04/17/19 added for Ivar debug
//const String WallFollowTelemStr = "New_D\tOld_D\tB_mean\tB_var\told_Im\told_Ivar\told_d^2\tnew_d^2\tI_mean\tIm_sq\toldIm_sq\tI_var\tB_uSec\tI_uSec\n";
//const String ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tATotI\tARunI\tChrging\n"; //rev 02/28/19 to chg 'BattI' to 'TotI', added 'RunI', ChgI	
const String ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\n"; //rev 05/02/20
#pragma endregion Mode-specific telemetry header strings

#pragma region LOOP_VARS
//loop() variables
double deltaD = 0;
double olddist = 0;
double newdist = 0;
boolean bStuck = false;
int leftspeednum = MOTOR_SPEED_HALF;
int rightspeednum = MOTOR_SPEED_HALF;

elapsedMillis 	sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec

NavCases NavCase = NAV_WALLTRK;
WallTrackingCases TrackingCase = TRACKING_NEITHER; //added 01/05/16
WallTrackingCases PrevTrackingCase = TRACKING_LEFT;
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 last_incvar = 0;
double last_incmean = 0;
elapsedMillis lastHomingTelemetryMsec; //used to space out telemetry prints

#pragma endregion Loop Variables

#pragma region I2C_VARS
#pragma endregion I2C related parameters

#pragma region TRISENSOR PARAMS
	//FRAM constants/params
const int NUM_FRAM_BYTES_TO_CLEAR = 2000;
const int FIRST_TIME_STORAGE_ADDR = 2;//addr 0/1 reserved for nextFramWriteAddr
const int NEXTFRAMWRITEADDR_FRAMSTORAGELOCATION = 0;
const int NUM_TIMES_TO_DISPLAY = 10;
//const unsigned long FRAM_WRITE_INTERVAL_MSEC = 60000; //one minute
const unsigned long FRAM_WRITE_INTERVAL_MSEC = 6000; //one minute
int nextFramWriteAddr = FIRST_TIME_STORAGE_ADDR;
elapsedMillis sinceLastFRAMWriteMsec; //added 09/19/18
#pragma endregion TRISENSOR

#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()
const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param
bool bFirstTime = true;
#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19
#pragma endregion MPU6050 Support

#pragma region FRAM/RTC Support
//RTC/FRAM/MPU6050 status flags
bool RTC_Avail = true;
bool bRTCLostPower = false; //added 10/17/18
bool bFRAMReady = true;
#pragma endregion FRAM/RTC Support

#pragma region WALL_FOLLOW_SUPPORT
//12/05/19 defiine 'TURNDIR_CCW' & 'TURN_CW' identifiers for better readability. Now I can call
//RollingTurn(TURNDIR_CCW,...) and RollingTurn(TURNDIR_CW) instead of just RollingTurn(true/false)
const bool TURNDIR_CCW = true;
const bool TURNDIR_CW = false;

const int DESIRED_WALL_OFFSET_DIST_CM = 30;
const int ROLLING_TURN_RADIUS_CM = 5;
//const int NINETY_DEG_FUDGE_FACTOR_CM = 5; //added 04/17/20
const int NINETY_DEG_FUDGE_FACTOR_CM = 10; //added 04/17/20
const int WALL_APPR_ERR_WIN_MULTFACT = 2; //added 08/12/19
const int WALL_APPR_MAX_AGGREGATE_CUT = 30; //added 05/09/20
const int WALL_TRK_ERR_WIN_MULTFACT = 1; //added 08/12/19
const int VERY_FAR_AWAY_CM = 5;
const int FAR_AWAY_CM = 2;
const int CLOSE_CM = 1;
const int VERY_FAR_AWAY_TURN_DEG = 20;
const int FAR_AWAY_TURN_DEG = 10;
const int CLOSE_TURN_DEG = 5;
const int TURN_INCREMENT_DEG = 10;

int m_PrevHdgCutDeg = 0;
bool m_PrevHdgCutDir = TURNDIR_CW; //CW
int m_PrevLeftDistCm = 0;
int m_PrevRightDistCm = 0;
int m_AggregateCutDeg = 0; //added 05/09/20 
elapsedMillis 	sinceLastHdgCutUpdateMsec; //added 10/15/18 to replace lastmillisec
int m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //used to increase close in response
bool bIsFirst180 = true; //added 01/25/20 for boomerang mode tracking
elapsedMillis mSecSinceLastParDistChk = 0;  //added 02/14/20 to steepen distance/measurement slope

#pragma endregion Wall Following Support

#pragma endregion PRE_SETUP

#pragma region TIMER_ISR
//08/10/20 added timer ISR
volatile bool bIsStuck = false;
volatile bool bIsStuck_Slow = false; //08/12/20 added for half-speed offset capture operations
volatile bool bObstacleAhead = false;
volatile uint32_t Last_ISR_Msec;
volatile bool bTimeForNavUpdate = false;


//DEBUG!!
	//uint32_t elapsedUsec = micros()-Last_ISR_Msec;
	//mySerial.printf("MSec\tUSec\tFdist\tFvar\n");
	//mySerial.printf("%lu\t%lu\t%d\t%2.3f\n", millis(), elaspsed, frontdist, frontvar);
//DEBUG!!
//

ISR(TIMER5_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED)
{
	//digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);
	//delayMicroseconds(30);
	//digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);


	bTimeForNavUpdate = true;

#ifndef NO_STUCK
	uint16_t frontdist = GetFrontDistCm();
	frontvar = CalcDistArrayVariance(frontdist, aFrontDist);
	bIsStuck = frontvar < STUCK_FRONT_VARIANCE_THRESHOLD;
	bIsStuck_Slow = frontvar < STUCK_FRONT_VARIANCE_THRESHOLD / 2;
	bObstacleAhead = frontdist < MIN_OBS_DIST_CM;
#endif // !NO_STUCK
}
#pragma endregion TIMER_ISR


#pragma region WALL_OFFSET_TRACKING
const int WALL_OFFSET_TRACK_Kp = 100;
const int WALL_OFFSET_TRACK_Ki = 0;
const int WALL_OFFSET_TRACK_Kd = 0;
const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.3;
double WallTrackSteerVal, WallTrackOutput, WallTrackSetPoint;
PID WallTrackPID(&WallTrackSteerVal, &WallTrackOutput, &WallTrackSetPoint, WALL_OFFSET_TRACK_Kp, 0.0, 0.0, REVERSE);
#pragma endregion Wall Offset Tracking Support


#pragma region SETUP
void setup()
{

	Serial.begin(115200);
	delay(1000);


	FramPacket = CFRAMStatePacket(&fram); //09/27/18 comes after Serial.begin().  Calls Wire.begin() & fram.begin() 

#pragma region RTC
	if (rtc.begin()) //02/19/19 this now returns FALSE if RTC doesn't respond
	{
		Serial.println("Found RTC...");
		delay(100);
		bRTCLostPower = rtc.lostPower(); //added 10/17/18
		mySerial.printf("rtc.lostPower() reports %d\n", bRTCLostPower);

		if (rtc.lostPower())
		{
			Serial.println("RTC lost power.  Setting RTC to last compile time");
			rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));// sets RTC to last compile date/time
		}

#ifdef FORCE_RTC_TO_LAST_COMPILE_TIME
		Serial.println("Forcing RTC to last compile time");
		rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));// sets RTC to last compile date/time
#endif 

		DateTime now = rtc.now();
		char buffer[100];
		memset(buffer, '\0', 100);
		mySerial.printf("Retrieving Date/Time from RTC with rtc.now() =  %ld\n", now.unixtime());
		GetDayDateTimeStringFromDateTime(now, buffer);
		Serial.print("Date/Time: "); Serial.println(buffer);
		RTC_Avail = true;
	}
	else
	{
		Serial.println("Couldn't find RTC. Real-time clock functions won't be available");
		RTC_Avail = false; //use this instead
	}

	if (RTC_Avail && rtc.lostPower())
	{
		DateTime Comp_dt = DateTime(F(__DATE__), F(__TIME__)); //__DATE__ & __TIME__ are environment variables)
		uint8_t mydayofweek = Comp_dt.dayOfTheWeek();  //returns 0 for Sunday, 6 for Saturday dayOfTheWeek
		int mymonth = Comp_dt.month();
		int myday = Comp_dt.day();
		int myyear = Comp_dt.year();
		int myhour = Comp_dt.hour();
		int mymin = Comp_dt.minute();
		int mysec = Comp_dt.second();
		long unixtime = Comp_dt.unixtime();
		char* dayofweek = (char*)daysOfTheWeek[mydayofweek];

		Serial.print("day of week value = "); Serial.println(mydayofweek);
		Serial.print("day of week value = "); Serial.println(Comp_dt.dayOfTheWeek());


		//mySerial.printf("RTC lost power - setting to datetime of last compile %ld (%s %4d/%02d/%02d at %02d:%02d:%02d)\n",
		//	dt.unixtime(), daysOfTheWeek[dt.dayOfTheWeek()], dt.year(), dt.month(), dt.day(),
		//	dt.hour(), dt.minute(), dt.second());
		mySerial.printf("RTC lost power - setting to datetime of last compile %ld (%s %4d/%02d/%02d at %02d:%02d:%02d)\n",
			unixtime, dayofweek, myyear, mymonth, myday, myhour, mymin, mysec);

		rtc.adjust(Comp_dt);
	}
#pragma endregion RTC

#pragma region FRAM
	if (fram.begin())// you can stick a non-default i2c addr in here, e.g. begin(0x51);
	{
		Serial.println("Found I2C FRAM");
		bFRAMReady = true;
	}
	else
	{
		Serial.println("I2C FRAM not identified ... check your connections?\r\n");
		Serial.println("Will continue in case this processor doesn't support repeated start\r\n");
		bFRAMReady = false;
	}

	// Read the stored nextFramWriteAddr value from the first two bytes, and the last stored packet
	if (bFRAMReady)
	{
		fram.FRAM_I2C_readAnything(NEXTFRAMWRITEADDR_FRAMSTORAGELOCATION, nextFramWriteAddr);
		mySerial.printf("Read %d (nextFramWriteAddr) from FRAM address %d\n",
			nextFramWriteAddr, NEXTFRAMWRITEADDR_FRAMSTORAGELOCATION);

		//if any packets have been written since last FRAM clear, display last-written one
		if (nextFramWriteAddr > FIRST_TIME_STORAGE_ADDR)
		{
			int FramPacketSize = CFRAMStatePacket::packet_size;
			mySerial.printf("printing packet written to FRAM at %d\n", nextFramWriteAddr - FramPacketSize);
			FramPacket.Read(nextFramWriteAddr - FramPacketSize);
			FramPacket.Print();
		}
	}
#pragma endregion FRAM

#pragma region MPU6050
	Serial.println(F("Initializing MPU6050 ..."));
	mpu.initialize();

	// verify connection
	Serial.println(F("Testing device connections..."));
	Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

	float StartSec = 0; //used to time MPU6050 init
	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// make sure it worked (returns 0 if successful)
	if (devStatus == 0)
	{
		//12/06/19 don't need to do this every time. Above cal constants should work.
		//// Calibration Time: generate offsets and calibrate our MPU6050
		//mpu.CalibrateAccel(6);
		//mpu.CalibrateGyro(6);
		//Serial.println();
		//mpu.PrintActiveOffsets();

		// turn on the DMP, now that it's ready
		Serial.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

		// set our DMP Ready flag so the main loop() function knows it's okay to use it
		Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
		bMPU6050Ready = true;
		StartSec = millis() / 1000.f;
		mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", StartSec);
	}
	else
	{
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		Serial.print(F("DMP Initialization failed (code "));
		Serial.print(devStatus);
		Serial.println(F(")"));

		bMPU6050Ready = false;
	}
#pragma endregion MPU6050


	//DEBUG!!
			//print out Homing PID parameters
	mySerial.printf("IRHomingPID Parameters (Kp,Ki,Kd,DIR) = (%2.2f,%2.2f,%2.2f,%d)\n",
		IRHomingPID.GetKp(), IRHomingPID.GetKi(), IRHomingPID.GetKd(), IRHomingPID.GetDirection());
	//DEBUG!!

#pragma region L/R/FRONT DISTANCE ARRAYS
	InitFrontDistArray(); //08/12/20 code extracted to fcn so can call elsewhere

	//04/28/19 put aLeftDist[], aRightDist[] arrays back in
	for (int i = 0; i < LR_PING_DIST_ARRAY_SIZE; i++)
	{
		aLeftDist[i] = random(0, 200);
		aRightDist[i] = random(0, 200);

		////DEBUG!!
		//		mySerial.printf("aLeft/RightDist[%d] = %d, %d\n",i, aLeftDist[i], aRightDist[i]);
		////DEBUG!!
	}
#pragma endregion L/R/FRONT DISTANCE ARRAYS

#pragma region I/O PIN SETUP
	//09/11/20 now using two DRV8871 motor drivers for all four motors
	pinMode(In1_Left, OUTPUT);
	pinMode(In2_Left, OUTPUT);
	pinMode(In1_Right, OUTPUT);
	pinMode(In2_Right, OUTPUT);

	//Laser pointer
	pinMode(RED_LASER_DIODE_PIN, OUTPUT);

	//01/30/17 added for chg stn supp
	pinMode(BATT_MON_PIN, INPUT);//Battery voltage monitor input
	analogReference(EXTERNAL); //03/18/18 now using external 3.3V ref with 5.84V zener voltage shifter 

	//02/28/19 revised to repurpose CHG_SIG_PIN AND CHG_FIN_PIN for current measurement
	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

	pinMode(CHG_CONNECT_PIN, INPUT_PULLUP);  //goes HIGH when chg cable connected

	//Charge status LED en/dis pins
	pinMode(FIN_LED_PIN, OUTPUT);
	pinMode(_80PCT_LED_PIN, OUTPUT);
	pinMode(_60PCT_LED_PIN, OUTPUT);
	pinMode(_40PCT_LED_PIN, OUTPUT);
	pinMode(_20PCT_LED_PIN, OUTPUT);
	pinMode(CHG_LED_PIN, OUTPUT);

	//added 03/27/18
	//pinMode(SOS_PWM_PIN, OUTPUT);

	//LIDAR mode pin
	pinMode(LIDAR_MODE_PIN, INPUT); // Set LIDAR input monitor pin

#pragma endregion I/O PIN SETUP


	//11/14/18 added this section for distance printout only
	//08/06/20 revised for VL53L0X support
#ifdef DISTANCES_ONLY
	sinceLastNavUpdateMsec = 0;
	digitalWrite(RED_LASER_DIODE_PIN, HIGH); //enable the front laser dot
	mySerial.printf("\n------------ DISTANCES ONLY MODE!!! -----------------\n\n");

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

			//09/20/20 re-display the column headers
			//if (i % 20 == 0)
			//{
			//}

			GetRequestedVL53l0xValues(VL53L0X_BOTH);

			//mySerial.printf("Both: %lu\t%d\t%d\t%d\t%d\t%d\t%d\n",
			//	millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
			//	Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);
			mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n",
				millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
				Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);
			//i++;

		//}
	}
#else
	prevleftdistval = GetAvgLeftDistCm();
	prevrightdistval = GetAvgRightDistCm();
	prevfrontdistcm = GetFrontDistCm();
#endif // DISTANCES_ONLY

#pragma region TIMER_INTERRUPT
	//set timer5 interrupt at (1000/MIN_PING_INTERVAL_MSEC)Hz 5Hz a/o 08/10/20
	//09/18/20 can't use Timer1 cuz doing so conflicts with PWM on pins 10-12
	cli();//stop interrupts
	TCCR5A = 0;// set entire TCCR5A register to 0
	TCCR5B = 0;// same for TCCR5B
	TCNT5 = 0;//initialize counter value to 0

	// set compare match register for 5hz increments
	OCR5A = 3124;// = (16*10^6) / (5*1024) - 1 (must be <65536)
	TCCR5B |= (1 << WGM52);// turn on CTC mode
	TCCR5B |= (1 << CS52) | (1 << CS50);// Set CS10 and CS12 bits for 1024 prescaler
	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt
	sei();//allow interrupts
#pragma endregion TIMER_INTERRUPT

	pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);

#pragma region POST

	//Check battery voltage
	mySerial.printf("Checking Battery Voltage...\n");
	mySerial.printf("Battery voltage = %2.3f\n", GetBattVoltage());


#ifndef NO_POST
	//Power On Self-Test (POST)

	//Check speaker
	mySerial.printf("Checking Speaker...\n");
	tone(SOS_PWM_PIN, HIGHTONE, DOT_MS); //returns immediately
	delay(DOT_MS);              //delay for LED viewing
	tone(SOS_PWM_PIN, LOWTONE, DASH_MS); //returns immediately
	delay(DOT_MS);              //delay for LED viewing
	tone(SOS_PWM_PIN, HIGHTONE, DOT_MS); //returns immediately
	delay(DOT_MS);              //delay for LED viewing
	tone(SOS_PWM_PIN, LOWTONE, DASH_MS); //returns immediately

	//Turn on LASER
	mySerial.printf("Checking Laser pointer\n");
	digitalWrite(RED_LASER_DIODE_PIN, HIGH);
	delay(1000);
	digitalWrite(RED_LASER_DIODE_PIN, LOW);
	delay(1000);
	digitalWrite(RED_LASER_DIODE_PIN, HIGH);
	delay(1000);
	digitalWrite(RED_LASER_DIODE_PIN, LOW);

	//check for reasonable LIDAR value
#ifndef NO_LIDAR
	mySerial.printf("LIDAR reports %d cm\n", GetFrontDistCm());
#else
	mySerial.printf("LIDAR Disabled\n");
#endif // !NO_LIDAR
	//
	//	mySerial.printf("Checking Left & Right Distance Sensors...\n");
	//#ifndef NO_PINGS
	//	mySerial.printf("Left\tRight\n");
	//	for (size_t i = 0; i < 10; i++)
	//	{
	//		mySerial.printf("%d\t%d\n", GetLeftDistCm(), GetRightDistCm());
	//	}
	//
	//#else
	//	mySerial.printf("Distance Sensors Disabled\n");
	//#endif // !NO_LIDAR
	//

		//set all chg status LEDs OFF (HIGH)
	digitalWrite(FIN_LED_PIN, HIGH);
	digitalWrite(_80PCT_LED_PIN, HIGH);
	digitalWrite(_60PCT_LED_PIN, HIGH);
	digitalWrite(_40PCT_LED_PIN, HIGH);
	digitalWrite(_20PCT_LED_PIN, HIGH);
	digitalWrite(CHG_LED_PIN, HIGH);

	delay(1000);

	//set all chg status LEDs ON (LOW)
	digitalWrite(_40PCT_LED_PIN, LOW);
	digitalWrite(_20PCT_LED_PIN, LOW);
	digitalWrite(CHG_LED_PIN, LOW);
	digitalWrite(_60PCT_LED_PIN, LOW);
	digitalWrite(_80PCT_LED_PIN, LOW);
	digitalWrite(FIN_LED_PIN, LOW);

	//Disable LEDs in sequence from center out
	delay(500);
	digitalWrite(_40PCT_LED_PIN, HIGH);
	digitalWrite(_60PCT_LED_PIN, HIGH);
	delay(100);
	digitalWrite(_20PCT_LED_PIN, HIGH);
	digitalWrite(_80PCT_LED_PIN, HIGH);
	delay(100);
	digitalWrite(CHG_LED_PIN, HIGH);
	digitalWrite(FIN_LED_PIN, HIGH);

	delay(500);

	//Enable LEDs in sequence from outside in
	digitalWrite(CHG_LED_PIN, LOW);
	digitalWrite(FIN_LED_PIN, LOW);
	delay(100);
	digitalWrite(_20PCT_LED_PIN, LOW);
	digitalWrite(_80PCT_LED_PIN, LOW);
	delay(100);
	digitalWrite(_40PCT_LED_PIN, LOW);
	digitalWrite(_60PCT_LED_PIN, LOW);

	delay(500);

	//Disable LEDs in sequence from center out
	digitalWrite(_40PCT_LED_PIN, HIGH);
	digitalWrite(_60PCT_LED_PIN, HIGH);
	delay(100);
	digitalWrite(_20PCT_LED_PIN, HIGH);
	digitalWrite(_80PCT_LED_PIN, HIGH);
	delay(100);
	digitalWrite(CHG_LED_PIN, HIGH);
	digitalWrite(FIN_LED_PIN, HIGH);

	//10/08/17 make sure motors control lines are all in STOP state
	StopLeftMotors();
	StopRightMotors();

	//run through motor test sequence
	//10/06/17 added check for charger connection
	//if (!IsChargerConnected())
	//{
	//	//fwd 1 sec
	//	SetLeftMotorDir(FWD_DIR);
	//	SetRightMotorDir(FWD_DIR);
	//	RunBothMotorsMsec(1000, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //run for 1 sec

	//	//reverse 1 sec
	//	SetLeftMotorDir(REV_DIR);
	//	SetRightMotorDir(REV_DIR);
	//	RunBothMotorsMsec(1000, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //run for 1 sec

	//	StopBothMotors();
	//}

	//08/09/20 modified to use DRV8871 motor driver
	if (!IsChargerConnected())
	{
		//fwd 1 sec
		RunBothMotorsMsec(true, 1000, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //run for 1 sec

		//reverse 1 sec
		RunBothMotorsMsec(false, 1000, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //run for 1 sec

		StopBothMotors();
	}

#else
	mySerial.printf("\n***********  POST checks Skipped!!********************\n\n");
#endif // !NO_POST
#pragma endregion POST

	//09/20/20 have to do this for parallel finding to go the right way
	mySerial.printf("Initializing Left & Right Distance Arrays...\n");
#ifndef NO_PINGS
	mySerial.printf("Left\tRight\n");
	for (size_t i = 0; i < LR_PING_DIST_ARRAY_SIZE; i++)
	{
		int leftdist = GetLeftDistCm();
		int rightdist = GetRightDistCm();
		mySerial.printf("%d\t%d\n", leftdist, rightdist);
		UpdateLRDistanceArrays(leftdist, rightdist);
	}

#else
	mySerial.printf("Distance Sensors Disabled\n");
#endif // !NO_LIDAR

	////01/26/15 start the robot going straight
	MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

	//04/13/20 initialize just before loop()
	leftdistval = GetLeftDistCm();
	rightdistval = GetRightDistCm();
	frontdistval = GetFrontDistCm();

	sinceLastNavUpdateMsec = 0; //added 10/15/18
	sinceLastFRAMWriteMsec = 0; //added 09/19/18
	mySerial.printf("sinceLastNavUpdateMsec in setup() = %lu\n", (uint32_t)sinceLastNavUpdateMsec);
#pragma endregion Setup

	//09/24/20 set WallOffsetTrackingPID parameters here
	WallTrackPID.SetTunings(WALL_OFFSET_TRACK_Kp, WALL_OFFSET_TRACK_Ki, WALL_OFFSET_TRACK_Kd); //try more aggresive than capture, but less than parallel rotate
	WallTrackPID.SetOutputLimits(-MOTOR_SPEED_QTR, MOTOR_SPEED_QTR); 
	WallTrackSetPoint = DESIRED_WALL_OFFSET_DIST_CM * 10; //now track the desired offset distance, in mm
	mySerial.printf("WallTrackPID Parameters (Kp,Ki,Kd) = (%2.f,%2.f,%2.f)\n",
		WallTrackPID.GetKp(), WallTrackPID.GetKi(), WallTrackPID.GetKd());
	WallTrackPID.SetMode(AUTOMATIC);

	mySerial.printf("mSec\tFront\tCenter\tRear\tSteer\tSetPt\tOut\tleftSpd\tRightSpd\n");
	MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);

	CaptureWallOffset(TRACKING_LEFT, DESIRED_WALL_OFFSET_DIST_CM + ROLLING_TURN_RADIUS_CM);

	//delay for 2 sec, but check for user input during delay.
	unsigned long now = millis();
	while (millis()-now < 2000)
	{
		CheckForUserInput();
	}

	TrackLeftWall(DESIRED_WALL_OFFSET_DIST_CM); //infinite loop

} //setup


int ComputeCount = 0;
const int ComputeSkipsPerHeaderPrint = 20;


void loop()
{
	CheckForUserInput();


	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	WallTrackSteerVal = LeftSteeringVal;

	//09/25/20 need a positive offset when ctr dist < desired.  4cm diff ->> 0.4 ->> approx 20 deg
	WallTrackSetPoint = (double)(10*DESIRED_WALL_OFFSET_DIST_CM - Lidar_LeftCenter) / 100.f; 

	//update motor speeds, skipping bad values
	if (!isnan(WallTrackSteerVal))
	{
		//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)
		if (WallTrackPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value
		{
			digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);
			delayMicroseconds(10);
			digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);

			leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;
			rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

			mySerial.printf("%lu\t%d\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%d\t%d\n", millis(),
				Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, WallTrackSteerVal,
				WallTrackSetPoint, WallTrackOutput, leftspeednum, rightspeednum);

			MoveAhead(leftspeednum, rightspeednum);

			ComputeCount++;
			if (ComputeCount > ComputeSkipsPerHeaderPrint)
			{
				mySerial.printf("mSec\tFront\tCenter\tRear\tSteer\tSetPt\tOut\tleftSpd\tRightSpd\n");
				ComputeCount = 0;
			}
		}
	}


}

//06/18/20 Rewritten to utilize VL53L0X ToF sensor array
void TrackLeftWall(int tgt_offset)
{
	WallTrackPID.SetTunings(WALL_OFFSET_TRACK_Kp, WALL_OFFSET_TRACK_Ki, WALL_OFFSET_TRACK_Kd); //try more aggresive than capture, but less than parallel rotate
	WallTrackPID.SetOutputLimits(-MOTOR_SPEED_QTR, MOTOR_SPEED_QTR); //give PID full rein on motor speeds
	mySerial.printf("WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (%2.f,%2.f,%2.f)\n",
		WallTrackPID.GetKp(), WallTrackPID.GetKi(), WallTrackPID.GetKd());
		
	while (true)
	{
		GetRequestedVL53l0xValues(VL53L0X_LEFT);
		WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5

		//at 20mm from tgt offset, setpoint will be +/-0.2
		WallTrackSetPoint = (float)(10 * tgt_offset - Lidar_LeftCenter) / 100.f; //10/04/20 positive value drives robot toward wall
		if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;
		if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

		//update motor speeds, skipping bad values
		if (!isnan(WallTrackSteerVal))
		{
			//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)
			if (WallTrackPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value
			{
				leftspeednum = MOTOR_SPEED_QTR - WallTrackOutput;
				rightspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

				MoveAhead(leftspeednum, rightspeednum);

				mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\n", millis(),
					Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, WallTrackSteerVal, WallTrackOutput,
					WallTrackSetPoint, leftspeednum, rightspeednum);
			}
		}

		//01/30/17 added for manual remote control via Wixel
		CheckForUserInput();
	}
}


#pragma region TIME/DATE FUNCTIONS
//02/18/19 copied from RTC Test project, and used to remove dependence on Time/TimeLib libraries
void GetDayDateTimeStringFromDateTime(DateTime dt, char* bufptr)
{
	int mydayofweek = dt.dayOfTheWeek();
	int myday = dt.day();
	int mymonth = dt.month();
	int myyear = dt.year();
	int myhour = dt.hour();
	int mymin = dt.minute();
	int mysec = dt.second();
	char* dayofweek = (char*)daysOfTheWeek[mydayofweek];

	//now concatenate everything into the provided buffer
	sprintf(bufptr, "%s %4d/%02d/%02d at %02d:%02d:%02d", dayofweek, mymonth, myday, myyear, myhour, mymin, mysec);
}

#pragma endregion Time & Date Support Functions

#pragma region DISTANCE_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!!
		//mySerial.printf("i = %d, newval = %d, aFrontdist[%d] = %d\n", i, newval, i, aFrontDist[i]);
		//DEBUG!!
		if (bgoingUp)
		{

			if (newval < MAX_FRONT_DISTANCE_CM - bumpval) //don't want newval > MAX_FRONT_DISTANCE_CM
			{
				newval += bumpval;
			}
			else
			{
				bgoingUp = false;
			}
		}
		else
		{
			if (newval > bumpval) //don't want newval < 0
			{
				newval -= bumpval;
			}
			else
			{
				bgoingUp = true;
			}
		}
	}

	//04/19/19 init last_incmean  & last_incvar 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
	}
	last_incmean = (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] - last_incmean) * (aFrontDist[i] - last_incmean);
	}

	last_incvar = sumsquares / FRONT_DIST_ARRAY_SIZE;
	mySerial.printf("aFrontDist Init: last_incmean = %3.2f, last_incvar = %3.2f\n", last_incmean, last_incvar);
}

float GetAvgFrontDistCm()
{
	int dist = 0;
	for (size_t i = 0; i < 3; i++)
	{
		dist += GetFrontDistCm();
		delay(50);
		//delay(10);
	}
	return dist / 3;
}

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

//DEBUG!!
	//int totdist = 0;
	//for (size_t i = 0; i < LR_PING_AVG_WINDOW_SIZE; i++)
	//{
	//	totdist += aRightDist[i];
	//	//mySerial.printf("dist/total = %d\t%d\n", aRightDist[i], totdist);
	//}
	//float avg = (float)totdist / (float)LR_PING_AVG_WINDOW_SIZE;
	//return avg;
//DEBUG!!

	int rightavgdist_cm = 0;
	for (int validx = 0; validx < LR_PING_AVG_WINDOW_SIZE; validx++)
	{
		rightavgdist_cm += aRightDist[LR_PING_DIST_ARRAY_SIZE - 1 - validx];
	}

	float avg = (float)rightavgdist_cm / (float)LR_PING_AVG_WINDOW_SIZE;
	return avg;
}

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

	//int totdist = 0;
	//for (size_t i = 0; i < LR_PING_AVG_WINDOW_SIZE; i++)
	//{
	//	totdist += aLeftDist[i];
	//}
	//float avg = (float)totdist / (float)LR_PING_AVG_WINDOW_SIZE;

	int leftavgdist_cm = 0;
	for (int validx = 0; validx < LR_PING_AVG_WINDOW_SIZE; validx++)
	{
		leftavgdist_cm += aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1 - validx];
	}

	float avg = (float)leftavgdist_cm / (float)LR_PING_AVG_WINDOW_SIZE;
	return avg;
}

//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
	//mySerial.printf("Front Dist = %d at %lu mSec\n", LIDARdistCm, millis());

	//chk for erroneous reading
	if (LIDARdistCm == 0)
	{
		mySerial.printf("%lu: Error in GetFrontDistCm()\n", millis());

		//replace with average of last three readings from aFrontDist
		int avgdist = 0;
		for (int i = 0; i < FRONT_DIST_AVG_WINDOW_SIZE; i++)
		{
			avgdist += aFrontDist[FRONT_DIST_ARRAY_SIZE - 1 - i];
		}
		avgdist = avgdist / FRONT_DIST_AVG_WINDOW_SIZE;
		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
}

//08/09/20 added no_param version f/u/by blocking functions like IRHomeToChgStn() and RotateToParallelOrientation()
double CalcDistArrayVariance()
{
	frontdistval = GetFrontDistCm();
	return CalcDistArrayVariance(frontdistval, aFrontDist);
}

double  CalcDistArrayVariance(unsigned long newdistval, uint16_t* aDistArray)
{
	//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 = last_incmean - (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 = last_incmean * last_incmean;
	double inc_mean_squared = inc_mean * inc_mean;

	double inc_var = last_incvar + ((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:
	//mySerial.printf("%lu\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\n", millis(),
	//	newdistval, oldestDistVal, last_incmean, last_incvar, inc_var);
//DEBUG!!

	last_incvar = inc_var; //save for next time
	last_incmean = 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_PING_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_PING_DIST_ARRAY_SIZE - 1].
	//Notes:

//DEBUG!!
	mySerial.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_PING_DIST_ARRAY_SIZE - 1; i++)
		//for (int i = 0; i < DIST_ARRAY_SIZE; i++)
	{
		aRightDist[i] = aRightDist[i + 1];
		aLeftDist[i] = aLeftDist[i + 1];
	}

	//Step 2: Place the latest reading at [DIST_ARRAY_SIZE - 1].
	aRightDist[LR_PING_DIST_ARRAY_SIZE - 1] = rightdistval;
	aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1] = leftdistval;
}

#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

	//mySerial.printf("InMoveAhead(%d,%d)\n", leftspeednum, rightspeednum);

	//Step 1: Set forward direction and speed for both wheels
	SetLeftMotorDirAndSpeed(true, leftspeednum);
	SetRightMotorDirAndSpeed(true, rightspeednum);
}

//09/08/10 modified for DRV8871 motor driver
void StopLeftMotors()
{
	analogWrite(In1_Left, MOTOR_SPEED_OFF);
	analogWrite(In2_Left, MOTOR_SPEED_OFF);
}

void StopRightMotors()
{
	analogWrite(In1_Right, MOTOR_SPEED_OFF);
	analogWrite(In2_Right, MOTOR_SPEED_OFF);
}

//09/08/20 added bool bisFwd param for DRV8871 motor driver
void RunBothMotors(bool bisFwd, int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors at left/rightspeednum speeds
	//Inputs:
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//    Step 1: Apply drive to both wheels
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//    01/24/15 - revised for continuous run - no timing
	//  01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()
	//	12/07/15 - chg args from &int to int
	//Step 1: Apply drive to both wheels

////DEBUG!!
//	mySerial.printf("In RunBothMotors(%d,%d)\n", leftspeednum, rightspeednum);
////DEBUG!!

	SetLeftMotorDirAndSpeed(bisFwd, leftspeednum);
	SetRightMotorDirAndSpeed(bisFwd, rightspeednum);
}

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

//11/25/15 added for symmetry ;-).
void StopBothMotors()
{
	StopLeftMotors();
	StopRightMotors();
}

//01/10/18 reverted to regular ping().  median distance function takes too long
int GetLeftDistCm()
{
	//Serial.print("LeftPing\t"); Serial.println(millis());
	//Notes:
	//	04/30/17 added limit detection/correction
	//	07/20/20 rewritten for VL53L0X vs 'ping' sensors

	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	int distCm = round((float)Lidar_LeftCenter / 10.f); //try to make this accurate

	return distCm;
}

//06/17/20 rewritten for VL53L0X sensor
int GetRightDistCm()
{
	//Purpose:  Get right center VL53L0X distance in Cm
	//Notes:
	//	06/17/20: Copied from 'ping' version and adapted for VL53L0X

//DEBUG!!
	//unsigned long now = millis();
//DEBUG!!

	GetRequestedVL53l0xValues(VL53L0X_RIGHT);
	int distCm = round((float)Lidar_RightCenter / 10.f); //try to make this accurate

//DEBUG!!
	//mySerial.printf("GetRightDistCm() returned %d in %lu Msec\n", distCm, millis() - now);
//DEBUG!!

	return distCm;
}

void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)
{
	//mySerial.printf("SetLeftMotorDirAndSpeed(%d,%d)\n", bIsFwd, speed);

#ifndef NO_MOTORS


	if (bIsFwd)
	{
		digitalWrite(In1_Left, LOW);
		analogWrite(In2_Left, speed);
		//mySerial.printf("In SetLeftMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
	}
	else
	{
		digitalWrite(In2_Left, LOW);
		analogWrite(In1_Left, speed);
	}
#endif // !NO_MOTORS
}
void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)
{
	//mySerial.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",
	//	(bIsFwd == true) ? "true" : "false", speed);

#ifndef NO_MOTORS

	if (bIsFwd)
	{
		digitalWrite(In1_Right, LOW);
		analogWrite(In2_Right, speed);
	}
	else
	{
		digitalWrite(In2_Right, LOW);
		analogWrite(In1_Right, speed);
	}

#endif // !NO_MOTORS
}


#pragma endregion Motor Support Functions

#pragma region CHARGE SUPPORT

bool IsIRBeamAvail()
{
	//Purpose: Determine whether or not an IR beam is available for homing
	//Inputs: call from loop()
	//Outputs: true if an IR beam is detected, false otherwise
	//Plan:
	//	Step1: Get analog levels from all 4 IR detectors
	//	Step2: return true if any of the 4 show detection level
	//Notes:
	//	Might need some hysteresis here to avoid toggling in and out of the TRACKING_IRBEAM mode
	//	02/21/17 read each det 3 times. If any sum is less than 3 X threshold, return true.  Otherwise return false
	//	10/15/17 rewritten to use info from IR Demod Module
	//	04/05/20 revised to incorporate changes from 'I2C_Master_Tut5.ino'

	//get latest info from IR Demod Module
	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);

	float total = Fin1 + Fin2;
	////DEBUG!!
	//	mySerial.printf("Fin1 = %ld, Fin2 = %ld, SteeringValue = %3.2f\n", Fin1, Fin2, SteeringValue);
	////DEBUG!!

	return (total > IR_BEAM_DETECTION_THRESHOLD);
}

float GetTotalAmps()
{
	//Purpose:  Get total current in amps
	//Inputs: 
	//	Voltage on TOT_CURR_PIN is approximately Ichg*2 Amps
	//	VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio
	//Outputs:
	//	returns total robot current (chg current plus running current)
	//Notes:
	//	02/28/18 chg name from GetBattChgAmps() to GetTotalAmps()

	int ITotAnalogReading = GetAverageAnalogReading(TOT_CURR_PIN, CURRENT_AVERAGE_NUMBER); //range is 0-1023
	float ITotVolts = ((float)ITotAnalogReading / (float)MAX_AD_VALUE) * ADC_REF_VOLTS;
	float ITotAmps = ITotVolts * VOLTAGE_TO_CURRENT_RATIO;

	////DEBUG!!
	//mySerial.printf("GetTotalAmps(): Areading, ITotVolts, ITotAmps = %d, %3.2f, %3.2f\n", 
	//	ITotAnalogReading, ITotVolts, ITotAmps);
	////DEBUG!!

	return ITotAmps;
}

//added 02/28/19 to service 2nd 1Na169 current sensor
float GetRunningAmps()
{
	//Purpose:  Get robot running current in amps
	//Inputs: 
	//	Voltage on RUN_CURR_PIN is approximately IRun Amps
	//	VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio for running current
	//Outputs:
	//	returns robot running current 
	//Notes:
	//	02/28/18 copied from GetTotalAmps() and adapted

	//int IRunAnalogReading = analogRead(RUN_CURR_PIN); //range is 0-1023
	int IRunAnalogReading = GetAverageAnalogReading(RUN_CURR_PIN, CURRENT_AVERAGE_NUMBER); //range is 0-1023
	float IRunVolts = ((float)IRunAnalogReading / (float)MAX_AD_VALUE) * ADC_REF_VOLTS;

	float IRunAmps = IRunVolts * VOLTAGE_TO_CURRENT_RATIO;
	////DEBUG!!
	//	mySerial.printf("GetRunningAmps(): Areading, IRunvolts, IRunAmps = %d, %3.2f, %3.2f\n", 
	//		IRunAnalogReading, IRunVolts,IRunAmps);
	////DEBUG!!
	return IRunAmps;
}

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 = GetTotalAmps();
	float RunI = GetRunningAmps();

	////DEBUG!!
	//	mySerial.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 bConnect = digitalRead(CHG_CONNECT_PIN);  //goes HIGH when chg cable connected

////DEBUG!!
//	mySerial.printf("IsChargerConnected() returns %d\n", bConnect);
////DEBUG!!

	return bConnect;
}

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

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

	bool bChgConn = digitalRead(CHG_CONNECT_PIN);  //goes HIGH when chg cable connected

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

	bool bStillCharging = true;

	//04/27/20 re-arranged for clarity
	while (ElapsedChgTimeSec < MINIMUM_CHARGE_TIME_SEC ||
		(bStillCharging
			&& ElapsedChgTimeSec < BATT_CHG_TIMEOUT_SEC
			&& bChgConn)
		)
	{
		delay(1000); //one-second loop
		ElapsedChgTimeSec++;
		bStillCharging = IsStillCharging(); //02/24/19 - now using 1NA169 current sensor
		bChgConn = digitalRead(CHG_CONNECT_PIN);  //goes HIGH when chg cable connected	

		//05/02/20 rev to only print out 10 times/min
		if (ElapsedChgTimeSec % 6 == 0)
		{
			ElapsedChgTimeMin = (float)ElapsedChgTimeSec / 60.f;
			float BattV = GetBattVoltage();
			float TotalI = GetTotalAmps(); //rev 02/28/19: chg name from 'Batt' to 'Total'
			float RunI = GetRunningAmps();
			//mySerial.printf("%d\t%2.4f\t%2.4f\t%2.4f\t%2.4f\n", 
			//	ElapsedChgTimeSec, BattV, TotalI, RunI, TotalI - RunI); //rev 02/24/19 for 1Na169 sensor
			mySerial.printf("%3.1f\t%2.4f\t%2.4f\t%2.4f\t%2.4f\n",
				ElapsedChgTimeMin, BattV, TotalI, RunI, TotalI - RunI); //rev 02/24/19 for 1Na169 sensor
			UpdateChgStatusLEDs(BattV, bStillCharging); //updates 'fuel guage' LEDs 04/22/20 added bStillCharging to sig
		}
	}

	//Step2: Check for end-of-charge or failure (don't know what this would be yet...)
		//if charging ran over time, something went wrong
		//10/16/17 revised for better telemetry
	//if (bChgConn == LOW) //charger unplugged
	if (!bChgConn) //charger unplugged
	{
		Serial.print("Charge connection dropped after "); Serial.print(ElapsedChgTimeSec / 60);
		Serial.println(" minutes");
		return false;
	}
	else if (ElapsedChgTimeSec < BATT_CHG_TIMEOUT_SEC)
	{
		Serial.print("Charging Completed Successfully in "); Serial.print(ElapsedChgTimeSec / 60);
		Serial.print(" minutes at "); Serial.println(millis());
		return true;
	}
	else
	{
		Serial.print("Charging timout value of "); Serial.print(BATT_CHG_TIMEOUT_SEC);
		Serial.print(" secs expired at "); Serial.println(millis());
		return false;
	}
}

float GetBattVoltage()
{
	//02/18/17 get corrected battery voltage.  Voltage reading is 1/3 actual Vbatt value
	int analog_batt_reading = GetAverageAnalogReading(BATT_MON_PIN, VOLTS_AVERAGE_NUMBER);//rev 03/01/19 to average readings
	float calc_volts = ZENER_VOLTAGE_OFFSET + ADC_REF_VOLTS * (double)analog_batt_reading / (double)MAX_AD_VALUE;

	////DEBUG!!
	//	mySerial.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();
	mySerial.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
	//09/08/20 modified for DRV8871 motor driver
	//SetLeftMotorDir(REV_DIR);
	//SetRightMotorDir(REV_DIR);
	//RunBothMotorsMsec(2000, MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);
	RunBothMotorsMsec(false, 2000, MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);
	//
	//Step4: Turn 90 away from near side wall
	RollingTurn(rightdist < leftdist, true, 90); //turn 90 deg away from nearest wall

////DEBUG!!
//	MoveAhead(0, 0);
//	delay(500);
//	cli();
//	sleep_enable();
//	sleep_cpu();
////DEBUG!!

	return true; //can't think of anything else at the moment
}

long GetBatRunDurationSec()
{
	return 1; //dummy for now
}

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: Initialize PID for homing
	//	Step2: 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

	String trackstr = "IR"; //used for telemetry printouts
	String str = ""; //telemetry string
	bool result = true; //added 01/16/19 to supress warning

	//elapsedMillis IsStuckCallElapsedMillis = 0; //added 08/09/20

//Step1: Initialize PID for homing
	//set the target value
	//IRHomingSetpoint = 0.05; //10/15/17 this seems to be the best value for now
	//IRHomingSetpoint = -0.05; //07/12/20 new wheels, new motors
	//IRHomingSetpoint = -0.15; //07/12/20 new wheels, new motors
	//IRHomingSetpoint = -0.25; //07/12/20 new wheels, new motors
	//IRHomingSetpoint = 0.25; //07/12/20 new wheels, new motors
	//IRHomingSetpoint = 0.20; //07/12/20 new wheels, new motors
	IRHomingSetpoint = 0.15; //07/12/20 new wheels, new motors

	//turn the PID on
	IRHomingPID.SetMode(AUTOMATIC);

	//set the limits
	IRHomingPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

	//11/02/18 added dist var to CalcDistArrayVariance sig
	//08/09/20 now using no_param version of IsStuck();
	int frontdist = GetFrontDistCm();

	int bChgConn = LOW; //for testing
	lastHomingTelemetryMsec = 0; //used to space out telemetry prints

//Step2: If front distance < avoidancedistCm, turn 90 deg away from near wall
//	   otherwise continue homing until connected or stuck
	mySerial.printf("front dist = %d, lastHomingTelemetryMsec = ", frontdist);
	Serial.println(lastHomingTelemetryMsec);
	Serial.println(IRHomingTelemStr); //header for chg telemetry data

	//08/10/20 now using ISR for bIsStuck state
	while (bChgConn == LOW && !bIsStuck && frontdist > 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();

		//10/06/17 get new homing data from Teensy 3.2 IR Beacon Homing Module at address 8
		//2nd param in Wire.requestFrom not used by slave - but its val % 32 must be in range of 1-32

		//float tempfloat = 0; //IRHomingLRSteeringVal is a double, so use a temp float and then convert on assignment
		long FinalValue1, FinalValue2;
		Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, sizeof(FinalValue1) + sizeof(FinalValue1) + sizeof(IRHomingLRSteeringVal));
		I2C_readAnything(FinalValue1);
		I2C_readAnything(FinalValue2);
		I2C_readAnything(IRHomingLRSteeringVal);

		//skip bad values
		if (!isnan(IRHomingLRSteeringVal))
		{
			//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)
			if (IRHomingPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value
			{
				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;
					mySerial.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(), FinalValue1, FinalValue2, IRHomingLRSteeringVal, IRHomingOutput,
						leftspeednum, rightspeednum, frontdist);
				}
				//DEBUG!!

				//for testing, use charging LEDs for visualization of IRHomingLRSteeringVal
				int FINout = (IRHomingLRSteeringVal > -1.0 && IRHomingLRSteeringVal < -0.5) ? LOW : HIGH;
				int _80PCTout = (IRHomingLRSteeringVal >= -0.5 && IRHomingLRSteeringVal < -0.25) ? LOW : HIGH;
				int _60PCTout = (IRHomingLRSteeringVal >= -0.25 && IRHomingLRSteeringVal < 0) ? LOW : HIGH;
				int _40PCTout = (IRHomingLRSteeringVal >= 0 && IRHomingLRSteeringVal <= 0.25) ? LOW : HIGH;
				int _20PCTout = (IRHomingLRSteeringVal > 0.25 && IRHomingLRSteeringVal < 0.5) ? LOW : HIGH;
				int CHGout = (IRHomingLRSteeringVal > 0.5 && IRHomingLRSteeringVal < 1.0) ? LOW : HIGH;

				digitalWrite(FIN_LED_PIN, FINout);
				digitalWrite(_20PCT_LED_PIN, _20PCTout);
				digitalWrite(_40PCT_LED_PIN, _40PCTout);
				digitalWrite(_60PCT_LED_PIN, _60PCTout);
				digitalWrite(_80PCT_LED_PIN, _80PCTout);
				digitalWrite(CHG_LED_PIN, CHGout);
				//mySerial.printf("FIN/20/40/60/80/CHG = %d/%d/%d/%d/%d/%d\n", FINout, _20PCTout, _40PCTout, _60PCTout, _80PCTout, CHGout);

				//0424/20 experiment with going to full speed when near charge plug
				if (frontdist <= CHG_STN_FINAL_APPR_DIST_CM)
				{
					leftspeednum = rightspeednum = MOTOR_SPEED_MAX;
					mySerial.printf("Accelerating to Contact with frontdist = %d\n", frontdist);
				}

				MoveAhead(leftspeednum, rightspeednum);
			}
		}

		//11/11/18 moved outside 'if (!isnan(IRHomingLRSteeringVal))' block so will always get executed
		frontdist = GetFrontDistCm(); //this is also a loop exit condition
		bChgConn = digitalRead(CHG_CONNECT_PIN); //goes HIGH when chg cable connected

		//05/02/20 turn off Laser
		digitalWrite(RED_LASER_DIODE_PIN, LOW);
	}//	while (bChgConn == LOW && !bIsStuck && frontdist > avoidancedistCm)

	//find out why loop exited.  Could be stuck, connected, or inside avoidance dist
	int leftdist = GetAvgLeftDistCm();
	int rightdist = GetAvgRightDistCm();
	if (bIsStuck || frontdist <= avoidancedistCm)
	{
		mySerial.printf("Abnormal exit from homing routine\n");
		mySerial.printf("%lu: front/left/rightdist/bIsStuck = %d/%d/%d/%s\n",
			millis(), frontdist, leftdist, rightdist, bIsStuck ? "TRUE" : "FALSE");

		InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

		BackupAndTurn90Deg((leftdist > rightdist), true, MOTOR_SPEED_HALF);
		PrevOpMode = MODE_NONE; //reset so tracking wall can be re-captured

		//return false;
		result = false; //added 01/16/19 to supress warning
	}
	else if (bChgConn == HIGH)
	{
		Serial.print("Charger Connected at "); Serial.println(millis());
		//return true;
		result = true; //added 01/16/19 to supress warning
	}

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

#pragma endregion Charge Support Functions

#pragma region WALL TRACKING SUPPORT
bool RotateToParallelOrientation(int trkcase)
{
	//Purpose:  Rotate robot to parallel near wall using VL53L0X array and PID engine
	//Inputs: 
	//	TrackingCase = int representing current WallTrackingCases enum value
	//Outputs:
	//	Robot orientation changed to parallel nearest wall
	//Plan:
	//Step1: Initialize PID engine
	//Step2: Drive setpoint to zero using motors
	//Notes:
	//	06/18/20 have only right side array at the moment
	//	08/06/20 added left side support
	//	08/10-12/20 added 'stuck' detection

	mySerial.printf("In RotateToParallelOrientation(%s)\n", TrkStrArray[trkcase]);

	//int initleftspeed = MOTOR_SPEED_HALF;
	//int initrightspeed = MOTOR_SPEED_HALF;

	//elapsedMillis IsStuckCallElapsedMillis = 0; //added 08/09/20


//Step1: Initialize PID engine
	ToFSetpoint = ToFArray_PARALLEL_FIND_SETPOINT;

	//turn the PID on
	ToFArrayPID.SetMode(AUTOMATIC);
	ToFArrayPID.SetTunings(ToFArray_PARALLEL_FIND_Kp, ToFArray_PARALLEL_FIND_Ki, ToFArray_PARALLEL_FIND_Kd);

	//set the limits
	//09/12/20 experiment
	ToFArrayPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
	//ToFArrayPID.SetOutputLimits(-MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

	//DEBUG!!
		//print out PID parameters
	mySerial.printf("ToFArrayPID Parameters (Kp,Ki,Kd,DIR) = (%2.2f,%2.2f,%2.2f,%d)\n",
		ToFArrayPID.GetKp(), ToFArrayPID.GetKi(), ToFArrayPID.GetKd(), ToFArrayPID.GetDirection());

	int outmax = ToFArrayPID.GetOutputMax();
	int outmin = ToFArrayPID.GetOutputMin();
	mySerial.printf("ToFArrayPID output max/min = %d/%d\n", outmax, outmin);
	//DEBUG!!

	//DEBUG!!
	mySerial.printf("Time\tFdist\tCdist\tRdist\tSteer\tPIDout\tLspd\tRSpd\n");
	//DEBUG!!

	//Step2: Drive setpoint to zero using motors
	lastToFArrayTelemetryMsec = 0; //init telemetry spacing tracker

	if (trkcase == TRACKING_RIGHT)
	{
		//this really is necessary - else while() won't have correct starting value
		GetRequestedVL53l0xValues(VL53L0X_RIGHT);
		delay(100);
		GetRequestedVL53l0xValues(VL53L0X_RIGHT);

		ToFSteeringVal = RightSteeringVal; //08/06/20 now have separate left/right steering vals

		//08/09/20 added 'stuck' and 'upcoming obstacle' guards, but IsStuck() call must be limited to 5/sec or so
		//08/10/20 bIsStuck & bObstacleAhead now updated in timer1 ISR
		while (abs(ToFSteeringVal) > PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD && !bIsStuck && !bObstacleAhead)
		{
			GetRequestedVL53l0xValues(VL53L0X_RIGHT);
			ToFSteeringVal = RightSteeringVal; //08/06/20 now have separate left/right steering vals

//DEBUG!!
				//print out telemetry every 400 msec or so
			if (lastToFArrayTelemetryMsec > ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics
			{
				lastToFArrayTelemetryMsec -= ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC;
				mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\n", millis(),
					Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, ToFSteeringVal, ToFOutput,
					leftspeednum, rightspeednum);
			}
			//DEBUG!!

			//skip bad values
			if (!isnan(ToFSteeringVal))
			{
				//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)
				if (ToFArrayPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value
				{
					leftspeednum = MOTOR_SPEED_HALF - ToFOutput;
					rightspeednum = MOTOR_SPEED_HALF + ToFOutput;

					MoveAhead(leftspeednum, rightspeednum);
				}
			}

			CheckForUserInput();
		}//	while(abs(ToFSteeringVal) > PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD)
	}
	else //TRACKING_LEFT
	{
		//this really is necessary - else while() won't have correct starting value
		GetRequestedVL53l0xValues(VL53L0X_LEFT);
		ToFSteeringVal = LeftSteeringVal; //08/06/20 now have separate left/right steering vals

		//08/09/20 added guards for frontdist & 'stuck' condx
		//08/10/20 bIsStuck & bObstacleAhead now updated in timer1 ISR
		//while (abs(ToFSteeringVal) > PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD && !bIsStuck && !bObstacleAhead)
		while (abs(ToFSteeringVal) > ToFArray_PARALLEL_FIND_SETPOINT && !bIsStuck && !bObstacleAhead)
			//while (true)
		{
			GetRequestedVL53l0xValues(VL53L0X_LEFT);
			ToFSteeringVal = LeftSteeringVal; //08/06/20 now have separate left/right steering vals

//DEBUG!!
				//print out telemetry every 400 msec or so
			//if (lastToFArrayTelemetryMsec > ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics
			//{
			//	lastToFArrayTelemetryMsec -= ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC;
			//mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\n", millis(),
			//	Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal, ToFOutput,
			//	leftspeednum, rightspeednum);
			//}
			//DEBUG!!

					//skip bad values
			if (!isnan(ToFSteeringVal))
			{
				//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)
				if (ToFArrayPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value
				{

					//in DIRECT mode, a negative output means the near side should go slower,
					//and the far side should go faster
					leftspeednum = MOTOR_SPEED_HALF + ToFOutput;
					rightspeednum = MOTOR_SPEED_HALF - ToFOutput;

					MoveAhead(leftspeednum, rightspeednum);

					//mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\n", millis(),
					//	Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal, ToFOutput,
					//	leftspeednum, rightspeednum);
				}
			}

			CheckForUserInput();
		}//	while(abs(ToFSteeringVal) > PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD)
	}
	mySerial.printf("Parallel Orientation Achieved with SteeringVal = %3.2f\n", ToFSteeringVal);


	MoveAhead(0, 0); //need this to keep robot from continuing last turn

//DEBUG!!
	//while (true)
	//{
	//	CheckForUserInput();
	//}
//DEBUG!!


	return true;
}


bool CaptureWallOffset(WallTrackingCases trkcase, int offset)
{
	//Purpose: Position the robot parallel to the nearest wall, at the 
	//	desired offset
	//Inputs:
	//	trkcase = WallTrackingCases enum value denoting the wall to be tracked
	//	offset = int value denoting the offset in Cm
	//Outputs:
	//	Robot positioned parallel to the nearest wall, at the desired offset
	//	return value is TRUE if successful, otherwise FALSE
	//Plan:
	//	Step1: Rotate robot to be parallel to the selected wall using TofArrayPID
	//	Step2: Drive robot toward offset distance using ToFArray PID
	//	Step3: Rotate robot back to parallel orientation  using ToFArray PID
	//Notes:
	//	08/06/20 added left side processing
	//	08/09/20 added check for 'stuck' condx (can't check for frontdist < min, as this is sure to happen during approach)
	//	08/09/20 have to limit IsStuck() calls to 5/sec to avoid false positives
	//	08/10/20 now setting bIsStuck state in timer1 ISR
	//	08/12/20 chg bIsStuck to bIsStuck_Slow for half-speed offset capture ops
	//	08/12/20 added code to re-init aFrontDist on 'stuck' detection

//DEBUG!!
	mySerial.printf("In CaptureWallOffset(%s,%d)\n", TrkStrArray[trkcase], offset);
	//delay(500);
//DEBUG!!


//Step1: Rotate robot to be parallel to the selected wall using TofArrayPID
	RotateToParallelOrientation(trkcase);

	//Step2: Drive robot toward offset distance using ToFArray PID
		//reset PID setpoint for about 20 deg cut toward wall
	int dist;

	if (trkcase == TRACKING_RIGHT)
	{
		dist = GetRightDistCm();
		if (dist > offset)
		{
			ToFSetpoint = -CAPTURE_APPROACH_STEERING_VALUE; //this should result in about 20 deg

		}
		else
		{
			ToFSetpoint = CAPTURE_APPROACH_STEERING_VALUE; //this should result in about 20 deg
		}
	}
	else //TRACKING_LEFT case //populated 08/06/20
	{
		dist = GetLeftDistCm();
		if (dist > offset)
		{
			ToFSetpoint = -CAPTURE_APPROACH_STEERING_VALUE; //this should result in about 20 deg
		}
		else
		{
			ToFSetpoint = CAPTURE_APPROACH_STEERING_VALUE; //this should result in about 20 deg
		}
	}



	//DEBUG!!
	mySerial.printf("After || op - offset/rdist/ToFSetpoint now %d/%d/%2.3f\n", offset, dist, ToFSetpoint);
	mySerial.printf("Time\t\Fdist\tCdist\tRdist\tSteer\tPIDout\tLspd\tRSpd\n");
	delay(2000);
	//DEBUG!!


	////ToFArrayPID.SetTunings(ToFArray_OFFSET_CAPTURE_Kp, 0, 0); //try a bit less aggresive setup
	//ToFArrayPID.SetTunings(ToFArray_OFFSET_CAPTURE_Kp,
	//	ToFArray_OFFSET_CAPTURE_Ki,
	//	ToFArray_OFFSET_CAPTURE_Kd); //09/28/20 added Kd = 50
	//ToFArrayPID.SetOutputLimits(-MOTOR_SPEED_CAPTURE_OFFSET, MOTOR_SPEED_CAPTURE_OFFSET);

	//lastToFArrayTelemetryMsec = 0; //reset before entering while loop
	//if (trkcase == TRACKING_RIGHT)
	//{
	//	while (abs(GetRightDistCm() - offset) > WALL_OFFSET_CAPTURE_WINDOW_CM && !bIsStuck_Slow)
	//	{

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

	//		GetRequestedVL53l0xValues(VL53L0X_RIGHT);
	//		ToFSteeringVal = RightSteeringVal; //08/06/20 now have separate left/right steering vals

	//		//skip bad values
	//		if (!isnan(ToFSteeringVal))
	//		{
	//			//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)
	//			if (ToFArrayPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value
	//			{
	//				leftspeednum = MOTOR_SPEED_CAPTURE_OFFSET - ToFOutput;
	//				rightspeednum = MOTOR_SPEED_CAPTURE_OFFSET + ToFOutput;

	//				MoveAhead(leftspeednum, rightspeednum);
	//			}
	//		}
	//	}

	//	//DEBUG!!
	//				//print out telemetry every 200 msec or so
	//	//if (lastToFArrayTelemetryMsec > ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics
	//	//{
	//	//	lastToFArrayTelemetryMsec -= ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC;
	//	//	mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\n", millis(),
	//	//		Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, ToFSteeringVal, ToFOutput,
	//	//		leftspeednum, rightspeednum);
	//	//}
	//	//DEBUG!!

	//			//08/09/20 now have to check for abnormal exit
	//	if (bIsStuck)//08/10/20 bIsStuck now updated in timer1 ISR
	//	{
	//		InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

	//		//mySerial.printf("Stuck condition detected with front distance = %d\n", GetFrontDistCm());
	//		mySerial.printf("Stuck condition detected with front distance = %d and var = %3.2f\n", GetFrontDistCm(), frontvar);
	//		BackupAndTurn90Deg(true, true, MOTOR_SPEED_HALF);
	//		return false;
	//	}

	//	//DEBUG!!
	//	mySerial.printf("Offset distance achieved with VL53L0X reporting F/C/R = %d/%d/%d\n",
	//		Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);

	//	mySerial.printf("Rotating back to parallel orientation\n");
	//	//DEBUG!!
	//}
	//else //trkcase == TRACKING_LEFT
	//{
	//	mySerial.printf("CaptureWallOffset TRACKING_LEFT block with offset = %d\n", offset);
	//	GetRequestedVL53l0xValues(VL53L0X_LEFT);

	//	//09/29/20 can't use capture window - robot blows right through it :(
	//	int sign = 1; //used to modify while statement
	//	sign = (Lidar_LeftFront > 10 * offset) ? 1 : -1; //handle both 'inside' and 'outside' approaches

	//	//while (abs(GetRightDistCm() - offset) > WALL_OFFSET_CAPTURE_WINDOW_CM && !bIsStuck_Slow)
	//	//while (abs(Lidar_LeftFront - offset) > WALL_OFFSET_CAPTURE_WINDOW_CM && !bIsStuck_Slow)
	//		while (Lidar_LeftFront > sign * 10 * offset)
	//	{
	//		//01/30/17 added to kill motors remotely using Wixel & serial port
	//		CheckForUserInput();

	//		//if (lastToFArrayTelemetryMsec > ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics
	//		{
	//			lastToFArrayTelemetryMsec -= ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC;
	//			//GetRequestedVL53l0xValues(VL53L0X_LEFT);
	//			ToFSteeringVal = LeftSteeringVal; //08/06/20 now have separate left/right steering vals

	//			//skip bad values
	//			if (!isnan(ToFSteeringVal))
	//			{
	//				//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)
	//				if (ToFArrayPID.Compute())
	//				{
	//					leftspeednum = MOTOR_SPEED_CAPTURE_OFFSET + ToFOutput;
	//					rightspeednum = MOTOR_SPEED_CAPTURE_OFFSET - ToFOutput;

	//					MoveAhead(leftspeednum, rightspeednum);

	//					//mySerial.printf("%lu\t%d\t%d\t%d\t%2.1f\t%d\n",
	//					//	millis(),
	//					//	Lidar_LeftFront,
	//					//	10 * offset,
	//					//	abs(Lidar_LeftFront - 10 * offset),
	//					//	10 * WALL_OFFSET_CAPTURE_WINDOW_CM,
	//					//	float(abs(Lidar_LeftFront - 10 * offset)) > 10 * WALL_OFFSET_CAPTURE_WINDOW_CM);

	//					GetRequestedVL53l0xValues(VL53L0X_LEFT);
	//					ToFSteeringVal = LeftSteeringVal; //08/06/20 now have separate left/right steering vals
	//				}
	//			}

	//			//mySerial.printf("\t%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\t%d\n", millis(),
	//			//	Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal, ToFOutput,
	//			//	leftspeednum, rightspeednum, abs(Lidar_LeftFront - 10 * offset) > 10 * WALL_OFFSET_CAPTURE_WINDOW_CM);
	//		}
	//	}

	//	mySerial.printf("%lu\t%d\t%d\t%d\t%2.1f\t%d\n",
	//		millis(),
	//		Lidar_LeftFront,
	//		10 * offset,
	//		abs(Lidar_LeftFront - 10 * offset),
	//		10 * WALL_OFFSET_CAPTURE_WINDOW_CM,
	//		float(abs(Lidar_LeftFront - 10 * offset)) > 10 * WALL_OFFSET_CAPTURE_WINDOW_CM);

	//	//08/09/20 now have to check for abnormal exit
	//	if (bIsStuck_Slow)//08/10/20 bIsStuck now updated in timer1 ISR
	//	{
	//		InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

	//		mySerial.printf("Stuck_Slow condition detected with front distance = %d and var = %3.2f\n", GetFrontDistCm(), frontvar);
	//		BackupAndTurn90Deg(true, true, MOTOR_SPEED_HALF);
	//		return false;
	//	}

	//	//DEBUG!!
	//	mySerial.printf("Offset distance achieved with VL53L0X reporting F/C/R = %d/%d/%d\n",
	//		Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear);
	//	StopBothMotors(); //added 09/29/20
	//	delay(2000); //just for debug
	//	//DEBUG!!

	//}

	////if we get to here, offset capture succeeded
	////Step3: Rotate robot back to parallel orientation  using ToFArray PID

	//mySerial.printf("Rotating back to parallel orientation\n");
	//RotateToParallelOrientation(trkcase);
	return true;
}

void UpdateWallFollowMotorspeeds(int leftdistval, int rightdistval, int& leftspeednum, int& rightspeednum)
{
	//Purpose:  Update left & right motor speed values to follow the nearest wall
	//Provenance: Created 12/26/14 
	//Inputs:
	//    leftspeednum = integer denoting left motor drive value (0-255) 
	//    rightspeednum = integer denoting right motor drive value (0-255) 
	//    previous distance input from left & right ping sensors
	//    current distance input from left & right ping sensors
	//Outputs:
	//    leftspeednum = integer denoting left motor drive value (0-255) 
	//    rightspeednum = integer denoting right motor drive value (0-255)
	//Plan:
	//    Step1: Determine if we are closer to left or right walls & adjust speed accordingly
	//Notes:
	//  01/22/15 added some LED management code for debugging purposes
	//  01/25/15 rev to use TweakVal * |d1-d0| instead of just TweakVal
	//	03/14/15 rev to add left/rightdistval to sig
	//	03/25/15 new speed adj algorithm.  See https://fpaynter.com/2015/03/another-try-at-wall-following-adjustments/
	//	03/29/15 added code to use red LEDs as 'active wall' indicator as well as stop/backup.
	//	11/27/15 rev to remove speed limit checks - now done in FWDMotorDriver class
	//	12/06/15 pulled l/r speed limit adj back from FWDMotorDrive class
	//	12/07/15 chg bIsTrackingLeft from local to global var so can use in BackupAndTurn()
	//	01/05/16 rev to make bIsTrackingLeft into separate function vice global var
	//	05/03/17 commented out LED management code
	//	02/09/19 revised to use PID vs home-brew wall-following algorithm
	//	04/28/19 back to home-brew algorithm

	//02/24/19 commented these lines out - no longer used
	int prevrightspdnum, prevleftspdnum; //added 01/22/15 for LED mgmt
	prevrightspdnum = rightspeednum;
	prevleftspdnum = leftspeednum;

	if (leftdistval <= rightdistval) //tracking wall on left side
	{
		//03/25/15 using new algorithm LSPDn = LSPDn-1 - K * (Dn - Dn-1); RSPDn = RSPDn-1 + K * (Dn - Dn-1)
		leftspeednum = prevleftspdnum - MOTOR_SPEED_ADJ_FACTOR * (leftdistval - prevleftdistval);
		rightspeednum = prevrightspdnum + MOTOR_SPEED_ADJ_FACTOR * (leftdistval - prevleftdistval);
		//DEBUG!!
				//mySerial.printf("Left Wall is Closer, leftdist, prevleftdist, prevlspd, lspd\n",leftdistval, prevleftdistval, prevleftspdnum, leftspeednum);
				//mySerial.printf("Left Wall is Closer, leftdist = %d, prevleftdist = %d, prevlspd = %d, lspd = %d\n",leftdistval, prevleftdistval, prevleftspdnum, leftspeednum);
		//DEBUG!!
	}
	else //right wall is closer
	{
		////DEBUG!!
		//		Serial.println("Right Wall is Closer");
		////DEBUG!!

				//bIsTrackingLeft = false;

				//commented out 02/23/19
				//03/25/15 using new algorithm LSPDn = LSPDn-1 + K * (Dn - Dn-1); RSPDn = RSPDn-1 - K * (Dn - Dn-1)
		leftspeednum = prevleftspdnum + MOTOR_SPEED_ADJ_FACTOR * (rightdistval - prevrightdistval);
		rightspeednum = prevrightspdnum - MOTOR_SPEED_ADJ_FACTOR * (rightdistval - prevrightdistval);
	}

	prevleftdistval = leftdistval;
	prevrightdistval = rightdistval;

	//ManageWallTrackingLEDs(GetTrackingDir());

	//12/06/15 pulled l/r speed limit adj back from FWDMotorDrive class
	//12/20/15 changed top end from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (255)
	leftspeednum = (leftspeednum <= MOTOR_SPEED_MAX) ? leftspeednum : MOTOR_SPEED_MAX;
	leftspeednum = (leftspeednum >= MOTOR_SPEED_LOW) ? leftspeednum : MOTOR_SPEED_LOW;

	rightspeednum = (rightspeednum <= MOTOR_SPEED_MAX) ? rightspeednum : MOTOR_SPEED_MAX;
	rightspeednum = (rightspeednum >= MOTOR_SPEED_LOW) ? rightspeednum : MOTOR_SPEED_LOW;
}

int GetOpMode(float batv) //04/28/19 added batv to sig
{
	//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 (4)
	//	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

	int mode = MODE_NONE; //04/27/20 added so function has only one exit point
//Step1: If Charger is connected, return MODE_CHARGING (1)
	if (IsChargerConnected())
	{
		mode = MODE_CHARGING;
		mySerial.printf("Robot has successfully connected to charger!\n");
	}

	//Step2: Else If IR Homing beam detected, return MODE_IRHOMING (2)
	else if (IsIRBeamAvail()) 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) mode = MODE_DEADBATTERY;

	//03/04/2020 added to facilitate battery discharge
#ifdef BATTERY_DISCHARGE
	//05/01/2020 rewrote to eliminate OpMode section entirely.  Now all done here
	mySerial.printf("\n-------------------  DISCHARGE MODE ------------------------\n\n");
	mySerial.printf("Minutes\tVolts\tCurrent\n");

	MoveAhead(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

	float startmSec = millis(); //start time
	while (GetBattVoltage() > DEAD_BATT_THRESH_VOLTS)
	{
		float elapsedmin = (millis() - startmSec) / 60000.f; //elapsed minutes
		mySerial.printf("%2.2f\t%2.3f\t%2.3f\n", elapsedmin, GetBattVoltage(), GetRunningAmps());
		delay(6000); //10 readings/minute
	}
	mySerial.printf("Battery Discharge Complete at %3.2f Minutes and %3.2f Volts\n", millis() / 60000.f, GetBattVoltage());
	mySerial.printf("Stopping program!\n");
	StopBothMotors();
	while (true)
	{

	}

#else
//Step4: Else return MODE_WALLFOLLOW (3)
	else mode = MODE_WALLFOLLOW;
#endif // BATTERY_DISCHARGE

	return mode;
}

//01/05/16 added so tracking decision can be based on running avg vs just one value
//01/05/16 return value can't be TrackingCases type - causes Arduino pre-processor to choke
int GetTrackingDir()
{
	//Purpose:  Determine tracking condition (left, right, neither)
	//Provenance: 01/05/16 G. Frank Paynter
	//Inputs: Call from Loop()
	//Outputs:
	//	TRACKING_LEFT, TRACKING_RIGHT, or TRACKING_NEITHER values
	//	TRACKING_IRBEAM added 01/30/17
	//Plan:
	//	Step1: compute LR_PING_AVG_WINDOW_SIZE-point running average for left & right distances
	//	Step2: Determine current tracking case & (for TRACKING_LEFT & TRACKING_RIGHT) update PrevTrackingCase
	//		   For TRACKING_NEITHER, PrevTrackingCase is *not* updated so it can be used to determine turn direction
	//	Step3: (added 01/30/17) Determine state of charge and IR Beam availability
	//  Step4: return appropriate TrackingCases value
	//Notes:
	//	01/05/16: due to Arduino pre-processor quirk, can't return a TrackingCases object - must be a 'normal' type
	//  01/18/16: The TRACKING_NEITHER case only occurs when N-point avg of both l/r dists > max
	//	01/30/17: Added TRACKING_IRBEAM detection code
	//	04/28/19: Reinstated running average calc - NewPing::.ping_median() takes too long
	//	04/09/20: Revised to use new GetAvgLeft/RightDistCm() functions

////DEBUG!!
//	mySerial.printf("GetTrackingDir() TOP at %lu\n", millis());
////DEBUG!!

////Step1: compute LR_PING_AVG_WINDOW_SIZE-point running average for left & right distances
	//04/28/19 reinstated - NewPing::.ping_median() takes too long
	//int leftavgdist_cm = 0;
	//int rightavgdist_cm = 0;
	int retval = 0;

	//for (int validx = 0; validx < LR_PING_AVG_WINDOW_SIZE; validx++)
	//{
	//	leftavgdist_cm += aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1 - validx];
	//	rightavgdist_cm += aRightDist[LR_PING_DIST_ARRAY_SIZE - 1 - validx];
	//}

	//leftavgdist_cm = leftavgdist_cm / LR_PING_AVG_WINDOW_SIZE;
	//rightavgdist_cm = rightavgdist_cm / LR_PING_AVG_WINDOW_SIZE;

	//04/09/20: Revised to use new GetAvgLeft/RightDistCm() functions
	int leftavgdist_cm = (int)GetAvgLeftDistCm();
	int rightavgdist_cm = (int)GetAvgRightDistCm();

	//DEBUG!!
	mySerial.printf("Left/Right Avg Dist = %d/%d\n", leftavgdist_cm, rightavgdist_cm);
	//DEBUG!!

			//Step2: If result is TRACKING_LEFT or TRACKING_RIGHT, set bWasLastTrackingLeft boolean & tracking case appropriately

				//check for 'open space' condx first, as otherwise it will never be detected
	if (leftavgdist_cm == MAX_LR_DISTANCE_CM && rightavgdist_cm == MAX_LR_DISTANCE_CM)
	{
		retval = TRACKING_NEITHER; //don't update PrevTrackingCase - prev val needed for 'Open-Corner' turn dir
	}
	else if (leftavgdist_cm <= rightavgdist_cm)
	{
		PrevTrackingCase = TRACKING_LEFT;
		retval = PrevTrackingCase;
	}
	else if (leftavgdist_cm > rightavgdist_cm)
	{
		PrevTrackingCase = TRACKING_RIGHT;
		retval = PrevTrackingCase;
	}

	////DEBUG!!
	//	mySerial.printf("GetTrackingDir() BOTTOM at %lu\n", millis());
	////DEBUG!!

	//Step3: return appropriate TrackingCases value
	return retval;
}


#pragma endregion Wall Tracking Support

#pragma region MISCELLANEOUS SUPPORT FUNCTIONS
int GetIntegerParameter(String prompt, int defaultval)
{
	char Instr[20];
	int param = 0;
	bool bDone = false;

	while (!bDone)
	{
		Serial.print(prompt); Serial.print(" ("); Serial.print(defaultval); Serial.print("): ");
		while (Serial.available() == 0); //waits for input
										 //String res = Serial.readString().trim();
		String res = Serial.readString();
		res.trim();

		int reslen = res.length();
		if (reslen == 0) //user entered CR only
		{
			bDone = true;
			param = defaultval;
		}
		else
		{
			res.toCharArray(Instr, reslen + 1);
			//if (isNumeric(Instr) && atoi(Instr) >= 0)
			if (isNumeric(Instr))
			{
				param = atoi(Instr);
				bDone = true;
			}
			else
			{
				Serial.print(Instr); Serial.println(" Is invalid input - please try again");
			}
		}
	}
	Serial.println(param);
	return param;
}

// check a string to see if it is numeric and accept Decimal point
//copied from defragster's post at https://forum.pjrc.com/threads/27842-testing-if-a-string-is-numeric
bool isNumeric(char* str)
{
	byte ii = 0;
	bool RetVal = false;
	if ('-' == str[ii])
		ii++;
	while (str[ii])
	{
		if ('.' == str[ii]) {
			ii++;
			break;
		}
		if (!isdigit(str[ii])) return false;
		ii++;
		RetVal = true;
	}
	while (str[ii])
	{
		if (!isdigit(str[ii])) return false;
		ii++;
		RetVal = true;
	}
	return RetVal;
}


//04/20/15 added for turn signal support - replaces AllLedsOff()
void BackupSignal(bool bEnable)
{
	//Purpose:  Turns all rear LEDs off (HIGH is off)
	//Provenance: Created 01/24/15 gfp
	//03/19/18 now just an alias for EnableAllRearLEDs()

	EnableAllRearLEDs(bEnable);
}

int GetAverageAnalogReading(int pin, int numavgs)
{
	long totreads = 0;
	for (int i = 0; i < numavgs; i++)
	{

		////DEBUG!!
		//		int aVal = analogRead(pin);
		//		mySerial.printf("Analog value at pin %d = %d\n", pin, aVal);
		////DEBUG!!

		totreads += analogRead(pin);
	}

	return (int)((float)totreads / (float)numavgs); //truncation ok
}

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();

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

void DisableAllRearPanelLEDs()
{
	digitalWrite(_40PCT_LED_PIN, HIGH);
	digitalWrite(_20PCT_LED_PIN, HIGH);
	digitalWrite(CHG_LED_PIN, HIGH);
	digitalWrite(_60PCT_LED_PIN, HIGH);
	digitalWrite(_80PCT_LED_PIN, HIGH);
	digitalWrite(FIN_LED_PIN, HIGH);
}

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

	if (bEnable)
	{
		digitalWrite(FULL_LEFT_LED_PIN, LOW);
		digitalWrite(HALF_LEFT_LED_PIN, LOW);
		digitalWrite(QTR_LEFT_LED_PIN, LOW);
		digitalWrite(FULL_RIGHT_LED_PIN, LOW);
		digitalWrite(HALF_RIGHT_LED_PIN, LOW);
		digitalWrite(QTR_RIGHT_LED_PIN, LOW);
	}
	else
	{
		digitalWrite(FULL_LEFT_LED_PIN, HIGH);
		digitalWrite(HALF_LEFT_LED_PIN, HIGH);
		digitalWrite(QTR_LEFT_LED_PIN, HIGH);
		digitalWrite(FULL_RIGHT_LED_PIN, HIGH);
		digitalWrite(HALF_RIGHT_LED_PIN, HIGH);
		digitalWrite(QTR_RIGHT_LED_PIN, HIGH);
	}
}

//void UpdateChgStatusLEDs(float battv) //rev 02/24/19
void UpdateChgStatusLEDs(float battv, bool bStillCharging) //04/22/20 added bStillCharging to sig
{
	//Purpose: Update new 'fuel guage' LED status to show very rough approximation of battery charge level
	//Inputs:
	//	battv = battery voltage measurement from Mega A/D
	//	bChg = bool that is LOW while charging, HIGH when finished
	//	bFin = bool that is HIGH while charging, LOW when finished
	//	aBattVtoPct = table of battery voltage ranges vs pct charge
	//	
	//Outputs:
	//	Chg, 20-40-60-80%, and FIN LEDs illuminated as appropriate
	//Plan:
	//	Step1: turn all LEDs OFF
	//	Step2: Illuminate appropriate LEDs
	//Notes:
	//	04/22/20 added bStillCharging to sig to eliminate unneccessary call to IsStillCharging()

//Step1: turn all LEDs OFF
	EnableAllRearLEDs(false); //turns them all OFF


//Step2: illuminate appropriate LEDs
	//if (IsStillCharging()) //02/24/19 - now using 1NA169 current sensor
	if (bStillCharging) //04/22/20 rev to pass this in as calling parameter 
	{
		digitalWrite(CHG_LED_PIN, LOW);
	}
	if (battv < _20PCT_BATT_VOLTS)
	{
		//do nothing
		//mySerial.printf("In 0-20% section with BattV = %2.4f\n", battv);
	}

	if (battv >= _20PCT_BATT_VOLTS)
	{
		//mySerial.printf("In > 20% section with BattV = %2.4f\n", battv);
		digitalWrite(_20PCT_LED_PIN, LOW);
	}

	if (battv >= _40PCT_BATT_VOLTS)
	{
		//mySerial.printf("In > 40% section with BattV = %2.4f\n", battv);
		digitalWrite(_40PCT_LED_PIN, LOW);
	}

	if (battv >= _60PCT_BATT_VOLTS)
	{
		//mySerial.printf("In > 60% section with BattV = %2.4f\n", battv);
		digitalWrite(_60PCT_LED_PIN, LOW);
	}

	if (battv >= _80PCT_BATT_VOLTS)
	{
		//mySerial.printf("In > 80% section with BattV = %2.4f\n", battv);
		digitalWrite(_80PCT_LED_PIN, LOW);
	}

	//if (digitalRead(FIN_SIG_PIN) == LOW)
	//{
	//	//mySerial.printf("In Chg Finished section with BattV = %2.4f\n", battv);
	//	digitalWrite(FIN_LED_PIN, LOW);
	//}
}

void YellForHelp()
{
	//Purpose: last-ditch effort to preserve the battery.  All non-essential loads are shut down
	//		   and an visual/audible SOS is sounded forever
	//Inputs: Call from OpMode case switch when GetBattVoltage() returns a below-threshold value
	//Outputs: 
	//	All wheel motors stopped
	//	All rear panel LED's turned OFF
	//	PWM 'SOS' tones on SOS_PWM_PIN
	//Plan:
	//	Step1: Turn off wheel motors
	//	Step2: Turn off all rear panel LEDs
	//	Step3: Send SOS to speaker, the purple rear panel LEDs, and the red laser
	//Notes:
	//	01/16/18 - SOS tone code copied from PWMTest.pde

//Step1: Turn off wheel motors
	StopBothMotors();

	//Step2: Turn off all rear panel LEDs
	EnableAllRearLEDs(false);

	//Step3: infinte loop to xmit SOS on speaker and the purple rear panel LEDs
	while (!IsChargerConnected())
	{
		//int DOT_MS = 200;
		//int DASH_MS = 800;
		//int HIGHTONE = 1000;
		//int LOWTONE = 500;


		//Send 'S'
		for (size_t i = 0; i < 3; i++)
		{
			digitalWrite(RED_LASER_DIODE_PIN, HIGH); //turn laser on
			digitalWrite(FIN_LED_PIN, LOW); //turn LED on
			digitalWrite(CHG_LED_PIN, LOW); //turn LED on
			tone(SOS_PWM_PIN, HIGHTONE, DOT_MS); //returns immediately
			delay(DOT_MS);              //delay for LED viewing
			digitalWrite(FIN_LED_PIN, HIGH); //turn LED off
			digitalWrite(CHG_LED_PIN, HIGH); //turn LED off
			digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn laser off
			delay(DOT_MS);              //inter-symbol spacing
		}
		delay(DOT_MS); //inter-letter spacing

		//Send 'O'
		for (size_t i = 0; i < 3; i++)
		{
			digitalWrite(RED_LASER_DIODE_PIN, HIGH); //turn laser on
			digitalWrite(FIN_LED_PIN, LOW); //turn LED on
			digitalWrite(CHG_LED_PIN, LOW); //turn LED on
			tone(SOS_PWM_PIN, HIGHTONE, DASH_MS); //returns immediately
			delay(DASH_MS);              //delay for LED viewing
			digitalWrite(FIN_LED_PIN, HIGH); //turn LED off
			digitalWrite(CHG_LED_PIN, HIGH); //turn LED off
			digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn laser off
			delay(DOT_MS);              //inter-symbol spacing
		}
		delay(DOT_MS); //inter-letter spacing

		//Send 'S'
		for (size_t i = 0; i < 3; i++)
		{
			digitalWrite(RED_LASER_DIODE_PIN, HIGH); //turn laser on
			digitalWrite(FIN_LED_PIN, LOW); //turn LED on
			digitalWrite(CHG_LED_PIN, LOW); //turn LED on
			tone(SOS_PWM_PIN, HIGHTONE, DOT_MS); //returns immediately
			delay(DOT_MS);              //delay for LED viewing
			digitalWrite(FIN_LED_PIN, HIGH); //turn LED off
			digitalWrite(CHG_LED_PIN, HIGH); //turn LED off
			digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn laser off
			delay(DOT_MS);              //inter-symbol spacing
		}

		delay(10 * DOT_MS);         // inter-message spacing
	}
}

byte ByteValFromBattVolt()
{
	//Purpose:  Calculate a byte value representing the current battery voltage, 
	//			where 0 = DEAD_BATT_THRESH_VOLTS-1 = 5V
	//			and 255 = FULL_BATT_VOLTS + 1 = 9.4V
	//Inputs: none
	//Outputs:
	//	return a number between 0 and 255, where 0 represents 5V and 255 represents 9.4V
	float val = GetBattVoltage();
	float top = FULL_BATT_VOLTS + 1; //9.4
	float bot = DEAD_BATT_THRESH_VOLTS - 1;//5.0
	float range = top - bot; //4.4
	float res = 255 * (val - bot) / range;
	byte num = (byte)(res);

	return num;
}

float BattVoltFromByteVal(byte battnum)
{
	//Purpose:  Calculate battery voltage from byte value, 
	//			where 0 = DEAD_BATT_THRESH_VOLTS-1 = 5V
	//			and 255 = FULL_BATT_VOLTS + 1 = 9.4V
	//Inputs: byte value from packet
	//	0 represents DEAD_BATT_THRESH_VOLTS-1 = 5V
	//	255 represents FULL_BATT_VOLTS + 1 = 9.4V
	//Outputs:
	//	calculated battery voltage as (top-bot)*(battnum/255) + bot

	float top = FULL_BATT_VOLTS + 1; //9.4
	float bot = DEAD_BATT_THRESH_VOLTS - 1;//5.0
	float range = top - bot; //4.4
	float volts = range * ((float)battnum / (float)255) + bot;

	return volts;
}

//re-enabled 06/28/20
void PrintWallFollowTelemetry()
{
	//mySerial.printf("PrintWallFollowTelemetry() top at time %lu\n", millis());
	DateTime dt = rtc.now();
	uint32_t utime = dt.unixtime();

	//get time string in hh:mm:ss format
	char timestr[20];
	memset(timestr, 0, sizeof(timestr));
	GetTimeStringFromUnixTime(timestr, utime);

	//battery voltage
	float batv = GetBattVoltage(); //temporarily commented out

	//print everything out 
	//mySerial.printf("%s\t%1.2f\t%s\t%s\t%d\t%d\t%d\t%3.2f\t%d\t%d\n",
	//	timestr, batv, ModeStrArray[CurrentOpMode], TrkStrArray[TrackingCase], leftdistval, rightdistval, frontdistval,
	//	frontvar, leftspeednum, rightspeednum);
	mySerial.printf("%lu\t%1.2f\t%s\t%s\t%d\t%d\t%d\t%3.2f\t%d\t%d\n",
		millis(), batv, ModeStrArray[CurrentOpMode], TrkStrArray[TrackingCase], leftdistval, rightdistval, frontdistval,
		frontvar, leftspeednum, rightspeednum);
}

//09/01/20 added for wall-tracking debug
void PrintWallFollowTelemetry(WallTrackingCases trkcase)
{
	//mySerial.printf("PrintWallFollowTelemetry() top at time %lu\n", millis());
	DateTime dt = rtc.now();
	uint32_t utime = dt.unixtime();

	//get time string in hh:mm:ss format
	char timestr[20];
	memset(timestr, 0, sizeof(timestr));
	GetTimeStringFromUnixTime(timestr, utime);

	//battery voltage
	float batv = GetBattVoltage(); //temporarily commented out

	GetRequestedVL53l0xValues(VL53L0X_RIGHT);
	//ToFSteeringVal = Lidar_RightFront - Lidar_RightRear + Lidar_RightCenter; //use ALL info - not just distance!

//	const char* WallFollowTelemStr = "Msec\tMode\tTrack\tFront\tCtr\tRear\tSteer\tOutput\tLSpd\tRSpd"; //09/01/20 PID tuning

	//print everything out 
	mySerial.printf("%lu\t%s\t%s\t%d\t%d\t%d\t%3.2f\t%3.2f\t%d\t%d\n",
		millis(), ModeStrArray[CurrentOpMode], TrkStrArray[TrackingCase], Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear,
		RightSteeringVal, ToFOutput, leftspeednum, rightspeednum);
}

void GetTimeStringFromUnixTime(char* timestr, uint32_t unixtime)
{
	//time_t utime = (time_t)unixtime;
	//int myhour = hour(utime);
	//int mymin = minute(utime);
	//int mysec = second(utime);

	//02/19/19 now using RTCLib's DateTime class
	DateTime dt = DateTime(unixtime);
	int myhour = dt.hour();
	int mymin = dt.minute();
	int mysec = dt.second();

	//now concatenate everything into the provided buffer
	sprintf(timestr, "%02d:%02d:%02d", myhour, mymin, mysec);
}

void GetModeString(int mode, char* bufptr)
{
	sprintf(bufptr, "%s", ModeStrArray[mode]);
}

void GetWallTrackString(int mode, char* bufptr)
{
	sprintf(bufptr, "%s", TrkStrArray[mode]);
}

void CheckForUserInput()
{
	if (Serial.available() > 0)
	{
		// read the incoming byte:
		int incomingByte = Serial.read();

		// say what you got:
		Serial.print("I received: ");
		//Serial.println(incomingByte, DEC);
		Serial.println(incomingByte, HEX); //chg to HEX 02/18/20

		//02/18/20 experiment with multiple commands
		switch (incomingByte)
		{
		case 0x53: //ASCII 'S'
		case 0x73: //ASCII 's'
			mySerial.print("Stopping Motors!");
			StopBothMotors();
			while (true)
			{

			}
			break;
		case 0x43: //ASCII 'C'
		case 0x63: //ASCII 'c'
			//enter infinite loop for direct remote control
			mySerial.printf("ENTERING COMMAND MODE:\n");
			mySerial.printf("0 = 180 deg CCW Turn\n");
			mySerial.printf("1 = 180 deg CW Turn\n");
			mySerial.printf("A = Back to Auto Mode\n");
			mySerial.printf("S = Stop\n");
			mySerial.printf("F = Forward\n");
			mySerial.printf("R = Reverse\n");
			mySerial.printf("\n");
			mySerial.printf("       Faster\n");
			mySerial.printf("\t8\n");
			mySerial.printf("Left 4\t5  6 Right\n");
			mySerial.printf("\t2\n");
			mySerial.printf("       Slower\n");

			StopBothMotors();
			int speed = 0;

			bool bAutoMode = false;
			while (!bAutoMode)
			{
				if (Serial.available() > 0)
				{
					// read the incoming byte:
					int incomingByte = Serial.read();

					mySerial.printf("Got %c\n", incomingByte);

					switch (incomingByte)
					{
					case 0x30: //Dec '0'
						mySerial.print("CCW 180 deg Turn\n");
						RollingTurn(true, bIsForwardDir, 180);
						MoveAhead(speed, speed);
						break;
					case 0x31: //Dec '1'
						mySerial.print("CW 180 deg Turn\n");
						RollingTurn(false, bIsForwardDir, 180);
						MoveAhead(speed, speed);
						break;
					case 0x34: //Turn left 10 deg
						mySerial.print("CCW 10 deg Turn\n");
						MoveAhead(speed, speed);
						RollingTurn(true, bIsForwardDir, 10);

						//added 05/03/20
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x36: //Turn right 10 deg'
						mySerial.print("CW 10 deg Turn\n");
						MoveAhead(speed, speed);
						RollingTurn(false, bIsForwardDir, 10);

						//added 05/03/20
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x38: //Speed up 
						speed += 50;
						speed = (speed >= MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed;
						mySerial.printf("Speeding up: speed now %d\n", speed);
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x32: //Slow down 
						speed -= 50;
						speed = (speed < 0) ? 0 : speed;
						mySerial.printf("Slowing down: speed now %d\n", speed);
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x35: //05/07/20 changed to use '5' vs 'S'
					//case 0x53: //ASCII 'S'
					//case 0x73: //ASCII 's'
						mySerial.print("Stopping Motors!\n");
						StopBothMotors();
						speed = 0;
						break;
					case 0x41: //ASCII 'A'
					case 0x61: //ASCII 'a'
						StopBothMotors();
						mySerial.print("Re-entering AUTO mode\n");
						bAutoMode = true;
						break;
					case 0x52: //ASCII 'R'
					case 0x72: //ASCII 'r'
						mySerial.print("Setting both motors to reverse\n");
						bIsForwardDir = false;
						MoveReverse(speed, speed);
						break;
					case 0x46: //ASCII 'F'
					case 0x66: //ASCII 'f'
						mySerial.print("Setting both motors to forward\n");
						bIsForwardDir = true;
						MoveAhead(speed, speed);
						break;
					case 0x44: //ASCII 'D'
					case 0x64: //ASCII 'd'
						GetRequestedVL53l0xValues(VL53L0X_BOTH);
						mySerial.printf("Msec\tLFront\tLCtrc\tLRear\tRFront\tRCtr\tRRear\n");
						mySerial.printf("%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\n",
							millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);
						bIsForwardDir = true;
						MoveAhead(speed, speed);
						break;
					Default:
						mySerial.print("In Default Case: Stopping Motors!");
						MoveAhead(0, 0);
						while (true)
						{

						}
					}
				}
			}
		}
	}
}

#pragma endregion Miscellaneous

#pragma region HDG_BASED_TURN_SUPPORT


void BackupAndTurn90Deg(bool b_ccw, bool b_fwd, int motor_speed)
{
	//Purpose: Backup 1 second and then turn 90 degrees
	//Provenance: G. Frank Paynter 06/27/2020
	//Inputs:
	//	bIsCCW = bool denoting direction for the turn
	//	bIsFwd = bool denoting fwd/bkwd direction for the turn
	//	motor_speed = int denoting motor speed to use for the maneuver
	//Outputs:
	//	Robot backs up 1 sec, then turns 90 deg in the specified direction
	//Plan:
	//	Step1: Back up for 1 sec
	//	Step2: Turn 90 deg in specified direction
	//Notes:
	//	06/27/20 Replaces BackupAndTurn in all Tracking modes

//Step1: Back up for 1 sec
	BackupSignal(true);	//turn backup LEDs ON
	MoveReverse(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
	delay(1000);
	BackupSignal(false);//turn them back OFF

//Step2: Turn 90 deg fwd or backward in specified direction
	//RollingTurn(b_ccw, b_fwd, 90);
	SpinTurn(b_ccw, 90); //06/27/20 experiment
}

bool RollingTurn(bool bIsCCW, bool bIsFwd, float numDeg)
{
	//Purpose: Make a numDeg forwards or backwards CW turn
	//Inputs:
	//	bIsFwd - True if turn is to be forward, 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()
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Compute new target value
	//	Step3: Set motor speeds based on fwd/backwds flag
	//	Step4: Run motors until target reached
	//Notes:
	//	08/22/18 now using MPU6050_CCW_INCREASES_YAWVAL define to compute tgt
	//	08/29/18 removed MPU6050_CCW_INCREASES_YAWVAL - the MPU6050 DMP takes care of this
	//	09/08/18 revised end-of-turn detection to include slope calc.  Threshold alone is too fragile
	//	09/11/18 revised to handle both turn directions
	//	04/29/19 uncommented
	//	07/31/19 rev for better initial heading capture
	//	07/31/19 rev return type to bool
	//	07/31/19 rev to eliminate timed loop - now runs as fast as possible
	//	07/31/19 rev to not stop motors at end - now calling pgm is resp for this
	//	10/06/19 rev for new IMU polling setup
	//	12/05/19 put MPU6050_CCW_INCREASES_YAWVAL define back in
	//	02/29/20 copied here from two wheel robot
	//	04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding

	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point

//DEBUG!!
	mySerial.printf("In RollingTurn(%s, %s,%4.2f)\n", bIsCCW == TURNDIR_CCW ? "CCW" : "CW", bIsFwd == true ? "FWD" : "BKWD", numDeg);
	//DEBUG!!

		//no need to continue if the IMU isn't available
	if (!dmpReady)
	{
		return false;
	}

	//if we get to here, the IMU is OK and at least one packet is available
	IMUHdgValDeg = UpdateIMUHdgValDeg(); //updates IMUHdgValDeg if successful

//Step2: Compute new target value & timeout value
	timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG;

	//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 = bIsCCW ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;
#else
	tgt_deg = bIsCCW ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

	//correct for -180/180 transition
	if (tgt_deg < -180)
	{
		tgt_deg += 360;
	}

	//07/29/19 bugfix
	if (tgt_deg > 180)
	{
		tgt_deg -= 360;
	}

	//DEBUG!!
			//mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f  deg, tgt = %4.2f deg, timeout = %4.2f sec\n",
			//	IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);
	//DEBUG!!

			//Step3: Start motors
	if (bIsFwd)
	{
		//OK, the robot will be moving forward,
		if (bIsCCW) //CCW rotation (viewed from above) requires fast on right, slow on left
		{
			MoveAhead(OFFSIDE_MOTOR_SPEED, DRIVESIDE_MOTOR_SPEED_HIGH);
		}
		else //CW rotation (viewed from above) requires fast on left, slow on right
		{
			MoveAhead(DRIVESIDE_MOTOR_SPEED_HIGH, OFFSIDE_MOTOR_SPEED);
		}
	}
	else
	{
		//OK, the robot will be moving backward,
		if (bIsCCW) //CCW rotation (viewed from above) requires fast on left, slow on right
		{
			MoveReverse(DRIVESIDE_MOTOR_SPEED_HIGH, OFFSIDE_MOTOR_SPEED);
		}
		else //CW rotation (viewed from above) requires fast on left, slow on right
		{
			MoveReverse(OFFSIDE_MOTOR_SPEED, DRIVESIDE_MOTOR_SPEED_HIGH);
		}
	}

	sinceLastTimeCheck = 0; //needed for watchdog timer

//DEBUG!!
	//mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n");
//DEBUG!!

	bool bFirstPass = true; //for 'slowdown' print control
	float curHdgMatchVal = 0;

	//09/08/18 added to bolster end-of-turn detection
	float prevHdgMatchVal = 0;
	float matchSlope = 0;

	//Step4: Run motors until target reached
	while (!bDoneTurning && !bTimedOut)
	{
		CheckForUserInput(); //added 06/29/20

		IMUHdgValDeg = UpdateIMUHdgValDeg();  //updates IMUHdgValDeg if successful

//DEBUG!!
			//mySerial.printf("%lu\t%4.2f\n", millis(), IMUHdgValDeg);
//DEBUG!!

		//07/31/19 now running as fast as we can get valid hdgs from IMU
		//check for nearly there and all the way there
		curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);
		matchSlope = curHdgMatchVal - prevHdgMatchVal;

		if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL)
		{
			if (bFirstPass)
			{
				prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs

				//DEBUG!!
				//mySerial.printf("Slowing down at %4.2f deg\n", IMUHdgValDeg);
				//DEBUG!!

				bFirstPass = false;
			}
		}

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

		bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

		//		if (bIsFirst180)
		//		{
		////DEBUG!!
		//			mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, slope = %1.3f and bDone = %s\n",
		//				IMUHdgValDeg, tgt_deg, curHdgMatchVal, matchSlope, (bDoneTurning == true)?"TRUE":"FALSE");
		////DEBUG!!
		//		}

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

			//return false;
			bResult = false;

		}
	}
	////DEBUG!!
	//	mySerial.printf("%lu: Exiting RollingTurn()\n", millis());
	////DEBUG!!

		//04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding
	StopBothMotors();

	//return true;
	return bResult;
}

//added 06/27/20 to see if spin turns will work
bool SpinTurn(bool b_ccw, float numDeg)
{
	//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()
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Compute new target value
	//	Step3: Set motor speeds based on b_ccw
	//	Step4: Run motors until target reached
	//Notes:
	//	06/27/20 copied from RollingTurn & adapted
	//	09/02/20 experiment with adjustable turn rate

	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point
	double turnRate = 0; //added 09/02/20
	double prev_hdg = 0;
	unsigned long prev_uSec; //added 09/02/20


	//DEBUG!!
	mySerial.printf("In SpinTurn(%s, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg);
	//DEBUG!!

	//no need to continue if the IMU isn't available
	if (!dmpReady)
	{
		return false;
	}

	IMUHdgValDeg = UpdateIMUHdgValDeg(); //updates IMUHdgValDeg if successful
	prev_hdg = IMUHdgValDeg; //added 09/02/20

//Step2: Compute new target value & timeout value
	timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG;

	//05/17/20 limit timeout_sec to 1 sec or more
	timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

	//12/05/19 added #define back in to manage which direction increases yaw values
#ifdef MPU6050_CCW_INCREASES_YAWVAL
	tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;
#else
	tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

	//correct for -180/180 transition
	if (tgt_deg < -180)
	{
		tgt_deg += 360;
	}

	//07/29/19 bugfix
	if (tgt_deg > 180)
	{
		tgt_deg -= 360;
	}

	////DEBUG!!
	//	mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f  deg, tgt = %4.2f deg, timeout = %4.2f sec\n",
	//		IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);
	////DEBUG!!

		//Step3: Start motors
	//SetLeftMotorDir(!b_ccw); //left motors go backward for ccw, forward for cw
	//SetRightMotorDir(b_ccw); //right motors go forward for ccw, backward for cw

	const int TURN_RATE_MOTOR_SPEED_ADJUST_AMOUNT = 20;
	const int TURN_RATE_MOTOR_SPEED_MINIMUM = 75;
	int spinMotorSpeed = MOTOR_SPEED_FULL;
	const int TURN_RATE_TARGET_DEGPERSEC = 75;

	//09/08/20 modified for DRV8871 motor driver
	SetLeftMotorDirAndSpeed(!b_ccw, spinMotorSpeed);
	SetRightMotorDirAndSpeed(b_ccw, spinMotorSpeed);

	sinceLastTimeCheck = 0; //needed for watchdog timer

////DEBUG!!
//	mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n");
////DEBUG!!

	bool bFirstPass = true; //for 'slowdown' print control
	float curHdgMatchVal = 0;

	//09/08/18 added to bolster end-of-turn detection
	float prevHdgMatchVal = 0;
	float matchSlope = 0;

	//Step4: Run motors until target reached
	prev_uSec = micros();
	while (!bDoneTurning && !bTimedOut)
	{
		IMUHdgValDeg = UpdateIMUHdgValDeg();  //updates IMUHdgValDeg if successful
		turnRate = 1e6 * abs(IMUHdgValDeg - prev_hdg) / (micros() - prev_uSec);
		prev_uSec = micros();
		prev_hdg = IMUHdgValDeg;

		//09/03/20 see if I can modulate the turn rate
		if (turnRate > TURN_RATE_TARGET_DEGPERSEC)
		{
			spinMotorSpeed -= TURN_RATE_MOTOR_SPEED_ADJUST_AMOUNT;
		}
		else if (turnRate < TURN_RATE_TARGET_DEGPERSEC)
		{
			spinMotorSpeed += TURN_RATE_MOTOR_SPEED_ADJUST_AMOUNT;
		}

		//bound spinMotorSpeed
		spinMotorSpeed < TURN_RATE_MOTOR_SPEED_MINIMUM ? spinMotorSpeed = TURN_RATE_MOTOR_SPEED_MINIMUM : spinMotorSpeed;
		spinMotorSpeed > MOTOR_SPEED_MAX ? spinMotorSpeed = MOTOR_SPEED_MAX : spinMotorSpeed;

		SetLeftMotorDirAndSpeed(!b_ccw, spinMotorSpeed);
		SetRightMotorDirAndSpeed(b_ccw, spinMotorSpeed);

		//DEBUG!!
				//mySerial.printf("SpinTurn: %lu\t%4.2f\t%4.2f\t%2.4f\t%d\n", millis(), IMUHdgValDeg, prev_hdg, turnRate, spinMotorSpeed);
		//DEBUG!!


				//07/31/19 now running as fast as we can get valid hdgs from IMU
				//check for nearly there and all the way there
		curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);
		matchSlope = curHdgMatchVal - prevHdgMatchVal;

		if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL)
		{
			if (bFirstPass)
			{
				prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs

//DEBUG!!
	//mySerial.printf("Slowing down at %4.2f deg\n", IMUHdgValDeg);
//DEBUG!!

				bFirstPass = false;
			}
		}

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

		bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

		//		if (bIsFirst180)
		//		{
		////DEBUG!!
		//			mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, slope = %1.3f and bDone = %s\n",
		//				IMUHdgValDeg, tgt_deg, curHdgMatchVal, matchSlope, (bDoneTurning == true)?"TRUE":"FALSE");
		////DEBUG!!
		//		}

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

			//return false;
			bResult = false;

		}
	}
	//DEBUG!!
	mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg);
	//DEBUG!!

	//04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding
	StopBothMotors();

	//return true;
	return bResult;
}




float GetHdgMatchVal(float tgt_deg, float cur_deg)
{
	//Purpose:  Compute the match ratio between two compass headings
	//Inputs:
	//	tgt_deg = float representing target heading in +/-180 range
	//	IMUHdgValDeg = float representing sensor yaw value in +/-180 deg range
	//Outputs:
	//	returns result of 1 - abs(Tgt_deg - Hdg_deg)/180, all angles in 0-360 deg range
	//Plan:
	//	Step1: convert both inputs to 0-360 deg range
	//	Step2: compute match ratio
	//Notes:
	//	formula from https://gis.stackexchange.com/questions/129954/comparing-compass-bearings

//Step1: convert both inputs to 0-360 deg range
	float tgthdg = (tgt_deg < 0) ? tgt_deg + 360 : tgt_deg;
	float curhdg = (cur_deg < 0) ? cur_deg + 360 : cur_deg;

	//Step2: compute match ratio
	float match_ratio = 1 - abs(tgthdg - curhdg) / 180;

	//DEBUG!!
		//mySerial.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio);
	//DEBUG!!
	return abs(match_ratio);
}

float UpdateIMUHdgValDeg()
{
	//Purpose: Get latest yaw (heading) value from IMU
	//Inputs: None.  This function should only be called after mpu.dmpPacketAvailable() returns TRUE
	//Outputs: 
	//	returns true if successful, otherwise false
	//	IMUHdgValDeg updated on success
	//Plan:
	//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet
	//Step2: read all available packets to get to latest data
	//Step3: update IMUHdgValDeg with latest value
	//Notes:
	//	10/08/19 changed return type to boolean
	//	10/08/19 no longer need mpuIntStatus
	//	10/21/19 completely rewritten to use Homer's algorithm
	//	05/05/20 changed return type to float vs bool.

	bool retval = false;

	int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

	if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow
	{
		// display Euler angles in degrees
		mpu.dmpGetQuaternion(&q, fifoBuffer);
		mpu.dmpGetGravity(&gravity, &q);
		mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

		//compute the yaw value
		IMUHdgValDeg = ypr[0] * 180 / M_PI;
		retval = true;
	}

	//return retval;
	return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience
}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)
{
	mpu.resetFIFO();
	delay(1);
	//int countloop = 0;

	fifoCount = mpu.getFIFOCount();
	GetPacketLoopCount = 0;

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

	//mySerial.printf("In GetCurrentFIFOPacket: after loop fifoC = %d, loop count = %d\n", fifoCount, GetPacketLoopCount);

	if (GetPacketLoopCount >= max_loops)
	{
		return 0;
	}

	//if we get to here, there should be exactly one packet in the FIFO
	//mySerial.printf("In GetCurrentFIFOPacket: before getFIFOBytes fifoC = %d\n",fifoCount);
	mpu.getFIFOBytes(data, packetSize);
	return 1;
}

//04/10/20 dummy function for now
void StepTurn(WallTrackingCases trackingside)
{
	mySerial.printf("In StepTurn(Tracking %s)\n", TrkStrArray[trackingside]);
	//RollingTurn(trackingside == TRACKING_RIGHT, true, 90.f);
	SpinTurn(trackingside == TRACKING_RIGHT, 90.f);
}


int GetTrackTurnDeg(int error, int& trkwinmult)
{
	//Purpose: Compute new tracking turn value
	//Inputs:
	//	error = int value representing difference between actual and desired 
	//			offset distance from near wall
	//	trkwinmult = int multiplication factor (tracking gain) to be applied to
	//			distance error thresholds
	//Outputs:
	//	integer representing the required heading change, in degrees

//DEBUG!!
	mySerial.printf("GetTrackTurnDeg(%d, %d)\n", error, trkwinmult);
	//DEBUG!!

	int turndeg = 0;

	if (abs(error) > m_TrkErrWinMult * VERY_FAR_AWAY_CM) //use 20-deg cut
	{
		turndeg = VERY_FAR_AWAY_TURN_DEG;
	}
	else if (abs(error) > m_TrkErrWinMult * FAR_AWAY_CM) //use 10-deg cut
	{
		turndeg = FAR_AWAY_TURN_DEG;
	}
	else if (abs(error) > m_TrkErrWinMult * CLOSE_CM) //use 5-deg cut
	{
		turndeg = CLOSE_TURN_DEG;
		m_TrkErrWinMult = WALL_TRK_ERR_WIN_MULTFACT;
	}
	else //must be close to desired offset distance. Reduce error window size
	{
		//mySerial.printf("Close to target distance - setting turndeg to zero\n");
		turndeg = 0;
		m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //go back to approach window sizes
	}

	return turndeg;
}

//added 02/16/20 for wall tracking support
int GetNearWallDistCm(bool bTrackingRight)
{
	return  bTrackingRight ? GetRightDistCm() : GetLeftDistCm();
}
#pragma endregion HDG_BASED_TURN_SUPPORT

#pragma region VL53L0X Sensor Support

bool GetRequestedVL53l0xValues(VL53L0X_REQUEST which)
{
	//Purpose: Obtain VL53L0X ToF sensor data from Teensy sensor handler
	//Inputs: 
	//	which = VL53L0X_REQUEST value denoting which combination of value to retrieve
	//		VL53L0X_CENTERS_ONLY -> Just the left/right center sensor values
	//		VL53L0X_RIGHT -> All three right sensor values, in front/center/rear order
	//		VL53L0X_LEFT -> All three left sensor values, in front/center/rear order
	//		VL53L0X_BOTH -> All six sensor values, in left/right front/center/rear order

	//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_BOTH request type

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

	Wire.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);
	I2C_writeAnything((uint8_t)which);
	Wire.endTransmission();


	//Step2: get the requested data
	int readResult = 0;
	int data_size = 0;
	switch (which)
	{
	case VL53L0X_CENTERS_ONLY:
		//just two data values needed here
		data_size = 2 * sizeof(int);
		Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(2 * sizeof(Lidar_RightCenter)));
		readResult = I2C_readAnything(Lidar_RightCenter);

		if (readResult > 0)
		{
			I2C_readAnything(Lidar_LeftCenter);
		}

		//DEBUG!!
				//mySerial.printf("VL53L0X_CENTERS_ONLY case: Got LC/RC = %d, %d\n", Lidar_LeftCenter, Lidar_RightCenter);
		//DEBUG!!

		break;
	case VL53L0X_RIGHT:
		//four data values needed here
		data_size = 3 * sizeof(int) + sizeof(float);

		//DEBUG!!
				//mySerial.printf("data_size = %d\n", data_size);
		//DEBUG!!

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

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

		//DEBUG!!
				//mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",
				//	Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, ToFSteeringVal);
		//DEBUG!!

		break;
	case VL53L0X_LEFT:
		//four data values needed here
		data_size = 3 * sizeof(int) + sizeof(float);

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

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

		//DEBUG!!
				//mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",
				//	Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal);
		//DEBUG!!

		break;
	case VL53L0X_BOTH: //added 08/05/20
		//eight data values needed here
		data_size = 6 * sizeof(int) + 2 * sizeof(float);

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

		//left side
		Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);
		readResult = I2C_readAnything(Lidar_LeftFront);

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

		//right side
		readResult = I2C_readAnything(Lidar_RightFront);

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

	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

3629

3630

3631

3632

3633

3634

3635

3636

3637

3638

3639

3640

3641

3642

3643

3644

3645

3646

3647

3648

3649

3650

3651

3652

3653

3654

3655

3656

3657

3658

3659

3660

3661

3662

3663

3664

3665

3666

3667

3668

3669

3670

3671

3672

3673

3674

3675

3676

3677

3678

3679

3680

3681

3682

3683

3684

3685

3686

3687

3688

3689

3690

3691

3692

3693

3694

3695

3696

3697

3698

3699

3700

3701

3702

3703

3704

3705

3706

3707

3708

3709

3710

3711

3712

3713

3714

3715

3716

3717

3718

3719

3720

3721

3722

3723

3724

3725

3726

3727

3728

3729

3730

3731

3732

3733

3734

3735

3736

3737

3738

3739

3740

3741

3742

3743

3744

3745

3746

3747

3748

3749

3750

3751

3752

3753

3754

3755

3756

3757

3758

3759

3760

3761

3762

3763

3764

3765

3766

3767

3768

3769

3770

3771

3772

3773

3774

3775

3776

3777

3778

3779

3780

3781

3782

3783

3784

3785

3786

3787

3788

3789

3790

3791

3792

3793

3794

3795

3796

3797

3798

3799

3800

3801

3802

3803

3804

3805

3806

3807

3808

3809

3810

3811

3812

3813

3814

3815

3816

3817

3818

3819

3820

3821

3822

3823

3824

3825

3826

3827

3828

3829

3830

3831

3832

3833

3834

3835

3836

3837

3838

3839

3840

3841

3842

3843

3844

3845

3846

3847

3848

3849

3850

3851

3852

3853

3854

3855

3856

3857

3858

3859

3860

3861

3862

3863

3864

3865

3866

3867

3868

3869

3870

3871

3872

3873

3874

3875

3876

3877

3878

3879

3880

3881

3882

3883

3884

3885

3886

3887

3888

3889

3890

3891

3892

3893

3894

3895

3896

Name: FourWD_WallE2_V7.ino

Created: 9/24/2020 11:03:59 AM

Author: FRANKNEWXPS15\Frank

#define TIMER_INT_OUTPUT_PIN 51 //scope monitor pin

#pragma region INCLUDES

#include <avr/sleep.h> //needed for sleep_enable() & sleep_cpu() functions

#include <elapsedMillis.h>

#include <PrintEx.h> //allows printf-style printout syntax

#include "MPU6050_6Axis_MotionApps_V6_12.h" //changed to this version 10/05/19

#include "I2Cdev.h" //02/19/19: this includes SBWire.h

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

#include "Adafruit_FRAM_I2C.h"

#include "RTClib.h" //07/11/20 this MUST be the #include "file" form for the "Local files override.." option to work

#include <PID.h> //moved here 02/09/19

#include <limits.h> //added 04/19/19

#pragma endregion Includes

#pragma region DEFINES

//02/29/16 hardware defines

//#define NO_MOTORS

//#define NO_LIDAR

//#define NO_PINGS

//#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_FRAM_TELEMETRY //added 11/14/18

//#define FORCE_RTC_TO_LAST_COMPILE_TIME //added 02/18/19. Forces RTC to last compile time

#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

#pragma endregion Program #Defines

#pragma region PRE_SETUP

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

Adafruit_FRAM_I2C fram = Adafruit_FRAM_I2C(); //I2C addr = 0x50 (default)

//FramPacket.h holds 'inline' definition of CFRAMStatePacket class

/*this has to come after the following lines

#include "Adafruit_FRAM_I2C.h"

Adafruit_FRAM_I2C fram = Adafruit_FRAM_I2C();

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

#include "FramPacket.h"

//tri-sensor board

RTC_DS3231 rtc; //I2C addr = 0x68

MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0)

CFRAMStatePacket FramPacket = CFRAMStatePacket(&fram); //this object is used for all FRAM transactions

const char daysOfTheWeek[7][12] = { "Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday" };//used for RTC date/time readouts

//added 01/30/17 for IR homing support

#pragma region IRHOMING

uint8_t IR_HOMING_MODULE_SLAVE_ADDR = 8; //uint8_t type reqd here for Wire.requestFrom() call

double IRHomingSetpoint, IRHomingLRSteeringVal, IRHomingOutput;//10/06/17 chg input variable name to something more descriptive

PID IRHomingPID(&IRHomingLRSteeringVal, &IRHomingOutput, &IRHomingSetpoint, 1000, 0.0, 200.0, REVERSE);//10/15/17 'final' setup

const long IR_BEAM_DETECTION_CHANNEL_MAX = 2621440;

const long IR_BEAM_DETECTION_THRESHOLD = 10000;

#pragma endregion IR Homing Parameters

#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_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", "DeadBatt", "Discharge" };

const char* LongModeStrArray[] = { "None", "Charging", "IR Homing", "Wall Following", "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 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 int MAX_AD_VALUE = 1023;

const long BATT_CHG_TIMEOUT_SEC = 36000; //10 HRS, for test only

//const long BATT_CHG_TIMEOUT_MIN = 120; //2 hours. Chg to min vs sec 01/09/18

const float DEAD_BATT_THRESH_VOLTS = 6; //added 01/24/17

//const float DEAD_BATT_THRESH_VOLTS = 6.2; //chg to 6.2 03/02/19 per https://www.fpaynter.com/2019/03/better-battery-charging-for-wall-e2/

//const float FULL_BATT_VOLTS = 8.2; //added 03/17/18. Chg to 8.2 02/24/19

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 VOLTAGE_TO_CURRENT_RATIO = 0.75f; //Volts/Amp rev 03/01/19. Used for both 'Total' and 'Run' sensors

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

const float ADC_REF_VOLTS = 3.3; //03/27/18 now using analogReference(EXTERNAL) with Teensy 3.3V connected to AREF

#pragma endregion Battery Constants

#pragma region DISTANCE_MEASUREMENT_SUPPORT

//misc LIDAR and Ping sensor parameters

const int MIN_OBS_DIST_CM = 20; //rev 04/28/17 for better obstacle handling

const int CHG_STN_AVOIDANCE_DIST_CM = 30; //added 03/11/17 for charge stn avoidance

//const int STEPTURN_DIST_CM = 80; //rev 04/28/17 to be indep of MIN_OBS_DIST_CM

//const int STEPTURN_DIST_CM = 0; //rev 05/16/20 to temporarily disable

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 MIN_PING_INTERVAL_MSEC = 200; //rev 03/12/16

//const int MIN_PING_INTERVAL_MSEC = 50; //rev 10/03/20

const int MIN_PING_INTERVAL_MSEC = 30; //rev 10/03/20

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 = 25; //04/28/19 - final value (I hope)

const int FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19

const int LR_PING_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg

const int LR_PING_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_FRONT_VARIANCE_THRESHOLD = 0; //chg to 0 09/15/18

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

//byte aLeftDist[LR_PING_DIST_ARRAY_SIZE];

//byte aRightDist[LR_PING_DIST_ARRAY_SIZE];

uint16_t aLeftDist[LR_PING_DIST_ARRAY_SIZE];

uint16_t aRightDist[LR_PING_DIST_ARRAY_SIZE];

//added 12/26/14 for wall following support

int prevleftdistval = 0;

int prevrightdistval = 0;

int prevfrontdistcm = 0;

int curMinObstacleDistance = MIN_OBS_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()

int leftdistval = 0;

int rightdistval = 0;

int frontdistval = 0;

//double frontvar = 0;

volatile double frontvar = 0; //08/11/20 now updated in timer1 ISR

#pragma endregion Distance Measurement Support

#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 MOTOR_SPEED_CAPTURE_OFFSET = 125; //added 06/28/20 for offset capture

const int MOTOR_SPEED_ADJ_FACTOR = 40; //chg to 40 at 5:55pm

const int LEFT_SPEED_COMP_VAL_FWD = 15; //left speed compensation value

const int RIGHT_SPEED_COMP_VAL_FWD = -LEFT_SPEED_COMP_VAL_FWD; //right speed compensation value

const int LEFT_SPEED_COMP_VAL_REV = 5; //left speed compensation value

const int RIGHT_SPEED_COMP_VAL_REV = -LEFT_SPEED_COMP_VAL_REV; //right speed compensation value

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

bool bIsForwardDir = true; //default is foward direction

#pragma endregion Motor Parameters

#pragma region HEADING_BASED_TURN_PARAMS

//09/11/18 new heading-based turn support constants

const int ESS_TURN_DEG = 45;

const int QUARTER_TURN_DEG = 90;

const float DEFAULT_HDG_JUMP_VAL = 20.f;

const int DEFAULT_TURN_DEG = QUARTER_TURN_DEG;

const float ROLLING_TURN_MAX_SEC_PER_DEG = 1 / 15.0; //used to limit time in rolling turns

//const int OFFSIDE_MOTOR_SPEED = MOTOR_SPEED_LOW;

//const int OFFSIDE_MOTOR_SPEED = 0; //03/04/20 debug

const int OFFSIDE_MOTOR_SPEED = 25; //03/04/20 turn debug - leave at this value for now

const int DRIVESIDE_MOTOR_SPEED_HIGH = MOTOR_SPEED_MAX;

const int DRIVESIDE_MOTOR_SPEED_HALF = MOTOR_SPEED_HALF;//added 02/16/20 for wall tracking support

const int DRIVESIDE_MOTOR_SPEED_LOW = MOTOR_SPEED_HALF;

const long MSEC_PER_HDG_CHECK = 100; //added 08/29/18

const float HDG_NEAR_MATCH_VAL = 0.9; //slow the turn down here

//const float HDG_FULL_MATCH_VAL = 0.98; //stop the turn here

const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 05/17/20

const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started

elapsedMillis sinceLastTimeCheck; //used for rolling turn timeout

#pragma endregion Heading-based turn support parameters

#pragma region VL53L0X_TOF_LIDAR_SUPPORT

//const int ToFArray_PARALLEL_FIND_Kp = 800;

//const int ToFArray_PARALLEL_FIND_Kp = 80;

//const int ToFArray_PARALLEL_FIND_Kp = 120;

const int ToFArray_PARALLEL_FIND_Kp = 200;

const int ToFArray_PARALLEL_FIND_Ki = 20; //added 09/22/20

const int ToFArray_PARALLEL_FIND_Kd = 0; //added 09/22/20

//const float ToFArray_PARALLEL_FIND_SETPOINT = 0.05; //09/22/20 moved here

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 = 0; //06/28/20

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;

//const int ToFArray_OFFSET_TRACK_Kd = 50;

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.2; //added 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 = 5.0; //just a guess

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

int Lidar_RightFront;

int Lidar_RightCenter;

int Lidar_RightRear;

int Lidar_LeftFront;

int Lidar_LeftCenter;

int Lidar_LeftRear;

elapsedMillis lastToFArrayTelemetryMsec; //used to space out telemetry prints

enum VL53L0X_REQUEST

{

VL53L0X_CENTERS_ONLY,

VL53L0X_RIGHT,

VL53L0X_LEFT,

VL53L0X_BOTH //added 08/06/20

};

#pragma endregion VL53L0X ToF LIDAR Support

//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//

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

//vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv//

#pragma region MOTOR_PINS

//09/11/20 Now using two Adafruit DRV8871 drivers for all 4 motors

const int In1_Left = 10;

const int In2_Left = 11;

const int In1_Right = 8;

const int In2_Right = 9;

#pragma endregion Motor Pin Assignments

//Laser pointer

const int RED_LASER_DIODE_PIN = 7;

//LIDAR MODE pin (continuous mode)

const int LIDAR_MODE_PIN = 4; //mvd here 01/10/18

//BATT Monitor and IR Detector pin assignments added 01/30/17

const int BATT_MON_PIN = A1;

#pragma region CHG_SUPP_PINS

//04/22/20 bugfix - RUN & TOT input definitions were reversed

//const int RUN_CURR_PIN = A8; //02/25/19 bugfix 02/28/19 name chg

//const int TOT_CURR_PIN = A9; //02/24/19 now connected to 1NA619 charge current sensor

const int RUN_CURR_PIN = A9; //connected to 1NA619 between battery and rest of robot

const int TOT_CURR_PIN = A8; //connected to 1NA619 between charge plug and battery

const int CHG_CONNECT_PIN = 23; //goes HIGH when chg cable connected

//LED en/dis output pins

//03/15/18 revised for TP5100 module

const int FIN_LED_PIN = 35;

const int _80PCT_LED_PIN = 33;

const int _60PCT_LED_PIN = 31;

const int _40PCT_LED_PIN = 29;

const int _20PCT_LED_PIN = 27;

const int CHG_LED_PIN = 25; //pwr2 signal line repl by Batt2

long chgStartMsec;//added 02/24/17

#pragma endregion Charge Control/Status Pins

//05/03/17 moved below Chg supp LED defs so can re-use them

//03/15/18 revised for TP5100 module

#pragma region BACKUP_TURN_LED_PINS

//03/19/18 new plan - use 'full/half/qtr_left, full/half/qtr_right aliases

const int FULL_LEFT_LED_PIN = FIN_LED_PIN;

const int HALF_LEFT_LED_PIN = _80PCT_LED_PIN;

const int QTR_LEFT_LED_PIN = _60PCT_LED_PIN;

const int QTR_RIGHT_LED_PIN = _40PCT_LED_PIN;

const int HALF_RIGHT_LED_PIN = _20PCT_LED_PIN;

const int FULL_RIGHT_LED_PIN = CHG_LED_PIN;

#pragma endregion Rear Bumper Display LEDs

#pragma region SPEAKER_CONSTANTS

//const int SOS_PWM_PIN = 3;

const int SOS_PWM_PIN = 2; //chg to proper pin 09/13/2020

const int DOT_MS = 200;

const int DASH_MS = 800;

const int HIGHTONE = 1000;

const int LOWTONE = 500;

#pragma endregion Speaker Constants

//const int PWRDWN_INTERRUPT_PIN = 2; //added 03/27/18

//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^//

//=================== END PIN ASSIGNMENTS ================//

//^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^//

//03/08/17 added Mode/state-specific telemetry header strings

#pragma region TELEMETRYSTRINGS

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

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

//const String WallFollowTelemStr = "DateTime\tBatt\tMode\tTrack\tLeft\tRight\tFront\tVar\tLSpd\tRSpd";

//const char* WallFollowTelemStr = "DateTime\tBatt\tMode\tTrack\tLeft\tRight\tFront\tVar\tLSpd\tRSpd";

const char* WallFollowTelemStr = "Msec\tMode\tTrack\tFront\tCtr\tRear\tSteer\tOutput\tLSpd\tRSpd"; //09/01/20 PID tuning

//const String WallFollowTelemStr = "NewDist\tOldDist\tBmean\tBvar\tOldImean\tOldIvar\tOldD^2\tNewD^2\tImean\tIvar"; //04/17/19 added for Ivar debug

//const String WallFollowTelemStr = "New_D\tOld_D\tB_mean\tB_var\told_Im\told_Ivar\told_d^2\tnew_d^2\tI_mean\tIm_sq\toldIm_sq\tI_var\tB_uSec\tI_uSec\n";

//const String ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tATotI\tARunI\tChrging\n"; //rev 02/28/19 to chg 'BattI' to 'TotI', added 'RunI', ChgI

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

#pragma endregion Mode-specific telemetry header strings

#pragma region LOOP_VARS

//loop() variables

double deltaD = 0;

double olddist = 0;

double newdist = 0;

boolean bStuck = false;

int leftspeednum = MOTOR_SPEED_HALF;

int rightspeednum = MOTOR_SPEED_HALF;

elapsedMillis sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec

NavCases NavCase = NAV_WALLTRK;

WallTrackingCases TrackingCase = TRACKING_NEITHER; //added 01/05/16

WallTrackingCases PrevTrackingCase = TRACKING_LEFT;

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 last_incvar = 0;

double last_incmean = 0;

elapsedMillis lastHomingTelemetryMsec; //used to space out telemetry prints

#pragma endregion Loop Variables

#pragma region I2C_VARS

#pragma endregion I2C related parameters

#pragma region TRISENSOR PARAMS

//FRAM constants/params

const int NUM_FRAM_BYTES_TO_CLEAR = 2000;

const int FIRST_TIME_STORAGE_ADDR = 2;//addr 0/1 reserved for nextFramWriteAddr

const int NEXTFRAMWRITEADDR_FRAMSTORAGELOCATION = 0;

const int NUM_TIMES_TO_DISPLAY = 10;

//const unsigned long FRAM_WRITE_INTERVAL_MSEC = 60000; //one minute

const unsigned long FRAM_WRITE_INTERVAL_MSEC = 6000; //one minute

int nextFramWriteAddr = FIRST_TIME_STORAGE_ADDR;

elapsedMillis sinceLastFRAMWriteMsec; //added 09/19/18

#pragma endregion TRISENSOR

#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()

const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param

bool bFirstTime = true;

#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19

#pragma endregion MPU6050 Support

#pragma region FRAM/RTC Support

//RTC/FRAM/MPU6050 status flags

bool RTC_Avail = true;

bool bRTCLostPower = false; //added 10/17/18

bool bFRAMReady = true;

#pragma endregion FRAM/RTC Support

#pragma region WALL_FOLLOW_SUPPORT

//12/05/19 defiine 'TURNDIR_CCW' & 'TURN_CW' identifiers for better readability. Now I can call

//RollingTurn(TURNDIR_CCW,...) and RollingTurn(TURNDIR_CW) instead of just RollingTurn(true/false)

const bool TURNDIR_CCW = true;

const bool TURNDIR_CW = false;

const int DESIRED_WALL_OFFSET_DIST_CM = 30;

const int ROLLING_TURN_RADIUS_CM = 5;

//const int NINETY_DEG_FUDGE_FACTOR_CM = 5; //added 04/17/20

const int NINETY_DEG_FUDGE_FACTOR_CM = 10; //added 04/17/20

const int WALL_APPR_ERR_WIN_MULTFACT = 2; //added 08/12/19

const int WALL_APPR_MAX_AGGREGATE_CUT = 30; //added 05/09/20

const int WALL_TRK_ERR_WIN_MULTFACT = 1; //added 08/12/19

const int VERY_FAR_AWAY_CM = 5;

const int FAR_AWAY_CM = 2;

const int CLOSE_CM = 1;

const int VERY_FAR_AWAY_TURN_DEG = 20;

const int FAR_AWAY_TURN_DEG = 10;

const int CLOSE_TURN_DEG = 5;

const int TURN_INCREMENT_DEG = 10;

int m_PrevHdgCutDeg = 0;

bool m_PrevHdgCutDir = TURNDIR_CW; //CW

int m_PrevLeftDistCm = 0;

int m_PrevRightDistCm = 0;

int m_AggregateCutDeg = 0; //added 05/09/20

elapsedMillis sinceLastHdgCutUpdateMsec; //added 10/15/18 to replace lastmillisec

int m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //used to increase close in response

bool bIsFirst180 = true; //added 01/25/20 for boomerang mode tracking

elapsedMillis mSecSinceLastParDistChk = 0; //added 02/14/20 to steepen distance/measurement slope

#pragma endregion Wall Following Support

#pragma endregion PRE_SETUP

#pragma region TIMER_ISR

//08/10/20 added timer ISR

volatile bool bIsStuck = false;

volatile bool bIsStuck_Slow = false; //08/12/20 added for half-speed offset capture operations

volatile bool bObstacleAhead = false;

volatile uint32_t Last_ISR_Msec;

volatile bool bTimeForNavUpdate = false;

//DEBUG!!

//uint32_t elapsedUsec = micros()-Last_ISR_Msec;

//mySerial.printf("MSec\tUSec\tFdist\tFvar\n");

//mySerial.printf("%lu\t%lu\t%d\t%2.3f\n", millis(), elaspsed, frontdist, frontvar);

//DEBUG!!

ISR(TIMER5_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED)

{

//digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);

//delayMicroseconds(30);

//digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);

bTimeForNavUpdate = true;

#ifndef NO_STUCK

uint16_t frontdist = GetFrontDistCm();

frontvar = CalcDistArrayVariance(frontdist, aFrontDist);

bIsStuck = frontvar < STUCK_FRONT_VARIANCE_THRESHOLD;

bIsStuck_Slow = frontvar < STUCK_FRONT_VARIANCE_THRESHOLD / 2;

bObstacleAhead = frontdist < MIN_OBS_DIST_CM;

#endif // !NO_STUCK

}

#pragma endregion TIMER_ISR

#pragma region WALL_OFFSET_TRACKING

const int WALL_OFFSET_TRACK_Kp = 100;

const int WALL_OFFSET_TRACK_Ki = 0;

const int WALL_OFFSET_TRACK_Kd = 0;

const double WALL_OFFSET_TRACK_SETPOINT_LIMIT = 0.3;

double WallTrackSteerVal, WallTrackOutput, WallTrackSetPoint;

PID WallTrackPID(&WallTrackSteerVal, &WallTrackOutput, &WallTrackSetPoint, WALL_OFFSET_TRACK_Kp, 0.0, 0.0, REVERSE);

#pragma endregion Wall Offset Tracking Support

#pragma region SETUP

void setup()

{

Serial.begin(115200);

delay(1000);

FramPacket = CFRAMStatePacket(&fram); //09/27/18 comes after Serial.begin(). Calls Wire.begin() & fram.begin()

#pragma region RTC

if (rtc.begin()) //02/19/19 this now returns FALSE if RTC doesn't respond

{

Serial.println("Found RTC...");

delay(100);

bRTCLostPower = rtc.lostPower(); //added 10/17/18

mySerial.printf("rtc.lostPower() reports %d\n", bRTCLostPower);

if (rtc.lostPower())

{

Serial.println("RTC lost power. Setting RTC to last compile time");

rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));// sets RTC to last compile date/time

}

#ifdef FORCE_RTC_TO_LAST_COMPILE_TIME

Serial.println("Forcing RTC to last compile time");

rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));// sets RTC to last compile date/time

#endif

DateTime now = rtc.now();

char buffer[100];

memset(buffer, '\0', 100);

mySerial.printf("Retrieving Date/Time from RTC with rtc.now() = %ld\n", now.unixtime());

GetDayDateTimeStringFromDateTime(now, buffer);

Serial.print("Date/Time: "); Serial.println(buffer);

RTC_Avail = true;

}

else

{

Serial.println("Couldn't find RTC. Real-time clock functions won't be available");

RTC_Avail = false; //use this instead

}

if (RTC_Avail && rtc.lostPower())

{

DateTime Comp_dt = DateTime(F(__DATE__), F(__TIME__)); //__DATE__ & __TIME__ are environment variables)

uint8_t mydayofweek = Comp_dt.dayOfTheWeek(); //returns 0 for Sunday, 6 for Saturday dayOfTheWeek

int mymonth = Comp_dt.month();

int myday = Comp_dt.day();

int myyear = Comp_dt.year();

int myhour = Comp_dt.hour();

int mymin = Comp_dt.minute();

int mysec = Comp_dt.second();

long unixtime = Comp_dt.unixtime();

char* dayofweek = (char*)daysOfTheWeek[mydayofweek];

Serial.print("day of week value = "); Serial.println(mydayofweek);

Serial.print("day of week value = "); Serial.println(Comp_dt.dayOfTheWeek());

//mySerial.printf("RTC lost power - setting to datetime of last compile %ld (%s %4d/%02d/%02d at %02d:%02d:%02d)\n",

// dt.unixtime(), daysOfTheWeek[dt.dayOfTheWeek()], dt.year(), dt.month(), dt.day(),

// dt.hour(), dt.minute(), dt.second());

mySerial.printf("RTC lost power - setting to datetime of last compile %ld (%s %4d/%02d/%02d at %02d:%02d:%02d)\n",

unixtime, dayofweek, myyear, mymonth, myday, myhour, mymin, mysec);

rtc.adjust(Comp_dt);

}

#pragma endregion RTC

#pragma region FRAM

if (fram.begin())// you can stick a non-default i2c addr in here, e.g. begin(0x51);

{

Serial.println("Found I2C FRAM");

bFRAMReady = true;

}

else

{

Serial.println("I2C FRAM not identified ... check your connections?\r\n");

Serial.println("Will continue in case this processor doesn't support repeated start\r\n");

bFRAMReady = false;

}

// Read the stored nextFramWriteAddr value from the first two bytes, and the last stored packet

if (bFRAMReady)

{

fram.FRAM_I2C_readAnything(NEXTFRAMWRITEADDR_FRAMSTORAGELOCATION, nextFramWriteAddr);

mySerial.printf("Read %d (nextFramWriteAddr) from FRAM address %d\n",

nextFramWriteAddr, NEXTFRAMWRITEADDR_FRAMSTORAGELOCATION);

//if any packets have been written since last FRAM clear, display last-written one

if (nextFramWriteAddr > FIRST_TIME_STORAGE_ADDR)

{

int FramPacketSize = CFRAMStatePacket::packet_size;

mySerial.printf("printing packet written to FRAM at %d\n", nextFramWriteAddr - FramPacketSize);

FramPacket.Read(nextFramWriteAddr - FramPacketSize);

FramPacket.Print();

}

#pragma endregion FRAM

#pragma region MPU6050

Serial.println(F("Initializing MPU6050 ..."));

mpu.initialize();

// verify connection

Serial.println(F("Testing device connections..."));

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

float StartSec = 0; //used to time MPU6050 init

Serial.println(F("Initializing DMP..."));

devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if successful)

if (devStatus == 0)

{

//12/06/19 don't need to do this every time. Above cal constants should work.

//// Calibration Time: generate offsets and calibrate our MPU6050

//mpu.CalibrateAccel(6);

//mpu.CalibrateGyro(6);

//Serial.println();

//mpu.PrintActiveOffsets();

// turn on the DMP, now that it's ready

Serial.println(F("Enabling DMP..."));

mpu.setDMPEnabled(true);

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

bMPU6050Ready = true;

StartSec = millis() / 1000.f;

mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", StartSec);

}

else

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

Serial.print(F("DMP Initialization failed (code "));

Serial.print(devStatus);

Serial.println(F(")"));

bMPU6050Ready = false;

}

#pragma endregion MPU6050

//DEBUG!!

//print out Homing PID parameters

mySerial.printf("IRHomingPID Parameters (Kp,Ki,Kd,DIR) = (%2.2f,%2.2f,%2.2f,%d)\n",

IRHomingPID.GetKp(), IRHomingPID.GetKi(), IRHomingPID.GetKd(), IRHomingPID.GetDirection());

//DEBUG!!

#pragma region L/R/FRONT DISTANCE ARRAYS

InitFrontDistArray(); //08/12/20 code extracted to fcn so can call elsewhere

//04/28/19 put aLeftDist[], aRightDist[] arrays back in

for (int i = 0; i < LR_PING_DIST_ARRAY_SIZE; i++)

{

aLeftDist[i] = random(0, 200);

aRightDist[i] = random(0, 200);

////DEBUG!!

// mySerial.printf("aLeft/RightDist[%d] = %d, %d\n",i, aLeftDist[i], aRightDist[i]);

////DEBUG!!

}

#pragma endregion L/R/FRONT DISTANCE ARRAYS

#pragma region I/O PIN SETUP

//09/11/20 now using two DRV8871 motor drivers for all four motors

pinMode(In1_Left, OUTPUT);

pinMode(In2_Left, OUTPUT);

pinMode(In1_Right, OUTPUT);

pinMode(In2_Right, OUTPUT);

//Laser pointer

pinMode(RED_LASER_DIODE_PIN, OUTPUT);

//01/30/17 added for chg stn supp

pinMode(BATT_MON_PIN, INPUT);//Battery voltage monitor input

analogReference(EXTERNAL); //03/18/18 now using external 3.3V ref with 5.84V zener voltage shifter

//02/28/19 revised to repurpose CHG_SIG_PIN AND CHG_FIN_PIN for current measurement

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

pinMode(CHG_CONNECT_PIN, INPUT_PULLUP); //goes HIGH when chg cable connected

//Charge status LED en/dis pins

pinMode(FIN_LED_PIN, OUTPUT);

pinMode(_80PCT_LED_PIN, OUTPUT);

pinMode(_60PCT_LED_PIN, OUTPUT);

pinMode(_40PCT_LED_PIN, OUTPUT);

pinMode(_20PCT_LED_PIN, OUTPUT);

pinMode(CHG_LED_PIN, OUTPUT);

//added 03/27/18

//pinMode(SOS_PWM_PIN, OUTPUT);

//LIDAR mode pin

pinMode(LIDAR_MODE_PIN, INPUT); // Set LIDAR input monitor pin

#pragma endregion I/O PIN SETUP

//11/14/18 added this section for distance printout only

//08/06/20 revised for VL53L0X support

#ifdef DISTANCES_ONLY

sinceLastNavUpdateMsec = 0;

digitalWrite(RED_LASER_DIODE_PIN, HIGH); //enable the front laser dot

mySerial.printf("\n------------ DISTANCES ONLY MODE!!! -----------------\n\n");

int i = 0; //added 09/20/20 for in-line header display

mySerial.printf("Msec\tLFront\tLCenter\tLRear\tRFront\tRCenter\tRRear\n");

while (true)

{

//if (sinceLastNavUpdateMsec > MIN_PING_INTERVAL_MSEC)

//{

// sinceLastNavUpdateMsec -= MIN_PING_INTERVAL_MSEC;

//09/20/20 re-display the column headers

//if (i % 20 == 0)

//{

//}

GetRequestedVL53l0xValues(VL53L0X_BOTH);

//mySerial.printf("Both: %lu\t%d\t%d\t%d\t%d\t%d\t%d\n",

// millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

// Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);

mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n",

millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);

//i++;

//}

}

#else

prevleftdistval = GetAvgLeftDistCm();

prevrightdistval = GetAvgRightDistCm();

prevfrontdistcm = GetFrontDistCm();

#endif // DISTANCES_ONLY

#pragma region TIMER_INTERRUPT

//set timer5 interrupt at (1000/MIN_PING_INTERVAL_MSEC)Hz 5Hz a/o 08/10/20

//09/18/20 can't use Timer1 cuz doing so conflicts with PWM on pins 10-12

cli();//stop interrupts

TCCR5A = 0;// set entire TCCR5A register to 0

TCCR5B = 0;// same for TCCR5B

TCNT5 = 0;//initialize counter value to 0

// set compare match register for 5hz increments

OCR5A = 3124;// = (16*10^6) / (5*1024) - 1 (must be <65536)

TCCR5B |= (1 << WGM52);// turn on CTC mode

TCCR5B |= (1 << CS52) | (1 << CS50);// Set CS10 and CS12 bits for 1024 prescaler

TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

sei();//allow interrupts

#pragma endregion TIMER_INTERRUPT

pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);

#pragma region POST

//Check battery voltage

mySerial.printf("Checking Battery Voltage...\n");

mySerial.printf("Battery voltage = %2.3f\n", GetBattVoltage());

#ifndef NO_POST

//Power On Self-Test (POST)

//Check speaker

mySerial.printf("Checking Speaker...\n");

tone(SOS_PWM_PIN, HIGHTONE, DOT_MS); //returns immediately

delay(DOT_MS); //delay for LED viewing

tone(SOS_PWM_PIN, LOWTONE, DASH_MS); //returns immediately

delay(DOT_MS); //delay for LED viewing

tone(SOS_PWM_PIN, HIGHTONE, DOT_MS); //returns immediately

delay(DOT_MS); //delay for LED viewing

tone(SOS_PWM_PIN, LOWTONE, DASH_MS); //returns immediately

//Turn on LASER

mySerial.printf("Checking Laser pointer\n");

digitalWrite(RED_LASER_DIODE_PIN, HIGH);

delay(1000);

digitalWrite(RED_LASER_DIODE_PIN, LOW);

delay(1000);

digitalWrite(RED_LASER_DIODE_PIN, HIGH);

delay(1000);

digitalWrite(RED_LASER_DIODE_PIN, LOW);

//check for reasonable LIDAR value

#ifndef NO_LIDAR

mySerial.printf("LIDAR reports %d cm\n", GetFrontDistCm());

#else

mySerial.printf("LIDAR Disabled\n");

#endif // !NO_LIDAR

// mySerial.printf("Checking Left & Right Distance Sensors...\n");

//#ifndef NO_PINGS

// mySerial.printf("Left\tRight\n");

// for (size_t i = 0; i < 10; i++)

// {

// mySerial.printf("%d\t%d\n", GetLeftDistCm(), GetRightDistCm());

// }

//#else

// mySerial.printf("Distance Sensors Disabled\n");

//#endif // !NO_LIDAR

//set all chg status LEDs OFF (HIGH)

digitalWrite(FIN_LED_PIN, HIGH);

digitalWrite(_80PCT_LED_PIN, HIGH);

digitalWrite(_60PCT_LED_PIN, HIGH);

digitalWrite(_40PCT_LED_PIN, HIGH);

digitalWrite(_20PCT_LED_PIN, HIGH);

digitalWrite(CHG_LED_PIN, HIGH);

delay(1000);

//set all chg status LEDs ON (LOW)

digitalWrite(_40PCT_LED_PIN, LOW);

digitalWrite(_20PCT_LED_PIN, LOW);

digitalWrite(CHG_LED_PIN, LOW);

digitalWrite(_60PCT_LED_PIN, LOW);

digitalWrite(_80PCT_LED_PIN, LOW);

digitalWrite(FIN_LED_PIN, LOW);

//Disable LEDs in sequence from center out

delay(500);

digitalWrite(_40PCT_LED_PIN, HIGH);

digitalWrite(_60PCT_LED_PIN, HIGH);

delay(100);

digitalWrite(_20PCT_LED_PIN, HIGH);

digitalWrite(_80PCT_LED_PIN, HIGH);

delay(100);

digitalWrite(CHG_LED_PIN, HIGH);

digitalWrite(FIN_LED_PIN, HIGH);

delay(500);

//Enable LEDs in sequence from outside in

digitalWrite(CHG_LED_PIN, LOW);

digitalWrite(FIN_LED_PIN, LOW);

delay(100);

digitalWrite(_20PCT_LED_PIN, LOW);

digitalWrite(_80PCT_LED_PIN, LOW);

delay(100);

digitalWrite(_40PCT_LED_PIN, LOW);

digitalWrite(_60PCT_LED_PIN, LOW);

delay(500);

//Disable LEDs in sequence from center out

digitalWrite(_40PCT_LED_PIN, HIGH);

digitalWrite(_60PCT_LED_PIN, HIGH);

delay(100);

digitalWrite(_20PCT_LED_PIN, HIGH);

digitalWrite(_80PCT_LED_PIN, HIGH);

delay(100);

digitalWrite(CHG_LED_PIN, HIGH);

digitalWrite(FIN_LED_PIN, HIGH);

//10/08/17 make sure motors control lines are all in STOP state

StopLeftMotors();

StopRightMotors();

//run through motor test sequence

//10/06/17 added check for charger connection

//if (!IsChargerConnected())

//{

// //fwd 1 sec

// SetLeftMotorDir(FWD_DIR);

// SetRightMotorDir(FWD_DIR);

// RunBothMotorsMsec(1000, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //run for 1 sec

// //reverse 1 sec

// SetLeftMotorDir(REV_DIR);

// SetRightMotorDir(REV_DIR);

// RunBothMotorsMsec(1000, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //run for 1 sec

// StopBothMotors();

//}

//08/09/20 modified to use DRV8871 motor driver

if (!IsChargerConnected())

{

//fwd 1 sec

RunBothMotorsMsec(true, 1000, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //run for 1 sec

//reverse 1 sec

RunBothMotorsMsec(false, 1000, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //run for 1 sec

StopBothMotors();

}

#else

mySerial.printf("\n*********** POST checks Skipped!!********************\n\n");

#endif // !NO_POST

#pragma endregion POST

//09/20/20 have to do this for parallel finding to go the right way

mySerial.printf("Initializing Left & Right Distance Arrays...\n");

#ifndef NO_PINGS

mySerial.printf("Left\tRight\n");

for (size_t i = 0; i < LR_PING_DIST_ARRAY_SIZE; i++)

{

int leftdist = GetLeftDistCm();

int rightdist = GetRightDistCm();

mySerial.printf("%d\t%d\n", leftdist, rightdist);

UpdateLRDistanceArrays(leftdist, rightdist);

}

#else

mySerial.printf("Distance Sensors Disabled\n");

#endif // !NO_LIDAR

////01/26/15 start the robot going straight

MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

//04/13/20 initialize just before loop()

leftdistval = GetLeftDistCm();

rightdistval = GetRightDistCm();

frontdistval = GetFrontDistCm();

sinceLastNavUpdateMsec = 0; //added 10/15/18

sinceLastFRAMWriteMsec = 0; //added 09/19/18

mySerial.printf("sinceLastNavUpdateMsec in setup() = %lu\n", (uint32_t)sinceLastNavUpdateMsec);

#pragma endregion Setup

//09/24/20 set WallOffsetTrackingPID parameters here

WallTrackPID.SetTunings(WALL_OFFSET_TRACK_Kp, WALL_OFFSET_TRACK_Ki, WALL_OFFSET_TRACK_Kd); //try more aggresive than capture, but less than parallel rotate

WallTrackPID.SetOutputLimits(-MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);

WallTrackSetPoint = DESIRED_WALL_OFFSET_DIST_CM * 10; //now track the desired offset distance, in mm

mySerial.printf("WallTrackPID Parameters (Kp,Ki,Kd) = (%2.f,%2.f,%2.f)\n",

WallTrackPID.GetKp(), WallTrackPID.GetKi(), WallTrackPID.GetKd());

WallTrackPID.SetMode(AUTOMATIC);

mySerial.printf("mSec\tFront\tCenter\tRear\tSteer\tSetPt\tOut\tleftSpd\tRightSpd\n");

MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);

CaptureWallOffset(TRACKING_LEFT, DESIRED_WALL_OFFSET_DIST_CM + ROLLING_TURN_RADIUS_CM);

//delay for 2 sec, but check for user input during delay.

unsigned long now = millis();

while (millis()-now < 2000)

{

CheckForUserInput();

}

TrackLeftWall(DESIRED_WALL_OFFSET_DIST_CM); //infinite loop

} //setup

int ComputeCount = 0;

const int ComputeSkipsPerHeaderPrint = 20;

void loop()

{

CheckForUserInput();

GetRequestedVL53l0xValues(VL53L0X_LEFT);

WallTrackSteerVal = LeftSteeringVal;

//09/25/20 need a positive offset when ctr dist < desired. 4cm diff ->> 0.4 ->> approx 20 deg

WallTrackSetPoint = (double)(10*DESIRED_WALL_OFFSET_DIST_CM - Lidar_LeftCenter) / 100.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)

if (WallTrackPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value

{

digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);

delayMicroseconds(10);

digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);

leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

mySerial.printf("%lu\t%d\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%d\t%d\n", millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, WallTrackSteerVal,

WallTrackSetPoint, WallTrackOutput, leftspeednum, rightspeednum);

MoveAhead(leftspeednum, rightspeednum);

ComputeCount++;

if (ComputeCount > ComputeSkipsPerHeaderPrint)

{

mySerial.printf("mSec\tFront\tCenter\tRear\tSteer\tSetPt\tOut\tleftSpd\tRightSpd\n");

ComputeCount = 0;

}

//06/18/20 Rewritten to utilize VL53L0X ToF sensor array

void TrackLeftWall(int tgt_offset)

{

WallTrackPID.SetTunings(WALL_OFFSET_TRACK_Kp, WALL_OFFSET_TRACK_Ki, WALL_OFFSET_TRACK_Kd); //try more aggresive than capture, but less than parallel rotate

WallTrackPID.SetOutputLimits(-MOTOR_SPEED_QTR, MOTOR_SPEED_QTR); //give PID full rein on motor speeds

mySerial.printf("WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (%2.f,%2.f,%2.f)\n",

WallTrackPID.GetKp(), WallTrackPID.GetKi(), WallTrackPID.GetKd());

while (true)

{

GetRequestedVL53l0xValues(VL53L0X_LEFT);

WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5

//at 20mm from tgt offset, setpoint will be +/-0.2

WallTrackSetPoint = (float)(10 * tgt_offset - Lidar_LeftCenter) / 100.f; //10/04/20 positive value drives robot toward wall

if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;

if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)

if (WallTrackPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value

{

leftspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

rightspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

MoveAhead(leftspeednum, rightspeednum);

mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\n", millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, WallTrackSteerVal, WallTrackOutput,

WallTrackSetPoint, leftspeednum, rightspeednum);

}

//01/30/17 added for manual remote control via Wixel

CheckForUserInput();

}

#pragma region TIME/DATE FUNCTIONS

//02/18/19 copied from RTC Test project, and used to remove dependence on Time/TimeLib libraries

void GetDayDateTimeStringFromDateTime(DateTime dt, char* bufptr)

{

int mydayofweek = dt.dayOfTheWeek();

int myday = dt.day();

int mymonth = dt.month();

int myyear = dt.year();

int myhour = dt.hour();

int mymin = dt.minute();

int mysec = dt.second();

char* dayofweek = (char*)daysOfTheWeek[mydayofweek];

//now concatenate everything into the provided buffer

sprintf(bufptr, "%s %4d/%02d/%02d at %02d:%02d:%02d", dayofweek, mymonth, myday, myyear, myhour, mymin, mysec);

}

#pragma endregion Time & Date Support Functions

#pragma region DISTANCE_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!!

//mySerial.printf("i = %d, newval = %d, aFrontdist[%d] = %d\n", i, newval, i, aFrontDist[i]);

//DEBUG!!

if (bgoingUp)

{

if (newval < MAX_FRONT_DISTANCE_CM - bumpval) //don't want newval > MAX_FRONT_DISTANCE_CM

{

newval += bumpval;

}

else

{

bgoingUp = false;

}

else

{

if (newval > bumpval) //don't want newval < 0

{

newval -= bumpval;

}

else

{

bgoingUp = true;

}

//04/19/19 init last_incmean & last_incvar 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

}

last_incmean = (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] - last_incmean) * (aFrontDist[i] - last_incmean);

}

last_incvar = sumsquares / FRONT_DIST_ARRAY_SIZE;

mySerial.printf("aFrontDist Init: last_incmean = %3.2f, last_incvar = %3.2f\n", last_incmean, last_incvar);

}

float GetAvgFrontDistCm()

{

int dist = 0;

for (size_t i = 0; i < 3; i++)

{

dist += GetFrontDistCm();

delay(50);

//delay(10);

}

return dist / 3;

}

float GetAvgRightDistCm()

{

//Notes:

// 04/09/20 revised to compute proper running average of

// latest LR_PING_AVG_WINDOW_SIZE ping measurements

//DEBUG!!

//int totdist = 0;

//for (size_t i = 0; i < LR_PING_AVG_WINDOW_SIZE; i++)

//{

// totdist += aRightDist[i];

// //mySerial.printf("dist/total = %d\t%d\n", aRightDist[i], totdist);

//}

//float avg = (float)totdist / (float)LR_PING_AVG_WINDOW_SIZE;

//return avg;

//DEBUG!!

int rightavgdist_cm = 0;

for (int validx = 0; validx < LR_PING_AVG_WINDOW_SIZE; validx++)

{

rightavgdist_cm += aRightDist[LR_PING_DIST_ARRAY_SIZE - 1 - validx];

}

float avg = (float)rightavgdist_cm / (float)LR_PING_AVG_WINDOW_SIZE;

return avg;

}

float GetAvgLeftDistCm()

{

//Notes:

// 04/09/20 revised to compute proper running average of

// latest LR_PING_AVG_WINDOW_SIZE ping measurements

//int totdist = 0;

//for (size_t i = 0; i < LR_PING_AVG_WINDOW_SIZE; i++)

//{

// totdist += aLeftDist[i];

//}

//float avg = (float)totdist / (float)LR_PING_AVG_WINDOW_SIZE;

int leftavgdist_cm = 0;

for (int validx = 0; validx < LR_PING_AVG_WINDOW_SIZE; validx++)

{

leftavgdist_cm += aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1 - validx];

}

float avg = (float)leftavgdist_cm / (float)LR_PING_AVG_WINDOW_SIZE;

return avg;

}

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

//mySerial.printf("Front Dist = %d at %lu mSec\n", LIDARdistCm, millis());

//chk for erroneous reading

if (LIDARdistCm == 0)

{

mySerial.printf("%lu: Error in GetFrontDistCm()\n", millis());

//replace with average of last three readings from aFrontDist

int avgdist = 0;

for (int i = 0; i < FRONT_DIST_AVG_WINDOW_SIZE; i++)

{

avgdist += aFrontDist[FRONT_DIST_ARRAY_SIZE - 1 - i];

}

avgdist = avgdist / FRONT_DIST_AVG_WINDOW_SIZE;

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

}

//08/09/20 added no_param version f/u/by blocking functions like IRHomeToChgStn() and RotateToParallelOrientation()

double CalcDistArrayVariance()

{

frontdistval = GetFrontDistCm();

return CalcDistArrayVariance(frontdistval, aFrontDist);

}

double CalcDistArrayVariance(unsigned long newdistval, uint16_t* aDistArray)

{

//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 = last_incmean - (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 = last_incmean * last_incmean;

double inc_mean_squared = inc_mean * inc_mean;

double inc_var = last_incvar + ((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:

//mySerial.printf("%lu\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\n", millis(),

// newdistval, oldestDistVal, last_incmean, last_incvar, inc_var);

//DEBUG!!

last_incvar = inc_var; //save for next time

last_incmean = 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_PING_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_PING_DIST_ARRAY_SIZE - 1].

//Notes:

//DEBUG!!

mySerial.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_PING_DIST_ARRAY_SIZE - 1; i++)

//for (int i = 0; i < DIST_ARRAY_SIZE; i++)

{

aRightDist[i] = aRightDist[i + 1];

aLeftDist[i] = aLeftDist[i + 1];

}

//Step 2: Place the latest reading at [DIST_ARRAY_SIZE - 1].

aRightDist[LR_PING_DIST_ARRAY_SIZE - 1] = rightdistval;

aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1] = leftdistval;

}

#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

//mySerial.printf("InMoveAhead(%d,%d)\n", leftspeednum, rightspeednum);

//Step 1: Set forward direction and speed for both wheels

SetLeftMotorDirAndSpeed(true, leftspeednum);

SetRightMotorDirAndSpeed(true, rightspeednum);

}

//09/08/10 modified for DRV8871 motor driver

void StopLeftMotors()

{

analogWrite(In1_Left, MOTOR_SPEED_OFF);

analogWrite(In2_Left, MOTOR_SPEED_OFF);

}

void StopRightMotors()

{

analogWrite(In1_Right, MOTOR_SPEED_OFF);

analogWrite(In2_Right, MOTOR_SPEED_OFF);

}

//09/08/20 added bool bisFwd param for DRV8871 motor driver

void RunBothMotors(bool bisFwd, int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors at left/rightspeednum speeds

//Inputs:

// speednum = speed value (0 = OFF, 255 = full speed)

//Outputs: Both Motors run for timesec seconds at speednum speed

//Plan:

// Step 1: Apply drive to both wheels

//Notes:

// 01/14/15 - added left/right speed offset for straightness compensation

// 01/22/15 - added code to restrict fast/slow values

// 01/24/15 - revised for continuous run - no timing

// 01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()

// 12/07/15 - chg args from &int to int

//Step 1: Apply drive to both wheels

////DEBUG!!

// mySerial.printf("In RunBothMotors(%d,%d)\n", leftspeednum, rightspeednum);

////DEBUG!!

SetLeftMotorDirAndSpeed(bisFwd, leftspeednum);

SetRightMotorDirAndSpeed(bisFwd, rightspeednum);

}

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

}

//11/25/15 added for symmetry ;-).

void StopBothMotors()

{

StopLeftMotors();

StopRightMotors();

}

//01/10/18 reverted to regular ping(). median distance function takes too long

int GetLeftDistCm()

{

//Serial.print("LeftPing\t"); Serial.println(millis());

//Notes:

// 04/30/17 added limit detection/correction

// 07/20/20 rewritten for VL53L0X vs 'ping' sensors

GetRequestedVL53l0xValues(VL53L0X_LEFT);

int distCm = round((float)Lidar_LeftCenter / 10.f); //try to make this accurate

return distCm;

}

//06/17/20 rewritten for VL53L0X sensor

int GetRightDistCm()

{

//Purpose: Get right center VL53L0X distance in Cm

//Notes:

// 06/17/20: Copied from 'ping' version and adapted for VL53L0X

//DEBUG!!

//unsigned long now = millis();

//DEBUG!!

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

int distCm = round((float)Lidar_RightCenter / 10.f); //try to make this accurate

//DEBUG!!

//mySerial.printf("GetRightDistCm() returned %d in %lu Msec\n", distCm, millis() - now);

//DEBUG!!

return distCm;

}

void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)

{

//mySerial.printf("SetLeftMotorDirAndSpeed(%d,%d)\n", bIsFwd, speed);

#ifndef NO_MOTORS

if (bIsFwd)

{

digitalWrite(In1_Left, LOW);

analogWrite(In2_Left, speed);

//mySerial.printf("In SetLeftMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

}

else

{

digitalWrite(In2_Left, LOW);

analogWrite(In1_Left, speed);

}

#endif // !NO_MOTORS

}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)

{

//mySerial.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",

// (bIsFwd == true) ? "true" : "false", speed);

#ifndef NO_MOTORS

if (bIsFwd)

{

digitalWrite(In1_Right, LOW);

analogWrite(In2_Right, speed);

}

else

{

digitalWrite(In2_Right, LOW);

analogWrite(In1_Right, speed);

}

#endif // !NO_MOTORS

}

#pragma endregion Motor Support Functions

#pragma region CHARGE SUPPORT

bool IsIRBeamAvail()

{

//Purpose: Determine whether or not an IR beam is available for homing

//Inputs: call from loop()

//Outputs: true if an IR beam is detected, false otherwise

//Plan:

// Step1: Get analog levels from all 4 IR detectors

// Step2: return true if any of the 4 show detection level

//Notes:

// Might need some hysteresis here to avoid toggling in and out of the TRACKING_IRBEAM mode

// 02/21/17 read each det 3 times. If any sum is less than 3 X threshold, return true. Otherwise return false

// 10/15/17 rewritten to use info from IR Demod Module

// 04/05/20 revised to incorporate changes from 'I2C_Master_Tut5.ino'

//get latest info from IR Demod Module

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

float total = Fin1 + Fin2;

////DEBUG!!

// mySerial.printf("Fin1 = %ld, Fin2 = %ld, SteeringValue = %3.2f\n", Fin1, Fin2, SteeringValue);

////DEBUG!!

return (total > IR_BEAM_DETECTION_THRESHOLD);

}

float GetTotalAmps()

{

//Purpose: Get total current in amps

//Inputs:

// Voltage on TOT_CURR_PIN is approximately Ichg*2 Amps

// VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio

//Outputs:

// returns total robot current (chg current plus running current)

//Notes:

// 02/28/18 chg name from GetBattChgAmps() to GetTotalAmps()

int ITotAnalogReading = GetAverageAnalogReading(TOT_CURR_PIN, CURRENT_AVERAGE_NUMBER); //range is 0-1023

float ITotVolts = ((float)ITotAnalogReading / (float)MAX_AD_VALUE) * ADC_REF_VOLTS;

float ITotAmps = ITotVolts * VOLTAGE_TO_CURRENT_RATIO;

////DEBUG!!

//mySerial.printf("GetTotalAmps(): Areading, ITotVolts, ITotAmps = %d, %3.2f, %3.2f\n",

// ITotAnalogReading, ITotVolts, ITotAmps);

////DEBUG!!

return ITotAmps;

}

//added 02/28/19 to service 2nd 1Na169 current sensor

float GetRunningAmps()

{

//Purpose: Get robot running current in amps

//Inputs:

// Voltage on RUN_CURR_PIN is approximately IRun Amps

// VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio for running current

//Outputs:

// returns robot running current

//Notes:

// 02/28/18 copied from GetTotalAmps() and adapted

//int IRunAnalogReading = analogRead(RUN_CURR_PIN); //range is 0-1023

int IRunAnalogReading = GetAverageAnalogReading(RUN_CURR_PIN, CURRENT_AVERAGE_NUMBER); //range is 0-1023

float IRunVolts = ((float)IRunAnalogReading / (float)MAX_AD_VALUE) * ADC_REF_VOLTS;

float IRunAmps = IRunVolts * VOLTAGE_TO_CURRENT_RATIO;

////DEBUG!!

// mySerial.printf("GetRunningAmps(): Areading, IRunvolts, IRunAmps = %d, %3.2f, %3.2f\n",

// IRunAnalogReading, IRunVolts,IRunAmps);

////DEBUG!!

return IRunAmps;

}

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 = GetTotalAmps();

float RunI = GetRunningAmps();

////DEBUG!!

// mySerial.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 bConnect = digitalRead(CHG_CONNECT_PIN); //goes HIGH when chg cable connected

////DEBUG!!

// mySerial.printf("IsChargerConnected() returns %d\n", bConnect);

////DEBUG!!

return bConnect;

}

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

//Step1: Get current time & check for sufficient elapsed time

int ElapsedChgTimeSec = 0;

float ElapsedChgTimeMin = 0; //added 05/02/20

bool bChgConn = digitalRead(CHG_CONNECT_PIN); //goes HIGH when chg cable connected

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

bool bStillCharging = true;

//04/27/20 re-arranged for clarity

while (ElapsedChgTimeSec < MINIMUM_CHARGE_TIME_SEC ||

(bStillCharging

&& ElapsedChgTimeSec < BATT_CHG_TIMEOUT_SEC

&& bChgConn)

)

{

delay(1000); //one-second loop

ElapsedChgTimeSec++;

bStillCharging = IsStillCharging(); //02/24/19 - now using 1NA169 current sensor

bChgConn = digitalRead(CHG_CONNECT_PIN); //goes HIGH when chg cable connected

//05/02/20 rev to only print out 10 times/min

if (ElapsedChgTimeSec % 6 == 0)

{

ElapsedChgTimeMin = (float)ElapsedChgTimeSec / 60.f;

float BattV = GetBattVoltage();

float TotalI = GetTotalAmps(); //rev 02/28/19: chg name from 'Batt' to 'Total'

float RunI = GetRunningAmps();

//mySerial.printf("%d\t%2.4f\t%2.4f\t%2.4f\t%2.4f\n",

// ElapsedChgTimeSec, BattV, TotalI, RunI, TotalI - RunI); //rev 02/24/19 for 1Na169 sensor

mySerial.printf("%3.1f\t%2.4f\t%2.4f\t%2.4f\t%2.4f\n",

ElapsedChgTimeMin, BattV, TotalI, RunI, TotalI - RunI); //rev 02/24/19 for 1Na169 sensor

UpdateChgStatusLEDs(BattV, bStillCharging); //updates 'fuel guage' LEDs 04/22/20 added bStillCharging to sig

}

//Step2: Check for end-of-charge or failure (don't know what this would be yet...)

//if charging ran over time, something went wrong

//10/16/17 revised for better telemetry

//if (bChgConn == LOW) //charger unplugged

if (!bChgConn) //charger unplugged

{

Serial.print("Charge connection dropped after "); Serial.print(ElapsedChgTimeSec / 60);

Serial.println(" minutes");

return false;

}

else if (ElapsedChgTimeSec < BATT_CHG_TIMEOUT_SEC)

{

Serial.print("Charging Completed Successfully in "); Serial.print(ElapsedChgTimeSec / 60);

Serial.print(" minutes at "); Serial.println(millis());

return true;

}

else

{

Serial.print("Charging timout value of "); Serial.print(BATT_CHG_TIMEOUT_SEC);

Serial.print(" secs expired at "); Serial.println(millis());

return false;

}

float GetBattVoltage()

{

//02/18/17 get corrected battery voltage. Voltage reading is 1/3 actual Vbatt value

int analog_batt_reading = GetAverageAnalogReading(BATT_MON_PIN, VOLTS_AVERAGE_NUMBER);//rev 03/01/19 to average readings

float calc_volts = ZENER_VOLTAGE_OFFSET + ADC_REF_VOLTS * (double)analog_batt_reading / (double)MAX_AD_VALUE;

////DEBUG!!

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

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

//09/08/20 modified for DRV8871 motor driver

//SetLeftMotorDir(REV_DIR);

//SetRightMotorDir(REV_DIR);

//RunBothMotorsMsec(2000, MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

RunBothMotorsMsec(false, 2000, MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

//Step4: Turn 90 away from near side wall

RollingTurn(rightdist < leftdist, true, 90); //turn 90 deg away from nearest wall

////DEBUG!!

// MoveAhead(0, 0);

// delay(500);

// cli();

// sleep_enable();

// sleep_cpu();

////DEBUG!!

return true; //can't think of anything else at the moment

}

long GetBatRunDurationSec()

{

return 1; //dummy for now

}

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: Initialize PID for homing

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

String trackstr = "IR"; //used for telemetry printouts

String str = ""; //telemetry string

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

//elapsedMillis IsStuckCallElapsedMillis = 0; //added 08/09/20

//Step1: Initialize PID for homing

//set the target value

//IRHomingSetpoint = 0.05; //10/15/17 this seems to be the best value for now

//IRHomingSetpoint = -0.05; //07/12/20 new wheels, new motors

//IRHomingSetpoint = -0.15; //07/12/20 new wheels, new motors

//IRHomingSetpoint = -0.25; //07/12/20 new wheels, new motors

//IRHomingSetpoint = 0.25; //07/12/20 new wheels, new motors

//IRHomingSetpoint = 0.20; //07/12/20 new wheels, new motors

IRHomingSetpoint = 0.15; //07/12/20 new wheels, new motors

//turn the PID on

IRHomingPID.SetMode(AUTOMATIC);

//set the limits

IRHomingPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

//11/02/18 added dist var to CalcDistArrayVariance sig

//08/09/20 now using no_param version of IsStuck();

int frontdist = GetFrontDistCm();

int bChgConn = LOW; //for testing

lastHomingTelemetryMsec = 0; //used to space out telemetry prints

//Step2: If front distance < avoidancedistCm, turn 90 deg away from near wall

// otherwise continue homing until connected or stuck

mySerial.printf("front dist = %d, lastHomingTelemetryMsec = ", frontdist);

Serial.println(lastHomingTelemetryMsec);

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

//08/10/20 now using ISR for bIsStuck state

while (bChgConn == LOW && !bIsStuck && frontdist > 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();

//10/06/17 get new homing data from Teensy 3.2 IR Beacon Homing Module at address 8

//2nd param in Wire.requestFrom not used by slave - but its val % 32 must be in range of 1-32

//float tempfloat = 0; //IRHomingLRSteeringVal is a double, so use a temp float and then convert on assignment

long FinalValue1, FinalValue2;

Wire.requestFrom(IR_HOMING_MODULE_SLAVE_ADDR, sizeof(FinalValue1) + sizeof(FinalValue1) + sizeof(IRHomingLRSteeringVal));

I2C_readAnything(FinalValue1);

I2C_readAnything(FinalValue2);

I2C_readAnything(IRHomingLRSteeringVal);

//skip bad values

if (!isnan(IRHomingLRSteeringVal))

{

//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)

if (IRHomingPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value

{

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;

mySerial.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(), FinalValue1, FinalValue2, IRHomingLRSteeringVal, IRHomingOutput,

leftspeednum, rightspeednum, frontdist);

}

//DEBUG!!

//for testing, use charging LEDs for visualization of IRHomingLRSteeringVal

int FINout = (IRHomingLRSteeringVal > -1.0 && IRHomingLRSteeringVal < -0.5) ? LOW : HIGH;

int _80PCTout = (IRHomingLRSteeringVal >= -0.5 && IRHomingLRSteeringVal < -0.25) ? LOW : HIGH;

int _60PCTout = (IRHomingLRSteeringVal >= -0.25 && IRHomingLRSteeringVal < 0) ? LOW : HIGH;

int _40PCTout = (IRHomingLRSteeringVal >= 0 && IRHomingLRSteeringVal <= 0.25) ? LOW : HIGH;

int _20PCTout = (IRHomingLRSteeringVal > 0.25 && IRHomingLRSteeringVal < 0.5) ? LOW : HIGH;

int CHGout = (IRHomingLRSteeringVal > 0.5 && IRHomingLRSteeringVal < 1.0) ? LOW : HIGH;

digitalWrite(FIN_LED_PIN, FINout);

digitalWrite(_20PCT_LED_PIN, _20PCTout);

digitalWrite(_40PCT_LED_PIN, _40PCTout);

digitalWrite(_60PCT_LED_PIN, _60PCTout);

digitalWrite(_80PCT_LED_PIN, _80PCTout);

digitalWrite(CHG_LED_PIN, CHGout);

//mySerial.printf("FIN/20/40/60/80/CHG = %d/%d/%d/%d/%d/%d\n", FINout, _20PCTout, _40PCTout, _60PCTout, _80PCTout, CHGout);

//0424/20 experiment with going to full speed when near charge plug

if (frontdist <= CHG_STN_FINAL_APPR_DIST_CM)

{

leftspeednum = rightspeednum = MOTOR_SPEED_MAX;

mySerial.printf("Accelerating to Contact with frontdist = %d\n", frontdist);

}

MoveAhead(leftspeednum, rightspeednum);

}

//11/11/18 moved outside 'if (!isnan(IRHomingLRSteeringVal))' block so will always get executed

frontdist = GetFrontDistCm(); //this is also a loop exit condition

bChgConn = digitalRead(CHG_CONNECT_PIN); //goes HIGH when chg cable connected

//05/02/20 turn off Laser

digitalWrite(RED_LASER_DIODE_PIN, LOW);

}// while (bChgConn == LOW && !bIsStuck && frontdist > avoidancedistCm)

//find out why loop exited. Could be stuck, connected, or inside avoidance dist

int leftdist = GetAvgLeftDistCm();

int rightdist = GetAvgRightDistCm();

if (bIsStuck || frontdist <= avoidancedistCm)

{

mySerial.printf("Abnormal exit from homing routine\n");

mySerial.printf("%lu: front/left/rightdist/bIsStuck = %d/%d/%d/%s\n",

millis(), frontdist, leftdist, rightdist, bIsStuck ? "TRUE" : "FALSE");

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

BackupAndTurn90Deg((leftdist > rightdist), true, MOTOR_SPEED_HALF);

PrevOpMode = MODE_NONE; //reset so tracking wall can be re-captured

//return false;

result = false; //added 01/16/19 to supress warning

}

else if (bChgConn == HIGH)

{

Serial.print("Charger Connected at "); Serial.println(millis());

//return true;

result = true; //added 01/16/19 to supress warning

}

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

}

#pragma endregion Charge Support Functions

#pragma region WALL TRACKING SUPPORT

bool RotateToParallelOrientation(int trkcase)

{

//Purpose: Rotate robot to parallel near wall using VL53L0X array and PID engine

//Inputs:

// TrackingCase = int representing current WallTrackingCases enum value

//Outputs:

// Robot orientation changed to parallel nearest wall

//Plan:

//Step1: Initialize PID engine

//Step2: Drive setpoint to zero using motors

//Notes:

// 06/18/20 have only right side array at the moment

// 08/06/20 added left side support

// 08/10-12/20 added 'stuck' detection

mySerial.printf("In RotateToParallelOrientation(%s)\n", TrkStrArray[trkcase]);

//int initleftspeed = MOTOR_SPEED_HALF;

//int initrightspeed = MOTOR_SPEED_HALF;

//elapsedMillis IsStuckCallElapsedMillis = 0; //added 08/09/20

//Step1: Initialize PID engine

ToFSetpoint = ToFArray_PARALLEL_FIND_SETPOINT;

//turn the PID on

ToFArrayPID.SetMode(AUTOMATIC);

ToFArrayPID.SetTunings(ToFArray_PARALLEL_FIND_Kp, ToFArray_PARALLEL_FIND_Ki, ToFArray_PARALLEL_FIND_Kd);

//set the limits

//09/12/20 experiment

ToFArrayPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

//ToFArrayPID.SetOutputLimits(-MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

//DEBUG!!

//print out PID parameters

mySerial.printf("ToFArrayPID Parameters (Kp,Ki,Kd,DIR) = (%2.2f,%2.2f,%2.2f,%d)\n",

ToFArrayPID.GetKp(), ToFArrayPID.GetKi(), ToFArrayPID.GetKd(), ToFArrayPID.GetDirection());

int outmax = ToFArrayPID.GetOutputMax();

int outmin = ToFArrayPID.GetOutputMin();

mySerial.printf("ToFArrayPID output max/min = %d/%d\n", outmax, outmin);

//DEBUG!!

mySerial.printf("Time\tFdist\tCdist\tRdist\tSteer\tPIDout\tLspd\tRSpd\n");

//DEBUG!!

//Step2: Drive setpoint to zero using motors

lastToFArrayTelemetryMsec = 0; //init telemetry spacing tracker

if (trkcase == TRACKING_RIGHT)

{

//this really is necessary - else while() won't have correct starting value

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

delay(100);

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

ToFSteeringVal = RightSteeringVal; //08/06/20 now have separate left/right steering vals

//08/09/20 added 'stuck' and 'upcoming obstacle' guards, but IsStuck() call must be limited to 5/sec or so

//08/10/20 bIsStuck & bObstacleAhead now updated in timer1 ISR

while (abs(ToFSteeringVal) > PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD && !bIsStuck && !bObstacleAhead)

{

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

ToFSteeringVal = RightSteeringVal; //08/06/20 now have separate left/right steering vals

//DEBUG!!

//print out telemetry every 400 msec or so

if (lastToFArrayTelemetryMsec > ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics

{

lastToFArrayTelemetryMsec -= ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC;

mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\n", millis(),

Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, ToFSteeringVal, ToFOutput,

leftspeednum, rightspeednum);

}

//DEBUG!!

//skip bad values

if (!isnan(ToFSteeringVal))

{

//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)

if (ToFArrayPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value

{

leftspeednum = MOTOR_SPEED_HALF - ToFOutput;

rightspeednum = MOTOR_SPEED_HALF + ToFOutput;

MoveAhead(leftspeednum, rightspeednum);

}

CheckForUserInput();

}// while(abs(ToFSteeringVal) > PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD)

}

else //TRACKING_LEFT

{

//this really is necessary - else while() won't have correct starting value

GetRequestedVL53l0xValues(VL53L0X_LEFT);

ToFSteeringVal = LeftSteeringVal; //08/06/20 now have separate left/right steering vals

//08/09/20 added guards for frontdist & 'stuck' condx

//08/10/20 bIsStuck & bObstacleAhead now updated in timer1 ISR

//while (abs(ToFSteeringVal) > PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD && !bIsStuck && !bObstacleAhead)

while (abs(ToFSteeringVal) > ToFArray_PARALLEL_FIND_SETPOINT && !bIsStuck && !bObstacleAhead)

//while (true)

{

GetRequestedVL53l0xValues(VL53L0X_LEFT);

ToFSteeringVal = LeftSteeringVal; //08/06/20 now have separate left/right steering vals

//DEBUG!!

//print out telemetry every 400 msec or so

//if (lastToFArrayTelemetryMsec > ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics

//{

// lastToFArrayTelemetryMsec -= ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC;

//mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\n", millis(),

// Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal, ToFOutput,

// leftspeednum, rightspeednum);

//}

//DEBUG!!

//skip bad values

if (!isnan(ToFSteeringVal))

{

//By default, computes new output approx 10 times/sec (use SetSampleTime() to change)

if (ToFArrayPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value

{

//in DIRECT mode, a negative output means the near side should go slower,

//and the far side should go faster

leftspeednum = MOTOR_SPEED_HALF + ToFOutput;

rightspeednum = MOTOR_SPEED_HALF - ToFOutput;

MoveAhead(leftspeednum, rightspeednum);

//mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\n", millis(),

// Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal, ToFOutput,

// leftspeednum, rightspeednum);

}

CheckForUserInput();

}// while(abs(ToFSteeringVal) > PARALLEL_ORIENTATION_STEERING_VALUE_THRESHOLD)

}

mySerial.printf("Parallel Orientation Achieved with SteeringVal = %3.2f\n", ToFSteeringVal);

MoveAhead(0, 0); //need this to keep robot from continuing last turn

//DEBUG!!

//while (true)

//{

// CheckForUserInput();

//}

//DEBUG!!

return true;

}

bool CaptureWallOffset(WallTrackingCases trkcase, int offset)

{

//Purpose: Position the robot parallel to the nearest wall, at the

// desired offset

//Inputs:

// trkcase = WallTrackingCases enum value denoting the wall to be tracked

// offset = int value denoting the offset in Cm

//Outputs:

// Robot positioned parallel to the nearest wall, at the desired offset

// return value is TRUE if successful, otherwise FALSE

//Plan:

// Step1: Rotate robot to be parallel to the selected wall using TofArrayPID

// Step2: Drive robot toward offset distance using ToFArray PID

// Step3: Rotate robot back to parallel orientation using ToFArray PID

//Notes:

// 08/06/20 added left side processing

// 08/09/20 added check for 'stuck' condx (can't check for frontdist < min, as this is sure to happen during approach)

// 08/09/20 have to limit IsStuck() calls to 5/sec to avoid false positives

// 08/10/20 now setting bIsStuck state in timer1 ISR

// 08/12/20 chg bIsStuck to bIsStuck_Slow for half-speed offset capture ops

// 08/12/20 added code to re-init aFrontDist on 'stuck' detection

//DEBUG!!

mySerial.printf("In CaptureWallOffset(%s,%d)\n", TrkStrArray[trkcase], offset);

//delay(500);

//DEBUG!!

//Step1: Rotate robot to be parallel to the selected wall using TofArrayPID

RotateToParallelOrientation(trkcase);

//Step2: Drive robot toward offset distance using ToFArray PID

//reset PID setpoint for about 20 deg cut toward wall

int dist;

if (trkcase == TRACKING_RIGHT)

{

dist = GetRightDistCm();

if (dist > offset)

{

ToFSetpoint = -CAPTURE_APPROACH_STEERING_VALUE; //this should result in about 20 deg

}

else

{

ToFSetpoint = CAPTURE_APPROACH_STEERING_VALUE; //this should result in about 20 deg

}

else //TRACKING_LEFT case //populated 08/06/20

{

dist = GetLeftDistCm();

if (dist > offset)

{

ToFSetpoint = -CAPTURE_APPROACH_STEERING_VALUE; //this should result in about 20 deg

}

else

{

ToFSetpoint = CAPTURE_APPROACH_STEERING_VALUE; //this should result in about 20 deg

}

//DEBUG!!

mySerial.printf("After || op - offset/rdist/ToFSetpoint now %d/%d/%2.3f\n", offset, dist, ToFSetpoint);

mySerial.printf("Time\t\Fdist\tCdist\tRdist\tSteer\tPIDout\tLspd\tRSpd\n");

delay(2000);

//DEBUG!!

////ToFArrayPID.SetTunings(ToFArray_OFFSET_CAPTURE_Kp, 0, 0); //try a bit less aggresive setup

//ToFArrayPID.SetTunings(ToFArray_OFFSET_CAPTURE_Kp,

// ToFArray_OFFSET_CAPTURE_Ki,

// ToFArray_OFFSET_CAPTURE_Kd); //09/28/20 added Kd = 50

//ToFArrayPID.SetOutputLimits(-MOTOR_SPEED_CAPTURE_OFFSET, MOTOR_SPEED_CAPTURE_OFFSET);

//lastToFArrayTelemetryMsec = 0; //reset before entering while loop

//if (trkcase == TRACKING_RIGHT)

//{

// while (abs(GetRightDistCm() - offset) > WALL_OFFSET_CAPTURE_WINDOW_CM && !bIsStuck_Slow)

// {

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

// GetRequestedVL53l0xValues(VL53L0X_RIGHT);

// ToFSteeringVal = RightSteeringVal; //08/06/20 now have separate left/right steering vals

// //skip bad values

// if (!isnan(ToFSteeringVal))

// {

// //By default, computes new output approx 10 times/sec (use SetSampleTime() to change)

// if (ToFArrayPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value

// {

// leftspeednum = MOTOR_SPEED_CAPTURE_OFFSET - ToFOutput;

// rightspeednum = MOTOR_SPEED_CAPTURE_OFFSET + ToFOutput;

// MoveAhead(leftspeednum, rightspeednum);

// }

// //DEBUG!!

// //print out telemetry every 200 msec or so

// //if (lastToFArrayTelemetryMsec > ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics

// //{

// // lastToFArrayTelemetryMsec -= ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC;

// // mySerial.printf("%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\n", millis(),

// // Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, ToFSteeringVal, ToFOutput,

// // leftspeednum, rightspeednum);

// //}

// //DEBUG!!

// //08/09/20 now have to check for abnormal exit

// if (bIsStuck)//08/10/20 bIsStuck now updated in timer1 ISR

// {

// InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

// //mySerial.printf("Stuck condition detected with front distance = %d\n", GetFrontDistCm());

// mySerial.printf("Stuck condition detected with front distance = %d and var = %3.2f\n", GetFrontDistCm(), frontvar);

// BackupAndTurn90Deg(true, true, MOTOR_SPEED_HALF);

// return false;

// }

// //DEBUG!!

// mySerial.printf("Offset distance achieved with VL53L0X reporting F/C/R = %d/%d/%d\n",

// Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);

// mySerial.printf("Rotating back to parallel orientation\n");

// //DEBUG!!

//}

//else //trkcase == TRACKING_LEFT

//{

// mySerial.printf("CaptureWallOffset TRACKING_LEFT block with offset = %d\n", offset);

// GetRequestedVL53l0xValues(VL53L0X_LEFT);

// //09/29/20 can't use capture window - robot blows right through it :(

// int sign = 1; //used to modify while statement

// sign = (Lidar_LeftFront > 10 * offset) ? 1 : -1; //handle both 'inside' and 'outside' approaches

// //while (abs(GetRightDistCm() - offset) > WALL_OFFSET_CAPTURE_WINDOW_CM && !bIsStuck_Slow)

// //while (abs(Lidar_LeftFront - offset) > WALL_OFFSET_CAPTURE_WINDOW_CM && !bIsStuck_Slow)

// while (Lidar_LeftFront > sign * 10 * offset)

// {

// //01/30/17 added to kill motors remotely using Wixel & serial port

// CheckForUserInput();

// //if (lastToFArrayTelemetryMsec > ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC) //c/o to get more resolution on steering dynamics

// {

// lastToFArrayTelemetryMsec -= ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC;

// //GetRequestedVL53l0xValues(VL53L0X_LEFT);

// ToFSteeringVal = LeftSteeringVal; //08/06/20 now have separate left/right steering vals

// //skip bad values

// if (!isnan(ToFSteeringVal))

// {

// //By default, computes new output approx 10 times/sec (use SetSampleTime() to change)

// if (ToFArrayPID.Compute())

// {

// leftspeednum = MOTOR_SPEED_CAPTURE_OFFSET + ToFOutput;

// rightspeednum = MOTOR_SPEED_CAPTURE_OFFSET - ToFOutput;

// MoveAhead(leftspeednum, rightspeednum);

// //mySerial.printf("%lu\t%d\t%d\t%d\t%2.1f\t%d\n",

// // millis(),

// // Lidar_LeftFront,

// // 10 * offset,

// // abs(Lidar_LeftFront - 10 * offset),

// // 10 * WALL_OFFSET_CAPTURE_WINDOW_CM,

// // float(abs(Lidar_LeftFront - 10 * offset)) > 10 * WALL_OFFSET_CAPTURE_WINDOW_CM);

// GetRequestedVL53l0xValues(VL53L0X_LEFT);

// ToFSteeringVal = LeftSteeringVal; //08/06/20 now have separate left/right steering vals

// }

// //mySerial.printf("\t%lu\t%d\t%d\t%d\t%2.2f\t%2.2f\t%d\t%d\t%d\n", millis(),

// // Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal, ToFOutput,

// // leftspeednum, rightspeednum, abs(Lidar_LeftFront - 10 * offset) > 10 * WALL_OFFSET_CAPTURE_WINDOW_CM);

// }

// mySerial.printf("%lu\t%d\t%d\t%d\t%2.1f\t%d\n",

// millis(),

// Lidar_LeftFront,

// 10 * offset,

// abs(Lidar_LeftFront - 10 * offset),

// 10 * WALL_OFFSET_CAPTURE_WINDOW_CM,

// float(abs(Lidar_LeftFront - 10 * offset)) > 10 * WALL_OFFSET_CAPTURE_WINDOW_CM);

// //08/09/20 now have to check for abnormal exit

// if (bIsStuck_Slow)//08/10/20 bIsStuck now updated in timer1 ISR

// {

// InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

// mySerial.printf("Stuck_Slow condition detected with front distance = %d and var = %3.2f\n", GetFrontDistCm(), frontvar);

// BackupAndTurn90Deg(true, true, MOTOR_SPEED_HALF);

// return false;

// }

// //DEBUG!!

// mySerial.printf("Offset distance achieved with VL53L0X reporting F/C/R = %d/%d/%d\n",

// Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear);

// StopBothMotors(); //added 09/29/20

// delay(2000); //just for debug

// //DEBUG!!

//}

////if we get to here, offset capture succeeded

////Step3: Rotate robot back to parallel orientation using ToFArray PID

//mySerial.printf("Rotating back to parallel orientation\n");

//RotateToParallelOrientation(trkcase);

return true;

}

void UpdateWallFollowMotorspeeds(int leftdistval, int rightdistval, int& leftspeednum, int& rightspeednum)

{

//Purpose: Update left & right motor speed values to follow the nearest wall

//Provenance: Created 12/26/14

//Inputs:

// leftspeednum = integer denoting left motor drive value (0-255)

// rightspeednum = integer denoting right motor drive value (0-255)

// previous distance input from left & right ping sensors

// current distance input from left & right ping sensors

//Outputs:

// leftspeednum = integer denoting left motor drive value (0-255)

// rightspeednum = integer denoting right motor drive value (0-255)

//Plan:

// Step1: Determine if we are closer to left or right walls & adjust speed accordingly

//Notes:

// 01/22/15 added some LED management code for debugging purposes

// 01/25/15 rev to use TweakVal * |d1-d0| instead of just TweakVal

// 03/14/15 rev to add left/rightdistval to sig

// 03/25/15 new speed adj algorithm. See https://fpaynter.com/2015/03/another-try-at-wall-following-adjustments/

// 03/29/15 added code to use red LEDs as 'active wall' indicator as well as stop/backup.

// 11/27/15 rev to remove speed limit checks - now done in FWDMotorDriver class

// 12/06/15 pulled l/r speed limit adj back from FWDMotorDrive class

// 12/07/15 chg bIsTrackingLeft from local to global var so can use in BackupAndTurn()

// 01/05/16 rev to make bIsTrackingLeft into separate function vice global var

// 05/03/17 commented out LED management code

// 02/09/19 revised to use PID vs home-brew wall-following algorithm

// 04/28/19 back to home-brew algorithm

//02/24/19 commented these lines out - no longer used

int prevrightspdnum, prevleftspdnum; //added 01/22/15 for LED mgmt

prevrightspdnum = rightspeednum;

prevleftspdnum = leftspeednum;

if (leftdistval <= rightdistval) //tracking wall on left side

{

//03/25/15 using new algorithm LSPDn = LSPDn-1 - K * (Dn - Dn-1); RSPDn = RSPDn-1 + K * (Dn - Dn-1)

leftspeednum = prevleftspdnum - MOTOR_SPEED_ADJ_FACTOR * (leftdistval - prevleftdistval);

rightspeednum = prevrightspdnum + MOTOR_SPEED_ADJ_FACTOR * (leftdistval - prevleftdistval);

//DEBUG!!

//mySerial.printf("Left Wall is Closer, leftdist, prevleftdist, prevlspd, lspd\n",leftdistval, prevleftdistval, prevleftspdnum, leftspeednum);

//mySerial.printf("Left Wall is Closer, leftdist = %d, prevleftdist = %d, prevlspd = %d, lspd = %d\n",leftdistval, prevleftdistval, prevleftspdnum, leftspeednum);

//DEBUG!!

}

else //right wall is closer

{

////DEBUG!!

// Serial.println("Right Wall is Closer");

////DEBUG!!

//bIsTrackingLeft = false;

//commented out 02/23/19

//03/25/15 using new algorithm LSPDn = LSPDn-1 + K * (Dn - Dn-1); RSPDn = RSPDn-1 - K * (Dn - Dn-1)

leftspeednum = prevleftspdnum + MOTOR_SPEED_ADJ_FACTOR * (rightdistval - prevrightdistval);

rightspeednum = prevrightspdnum - MOTOR_SPEED_ADJ_FACTOR * (rightdistval - prevrightdistval);

}

prevleftdistval = leftdistval;

prevrightdistval = rightdistval;

//ManageWallTrackingLEDs(GetTrackingDir());

//12/06/15 pulled l/r speed limit adj back from FWDMotorDrive class

//12/20/15 changed top end from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (255)

leftspeednum = (leftspeednum <= MOTOR_SPEED_MAX) ? leftspeednum : MOTOR_SPEED_MAX;

leftspeednum = (leftspeednum >= MOTOR_SPEED_LOW) ? leftspeednum : MOTOR_SPEED_LOW;

rightspeednum = (rightspeednum <= MOTOR_SPEED_MAX) ? rightspeednum : MOTOR_SPEED_MAX;

rightspeednum = (rightspeednum >= MOTOR_SPEED_LOW) ? rightspeednum : MOTOR_SPEED_LOW;

}

int GetOpMode(float batv) //04/28/19 added batv to sig

{

//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 (4)

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

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

//Step1: If Charger is connected, return MODE_CHARGING (1)

if (IsChargerConnected())

{

mode = MODE_CHARGING;

mySerial.printf("Robot has successfully connected to charger!\n");

}

//Step2: Else If IR Homing beam detected, return MODE_IRHOMING (2)

else if (IsIRBeamAvail()) 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) mode = MODE_DEADBATTERY;

//03/04/2020 added to facilitate battery discharge

#ifdef BATTERY_DISCHARGE

//05/01/2020 rewrote to eliminate OpMode section entirely. Now all done here

mySerial.printf("\n------------------- DISCHARGE MODE ------------------------\n\n");

mySerial.printf("Minutes\tVolts\tCurrent\n");

MoveAhead(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

float startmSec = millis(); //start time

while (GetBattVoltage() > DEAD_BATT_THRESH_VOLTS)

{

float elapsedmin = (millis() - startmSec) / 60000.f; //elapsed minutes

mySerial.printf("%2.2f\t%2.3f\t%2.3f\n", elapsedmin, GetBattVoltage(), GetRunningAmps());

delay(6000); //10 readings/minute

}

mySerial.printf("Battery Discharge Complete at %3.2f Minutes and %3.2f Volts\n", millis() / 60000.f, GetBattVoltage());

mySerial.printf("Stopping program!\n");

StopBothMotors();

while (true)

{

}

#else

//Step4: Else return MODE_WALLFOLLOW (3)

else mode = MODE_WALLFOLLOW;

#endif // BATTERY_DISCHARGE

return mode;

}

//01/05/16 added so tracking decision can be based on running avg vs just one value

//01/05/16 return value can't be TrackingCases type - causes Arduino pre-processor to choke

int GetTrackingDir()

{

//Purpose: Determine tracking condition (left, right, neither)

//Provenance: 01/05/16 G. Frank Paynter

//Inputs: Call from Loop()

//Outputs:

// TRACKING_LEFT, TRACKING_RIGHT, or TRACKING_NEITHER values

// TRACKING_IRBEAM added 01/30/17

//Plan:

// Step1: compute LR_PING_AVG_WINDOW_SIZE-point running average for left & right distances

// Step2: Determine current tracking case & (for TRACKING_LEFT & TRACKING_RIGHT) update PrevTrackingCase

// For TRACKING_NEITHER, PrevTrackingCase is *not* updated so it can be used to determine turn direction

// Step3: (added 01/30/17) Determine state of charge and IR Beam availability

// Step4: return appropriate TrackingCases value

//Notes:

// 01/05/16: due to Arduino pre-processor quirk, can't return a TrackingCases object - must be a 'normal' type

// 01/18/16: The TRACKING_NEITHER case only occurs when N-point avg of both l/r dists > max

// 01/30/17: Added TRACKING_IRBEAM detection code

// 04/28/19: Reinstated running average calc - NewPing::.ping_median() takes too long

// 04/09/20: Revised to use new GetAvgLeft/RightDistCm() functions

////DEBUG!!

// mySerial.printf("GetTrackingDir() TOP at %lu\n", millis());

////DEBUG!!

////Step1: compute LR_PING_AVG_WINDOW_SIZE-point running average for left & right distances

//04/28/19 reinstated - NewPing::.ping_median() takes too long

//int leftavgdist_cm = 0;

//int rightavgdist_cm = 0;

int retval = 0;

//for (int validx = 0; validx < LR_PING_AVG_WINDOW_SIZE; validx++)

//{

// leftavgdist_cm += aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1 - validx];

// rightavgdist_cm += aRightDist[LR_PING_DIST_ARRAY_SIZE - 1 - validx];

//}

//leftavgdist_cm = leftavgdist_cm / LR_PING_AVG_WINDOW_SIZE;

//rightavgdist_cm = rightavgdist_cm / LR_PING_AVG_WINDOW_SIZE;

//04/09/20: Revised to use new GetAvgLeft/RightDistCm() functions

int leftavgdist_cm = (int)GetAvgLeftDistCm();

int rightavgdist_cm = (int)GetAvgRightDistCm();

//DEBUG!!

mySerial.printf("Left/Right Avg Dist = %d/%d\n", leftavgdist_cm, rightavgdist_cm);

//DEBUG!!

//Step2: If result is TRACKING_LEFT or TRACKING_RIGHT, set bWasLastTrackingLeft boolean & tracking case appropriately

//check for 'open space' condx first, as otherwise it will never be detected

if (leftavgdist_cm == MAX_LR_DISTANCE_CM && rightavgdist_cm == MAX_LR_DISTANCE_CM)

{

retval = TRACKING_NEITHER; //don't update PrevTrackingCase - prev val needed for 'Open-Corner' turn dir

}

else if (leftavgdist_cm <= rightavgdist_cm)

{

PrevTrackingCase = TRACKING_LEFT;

retval = PrevTrackingCase;

}

else if (leftavgdist_cm > rightavgdist_cm)

{

PrevTrackingCase = TRACKING_RIGHT;

retval = PrevTrackingCase;

}

////DEBUG!!

// mySerial.printf("GetTrackingDir() BOTTOM at %lu\n", millis());

////DEBUG!!

//Step3: return appropriate TrackingCases value

return retval;

}

#pragma endregion Wall Tracking Support

#pragma region MISCELLANEOUS SUPPORT FUNCTIONS

int GetIntegerParameter(String prompt, int defaultval)

{

char Instr[20];

int param = 0;

bool bDone = false;

while (!bDone)

{

Serial.print(prompt); Serial.print(" ("); Serial.print(defaultval); Serial.print("): ");

while (Serial.available() == 0); //waits for input

//String res = Serial.readString().trim();

String res = Serial.readString();

res.trim();

int reslen = res.length();

if (reslen == 0) //user entered CR only

{

bDone = true;

param = defaultval;

}

else

{

res.toCharArray(Instr, reslen + 1);

//if (isNumeric(Instr) && atoi(Instr) >= 0)

if (isNumeric(Instr))

{

param = atoi(Instr);

bDone = true;

}

else

{

Serial.print(Instr); Serial.println(" Is invalid input - please try again");

}

Serial.println(param);

return param;

}

// check a string to see if it is numeric and accept Decimal point

//copied from defragster's post at https://forum.pjrc.com/threads/27842-testing-if-a-string-is-numeric

bool isNumeric(char* str)

{

byte ii = 0;

bool RetVal = false;

if ('-' == str[ii])

ii++;

while (str[ii])

{

if ('.' == str[ii]) {

ii++;

break;

}

if (!isdigit(str[ii])) return false;

ii++;

RetVal = true;

}

while (str[ii])

{

if (!isdigit(str[ii])) return false;

ii++;

RetVal = true;

}

return RetVal;

}

//04/20/15 added for turn signal support - replaces AllLedsOff()

void BackupSignal(bool bEnable)

{

//Purpose: Turns all rear LEDs off (HIGH is off)

//Provenance: Created 01/24/15 gfp

//03/19/18 now just an alias for EnableAllRearLEDs()

EnableAllRearLEDs(bEnable);

}

int GetAverageAnalogReading(int pin, int numavgs)

{

long totreads = 0;

for (int i = 0; i < numavgs; i++)

{

////DEBUG!!

// int aVal = analogRead(pin);

// mySerial.printf("Analog value at pin %d = %d\n", pin, aVal);

////DEBUG!!

totreads += analogRead(pin);

}

return (int)((float)totreads / (float)numavgs); //truncation ok

}

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();

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

}

void DisableAllRearPanelLEDs()

{

digitalWrite(_40PCT_LED_PIN, HIGH);

digitalWrite(_20PCT_LED_PIN, HIGH);

digitalWrite(CHG_LED_PIN, HIGH);

digitalWrite(_60PCT_LED_PIN, HIGH);

digitalWrite(_80PCT_LED_PIN, HIGH);

digitalWrite(FIN_LED_PIN, HIGH);

}

void EnableAllRearLEDs(bool bEnable)

{

//Purpose: Turns all 4 LEDs ON or OFF (LOW is ON)

//Provenance: Created 05/02/17 gfp

if (bEnable)

{

digitalWrite(FULL_LEFT_LED_PIN, LOW);

digitalWrite(HALF_LEFT_LED_PIN, LOW);

digitalWrite(QTR_LEFT_LED_PIN, LOW);

digitalWrite(FULL_RIGHT_LED_PIN, LOW);

digitalWrite(HALF_RIGHT_LED_PIN, LOW);

digitalWrite(QTR_RIGHT_LED_PIN, LOW);

}

else

{

digitalWrite(FULL_LEFT_LED_PIN, HIGH);

digitalWrite(HALF_LEFT_LED_PIN, HIGH);

digitalWrite(QTR_LEFT_LED_PIN, HIGH);

digitalWrite(FULL_RIGHT_LED_PIN, HIGH);

digitalWrite(HALF_RIGHT_LED_PIN, HIGH);

digitalWrite(QTR_RIGHT_LED_PIN, HIGH);

}

//void UpdateChgStatusLEDs(float battv) //rev 02/24/19

void UpdateChgStatusLEDs(float battv, bool bStillCharging) //04/22/20 added bStillCharging to sig

{

//Purpose: Update new 'fuel guage' LED status to show very rough approximation of battery charge level

//Inputs:

// battv = battery voltage measurement from Mega A/D

// bChg = bool that is LOW while charging, HIGH when finished

// bFin = bool that is HIGH while charging, LOW when finished

// aBattVtoPct = table of battery voltage ranges vs pct charge

//Outputs:

// Chg, 20-40-60-80%, and FIN LEDs illuminated as appropriate

//Plan:

// Step1: turn all LEDs OFF

// Step2: Illuminate appropriate LEDs

//Notes:

// 04/22/20 added bStillCharging to sig to eliminate unneccessary call to IsStillCharging()

//Step1: turn all LEDs OFF

EnableAllRearLEDs(false); //turns them all OFF

//Step2: illuminate appropriate LEDs

//if (IsStillCharging()) //02/24/19 - now using 1NA169 current sensor

if (bStillCharging) //04/22/20 rev to pass this in as calling parameter

{

digitalWrite(CHG_LED_PIN, LOW);

}

if (battv < _20PCT_BATT_VOLTS)

{

//do nothing

//mySerial.printf("In 0-20% section with BattV = %2.4f\n", battv);

}

if (battv >= _20PCT_BATT_VOLTS)

{

//mySerial.printf("In > 20% section with BattV = %2.4f\n", battv);

digitalWrite(_20PCT_LED_PIN, LOW);

}

if (battv >= _40PCT_BATT_VOLTS)

{

//mySerial.printf("In > 40% section with BattV = %2.4f\n", battv);

digitalWrite(_40PCT_LED_PIN, LOW);

}

if (battv >= _60PCT_BATT_VOLTS)

{

//mySerial.printf("In > 60% section with BattV = %2.4f\n", battv);

digitalWrite(_60PCT_LED_PIN, LOW);

}

if (battv >= _80PCT_BATT_VOLTS)

{

//mySerial.printf("In > 80% section with BattV = %2.4f\n", battv);

digitalWrite(_80PCT_LED_PIN, LOW);

}

//if (digitalRead(FIN_SIG_PIN) == LOW)

//{

// //mySerial.printf("In Chg Finished section with BattV = %2.4f\n", battv);

// digitalWrite(FIN_LED_PIN, LOW);

//}

}

void YellForHelp()

{

//Purpose: last-ditch effort to preserve the battery. All non-essential loads are shut down

// and an visual/audible SOS is sounded forever

//Inputs: Call from OpMode case switch when GetBattVoltage() returns a below-threshold value

//Outputs:

// All wheel motors stopped

// All rear panel LED's turned OFF

// PWM 'SOS' tones on SOS_PWM_PIN

//Plan:

// Step1: Turn off wheel motors

// Step2: Turn off all rear panel LEDs

// Step3: Send SOS to speaker, the purple rear panel LEDs, and the red laser

//Notes:

// 01/16/18 - SOS tone code copied from PWMTest.pde

//Step1: Turn off wheel motors

StopBothMotors();

//Step2: Turn off all rear panel LEDs

EnableAllRearLEDs(false);

//Step3: infinte loop to xmit SOS on speaker and the purple rear panel LEDs

while (!IsChargerConnected())

{

//int DOT_MS = 200;

//int DASH_MS = 800;

//int HIGHTONE = 1000;

//int LOWTONE = 500;

//Send 'S'

for (size_t i = 0; i < 3; i++)

{

digitalWrite(RED_LASER_DIODE_PIN, HIGH); //turn laser on

digitalWrite(FIN_LED_PIN, LOW); //turn LED on

digitalWrite(CHG_LED_PIN, LOW); //turn LED on

tone(SOS_PWM_PIN, HIGHTONE, DOT_MS); //returns immediately

delay(DOT_MS); //delay for LED viewing

digitalWrite(FIN_LED_PIN, HIGH); //turn LED off

digitalWrite(CHG_LED_PIN, HIGH); //turn LED off

digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn laser off

delay(DOT_MS); //inter-symbol spacing

}

delay(DOT_MS); //inter-letter spacing

//Send 'O'

for (size_t i = 0; i < 3; i++)

{

digitalWrite(RED_LASER_DIODE_PIN, HIGH); //turn laser on

digitalWrite(FIN_LED_PIN, LOW); //turn LED on

digitalWrite(CHG_LED_PIN, LOW); //turn LED on

tone(SOS_PWM_PIN, HIGHTONE, DASH_MS); //returns immediately

delay(DASH_MS); //delay for LED viewing

digitalWrite(FIN_LED_PIN, HIGH); //turn LED off

digitalWrite(CHG_LED_PIN, HIGH); //turn LED off

digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn laser off

delay(DOT_MS); //inter-symbol spacing

}

delay(DOT_MS); //inter-letter spacing

//Send 'S'

for (size_t i = 0; i < 3; i++)

{

digitalWrite(RED_LASER_DIODE_PIN, HIGH); //turn laser on

digitalWrite(FIN_LED_PIN, LOW); //turn LED on

digitalWrite(CHG_LED_PIN, LOW); //turn LED on

tone(SOS_PWM_PIN, HIGHTONE, DOT_MS); //returns immediately

delay(DOT_MS); //delay for LED viewing

digitalWrite(FIN_LED_PIN, HIGH); //turn LED off

digitalWrite(CHG_LED_PIN, HIGH); //turn LED off

digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn laser off

delay(DOT_MS); //inter-symbol spacing

}

delay(10 * DOT_MS); // inter-message spacing

}

byte ByteValFromBattVolt()

{

//Purpose: Calculate a byte value representing the current battery voltage,

// where 0 = DEAD_BATT_THRESH_VOLTS-1 = 5V

// and 255 = FULL_BATT_VOLTS + 1 = 9.4V

//Inputs: none

//Outputs:

// return a number between 0 and 255, where 0 represents 5V and 255 represents 9.4V

float val = GetBattVoltage();

float top = FULL_BATT_VOLTS + 1; //9.4

float bot = DEAD_BATT_THRESH_VOLTS - 1;//5.0

float range = top - bot; //4.4

float res = 255 * (val - bot) / range;

byte num = (byte)(res);

return num;

}

float BattVoltFromByteVal(byte battnum)

{

//Purpose: Calculate battery voltage from byte value,

// where 0 = DEAD_BATT_THRESH_VOLTS-1 = 5V

// and 255 = FULL_BATT_VOLTS + 1 = 9.4V

//Inputs: byte value from packet

// 0 represents DEAD_BATT_THRESH_VOLTS-1 = 5V

// 255 represents FULL_BATT_VOLTS + 1 = 9.4V

//Outputs:

// calculated battery voltage as (top-bot)*(battnum/255) + bot

float top = FULL_BATT_VOLTS + 1; //9.4

float bot = DEAD_BATT_THRESH_VOLTS - 1;//5.0

float range = top - bot; //4.4

float volts = range * ((float)battnum / (float)255) + bot;

return volts;

}

//re-enabled 06/28/20

void PrintWallFollowTelemetry()

{

//mySerial.printf("PrintWallFollowTelemetry() top at time %lu\n", millis());

DateTime dt = rtc.now();

uint32_t utime = dt.unixtime();

//get time string in hh:mm:ss format

char timestr[20];

memset(timestr, 0, sizeof(timestr));

GetTimeStringFromUnixTime(timestr, utime);

//battery voltage

float batv = GetBattVoltage(); //temporarily commented out

//print everything out

//mySerial.printf("%s\t%1.2f\t%s\t%s\t%d\t%d\t%d\t%3.2f\t%d\t%d\n",

// timestr, batv, ModeStrArray[CurrentOpMode], TrkStrArray[TrackingCase], leftdistval, rightdistval, frontdistval,

// frontvar, leftspeednum, rightspeednum);

mySerial.printf("%lu\t%1.2f\t%s\t%s\t%d\t%d\t%d\t%3.2f\t%d\t%d\n",

millis(), batv, ModeStrArray[CurrentOpMode], TrkStrArray[TrackingCase], leftdistval, rightdistval, frontdistval,

frontvar, leftspeednum, rightspeednum);

}

//09/01/20 added for wall-tracking debug

void PrintWallFollowTelemetry(WallTrackingCases trkcase)

{

//mySerial.printf("PrintWallFollowTelemetry() top at time %lu\n", millis());

DateTime dt = rtc.now();

uint32_t utime = dt.unixtime();

//get time string in hh:mm:ss format

char timestr[20];

memset(timestr, 0, sizeof(timestr));

GetTimeStringFromUnixTime(timestr, utime);

//battery voltage

float batv = GetBattVoltage(); //temporarily commented out

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

//ToFSteeringVal = Lidar_RightFront - Lidar_RightRear + Lidar_RightCenter; //use ALL info - not just distance!

// const char* WallFollowTelemStr = "Msec\tMode\tTrack\tFront\tCtr\tRear\tSteer\tOutput\tLSpd\tRSpd"; //09/01/20 PID tuning

//print everything out

mySerial.printf("%lu\t%s\t%s\t%d\t%d\t%d\t%3.2f\t%3.2f\t%d\t%d\n",

millis(), ModeStrArray[CurrentOpMode], TrkStrArray[TrackingCase], Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear,

RightSteeringVal, ToFOutput, leftspeednum, rightspeednum);

}

void GetTimeStringFromUnixTime(char* timestr, uint32_t unixtime)

{

//time_t utime = (time_t)unixtime;

//int myhour = hour(utime);

//int mymin = minute(utime);

//int mysec = second(utime);

//02/19/19 now using RTCLib's DateTime class

DateTime dt = DateTime(unixtime);

int myhour = dt.hour();

int mymin = dt.minute();

int mysec = dt.second();

//now concatenate everything into the provided buffer

sprintf(timestr, "%02d:%02d:%02d", myhour, mymin, mysec);

}

void GetModeString(int mode, char* bufptr)

{

sprintf(bufptr, "%s", ModeStrArray[mode]);

}

void GetWallTrackString(int mode, char* bufptr)

{

sprintf(bufptr, "%s", TrkStrArray[mode]);

}

void CheckForUserInput()

{

if (Serial.available() > 0)

{

// read the incoming byte:

int incomingByte = Serial.read();

// say what you got:

Serial.print("I received: ");

//Serial.println(incomingByte, DEC);

Serial.println(incomingByte, HEX); //chg to HEX 02/18/20

//02/18/20 experiment with multiple commands

switch (incomingByte)

{

case 0x53: //ASCII 'S'

case 0x73: //ASCII 's'

mySerial.print("Stopping Motors!");

StopBothMotors();

while (true)

{

}

break;

case 0x43: //ASCII 'C'

case 0x63: //ASCII 'c'

//enter infinite loop for direct remote control

mySerial.printf("ENTERING COMMAND MODE:\n");

mySerial.printf("0 = 180 deg CCW Turn\n");

mySerial.printf("1 = 180 deg CW Turn\n");

mySerial.printf("A = Back to Auto Mode\n");

mySerial.printf("S = Stop\n");

mySerial.printf("F = Forward\n");

mySerial.printf("R = Reverse\n");

mySerial.printf("\n");

mySerial.printf(" Faster\n");

mySerial.printf("\t8\n");

mySerial.printf("Left 4\t5 6 Right\n");

mySerial.printf("\t2\n");

mySerial.printf(" Slower\n");

StopBothMotors();

int speed = 0;

bool bAutoMode = false;

while (!bAutoMode)

{

if (Serial.available() > 0)

{

// read the incoming byte:

int incomingByte = Serial.read();

mySerial.printf("Got %c\n", incomingByte);

switch (incomingByte)

{

case 0x30: //Dec '0'

mySerial.print("CCW 180 deg Turn\n");

RollingTurn(true, bIsForwardDir, 180);

MoveAhead(speed, speed);

break;

case 0x31: //Dec '1'

mySerial.print("CW 180 deg Turn\n");

RollingTurn(false, bIsForwardDir, 180);

MoveAhead(speed, speed);

break;

case 0x34: //Turn left 10 deg

mySerial.print("CCW 10 deg Turn\n");

MoveAhead(speed, speed);

RollingTurn(true, bIsForwardDir, 10);

//added 05/03/20

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x36: //Turn right 10 deg'

mySerial.print("CW 10 deg Turn\n");

MoveAhead(speed, speed);

RollingTurn(false, bIsForwardDir, 10);

//added 05/03/20

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x38: //Speed up

speed += 50;

speed = (speed >= MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed;

mySerial.printf("Speeding up: speed now %d\n", speed);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x32: //Slow down

speed -= 50;

speed = (speed < 0) ? 0 : speed;

mySerial.printf("Slowing down: speed now %d\n", speed);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x35: //05/07/20 changed to use '5' vs 'S'

//case 0x53: //ASCII 'S'

//case 0x73: //ASCII 's'

mySerial.print("Stopping Motors!\n");

StopBothMotors();

speed = 0;

break;

case 0x41: //ASCII 'A'

case 0x61: //ASCII 'a'

StopBothMotors();

mySerial.print("Re-entering AUTO mode\n");

bAutoMode = true;

break;

case 0x52: //ASCII 'R'

case 0x72: //ASCII 'r'

mySerial.print("Setting both motors to reverse\n");

bIsForwardDir = false;

MoveReverse(speed, speed);

break;

case 0x46: //ASCII 'F'

case 0x66: //ASCII 'f'

mySerial.print("Setting both motors to forward\n");

bIsForwardDir = true;

MoveAhead(speed, speed);

break;

case 0x44: //ASCII 'D'

case 0x64: //ASCII 'd'

GetRequestedVL53l0xValues(VL53L0X_BOTH);

mySerial.printf("Msec\tLFront\tLCtrc\tLRear\tRFront\tRCtr\tRRear\n");

mySerial.printf("%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\n",

millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);

bIsForwardDir = true;

MoveAhead(speed, speed);

break;

Default:

mySerial.print("In Default Case: Stopping Motors!");

MoveAhead(0, 0);

while (true)

{

}

#pragma endregion Miscellaneous

#pragma region HDG_BASED_TURN_SUPPORT

void BackupAndTurn90Deg(bool b_ccw, bool b_fwd, int motor_speed)

{

//Purpose: Backup 1 second and then turn 90 degrees

//Provenance: G. Frank Paynter 06/27/2020

//Inputs:

// bIsCCW = bool denoting direction for the turn

// bIsFwd = bool denoting fwd/bkwd direction for the turn

// motor_speed = int denoting motor speed to use for the maneuver

//Outputs:

// Robot backs up 1 sec, then turns 90 deg in the specified direction

//Plan:

// Step1: Back up for 1 sec

// Step2: Turn 90 deg in specified direction

//Notes:

// 06/27/20 Replaces BackupAndTurn in all Tracking modes

//Step1: Back up for 1 sec

BackupSignal(true); //turn backup LEDs ON

MoveReverse(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

delay(1000);

BackupSignal(false);//turn them back OFF

//Step2: Turn 90 deg fwd or backward in specified direction

//RollingTurn(b_ccw, b_fwd, 90);

SpinTurn(b_ccw, 90); //06/27/20 experiment

}

bool RollingTurn(bool bIsCCW, bool bIsFwd, float numDeg)

{

//Purpose: Make a numDeg forwards or backwards CW turn

//Inputs:

// bIsFwd - True if turn is to be forward, 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()

//Plan:

// Step1: Get current heading as starting point

// Step2: Compute new target value

// Step3: Set motor speeds based on fwd/backwds flag

// Step4: Run motors until target reached

//Notes:

// 08/22/18 now using MPU6050_CCW_INCREASES_YAWVAL define to compute tgt

// 08/29/18 removed MPU6050_CCW_INCREASES_YAWVAL - the MPU6050 DMP takes care of this

// 09/08/18 revised end-of-turn detection to include slope calc. Threshold alone is too fragile

// 09/11/18 revised to handle both turn directions

// 04/29/19 uncommented

// 07/31/19 rev for better initial heading capture

// 07/31/19 rev return type to bool

// 07/31/19 rev to eliminate timed loop - now runs as fast as possible

// 07/31/19 rev to not stop motors at end - now calling pgm is resp for this

// 10/06/19 rev for new IMU polling setup

// 12/05/19 put MPU6050_CCW_INCREASES_YAWVAL define back in

// 02/29/20 copied here from two wheel robot

// 04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

//DEBUG!!

mySerial.printf("In RollingTurn(%s, %s,%4.2f)\n", bIsCCW == TURNDIR_CCW ? "CCW" : "CW", bIsFwd == true ? "FWD" : "BKWD", numDeg);

//DEBUG!!

//no need to continue if the IMU isn't available

if (!dmpReady)

{

return false;

}

//if we get to here, the IMU is OK and at least one packet is available

IMUHdgValDeg = UpdateIMUHdgValDeg(); //updates IMUHdgValDeg if successful

//Step2: Compute new target value & timeout value

timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG;

//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 = bIsCCW ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;

#else

tgt_deg = bIsCCW ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

//correct for -180/180 transition

if (tgt_deg < -180)

{

tgt_deg += 360;

}

//07/29/19 bugfix

if (tgt_deg > 180)

{

tgt_deg -= 360;

}

//DEBUG!!

//mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n",

// IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);

//DEBUG!!

//Step3: Start motors

if (bIsFwd)

{

//OK, the robot will be moving forward,

if (bIsCCW) //CCW rotation (viewed from above) requires fast on right, slow on left

{

MoveAhead(OFFSIDE_MOTOR_SPEED, DRIVESIDE_MOTOR_SPEED_HIGH);

}

else //CW rotation (viewed from above) requires fast on left, slow on right

{

MoveAhead(DRIVESIDE_MOTOR_SPEED_HIGH, OFFSIDE_MOTOR_SPEED);

}

else

{

//OK, the robot will be moving backward,

if (bIsCCW) //CCW rotation (viewed from above) requires fast on left, slow on right

{

MoveReverse(DRIVESIDE_MOTOR_SPEED_HIGH, OFFSIDE_MOTOR_SPEED);

}

else //CW rotation (viewed from above) requires fast on left, slow on right

{

MoveReverse(OFFSIDE_MOTOR_SPEED, DRIVESIDE_MOTOR_SPEED_HIGH);

}

sinceLastTimeCheck = 0; //needed for watchdog timer

//DEBUG!!

//mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n");

//DEBUG!!

bool bFirstPass = true; //for 'slowdown' print control

float curHdgMatchVal = 0;

//09/08/18 added to bolster end-of-turn detection

float prevHdgMatchVal = 0;

float matchSlope = 0;

//Step4: Run motors until target reached

while (!bDoneTurning && !bTimedOut)

{

CheckForUserInput(); //added 06/29/20

IMUHdgValDeg = UpdateIMUHdgValDeg(); //updates IMUHdgValDeg if successful

//DEBUG!!

//mySerial.printf("%lu\t%4.2f\n", millis(), IMUHdgValDeg);

//DEBUG!!

//07/31/19 now running as fast as we can get valid hdgs from IMU

//check for nearly there and all the way there

curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);

matchSlope = curHdgMatchVal - prevHdgMatchVal;

if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL)

{

if (bFirstPass)

{

prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs

//DEBUG!!

//mySerial.printf("Slowing down at %4.2f deg\n", IMUHdgValDeg);

//DEBUG!!

bFirstPass = false;

}

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start

bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

// if (bIsFirst180)

// {

////DEBUG!!

// mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, slope = %1.3f and bDone = %s\n",

// IMUHdgValDeg, tgt_deg, curHdgMatchVal, matchSlope, (bDoneTurning == true)?"TRUE":"FALSE");

////DEBUG!!

// }

if (bTimedOut)

{

//DEBUG!!

//mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

//DEBUG!!

//return false;

bResult = false;

}

////DEBUG!!

// mySerial.printf("%lu: Exiting RollingTurn()\n", millis());

////DEBUG!!

//04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding

StopBothMotors();

//return true;

return bResult;

}

//added 06/27/20 to see if spin turns will work

bool SpinTurn(bool b_ccw, float numDeg)

{

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

//Plan:

// Step1: Get current heading as starting point

// Step2: Compute new target value

// Step3: Set motor speeds based on b_ccw

// Step4: Run motors until target reached

//Notes:

// 06/27/20 copied from RollingTurn & adapted

// 09/02/20 experiment with adjustable turn rate

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

double turnRate = 0; //added 09/02/20

double prev_hdg = 0;

unsigned long prev_uSec; //added 09/02/20

//DEBUG!!

mySerial.printf("In SpinTurn(%s, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg);

//DEBUG!!

//no need to continue if the IMU isn't available

if (!dmpReady)

{

return false;

}

IMUHdgValDeg = UpdateIMUHdgValDeg(); //updates IMUHdgValDeg if successful

prev_hdg = IMUHdgValDeg; //added 09/02/20

//Step2: Compute new target value & timeout value

timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG;

//05/17/20 limit timeout_sec to 1 sec or more

timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

//12/05/19 added #define back in to manage which direction increases yaw values

#ifdef MPU6050_CCW_INCREASES_YAWVAL

tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;

#else

tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

//correct for -180/180 transition

if (tgt_deg < -180)

{

tgt_deg += 360;

}

//07/29/19 bugfix

if (tgt_deg > 180)

{

tgt_deg -= 360;

}

////DEBUG!!

// mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n",

// IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);

////DEBUG!!

//Step3: Start motors

//SetLeftMotorDir(!b_ccw); //left motors go backward for ccw, forward for cw

//SetRightMotorDir(b_ccw); //right motors go forward for ccw, backward for cw

const int TURN_RATE_MOTOR_SPEED_ADJUST_AMOUNT = 20;

const int TURN_RATE_MOTOR_SPEED_MINIMUM = 75;

int spinMotorSpeed = MOTOR_SPEED_FULL;

const int TURN_RATE_TARGET_DEGPERSEC = 75;

//09/08/20 modified for DRV8871 motor driver

SetLeftMotorDirAndSpeed(!b_ccw, spinMotorSpeed);

SetRightMotorDirAndSpeed(b_ccw, spinMotorSpeed);

sinceLastTimeCheck = 0; //needed for watchdog timer

////DEBUG!!

// mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n");

////DEBUG!!

bool bFirstPass = true; //for 'slowdown' print control

float curHdgMatchVal = 0;

//09/08/18 added to bolster end-of-turn detection

float prevHdgMatchVal = 0;

float matchSlope = 0;

//Step4: Run motors until target reached

prev_uSec = micros();

while (!bDoneTurning && !bTimedOut)

{

IMUHdgValDeg = UpdateIMUHdgValDeg(); //updates IMUHdgValDeg if successful

turnRate = 1e6 * abs(IMUHdgValDeg - prev_hdg) / (micros() - prev_uSec);

prev_uSec = micros();

prev_hdg = IMUHdgValDeg;

//09/03/20 see if I can modulate the turn rate

if (turnRate > TURN_RATE_TARGET_DEGPERSEC)

{

spinMotorSpeed -= TURN_RATE_MOTOR_SPEED_ADJUST_AMOUNT;

}

else if (turnRate < TURN_RATE_TARGET_DEGPERSEC)

{

spinMotorSpeed += TURN_RATE_MOTOR_SPEED_ADJUST_AMOUNT;

}

//bound spinMotorSpeed

spinMotorSpeed < TURN_RATE_MOTOR_SPEED_MINIMUM ? spinMotorSpeed = TURN_RATE_MOTOR_SPEED_MINIMUM : spinMotorSpeed;

spinMotorSpeed > MOTOR_SPEED_MAX ? spinMotorSpeed = MOTOR_SPEED_MAX : spinMotorSpeed;

SetLeftMotorDirAndSpeed(!b_ccw, spinMotorSpeed);

SetRightMotorDirAndSpeed(b_ccw, spinMotorSpeed);

//DEBUG!!

//mySerial.printf("SpinTurn: %lu\t%4.2f\t%4.2f\t%2.4f\t%d\n", millis(), IMUHdgValDeg, prev_hdg, turnRate, spinMotorSpeed);

//DEBUG!!

//07/31/19 now running as fast as we can get valid hdgs from IMU

//check for nearly there and all the way there

curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);

matchSlope = curHdgMatchVal - prevHdgMatchVal;

if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL)

{

if (bFirstPass)

{

prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs

//DEBUG!!

//mySerial.printf("Slowing down at %4.2f deg\n", IMUHdgValDeg);

//DEBUG!!

bFirstPass = false;

}

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start

bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

// if (bIsFirst180)

// {

////DEBUG!!

// mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, slope = %1.3f and bDone = %s\n",

// IMUHdgValDeg, tgt_deg, curHdgMatchVal, matchSlope, (bDoneTurning == true)?"TRUE":"FALSE");

////DEBUG!!

// }

if (bTimedOut)

{

//DEBUG!!

//mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);

//DEBUG!!

//return false;

bResult = false;

}

//DEBUG!!

mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg);

//DEBUG!!

//04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding

StopBothMotors();

//return true;

return bResult;

}

float GetHdgMatchVal(float tgt_deg, float cur_deg)

{

//Purpose: Compute the match ratio between two compass headings

//Inputs:

// tgt_deg = float representing target heading in +/-180 range

// IMUHdgValDeg = float representing sensor yaw value in +/-180 deg range

//Outputs:

// returns result of 1 - abs(Tgt_deg - Hdg_deg)/180, all angles in 0-360 deg range

//Plan:

// Step1: convert both inputs to 0-360 deg range

// Step2: compute match ratio

//Notes:

// formula from https://gis.stackexchange.com/questions/129954/comparing-compass-bearings

//Step1: convert both inputs to 0-360 deg range

float tgthdg = (tgt_deg < 0) ? tgt_deg + 360 : tgt_deg;

float curhdg = (cur_deg < 0) ? cur_deg + 360 : cur_deg;

//Step2: compute match ratio

float match_ratio = 1 - abs(tgthdg - curhdg) / 180;

//DEBUG!!

//mySerial.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio);

//DEBUG!!

return abs(match_ratio);

}

float UpdateIMUHdgValDeg()

{

//Purpose: Get latest yaw (heading) value from IMU

//Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE

//Outputs:

// returns true if successful, otherwise false

// IMUHdgValDeg updated on success

//Plan:

//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet

//Step2: read all available packets to get to latest data

//Step3: update IMUHdgValDeg with latest value

//Notes:

// 10/08/19 changed return type to boolean

// 10/08/19 no longer need mpuIntStatus

// 10/21/19 completely rewritten to use Homer's algorithm

// 05/05/20 changed return type to float vs bool.

bool retval = false;

int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow

{

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

IMUHdgValDeg = ypr[0] * 180 / M_PI;

retval = true;

}

//return retval;

return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience

}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)

{

mpu.resetFIFO();

delay(1);

//int countloop = 0;

fifoCount = mpu.getFIFOCount();

GetPacketLoopCount = 0;

//mySerial.printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);

//while (mpu.getFIFOCount() < packetSize && GetPacketLoopCount < max_loops)

while (fifoCount < packetSize && GetPacketLoopCount < max_loops)

{

GetPacketLoopCount++;

fifoCount = mpu.getFIFOCount();

delay(2);

}

//mySerial.printf("In GetCurrentFIFOPacket: after loop fifoC = %d, loop count = %d\n", fifoCount, GetPacketLoopCount);

if (GetPacketLoopCount >= max_loops)

{

return 0;

}

//if we get to here, there should be exactly one packet in the FIFO

//mySerial.printf("In GetCurrentFIFOPacket: before getFIFOBytes fifoC = %d\n",fifoCount);

mpu.getFIFOBytes(data, packetSize);

return 1;

}

//04/10/20 dummy function for now

void StepTurn(WallTrackingCases trackingside)

{

mySerial.printf("In StepTurn(Tracking %s)\n", TrkStrArray[trackingside]);

//RollingTurn(trackingside == TRACKING_RIGHT, true, 90.f);

SpinTurn(trackingside == TRACKING_RIGHT, 90.f);

}

int GetTrackTurnDeg(int error, int& trkwinmult)

{

//Purpose: Compute new tracking turn value

//Inputs:

// error = int value representing difference between actual and desired

// offset distance from near wall

// trkwinmult = int multiplication factor (tracking gain) to be applied to

// distance error thresholds

//Outputs:

// integer representing the required heading change, in degrees

//DEBUG!!

mySerial.printf("GetTrackTurnDeg(%d, %d)\n", error, trkwinmult);

//DEBUG!!

int turndeg = 0;

if (abs(error) > m_TrkErrWinMult * VERY_FAR_AWAY_CM) //use 20-deg cut

{

turndeg = VERY_FAR_AWAY_TURN_DEG;

}

else if (abs(error) > m_TrkErrWinMult * FAR_AWAY_CM) //use 10-deg cut

{

turndeg = FAR_AWAY_TURN_DEG;

}

else if (abs(error) > m_TrkErrWinMult * CLOSE_CM) //use 5-deg cut

{

turndeg = CLOSE_TURN_DEG;

m_TrkErrWinMult = WALL_TRK_ERR_WIN_MULTFACT;

}

else //must be close to desired offset distance. Reduce error window size

{

//mySerial.printf("Close to target distance - setting turndeg to zero\n");

turndeg = 0;

m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //go back to approach window sizes

}

return turndeg;

}

//added 02/16/20 for wall tracking support

int GetNearWallDistCm(bool bTrackingRight)

{

return bTrackingRight ? GetRightDistCm() : GetLeftDistCm();

}

#pragma endregion HDG_BASED_TURN_SUPPORT

#pragma region VL53L0X Sensor Support

bool GetRequestedVL53l0xValues(VL53L0X_REQUEST which)

{

//Purpose: Obtain VL53L0X ToF sensor data from Teensy sensor handler

//Inputs:

// which = VL53L0X_REQUEST value denoting which combination of value to retrieve

// VL53L0X_CENTERS_ONLY -> Just the left/right center sensor values

// VL53L0X_RIGHT -> All three right sensor values, in front/center/rear order

// VL53L0X_LEFT -> All three left sensor values, in front/center/rear order

// VL53L0X_BOTH -> All six sensor values, in left/right front/center/rear order

//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_BOTH request type

//Step1: Send request to VL53L0X handler

//DEBUG!!

//mySerial.printf("Sending %d to slave\n", which);

//DEBUG!!

Wire.beginTransmission(VL53L0X_I2C_SLAVE_ADDRESS);

I2C_writeAnything((uint8_t)which);

Wire.endTransmission();

//Step2: get the requested data

int readResult = 0;

int data_size = 0;

switch (which)

{

case VL53L0X_CENTERS_ONLY:

//just two data values needed here

data_size = 2 * sizeof(int);

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, (int)(2 * sizeof(Lidar_RightCenter)));

readResult = I2C_readAnything(Lidar_RightCenter);

if (readResult > 0)

{

I2C_readAnything(Lidar_LeftCenter);

}

//DEBUG!!

//mySerial.printf("VL53L0X_CENTERS_ONLY case: Got LC/RC = %d, %d\n", Lidar_LeftCenter, Lidar_RightCenter);

//DEBUG!!

break;

case VL53L0X_RIGHT:

//four data values needed here

data_size = 3 * sizeof(int) + sizeof(float);

//DEBUG!!

//mySerial.printf("data_size = %d\n", data_size);

//DEBUG!!

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

readResult = I2C_readAnything(Lidar_RightFront);

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_RightCenter);

}

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_RightRear);

}

if (readResult > 0)

{

readResult = I2C_readAnything(RightSteeringVal);

}

//DEBUG!!

//mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",

// Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, ToFSteeringVal);

//DEBUG!!

break;

case VL53L0X_LEFT:

//four data values needed here

data_size = 3 * sizeof(int) + sizeof(float);

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

readResult = I2C_readAnything(Lidar_LeftFront);

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_LeftCenter);

}

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_LeftRear);

}

if (readResult > 0)

{

readResult = I2C_readAnything(LeftSteeringVal);

}

//DEBUG!!

//mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",

// Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, ToFSteeringVal);

//DEBUG!!

break;

case VL53L0X_BOTH: //added 08/05/20

//eight data values needed here

data_size = 6 * sizeof(int) + 2 * sizeof(float);

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

//left side

Wire.requestFrom(VL53L0X_I2C_SLAVE_ADDRESS, data_size);

readResult = I2C_readAnything(Lidar_LeftFront);

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_LeftCenter);

}

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_LeftRear);

}

if (readResult > 0)

{

readResult = I2C_readAnything(LeftSteeringVal);

}

//right side

readResult = I2C_readAnything(Lidar_RightFront);

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_RightCenter);

}

if (readResult > 0)

{

readResult = I2C_readAnything(Lidar_RightRear);

}

if (readResult > 0)

{

readResult = I2C_readAnything(RightSteeringVal);

}

default:

break;

}

return readResult > 0; //this is true only if all reads succeed

}

#pragma endregion VL53L0X Support

Stay Tuned!

Frank

Post Views: 861

Paynter's Palace

Left Side Wall Tracking Success With VL53L0X Array

4 thoughts on “Left Side Wall Tracking Success With VL53L0X Array”

Leave a Reply Cancel reply