Tag Archives: robots

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

Replacing HC-SRO4 Ultrasonic Sensors with VL53L0X Arrays Part V

1 Reply

I have been running some wall-tracking tests with Wall-E2 and the new VL53L0X sensor array arrangement, but have been having poor results, especially with offset capture. After a bunch of test runs, I started to think that the distances aren’t updating fast enough to keep up with the robot’s forward speed, so it runs into the wall before it knows that it has gotten too close

Looking at the Teensy 3.5 I2C Slave code that manages the sensor array, I see the following loop() code

void loop()
{
  RF_Dist_mm = lidar_RF.readRangeSingleMillimeters();
  RC_Dist_mm = lidar_RC.readRangeSingleMillimeters();
  RR_Dist_mm = lidar_RR.readRangeSingleMillimeters();

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

  LF_Dist_mm = lidar_LF.readRangeSingleMillimeters();
  LC_Dist_mm = lidar_LC.readRangeSingleMillimeters();
  LR_Dist_mm = lidar_LR.readRangeSingleMillimeters();

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

  Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\n",
    millis(),
    LF_Dist_mm, LC_Dist_mm, LR_Dist_mm,
    RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, 
    LeftSteeringVal, RightSteeringVal);

  delay(100);
}

void loop()

{

RF_Dist_mm = lidar_RF.readRangeSingleMillimeters();

RC_Dist_mm = lidar_RC.readRangeSingleMillimeters();

RR_Dist_mm = lidar_RR.readRangeSingleMillimeters();

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

LF_Dist_mm = lidar_LF.readRangeSingleMillimeters();

LC_Dist_mm = lidar_LC.readRangeSingleMillimeters();

LR_Dist_mm = lidar_LR.readRangeSingleMillimeters();

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

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

millis(),

LF_Dist_mm, LC_Dist_mm, LR_Dist_mm,

RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,

LeftSteeringVal, RightSteeringVal);

delay(100);

}

And I get the following output:

Msec	LFront	LCtr	LRear	RFront	RCtr	RRear	LSteer	RSteer
1578	578	557	456	193	256	296	1.22	-1.03
1896	587	572	446	193	261	297	1.41	-1.04
2214	593	563	447	193	268	286	1.46	-0.93
2531	587	580	450	193	248	295	1.37	-1.02
2849	581	554	444	193	253	297	1.37	-1.04
3166	589	568	450	192	259	294	1.39	-1.02
3484	582	579	449	195	254	290	1.33	-0.95
3801	579	562	459	194	263	291	1.20	-0.97
4119	590	559	464	196	263	288	1.26	-0.92
4437	572	594	465	192	253	297	1.07	-1.05
4754	595	587	446	190	261	293	1.49	-1.03

Msec LFront LCtr LRear RFront RCtr RRear LSteer RSteer

1578 578 557 456 193 256 296 1.22 -1.03

1896 587 572 446 193 261 297 1.41 -1.04

2214 593 563 447 193 268 286 1.46 -0.93

2531 587 580 450 193 248 295 1.37 -1.02

2849 581 554 444 193 253 297 1.37 -1.04

3166 589 568 450 192 259 294 1.39 -1.02

3484 582 579 449 195 254 290 1.33 -0.95

3801 579 562 459 194 263 291 1.20 -0.97

4119 590 559 464 196 263 288 1.26 -0.92

4437 572 594 465 192 253 297 1.07 -1.05

4754 595 587 446 190 261 293 1.49 -1.03

Looking at the timestamps, it appears that a measurement cycle takes about 200 mSec, taking into account the added 100 mSec delay from the delay(100); statement. This is consistent with the default 30 mSec measurement time for a single VL53L0X, but unfortunately this is much greater than the default 100 mSec PID controller update rate.

The VL53L0X can make measurements faster, but at the cost of lower accuracy. In my case, the increased accuracy from a 30 mSec measurement time is useless if it isn’t fast enough to keep up with the robot. Searching around the net, I found this post on the Pololu support forum, dealing with just this problem. So, I modified my Teensy 3.5 I2C Slave program to use ‘continuous measurements and the shorter (20 mSec vs 30 mSec) timing budget, as follows:

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

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

*/


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

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

*/

#include <Wire.h> //using Wire.h here instead of i2c_t3.h for multiple I2C bus access
#include <VL53L0X.h>
#include "I2C_Anything.h" //local version modified to use Wire.h vs SBWire

volatile uint8_t request_type;


enum VL53L0X_REQUEST
{
  VL53L0X_CENTERS_ONLY,
  VL53L0X_RIGHT,
  VL53L0X_LEFT,
  VL53L0X_BOTH //added 08/05/20
};


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

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

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

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

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

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

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


float RightSteeringVal;
float LeftSteeringVal;

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

void setup()
{
  Serial.begin(115200);

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

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

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

  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);

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

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

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

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

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

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


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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

void loop()
{
  //from Pololu forum post
  RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();
  RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();
  RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

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

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

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

  Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\n",
    millis(),
    LF_Dist_mm, LC_Dist_mm, LR_Dist_mm,
    RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,
    LeftSteeringVal, RightSteeringVal);
}

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

Name: TeensyHexVL53L0XHighSpeedDemo.ino

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

Author: FRANKNEWXPS15\Frank

This demo program combines the code from my Teensy_VL53L0X_I2C_Slave_V2

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

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

Name: Teensy_VL53L0X_I2C_Slave_V2.ino

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

Author: FRANKNEWXPS15\Frank

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

with my previous Teensy_VL53L0X_I2C_Slave project, which used the Adafruit VL53L0X

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

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

#include <Wire.h> //using Wire.h here instead of i2c_t3.h for multiple I2C bus access

#include <VL53L0X.h>

#include "I2C_Anything.h" //local version modified to use Wire.h vs SBWire

volatile uint8_t request_type;

enum VL53L0X_REQUEST

{

VL53L0X_CENTERS_ONLY,

VL53L0X_RIGHT,

VL53L0X_LEFT,

VL53L0X_BOTH //added 08/05/20

};

//right side array

VL53L0X lidar_RF(&Wire1);

VL53L0X lidar_RC(&Wire1);

VL53L0X lidar_RR(&Wire1);

//left side array

VL53L0X lidar_LF(&Wire2);

VL53L0X lidar_LC(&Wire2);

VL53L0X lidar_LR(&Wire2);

//XSHUT required for I2C address init

//right side array

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

//left side array

const int LF_XSHUT_PIN = 5;

const int LC_XSHUT_PIN = 6;

const int LR_XSHUT_PIN = 7;

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

//right side array

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

//left side array

uint16_t LF_Dist_mm;

uint16_t LC_Dist_mm;

uint16_t LR_Dist_mm;

float RightSteeringVal;

float LeftSteeringVal;

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

const int DEFAULT_VL53L0X_ADDR = 0x29;

void setup()

{

Serial.begin(115200);

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

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

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

Wire1.begin();

Wire2.begin();

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

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

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(LF_XSHUT_PIN, OUTPUT);

pinMode(LC_XSHUT_PIN, OUTPUT);

pinMode(LR_XSHUT_PIN, OUTPUT);

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

digitalWrite(RF_XSHUT_PIN, LOW);

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

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

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

pinMode(RF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RF.init())

{

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

while (1) {}

}

//from Pololu forum post

lidar_RF.setMeasurementTimingBudget(20000);

lidar_RF.startContinuous();

lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);

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

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

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

pinMode(RC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RC.init())

{

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

while (1) {}

}

//from Pololu forum post

lidar_RC.setMeasurementTimingBudget(20000);

lidar_RC.startContinuous();

lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);

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

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

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

pinMode(RR_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RR.init())

{

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

while (1) {}

}

//from Pololu forum post

lidar_RR.setMeasurementTimingBudget(20000);

lidar_RR.startContinuous();

lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);

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

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

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

pinMode(LF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LF.init())

{

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

while (1) {}

}

//from Pololu forum post

lidar_LF.setMeasurementTimingBudget(20000);

lidar_LF.startContinuous();

lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);

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

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

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

pinMode(LC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LC.init())

{

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

while (1) {}

}

//from Pololu forum post

lidar_LC.setMeasurementTimingBudget(20000);

lidar_LC.startContinuous();

lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);

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

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

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

pinMode(LR_XSHUT_PIN, INPUT);

if (!lidar_LR.init())

{

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

while (1) {}

}

//from Pololu forum post

lidar_LR.setMeasurementTimingBudget(20000);

lidar_LR.startContinuous();

lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);

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

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

}

void loop()

{

//from Pololu forum post

RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();

RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();

RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

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

//from Pololu forum post

LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();

LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();

LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();

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

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

millis(),

LF_Dist_mm, LC_Dist_mm, LR_Dist_mm,

RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,

LeftSteeringVal, RightSteeringVal);

}

with the following results:

Opening port
Port open
lidar_RF address is 0x2a
lidar_RC address is 0x2b
lidar_RR address is 0x2c
lidar_LF address is 0x2d
lidar_LC address is 0x2e
lidar_LR address is 0x2f
Msec	LFront	LCtr	LRear	RFront	RCtr	RRear	LSteer	RSteer
1238	530	488	424	195	271	297	1.06	-1.02
1256	532	516	411	190	261	294	1.21	-1.04
1274	558	525	429	193	249	296	1.29	-1.03
1293	545	526	418	194	254	293	1.27	-0.99
1311	532	516	411	196	270	284	1.21	-0.88
1329	528	516	411	193	274	295	1.17	-1.02
1347	547	515	412	190	259	303	1.35	-1.13
1365	542	503	404	191	261	295	1.38	-1.04
1383	556	536	425	199	250	295	1.31	-0.96
1401	551	536	415	195	247	296	1.36	-1.01
1420	525	516	411	190	265	300	1.14	-1.10
1438	540	530	415	205	257	299	1.25	-0.94
1456	532	508	420	201	280	298	1.12	-0.97
1474	525	521	411	202	246	295	1.14	-0.93
1492	544	529	429	186	247	294	1.15	-1.08
1511	533	528	423	197	260	288	1.10	-0.91
1529	544	522	417	193	251	287	1.27	-0.94
1547	549	530	420	200	271	296	1.29	-0.96
1565	545	496	424	203	261	287	1.21	-0.84
1583	544	504	410	198	269	301	1.34	-1.03
1601	553	537	409	194	262	299	1.44	-1.05
1619	551	522	409	191	264	290	1.42	-0.99
1638	548	537	430	195	256	292	1.18	-0.97
1656	538	505	420	186	252	293	1.18	-1.07
1674	538	516	423	194	265	284	1.15	-0.90
1692	535	529	429	193	258	296	1.06	-1.03
1710	544	514	428	197	260	288	1.16	-0.91
1728	539	528	415	201	270	277	1.24	-0.76
1746	541	519	420	198	248	294	1.21	-0.96
1765	535	522	426	190	265	294	1.09	-1.04
1783	536	521	411	193	254	293	1.25	-1.00
1801	540	522	423	194	265	308	1.17	-1.14
1819	553	540	422	196	263	297	1.31	-1.01

Opening port

Port open

lidar_RF address is 0x2a

lidar_RC address is 0x2b

lidar_RR address is 0x2c

lidar_LF address is 0x2d

lidar_LC address is 0x2e

lidar_LR address is 0x2f

Msec LFront LCtr LRear RFront RCtr RRear LSteer RSteer

1238 530 488 424 195 271 297 1.06 -1.02

1256 532 516 411 190 261 294 1.21 -1.04

1274 558 525 429 193 249 296 1.29 -1.03

1293 545 526 418 194 254 293 1.27 -0.99

1311 532 516 411 196 270 284 1.21 -0.88

1329 528 516 411 193 274 295 1.17 -1.02

1347 547 515 412 190 259 303 1.35 -1.13

1365 542 503 404 191 261 295 1.38 -1.04

1383 556 536 425 199 250 295 1.31 -0.96

1401 551 536 415 195 247 296 1.36 -1.01

1420 525 516 411 190 265 300 1.14 -1.10

1438 540 530 415 205 257 299 1.25 -0.94

1456 532 508 420 201 280 298 1.12 -0.97

1474 525 521 411 202 246 295 1.14 -0.93

1492 544 529 429 186 247 294 1.15 -1.08

1511 533 528 423 197 260 288 1.10 -0.91

1529 544 522 417 193 251 287 1.27 -0.94

1547 549 530 420 200 271 296 1.29 -0.96

1565 545 496 424 203 261 287 1.21 -0.84

1583 544 504 410 198 269 301 1.34 -1.03

1601 553 537 409 194 262 299 1.44 -1.05

1619 551 522 409 191 264 290 1.42 -0.99

1638 548 537 430 195 256 292 1.18 -0.97

1656 538 505 420 186 252 293 1.18 -1.07

1674 538 516 423 194 265 284 1.15 -0.90

1692 535 529 429 193 258 296 1.06 -1.03

1710 544 514 428 197 260 288 1.16 -0.91

1728 539 528 415 201 270 277 1.24 -0.76

1746 541 519 420 198 248 294 1.21 -0.96

1765 535 522 426 190 265 294 1.09 -1.04

1783 536 521 411 193 254 293 1.25 -1.00

1801 540 522 423 194 265 308 1.17 -1.14

1819 553 540 422 196 263 297 1.31 -1.01

From the above it is apparent that the new loop time is about 19 mSec for all six sensors. This is very interesting, as it implies that in ‘continuous’ mode, all six sensors run all the time, and all the readContinuousMillimeters() function does is pull the latest measurement out of a buffer.

As a quick test, I rigged up a ‘fan blade’ (piece of paper taped to a old robot wheel on a motor) as shown, and then ran the program again with the motor spinning the ‘blade’ in front of the left-side sensor array (at about 100 RPM, I think). The plot shows that the sensor response is certainly fast enough to ‘see’ the rise and fall times on the ‘fan blade’.

86647	131	105	50	317	316	320	0.81	-0.03
86665	129	75	50	313	316	318	0.79	-0.05
86683	130	84	47	316	318	319	0.83	-0.03
86700	77	88	53	314	305	317	0.24	-0.03
86719	54	81	50	315	318	315	0.04	0.00
86737	60	80	64	313	307	328	-0.04	-0.15
86755	54	80	90	312	313	324	-0.36	-0.12
86773	56	84	100	316	315	323	-0.44	-0.07
86792	49	79	101	314	309	320	-0.52	-0.06
86809	48	97	98	316	314	321	-0.50	-0.05
86828	65	126	111	316	318	318	-0.46	-0.02
86846	96	110	101	317	306	321	-0.05	-0.04
86864	130	131	105	311	313	321	0.25	-0.10
86882	126	127	107	321	313	321	0.19	0.00
86901	129	110	61	314	313	307	0.68	0.07
86919	132	125	57	313	303	319	0.75	-0.06
86937	130	100	59	310	317	323	0.71	-0.13
86955	126	83	55	314	307	322	0.71	-0.08
86973	120	84	48	316	304	322	0.72	-0.06
86992	75	81	51	316	312	318	0.24	-0.02
87010	55	83	56	317	319	325	-0.01	-0.08
87028	58	79	68	313	322	319	-0.10	-0.06
87046	51	83	95	319	304	317	-0.44	0.02
87065	53	80	102	313	311	327	-0.49	-0.14
87083	49	81	108	313	303	323	-0.59	-0.10
87101	51	96	107	318	302	318	-0.56	0.00
87119	67	134	102	313	319	322	-0.35	-0.09
87137	103	108	111	309	314	319	-0.08	-0.10
87155	129	124	105	309	311	321	0.24	-0.12
87174	136	131	110	313	316	319	0.26	-0.06
87192	133	107	59	323	302	319	0.74	0.04
87210	129	130	55	312	299	315	0.74	-0.03
87228	130	92	57	312	315	323	0.73	-0.11
87247	124	84	54	307	315	323	0.70	-0.16
87265	127	83	51	305	317	322	0.76	-0.17
87283	71	84	51	313	303	319	0.20	-0.06
87301	57	80	50	316	317	320	0.07	-0.04
87319	53	80	68	317	307	318	-0.15	-0.01
87338	53	82	94	317	317	321	-0.41	-0.04
87356	56	81	109	310	315	318	-0.53	-0.08
87374	55	87	113	314	303	315	-0.58	-0.01
87392	52	103	101	309	314	325	-0.49	-0.16
87410	77	135	113	319	310	326	-0.36	-0.07
87428	117	114	105	318	310	322	0.12	-0.04
87447	125	140	106	314	307	323	0.19	-0.09
87465	131	122	94	314	312	326	0.37	-0.12
87483	129	99	65	314	318	320	0.64	-0.06
87501	124	140	59	313	314	325	0.65	-0.12
87520	131	88	55	312	317	323	0.76	-0.11
87538	127	79	50	313	308	321	0.77	-0.08
87556	119	82	49	318	306	322	0.70	-0.04
87574	67	83	52	310	312	319	0.15	-0.09
87592	52	82	54	310	310	322	-0.02	-0.12
87611	60	81	69	313	318	322	-0.09	-0.09
87629	57	80	102	317	298	317	-0.45	0.00
87647	49	85	106	309	303	317	-0.57	-0.08
87665	52	88	95	308	317	318	-0.43	-0.10
87684	56	101	116	313	313	318	-0.60	-0.05
87702	80	137	103	323	302	319	-0.23	0.04
87720	131	102	111	312	331	316	0.20	-0.04
87738	131	135	106	323	307	317	0.25	0.06
87756	126	110	95	310	303	316	0.31	-0.06
87774	133	110	60	315	309	324	0.73	-0.09
87793	128	128	59	314	306	321	0.69	-0.07
87811	127	89	55	317	310	321	0.72	-0.04
87829	128	88	52	320	323	323	0.76	-0.03
87847	115	85	49	315	316	320	0.66	-0.05
87865	63	84	52	315	324	323	0.11	-0.08
87884	51	77	51	313	319	321	0.00	-0.08
87902	56	79	64	306	307	320	-0.08	-0.14
87920	53	79	105	312	311	319	-0.52	-0.07
87938	56	78	100	313	319	317	-0.44	-0.04
87957	53	86	101	316	302	324	-0.48	-0.08
87975	56	113	96	313	302	321	-0.40	-0.08
87993	83	135	110	318	312	321	-0.27	-0.03
88011	127	118	113	307	313	322	0.14	-0.15
88029	125	145	104	318	325	323	0.21	-0.05
88047	128	127	85	315	314	324	0.43	-0.09
88066	125	121	58	319	297	311	0.67	0.08
88084	136	119	60	315	313	321	0.76	-0.06
88102	121	81	51	311	311	319	0.70	-0.08
88120	124	88	48	313	313	316	0.76	-0.03
88139	107	78	48	316	314	325	0.59	-0.09
88157	64	80	55	316	305	320	0.09	-0.04
88175	57	83	59	317	311	324	-0.02	-0.07
88193	49	77	67	318	310	315	-0.18	0.03
88212	51	83	100	312	301	327	-0.49	-0.15

86647 131 105 50 317 316 320 0.81 -0.03

86665 129 75 50 313 316 318 0.79 -0.05

86683 130 84 47 316 318 319 0.83 -0.03

86700 77 88 53 314 305 317 0.24 -0.03

86719 54 81 50 315 318 315 0.04 0.00

86737 60 80 64 313 307 328 -0.04 -0.15

86755 54 80 90 312 313 324 -0.36 -0.12

86773 56 84 100 316 315 323 -0.44 -0.07

86792 49 79 101 314 309 320 -0.52 -0.06

86809 48 97 98 316 314 321 -0.50 -0.05

86828 65 126 111 316 318 318 -0.46 -0.02

86846 96 110 101 317 306 321 -0.05 -0.04

86864 130 131 105 311 313 321 0.25 -0.10

86882 126 127 107 321 313 321 0.19 0.00

86901 129 110 61 314 313 307 0.68 0.07

86919 132 125 57 313 303 319 0.75 -0.06

86937 130 100 59 310 317 323 0.71 -0.13

86955 126 83 55 314 307 322 0.71 -0.08

86973 120 84 48 316 304 322 0.72 -0.06

86992 75 81 51 316 312 318 0.24 -0.02

87010 55 83 56 317 319 325 -0.01 -0.08

87028 58 79 68 313 322 319 -0.10 -0.06

87046 51 83 95 319 304 317 -0.44 0.02

87065 53 80 102 313 311 327 -0.49 -0.14

87083 49 81 108 313 303 323 -0.59 -0.10

87101 51 96 107 318 302 318 -0.56 0.00

87119 67 134 102 313 319 322 -0.35 -0.09

87137 103 108 111 309 314 319 -0.08 -0.10

87155 129 124 105 309 311 321 0.24 -0.12

87174 136 131 110 313 316 319 0.26 -0.06

87192 133 107 59 323 302 319 0.74 0.04

87210 129 130 55 312 299 315 0.74 -0.03

87228 130 92 57 312 315 323 0.73 -0.11

87247 124 84 54 307 315 323 0.70 -0.16

87265 127 83 51 305 317 322 0.76 -0.17

87283 71 84 51 313 303 319 0.20 -0.06

87301 57 80 50 316 317 320 0.07 -0.04

87319 53 80 68 317 307 318 -0.15 -0.01

87338 53 82 94 317 317 321 -0.41 -0.04

87356 56 81 109 310 315 318 -0.53 -0.08

87374 55 87 113 314 303 315 -0.58 -0.01

87392 52 103 101 309 314 325 -0.49 -0.16

87410 77 135 113 319 310 326 -0.36 -0.07

87428 117 114 105 318 310 322 0.12 -0.04

87447 125 140 106 314 307 323 0.19 -0.09

87465 131 122 94 314 312 326 0.37 -0.12

87483 129 99 65 314 318 320 0.64 -0.06

87501 124 140 59 313 314 325 0.65 -0.12

87520 131 88 55 312 317 323 0.76 -0.11

87538 127 79 50 313 308 321 0.77 -0.08

87556 119 82 49 318 306 322 0.70 -0.04

87574 67 83 52 310 312 319 0.15 -0.09

87592 52 82 54 310 310 322 -0.02 -0.12

87611 60 81 69 313 318 322 -0.09 -0.09

87629 57 80 102 317 298 317 -0.45 0.00

87647 49 85 106 309 303 317 -0.57 -0.08

87665 52 88 95 308 317 318 -0.43 -0.10

87684 56 101 116 313 313 318 -0.60 -0.05

87702 80 137 103 323 302 319 -0.23 0.04

87720 131 102 111 312 331 316 0.20 -0.04

87738 131 135 106 323 307 317 0.25 0.06

87756 126 110 95 310 303 316 0.31 -0.06

87774 133 110 60 315 309 324 0.73 -0.09

87793 128 128 59 314 306 321 0.69 -0.07

87811 127 89 55 317 310 321 0.72 -0.04

87829 128 88 52 320 323 323 0.76 -0.03

87847 115 85 49 315 316 320 0.66 -0.05

87865 63 84 52 315 324 323 0.11 -0.08

87884 51 77 51 313 319 321 0.00 -0.08

87902 56 79 64 306 307 320 -0.08 -0.14

87920 53 79 105 312 311 319 -0.52 -0.07

87938 56 78 100 313 319 317 -0.44 -0.04

87957 53 86 101 316 302 324 -0.48 -0.08

87975 56 113 96 313 302 321 -0.40 -0.08

87993 83 135 110 318 312 321 -0.27 -0.03

88011 127 118 113 307 313 322 0.14 -0.15

88029 125 145 104 318 325 323 0.21 -0.05

88047 128 127 85 315 314 324 0.43 -0.09

88066 125 121 58 319 297 311 0.67 0.08

88084 136 119 60 315 313 321 0.76 -0.06

88102 121 81 51 311 311 319 0.70 -0.08

88120 124 88 48 313 313 316 0.76 -0.03

88139 107 78 48 316 314 325 0.59 -0.09

88157 64 80 55 316 305 320 0.09 -0.04

88175 57 83 59 317 311 324 -0.02 -0.07

88193 49 77 67 318 310 315 -0.18 0.03

88212 51 83 100 312 301 327 -0.49 -0.15

03 October 2020 Update

With the above results in mind, I decided to try speeding up the ‘fan blade’ setup to see if I could find out how fast the VL53L0X sensor could go. I thought I should be able to use the shaft encoder setup on the back of the motor to acquire an accurate RPM reading and convert that into ‘milliseconds/blade’ to tell how short of an interval the VL53L0X could detect. As things often happen, determining motor RPM from encoder signals turned out to be a LOT harder than I thought. After a loooonnnng side-trip into geared-motor hell, I wound up more or less disregarding the encoder feature and modified the Teensy 3.5 ‘loop()’ code to produce a direct tachometer reading, as follows:

void loop()
{
  //from Pololu forum post
  RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();
  RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();
  RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

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

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

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

  Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\n",
    millis(),
    LF_Dist_mm, LC_Dist_mm, LR_Dist_mm,
    RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,
    LeftSteeringVal, RightSteeringVal);

  //10/03/20 produce digital 'tachometer' output using 
  //left-front VL53L0X sensor
  if (LF_Dist_mm > 0 && LF_Dist_mm < LEFT_FRONT_TACH_THRESHOLD_MM)
  {
    digitalWriteFast(LEFT_FRONT_TACH_OUT_PIN, HIGH);
  }
  else
  {
    digitalWriteFast(LEFT_FRONT_TACH_OUT_PIN, LOW);
  }
}

void loop()

{

//from Pololu forum post

RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();

RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();

RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

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

//from Pololu forum post

LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();

LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();

LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();

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

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

millis(),

LF_Dist_mm, LC_Dist_mm, LR_Dist_mm,

RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,

LeftSteeringVal, RightSteeringVal);

//10/03/20 produce digital 'tachometer' output using

//left-front VL53L0X sensor

if (LF_Dist_mm > 0 && LF_Dist_mm < LEFT_FRONT_TACH_THRESHOLD_MM)

{

digitalWriteFast(LEFT_FRONT_TACH_OUT_PIN, HIGH);

}

else

{

digitalWriteFast(LEFT_FRONT_TACH_OUT_PIN, LOW);

}

This allowed me to directly monitor ‘effective’ RPM & obstruction frequency. So I set up the experiment using a ‘four blade fan’ as shown below, and monitored the obstruction detector output with my Hanmatek DSO

DSO Output from VL53L0X Obstruction Detection loop() code

As can be seen from the DSO screenshot, the obstruction detection pulse frequency is about 26Hz, with a period of a little over 38 mSec. So it is clear that the VL53L0X running in continuous mode with a timing budget of 20 mSec can easily produce readings every 30 mSec or so.

04 October Update:

The next step was to see if the ‘VL53L0X fast/continuous’ code running on the Teensy VL53L0X sensor array manager would allow the main robot MCU to fetch distance readings faster. To do this, I uncommented the #define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop line in my program to eliminate all code except a short loop displaying distances. Then I took measurements with my 4-blade ‘fan’ running in front of the left-front sensor. I ran the motor voltage up to the point where the Teensy’s blade sensor output was showing about a 20Hz blade rate, and got the following output from the main MCU ‘DISTANCES_ONLY’ loop.

From the above, it is clear that the main MCU code can ‘see’ sensor output changes occurring at 20 Hz (50 mSec period). This should be fast enough to keep up with the physical movement of the robot during offset capture and wall-tracking activities.

In theory, I won’t have to do anything to the main MCU code to enjoy the faster response

Stay Tuned!

Frank

Replacing Wall-E2’s L298N Motor Drivers with Adafruit DRV8871

1 Reply

Posted 07 September 2020,

I’ve been having some ‘issues’ with driving Wall-E2’s Pololu 20D 125:1 12V metal geared motors with the old L298N motor drivers, so I thought it was time to replace them with the Adafruit DRV8871 models used in ‘re-motoring’ my two-wheel robot (see this post for some of the details).

The first step was to review the work I had done earlier replacing the L298N on my two-wheel robot with the same DRV8871 Driver. The two-wheel robot uses a UNO controller, while Wall-E2 uses a Mega, but they are similar enough so porting the wiring and code should be simple enough. The two-wheel robot uses UNO pins 5, 6, 9 & 10 (all PWM lines) for direction and speed control, while Wall-E2 uses pins 8-13 and 36, 38, 40, 42, 44 & 46 for controlling the two L298N motor drivers. My plan is to try using just two motor drivers, one for both left motors, and another for both right motors. If this works, I’ll need just 4 lines, say 8-11 (If I later need to use four drivers vs two, I’ll use 12/13 for one of the ‘extras’ and 6/7 for the other one. Currently 6 is unused and 7 drives the red laser diode, so moving it shouldn’t be a big problem).

So, I replaced the two L298N modules with two DRV8871 modules, and wired both left motors into one driver and both right motors into the other, as shown in the following photos

‘after’ – Dual Adafruit DRV8871 motor drivers

Since I previously replaced an L298N with a DRV8871 in my two-wheel robot, I had already modified all the relevant motor driver code to use the DRV8871 module vs the L298N, so all I had to do was replace the low-level motor interface modules in my four-wheel robot project with the corresponding ones from my two-wheel robot project. The replaced modules were:

SetLeftMotorDirAndSpeed
SetRightMotorDirAndSpeed
StopBothMotors
MoveAhead
MoveReverse

Note that the four-wheel robot code uses separate SetLeft/RightMotorDir & SetLeft/RightMotorSpeed functions, so these needed to be modified or replaced.

SetLeft/RightMotorSpeed() is called from RunBothMotors() & SpinTurn(). I could modify SpinTurn() to call RunBothMotors() instead of calling SetLeft/RightMotorSpeed() directly.

RunBothMotors() is called from RunBothMotorsMsec(), Setup(), MoveReverse() and MoveForward(). Every call to RunBothMotors() is paired with calls to SetLeft/RightMotorDir(), so I could replace Each set of calls with a single call to SetLeft/RightMotorDirAndSpeed(). The only issue with this are the RunBothMotorsMsec() calls in Setup() & ExecDisconManeuver().

I decided to modify the RunBothMotors() & RunBothMotorsMsec() functions to take a direction parameter, and then the functions will simply call SetLeft/RightDirAndSpeed().

10 September 2020 Update:

I got lost in the details of the Wall-E2 code, so I decided to simplify things to check out the new DRV8871-based motor setup. I modified my original ‘Adafruit_DRV8871_Driver_Test’ project to run both motor sets forward and backward from the minimum PWM value (about 50) to max (255) while monitoring the total current for all four motors, as shown in the following Excel plot

Total motor current for motor speed commands from 50-255

As can be seen from the above plot, the total motor current for all four motors is right around 0.8A or about 0.4A per driver – well within the current limits for the DRV8871 module. As a side note, the TO-3 cans on the modules barely got warm, so this looks like a real winner.

Now that I have the technical issues sorted out with respect to the driver replacement project, I can get back to the main project of improving Wall-E2’s wall-following ability.

Stay tuned!

Frank

13 September 2020 Update:

Unfortunately, when I started running the full Wall-E2 code, the right set of motors would go backwards, but not forwards – awkward to say the least. After a lot of troubleshooting, it finally dawned on me that the problem was being caused by my introduction of a TIMER1-based interrupt a while ago to manage ‘stuck’ detection. TIMER1 controls PWM on pins 11 & 12, and one of those pins was being used by the right motor – bummer!

After a LOT of screwing around, I finally decided that the only way to really figure things out was to remove everything but the timer and motor driver code from the program to figure out what timer (if any) I can use for the ‘stuck’ detection interrupt and still have proper motor control.

To that end, I created a new Arduino program ‘TimerISRvsPWMTest.ino’ as shown below:

/*
    Name:       TimerISRvsPWMTest2.ino
    Created:	9/15/2020 11:25:13 AM
    Author:     FRANKNEWXPS15\Frank

		Program to test interaction between TIMER1 ISR and PWM for motor control
*/

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

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

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

//Right Motors paralleled to left DRV8871 driver
const int In1_Right = 10;
const int In2_Right = 11;
#pragma endregion Motor Pin Assignments

#define TIMER_INT_OUTPUT_PIN 31 //scope monitor pin

void setup()
{

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


#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);
#pragma endregion I/O PIN SETUP

	//
#pragma region TIMER_INTERRUPT
	//08/10/20 added Timer interrupt
	//set timer1 interrupt at (1000/MIN_PING_INTERVAL_MSEC)Hz 5Hz a/o 08/10/20
	cli();//stop interrupts
	TCCR1A = 0;// set entire TCCR1A register to 0
	TCCR1B = 0;// same for TCCR1B
	TCNT1 = 0;//initialize counter value to 0

	// set compare match register for 5hz increments
	OCR1A = 3124;// = (16*10^6) / (5*1024) - 1 (must be <65536)
	TCCR1B |= (1 << WGM12);// turn on CTC mode
	TCCR1B |= (1 << CS12) | (1 << CS10);// Set CS10 and CS12 bits for 1024 prescaler
	TIMSK1 |= (1 << OCIE1A);// enable timer compare interrupt
	sei();//allow interrupts

	pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);
#pragma endregion TIMER_INTERRUPT

	while (1)
	{
		digitalWrite(In1_Right, LOW);
		digitalWrite(In2_Right, LOW);
		digitalWrite(In2_Left, LOW);
		digitalWrite(In1_Left, LOW);
		delay(1000);

		digitalWrite(In1_Right, LOW);
		analogWrite(In2_Right, 128);

		digitalWrite(In2_Left, LOW);
		analogWrite(In1_Left, 128);

		delay(2000);

		digitalWrite(In2_Right, LOW);
		analogWrite(In1_Right, 128);


		digitalWrite(In1_Left, LOW);
		analogWrite(In2_Left, 128);

		delay(2000);
	}
}
ISR(TIMER1_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED)
{
	digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);
	delayMicroseconds(10);
	digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);
}


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

void loop()
{

}

100

101

102

103

Name: TimerISRvsPWMTest2.ino

Created: 9/15/2020 11:25:13 AM

Author: FRANKNEWXPS15\Frank

Program to test interaction between TIMER1 ISR and PWM for motor control

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

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

#pragma region MOTOR_PINS

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

const int In1_Left = 8;

const int In2_Left = 9;

//Right Motors paralleled to left DRV8871 driver

const int In1_Right = 10;

const int In2_Right = 11;

#pragma endregion Motor Pin Assignments

#define TIMER_INT_OUTPUT_PIN 31 //scope monitor pin

void setup()

{

Serial.begin(115200);

delay(1000);

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

#pragma endregion I/O PIN SETUP

#pragma region TIMER_INTERRUPT

//08/10/20 added Timer interrupt

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

cli();//stop interrupts

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

TCCR1B = 0;// same for TCCR1B

TCNT1 = 0;//initialize counter value to 0

// set compare match register for 5hz increments

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

TCCR1B |= (1 << WGM12);// turn on CTC mode

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

TIMSK1 |= (1 << OCIE1A);// enable timer compare interrupt

sei();//allow interrupts

pinMode(TIMER_INT_OUTPUT_PIN, OUTPUT);

#pragma endregion TIMER_INTERRUPT

while (1)

{

digitalWrite(In1_Right, LOW);

digitalWrite(In2_Right, LOW);

digitalWrite(In2_Left, LOW);

digitalWrite(In1_Left, LOW);

delay(1000);

digitalWrite(In1_Right, LOW);

analogWrite(In2_Right, 128);

digitalWrite(In2_Left, LOW);

analogWrite(In1_Left, 128);

delay(2000);

digitalWrite(In2_Right, LOW);

analogWrite(In1_Right, 128);

digitalWrite(In1_Left, LOW);

analogWrite(In2_Left, 128);

delay(2000);

}

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

{

digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);

delayMicroseconds(10);

digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);

}

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

//{

// digitalWrite(TIMER_INT_OUTPUT_PIN, HIGH);

// delayMicroseconds(10);

// digitalWrite(TIMER_INT_OUTPUT_PIN, LOW);

//}

void loop()

{

}

With the TIMER1 setup commented out, both motors rotate forwards and backwards no problem. However, as soon as the TIMER1 code was un-commented, the motor driver on pins 10/11 would turn one way but not the other. With an O’scope I could see the PWM waveform on pin 10 but not on pin 11. This is consistent with Timer1 controlling PWM on pins 11,12 & 13.

So, I moved the wire connected to DRV8871 IN2 from 11 to 3 and changed the code to use pins 10 & 3 vs 10 & 11. Now the motor connected to these pins rotates both forward and backwards

Then I went back to my WallE2_V6 project and tried the same trick – moving the IN2 pin from 11 to 3; nope – still doesn’t work. In fact, moving from 11 to any other PWM pin doesn’t work in the WallE2 project, but does in the TimerPWM test project – weird.

So, thinking that maybe there were some timer dependencies hidden in one or more of the libraries being used for the WallE2 project, I copied them all over to the TimerPWM project folder, added them to the project in VS2019, and added them to the project code. After I got everything to compile, I ran the TimerPWM test project successfully using 10 & 7, 10 & 3, 10 & 2, etc (but 10 & 11 still doesn’t work with TIMER1 ISR enabled).

So, it’s not the libraries. Next I changed the timer interrupt code to use TIMER5 instead of TIMER1, moving the pin dependency from 11-13 to 44-46. I confirmed this by changing ‘In1_Right‘ to 44, 45, 46 in turn and moving the physical In1_Right connection to the corresponding pin, noting that the motors don’t rotate properly when driven from any of the affected pins.

Next, I changed the timer interrupt code in WallE2_V6 from TIMER1 to TIMER5 to see if I can get back pin 11 as a PWM pin. Nope – it still doesn’t work in the WallE2 code, nor does pin 7.

Back to the test program. Copied all the ‘pre-setup’ code from WallE2 to the test program; no change – test program still works properly.

After a few back-and-forths of this nature, I eventually narrowed the problem down to the driver modules themselves. A physical inspection revealed that I had forgotten to solder the second pin on both 2-pin screw terminals on one of the drivers – oops! So, like many seemingly intractable technical problems, this one was caused by two independent issues; the use of TIMER1 caused pins 10-12 to be unavailable for motor drive PWM, and the intermittent connections to the left-side motors complicated the symptoms. This was a perfect example of why ‘cutting the problem in half’ (in my case, by eliminating all the Wall-E2 hardware from the problem) is so effective in troubleshooting.

Stay tuned!

Frank

New Wheels for Wall-E2

1 Reply

Posted 24 August 2020, 1402 days into the Covid-19 Lockdown

My autonomous wall-following robot Wall-E2 is now smart enough to reliably follow walls and connect to a charging station, at least in my office ‘sandbox’ testing area, as shown in the following video

However, as can be seen toward the end of the video, Wall-E2 had some trouble and almost got stuck making the third 90 degree turn. Apparently the current thin 90mm wheels just don’t provide enough traction on carpet.

So, I decided to see what I could do about re-wheeling Wall-E2. After some research I found there are now plenty of larger diameter wheels for robots out there, but I couldn’t seem to find a set that would fit Wall-E2 and still allow me to keep the current set of wheel guards. I needed the same (or maybe slightly larger) diameter for ‘road’ clearance, but something less than about 20 mm thick to fit within the current wheel guard dimensions. Then it occurred to me while reading the specs for one of the wheels (ABS for the wheel, and TPU for the tire) that I already had two 3D printers standing around waiting for something to do, and I had a plentiful supply of ABS (or in my case, PETG) and TPU filaments – why not build my own? After all, how hard could it be? As you might guess, that question started what now feels like a 10-year slog through ‘3D printed wheel hell’

I wanted to create a spoked wheel with a hub that would accept a 3mm flatted motor shaft, and I wanted to fit this wheel with a simple TPU treaded tire. The wheel would have small ‘guard rail’ rims that would keep the tire from sliding off.

It started innocently enough with a search through Thingiverse, where I found several SCAD scripts for ‘parameterized’ wheels. Great – just what the doctor ordered! Well, except that the scripts, which may have worked fine for the authors, didn’t do what I wanted. and as soon as I tried to adjust them to fit my design specs, I discovered they were incomplete, buggy, or both.

I had wanted to learn a bit more about SCAD anyway and this seemed like a good project to do that with, so I persevered, and eventually came up with a SCAD design that I liked.

I started with bioconcave’s ‘Highly Modular Wheel_v1.0.scad’ file from Thingiverse, and (after what seemed like years trying to understand what was going on) was able to extract modular pieces into my own ‘FlatTireWheel’ scad script, as follows:

/*
The wheel is defined as having a hub, a rim, and a tire.  Wheel dimensions are overall diameter
and width, i.e. a 100mm x 30mm wheel will be a cylindrical shape with an overall diameter of 100mm
and a height (width) of 30mm.  The rim will be a hollow cylinder with ID = overall diam - tire
thickness - rim thickness, and OD = ID + rim thickness. The cylindrical area between the center of
the wheel and the ID of the rim may be solid or spoked, and there may or may not be a hub.
*/

$fn=150;

wheelDiameter        = 90; //overall diameter of the wheel, including rim &amp;amp; tire
wheelWidth           = 15; //overall width (height) of the wheel, including guardrails
rimThickness         = 5; //rim thickness (part of overall tire diameter)
tireThickness        = 5; //tire thickness (part of overall tire diameter)
guardrailThickness   = 2; //doesn't add to overall tire diameter
guardrailWidth       = 1; //included in overall tire width
spokeThickness       = 9;
numberOfSpokes       = 3;
spokeEccentricity    = 1.5; //how elliptical do the spokes look

//derived values
wheelMinusRimDiameter = wheelDiameter - rimThickness;
rimOD = wheelDiameter - tireThickness;
rimID = rimOD - rimThickness;

// The hub
includeHub           = true; // Set to false to remove the hub and only include the shaft diameter hole. 
hubDiameter          = 15;    // The diameter of the hub portion of the wheel
hubHeight            = 18;    // The total height of the hub
hubZOffset           =  -wheelWidth/2;     // The Z position of the hub, negative numbers from the surface of the wheel 
innerCircleDiameter  = 0;    // The diameter of the solid inner circle under the hub, or zero for none. 

baseFilletRadius     = 2;     // The radius of the fillet (rounded part) between the hub and wheel. 
topFilletRadius      = 2;     // The radius of the fillet (rounded part) at the top of the hub. 
chamferOnly          = false; // Set to true to use chamfers (straight 45-degree angles) instead of fillets. 
concavity = &#91;0,0];

//hardware
shaftDiameter        = 4.5;          // The diameter of the motor shaft
shaftFlatDiameter    = 3.5  ;          // The diameter of the motor shaft at the flat, or shaftDiameter for no flat.

setScrewCount        = 1;          // The number of set screws/nuts to render, spaced evenly around the shaft 
setScrewDiameter     = 3;          // The diameter of the set screw. 3 is the default for an M3 screw. 
setScrewTrap         = &#91;5.4, 2.3]; // Size &#91;indiameter, thickness] of set screw nut. The depth is set automatically.
setScrewNutDiameter  = 5.4;        // The "diameter" of the captive nut, from flat to flat (the "in-diameter")
setScrewNutThickness = 2.3;        // The thickness of the captive nut
setScrewNutOffset    = 0;          // The distance to offset the nut from the center of the material. -/+ = in/out

servoHoleDiameter    = 0;          // The diameter of servo arm hounting holes, or zero if no holes
servoHoleDistance1   = 25;         // Distance across servo horn from hole to hole (0 to ignore)
servoHoleDistance2   = 21;         // Distance across servo horn from hole to hole, rotated 90 degrees (0 to ignore)
servoArmRotation     = 45;         // The total rotation of all servo holes
servoNutTrap         = &#91;4,1.6];    // Size &#91;indiameter, depth] of servo arm captive nut, or 0 (any) for none.

outerNutTrap         = &#91;12.5,0];   // Size &#91;indiameter, depth] of a captive nut, or 0 (any) for none.


wheel_with_tire();

if ( includeHub ) 
{
   translate(&#91;0,0, hubHeight/2 + wheelWidth/2 + hubZOffset - concavity&#91;0]]) 
      hub(hubHeight, hubDiameter, shaftDiameter, shaftFlatDiameter,
         setScrewCount, setScrewTrap, setScrewDiameter, setScrewNutOffset,
         hubZOffset, baseFilletRadius, topFilletRadius, chamferOnly);
}


/////////////////////////////////////////////////////////////////////////////
// Modules...  
/////////////////////////////////////////////////////////////////////////////

// The hub (the part that holds the wheel onto the motor
module hub( height, diameter, shaftDiameter, shaftFlatDiameter, nuts, nutSize, setScrewDiameter, 
	setScrewNutOffset=0,	hubZOffset=0, baseFilletRadius=0, topFilletRadius=0, chamferOnly=false) {

	hubWidth=(diameter-shaftDiameter)/2;

	union() {	
		difference() {

			// Main hub shape
			union() {
				difference() {
					union() {
						cylinder( h=height, r=diameter/2, center=true );
			
						// First chamfer the base...
                  translate(&#91;0,0,hubZOffset])
						rotate_extrude() 
							translate(&#91;diameter/2,-(height/2)-hubZOffset,0])
								polygon(points=&#91;&#91;0,0],&#91;0,baseFilletRadius],&#91;baseFilletRadius,0]]);
					}
			
					// Chamfer the top...
					rotate_extrude() 
						translate(&#91;diameter/2,height/2,0])				
							polygon(points=&#91;&#91;0.5,0.5],&#91;-topFilletRadius-0.5,0.5],&#91;0.5, -topFilletRadius-0.5]]);
			
					// Carve the bottom fillet from the chamfer
					if ( !chamferOnly ) { 
                  translate(&#91;0,0,hubZOffset])
						rotate_extrude() {
							translate(&#91;(diameter/2)+baseFilletRadius,
								-(height-(2*baseFilletRadius))/2-hubZOffset,0]) {
								circle(r=baseFilletRadius);
							}
						}
					}
				}

				// Add the fillet back on top of the top chamfer 
				if (!chamferOnly) {
					rotate_extrude() {
						translate(&#91;
							(diameter/2)-topFilletRadius,
							(height-(2*topFilletRadius))/2,
							0])				
							circle(r=topFilletRadius);
					}
				}
			}
	
			// Remove the bore
         echo(str("shaftDiameter = ", shaftDiameter));
			difference() 
         {
				cylinder( h=height+1, r=shaftDiameter/2, center=true ); 
				translate(&#91;(shaftDiameter-shaftFlatDiameter+1)/2 + (shaftDiameter/2) 
						- (shaftDiameter - shaftFlatDiameter),0,0]) 
					cube( &#91;shaftDiameter-shaftFlatDiameter+1,shaftDiameter,height+2], center=true ); 
			}
	
			// Remove the captive nut
         //08/22/20 gfp bugfix: chg translate() z param to zero so it follows hub offsets
         //08/22/20 gfp bugfix: repl 'boreDiameter' with 'shaftDiameter' to reduce confusion 
			for( i=&#91;0:nuts-1] ) 
         {
				rotate(&#91; 0,0, (360/nuts)*i ])
				translate(&#91;shaftDiameter/2+(diameter-shaftDiameter)/4 +setScrewNutOffset,
						0, 0]) 
            {
					rotate(&#91;0,-90,0]) 
               { 
						captiveNut( nutSize, setScrewDiameter, 
							depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset
								+(shaftDiameter-shaftFlatDiameter), 
							holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);
					}
				}
			}
		}
	}
}



module wheel_with_tire()
{
   difference()
   {
      union()
      {
         cylinder(wheelWidth, rimOD/2, rimOD/2, $fn=150);
         cylinder(guardrailWidth, 
            guardrailThickness + rimOD/2, 
            guardrailThickness + rimOD/2, 
            $fn=150);
         
         translate(v=&#91;0,0,wheelWidth-guardrailWidth])
         {
            cylinder(guardrailWidth, 
               guardrailThickness + rimOD/2, 
               guardrailThickness + rimOD/2, 
               $fn=150);
         }
      }
      
      translate(v=&#91;0,0,-2])
      {
         cylinder(wheelWidth+4, rimID/2, rimID/2, $fn=150);
      }
         
         //08/22/20 remove setscrew access hole
         //extend the hub shaft capture screw hole out past rim, so that
         //it will pierce the rim if the hub sits low enough on the rim
         if ( includeHub ) 
         {
            nutSize=setScrewTrap;

           translate(&#91;0,0, hubHeight/2 + wheelWidth/2 + hubZOffset]) 
            hubHoleCutout(hubHeight, hubDiameter, shaftDiameter, shaftFlatDiameter,
                  setScrewCount, setScrewTrap, setScrewDiameter, setScrewNutOffset,
                  hubZOffset, baseFilletRadius, topFilletRadius, chamferOnly);
         }        
    
   }
   
   if(numberOfSpokes &amp;gt; 0)
   {
      difference()
      {
         for (step = &#91;0:numberOfSpokes-1]) 
            {
               rotate( &#91;0, 0, step*(360/numberOfSpokes)] )
               ellipticalSpoke(wheelWidth, wheelMinusRimDiameter, spokeEccentricity);
            }
            
			// Remove the motor shaft and setscrew nut trap cutouts if necessary
         if(includeHub)
         {
            union()
            {
               //flatted motor shaft cutout
               difference() 
               {
                  cylinder( h=(wheelWidth+hubHeight) + 2, r=shaftDiameter/2, center=true ); 
                  translate(&#91;(shaftDiameter-shaftFlatDiameter+1)/2 + (shaftDiameter/2) 
                        - (shaftDiameter - shaftFlatDiameter),0,0]) 
                     cube( &#91;shaftDiameter-shaftFlatDiameter+1,shaftDiameter,10*hubHeight+2], center=true ); 
               }
               
               //setscrew nut trap cutout
               //08/22/20 gfp bugfix: chg translate() z param to zero so it follows hub offsets
               //08/22/20 gfp bugfix: repl 'boreDiameter' with 'shaftDiameter' to reduce confusion 
//               for( i=&#91;0:nuts-1] ) 
               {
                  hubWidth=(hubDiameter-shaftDiameter)/2;
//                  rotate(&#91; 0,0, (360/nuts)*i ])
//                  translate(&#91;shaftDiameter/2+(hubDiameter-shaftDiameter)/4 +setScrewNutOffset,
//                        0, 0]) 
                  translate(&#91;shaftDiameter/2+(hubDiameter-shaftDiameter)/4 +setScrewNutOffset,0, hubHeight/2 + wheelWidth/2 + hubZOffset]) 
                  {
                     rotate(&#91;0,-90,0]) 
                     {
                       echo(str("captiveNut(",
                        setScrewTrap,
                        ",",setScrewDiameter,
                        ",",hubHeight/2+1,
                        ",",hubWidth/2+setScrewNutOffset
                              +(shaftDiameter-shaftFlatDiameter),
                        ",",hubWidth+baseFilletRadius-setScrewNutOffset,") call in spoke"));  
                        captiveNut( setScrewTrap, setScrewDiameter, 
                           depth=hubHeight/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset
                              +(shaftDiameter-shaftFlatDiameter), 
                           holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);
                     }
                  }
               }
               
            }
         }
         
         //08/22/20 remove setscrew access hole
         //extend the hub shaft capture screw hole out past rim, so that
         //it will pierce the rim if the hub sits low enough on the rim
         if ( includeHub ) 
         {
            
            hubWidth=(wheelDiameter-shaftDiameter)/2;
            nutSize=setScrewTrap;

           translate(&#91;0,0, hubHeight/2 + wheelWidth/2 + hubZOffset]) 
            hubHoleCutout(hubHeight, hubDiameter, shaftDiameter, shaftFlatDiameter,
                  setScrewCount, setScrewTrap, setScrewDiameter, setScrewNutOffset,
                  hubZOffset, baseFilletRadius, topFilletRadius, chamferOnly);
         }        
      }
   }
}




module hubHoleCutout( height, diameter, boreDiameter, shaftFlatDiameter, nuts, nutSize, setScrewDiameter, 
setScrewNutOffset=0,	hubZOffset=0, baseFilletRadius=0, topFilletRadius=0, chamferOnly=false) 
{
   hubWidth=(diameter-boreDiameter)/2;

   union() 
   {	
         // Remove the captive nut
         for( i=&#91;0:nuts-1] ) 
         {
            rotate(&#91; 0,0, (360/nuts)*i ])
//            translate(&#91;boreDiameter/2+(diameter-boreDiameter)/4 +setScrewNutOffset,
//                  0, height/2 - (height+hubZOffset)/2]) 
            translate(&#91;boreDiameter/2+(diameter-boreDiameter)/4 +setScrewNutOffset,
                  0,0]) 
            {
               rotate(&#91;0,-90,0]) 
               { 
                  echo(str("before call to captiveNutHoleExtension(",nutSize, ", ",setScrewDiameter, ", ",depth=height/2+1, ", ",hubWidth/2+setScrewNutOffset
                        +(boreDiameter-shaftFlatDiameter), ", ",hubWidth+baseFilletRadius-setScrewNutOffset));
//*                    captiveNut( nutSize, setScrewDiameter, 
//                     depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset
//                        +(boreDiameter-shaftFlatDiameter), 
//                     holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);
                   captiveNutHoleExtension( nutSize, setScrewDiameter, 
                     depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset
                        +(boreDiameter-shaftFlatDiameter), 
                     holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);
               }
            }
         }
//		}
	}
}



module ellipticalSpoke(width, diameter, eccentricity)
{
   translate(&#91;0,0,width/2])
   intersection()
   {
      cylinder(h=width, r=wheelMinusRimDiameter/2, center = true);
      translate(&#91;spokeEccentricity*wheelMinusRimDiameter/4, 0, 0])
      scale(&#91;spokeEccentricity,1,1])
      {
         difference()
         {
            cylinder(h = width, r = wheelMinusRimDiameter/4, center = true);
            
            //translation and width addition to make sure of good punch-out
            translate(&#91;spokeThickness,0,0])
            cylinder(h = width+4, r = wheelMinusRimDiameter/4, center = true);
         }
      }
   }
}
//circleSpokes( d, wheelWidth, spokeThickness, proportion, numberOfSpokes );
//
//// Circles pattern spokes
//module circleSpokes( wheelDiameter, wheelWidth, spokeThickness, proportion, numberofSpokes ) {
//	echo( "Circles Style..." ); 
//	intersection() {
//#		cylinder( h=wheelWidth, r=wheelDiameter/2, center = true ); 
//
//		for (step = &#91;0:numberofSpokes-1]) {
//		    rotate( &#91;0, 0, step*(360/numberofSpokes)] )
//				circleSpoke(  wheelDiameter, wheelWidth, spokeThickness, proportion );
//		}
//	}
//
//}
//module circleSpoke( wheelDiameter, wheelWidth, spokeThickness, proportion ) {
////	render()
//   echo(str("spokeThickness = ",spokeThickness));
//	let( ox=(wheelDiameter/2)*proportion&#91;0], oy=(wheelDiameter/2)*proportion&#91;1] )
//	let( ix=ox-(spokeThickness*2), iy=oy-(spokeThickness*2) ) 
//   {
//		translate ( &#91;-ox/2, 0, 0] ) 
//      {
//			difference() 
//         {
//				scale(&#91;proportion&#91;0],proportion&#91;1],1])
//#					cylinder( r=wheelDiameter/4, h=wheelWidth, center=true); 
//				scale(&#91;(ix/ox)*proportion&#91;0],(iy/oy)*proportion&#91;1],1])
//					cylinder( r=wheelDiameter/5, h=wheelWidth +1, center=true); 
//			}
//		}
// 	}
//}
// This is the captive nut module I use in several of my designs. 
module captiveNut( nutSize, setScrewHoleDiameter=3, 
	depth=10, holeLengthTop=5, holeLengthBottom=5 )
{
	render()
	union() {
		nut( nutSize ); 
	
		if ( depth &amp;gt; 0 ) 
			translate(&#91;depth/2,0,0]) 
				cube( &#91;depth, nutSize&#91;0], nutSize&#91;1]], center=true );
	
		translate(&#91;0,0,-(nutSize&#91;1]/2)-holeLengthBottom]) 
			cylinder(r=setScrewHoleDiameter/2, h=nutSize&#91;1]+holeLengthTop+holeLengthBottom, $fn=15);
	}
}

// nutSize = &#91;inDiameter,thickness]
module nut( nutSize ) { 
	side = nutSize&#91;0] * tan( 180/6 );
	if ( nutSize&#91;0] * nutSize&#91;1] != 0 ) {
		for ( i = &#91;0 : 2] ) {
			rotate( i*120, &#91;0, 0, 1]) 
				cube( &#91;side, nutSize&#91;0], nutSize&#91;1]], center=true );
		}
	}
}
// This extends the captive nut hole so through the wheel rim for tool access 
module captiveNutHoleExtension( nutSize, setScrewHoleDiameter=3, 
	depth=10, holeLengthTop=5, holeLengthBottom=5 )
{
		translate(&#91;0,0,-wheelDiameter/2]) 
			cylinder(r=1+setScrewDiameter/2, h=wheelDiameter/2, $fn=15);
}

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

The wheel is defined as having a hub, a rim, and a tire. Wheel dimensions are overall diameter

and width, i.e. a 100mm x 30mm wheel will be a cylindrical shape with an overall diameter of 100mm

and a height (width) of 30mm. The rim will be a hollow cylinder with ID = overall diam - tire

thickness - rim thickness, and OD = ID + rim thickness. The cylindrical area between the center of

the wheel and the ID of the rim may be solid or spoked, and there may or may not be a hub.

$fn=150;

wheelDiameter = 90; //overall diameter of the wheel, including rim &amp; tire

wheelWidth = 15; //overall width (height) of the wheel, including guardrails

rimThickness = 5; //rim thickness (part of overall tire diameter)

tireThickness = 5; //tire thickness (part of overall tire diameter)

guardrailThickness = 2; //doesn't add to overall tire diameter

guardrailWidth = 1; //included in overall tire width

spokeThickness = 9;

numberOfSpokes = 3;

spokeEccentricity = 1.5; //how elliptical do the spokes look

//derived values

wheelMinusRimDiameter = wheelDiameter - rimThickness;

rimOD = wheelDiameter - tireThickness;

rimID = rimOD - rimThickness;

// The hub

includeHub = true; // Set to false to remove the hub and only include the shaft diameter hole.

hubDiameter = 15; // The diameter of the hub portion of the wheel

hubHeight = 18; // The total height of the hub

hubZOffset = -wheelWidth/2; // The Z position of the hub, negative numbers from the surface of the wheel

innerCircleDiameter = 0; // The diameter of the solid inner circle under the hub, or zero for none.

baseFilletRadius = 2; // The radius of the fillet (rounded part) between the hub and wheel.

topFilletRadius = 2; // The radius of the fillet (rounded part) at the top of the hub.

chamferOnly = false; // Set to true to use chamfers (straight 45-degree angles) instead of fillets.

concavity = [0,0];

//hardware

shaftDiameter = 4.5; // The diameter of the motor shaft

shaftFlatDiameter = 3.5 ; // The diameter of the motor shaft at the flat, or shaftDiameter for no flat.

setScrewCount = 1; // The number of set screws/nuts to render, spaced evenly around the shaft

setScrewDiameter = 3; // The diameter of the set screw. 3 is the default for an M3 screw.

setScrewTrap = [5.4, 2.3]; // Size [indiameter, thickness] of set screw nut. The depth is set automatically.

setScrewNutDiameter = 5.4; // The "diameter" of the captive nut, from flat to flat (the "in-diameter")

setScrewNutThickness = 2.3; // The thickness of the captive nut

setScrewNutOffset = 0; // The distance to offset the nut from the center of the material. -/+ = in/out

servoHoleDiameter = 0; // The diameter of servo arm hounting holes, or zero if no holes

servoHoleDistance1 = 25; // Distance across servo horn from hole to hole (0 to ignore)

servoHoleDistance2 = 21; // Distance across servo horn from hole to hole, rotated 90 degrees (0 to ignore)

servoArmRotation = 45; // The total rotation of all servo holes

servoNutTrap = [4,1.6]; // Size [indiameter, depth] of servo arm captive nut, or 0 (any) for none.

outerNutTrap = [12.5,0]; // Size [indiameter, depth] of a captive nut, or 0 (any) for none.

wheel_with_tire();

if ( includeHub )

{

translate([0,0, hubHeight/2 + wheelWidth/2 + hubZOffset - concavity[0]])

hub(hubHeight, hubDiameter, shaftDiameter, shaftFlatDiameter,

setScrewCount, setScrewTrap, setScrewDiameter, setScrewNutOffset,

hubZOffset, baseFilletRadius, topFilletRadius, chamferOnly);

}

/////////////////////////////////////////////////////////////////////////////

// Modules...

/////////////////////////////////////////////////////////////////////////////

// The hub (the part that holds the wheel onto the motor

module hub( height, diameter, shaftDiameter, shaftFlatDiameter, nuts, nutSize, setScrewDiameter,

setScrewNutOffset=0, hubZOffset=0, baseFilletRadius=0, topFilletRadius=0, chamferOnly=false) {

hubWidth=(diameter-shaftDiameter)/2;

union() {

difference() {

// Main hub shape

union() {

difference() {

union() {

cylinder( h=height, r=diameter/2, center=true );

// First chamfer the base...

translate([0,0,hubZOffset])

rotate_extrude()

translate([diameter/2,-(height/2)-hubZOffset,0])

polygon(points=[[0,0],[0,baseFilletRadius],[baseFilletRadius,0]]);

}

// Chamfer the top...

rotate_extrude()

translate([diameter/2,height/2,0])

polygon(points=[[0.5,0.5],[-topFilletRadius-0.5,0.5],[0.5, -topFilletRadius-0.5]]);

// Carve the bottom fillet from the chamfer

if ( !chamferOnly ) {

translate([0,0,hubZOffset])

rotate_extrude() {

translate([(diameter/2)+baseFilletRadius,

-(height-(2*baseFilletRadius))/2-hubZOffset,0]) {

circle(r=baseFilletRadius);

}

// Add the fillet back on top of the top chamfer

if (!chamferOnly) {

rotate_extrude() {

translate([

(diameter/2)-topFilletRadius,

(height-(2*topFilletRadius))/2,

0])

circle(r=topFilletRadius);

}

// Remove the bore

echo(str("shaftDiameter = ", shaftDiameter));

difference()

{

cylinder( h=height+1, r=shaftDiameter/2, center=true );

translate([(shaftDiameter-shaftFlatDiameter+1)/2 + (shaftDiameter/2)

- (shaftDiameter - shaftFlatDiameter),0,0])

cube( [shaftDiameter-shaftFlatDiameter+1,shaftDiameter,height+2], center=true );

}

// Remove the captive nut

//08/22/20 gfp bugfix: chg translate() z param to zero so it follows hub offsets

//08/22/20 gfp bugfix: repl 'boreDiameter' with 'shaftDiameter' to reduce confusion

for( i=[0:nuts-1] )

{

rotate([ 0,0, (360/nuts)*i ])

translate([shaftDiameter/2+(diameter-shaftDiameter)/4 +setScrewNutOffset,

0, 0])

{

rotate([0,-90,0])

{

captiveNut( nutSize, setScrewDiameter,

depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset

+(shaftDiameter-shaftFlatDiameter),

holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);

}

module wheel_with_tire()

{

difference()

{

union()

{

cylinder(wheelWidth, rimOD/2, rimOD/2, $fn=150);

cylinder(guardrailWidth,

guardrailThickness + rimOD/2,

$fn=150);

translate(v=[0,0,wheelWidth-guardrailWidth])

{

cylinder(guardrailWidth,

guardrailThickness + rimOD/2,

$fn=150);

}

translate(v=[0,0,-2])

{

cylinder(wheelWidth+4, rimID/2, rimID/2, $fn=150);

}

//08/22/20 remove setscrew access hole

//extend the hub shaft capture screw hole out past rim, so that

//it will pierce the rim if the hub sits low enough on the rim

if ( includeHub )

{

nutSize=setScrewTrap;

translate([0,0, hubHeight/2 + wheelWidth/2 + hubZOffset])

hubHoleCutout(hubHeight, hubDiameter, shaftDiameter, shaftFlatDiameter,

setScrewCount, setScrewTrap, setScrewDiameter, setScrewNutOffset,

hubZOffset, baseFilletRadius, topFilletRadius, chamferOnly);

}

if(numberOfSpokes &gt; 0)

{

difference()

{

for (step = [0:numberOfSpokes-1])

{

rotate( [0, 0, step*(360/numberOfSpokes)] )

ellipticalSpoke(wheelWidth, wheelMinusRimDiameter, spokeEccentricity);

}

// Remove the motor shaft and setscrew nut trap cutouts if necessary

if(includeHub)

{

union()

{

//flatted motor shaft cutout

difference()

{

cylinder( h=(wheelWidth+hubHeight) + 2, r=shaftDiameter/2, center=true );

translate([(shaftDiameter-shaftFlatDiameter+1)/2 + (shaftDiameter/2)

- (shaftDiameter - shaftFlatDiameter),0,0])

cube( [shaftDiameter-shaftFlatDiameter+1,shaftDiameter,10*hubHeight+2], center=true );

}

//setscrew nut trap cutout

//08/22/20 gfp bugfix: chg translate() z param to zero so it follows hub offsets

//08/22/20 gfp bugfix: repl 'boreDiameter' with 'shaftDiameter' to reduce confusion

// for( i=[0:nuts-1] )

{

hubWidth=(hubDiameter-shaftDiameter)/2;

// rotate([ 0,0, (360/nuts)*i ])

// translate([shaftDiameter/2+(hubDiameter-shaftDiameter)/4 +setScrewNutOffset,

// 0, 0])

translate([shaftDiameter/2+(hubDiameter-shaftDiameter)/4 +setScrewNutOffset,0, hubHeight/2 + wheelWidth/2 + hubZOffset])

{

rotate([0,-90,0])

{

echo(str("captiveNut(",

setScrewTrap,

",",setScrewDiameter,

",",hubHeight/2+1,

",",hubWidth/2+setScrewNutOffset

+(shaftDiameter-shaftFlatDiameter),

",",hubWidth+baseFilletRadius-setScrewNutOffset,") call in spoke"));

captiveNut( setScrewTrap, setScrewDiameter,

depth=hubHeight/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset

+(shaftDiameter-shaftFlatDiameter),

holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);

}

//08/22/20 remove setscrew access hole

//extend the hub shaft capture screw hole out past rim, so that

//it will pierce the rim if the hub sits low enough on the rim

if ( includeHub )

{

hubWidth=(wheelDiameter-shaftDiameter)/2;

nutSize=setScrewTrap;

translate([0,0, hubHeight/2 + wheelWidth/2 + hubZOffset])

hubHoleCutout(hubHeight, hubDiameter, shaftDiameter, shaftFlatDiameter,

setScrewCount, setScrewTrap, setScrewDiameter, setScrewNutOffset,

hubZOffset, baseFilletRadius, topFilletRadius, chamferOnly);

}

module hubHoleCutout( height, diameter, boreDiameter, shaftFlatDiameter, nuts, nutSize, setScrewDiameter,

setScrewNutOffset=0, hubZOffset=0, baseFilletRadius=0, topFilletRadius=0, chamferOnly=false)

{

hubWidth=(diameter-boreDiameter)/2;

union()

{

// Remove the captive nut

for( i=[0:nuts-1] )

{

rotate([ 0,0, (360/nuts)*i ])

// translate([boreDiameter/2+(diameter-boreDiameter)/4 +setScrewNutOffset,

// 0, height/2 - (height+hubZOffset)/2])

translate([boreDiameter/2+(diameter-boreDiameter)/4 +setScrewNutOffset,

0,0])

{

rotate([0,-90,0])

{

echo(str("before call to captiveNutHoleExtension(",nutSize, ", ",setScrewDiameter, ", ",depth=height/2+1, ", ",hubWidth/2+setScrewNutOffset

+(boreDiameter-shaftFlatDiameter), ", ",hubWidth+baseFilletRadius-setScrewNutOffset));

//* captiveNut( nutSize, setScrewDiameter,

// depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset

// +(boreDiameter-shaftFlatDiameter),

// holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);

captiveNutHoleExtension( nutSize, setScrewDiameter,

depth=height/2+1, holeLengthTop=hubWidth/2+setScrewNutOffset

+(boreDiameter-shaftFlatDiameter),

holeLengthBottom=hubWidth+baseFilletRadius-setScrewNutOffset);

}

// }

}

module ellipticalSpoke(width, diameter, eccentricity)

{

translate([0,0,width/2])

intersection()

{

cylinder(h=width, r=wheelMinusRimDiameter/2, center = true);

translate([spokeEccentricity*wheelMinusRimDiameter/4, 0, 0])

scale([spokeEccentricity,1,1])

{

difference()

{

cylinder(h = width, r = wheelMinusRimDiameter/4, center = true);

//translation and width addition to make sure of good punch-out

translate([spokeThickness,0,0])

cylinder(h = width+4, r = wheelMinusRimDiameter/4, center = true);

}

//circleSpokes( d, wheelWidth, spokeThickness, proportion, numberOfSpokes );

//// Circles pattern spokes

//module circleSpokes( wheelDiameter, wheelWidth, spokeThickness, proportion, numberofSpokes ) {

// echo( "Circles Style..." );

// intersection() {

//# cylinder( h=wheelWidth, r=wheelDiameter/2, center = true );

// for (step = [0:numberofSpokes-1]) {

// rotate( [0, 0, step*(360/numberofSpokes)] )

// circleSpoke( wheelDiameter, wheelWidth, spokeThickness, proportion );

// }

//}

//module circleSpoke( wheelDiameter, wheelWidth, spokeThickness, proportion ) {

//// render()

// echo(str("spokeThickness = ",spokeThickness));

// let( ox=(wheelDiameter/2)*proportion[0], oy=(wheelDiameter/2)*proportion[1] )

// let( ix=ox-(spokeThickness*2), iy=oy-(spokeThickness*2) )

// {

// translate ( [-ox/2, 0, 0] )

// {

// difference()

// {

// scale([proportion[0],proportion[1],1])

//# cylinder( r=wheelDiameter/4, h=wheelWidth, center=true);

// scale([(ix/ox)*proportion[0],(iy/oy)*proportion[1],1])

// cylinder( r=wheelDiameter/5, h=wheelWidth +1, center=true);

// }

//}

// This is the captive nut module I use in several of my designs.

module captiveNut( nutSize, setScrewHoleDiameter=3,

depth=10, holeLengthTop=5, holeLengthBottom=5 )

{

render()

union() {

nut( nutSize );

if ( depth &gt; 0 )

translate([depth/2,0,0])

cube( [depth, nutSize[0], nutSize[1]], center=true );

translate([0,0,-(nutSize[1]/2)-holeLengthBottom])

cylinder(r=setScrewHoleDiameter/2, h=nutSize[1]+holeLengthTop+holeLengthBottom, $fn=15);

}

// nutSize = [inDiameter,thickness]

module nut( nutSize ) {

side = nutSize[0] * tan( 180/6 );

if ( nutSize[0] * nutSize[1] != 0 ) {

for ( i = [0 : 2] ) {

rotate( i*120, [0, 0, 1])

cube( [side, nutSize[0], nutSize[1]], center=true );

}

// This extends the captive nut hole so through the wheel rim for tool access

module captiveNutHoleExtension( nutSize, setScrewHoleDiameter=3,

depth=10, holeLengthTop=5, holeLengthBottom=5 )

{

translate([0,0,-wheelDiameter/2])

cylinder(r=1+setScrewDiameter/2, h=wheelDiameter/2, $fn=15);

}

Here’s a screenshot of a completed 86mm wheel with elliptical spokes and a hub compatible with a 3mm flatted shaft. When a TPU tire is added to the rim, the assembly should be about 90mm in diameter.

SCAD-generated wheel with elliptical spokes and a hub compatible with a 3mm flatted shaft. Note extensions to set screw hole through spoke and rim

One of the many issues I had with the original code is it assumed the hub would sit on top of the spokes, and therefore there was no need to worry about whether or not the setscrew arrangement would be blocked by the spokes and/or rim. Since I wanted a wheel that was mostly tread, I wanted to ‘sink’ the hub into the spokes as shown above. In order to make this work, I needed to extend the setscrew access hole through the spoke assembly and through the rim. In the finished design, the hub assembly can be moved freely up and down in the center of the wheel, and the hole extensions will follow. If the hub setscrew hole isn’t blocked by the spokes and/or rim, then the extensions don’t do anything; otherwise they extend the setscrew hole as shown above.

Here’s a photo of separate wheel/tire pieces, and a completed wheel/tire combination on Wall-E2

Separate wheel & tire parts, plus a completed wheel on Wall-E2

2 September 2020 Update:

After running some sandbox tests with my new wheels, I discovered that the new tires didn’t have much more traction than the old ones. However, now that I ‘had the technology’, it was a fairly simple task to design and print new tires to fit onto the existing new rims. Rather than do the tire design in SCAD, I found it much much easier to do this in TinkerCad. Here’s a couple of screenshots showing the TCad design.

Original printed tire on the left, new one on the right. Note more aggressive tread on new tire

Exploded view showing construction technique used for new (and old) tire. Very easy to do in TinkerCad!

03 September 2020 Update:

The increased traction provided by the new tires have caused a new problem; on a hard surface the rotation during a ‘spin turn’ (one side’s motors going forward, the other going in reverse) is too fast, causing the robot to slide well past the target heading. Not so much of a problem on carpet, but how would the robot know which surface is in play at the moment. After some thought, I decided to try and modulate the turn rate, in deg/sec, as a proxy for the surface type. So, in ‘SpinTurn()’ I put in some code to monitor the turn rate and adjust the motor speeds upward or downward to try and keep the turn rate at a reasonable level.

Here’s a video of a recent run utilizing the new ‘SpinTurn’ rate modulation algorithm

And the data from the three ‘spin turn’ executions, one on hard surface, and two on carpet.

Spin Turn executions; hard surface, followed by two carpet turns

As can be seen in the above video and plots, the motor speeds used on the hard surface turn is much lower than the speeds used during the carpet turns, as would be expected. This is a much nicer result than the ‘fire and forget’ algorithm used before. Moreover, the carpet turns are much more positive now thanks to the more aggressive tread on the new tires – yay!

Offloading Distance Measurements from Wall-E2’s Main (Mega) MCU

1 Reply

Posted 08 August 2020,

Now that I have the two 3-element VL53L0X proximity sensors integrated into Wall-E2, my autonomous wall following robot, I have been running some ‘field’ tests in my ‘sandbox’ test area, as shown in the photo below.

‘Sandbox’ field test area. Very simple layout with no obstacles (other than my feet)

As can be seen from the above video, Wall-E2 ‘ran into’ a problem at the end of the second leg. The problem is caused by the extended time required for finding the parallel orientation and capturing the desired wall offset. During this time, Wall-E2 isn’t checking the front distance for upcoming obstacles, and isn’t checking for the ‘isStuck’ condition. In the function that homes to the IR beam on a charging station have the following guard code:

	while (bChgConn == LOW && !bIsStuck && frontdist > avoidancedistCm)
	{

1 2	while (bChgConn == LOW && !bIsStuck && frontdist > avoidancedistCm) {

that checks for a ‘stuck’ condition or an upcoming obstacle. I could also put this same guard code in the functions that handle parallel orientation and offset acquisition/tracking, but it occurred to me that I might want to try off-loading this responsibility to the newly-added Teensy 3.5 I2C slave that manages the dual 3-element VL53L0X proximity sensors. This Teensy spends 99.9% of its time just waiting for the next distance check interval to appear on the horizon, so it would probably welcome something else to do. I could route the LIDAR-lite front LIDAR data to it as well, which would give it all the distance sensors to play with. It could then do the forward/left/right distance array updates, and calculate the forward distance variance that is the heart of the ‘isStuck()’ function. This is all great, but I’m not sure how all that gets integrated back into the main Mega MCU processing loop.

I currently have a ‘receiveEvent(int numBytes)’ implemented on the Teensy to receive a ‘request type’ value from the Mega. This value determines what dataset (left, right, both, just centers, etc) gets sent back to the Mega at the next ‘requestEvent()’ event (triggered by a ‘Wire.requestFrom()’ call from the Mega). So, I could simply add some more request types to ‘GetRequestedVL53l0xValues(VL53L0X_REQUEST which)’ or better yet, add a new function to the Mega to specifically request things like front distance or front distance variance (or simply have the Mega request that the Teensy report the current value of the ‘bisStuck’ boolean variable.

I could also set up a Mega input as an interrupt pin with a CHANGE condition, and have the Teensy interrupt the Mega whenever the ‘stuck’ condition changed (from ‘not stuck’ to ‘stuck, or vice versa). The Mega’s ISR would simply set the ‘bisStuck’ boolean variable to the state of the interrupt input (HIGH or LOW).

Of course, this all presumes there is some real advantage to moving this functionality to the Teensy. If the Mega isn’t processing-challenged, there is no reason to do all this. As this post shows, the time cost for the incremental variance calculation on the Mega is only about 225 uSec, or less than 0.5% of the current 200 mSec loop time.

And, looking at the timestamps on the last actual ‘sandbox’ run, I see that the timestamps during the wall-tracking portion were pretty constant – between 197 and 202 mSec – not the kind of data one would expect from an MCU struggling to keep up.

10 August 2020 Update:

After realizing that the Mega really wasn’t having much problem keeping up with a 200 mSec loop cycle, I went ahead and added the ‘!bIsStuck’ guard to both the ‘RotateToParallelOrientation()’ and ‘CaptureWallOffset()’ functions, thinking this would solve the problem. What actually happened is that as soon as Wall-E2 started running, it immediately sensed a ‘Stuck’ condition and started backing up – WTF! Some head-scratching and some troubleshooting revealed that the while() loops in which I had put the new guard code were running much faster than the main loop (which runs at 200 mSec intervals via use of a ‘elapsedMillis’ variable). What this meant was that it was calculating the forward distance variance hundreds of times per second rather than five, and the forward distance wasn’t changing nearly fast enough to prevent the variance from quickly winding down to zero – oops! In order to fix this problem I had to install some more ‘elapseMillis’ variables to make sure that CalcDistArrayVariance() was called at the same rate as in the main loop, namely every MIN_PING_INTERVAL_MSEC mSec. This works, but now I not only had more ‘Stuck’ guard code scattered throughout my code, but additional kludge-code needed to make the ‘Stuck’ kludge-code work – YUK!!

After thinking about this a bit more, I realized there was another option I hadn’t considered – a timer interrupt set at a convenient interval (like MIN_PING_INTERVAL_MSEC mSec as I am doing now) that does nothing but calculate front distance variance and set bIsStuck accordingly. I haven’t used any timer interrupts up to this point in my Arduino journey, but this seems like a perfect application.

As I normally do, I grabbed a spare Arduino Mega from my parts box, Googled for some timer interrupt examples, and created a demo program to test my theory. First I just did the normal ‘blink without delay’ demo, copying the ‘Timer1’ portion of the code from this post to a new project, and then modifying the ISR to call my ‘CalcDistArrayVariance()’ function with a constant number for the ‘frontdist’ parameter. The CalcDistArrayVariance() function computes the variance of a 25-element array of distance values, and feeding a constant into the function simulates what would happen if the robot gets stuck at some point.

I set up the program to show how long it takes to calculate the variance each time, and how long it takes for the variance to fall to zero. When I ran the program, I got this output:

Port open
Timer Interrupt Example
last_incmean = 220.00, last_incvar = 14400.00
Msec	uSec	Var
199	96	12416.000
399	152	10664.961
599	156	9149.446
798	156	7868.160
998	156	6816.000
1198	152	5984.000
1397	156	5359.352
1597	156	4925.437
1797	156	4661.750
1996	156	4544.000
2196	152	4544.000
2396	156	4629.750
2595	156	4765.437
2795	160	4911.352
2995	160	5024.000
3194	156	5056.000
3394	156	4956.160
3594	156	4669.445
3793	156	4136.961
3993	160	3296.000
4193	156	2351.359
4392	156	1552.637
4592	160	898.559
4792	160	384.000
4993	152	0.000

Port open

Timer Interrupt Example

last_incmean = 220.00, last_incvar = 14400.00

Msec uSec Var

199 96 12416.000

399 152 10664.961

599 156 9149.446

798 156 7868.160

998 156 6816.000

1198 152 5984.000

1397 156 5359.352

1597 156 4925.437

1797 156 4661.750

1996 156 4544.000

2196 152 4544.000

2396 156 4629.750

2595 156 4765.437

2795 160 4911.352

2995 160 5024.000

3194 156 5056.000

3394 156 4956.160

3594 156 4669.445

3793 156 4136.961

3993 160 3296.000

4193 156 2351.359

4392 156 1552.637

4592 160 898.559

4792 160 384.000

4993 152 0.000

As can be seen from the above, calls to CalcDistArrayVariance() occur at 200 mSec intervals and each call takes about 150 uSec (insignificant compared to the 200 mSec loop interval), and it takes about 5 seconds for a constant distance input to produce zero variance on the output. This is pretty much perfect for my application. Before implementing the timer idea, I had over 20 calls to IsStuck() scattered throughout my code, and now they can all be replaced by the boolean bIsStuck variable, which is managed by the timer1 ISR.

Here’s the entire timer interrupt demo code:

/*
    Name:       TimerInterruptDemo.ino
    Created:	8/10/2020 10:40:48 AM
    Author:     FRANKNEWXPS15\Frank
*/
#include <PrintEx.h> //allows printf-style printout syntax
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

//storage variables
boolean toggle1 = 0;

const int MAX_FRONT_DISTANCE_CM = 400;
const int FRONT_DIST_ARRAY_SIZE = 25; //04/28/19 - final value (I hope)
uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte

//11/03/18 added for new incremental variance calc
double last_incvar = 0;
double last_incmean = 0;

void setup()
{
  Serial.begin(115200);
  mySerial.printf("Timer Interrupt Example\n");
  cli();//stop interrupts

  //set timer1 interrupt at 1Hz
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1 = 0;//initialize counter value to 0

  // set compare match register for 1hz increments
  //OCR1A = 15624;// = (16*10^6) / (1*1024) - 1 (must be <65536)
  OCR1A = 3124;// = (16*10^6) / (5*1024) - 1 (must be <65536)

  // turn on CTC mode
  TCCR1B |= (1 << WGM12);

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

  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);


  sei();//allow interrupts

  pinMode(LED_BUILTIN, OUTPUT);

	//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 = 20;
	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("last_incmean = %3.2f, last_incvar = %3.2f\n", last_incmean, last_incvar);
}

ISR(TIMER1_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED)
{
	uint32_t now = micros();
	float frontvar = CalcDistArrayVariance(200, aFrontDist);
	mySerial.printf("%lu\t%lu\t%2.3f\n", millis(), micros() - now, frontvar);
//generates pulse wave of frequency 1Hz/2 = 0.5kHz (takes two cycles for full wave- toggle high then toggle low)
  if (toggle1) 
  {
    digitalWrite(13, HIGH);
    toggle1 = 0;
  }
  else 
  {
    digitalWrite(13, LOW);
    toggle1 = 1;
  }
}


void loop()
{


}

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%4.2f\t%4.2f\t%4.2f\t%4.2f\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\t%4.2f\t%lu\t%lu\n",
	//	newdistval, oldestDistVal, brute_mean, brute_var, last_incmean, last_incvar, olddist_squared,
	//	newdist_squared, inc_mean, inc_mean_squared, last_incmean_squared, inc_var, uSecB, uSecI);
	//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
}

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

Name: TimerInterruptDemo.ino

Created: 8/10/2020 10:40:48 AM

Author: FRANKNEWXPS15\Frank

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

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

//storage variables

boolean toggle1 = 0;

const int MAX_FRONT_DISTANCE_CM = 400;

const int FRONT_DIST_ARRAY_SIZE = 25; //04/28/19 - final value (I hope)

uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte

//11/03/18 added for new incremental variance calc

double last_incvar = 0;

double last_incmean = 0;

void setup()

{

Serial.begin(115200);

mySerial.printf("Timer Interrupt Example\n");

cli();//stop interrupts

//set timer1 interrupt at 1Hz

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

TCCR1B = 0;// same for TCCR1B

TCNT1 = 0;//initialize counter value to 0

// set compare match register for 1hz increments

//OCR1A = 15624;// = (16*10^6) / (1*1024) - 1 (must be <65536)

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

// turn on CTC mode

TCCR1B |= (1 << WGM12);

// Set CS10 and CS12 bits for 1024 prescaler

TCCR1B |= (1 << CS12) | (1 << CS10);

// enable timer compare interrupt

TIMSK1 |= (1 << OCIE1A);

sei();//allow interrupts

pinMode(LED_BUILTIN, OUTPUT);

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

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("last_incmean = %3.2f, last_incvar = %3.2f\n", last_incmean, last_incvar);

}

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

{

uint32_t now = micros();

float frontvar = CalcDistArrayVariance(200, aFrontDist);

mySerial.printf("%lu\t%lu\t%2.3f\n", millis(), micros() - now, frontvar);

//generates pulse wave of frequency 1Hz/2 = 0.5kHz (takes two cycles for full wave- toggle high then toggle low)

if (toggle1)

{

digitalWrite(13, HIGH);

toggle1 = 0;

}

else

{

digitalWrite(13, LOW);

toggle1 = 1;

}

void loop()

{

}

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%4.2f\t%4.2f\t%4.2f\t%4.2f\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\t%4.2f\t%lu\t%lu\n",

// newdistval, oldestDistVal, brute_mean, brute_var, last_incmean, last_incvar, olddist_squared,

// newdist_squared, inc_mean, inc_mean_squared, last_incmean_squared, inc_var, uSecB, uSecI);

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

}

Stay tuned,

Frank

Replacing HC-SRO4 Ultrasonic Sensors with VL53L0X Arrays Part IV

2 Replies

Posted 18 July 2020

In my continuing effort to update Wall-E2’s superpowers, I have been trying to replace the HC-SR04 ‘ping’ sensors with ST Microelectronics VL53L0X Time-of-Flight (ToF) sensors, as implemented by the popular GY530 modules available on eBay.

First, I got a 3-element array working and demonstrated effective parallel-heading determination and wall tracking, as described in this post.

Next, I added a second 3-element array on the other side of the robot, but I have been running into trouble getting both arrays to work properly at the same time. Somehow there seems to be some interaction between the two arrays that I can’t seem to nail down.

I have determined that all six elements respond properly when operated individually or as a member of a 3-element array
Adding a 4th element to the array causes one or more of the first three elements to respond with an out-of-range measurement
Adding 2.2K pullups to the I2C bus makes the problem worse, not better. After some investigation, I discovered that the GY530 module already has 10K pullups included, so three modules on the bus would reduce the pullups to 3.3K, and four would already reduce the value to 2.5K. Adding a 2.2K in parallel with 3.3 or 2.5K would drive the value down to around 1.2-1.5K. However, that did lead me to my next idea – using separate I2C busses for the left and right 3-element arrays.
Moved the left-hand 3-element array from the Wire1 I2C bus to the Wire2 I2C bus. Now the 10K pullups shouldn’t be an issue, as I had already demonstrated proper operation of a 3-element array on the Wire1 I2C bus. Unfortunately, this exhibited similar problems; When running all six elements, all three left-side elements measure properly, but only one right-side element produces reasonable values – the other two give nonsense readings.

Here’s a photo of the top deck of my autonomous wall-following robot, with the two 3-element arrays installed on the Wire1 and Wire2 I2C busses of a Teensy 3.5.

two 3-element VL53L0X arrays installed on a Teensy 3.5 Wire1 & Wire2 busses

And here is the schematic for the split-bus configuration:

A typical output sequence follows: The first column is milliseconds since program start, and the following 6 columns are the front, center, and rear sensor measurements for the right & left arrays, respectively.

Msec	Front	Center	Rear	Front	Center	Rear
9502	1000	1000	133	642	342	379
10396	1000	1000	135	621	334	375
11290	1000	1000	136	1000	340	375
12184	1000	1000	134	667	328	359
13078	1000	1000	133	680	321	377
13973	1000	1000	139	632	335	382
14866	1000	1000	26	592	329	386
15760	1000	1000	28	619	327	383
16654	1000	1000	33	618	333	363
17547	1000	1000	137	64	56	32
18441	1000	1000	138	33	46	28
19334	1000	1000	140	33	43	29
20227	1000	1000	138	36	40	25
21121	1000	1000	140	34	41	23
22015	1000	1000	133	674	293	364
22909	1000	1000	136	615	292	383
23803	1000	1000	139	655	288	384

Msec Front Center Rear Front Center Rear

9502 1000 1000 133 642 342 379

10396 1000 1000 135 621 334 375

11290 1000 1000 136 1000 340 375

12184 1000 1000 134 667 328 359

13078 1000 1000 133 680 321 377

13973 1000 1000 139 632 335 382

14866 1000 1000 26 592 329 386

15760 1000 1000 28 619 327 383

16654 1000 1000 33 618 333 363

17547 1000 1000 137 64 56 32

18441 1000 1000 138 33 46 28

19334 1000 1000 140 33 43 29

20227 1000 1000 138 36 40 25

21121 1000 1000 140 34 41 23

22015 1000 1000 133 674 293 364

22909 1000 1000 136 615 292 383

23803 1000 1000 139 655 288 384

In the above, the data from the first two sensors on the right side is invalid, but all the rest show ‘real’ values.

If the left-side array is disconnected (unplugged) from the Wire2 bus and the program modified to not initialize/measure the left-side array, then the right-side array reads normally, as shown below

Msec	Front	Center	Rear
8182	115	148	102
8765	117	149	103
9349	118	146	100
9932	114	148	100
10515	118	150	100
11098	52	21	28
11680	52	21	27
12262	52	20	103
12846	95	106	52
13428	49	16	31
14010	51	17	27
14593	100	149	116
15176	50	29	23
15758	50	21	118
16341	114	167	117
16925	112	162	114
17508	114	162	116
18092	114	170	117
18675	118	170	115
19258	111	161	113

Msec Front Center Rear

8182 115 148 102

8765 117 149 103

9349 118 146 100

9932 114 148 100

10515 118 150 100

11098 52 21 28

11680 52 21 27

12262 52 20 103

12846 95 106 52

13428 49 16 31

14010 51 17 27

14593 100 149 116

15176 50 29 23

15758 50 21 118

16341 114 167 117

16925 112 162 114

17508 114 162 116

18092 114 170 117

18675 118 170 115

19258 111 161 113

If the right-side array is disconnected (unplugged) from the Wire1 bus and the program modified to not initialize/measure the right-side array, then the left-side array reads normally, as shown below

Msec	Front	Center	Rear
8216	1000	487	1000
8785	1000	450	728
9353	1000	495	1000
9921	1000	495	1000
10490	1000	556	1000
11058	1000	521	33
11625	41	42	28
12192	35	40	26
12759	37	43	25
13327	71	208	1000
13895	1000	220	716
14463	1000	216	1000
15031	1000	219	30
15598	47	47	23
16165	47	49	27
16732	76	85	53
17300	92	116	96
17868	131	146	135
18436	178	262	658
19004	1000	334	1000
19572	1000	325	1000
20140	1000	348	1000
20708	1000	372	680
21276	1000	326	1000
21844	1000	328	806

Msec Front Center Rear

8216 1000 487 1000

8785 1000 450 728

9353 1000 495 1000

9921 1000 495 1000

10490 1000 556 1000

11058 1000 521 33

11625 41 42 28

12192 35 40 26

12759 37 43 25

13327 71 208 1000

13895 1000 220 716

14463 1000 216 1000

15031 1000 219 30

15598 47 47 23

16165 47 49 27

16732 76 85 53

17300 92 116 96

17868 131 146 135

18436 178 262 658

19004 1000 334 1000

19572 1000 325 1000

20140 1000 348 1000

20708 1000 372 680

21276 1000 326 1000

21844 1000 328 806

Here’s the code being used to drive just the left-side VL53L0X array (right-side array code commented out and the right-side array physically disconnected from the Teensy 3.5):

/*
    Name:       Teensy_Hex_V53L0X.ino
    Created:	7/16/2020 2:56:35 PM
    Author:     FRANKNEWXPS15\Frank

    Notes:
      07/18/20: I discovered that only 3 VL53L0X units can share the I2C bus. The GY530
                modules have internal 10K pullups on the I2C lines, and the 4th one on the
                bus reduces the combined pullup value too much.

                Instead, I put the right-hand array of 3 on Wire1 and the left-hand array 
                of 3 on Wire2.
*/


/*
    Name:       Teensy_Triple_VL53L0X_V2.ino
    Created:	5/23/2020 1:16:52 PM
    Author:     FRANKNEWXPS15\Frank
*/


/* This code copied from the following post on the Adafruit forum:
https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5
*/


#include <Wire.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Right-center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//07/16/20 added three left-side sensors
Adafruit_VL53L0X lidar_LF = Adafruit_VL53L0X(); //Left-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_LC = Adafruit_VL53L0X(); //Left-center LIDAR
Adafruit_VL53L0X lidar_LR = Adafruit_VL53L0X(); //left-rear LIDAR (angled 30 deg rearward)


//XSHUT required for I2C address init 
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;
const int LF_XSHUT_PIN = 5;
const int LC_XSHUT_PIN = 6;
const int LR_XSHUT_PIN = 7;

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;

void setup()
{
  Serial.begin(115200); //Teensy ignores rate parameter

  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);
  pinMode(LF_XSHUT_PIN, OUTPUT);
  pinMode(LC_XSHUT_PIN, OUTPUT);
  pinMode(LR_XSHUT_PIN, OUTPUT);

  pinMode(RED_LASER_PIN, OUTPUT);

  // wait until serial port opens for native USB devices
  while (!Serial)
  {
    delay(1);
  }

  delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

  Serial.printf("Teensy Triple VL53L0X V2\n");

  //initialize all six LIDAR sensors
// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
//    all XSHUT high to bring out of reset

  //right side
  digitalWrite(RF_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RC_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RR_XSHUT_PIN, LOW);
  delay(10);

  //left side
  digitalWrite(LF_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(LC_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(LR_XSHUT_PIN, LOW);
  delay(10);

  //right side
  digitalWrite(RF_XSHUT_PIN, HIGH);
  digitalWrite(RC_XSHUT_PIN, HIGH);
  digitalWrite(RR_XSHUT_PIN, HIGH);

  //left side
  digitalWrite(LF_XSHUT_PIN, HIGH);
  digitalWrite(LC_XSHUT_PIN, HIGH);
  digitalWrite(LR_XSHUT_PIN, HIGH);

  delay(1000);


  // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
  //    shutdown by pulling XSHUT pins low

  //right side
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  //left side
  digitalWrite(LF_XSHUT_PIN, LOW);
  digitalWrite(LC_XSHUT_PIN, LOW);
  digitalWrite(LR_XSHUT_PIN, LOW);


  // 4. Initialize sensor #1 
  //if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))
  //if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, true, &Wire1))
  //{
  //  Serial.println(F("Failed to boot lidar_RF"));
  //  while (1);
  //}

  //// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
  //// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
  ////    you set the first sensor to
  //digitalWrite(RC_XSHUT_PIN, HIGH);
  ////if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
  //if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, true, &Wire1))
  //{
  //  Serial.println(F("Failed to boot lidar_RC"));
  //  while (1);
  //}

  //// 7. Repeat for each sensor, turning each one on, setting a unique address.
  //digitalWrite(RR_XSHUT_PIN, HIGH);
  ////if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
  //if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, true, &Wire1))
  //{
  //  Serial.println(F("Failed to boot lidar_RR"));
  //  while (1);
  //}

  //07/15/20 do same thing for left side sensors
  // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
  //    shutdown by pulling XSHUT pins low

  // 4. Initialize sensor #1 
  digitalWrite(LF_XSHUT_PIN, HIGH);
  //if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire1))
  if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, true, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_LF"));
    while (1);
  }

  // 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
  // 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
  //    you set the first sensor to
  digitalWrite(LC_XSHUT_PIN, HIGH);
  if (!lidar_LC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_RC"));
    while (1);
  }

  // 7. Repeat for each sensor, turning each one on, setting a unique address.
  digitalWrite(LR_XSHUT_PIN, HIGH);
  if (!lidar_LR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_LR"));
    while (1);
  }



  //turn the laser OFF
  digitalWrite(RED_LASER_PIN, LOW);

  Serial.printf("\nInit complete\n \nVL53L0X API Simple Ranging example\n \n");

  ////05/23/20 experiment with accessing API functions
  //VL53L0X_Dev_t RightFrontDevice = lidar_RF.GetDeviceHandle();
  //VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();
  //Serial.printf("Device Info for Right Front Sensor\n");
  //Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",
  //  RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,
  //  RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,
  //  RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

  ////get measurement timing budget?
  //prog_uint32_t MeasBudgetUSec;
  ////VL53L0X_Error ApiError = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);
  //VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);
  //Serial.printf("Right Front Measurement Budget (uSec) = %d\n", MeasBudgetUSec);
  //delay(1000);
  //Serial.printf("Changing budget from %d to %d....\n", MeasBudgetUSec, 2 * MeasBudgetUSec);
  //delay(1000);
  //MeasBudgetUSec = 2 * MeasBudgetUSec;
  ////ApiError = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);
  //VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);
  //Serial.printf("Right Front Measurement Budget (uSec) Now = %d\n \n", MeasBudgetUSec);



  Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");
}

void loop()
{
  //VL53L0X_RangingMeasurementData_t measure1;
  //VL53L0X_RangingMeasurementData_t measure2;
  //VL53L0X_RangingMeasurementData_t measure3;
  VL53L0X_RangingMeasurementData_t measure4;
  VL53L0X_RangingMeasurementData_t measure5;
  VL53L0X_RangingMeasurementData_t measure6;

  //turn the laser ON
  digitalWrite(RED_LASER_PIN, HIGH);

  //lidar_RF.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
  ////lidar_RF.rangingTest(&measure1, true); // pass in 'true' to get debug data printout!
  //if (measure1.RangeStatus != 4)  // phase failures have incorrect data
  //{
  //  RF_Dist_mm = measure1.RangeMilliMeter;
  //  if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
  //}
  //else RF_Dist_mm = 0;

  //delay(100);

  //lidar_RC.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
  ////lidar_RC.rangingTest(&measure2, true); // pass in 'true' to get debug data printout!
  //if (measure2.RangeStatus != 4)  // phase failures have incorrect data
  //{
  //  RC_Dist_mm = measure2.RangeMilliMeter;
  //  if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
  //}
  //else RC_Dist_mm = 0;

  //delay(100);

  //lidar_RR.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!
  ////lidar_RR.rangingTest(&measure3, true); // pass in 'true' to get debug data printout!
  //if (measure3.RangeStatus != 4)  // phase failures have incorrect data
  //{
  //  RR_Dist_mm = measure3.RangeMilliMeter;
  //  if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
  //}
  //else RR_Dist_mm = 0;

  ////07/16/20 added left side
  lidar_LF.rangingTest(&measure4, false); // pass in 'true' to get debug data printout!
  //lidar_LF.rangingTest(&measure4, true); // pass in 'true' to get debug data printout!
  if (measure4.RangeStatus != 4)  // phase failures have incorrect data
  {
    LF_Dist_mm = measure4.RangeMilliMeter;
    if (LF_Dist_mm > MAX_LIDAR_RANGE_MM) LF_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else LF_Dist_mm = 0;

  delay(100);

  lidar_LC.rangingTest(&measure5, false); // pass in 'true' to get debug data printout!
  if (measure5.RangeStatus != 4)  // phase failures have incorrect data
  {
    LC_Dist_mm = measure5.RangeMilliMeter;
    if (LC_Dist_mm > MAX_LIDAR_RANGE_MM) LC_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else LC_Dist_mm = 0;

  delay(100);

  lidar_LR.rangingTest(&measure6, false); // pass in 'true' to get debug data printout!
  if (measure6.RangeStatus != 4)  // phase failures have incorrect data
  {
    LR_Dist_mm = measure6.RangeMilliMeter;
    if (LR_Dist_mm > MAX_LIDAR_RANGE_MM) LR_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RR_Dist_mm = 0;



  //float RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;
  //float LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / (float)LC_Dist_mm;
  //float RightSteeringVal = (RF_Dist_mm - RR_Dist_mm);

  //Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),
  //  RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);
  //Serial.printf("%lu\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);
  Serial.printf("%lu\t%d\t%d\t%d\n", millis(), LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);
  //Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,
    //LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

  //Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%3.2f\n", millis(),
  //  RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal,
  //  LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

  //turn the laser OFF
  digitalWrite(RED_LASER_PIN, LOW);

  //delay(100);
  delay(250);
}

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

Name: Teensy_Hex_V53L0X.ino

Created: 7/16/2020 2:56:35 PM

Author: FRANKNEWXPS15\Frank

Notes:

07/18/20: I discovered that only 3 VL53L0X units can share the I2C bus. The GY530

modules have internal 10K pullups on the I2C lines, and the 4th one on the

bus reduces the combined pullup value too much.

Instead, I put the right-hand array of 3 on Wire1 and the left-hand array

of 3 on Wire2.

Name: Teensy_Triple_VL53L0X_V2.ino

Created: 5/23/2020 1:16:52 PM

Author: FRANKNEWXPS15\Frank

/* This code copied from the following post on the Adafruit forum:

https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5

#include <Wire.h>

#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Right-center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//07/16/20 added three left-side sensors

Adafruit_VL53L0X lidar_LF = Adafruit_VL53L0X(); //Left-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_LC = Adafruit_VL53L0X(); //Left-center LIDAR

Adafruit_VL53L0X lidar_LR = Adafruit_VL53L0X(); //left-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

const int LF_XSHUT_PIN = 5;

const int LC_XSHUT_PIN = 6;

const int LR_XSHUT_PIN = 7;

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

uint16_t LF_Dist_mm;

uint16_t LC_Dist_mm;

uint16_t LR_Dist_mm;

void setup()

{

Serial.begin(115200); //Teensy ignores rate parameter

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(LF_XSHUT_PIN, OUTPUT);

pinMode(LC_XSHUT_PIN, OUTPUT);

pinMode(LR_XSHUT_PIN, OUTPUT);

pinMode(RED_LASER_PIN, OUTPUT);

// wait until serial port opens for native USB devices

while (!Serial)

{

delay(1);

}

delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

Serial.printf("Teensy Triple VL53L0X V2\n");

//initialize all six LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

//right side

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

//left side

digitalWrite(LF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(LC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(LR_XSHUT_PIN, LOW);

delay(10);

//right side

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

//left side

digitalWrite(LF_XSHUT_PIN, HIGH);

digitalWrite(LC_XSHUT_PIN, HIGH);

digitalWrite(LR_XSHUT_PIN, HIGH);

delay(1000);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

//right side

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

//left side

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

// 4. Initialize sensor #1

//if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))

//if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, true, &Wire1))

//{

// Serial.println(F("Failed to boot lidar_RF"));

// while (1);

//}

//// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

//// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

//// you set the first sensor to

//digitalWrite(RC_XSHUT_PIN, HIGH);

////if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))

//if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, true, &Wire1))

//{

// Serial.println(F("Failed to boot lidar_RC"));

// while (1);

//}

//// 7. Repeat for each sensor, turning each one on, setting a unique address.

//digitalWrite(RR_XSHUT_PIN, HIGH);

////if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))

//if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, true, &Wire1))

//{

// Serial.println(F("Failed to boot lidar_RR"));

// while (1);

//}

//07/15/20 do same thing for left side sensors

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

// 4. Initialize sensor #1

digitalWrite(LF_XSHUT_PIN, HIGH);

//if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire1))

if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, true, &Wire2))

{

Serial.println(F("Failed to boot lidar_LF"));

while (1);

}

// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(LC_XSHUT_PIN, HIGH);

if (!lidar_LC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 7. Repeat for each sensor, turning each one on, setting a unique address.

digitalWrite(LR_XSHUT_PIN, HIGH);

if (!lidar_LR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))

{

Serial.println(F("Failed to boot lidar_LR"));

while (1);

}

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

Serial.printf("\nInit complete\n \nVL53L0X API Simple Ranging example\n \n");

////05/23/20 experiment with accessing API functions

//VL53L0X_Dev_t RightFrontDevice = lidar_RF.GetDeviceHandle();

//VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();

//Serial.printf("Device Info for Right Front Sensor\n");

//Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",

// RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,

// RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,

// RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

////get measurement timing budget?

//prog_uint32_t MeasBudgetUSec;

////VL53L0X_Error ApiError = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);

//VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);

//Serial.printf("Right Front Measurement Budget (uSec) = %d\n", MeasBudgetUSec);

//delay(1000);

//Serial.printf("Changing budget from %d to %d....\n", MeasBudgetUSec, 2 * MeasBudgetUSec);

//delay(1000);

//MeasBudgetUSec = 2 * MeasBudgetUSec;

////ApiError = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);

//VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);

//Serial.printf("Right Front Measurement Budget (uSec) Now = %d\n \n", MeasBudgetUSec);

Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");

}

void loop()

{

//VL53L0X_RangingMeasurementData_t measure1;

//VL53L0X_RangingMeasurementData_t measure2;

//VL53L0X_RangingMeasurementData_t measure3;

VL53L0X_RangingMeasurementData_t measure4;

VL53L0X_RangingMeasurementData_t measure5;

VL53L0X_RangingMeasurementData_t measure6;

//turn the laser ON

digitalWrite(RED_LASER_PIN, HIGH);

//lidar_RF.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!

////lidar_RF.rangingTest(&measure1, true); // pass in 'true' to get debug data printout!

//if (measure1.RangeStatus != 4) // phase failures have incorrect data

//{

// RF_Dist_mm = measure1.RangeMilliMeter;

// if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

//}

//else RF_Dist_mm = 0;

//delay(100);

//lidar_RC.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!

////lidar_RC.rangingTest(&measure2, true); // pass in 'true' to get debug data printout!

//if (measure2.RangeStatus != 4) // phase failures have incorrect data

//{

// RC_Dist_mm = measure2.RangeMilliMeter;

// if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

//}

//else RC_Dist_mm = 0;

//delay(100);

//lidar_RR.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!

////lidar_RR.rangingTest(&measure3, true); // pass in 'true' to get debug data printout!

//if (measure3.RangeStatus != 4) // phase failures have incorrect data

//{

// RR_Dist_mm = measure3.RangeMilliMeter;

// if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

//}

//else RR_Dist_mm = 0;

////07/16/20 added left side

lidar_LF.rangingTest(&measure4, false); // pass in 'true' to get debug data printout!

//lidar_LF.rangingTest(&measure4, true); // pass in 'true' to get debug data printout!

if (measure4.RangeStatus != 4) // phase failures have incorrect data

{

LF_Dist_mm = measure4.RangeMilliMeter;

if (LF_Dist_mm > MAX_LIDAR_RANGE_MM) LF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else LF_Dist_mm = 0;

delay(100);

lidar_LC.rangingTest(&measure5, false); // pass in 'true' to get debug data printout!

if (measure5.RangeStatus != 4) // phase failures have incorrect data

{

LC_Dist_mm = measure5.RangeMilliMeter;

if (LC_Dist_mm > MAX_LIDAR_RANGE_MM) LC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else LC_Dist_mm = 0;

delay(100);

lidar_LR.rangingTest(&measure6, false); // pass in 'true' to get debug data printout!

if (measure6.RangeStatus != 4) // phase failures have incorrect data

{

LR_Dist_mm = measure6.RangeMilliMeter;

if (LR_Dist_mm > MAX_LIDAR_RANGE_MM) LR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

//float RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

//float LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / (float)LC_Dist_mm;

//float RightSteeringVal = (RF_Dist_mm - RR_Dist_mm);

//Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),

// RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);

//Serial.printf("%lu\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

Serial.printf("%lu\t%d\t%d\t%d\n", millis(), LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

//Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,

//LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

//Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%3.2f\n", millis(),

// RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal,

// LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

//delay(100);

delay(250);

}

And the same program, with the left-side array commented out

/*
    Name:       Teensy_Hex_V53L0X.ino
    Created:	7/16/2020 2:56:35 PM
    Author:     FRANKNEWXPS15\Frank

    Notes:
      07/18/20: I discovered that only 3 VL53L0X units can share the I2C bus. The GY530
                modules have internal 10K pullups on the I2C lines, and the 4th one on the
                bus reduces the combined pullup value too much.

                Instead, I put the right-hand array of 3 on Wire1 and the left-hand array 
                of 3 on Wire2.
*/


/*
    Name:       Teensy_Triple_VL53L0X_V2.ino
    Created:	5/23/2020 1:16:52 PM
    Author:     FRANKNEWXPS15\Frank
*/


/* This code copied from the following post on the Adafruit forum:
https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5
*/


#include <Wire.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Right-center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//07/16/20 added three left-side sensors
Adafruit_VL53L0X lidar_LF = Adafruit_VL53L0X(); //Left-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_LC = Adafruit_VL53L0X(); //Left-center LIDAR
Adafruit_VL53L0X lidar_LR = Adafruit_VL53L0X(); //left-rear LIDAR (angled 30 deg rearward)


//XSHUT required for I2C address init 
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;
const int LF_XSHUT_PIN = 5;
const int LC_XSHUT_PIN = 6;
const int LR_XSHUT_PIN = 7;

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;

void setup()
{
  Serial.begin(115200); //Teensy ignores rate parameter

  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);
  pinMode(LF_XSHUT_PIN, OUTPUT);
  pinMode(LC_XSHUT_PIN, OUTPUT);
  pinMode(LR_XSHUT_PIN, OUTPUT);

  pinMode(RED_LASER_PIN, OUTPUT);

  // wait until serial port opens for native USB devices
  while (!Serial)
  {
    delay(1);
  }

  delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

  Serial.printf("Teensy Triple VL53L0X V2\n");

  //initialize all six LIDAR sensors
// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
//    all XSHUT high to bring out of reset

  //right side
  digitalWrite(RF_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RC_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RR_XSHUT_PIN, LOW);
  delay(10);

  //left side
  digitalWrite(LF_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(LC_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(LR_XSHUT_PIN, LOW);
  delay(10);

  //right side
  digitalWrite(RF_XSHUT_PIN, HIGH);
  digitalWrite(RC_XSHUT_PIN, HIGH);
  digitalWrite(RR_XSHUT_PIN, HIGH);

  //left side
  digitalWrite(LF_XSHUT_PIN, HIGH);
  digitalWrite(LC_XSHUT_PIN, HIGH);
  digitalWrite(LR_XSHUT_PIN, HIGH);

  delay(1000);


  // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
  //    shutdown by pulling XSHUT pins low

  //right side
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  //left side
  digitalWrite(LF_XSHUT_PIN, LOW);
  digitalWrite(LC_XSHUT_PIN, LOW);
  digitalWrite(LR_XSHUT_PIN, LOW);


  // 4. Initialize sensor #1 
  if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))
  if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, true, &Wire1))
  {
    Serial.println(F("Failed to boot lidar_RF"));
    while (1);
  }

  // 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
  // 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
  //    you set the first sensor to
  digitalWrite(RC_XSHUT_PIN, HIGH);
  //if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
  if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, true, &Wire1))
  {
    Serial.println(F("Failed to boot lidar_RC"));
    while (1);
  }

  // 7. Repeat for each sensor, turning each one on, setting a unique address.
  digitalWrite(RR_XSHUT_PIN, HIGH);
  //if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
  if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, true, &Wire1))
  {
    Serial.println(F("Failed to boot lidar_RR"));
    while (1);
  }

  //07/15/20 do same thing for left side sensors
  // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
  //    shutdown by pulling XSHUT pins low

  //// 4. Initialize sensor #1 
  //digitalWrite(LF_XSHUT_PIN, HIGH);
  ////if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire1))
  //if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, true, &Wire2))
  //{
  //  Serial.println(F("Failed to boot lidar_LF"));
  //  while (1);
  //}

  //// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
  //// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
  ////    you set the first sensor to
  //digitalWrite(LC_XSHUT_PIN, HIGH);
  //if (!lidar_LC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))
  //{
  //  Serial.println(F("Failed to boot lidar_RC"));
  //  while (1);
  //}

  //// 7. Repeat for each sensor, turning each one on, setting a unique address.
  //digitalWrite(LR_XSHUT_PIN, HIGH);
  //if (!lidar_LR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))
  //{
  //  Serial.println(F("Failed to boot lidar_LR"));
  //  while (1);
  //}



  //turn the laser OFF
  digitalWrite(RED_LASER_PIN, LOW);

  Serial.printf("\nInit complete\n \nVL53L0X API Simple Ranging example\n \n");

  ////05/23/20 experiment with accessing API functions
  //VL53L0X_Dev_t RightFrontDevice = lidar_RF.GetDeviceHandle();
  //VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();
  //Serial.printf("Device Info for Right Front Sensor\n");
  //Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",
  //  RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,
  //  RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,
  //  RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

  ////get measurement timing budget?
  //prog_uint32_t MeasBudgetUSec;
  ////VL53L0X_Error ApiError = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);
  //VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);
  //Serial.printf("Right Front Measurement Budget (uSec) = %d\n", MeasBudgetUSec);
  //delay(1000);
  //Serial.printf("Changing budget from %d to %d....\n", MeasBudgetUSec, 2 * MeasBudgetUSec);
  //delay(1000);
  //MeasBudgetUSec = 2 * MeasBudgetUSec;
  ////ApiError = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);
  //VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);
  //Serial.printf("Right Front Measurement Budget (uSec) Now = %d\n \n", MeasBudgetUSec);



  Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");
}

void loop()
{
  VL53L0X_RangingMeasurementData_t measure1;
  VL53L0X_RangingMeasurementData_t measure2;
  VL53L0X_RangingMeasurementData_t measure3;
  //VL53L0X_RangingMeasurementData_t measure4;
  //VL53L0X_RangingMeasurementData_t measure5;
  //VL53L0X_RangingMeasurementData_t measure6;

  //turn the laser ON
  digitalWrite(RED_LASER_PIN, HIGH);

  lidar_RF.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
  //lidar_RF.rangingTest(&measure1, true); // pass in 'true' to get debug data printout!
  if (measure1.RangeStatus != 4)  // phase failures have incorrect data
  {
    RF_Dist_mm = measure1.RangeMilliMeter;
    if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RF_Dist_mm = 0;

  delay(100);

  lidar_RC.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
  //lidar_RC.rangingTest(&measure2, true); // pass in 'true' to get debug data printout!
  if (measure2.RangeStatus != 4)  // phase failures have incorrect data
  {
    RC_Dist_mm = measure2.RangeMilliMeter;
    if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RC_Dist_mm = 0;

  delay(100);

  lidar_RR.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!
  //lidar_RR.rangingTest(&measure3, true); // pass in 'true' to get debug data printout!
  if (measure3.RangeStatus != 4)  // phase failures have incorrect data
  {
    RR_Dist_mm = measure3.RangeMilliMeter;
    if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RR_Dist_mm = 0;

  //07/16/20 added left side
  //lidar_LF.rangingTest(&measure4, false); // pass in 'true' to get debug data printout!
  ////lidar_LF.rangingTest(&measure4, true); // pass in 'true' to get debug data printout!
  //if (measure4.RangeStatus != 4)  // phase failures have incorrect data
  //{
  //  LF_Dist_mm = measure4.RangeMilliMeter;
  //  if (LF_Dist_mm > MAX_LIDAR_RANGE_MM) LF_Dist_mm = MAX_LIDAR_RANGE_MM;
  //}
  //else LF_Dist_mm = 0;

  //delay(100);

  //lidar_LC.rangingTest(&measure5, false); // pass in 'true' to get debug data printout!
  //if (measure5.RangeStatus != 4)  // phase failures have incorrect data
  //{
  //  LC_Dist_mm = measure5.RangeMilliMeter;
  //  if (LC_Dist_mm > MAX_LIDAR_RANGE_MM) LC_Dist_mm = MAX_LIDAR_RANGE_MM;
  //}
  //else LC_Dist_mm = 0;

  //delay(100);

  //lidar_LR.rangingTest(&measure6, false); // pass in 'true' to get debug data printout!
  //if (measure6.RangeStatus != 4)  // phase failures have incorrect data
  //{
  //  LR_Dist_mm = measure6.RangeMilliMeter;
  //  if (LR_Dist_mm > MAX_LIDAR_RANGE_MM) LR_Dist_mm = MAX_LIDAR_RANGE_MM;
  //}
  //else RR_Dist_mm = 0;



  //float RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;
  //float LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / (float)LC_Dist_mm;
  //float RightSteeringVal = (RF_Dist_mm - RR_Dist_mm);

  //Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),
  //  RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);
  Serial.printf("%lu\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);
  //Serial.printf("%lu\t%d\t%d\t%d\n", millis(), LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);
  //Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,
    //LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

  //Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%3.2f\n", millis(),
  //  RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal,
  //  LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

  //turn the laser OFF
  digitalWrite(RED_LASER_PIN, LOW);

  //delay(100);
  delay(250);
}

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

Name: Teensy_Hex_V53L0X.ino

Created: 7/16/2020 2:56:35 PM

Author: FRANKNEWXPS15\Frank

Notes:

07/18/20: I discovered that only 3 VL53L0X units can share the I2C bus. The GY530

modules have internal 10K pullups on the I2C lines, and the 4th one on the

bus reduces the combined pullup value too much.

Instead, I put the right-hand array of 3 on Wire1 and the left-hand array

of 3 on Wire2.

Name: Teensy_Triple_VL53L0X_V2.ino

Created: 5/23/2020 1:16:52 PM

Author: FRANKNEWXPS15\Frank

/* This code copied from the following post on the Adafruit forum:

https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5

#include <Wire.h>

#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Right-center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//07/16/20 added three left-side sensors

Adafruit_VL53L0X lidar_LF = Adafruit_VL53L0X(); //Left-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_LC = Adafruit_VL53L0X(); //Left-center LIDAR

Adafruit_VL53L0X lidar_LR = Adafruit_VL53L0X(); //left-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

const int LF_XSHUT_PIN = 5;

const int LC_XSHUT_PIN = 6;

const int LR_XSHUT_PIN = 7;

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

uint16_t LF_Dist_mm;

uint16_t LC_Dist_mm;

uint16_t LR_Dist_mm;

void setup()

{

Serial.begin(115200); //Teensy ignores rate parameter

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(LF_XSHUT_PIN, OUTPUT);

pinMode(LC_XSHUT_PIN, OUTPUT);

pinMode(LR_XSHUT_PIN, OUTPUT);

pinMode(RED_LASER_PIN, OUTPUT);

// wait until serial port opens for native USB devices

while (!Serial)

{

delay(1);

}

delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

Serial.printf("Teensy Triple VL53L0X V2\n");

//initialize all six LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

//right side

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

//left side

digitalWrite(LF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(LC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(LR_XSHUT_PIN, LOW);

delay(10);

//right side

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

//left side

digitalWrite(LF_XSHUT_PIN, HIGH);

digitalWrite(LC_XSHUT_PIN, HIGH);

digitalWrite(LR_XSHUT_PIN, HIGH);

delay(1000);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

//right side

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

//left side

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

// 4. Initialize sensor #1

if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))

if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, true, &Wire1))

{

Serial.println(F("Failed to boot lidar_RF"));

while (1);

}

// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(RC_XSHUT_PIN, HIGH);

//if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))

if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, true, &Wire1))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 7. Repeat for each sensor, turning each one on, setting a unique address.

digitalWrite(RR_XSHUT_PIN, HIGH);

//if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))

if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, true, &Wire1))

{

Serial.println(F("Failed to boot lidar_RR"));

while (1);

}

//07/15/20 do same thing for left side sensors

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

//// 4. Initialize sensor #1

//digitalWrite(LF_XSHUT_PIN, HIGH);

////if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire1))

//if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, true, &Wire2))

//{

// Serial.println(F("Failed to boot lidar_LF"));

// while (1);

//}

//// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

//// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

//// you set the first sensor to

//digitalWrite(LC_XSHUT_PIN, HIGH);

//if (!lidar_LC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))

//{

// Serial.println(F("Failed to boot lidar_RC"));

// while (1);

//}

//// 7. Repeat for each sensor, turning each one on, setting a unique address.

//digitalWrite(LR_XSHUT_PIN, HIGH);

//if (!lidar_LR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))

//{

// Serial.println(F("Failed to boot lidar_LR"));

// while (1);

//}

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

Serial.printf("\nInit complete\n \nVL53L0X API Simple Ranging example\n \n");

////05/23/20 experiment with accessing API functions

//VL53L0X_Dev_t RightFrontDevice = lidar_RF.GetDeviceHandle();

//VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();

//Serial.printf("Device Info for Right Front Sensor\n");

//Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",

// RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,

// RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,

// RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

////get measurement timing budget?

//prog_uint32_t MeasBudgetUSec;

////VL53L0X_Error ApiError = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);

//VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);

//Serial.printf("Right Front Measurement Budget (uSec) = %d\n", MeasBudgetUSec);

//delay(1000);

//Serial.printf("Changing budget from %d to %d....\n", MeasBudgetUSec, 2 * MeasBudgetUSec);

//delay(1000);

//MeasBudgetUSec = 2 * MeasBudgetUSec;

////ApiError = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);

//VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);

//Serial.printf("Right Front Measurement Budget (uSec) Now = %d\n \n", MeasBudgetUSec);

Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");

}

void loop()

{

VL53L0X_RangingMeasurementData_t measure1;

VL53L0X_RangingMeasurementData_t measure2;

VL53L0X_RangingMeasurementData_t measure3;

//VL53L0X_RangingMeasurementData_t measure4;

//VL53L0X_RangingMeasurementData_t measure5;

//VL53L0X_RangingMeasurementData_t measure6;

//turn the laser ON

digitalWrite(RED_LASER_PIN, HIGH);

lidar_RF.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!

//lidar_RF.rangingTest(&measure1, true); // pass in 'true' to get debug data printout!

if (measure1.RangeStatus != 4) // phase failures have incorrect data

{

RF_Dist_mm = measure1.RangeMilliMeter;

if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RF_Dist_mm = 0;

delay(100);

lidar_RC.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!

//lidar_RC.rangingTest(&measure2, true); // pass in 'true' to get debug data printout!

if (measure2.RangeStatus != 4) // phase failures have incorrect data

{

RC_Dist_mm = measure2.RangeMilliMeter;

if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RC_Dist_mm = 0;

delay(100);

lidar_RR.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!

//lidar_RR.rangingTest(&measure3, true); // pass in 'true' to get debug data printout!

if (measure3.RangeStatus != 4) // phase failures have incorrect data

{

RR_Dist_mm = measure3.RangeMilliMeter;

if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

//07/16/20 added left side

//lidar_LF.rangingTest(&measure4, false); // pass in 'true' to get debug data printout!

////lidar_LF.rangingTest(&measure4, true); // pass in 'true' to get debug data printout!

//if (measure4.RangeStatus != 4) // phase failures have incorrect data

//{

// LF_Dist_mm = measure4.RangeMilliMeter;

// if (LF_Dist_mm > MAX_LIDAR_RANGE_MM) LF_Dist_mm = MAX_LIDAR_RANGE_MM;

//}

//else LF_Dist_mm = 0;

//delay(100);

//lidar_LC.rangingTest(&measure5, false); // pass in 'true' to get debug data printout!

//if (measure5.RangeStatus != 4) // phase failures have incorrect data

//{

// LC_Dist_mm = measure5.RangeMilliMeter;

// if (LC_Dist_mm > MAX_LIDAR_RANGE_MM) LC_Dist_mm = MAX_LIDAR_RANGE_MM;

//}

//else LC_Dist_mm = 0;

//delay(100);

//lidar_LR.rangingTest(&measure6, false); // pass in 'true' to get debug data printout!

//if (measure6.RangeStatus != 4) // phase failures have incorrect data

//{

// LR_Dist_mm = measure6.RangeMilliMeter;

// if (LR_Dist_mm > MAX_LIDAR_RANGE_MM) LR_Dist_mm = MAX_LIDAR_RANGE_MM;

//}

//else RR_Dist_mm = 0;

//float RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

//float LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / (float)LC_Dist_mm;

//float RightSteeringVal = (RF_Dist_mm - RR_Dist_mm);

//Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),

// RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);

Serial.printf("%lu\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//Serial.printf("%lu\t%d\t%d\t%d\n", millis(), LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

//Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,

//LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

//Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%3.2f\n", millis(),

// RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal,

// LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

//delay(100);

delay(250);

}

So, it appears that there is some sort of interaction between the Wire1 & Wire2 I2C busses on the Teensy 3.5.

22 July 2020 Update:

Based on some feedback from the Teensy forum, I added some code to my program to verify that each VL53L0X sensor I2C address had been set properly in the setup code. When I did this, I got results that were more than a little mystifying; Initialization of the 3 sensors on the Wire1 bus all reported success, but the I2C scanning code reported a different story, as shown below:

Opening port
Port open
Teensy Triple VL53L0X V2
In Adafruit_VL53L0X::begin(2A,0,7D0)
In Adafruit_VL53L0X::begin(2B,0,7D0)
In Adafruit_VL53L0X::begin(2C,0,7D0)
In Adafruit_VL53L0X::begin(2D,0,6F8)
In Adafruit_VL53L0X::begin(2E,0,6F8)
In Adafruit_VL53L0X::begin(2F,0,6F8)

Init complete
 
VL53L0X API Simple Ranging example
 
Scanning Wire1...I2C device found at address 0x29  !
I2C device found at address 0x2C  !
done

Scanning Wire2...I2C device found at address 0x2D  !
I2C device found at address 0x2E  !
I2C device found at address 0x2F  !
done

Opening port

Port open

Teensy Triple VL53L0X V2

In Adafruit_VL53L0X::begin(2A,0,7D0)

In Adafruit_VL53L0X::begin(2B,0,7D0)

In Adafruit_VL53L0X::begin(2C,0,7D0)

In Adafruit_VL53L0X::begin(2D,0,6F8)

In Adafruit_VL53L0X::begin(2E,0,6F8)

In Adafruit_VL53L0X::begin(2F,0,6F8)

Init complete

VL53L0X API Simple Ranging example

Scanning Wire1...I2C device found at address 0x29 !

I2C device found at address 0x2C !

done

Scanning Wire2...I2C device found at address 0x2D !

I2C device found at address 0x2E !

I2C device found at address 0x2F !

done

The three sensors on the Wire1 bus were supposed to wind up at 2A, 2B & 2C, but the scanner showed them at 29 and 2C, with one of them missing entirely – wow!

So, I decided to go back to basics. I modified my original triple VL53L0X demo program to include a I2C bus scan to verify the actual addresses of the sensors, as follows:

/*
    Name:       Triple_VL53L0X_Demo.ino
    Created:	5/19/2020 9:56:56 PM
    Author:     FRANKNEWXPS15\Frank
*/

/* This code copied from the following post on the Adafruit forum:
https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5
*/


#include <Wire.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)


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

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;

void setup() 
{
    Serial.begin(115200); //Teensy ignores rate parameter

    pinMode(RF_XSHUT_PIN, OUTPUT);
    pinMode(RC_XSHUT_PIN, OUTPUT);
    pinMode(RR_XSHUT_PIN, OUTPUT);

    pinMode(RED_LASER_PIN, OUTPUT);

    // wait until serial port opens for native USB devices
    while (!Serial) {
        delay(1);
    }

    Serial.println("Teensy Adafruit Triple VL53L0X test");

    //initialize 3 LIDAR sensors
  // 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
  //    all XSHUT high to bring out of reset
    digitalWrite(RF_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RC_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RR_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RF_XSHUT_PIN, HIGH);
    digitalWrite(RC_XSHUT_PIN, HIGH);
    digitalWrite(RR_XSHUT_PIN, HIGH);

    // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
    //    shutdown by pulling XSHUT pins low
    digitalWrite(RC_XSHUT_PIN, LOW);
    digitalWrite(RR_XSHUT_PIN, LOW);

    // 4. Initialize sensor #1 with lox.begin(new_i2c_address) Pick any number but 0x29 and it
    //    must be under 0x7F. Going with 0x30 to 0x3F is probably OK.
    if (!lidar_RF.begin(VL53L0X_I2C_ADDR+1, false, &Wire1)) 
    {
        Serial.println(F("Failed to boot lidar_RF"));
        while (1);
    }

    // 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
    // 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
    //    you set the first sensor to
    digitalWrite(RC_XSHUT_PIN, HIGH);
    if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RC"));
        while (1);
    }

    // 7. Repeat for each sensor, turning each one on, setting a unique address.
    digitalWrite(RR_XSHUT_PIN, HIGH);
    if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RR"));
        while (1);
    }

    //turn the laser OFF
    digitalWrite(RED_LASER_PIN, LOW);

    Serial.printf("\nInit complete\n \nSimple Triple VL53L0X Ranging example\n \n");

    //Scan I2C bus to verify that sensor addresses got programmed correctly
    Serial.printf("Scanning Wire1 I2C bus...\n");

    int nDevices = 0;

    for (byte address = 1; address < 127; ++address) {
      // The i2c_scanner uses the return value of
      // the Write.endTransmisstion to see if
      // a device did acknowledge to the address.
      //Wire.beginTransmission(address);
      Wire1.beginTransmission(address);
      byte error = Wire1.endTransmission();

      if (error == 0) {
        Serial.print("I2C device found at address 0x");
        if (address < 16) {
          Serial.print("0");
        }
        Serial.print(address, HEX);
        Serial.println("  !");

        ++nDevices;
      }
      else if (error == 4) {
        Serial.print("Unknown error at address 0x");
        if (address < 16) {
          Serial.print("0");
        }
        Serial.println(address, HEX);
      }
    }
    if (nDevices == 0) {
      Serial.println("No I2C devices found\n");
    }
    else {
      Serial.println("done\n");
    }


    ////05/23/20 experiment with accessing API functions

    //VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();
    //Serial.printf("Device Info for Right Front Sensor\n");
    //Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n", 
    //    RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,
    //    RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,
    //    RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

    Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");
}

void loop() 
{
    VL53L0X_RangingMeasurementData_t measure;

    //turn the laser ON
    digitalWrite(RED_LASER_PIN, HIGH);

    lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    { 
        RF_Dist_mm = measure.RangeMilliMeter;
        if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RF_Dist_mm = 0;

    //delay(100);

    lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RC_Dist_mm = measure.RangeMilliMeter;
        if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RC_Dist_mm = 0;

    //delay(100);

    lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RR_Dist_mm = measure.RangeMilliMeter;
        if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RR_Dist_mm = 0;

    float SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

    Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(), 
        RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);


    //turn the laser OFF
    digitalWrite(RED_LASER_PIN, LOW);

    delay(100);
}

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

Name: Triple_VL53L0X_Demo.ino

Created: 5/19/2020 9:56:56 PM

Author: FRANKNEWXPS15\Frank

/* This code copied from the following post on the Adafruit forum:

https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5

#include <Wire.h>

#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

void setup()

{

Serial.begin(115200); //Teensy ignores rate parameter

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(RED_LASER_PIN, OUTPUT);

// wait until serial port opens for native USB devices

while (!Serial) {

delay(1);

}

Serial.println("Teensy Adafruit Triple VL53L0X test");

//initialize 3 LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

// 4. Initialize sensor #1 with lox.begin(new_i2c_address) Pick any number but 0x29 and it

// must be under 0x7F. Going with 0x30 to 0x3F is probably OK.

if (!lidar_RF.begin(VL53L0X_I2C_ADDR+1, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RF"));

while (1);

}

// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(RC_XSHUT_PIN, HIGH);

if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 7. Repeat for each sensor, turning each one on, setting a unique address.

digitalWrite(RR_XSHUT_PIN, HIGH);

if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RR"));

while (1);

}

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

Serial.printf("\nInit complete\n \nSimple Triple VL53L0X Ranging example\n \n");

//Scan I2C bus to verify that sensor addresses got programmed correctly

Serial.printf("Scanning Wire1 I2C bus...\n");

int nDevices = 0;

for (byte address = 1; address < 127; ++address) {

// The i2c_scanner uses the return value of

// the Write.endTransmisstion to see if

// a device did acknowledge to the address.

//Wire.beginTransmission(address);

Wire1.beginTransmission(address);

byte error = Wire1.endTransmission();

if (error == 0) {

Serial.print("I2C device found at address 0x");

if (address < 16) {

Serial.print("0");

}

Serial.print(address, HEX);

Serial.println(" !");

++nDevices;

}

else if (error == 4) {

Serial.print("Unknown error at address 0x");

if (address < 16) {

Serial.print("0");

}

Serial.println(address, HEX);

}

if (nDevices == 0) {

Serial.println("No I2C devices found\n");

}

else {

Serial.println("done\n");

}

////05/23/20 experiment with accessing API functions

//VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();

//Serial.printf("Device Info for Right Front Sensor\n");

//Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",

// RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,

// RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,

// RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");

}

void loop()

{

VL53L0X_RangingMeasurementData_t measure;

//turn the laser ON

digitalWrite(RED_LASER_PIN, HIGH);

lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RF_Dist_mm = measure.RangeMilliMeter;

if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RF_Dist_mm = 0;

//delay(100);

lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RC_Dist_mm = measure.RangeMilliMeter;

if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RC_Dist_mm = 0;

//delay(100);

lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RR_Dist_mm = measure.RangeMilliMeter;

if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

float SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),

RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

delay(100);

}

And got the following output:

Opening port
Port open
Teensy Adafruit Triple VL53L0X test
In Adafruit_VL53L0X::begin(2A,0,7D0)
In Adafruit_VL53L0X::begin(2B,0,7D0)
In Adafruit_VL53L0X::begin(2C,0,7D0)

Init complete
 
Simple Triple VL53L0X Ranging example
 
Scanning Wire1 I2C bus...
I2C device found at address 0x2A  !
I2C device found at address 0x2B  !
I2C device found at address 0x2C  !
done

Msec	Front	Center	Rear	Steer
4768	65	179	123	-0.32
5001	61	178	125	-0.36
5234	65	178	126	-0.34
5468	63	179	126	-0.35
5701	64	176	125	-0.35
5934	62	181	129	-0.37
6167	64	178	126	-0.35
6401	63	181	126	-0.35
6634	62	182	128	-0.36
6868	64	177	123	-0.33
7101	63	181	126	-0.35
7334	61	179	127	-0.37
7567	63	182	125	-0.34
7800	62	179	127	-0.36
8033	63	166	31	0.19
8265	63	191	22	0.21
8498	63	123	29	0.28
8731	61	23	25	1.57
8963	64	24	25	1.62
9196	66	24	131	-2.71
9429	62	55	12	0.91
9662	69	170	33	0.21
9895	71	171	27	0.26
10128	70	167	24	0.28
10361	68	17	145	-4.53
10593	58	29	143	-2.93
10827	27	176	150	-0.70
11060	31	182	151	-0.66
11293	16	181	147	-0.72
11526	28	123	148	-0.98
11758	58	14	147	-6.36
11991	60	17	150	-5.29
12223	58	18	146	-4.89

Opening port

Port open

Teensy Adafruit Triple VL53L0X test

In Adafruit_VL53L0X::begin(2A,0,7D0)

In Adafruit_VL53L0X::begin(2B,0,7D0)

In Adafruit_VL53L0X::begin(2C,0,7D0)

Init complete

Simple Triple VL53L0X Ranging example

Scanning Wire1 I2C bus...

I2C device found at address 0x2A !

I2C device found at address 0x2B !

I2C device found at address 0x2C !

done

Msec Front Center Rear Steer

4768 65 179 123 -0.32

5001 61 178 125 -0.36

5234 65 178 126 -0.34

5468 63 179 126 -0.35

5701 64 176 125 -0.35

5934 62 181 129 -0.37

6167 64 178 126 -0.35

6401 63 181 126 -0.35

6634 62 182 128 -0.36

6868 64 177 123 -0.33

7101 63 181 126 -0.35

7334 61 179 127 -0.37

7567 63 182 125 -0.34

7800 62 179 127 -0.36

8033 63 166 31 0.19

8265 63 191 22 0.21

8498 63 123 29 0.28

8731 61 23 25 1.57

8963 64 24 25 1.62

9196 66 24 131 -2.71

9429 62 55 12 0.91

9662 69 170 33 0.21

9895 71 171 27 0.26

10128 70 167 24 0.28

10361 68 17 145 -4.53

10593 58 29 143 -2.93

10827 27 176 150 -0.70

11060 31 182 151 -0.66

11293 16 181 147 -0.72

11526 28 123 148 -0.98

11758 58 14 147 -6.36

11991 60 17 150 -5.29

12223 58 18 146 -4.89

As shown above, the VL53L0X sensors got programmed correctly, and appear to operate correctly as well.

Then I created a new program identical to the above, except for using the Wire2 bus instead of the Wire 1 bus, using different I2C addresses and different XSHUT pin assignments for programming the sensors.

/*
    Name:       Teensy_Triple_ VL53L0X_Wire2.ino
    Created:	7/22/2020 10:33:15 AM
    Author:     FRANKNEWXPS15\Frank
*/


/*
    Name:       Triple_VL53L0X_Demo.ino
    Created:	5/19/2020 9:56:56 PM
    Author:     FRANKNEWXPS15\Frank
*/

/* This code copied from the following post on the Adafruit forum:
https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire2 I2C bus on a Teensy 3.5
*/


#include <Wire.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)


//XSHUT required for I2C address init 
const int RF_XSHUT_PIN = 5;
const int RC_XSHUT_PIN = 6;
const int RR_XSHUT_PIN = 7;

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;

void setup()
{
  Serial.begin(115200); //Teensy ignores rate parameter

  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);

  // wait until serial port opens for native USB devices
  while (!Serial) {
    delay(1);
  }

  Serial.println("Teensy Adafruit Triple VL53L0X test");

  //initialize 3 LIDAR sensors
// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
//    all XSHUT high to bring out of reset
  digitalWrite(RF_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RC_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RR_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RF_XSHUT_PIN, HIGH);
  digitalWrite(RC_XSHUT_PIN, HIGH);
  digitalWrite(RR_XSHUT_PIN, HIGH);

  // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
  //    shutdown by pulling XSHUT pins low
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  // 4. Initialize sensor #1 with lox.begin(new_i2c_address) Pick any number but 0x29 and it
  //    must be under 0x7F. Going with 0x30 to 0x3F is probably OK.
  if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_RF"));
    while (1);
  }

  // 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
  // 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
  //    you set the first sensor to
  digitalWrite(RC_XSHUT_PIN, HIGH);
  if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_RC"));
    while (1);
  }

  // 7. Repeat for each sensor, turning each one on, setting a unique address.
  digitalWrite(RR_XSHUT_PIN, HIGH);
  if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_RR"));
    while (1);
  }

  Serial.printf("\nInit complete\n \nSimple Triple VL53L0X Ranging example\n \n");

  //Scan I2C bus to verify that sensor addresses got programmed correctly
  Serial.printf("Scanning Wire2 I2C bus...\n");

  int nDevices = 0;

  for (byte address = 1; address < 127; ++address) {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    //Wire.beginTransmission(address);
    Wire2.beginTransmission(address);
    byte error = Wire2.endTransmission();

    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address < 16) {
        Serial.print("0");
      }
      Serial.print(address, HEX);
      Serial.println("  !");

      ++nDevices;
    }
    else if (error == 4) {
      Serial.print("Unknown error at address 0x");
      if (address < 16) {
        Serial.print("0");
      }
      Serial.println(address, HEX);
    }
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");
  }
  else {
    Serial.println("done\n");
  }




  ////05/23/20 experiment with accessing API functions

  //VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();
  //Serial.printf("Device Info for Right Front Sensor\n");
  //Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",
  //  RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,
  //  RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,
  //  RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

  Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");
}

void loop()
{
  VL53L0X_RangingMeasurementData_t measure;

  lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
  if (measure.RangeStatus != 4)  // phase failures have incorrect data
  {
    RF_Dist_mm = measure.RangeMilliMeter;
    if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RF_Dist_mm = 0;

  //delay(100);

  lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
  if (measure.RangeStatus != 4)  // phase failures have incorrect data
  {
    RC_Dist_mm = measure.RangeMilliMeter;
    if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RC_Dist_mm = 0;

  //delay(100);

  lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
  if (measure.RangeStatus != 4)  // phase failures have incorrect data
  {
    RR_Dist_mm = measure.RangeMilliMeter;
    if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RR_Dist_mm = 0;

  float SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

  Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),
    RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

  delay(100);
}

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

Name: Teensy_Triple_ VL53L0X_Wire2.ino

Created: 7/22/2020 10:33:15 AM

Author: FRANKNEWXPS15\Frank

Name: Triple_VL53L0X_Demo.ino

Created: 5/19/2020 9:56:56 PM

Author: FRANKNEWXPS15\Frank

/* This code copied from the following post on the Adafruit forum:

https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire2 I2C bus on a Teensy 3.5

#include <Wire.h>

#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 5;

const int RC_XSHUT_PIN = 6;

const int RR_XSHUT_PIN = 7;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

void setup()

{

Serial.begin(115200); //Teensy ignores rate parameter

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

// wait until serial port opens for native USB devices

while (!Serial) {

delay(1);

}

Serial.println("Teensy Adafruit Triple VL53L0X test");

//initialize 3 LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

// 4. Initialize sensor #1 with lox.begin(new_i2c_address) Pick any number but 0x29 and it

// must be under 0x7F. Going with 0x30 to 0x3F is probably OK.

if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire2))

{

Serial.println(F("Failed to boot lidar_RF"));

while (1);

}

// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(RC_XSHUT_PIN, HIGH);

if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 7. Repeat for each sensor, turning each one on, setting a unique address.

digitalWrite(RR_XSHUT_PIN, HIGH);

if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))

{

Serial.println(F("Failed to boot lidar_RR"));

while (1);

}

Serial.printf("\nInit complete\n \nSimple Triple VL53L0X Ranging example\n \n");

//Scan I2C bus to verify that sensor addresses got programmed correctly

Serial.printf("Scanning Wire2 I2C bus...\n");

int nDevices = 0;

for (byte address = 1; address < 127; ++address) {

// The i2c_scanner uses the return value of

// the Write.endTransmisstion to see if

// a device did acknowledge to the address.

//Wire.beginTransmission(address);

Wire2.beginTransmission(address);

byte error = Wire2.endTransmission();

if (error == 0) {

Serial.print("I2C device found at address 0x");

if (address < 16) {

Serial.print("0");

}

Serial.print(address, HEX);

Serial.println(" !");

++nDevices;

}

else if (error == 4) {

Serial.print("Unknown error at address 0x");

if (address < 16) {

Serial.print("0");

}

Serial.println(address, HEX);

}

if (nDevices == 0) {

Serial.println("No I2C devices found\n");

}

else {

Serial.println("done\n");

}

////05/23/20 experiment with accessing API functions

//VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();

//Serial.printf("Device Info for Right Front Sensor\n");

//Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",

// RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,

// RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,

// RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");

}

void loop()

{

VL53L0X_RangingMeasurementData_t measure;

lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RF_Dist_mm = measure.RangeMilliMeter;

if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RF_Dist_mm = 0;

//delay(100);

lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RC_Dist_mm = measure.RangeMilliMeter;

if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RC_Dist_mm = 0;

//delay(100);

lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RR_Dist_mm = measure.RangeMilliMeter;

if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

float SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),

RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

delay(100);

}

When I ran this program, I got the following output:

Opening port
Port open
In Adafruit_VL53L0X::begin(2D,0,6F8)
In Adafruit_VL53L0X::begin(2E,0,6F8)
In Adafruit_VL53L0X::begin(2F,0,6F8)

Init complete
 
Simple Triple VL53L0X Ranging example
 
Scanning Wire2 I2C bus...
I2C device found at address 0x2D  !
I2C device found at address 0x2E  !
I2C device found at address 0x2F  !
done

Msec	Front	Center	Rear	Steer
1986	510	547	571	-0.11
2204	503	542	534	-0.06
2422	512	539	549	-0.07
2640	507	548	591	-0.15
2858	502	567	542	-0.07
3076	512	564	551	-0.07
3294	498	532	559	-0.11
3513	510	554	553	-0.08
3731	505	556	542	-0.07
3949	510	546	577	-0.12
4167	502	552	558	-0.10
4385	505	546	531	-0.05
4603	505	537	525	-0.04
4821	500	534	564	-0.12
5039	499	539	580	-0.15
5257	505	556	592	-0.16
5475	460	559	552	-0.16
5693	68	59	32	0.61
5910	44	40	15	0.73
6127	52	47	22	0.64
6344	58	49	28	0.61
6562	45	47	30	0.32
6779	44	54	47	-0.06
6997	77	88	88	-0.12
7214	111	125	130	-0.15
7432	114	158	1000	-5.61
7650	102	1000	1000	-0.90
7868	44	42	16	0.67
8085	49	47	23	0.55
8285	129	1000	0	0.13
8485	1000	1000	0	1.00
8703	1000	1000	0	1.00

Opening port

Port open

In Adafruit_VL53L0X::begin(2D,0,6F8)

In Adafruit_VL53L0X::begin(2E,0,6F8)

In Adafruit_VL53L0X::begin(2F,0,6F8)

Init complete

Simple Triple VL53L0X Ranging example

Scanning Wire2 I2C bus...

I2C device found at address 0x2D !

I2C device found at address 0x2E !

I2C device found at address 0x2F !

done

Msec Front Center Rear Steer

1986 510 547 571 -0.11

2204 503 542 534 -0.06

2422 512 539 549 -0.07

2640 507 548 591 -0.15

2858 502 567 542 -0.07

3076 512 564 551 -0.07

3294 498 532 559 -0.11

3513 510 554 553 -0.08

3731 505 556 542 -0.07

3949 510 546 577 -0.12

4167 502 552 558 -0.10

4385 505 546 531 -0.05

4603 505 537 525 -0.04

4821 500 534 564 -0.12

5039 499 539 580 -0.15

5257 505 556 592 -0.16

5475 460 559 552 -0.16

5693 68 59 32 0.61

5910 44 40 15 0.73

6127 52 47 22 0.64

6344 58 49 28 0.61

6562 45 47 30 0.32

6779 44 54 47 -0.06

6997 77 88 88 -0.12

7214 111 125 130 -0.15

7432 114 158 1000 -5.61

7650 102 1000 1000 -0.90

7868 44 42 16 0.67

8085 49 47 23 0.55

8285 129 1000 0 0.13

8485 1000 1000 0 1.00

8703 1000 1000 0 1.00

Then I modified my original hex-sensor program to initialize one array at a time, with a I2C bus scan in between, as follows:

/*
    Name:       Teensy_Hex_VL53L0X_Demo_V3.ino
    Created:	7/22/2020 11:31:10 AM
    Author:     FRANKNEWXPS15\Frank
*/

/*
    Name:       Teensy_Hex_V53L0X.ino
    Created:	7/16/2020 2:56:35 PM
    Author:     FRANKNEWXPS15\Frank

    Notes:
      07/18/20: I discovered that only 3 VL53L0X units can share the I2C bus. The GY530
                modules have internal 10K pullups on the I2C lines, and the 4th one on the
                bus reduces the combined pullup value too much.

                Instead, I put the right-hand array of 3 on Wire1 and the left-hand array
                of 3 on Wire2.
*/


/*
    Name:       Teensy_Triple_VL53L0X_V2.ino
    Created:	5/23/2020 1:16:52 PM
    Author:     FRANKNEWXPS15\Frank
*/


/* This code copied from the following post on the Adafruit forum:
https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5
*/


#include <Wire.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Right-center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//07/16/20 added three left-side sensors
Adafruit_VL53L0X lidar_LF = Adafruit_VL53L0X(); //Left-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_LC = Adafruit_VL53L0X(); //Left-center LIDAR
Adafruit_VL53L0X lidar_LR = Adafruit_VL53L0X(); //left-rear LIDAR (angled 30 deg rearward)


//XSHUT required for I2C address init 
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;
const int LF_XSHUT_PIN = 5;
const int LC_XSHUT_PIN = 6;
const int LR_XSHUT_PIN = 7;

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;

void setup()
{
  Serial.begin(115200); //Teensy ignores rate parameter

  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);
  pinMode(LF_XSHUT_PIN, OUTPUT);
  pinMode(LC_XSHUT_PIN, OUTPUT);
  pinMode(LR_XSHUT_PIN, OUTPUT);

  // wait until serial port opens for native USB devices
  while (!Serial)
  {
    delay(1);
  }

  delay(3000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

  Serial.printf("Teensy Hex VL53L0X Demo\n");

  //initialize all six LIDAR sensors
// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
//    all XSHUT high to bring out of reset

  //right side
  digitalWrite(RF_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RC_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(RR_XSHUT_PIN, LOW);
  delay(10);

  //right side
  digitalWrite(RF_XSHUT_PIN, HIGH);
  digitalWrite(RC_XSHUT_PIN, HIGH);
  digitalWrite(RR_XSHUT_PIN, HIGH);

  delay(1000);

  // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
  //    shutdown by pulling XSHUT pins low

  //right side
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  // 4. Initialize sensor #1 
  if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))
  //if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, true, &Wire1))
  {
    Serial.println(F("Failed to boot lidar_RF"));
    while (1);
  }

  // 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
  // 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
  //    you set the first sensor to
  digitalWrite(RC_XSHUT_PIN, HIGH);
  if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
  //if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, true, &Wire1))
  {
    Serial.println(F("Failed to boot lidar_RC"));
    while (1);
  }

  // 7. Repeat for each sensor, turning each one on, setting a unique address.
  digitalWrite(RR_XSHUT_PIN, HIGH);
  if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
  //if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, true, &Wire1))
  {
    Serial.println(F("Failed to boot lidar_RR"));
    while (1);
  }

  //07/21/20 now go back and verify that all I2C addresses are unique
  Serial.printf("Scanning Wire1...");

  int nDevices = 0;

  for (byte address = 1; address < 127; ++address) {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    //Wire.beginTransmission(address);
    Wire1.beginTransmission(address);
    byte error = Wire1.endTransmission();

    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address < 16) {
        Serial.print("0");
      }
      Serial.print(address, HEX);
      Serial.println("  !");

      ++nDevices;
    }
    else if (error == 4) {
      Serial.print("Unknown error at address 0x");
      if (address < 16) {
        Serial.print("0");
      }
      Serial.println(address, HEX);
    }
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");
  }
  else {
    Serial.println("done\n");
  }

  delay(2000);


  //left side
  digitalWrite(LF_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(LC_XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(LR_XSHUT_PIN, LOW);
  delay(10);

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

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


  //07/15/20 do same thing for left side sensors
   //2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
   //   shutdown by pulling XSHUT pins low

  // 4. Initialize sensor #1 
  if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire2))
    // if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, true, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_LF"));
    while (1);
  }

  // 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
  // 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
  //    you set the first sensor to
  digitalWrite(LC_XSHUT_PIN, HIGH);
  if (!lidar_LC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_RC"));
    while (1);
  }

  // 7. Repeat for each sensor, turning each one on, setting a unique address.
  digitalWrite(LR_XSHUT_PIN, HIGH);
  if (!lidar_LR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))
  {
    Serial.println(F("Failed to boot lidar_LR"));
    while (1);
  }


  Serial.printf("\nInit complete\n \nVL53L0X API Simple Ranging example\n \n");
  Serial.printf("Scanning Wire2...");

  nDevices = 0;

  for (byte address = 1; address < 127; ++address) {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    //Wire.beginTransmission(address);
    Wire2.beginTransmission(address);
    byte error = Wire2.endTransmission();

    if (error == 0) {
      Serial.print("I2C device found at address 0x");
      if (address < 16) {
        Serial.print("0");
      }
      Serial.print(address, HEX);
      Serial.println("  !");

      ++nDevices;
    }
    else if (error == 4) {
      Serial.print("Unknown error at address 0x");
      if (address < 16) {
        Serial.print("0");
      }
      Serial.println(address, HEX);
    }
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");
  }
  else {
    Serial.println("done\n");
  }

  delay(2000);

  Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");
}

void loop()
{
  VL53L0X_RangingMeasurementData_t measure1;
  VL53L0X_RangingMeasurementData_t measure2;
  VL53L0X_RangingMeasurementData_t measure3;
  VL53L0X_RangingMeasurementData_t measure4;
  VL53L0X_RangingMeasurementData_t measure5;
  VL53L0X_RangingMeasurementData_t measure6;

  lidar_RF.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
  //lidar_RF.rangingTest(&measure1, true); // pass in 'true' to get debug data printout!
  if (measure1.RangeStatus != 4)  // phase failures have incorrect data
  {
    RF_Dist_mm = measure1.RangeMilliMeter;
    if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RF_Dist_mm = 0;

  delay(100);

  lidar_RC.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
  //lidar_RC.rangingTest(&measure2, true); // pass in 'true' to get debug data printout!
  if (measure2.RangeStatus != 4)  // phase failures have incorrect data
  {
    RC_Dist_mm = measure2.RangeMilliMeter;
    if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RC_Dist_mm = 0;

  delay(100);

  lidar_RR.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!
  //lidar_RR.rangingTest(&measure3, true); // pass in 'true' to get debug data printout!
  if (measure3.RangeStatus != 4)  // phase failures have incorrect data
  {
    RR_Dist_mm = measure3.RangeMilliMeter;
    if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RR_Dist_mm = 0;

  //07/16/20 added left side
  lidar_LF.rangingTest(&measure4, false); // pass in 'true' to get debug data printout!
  //lidar_LF.rangingTest(&measure4, true); // pass in 'true' to get debug data printout!
  if (measure4.RangeStatus != 4)  // phase failures have incorrect data
  {
    LF_Dist_mm = measure4.RangeMilliMeter;
    if (LF_Dist_mm > MAX_LIDAR_RANGE_MM) LF_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else LF_Dist_mm = 0;

  delay(100);

  lidar_LC.rangingTest(&measure5, false); // pass in 'true' to get debug data printout!
  //lidar_LC.rangingTest(&measure5, true); // pass in 'true' to get debug data printout!
  if (measure5.RangeStatus != 4)  // phase failures have incorrect data
  {
    LC_Dist_mm = measure5.RangeMilliMeter;
    if (LC_Dist_mm > MAX_LIDAR_RANGE_MM) LC_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else LC_Dist_mm = 0;

  delay(100);

  lidar_LR.rangingTest(&measure6, false); // pass in 'true' to get debug data printout!
  //lidar_LR.rangingTest(&measure6, true); // pass in 'true' to get debug data printout!
  if (measure6.RangeStatus != 4)  // phase failures have incorrect data
  {
    LR_Dist_mm = measure6.RangeMilliMeter;
    if (LR_Dist_mm > MAX_LIDAR_RANGE_MM) LR_Dist_mm = MAX_LIDAR_RANGE_MM;
  }
  else RR_Dist_mm = 0;

  Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,
    LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

  delay(250);
}

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

Name: Teensy_Hex_VL53L0X_Demo_V3.ino

Created: 7/22/2020 11:31:10 AM

Author: FRANKNEWXPS15\Frank

Name: Teensy_Hex_V53L0X.ino

Created: 7/16/2020 2:56:35 PM

Author: FRANKNEWXPS15\Frank

Notes:

07/18/20: I discovered that only 3 VL53L0X units can share the I2C bus. The GY530

modules have internal 10K pullups on the I2C lines, and the 4th one on the

bus reduces the combined pullup value too much.

Instead, I put the right-hand array of 3 on Wire1 and the left-hand array

of 3 on Wire2.

Name: Teensy_Triple_VL53L0X_V2.ino

Created: 5/23/2020 1:16:52 PM

Author: FRANKNEWXPS15\Frank

/* This code copied from the following post on the Adafruit forum:

https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5

#include <Wire.h>

#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Right-center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//07/16/20 added three left-side sensors

Adafruit_VL53L0X lidar_LF = Adafruit_VL53L0X(); //Left-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_LC = Adafruit_VL53L0X(); //Left-center LIDAR

Adafruit_VL53L0X lidar_LR = Adafruit_VL53L0X(); //left-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

const int LF_XSHUT_PIN = 5;

const int LC_XSHUT_PIN = 6;

const int LR_XSHUT_PIN = 7;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

uint16_t LF_Dist_mm;

uint16_t LC_Dist_mm;

uint16_t LR_Dist_mm;

void setup()

{

Serial.begin(115200); //Teensy ignores rate parameter

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(LF_XSHUT_PIN, OUTPUT);

pinMode(LC_XSHUT_PIN, OUTPUT);

pinMode(LR_XSHUT_PIN, OUTPUT);

// wait until serial port opens for native USB devices

while (!Serial)

{

delay(1);

}

delay(3000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

Serial.printf("Teensy Hex VL53L0X Demo\n");

//initialize all six LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

//right side

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

//right side

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

delay(1000);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

//right side

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

// 4. Initialize sensor #1

if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))

//if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, true, &Wire1))

{

Serial.println(F("Failed to boot lidar_RF"));

while (1);

}

// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(RC_XSHUT_PIN, HIGH);

if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))

//if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, true, &Wire1))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 7. Repeat for each sensor, turning each one on, setting a unique address.

digitalWrite(RR_XSHUT_PIN, HIGH);

if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))

//if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, true, &Wire1))

{

Serial.println(F("Failed to boot lidar_RR"));

while (1);

}

//07/21/20 now go back and verify that all I2C addresses are unique

Serial.printf("Scanning Wire1...");

int nDevices = 0;

for (byte address = 1; address < 127; ++address) {

// The i2c_scanner uses the return value of

// the Write.endTransmisstion to see if

// a device did acknowledge to the address.

//Wire.beginTransmission(address);

Wire1.beginTransmission(address);

byte error = Wire1.endTransmission();

if (error == 0) {

Serial.print("I2C device found at address 0x");

if (address < 16) {

Serial.print("0");

}

Serial.print(address, HEX);

Serial.println(" !");

++nDevices;

}

else if (error == 4) {

Serial.print("Unknown error at address 0x");

if (address < 16) {

Serial.print("0");

}

Serial.println(address, HEX);

}

if (nDevices == 0) {

Serial.println("No I2C devices found\n");

}

else {

Serial.println("done\n");

}

delay(2000);

//left side

digitalWrite(LF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(LC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(LR_XSHUT_PIN, LOW);

delay(10);

digitalWrite(LF_XSHUT_PIN, HIGH);

digitalWrite(LC_XSHUT_PIN, HIGH);

digitalWrite(LR_XSHUT_PIN, HIGH);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

//07/15/20 do same thing for left side sensors

//2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

// 4. Initialize sensor #1

if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire2))

// if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, true, &Wire2))

{

Serial.println(F("Failed to boot lidar_LF"));

while (1);

}

// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(LC_XSHUT_PIN, HIGH);

if (!lidar_LC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 7. Repeat for each sensor, turning each one on, setting a unique address.

digitalWrite(LR_XSHUT_PIN, HIGH);

if (!lidar_LR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))

{

Serial.println(F("Failed to boot lidar_LR"));

while (1);

}

Serial.printf("\nInit complete\n \nVL53L0X API Simple Ranging example\n \n");

Serial.printf("Scanning Wire2...");

nDevices = 0;

for (byte address = 1; address < 127; ++address) {

// The i2c_scanner uses the return value of

// the Write.endTransmisstion to see if

// a device did acknowledge to the address.

//Wire.beginTransmission(address);

Wire2.beginTransmission(address);

byte error = Wire2.endTransmission();

if (error == 0) {

Serial.print("I2C device found at address 0x");

if (address < 16) {

Serial.print("0");

}

Serial.print(address, HEX);

Serial.println(" !");

++nDevices;

}

else if (error == 4) {

Serial.print("Unknown error at address 0x");

if (address < 16) {

Serial.print("0");

}

Serial.println(address, HEX);

}

if (nDevices == 0) {

Serial.println("No I2C devices found\n");

}

else {

Serial.println("done\n");

}

delay(2000);

Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");

}

void loop()

{

VL53L0X_RangingMeasurementData_t measure1;

VL53L0X_RangingMeasurementData_t measure2;

VL53L0X_RangingMeasurementData_t measure3;

VL53L0X_RangingMeasurementData_t measure4;

VL53L0X_RangingMeasurementData_t measure5;

VL53L0X_RangingMeasurementData_t measure6;

lidar_RF.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!

//lidar_RF.rangingTest(&measure1, true); // pass in 'true' to get debug data printout!

if (measure1.RangeStatus != 4) // phase failures have incorrect data

{

RF_Dist_mm = measure1.RangeMilliMeter;

if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RF_Dist_mm = 0;

delay(100);

lidar_RC.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!

//lidar_RC.rangingTest(&measure2, true); // pass in 'true' to get debug data printout!

if (measure2.RangeStatus != 4) // phase failures have incorrect data

{

RC_Dist_mm = measure2.RangeMilliMeter;

if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RC_Dist_mm = 0;

delay(100);

lidar_RR.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!

//lidar_RR.rangingTest(&measure3, true); // pass in 'true' to get debug data printout!

if (measure3.RangeStatus != 4) // phase failures have incorrect data

{

RR_Dist_mm = measure3.RangeMilliMeter;

if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

//07/16/20 added left side

lidar_LF.rangingTest(&measure4, false); // pass in 'true' to get debug data printout!

//lidar_LF.rangingTest(&measure4, true); // pass in 'true' to get debug data printout!

if (measure4.RangeStatus != 4) // phase failures have incorrect data

{

LF_Dist_mm = measure4.RangeMilliMeter;

if (LF_Dist_mm > MAX_LIDAR_RANGE_MM) LF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else LF_Dist_mm = 0;

delay(100);

lidar_LC.rangingTest(&measure5, false); // pass in 'true' to get debug data printout!

//lidar_LC.rangingTest(&measure5, true); // pass in 'true' to get debug data printout!

if (measure5.RangeStatus != 4) // phase failures have incorrect data

{

LC_Dist_mm = measure5.RangeMilliMeter;

if (LC_Dist_mm > MAX_LIDAR_RANGE_MM) LC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else LC_Dist_mm = 0;

delay(100);

lidar_LR.rangingTest(&measure6, false); // pass in 'true' to get debug data printout!

//lidar_LR.rangingTest(&measure6, true); // pass in 'true' to get debug data printout!

if (measure6.RangeStatus != 4) // phase failures have incorrect data

{

LR_Dist_mm = measure6.RangeMilliMeter;

if (LR_Dist_mm > MAX_LIDAR_RANGE_MM) LR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,

LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

delay(250);

}

When I ran this program, I got the following output:

Opening port
Port open
Teensy Hex VL53L0X Demo
In Adafruit_VL53L0X::begin(2A,0,7D0)
In Adafruit_VL53L0X::begin(2B,0,7D0)
In Adafruit_VL53L0X::begin(2C,0,7D0)
Scanning Wire1...I2C device found at address 0x2A  !
I2C device found at address 0x2B  !
I2C device found at address 0x2C  !
done

In Adafruit_VL53L0X::begin(2D,0,6F8)
In Adafruit_VL53L0X::begin(2E,0,6F8)
In Adafruit_VL53L0X::begin(2F,0,6F8)

Init complete
 
VL53L0X API Simple Ranging example
 
Scanning Wire2...I2C device found at address 0x2D  !
I2C device found at address 0x2E  !
I2C device found at address 0x2F  !
done

Msec	Front	Center	Rear	Steer
11267	0	12	163	1000	1000	1000
12161	0	12	160	1000	1000	1000
13055	0	12	163	416	1000	1000
13948	0	12	161	93	102	79
14842	0	12	163	154	102	69
15735	0	12	167	58	113	1000
16629	0	12	167	70	71	40
17522	0	12	167	45	53	69
18399	0	12	0	472	381	69
19292	0	12	60	0	1000	1000
20186	0	12	35	1000	1000	1000
21045	0	12	59	0	0	1000
21938	0	12	75	1000	1000	1000
22814	0	12	0	1000	1000	1000
23708	0	12	0	1000	1000	1000
24585	0	12	165	0	1000	1000
25479	0	12	168	1000	1000	1000

Port closed

Opening port

Port open

Teensy Hex VL53L0X Demo

In Adafruit_VL53L0X::begin(2A,0,7D0)

In Adafruit_VL53L0X::begin(2B,0,7D0)

In Adafruit_VL53L0X::begin(2C,0,7D0)

Scanning Wire1...I2C device found at address 0x2A !

I2C device found at address 0x2B !

I2C device found at address 0x2C !

done

In Adafruit_VL53L0X::begin(2D,0,6F8)

In Adafruit_VL53L0X::begin(2E,0,6F8)

In Adafruit_VL53L0X::begin(2F,0,6F8)

Init complete

VL53L0X API Simple Ranging example

Scanning Wire2...I2C device found at address 0x2D !

I2C device found at address 0x2E !

I2C device found at address 0x2F !

done

Msec Front Center Rear Steer

11267 0 12 163 1000 1000 1000

12161 0 12 160 1000 1000 1000

13055 0 12 163 416 1000 1000

13948 0 12 161 93 102 79

14842 0 12 163 154 102 69

15735 0 12 167 58 113 1000

16629 0 12 167 70 71 40

17522 0 12 167 45 53 69

18399 0 12 0 472 381 69

19292 0 12 60 0 1000 1000

20186 0 12 35 1000 1000 1000

21045 0 12 59 0 0 1000

21938 0 12 75 1000 1000 1000

22814 0 12 0 1000 1000 1000

23708 0 12 0 1000 1000 1000

24585 0 12 165 0 1000 1000

25479 0 12 168 1000 1000 1000

Port closed

So it seems pretty clear that there is something going on with the Teensy 3.5 that doesn’t like it when I try to run both Wire1 & Wire2 buses at the same time.

As additional background data, the original impetus for splitting the six sensors between two I2C buses was my discovery that adding the 4th through 6th sensors on the Wire1 bus caused a similar problem to the one described here; clearly bad readings from the 1st & second sensors, while readings from later ones were fine. I don’t know if these issues are related, but something is happening for sure.

23 July 2020 Update:

I’m frustrated at the lack of response from both the Teensy and ST Micro support forums on this issue. The Teensy guys are trying to help, but nobody wants to look at the elephant in the room – the fact that all six VL53L0X units work fine when their respective I2C bus is the only one operating, but not when both buses are in operation. The ST Micro guys just don’t answer at all.

I went back and modified my program to print out as many of the detailed measurement parameters as I could find for each sensor, in an effort to gain some understanding about what is happening, and got the following output:

Opening port
Port open
Teensy Hex VL53L0X Demo

Init complete
 
VL53L0X API Simple Ranging example
 
distances = 0	0	140	374	478	489
SignalRate = 0	0	822272	185856	154112	127488
SpadCount = 0	0	49156	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	144	419	473	524
SignalRate = 0	0	815616	175616	121856	92160
SpadCount = 0	0	49156	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	145	494	504	544
SignalRate = 0	0	826880	134656	81408	67072
SpadCount = 0	0	49156	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	146	499	509	505
SignalRate = 0	0	817152	137728	88576	68096
SpadCount = 0	0	49156	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	144	471	501	525
SignalRate = 0	0	820224	145920	90112	51200
SpadCount = 0	0	49156	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	143	186	184	140
SignalRate = 0	0	818688	1047552	1006592	1078272
SpadCount = 0	0	49156	49156	47620	47108
RangeStatus = 0	0	0	0	0	0
distances = 0	0	146	72	80	43
SignalRate = 0	0	823808	1641472	1388544	2066432
SpadCount = 0	0	49156	3332	2052	2564
RangeStatus = 0	0	0	0	0	0
distances = 0	0	144	44	55	27
SignalRate = 0	0	813056	1105920	1049600	1935872
SpadCount = 0	0	49156	828	724	1284
RangeStatus = 0	0	0	0	0	0
distances = 0	0	101	42	61	31
SignalRate = 0	0	1653248	1267200	757248	2098176
SpadCount = 0	0	34820	824	676	1284
RangeStatus = 0	0	0	0	0	0
distances = 0	0	138	29	50	50
SignalRate = 0	0	820224	1120768	980480	1505792
SpadCount = 0	0	49156	676	572	2308
RangeStatus = 0	0	0	0	0	0
distances = 0	0	143	37	61	23
SignalRate = 0	0	823296	1147904	1652224	1979904
SpadCount = 0	0	49156	572	3076	2052
RangeStatus = 0	0	0	0	0	0
distances = 0	0	179	32	42	37
SignalRate = 0	0	727552	1956352	1411584	1645568
SpadCount = 0	0	49156	364	520	1540
RangeStatus = 0	0	0	0	0	0
distances = 0	0	156	37	64	28
SignalRate = 0	0	786944	1586688	1738752	1842176
SpadCount = 0	0	49156	1028	3588	2308
RangeStatus = 0	0	0	0	0	0
distances = 0	0	25	42	49	89
SignalRate = 0	0	1868288	1352192	964608	1336832
SpadCount = 0	0	980	520	932	14852
RangeStatus = 0	0	0	0	0	0
distances = 0	0	152	270	141	117
SignalRate = 0	0	776192	376320	1430016	1573888
SpadCount = 0	0	49156	49156	44292	47108
RangeStatus = 0	0	0	2	0	0
distances = 0	0	29	84	64	22
SignalRate = 0	0	1496576	1457664	1726464	1656320
SpadCount = 0	0	2052	5124	3332	1540
RangeStatus = 0	0	0	0	0	0
distances = 0	0	149	93	182	358
SignalRate = 0	0	819200	1301504	800256	95232
SpadCount = 0	0	49156	8964	47620	47108
RangeStatus = 0	0	0	0	0	2
distances = 0	0	148	384	474	641
SignalRate = 0	0	821760	130560	56320	50688
SpadCount = 0	0	49156	49156	47620	47108
RangeStatus = 0	0	0	2	2	2

Port closed

Opening port

Port open

Teensy Hex VL53L0X Demo

Init complete

VL53L0X API Simple Ranging example

distances = 0 0 140 374 478 489

SignalRate = 0 0 822272 185856 154112 127488

SpadCount = 0 0 49156 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 144 419 473 524

SignalRate = 0 0 815616 175616 121856 92160

SpadCount = 0 0 49156 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 145 494 504 544

SignalRate = 0 0 826880 134656 81408 67072

SpadCount = 0 0 49156 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 146 499 509 505

SignalRate = 0 0 817152 137728 88576 68096

SpadCount = 0 0 49156 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 144 471 501 525

SignalRate = 0 0 820224 145920 90112 51200

SpadCount = 0 0 49156 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 143 186 184 140

SignalRate = 0 0 818688 1047552 1006592 1078272

SpadCount = 0 0 49156 49156 47620 47108

RangeStatus = 0 0 0 0 0 0

distances = 0 0 146 72 80 43

SignalRate = 0 0 823808 1641472 1388544 2066432

SpadCount = 0 0 49156 3332 2052 2564

RangeStatus = 0 0 0 0 0 0

distances = 0 0 144 44 55 27

SignalRate = 0 0 813056 1105920 1049600 1935872

SpadCount = 0 0 49156 828 724 1284

RangeStatus = 0 0 0 0 0 0

distances = 0 0 101 42 61 31

SignalRate = 0 0 1653248 1267200 757248 2098176

SpadCount = 0 0 34820 824 676 1284

RangeStatus = 0 0 0 0 0 0

distances = 0 0 138 29 50 50

SignalRate = 0 0 820224 1120768 980480 1505792

SpadCount = 0 0 49156 676 572 2308

RangeStatus = 0 0 0 0 0 0

distances = 0 0 143 37 61 23

SignalRate = 0 0 823296 1147904 1652224 1979904

SpadCount = 0 0 49156 572 3076 2052

RangeStatus = 0 0 0 0 0 0

distances = 0 0 179 32 42 37

SignalRate = 0 0 727552 1956352 1411584 1645568

SpadCount = 0 0 49156 364 520 1540

RangeStatus = 0 0 0 0 0 0

distances = 0 0 156 37 64 28

SignalRate = 0 0 786944 1586688 1738752 1842176

SpadCount = 0 0 49156 1028 3588 2308

RangeStatus = 0 0 0 0 0 0

distances = 0 0 25 42 49 89

SignalRate = 0 0 1868288 1352192 964608 1336832

SpadCount = 0 0 980 520 932 14852

RangeStatus = 0 0 0 0 0 0

distances = 0 0 152 270 141 117

SignalRate = 0 0 776192 376320 1430016 1573888

SpadCount = 0 0 49156 49156 44292 47108

RangeStatus = 0 0 0 2 0 0

distances = 0 0 29 84 64 22

SignalRate = 0 0 1496576 1457664 1726464 1656320

SpadCount = 0 0 2052 5124 3332 1540

RangeStatus = 0 0 0 0 0 0

distances = 0 0 149 93 182 358

SignalRate = 0 0 819200 1301504 800256 95232

SpadCount = 0 0 49156 8964 47620 47108

RangeStatus = 0 0 0 0 0 2

distances = 0 0 148 384 474 641

SignalRate = 0 0 821760 130560 56320 50688

SpadCount = 0 0 49156 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

Port closed

This output style is much harder to read, but is also much more complete. Each line (distances, signal rate, SpadCount, and RangeStatus) has six entries – one for each of the six sensors. The first three entries are sensors 1-3 on Teensy Wire1, and the remaining three are sensors 4-6 on Teensy Wire2. As the data shows, sensors 1 & 2 always have bogus results, while sensors 3-6 have what appears to be valid data, although I’m not competent to say anything more than “the distance values for sensors 3-6 track reality, while the ones for sensors 1-2 do not”.

Then I modified the program yet again to just use sensors 1-3 on Teensy Wire1 I2C bus, without changing any hardware (leaving sensors 4-6 still attached to Teensy Wire2, but not initializing or addressing them in any way, and got the following output:

Opening port
Port open
Teensy Hex VL53L0X Demo

Init complete
 
VL53L0X API Simple Ranging example
 
distances = 90	141	148	0	0	0
SignalRate = 1421312	299520	822272	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 89	145	150	0	0	0
SignalRate = 1410560	300032	826880	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 87	144	148	0	0	0
SignalRate = 1410048	301056	824832	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 85	145	144	0	0	0
SignalRate = 1415168	303104	829440	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 87	145	147	0	0	0
SignalRate = 1422336	300544	823296	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 89	143	147	0	0	0
SignalRate = 1410560	292864	820736	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 87	148	148	0	0	0
SignalRate = 1419264	300032	820224	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 88	144	147	0	0	0
SignalRate = 1414656	295936	825344	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 87	142	147	0	0	0
SignalRate = 1424896	303104	819712	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 89	144	149	0	0	0
SignalRate = 1412096	296448	827904	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 89	144	147	0	0	0
SignalRate = 1431552	301056	825344	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 90	143	148	0	0	0
SignalRate = 1423872	300544	823296	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 86	139	102	0	0	0
SignalRate = 1421312	306688	2149376	0	0	0
SpadCount = 1028	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 46	46	35	0	0	0
SignalRate = 1459712	1107456	1230336	0	0	0
SpadCount = 2564	3076	572	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 38	42	48	0	0	0
SignalRate = 1839104	1229312	788480	0	0	0
SpadCount = 1540	1796	676	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 28	34	64	0	0	0
SignalRate = 2347008	1183744	1709056	0	0	0
SpadCount = 312	2052	3332	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 31	43	41	0	0	0
SignalRate = 1407488	1260544	1876480	0	0	0
SpadCount = 312	1540	1284	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 30	29	35	0	0	0
SignalRate = 1477120	1118720	1345536	0	0	0
SpadCount = 364	1028	520	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 23	31	32	0	0	0
SignalRate = 2257408	1090560	1561600	0	0	0
SpadCount = 260	468	468	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 22	10	44	0	0	0
SignalRate = 2420224	1034752	918528	0	0	0
SpadCount = 260	520	572	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 23	40	75	0	0	0
SignalRate = 2063360	1061888	1448960	0	0	0
SpadCount = 260	3076	5124	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 22	56	78	0	0	0
SignalRate = 1777152	1045504	1638400	0	0	0
SpadCount = 260	8196	5636	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 41	38	39	0	0	0
SignalRate = 1574912	1059840	1111552	0	0	0
SpadCount = 1540	2820	624	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 45	46	60	0	0	0
SignalRate = 1644032	1007104	1708544	0	0	0
SpadCount = 2052	3332	2564	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 25	49	48	0	0	0
SignalRate = 1822208	1109504	1946624	0	0	0
SpadCount = 312	6660	1796	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 30	17	25	0	0	0
SignalRate = 2045440	1556992	2938368	0	0	0
SpadCount = 260	364	364	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 30	31	30	0	0	0
SignalRate = 1498624	1258496	2680832	0	0	0
SpadCount = 1540	520	416	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 33	26	147	0	0	0
SignalRate = 1547776	1767424	816640	0	0	0
SpadCount = 1796	468	49156	0	0	0
RangeStatus = 0	0	0	0	0	0
distances = 90	149	147	0	0	0
SignalRate = 1627136	294912	819200	0	0	0
SpadCount = 1284	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 91	136	146	0	0	0
SignalRate = 1653760	298496	819200	0	0	0
SpadCount = 1284	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 89	147	145	0	0	0
SignalRate = 1652736	297984	818176	0	0	0
SpadCount = 1284	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0
distances = 89	141	150	0	0	0
SignalRate = 1661440	300032	820224	0	0	0
SpadCount = 1284	49924	49156	0	0	0
RangeStatus = 0	2	0	0	0	0

Port closed

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

Opening port

Port open

Teensy Hex VL53L0X Demo

Init complete

VL53L0X API Simple Ranging example

distances = 90 141 148 0 0 0

SignalRate = 1421312 299520 822272 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 89 145 150 0 0 0

SignalRate = 1410560 300032 826880 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 87 144 148 0 0 0

SignalRate = 1410048 301056 824832 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 85 145 144 0 0 0

SignalRate = 1415168 303104 829440 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 87 145 147 0 0 0

SignalRate = 1422336 300544 823296 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 89 143 147 0 0 0

SignalRate = 1410560 292864 820736 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 87 148 148 0 0 0

SignalRate = 1419264 300032 820224 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 88 144 147 0 0 0

SignalRate = 1414656 295936 825344 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 87 142 147 0 0 0

SignalRate = 1424896 303104 819712 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 89 144 149 0 0 0

SignalRate = 1412096 296448 827904 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 89 144 147 0 0 0

SignalRate = 1431552 301056 825344 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 90 143 148 0 0 0

SignalRate = 1423872 300544 823296 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 86 139 102 0 0 0

SignalRate = 1421312 306688 2149376 0 0 0

SpadCount = 1028 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 46 46 35 0 0 0

SignalRate = 1459712 1107456 1230336 0 0 0

SpadCount = 2564 3076 572 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 38 42 48 0 0 0

SignalRate = 1839104 1229312 788480 0 0 0

SpadCount = 1540 1796 676 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 28 34 64 0 0 0

SignalRate = 2347008 1183744 1709056 0 0 0

SpadCount = 312 2052 3332 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 31 43 41 0 0 0

SignalRate = 1407488 1260544 1876480 0 0 0

SpadCount = 312 1540 1284 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 30 29 35 0 0 0

SignalRate = 1477120 1118720 1345536 0 0 0

SpadCount = 364 1028 520 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 23 31 32 0 0 0

SignalRate = 2257408 1090560 1561600 0 0 0

SpadCount = 260 468 468 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 22 10 44 0 0 0

SignalRate = 2420224 1034752 918528 0 0 0

SpadCount = 260 520 572 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 23 40 75 0 0 0

SignalRate = 2063360 1061888 1448960 0 0 0

SpadCount = 260 3076 5124 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 22 56 78 0 0 0

SignalRate = 1777152 1045504 1638400 0 0 0

SpadCount = 260 8196 5636 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 41 38 39 0 0 0

SignalRate = 1574912 1059840 1111552 0 0 0

SpadCount = 1540 2820 624 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 45 46 60 0 0 0

SignalRate = 1644032 1007104 1708544 0 0 0

SpadCount = 2052 3332 2564 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 25 49 48 0 0 0

SignalRate = 1822208 1109504 1946624 0 0 0

SpadCount = 312 6660 1796 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 30 17 25 0 0 0

SignalRate = 2045440 1556992 2938368 0 0 0

SpadCount = 260 364 364 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 30 31 30 0 0 0

SignalRate = 1498624 1258496 2680832 0 0 0

SpadCount = 1540 520 416 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 33 26 147 0 0 0

SignalRate = 1547776 1767424 816640 0 0 0

SpadCount = 1796 468 49156 0 0 0

RangeStatus = 0 0 0 0 0 0

distances = 90 149 147 0 0 0

SignalRate = 1627136 294912 819200 0 0 0

SpadCount = 1284 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 91 136 146 0 0 0

SignalRate = 1653760 298496 819200 0 0 0

SpadCount = 1284 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 89 147 145 0 0 0

SignalRate = 1652736 297984 818176 0 0 0

SpadCount = 1284 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

distances = 89 141 150 0 0 0

SignalRate = 1661440 300032 820224 0 0 0

SpadCount = 1284 49924 49156 0 0 0

RangeStatus = 0 2 0 0 0 0

Port closed

Now the parameters for sensors 1-3 all look real, and of course all the parameters for sensors 4-6 are zeroed out.

Then I modified the program to just initialize and access sensors 4-6 on Teensy Wire2

Opening port
Port open
Teensy Hex VL53L0X Demo

Init complete
 
VL53L0X API Simple Ranging example
 
distances = 0	0	0	444	416	417
SignalRate = 0	0	0	186368	132608	101376
SpadCount = 0	0	0	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	0	458	420	416
SignalRate = 0	0	0	193536	139264	120832
SpadCount = 0	0	0	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	0	552	547	441
SignalRate = 0	0	0	182784	141824	148992
SpadCount = 0	0	0	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	0	549	47	31
SignalRate = 0	0	0	181760	1175040	1395712
SpadCount = 0	0	0	49156	624	676
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	549	53	60
SignalRate = 0	0	0	179712	1097728	1451008
SpadCount = 0	0	0	49156	828	3076
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	134	142	145
SignalRate = 0	0	0	1542144	1464320	1479168
SpadCount = 0	0	0	49156	20228	25604
RangeStatus = 0	0	0	0	0	0
distances = 0	0	0	312	51	27
SignalRate = 0	0	0	415232	2140672	2277376
SpadCount = 0	0	0	49156	1540	1284
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	544	33	43
SignalRate = 0	0	0	184320	1910272	1748480
SpadCount = 0	0	0	49156	416	2564
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	521	37	33
SignalRate = 0	0	0	199680	1656832	1090048
SpadCount = 0	0	0	49156	416	728
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	524	85	31
SignalRate = 0	0	0	201728	1552384	1851392
SpadCount = 0	0	0	49156	6404	1540
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	519	33	35
SignalRate = 0	0	0	194048	2048512	1491968
SpadCount = 0	0	0	49156	416	1796
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	499	49	68
SignalRate = 0	0	0	179712	1519104	1511424
SpadCount = 0	0	0	49156	1028	6916
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	50	39	33
SignalRate = 0	0	0	1575936	1763328	1301504
SpadCount = 0	0	0	8964	364	468
RangeStatus = 0	0	0	0	0	0
distances = 0	0	0	512	50	26
SignalRate = 0	0	0	188928	1815552	1411584
SpadCount = 0	0	0	49156	1284	572
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	513	41	81
SignalRate = 0	0	0	193536	1374720	1184768
SpadCount = 0	0	0	49156	416	8452
RangeStatus = 0	0	0	2	0	0
distances = 0	0	0	459	389	401
SignalRate = 0	0	0	206848	191488	169984
SpadCount = 0	0	0	49156	47620	47108
RangeStatus = 0	0	0	2	2	2
distances = 0	0	0	430	439	422
SignalRate = 0	0	0	188416	175616	153600
SpadCount = 0	0	0	49156	47620	47108
RangeStatus = 0	0	0	2	2	2

Port closed

Opening port

Port open

Teensy Hex VL53L0X Demo

Init complete

VL53L0X API Simple Ranging example

distances = 0 0 0 444 416 417

SignalRate = 0 0 0 186368 132608 101376

SpadCount = 0 0 0 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 0 458 420 416

SignalRate = 0 0 0 193536 139264 120832

SpadCount = 0 0 0 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 0 552 547 441

SignalRate = 0 0 0 182784 141824 148992

SpadCount = 0 0 0 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 0 549 47 31

SignalRate = 0 0 0 181760 1175040 1395712

SpadCount = 0 0 0 49156 624 676

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 549 53 60

SignalRate = 0 0 0 179712 1097728 1451008

SpadCount = 0 0 0 49156 828 3076

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 134 142 145

SignalRate = 0 0 0 1542144 1464320 1479168

SpadCount = 0 0 0 49156 20228 25604

RangeStatus = 0 0 0 0 0 0

distances = 0 0 0 312 51 27

SignalRate = 0 0 0 415232 2140672 2277376

SpadCount = 0 0 0 49156 1540 1284

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 544 33 43

SignalRate = 0 0 0 184320 1910272 1748480

SpadCount = 0 0 0 49156 416 2564

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 521 37 33

SignalRate = 0 0 0 199680 1656832 1090048

SpadCount = 0 0 0 49156 416 728

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 524 85 31

SignalRate = 0 0 0 201728 1552384 1851392

SpadCount = 0 0 0 49156 6404 1540

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 519 33 35

SignalRate = 0 0 0 194048 2048512 1491968

SpadCount = 0 0 0 49156 416 1796

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 499 49 68

SignalRate = 0 0 0 179712 1519104 1511424

SpadCount = 0 0 0 49156 1028 6916

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 50 39 33

SignalRate = 0 0 0 1575936 1763328 1301504

SpadCount = 0 0 0 8964 364 468

RangeStatus = 0 0 0 0 0 0

distances = 0 0 0 512 50 26

SignalRate = 0 0 0 188928 1815552 1411584

SpadCount = 0 0 0 49156 1284 572

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 513 41 81

SignalRate = 0 0 0 193536 1374720 1184768

SpadCount = 0 0 0 49156 416 8452

RangeStatus = 0 0 0 2 0 0

distances = 0 0 0 459 389 401

SignalRate = 0 0 0 206848 191488 169984

SpadCount = 0 0 0 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

distances = 0 0 0 430 439 422

SignalRate = 0 0 0 188416 175616 153600

SpadCount = 0 0 0 49156 47620 47108

RangeStatus = 0 0 0 2 2 2

Port closed

Now it is clear that the data for sensors 1-3 (Teensy Wire1) are all zeroes, and the data for sensors 4-6 (Teensy Wire2) are valid. Again, this is with no hardware changes at all; all sensors are still powered and connected to their respective I2C buses.

29 July 2020 Update:

Still working the multiple VL53L0X issue. After getting nowhere with the Teensy and ST Micro forums, I decided to try a different tack. I decided to try controlling all six VL53L0X sensors using the single I2C bus on an Arduino Mega. I reasoned that if I could get them all to play using a Mega, this would lend credence to my theory that something funny is going on with the Teensy 3.5 auxiliary I2C buses.

Unfortunately, I immediately ran into problems getting multiple sensors to work using the Arduino Mega. At first I thought this was due to the fact that the Mega is a 5V controller and so I needed a level shifter setup on the I2C bus between the Mega and the VL53L0X sensors, but that didn’t change anything. Then, after a more thorough look at the VL53L0X schematic and documentation I discovered that the real problem was that while the I2C bus lines have internal level shifters already implemented on the module, the XSHUT & GPIO lines do not. This meant that I had been driving the XSHUT line of each of my attached sensors well over the do-not-exceed level — oops!

At this point I was starting to wonder if I had damaged the sensors’ XSHUT lines, thereby making any further diagnostic attempts with these sensors fruitless. In addition, I was starting to wonder if I hadn’t also given myself problems by using ‘no-name’ modules – cheap, but maybe worth every penny? I also had read some posts that indicated that the Adafruit VL53L0X driver library might have some problems, so maybe I had a trifecta going – cheap no-name modules, potentially damaged by my abuse of the XSHUT lines, being driven by a questionable library – yikes!

So, I started over; I acquired some Pololu VL53L0X modules, installed their Arduino driver library, and used their ‘Single’ code example to verify basic operation with a single VL53L0X sensor connected to an Arduino Mega controller. Then I added in the multi-sensor initialization code, being careful to simply switch the XSHUT lines from output (for outputting a LOW signal) to input (for ‘outputting’ a HIGH signal by allowing the onboard 47K pullup to take over) for XSHUT line management.

With the above setup I have been able to demonstrate a working 3-sensor setup using an Arduino Mega controller. When my remaining Pololu VL53L0X modules arrive later this week, I hope to show that I can run (at least) six Pololu VL53L0X sensors on a single I2C bus. If I can do that, then I’ll be in a position to make some more waves on the Teensy forum (I hope).

By the way, one of the side-effects of this effort was a reply from John Kvam mentioning that ST Micro makes an Arduino compatible 3.3V 32-bit microcontroller called the ST32 (also known as the ‘blue pill’ for it’s PCB color). This is pretty capable device, with the single drawback that it doesn’t come with an Arduino bootloader installed, meaning it can’t be directly programmed via its USB-C connector. Instead, one has to use a FTDI device (like the CKDevices FTDI Pro or the Sparkfun FTDI breakout board. However, there are plenty of “How To’s” out there describing how a bootloader can be loaded into flash memory, after which you can program it just like any other Arduino device – pretty cool! Anyway, I ordered a couple of these boards to play with the next time I need something Arduino-ish but not as fast (or expensive) as a Teensy.

31 July 2020 Update:

Success! I finally got more than three VL53L0X sensors working on the same I2C bus using an Arduino controller! However, I’m embarrassed to say that in the process, I discovered a hidden broken ground wire in one of my two I2C bus daisy chains, and this may have been causing the symptoms I was seeing with the Teensy 2-bus configuration – don’t know yet.

In any case, after repairing the wire break I got a set of six VL53L0X sensors working, consisting of the three Pololu modules I just got in, plus three of the older GY530 modules I was using on earlier Teensy-based experiments. After that, I was able to demonstrate proper operation of the two 3-sensor arrays from my Wall-E2 robot, as shown in the following photo and Excel plot.

Sensor arrays dismounted from Wall-E2 and connected to Arduino Mega I2C bus

01 August 2020 Update:

Well, it is officially time to eat crow. After all the whooping and hollering I’ve done, it turns out the entire problem was a hidden ground wire break in I2C daisy-chain cable attached to the Teensy 3.5’s Wire2 I2C bus. After repairing the break, I can now demonstrate operation of six VL53L0X sensors on two different I2C buses on the Teensy 3.5, as shown in photo and Excel plot below.

Six VL53L0X sensors on Teensy 3.5 Wire1 & Wire2 I2C buses. Note ground wire repair on left rear connector (top right in photo)

Now that this saga has been thankfully resolved, I can get back to the original project of integrating these two sensor arrays onto Wall-E2, my autonomous wall-following robot.

August 08, 2020 Update:

I believe I have finally completed the effort to integrate the VL53L0X sensor arrays onto Wall-E2, my autonomous wall-following robot. Here’s the physical setup

Dual 3-element VL53L0X sensor arrays on top deck of Wall-E2.

Note that the USB cable to the Teensy 3.5 is temporary, just for testing. To verify proper operation, I wrote a small program for the Mega 2560 main controller containing only the code from the main FourWD_WallE2_V5.ino required to retrieve sensor values from the Teensy 3.5, and used this program to verify and debug the Teensy 3.5 program. As the two Excel plots below show, the main Mega 2560 controller can now retrieve distance data from all six VL53L0X sensors at once.

VL53L0X distances reported locally by the Teensy 3.5

VL53L0X distances as retrieved from the Teensy 3.5 by the Mega 2560

There are some very small differences in these two plots, which I attribute to the fact that the Teensy measurement timing and the Mega 2560 retrieval timing are asynchronous, so the Mega may be retrieving some new and some ‘old’ (in the sense that it might be 100 mSec older than the rest) measurement data. This is insignificant operationally, and wouldn’t be evident unless this sort of simultaneous local/remote reporting was done.

A minor side note; I wound up using the GY530 ‘no name’ sensors rather than the Pololu ones because they were a) smaller, and b) already mounted on the two custom brackets I printed for them. The Pololu sensors (along with a whole bunch of GY530’s that finally arrived from Ali Express) went into my ‘Sensors’ parts bin for the next project. If anyone needs VL53L0X sensors, let me know! 😉

Stay tuned,

Frank

I2C Hangup bug cured! Miracle of Miracles! Film at 11!

12 Replies

Posted 06 July 2020

Miracle of miracles! Arduino finally got off their collective asses and decided to do something about the well-known, well-documented, and long-ignored I2C hangup bug. Thanks to Grey Christoforo of Oxford, England for submitting the pull request that started the ball rolling. See this github issue thread for all the gory details. However, in a bizarre outcome, the implementation of the needed timeouts isn’t implemented by default! You have to modify your code to add a call to a new function, like the following:

Wire.setWireTimeout(3000, true); //timeout value in uSec - SBWire uses 100 uSec, so 1000 should be OK

1	Wire.setWireTimeout(3000, true); //timeout value in uSec - SBWire uses 100 uSec, so 1000 should be OK

Note that you have to explicitly add a timeout value (3000 in my example above) or the timeout feature will still not be enabled! The ‘true’ parameter tells the library to reset the I2C bus if a timeout is detected – surely something you will want to do.

I’m currently working on a ‘before/after’ post to demonstrate that the new timeout feature actually works with real hardware scenarios. However, due to the intermittent nature of the I2C hangup bug, it takes a while (hours/days) to grind through enough iterations to excite the bug reliably, so it may be a while before I have a good demonstration

One last thing; at some point the examples in C:\Program Files (x86)\Arduino\hardware\arduino\avr\libraries\Wire\examples (on my Win 10 machine) will probably be updated/expanded to show how to properly implement the new timeout feature, but this has not happened yet AFAICT.

The rest of this post describes my attempt to verify that the new timeout feature does, in fact, work as advertised. The idea is to construct a “before-and-after” demonstration, where the ‘before’ configuration reliably hangs up using the Wire library without the timeout enabled, and an ‘after’ configuration that is identical to the ‘before’ setup except with the timeout enabled.

Before Configuration:

I actually started with a ‘before-before’ configuration using the SBWire library, as I have been working with I2C projects and the SBWire library ever since I gave up on the Arduino Wire library two years ago. This configuration is patterned after Wall-E2, my current autonomous wall-following robot, which uses an Adafruit RTC, an Adafruit FRAM, a DFRobots MPU6050 IMU, and six VL53L0X time-of-flight proximity sensors (the ToF sensors are managed by a slave Teensy over the I2C bus). For this test, I arranged all the I2C components on a plug board and connected to them using an Arduino Mega 2560 (the same controller I have on Wall-E2), as shown in the following photo.

From left to right; two VL53L0X ToF modules, FRAM module, DS3231 RTC module, MPU6050 IMU module

The software is a cut down version of the robot software, and in this first test all it does is print out time/date from the RTC and the relative heading value from the IMU. After almost 13 hours, it was still running fine, as shown below:

Date/Time		Min	HdgDeg
07/06/2020 10:27:47	762.43	116.50
07/06/2020 10:27:47	762.43	116.50
07/06/2020 10:27:48	762.44	116.49
07/06/2020 10:27:48	762.44	116.49
07/06/2020 10:27:48	762.44	116.50
07/06/2020 10:27:48	762.44	116.49
07/06/2020 10:27:48	762.44	116.49
07/06/2020 10:27:48	762.44	116.49
07/06/2020 10:27:48	762.45	116.49

Date/Time Min HdgDeg

07/06/2020 10:27:47 762.43 116.50

07/06/2020 10:27:48 762.44 116.49

07/06/2020 10:27:48 762.44 116.50

07/06/2020 10:27:48 762.44 116.49

07/06/2020 10:27:48 762.45 116.49

So now I have a ‘known good’ (with SBWire) hardware configuration. The next step is to change the software back from SBWire to Wire without the timeout implemented. This should fail – the IMU readout should hangup within a few hours as it did before I originally switched to SBWire.

July 08 2020 Update:

After laboriously changing back from SBWire to Wire, I got the configuration shown in the following photo to work properly using the new Wire library without the new timeout feature enabled.

From left to right: MPU6050 IMU, DS3231 RTC, Adafruit I2C FRAM, and 3e VL53L0X ToF proximity sensors, all on the Mega 2560’s I2C bus

I programmed the Mega to access everything but the FRAM 10 times/second, and print out the results on the serial monitor, and then let it run overnight. When I got up this morning I expected to see that it had hung up after a few hours, but discovered that it was still running fine after eight hours – bummer! at 10 meas/sec that is 480 min * 60 sec/min * 10 = 288,000 I2C measurement cycles * 5 I2C transactions per cycle = 1,440,000 I2C transactions. I was bummed out because it will be impossible to verify whether or not the timeout feature actually works if I can’t get a configuration that reliably hangs up. When I came back a few hours later, I saw that the printout to the serial monitor had stopped at around 700 minutes, but this turned out to be the monitor hanging up – not the I2C bus – double bummer.

So, I modified the program to only report results every second instead of 10/second so I won’t run out of serial monitor again, and restarted the ‘before’ configuration.

10 July 2020 Update:

I added the Sunfounder 20 x 4 I2C LCD display to the setup so I could display the IMU heading and proximity sensor distances locally, as shown below

I2C Test setup with Sunfounder 20 x 4 I2C LCD added

After getting this setup running, I was trying to figure out how to definitively demonstrate I2C bus hangups without the Wire library timeout feature (the ‘before’ configuration) and then demonstrate continued operation with timeouts enabled (the ‘after’ configuration). In an email conversation, Grey Christoforo pointed me to another poster who was doing the same thing, by using an external transistor to short one I2C line to ground under program control, thereby demonstrating that the timeout feature allowed continued operation. This gave me the idea that manually shorting one of the I2C lines to ground should do the same thing, and would allow me to demonstrate the ‘before’ and ‘after’ configurations.

The following code snippet shows the code necessary to enable the Wire library timeout feature

void setup()
{
	Serial.begin(115200);

         < all my other setup code removed >

	//Wire.setWireTimeout(25000,true);
	//Wire.setWireTimeout(10000,true);
	//Wire.setWireTimeout(5000,true);
	//Wire.setWireTimeout(2000,true); //too small
	Wire.setWireTimeout(3000,true); 
	wireTimeoutCount = 0;
	Wire.clearWireTimeoutFlag();
}

void setup()

{

Serial.begin(115200);

< all my other setup code removed >

//Wire.setWireTimeout(25000,true);

//Wire.setWireTimeout(10000,true);

//Wire.setWireTimeout(5000,true);

//Wire.setWireTimeout(2000,true); //too small

Wire.setWireTimeout(3000,true);

wireTimeoutCount = 0;

Wire.clearWireTimeoutFlag();

}

Although not entirely necessary, this is how I instrumented my code to capture timeout events and display them on my serial monitor

void loop()
{
	if (Wire.getWireTimeoutFlag())
	{
		wireTimeoutCount++;
		Wire.clearWireTimeoutFlag();
		mySerial.printf("Wire timeout detected; count now %d\n", wireTimeoutCount);
	}

	if (sinceLastNavUpdateMsec > NAV_UPDATE_INTERVAL_MSEC)
	{
		update_count++;
		if (update_count > NAV_UPDATE_HEADER_INSERTION_INTERVAL)
		{
			update_count = 0;
			mySerial.printf("\n%s\n", NavUpdateHeaderStr);
		}
		sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC;

		//update heading value
		UpdateIMUHdgValDeg(); //updates IMUHdgValDeg

		//update lidar1/2/3 values
		lidar_1.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
		lidar_2.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
		lidar_3.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!

		lidar1_dist = (measure1.RangeMilliMeter <= MAX_LIDAR_RANGE_MM) ? measure1.RangeMilliMeter : MAX_LIDAR_RANGE_MM;
		lidar2_dist = (measure2.RangeMilliMeter <= MAX_LIDAR_RANGE_MM) ? measure2.RangeMilliMeter : MAX_LIDAR_RANGE_MM;
		lidar3_dist = (measure3.RangeMilliMeter <= MAX_LIDAR_RANGE_MM) ? measure3.RangeMilliMeter : MAX_LIDAR_RANGE_MM;

		float elapsedMin = millis() / 60000.f;

		DateTime now = rtc.now();
		char buffer[100];
		memset(buffer, '\0', 100);
		GetShortDayDateTimeStringFromDateTime(now, buffer);
		mySerial.printf("%s\t%3.2f\t%3.2f\t%d\t%d\t%d\n",
			buffer, elapsedMin, IMUHdgValDeg,measure1.RangeMilliMeter,
			measure2.RangeMilliMeter,measure3.RangeMilliMeter);
	}

	if (sinceLastLCDUpdate > LCD_UPDATE_INTERVAL_MSEC) //used for LCD update
	{
		sinceLastLCDUpdate -= LCD_UPDATE_INTERVAL_MSEC;

		memset(lcdbuffer, '\0', NUM_LCD_COLS);

		sprintf(lcdbuffer, "%3.1f %d %d %d", IMUHdgValDeg, 
			lidar1_dist, lidar2_dist, lidar3_dist);
		lcd.setCursor(0, lcdRownum);
		//mySerial.printf("lcdClearLine =  %s\n", lcdClearLine);
		lcd.print(lcdClearLine);
		lcd.setCursor(0, lcdRownum);
		lcd.print(lcdbuffer);
		lcdRownum--; 
		lcdRownum = (lcdRownum < 1) ? 3 : lcdRownum; //valid values are 3,2,1
	}

}//loop

void loop()

{

if (Wire.getWireTimeoutFlag())

{

wireTimeoutCount++;

Wire.clearWireTimeoutFlag();

mySerial.printf("Wire timeout detected; count now %d\n", wireTimeoutCount);

}

if (sinceLastNavUpdateMsec > NAV_UPDATE_INTERVAL_MSEC)

{

update_count++;

if (update_count > NAV_UPDATE_HEADER_INSERTION_INTERVAL)

{

update_count = 0;

mySerial.printf("\n%s\n", NavUpdateHeaderStr);

}

sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC;

//update heading value

UpdateIMUHdgValDeg(); //updates IMUHdgValDeg

//update lidar1/2/3 values

lidar_1.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!

lidar_2.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!

lidar_3.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!

lidar1_dist = (measure1.RangeMilliMeter <= MAX_LIDAR_RANGE_MM) ? measure1.RangeMilliMeter : MAX_LIDAR_RANGE_MM;

lidar2_dist = (measure2.RangeMilliMeter <= MAX_LIDAR_RANGE_MM) ? measure2.RangeMilliMeter : MAX_LIDAR_RANGE_MM;

lidar3_dist = (measure3.RangeMilliMeter <= MAX_LIDAR_RANGE_MM) ? measure3.RangeMilliMeter : MAX_LIDAR_RANGE_MM;

float elapsedMin = millis() / 60000.f;

DateTime now = rtc.now();

char buffer[100];

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

GetShortDayDateTimeStringFromDateTime(now, buffer);

mySerial.printf("%s\t%3.2f\t%3.2f\t%d\t%d\t%d\n",

buffer, elapsedMin, IMUHdgValDeg,measure1.RangeMilliMeter,

measure2.RangeMilliMeter,measure3.RangeMilliMeter);

}

if (sinceLastLCDUpdate > LCD_UPDATE_INTERVAL_MSEC) //used for LCD update

{

sinceLastLCDUpdate -= LCD_UPDATE_INTERVAL_MSEC;

memset(lcdbuffer, '\0', NUM_LCD_COLS);

sprintf(lcdbuffer, "%3.1f %d %d %d", IMUHdgValDeg,

lidar1_dist, lidar2_dist, lidar3_dist);

lcd.setCursor(0, lcdRownum);

//mySerial.printf("lcdClearLine = %s\n", lcdClearLine);

lcd.print(lcdClearLine);

lcd.setCursor(0, lcdRownum);

lcd.print(lcdbuffer);

lcdRownum--;

lcdRownum = (lcdRownum < 1) ? 3 : lcdRownum; //valid values are 3,2,1

}

}//loop

All my other hardware setup code has been removed for clarity. Notice though, that I tried a number of different timeout values, starting from the default value of 25000 (25 mSec) down to 2000, and then back up to 3000. At least in my particular configuration, the 1000 value was too small – it caused a timeout flag to be generated on every pass through the loop. This was an unexpected result, as the SBWire library uses a 100 uSec (i.e. a timeout value of 100) for it’s default timeout value, and this setting has always worked fine in all my I2C projects.

In any case, here’s a short video that demonstrates that the Wire library can now recover from an I2C bus traffic interruption via the use of the new timeout feature.

Stay tuned!

Replacing HC-SRO4 Ultrasonic Sensors with VL53L0X Arrays Part II

2 Replies

Posted 16 June 2020

In my previous post on this subject, I described my efforts to replace the ultrasonic ‘ping’ distance sensors with modules built around the ST Microelectronics VL53L0X ‘Time of Flight’ LIDAR distance sensor to improve Wall_E2’s (my autonomous wall-following robot) ability to track a nearby wall at a specified standoff distance.

At the conclusion of my last post, I had determined that a three-element linear array of VL53L0X controlled by a Teensy 3.5 was effective in achieving and tracking parallel orientation to a nearby wall. This, should allow the robot to initially orient itself parallel to the target wall and then capture and maintain the desired offset distance.

This post describes follow-on efforts to verify that the Arduino Mega 2560 robot controller can acquire distance and steering information from the Teensy 3.5 sensor controller via its I2C bus. Currently the robot system communicates with four devices via I2C – a DF Robots MPU6050 6DOF IMU, an Adafruit DS3231 RTC, an Adafruit FRAM, and the Teensy 3.2 controller for the IR Homing module for autonomous charging. The idea here is to use two three-element linear arrays of VL53L0X modules controlled by a Teensy 3.5 on its secondary I2C bus, controlled by the Mega system controller on the main I2C bus. The Mega would see the Teensy 3.5 as a just a fifth slave device on its I2C bus, and the Teensy 3.5 would handle all the interactions with the VL53L0X sensors. As an added benefit, the Teensy 3.5 can calculate the steering value for tracking, as needed.

The first step in this process was to verify that the Mega could communicate with the Teensy 3.5 over the main I2C bus, while the Teensy 3.5 communicated with the sensor array(s) via its secondary I2C bus. To do this I created two programs – an Arduino Mega program to simulate the main robot controller, and a Teensy 3.5 program to act as the sensor controller (the Teensy program will eventually be the final sensor controller program).

Here’s the Mega 2560 simulator program:

/*
    Name:       VL53L0X_Master.ino
    Created:	6/15/2020 1:38:36 PM
    Author:     FRANKNEWXPS15\Frank

    Program to verify that I can communicate with the Teensy
    VL53L0X controller via I2C

*/

#include <PrintEx.h> //allows printf-style printout syntax
#include "I2C_Anything.h" //needed for sending float data over I2C
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

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


//Sensor data values
int Lidar_RightFront;
int Lidar_RightCenter;
int Lidar_RightRear;
int Lidar_LeftFront;
int Lidar_LeftCenter;
int Lidar_LeftRear;
float SteeringVal;

enum VL53L0X_REQUEST
{
	VL53L0X_CENTERS_ONLY,
	VL53L0X_RIGHT,
	VL53L0X_LEFT
};


void setup()
{
	Serial.begin(115200);
    Wire.begin(); //makes this program the I2C MASTER
	pinMode(LED_BUILTIN, OUTPUT);
	mySerial.printf("VL53L0X Master Demo Program\n");
}

void loop()
{
	digitalWrite(LED_BUILTIN, HIGH);
	GetRequestedVL53l0xValues(VL53L0X_RIGHT);
	//mySerial.printf("VL53L0X F/C/R Values: %d\t%d\t%d\n", Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);
	digitalWrite(LED_BUILTIN, LOW);
	delay(500);
}


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

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

//Step1: Send request to VL53L0X handler
	mySerial.printf("Sending %d to slave\n", which);
	Wire.beginTransmission(VL53L0X_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_SLAVE_ADDRESS, (int)(2*sizeof(Lidar_RightCenter)));
		readResult = I2C_readAnything(Lidar_RightCenter);

		if (readResult > 0)
		{
			I2C_readAnything(Lidar_LeftCenter);
		}
		mySerial.printf("VL53L0X_CENTERS_ONLY case: Got LC/RC = %d, %d\n", Lidar_LeftCenter, Lidar_RightCenter);
		break;
	case VL53L0X_RIGHT:
		//four data values needed here
		data_size = 3 * sizeof(int) + sizeof(float);
		mySerial.printf("data_size = %d\n", data_size);
		Wire.requestFrom(VL53L0X_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(SteeringVal);
		}
		mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n", 
			Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, SteeringVal);
		break;
	case VL53L0X_LEFT:
		//three data values needed here
		data_size = 3 * sizeof(int) + sizeof(float);

		Wire.requestFrom(VL53L0X_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(SteeringVal);
		}
		mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",
			Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, SteeringVal);
		break;
	default:
		break;
	}

	return readResult > 0; //this is true only if all reads succeed
}

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

Name: VL53L0X_Master.ino

Created: 6/15/2020 1:38:36 PM

Author: FRANKNEWXPS15\Frank

Program to verify that I can communicate with the Teensy

VL53L0X controller via I2C

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

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

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

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

//Sensor data values

int Lidar_RightFront;

int Lidar_RightCenter;

int Lidar_RightRear;

int Lidar_LeftFront;

int Lidar_LeftCenter;

int Lidar_LeftRear;

float SteeringVal;

enum VL53L0X_REQUEST

{

VL53L0X_CENTERS_ONLY,

VL53L0X_RIGHT,

VL53L0X_LEFT

};

void setup()

{

Serial.begin(115200);

Wire.begin(); //makes this program the I2C MASTER

pinMode(LED_BUILTIN, OUTPUT);

mySerial.printf("VL53L0X Master Demo Program\n");

}

void loop()

{

digitalWrite(LED_BUILTIN, HIGH);

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

//mySerial.printf("VL53L0X F/C/R Values: %d\t%d\t%d\n", Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear);

digitalWrite(LED_BUILTIN, LOW);

delay(500);

}

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

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

//Step1: Send request to VL53L0X handler

mySerial.printf("Sending %d to slave\n", which);

Wire.beginTransmission(VL53L0X_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_SLAVE_ADDRESS, (int)(2*sizeof(Lidar_RightCenter)));

readResult = I2C_readAnything(Lidar_RightCenter);

if (readResult > 0)

{

I2C_readAnything(Lidar_LeftCenter);

}

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

break;

case VL53L0X_RIGHT:

//four data values needed here

data_size = 3 * sizeof(int) + sizeof(float);

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

Wire.requestFrom(VL53L0X_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(SteeringVal);

}

mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",

Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear, SteeringVal);

break;

case VL53L0X_LEFT:

//three data values needed here

data_size = 3 * sizeof(int) + sizeof(float);

Wire.requestFrom(VL53L0X_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(SteeringVal);

}

mySerial.printf("VL53L0X_RIGHT case: Got L/C/R/S = %d, %d, %d, %3.2f\n",

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, SteeringVal);

break;

default:

break;

}

return readResult > 0; //this is true only if all reads succeed

}

And the Teensy 3.5 program

/*
    Name:       Teensy_VL53L0X_I2C_Slave.ino
    Created:	6/15/2020 11:45:03 AM
    Author:     FRANKNEWXPS15\Frank

    This program builds on Teensy_Triple_VL53L0X_V2.ino and adds
    an I2C SLAVE connection for transmitting sensor readings to 
    a master controller (a Mega 2560 in the case of the 4-wheel robot)

*/

#include <Wire.h> //using Wire.h here instead of i2c_t3.h for multiple I2C bus access
#include "Adafruit_VL53L0X.h"
#include "I2C_Anything.h" //local version modified to use Wire.h vs SBWire

volatile uint8_t request_type;

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)


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

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;
float SteeringVal;

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

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

    Serial.begin(115200); //Teensy ignores rate parameter

    pinMode(RF_XSHUT_PIN, OUTPUT);
    pinMode(RC_XSHUT_PIN, OUTPUT);
    pinMode(RR_XSHUT_PIN, OUTPUT);

    // wait until serial port opens for native USB devices
    while (!Serial)
    {
        delay(1);
    }

    delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

    Serial.printf("Teensy 3.5 VL53L0X Sensor Array Controller\n");

    //initialize 3 LIDAR sensors
  // 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
  //    all XSHUT high to bring out of reset
    digitalWrite(RF_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RC_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RR_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RF_XSHUT_PIN, HIGH);
    digitalWrite(RC_XSHUT_PIN, HIGH);
    digitalWrite(RR_XSHUT_PIN, HIGH);

    // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
    //    shutdown by pulling XSHUT pins low
    digitalWrite(RC_XSHUT_PIN, LOW);
    digitalWrite(RR_XSHUT_PIN, LOW);

    // 4. Initialize sensor #1 
    if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RF"));
        while (1);
    }

    // 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
    // 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
    //    you set the first sensor to
    digitalWrite(RC_XSHUT_PIN, HIGH);
    if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RC"));
        while (1);
    }

    // 7. Repeat for each sensor, turning each one on, setting a unique address.
    digitalWrite(RR_XSHUT_PIN, HIGH);
    if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RR"));
        while (1);
    }

    Serial.printf("\nInit complete\n\n");

    ////05/23/20 experiment with accessing API functions
    //VL53L0X_Dev_t RightFrontDevice = lidar_RF.GetDeviceHandle();
    //VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();
    //Serial.printf("Device Info for Right Front Sensor\n");
    //Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",
    //    RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,
    //    RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,
    //    RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

    ////get measurement timing budget?
    //prog_uint32_t MeasBudgetUSec;
    ////VL53L0X_Error ApiError = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);
    //VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);
    //Serial.printf("Right Front Measurement Budget (uSec) = %d\n", MeasBudgetUSec);
    //delay(1000);
    //Serial.printf("Changing budget from %d to %d....\n", MeasBudgetUSec, 2 * MeasBudgetUSec);
    //delay(1000);
    //MeasBudgetUSec = 2 * MeasBudgetUSec;
    ////ApiError = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);
    //VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);
    //Serial.printf("Right Front Measurement Budget (uSec) Now = %d\n \n", MeasBudgetUSec);



    //Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");
}

void loop()
{
    VL53L0X_RangingMeasurementData_t measure;

    lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RF_Dist_mm = measure.RangeMilliMeter;
        if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RF_Dist_mm = 0;

    //delay(100);

    lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RC_Dist_mm = measure.RangeMilliMeter;
        if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RC_Dist_mm = 0;

    //delay(100);

    lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RR_Dist_mm = measure.RangeMilliMeter;
        if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RR_Dist_mm = 0;

    SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

//DEBUG!!
    //Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),
    // RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);
//DEBUG!!

    delay(100);
}

// function that executes whenever data is requested by master
// this function is registered as an event, see setup()
void requestEvent()
{
    //Purpose: Send requested sensor data to the Mega controller via main I2C bus
    //Inputs: 
    //  request_type = uint8_t value denoting type of data requested
    //      0 = left & right center distances only
    //      1 = right side front, center and rear distances, plus steering value
    //      2 = left side front, center and rear distances, plus steering value
    //Outputs:
    //  Requested data sent to master

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

    int data_size;
    switch (request_type)
    {
    case 0: //VL53L0X_CENTERS_ONLY

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

        I2C_writeAnything(RC_Dist_mm); 
        break;
    case 1: //VL53L0X_RIGHT

//DEBUG!!
        data_size = 3 * sizeof(uint16_t) + sizeof(float);
        Serial.printf("Sending %d bytes F/C/R/S vals = %d, %d, %d, %3.2f to master\n",
            data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);
//DEBUG!!

        I2C_writeAnything(RF_Dist_mm);
        I2C_writeAnything(RC_Dist_mm);
        I2C_writeAnything(RR_Dist_mm);
        I2C_writeAnything(SteeringVal);
        break;
    case 2: //VL53L0X_LEFT

//DEBUG!!
        //data_size = 3 * sizeof(uint16_t) + sizeof(float);
        //Serial.printf("Sending %d bytes F/C/R/S vals = %d, %d, %d, %3.2f to master\n",
        //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);
//DEBUG!!

        //I2C_writeAnything(LF_Dist_mm);
        //I2C_writeAnything(LC_Dist_mm);
        //I2C_writeAnything(LR_Dist_mm);
        //I2C_writeAnything(SteeringVal);
        break;
    default:
        break;
    }
}

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


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

    I2C_readAnything(request_type);

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

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

Name: Teensy_VL53L0X_I2C_Slave.ino

Created: 6/15/2020 11:45:03 AM

Author: FRANKNEWXPS15\Frank

This program builds on Teensy_Triple_VL53L0X_V2.ino and adds

an I2C SLAVE connection for transmitting sensor readings to

a master controller (a Mega 2560 in the case of the 4-wheel robot)

#include <Wire.h> //using Wire.h here instead of i2c_t3.h for multiple I2C bus access

#include "Adafruit_VL53L0X.h"

#include "I2C_Anything.h" //local version modified to use Wire.h vs SBWire

volatile uint8_t request_type;

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

float SteeringVal;

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

void setup()

{

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

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

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

Serial.begin(115200); //Teensy ignores rate parameter

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

// wait until serial port opens for native USB devices

while (!Serial)

{

delay(1);

}

delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

Serial.printf("Teensy 3.5 VL53L0X Sensor Array Controller\n");

//initialize 3 LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

// 4. Initialize sensor #1

if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RF"));

while (1);

}

// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(RC_XSHUT_PIN, HIGH);

if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 7. Repeat for each sensor, turning each one on, setting a unique address.

digitalWrite(RR_XSHUT_PIN, HIGH);

if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RR"));

while (1);

}

Serial.printf("\nInit complete\n\n");

////05/23/20 experiment with accessing API functions

//VL53L0X_Dev_t RightFrontDevice = lidar_RF.GetDeviceHandle();

//VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();

//Serial.printf("Device Info for Right Front Sensor\n");

//Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",

// RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,

// RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,

// RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

////get measurement timing budget?

//prog_uint32_t MeasBudgetUSec;

////VL53L0X_Error ApiError = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);

//VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);

//Serial.printf("Right Front Measurement Budget (uSec) = %d\n", MeasBudgetUSec);

//delay(1000);

//Serial.printf("Changing budget from %d to %d....\n", MeasBudgetUSec, 2 * MeasBudgetUSec);

//delay(1000);

//MeasBudgetUSec = 2 * MeasBudgetUSec;

////ApiError = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);

//VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);

//Serial.printf("Right Front Measurement Budget (uSec) Now = %d\n \n", MeasBudgetUSec);

//Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");

}

void loop()

{

VL53L0X_RangingMeasurementData_t measure;

lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RF_Dist_mm = measure.RangeMilliMeter;

if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RF_Dist_mm = 0;

//delay(100);

lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RC_Dist_mm = measure.RangeMilliMeter;

if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RC_Dist_mm = 0;

//delay(100);

lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RR_Dist_mm = measure.RangeMilliMeter;

if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

//DEBUG!!

//Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n", millis(),

// RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

//DEBUG!!

delay(100);

}

// function that executes whenever data is requested by master

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

void requestEvent()

{

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

//Inputs:

// request_type = uint8_t value denoting type of data requested

// 0 = left & right center distances only

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

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

//Outputs:

// Requested data sent to master

//DEBUG!!

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

request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//DEBUG!!

int data_size;

switch (request_type)

{

case 0: //VL53L0X_CENTERS_ONLY

//DEBUG!!

data_size = sizeof(uint16_t);

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

//DEBUG!!

I2C_writeAnything(RC_Dist_mm);

break;

case 1: //VL53L0X_RIGHT

//DEBUG!!

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

Serial.printf("Sending %d bytes F/C/R/S vals = %d, %d, %d, %3.2f to master\n",

data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

//DEBUG!!

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(SteeringVal);

break;

case 2: //VL53L0X_LEFT

//DEBUG!!

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

//Serial.printf("Sending %d bytes F/C/R/S vals = %d, %d, %d, %3.2f to master\n",

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

//DEBUG!!

//I2C_writeAnything(LF_Dist_mm);

//I2C_writeAnything(LC_Dist_mm);

//I2C_writeAnything(LR_Dist_mm);

//I2C_writeAnything(SteeringVal);

break;

default:

break;

}

// handle Rx Event (incoming I2C data)

void receiveEvent(int numBytes)

{

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

//Inputs:

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

//Outputs:

// uint8_t request_type filled with request type value from master

//DEBUG!!

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

//DEBUG!!

I2C_readAnything(request_type);

//DEBUG!!

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

//DEBUG!!

}

Here’s the hardware layout for the first test:

Arduino Mega system controller in the foreground, followed by the Teensy 3.5 sensor array controller in the middle, followed by a single 3-element sensor array in the background

The next step was to mount everything on the robot’s second deck, and verify that the Teensy 3.5 sensor array controller can talk to the robot’s Mega controller via the robot’s I2C bus. Here’s the hardware layout:

Right-side three sensor array mounted on robot second deck. Teensy 3.5 sensor array controller shown at middle foreground.

After mounting the array and array controller on the robot’s second deck, I connected the Teensy to robot +5V, GND and the I2C bus, and loaded the VL53L0X_Master.ino sketch into the robot’s Mega system controller. With this setup I was able to demonstrate much improved parallel orientation detection. The setup is much more precise and straightforward than the previous algorithm using the ‘ping’ sensors. Instead of having to make several turns toward and away from the near wall looking for a distance inflection point, I can now determine immediately from the sign of the steering value which way to turn, and can detect the parallel condition when the steering value changes sign.

I think I may even be able to use this new ‘super-power’ to simplify the initial ‘offset capture’ phase, as well as the offset tracking phase. In earlier work I demonstrated that I can use a PID engine to drive a stepper motor to keep the sensor array parallel to a moving target ‘wall’, so I’m confident I can use the same technique to drive the robot’s motors to maintain a parallel condition with respect to a nearby wall, by driving the steering value toward zero. In addition, I should be able to set the PID ‘setpoint’ to a non-zero value and cause the robot to assume a stable oblique orientation with respect to the wall, meaning I should be able to have the robot drive at a stable oblique angle toward or away from the wall to capture the desired offset.

19 June 2020 Update:

As a preliminary step to using a PID engine to drive the ‘RotateToParallelOrientation’ maneuver, I modified the robot code to report the front, center, and rear distances from the VL53L0X array, along with the calculated steering value computed as (F-R)/C. Then I recorded the distance and steering value vs relative pointing angle for 20, 30, and 40 cm offsets from my target ‘wall’.

Here’s the physical setup for the experiment (only the setup for the 20 cm offset is shown – the others are identical except for the offset).

Experiment setup for 20 cm offset from target wall

Then I recorded the three distances and steering value for -40 to +40º relative pointing angles into an Excel workbook and created the plots shown below:

Array distances and steering value for 20 cm offset. Note steering value zero is very close to parallel orientation

Array distances and steering value for 30 cm offset. Note steering value zero is very close to parallel orientation

Array distances and steering value for 40 cm offset. Note steering value zero is very close to parallel orientation

The front, center, and rear distance and steering value plots all look very similar from 20-40 cm offset, as one would expect. Here’s a combined distance plot of all three offset values.

Combined distance plots for all three offsets. Note that the ‘flyback’ lines are an artifact of the plotting arrangement

In the above chart, it is clear that the curves for each offset are very similar, so an algorithm that works at one offset should also work for all offsets between 20 and 40 cm. The next chart shows the steering values for all three offsets on the same plot.

Steering values vs relative pointing angle for all three offset distances

As might be expected from the way in which the steering value is computed, i.e. (Front-Rear)/Center, the value range decreases significantly as the offset distance increases. At 20 cm the range is from -0.35 to +0.25, but for 40 cm it is only -0.18 to +0.15. So, whatever algorithm I come up with for achieving parallel orientation should be effective for the lowest range in order to be effective at all anticipated starting offsets.

To try out my new VL53L0X super-powers, I re-wrote the ‘RotateToParallelOrientation’ subroutine that attempts to rotate the robot so it is parallel to the nearest wall. The modification uses the PID engine to drive the calculated steering value from the VL53L0X sensor array to zero. After verifying that this works (quite well, actually!) I decided to modify it some more to see if I could also use the PID engine and the VL53L0X steering value to track the wall once parallel orientation was obtained. So I set up a two-stage process; the parallel orientation stage uses a PID with Kp = 800 (Ki = Kd = 0), and the tracking stage uses the same PID but with Kp set to half the parallel search value. This worked amazingly well – much better than anything I have been able to achieve to date.

The following short video is composed of two separate ‘takes’ the first one shows a ‘parallel orientation’ find operation with Kp = 800. The second one shows the effect of combining the ‘parallel find’ operation with Kp = 800 with a short segment of wall tracking with Kp = 400. As can be seen in the video, the tracking portion looks for all the world as if there were no corrections being made at all – it’s that smooth! I actually had to look through the telemetry data to verify that the tracking PID was actually making corrections to keep the steering value near zero. Here’s the video

and here’s the output from the two-step ‘find parallel and track’ portion of the video

PID Init(): ITerm = 0.00, lastInput = 0.00
ToFArrayPID Parameters (Kp,Ki,Kd,DIR) = (800.00,0.00,0.00,1)
ToFArrayPID output max/min = 255/-255
Millis	PID Out	Steer	leftspd	rightspd	dist
2365	0.00	0.15	127	127	363
2562	109.45	0.19	236	17	364
2762	43.98	0.10	170	83	362
2962	4.31	0.06	131	122	343
Parallel Orientation Detected
Fine Tuning ToFArrayPID Parameters (Kp,Ki,Kd,DIR) = (400.00,0.00,0.00,1)
3162	-13.02	0.02	113	140	344
3362	-13.02	0.04	113	140	342
3562	-5.96	0.04	121	132	362
3762	-2.32	0.03	124	129	349
3963	-6.25	0.03	120	133	361
4162	-8.92	0.01	118	135	361
4362	-17.78	0.01	109	144	357
4563	-3.19	0.04	123	130	369
4762	-8.08	0.03	118	135	369
4962	-13.44	0.02	113	140	366
5164	-11.16	0.02	115	138	362
5362	-8.04	0.03	118	135	368
5562	6.16	0.07	133	120	367

PID Init(): ITerm = 0.00, lastInput = 0.00

ToFArrayPID Parameters (Kp,Ki,Kd,DIR) = (800.00,0.00,0.00,1)

ToFArrayPID output max/min = 255/-255

Millis PID Out Steer leftspd rightspd dist

2365 0.00 0.15 127 127 363

2562 109.45 0.19 236 17 364

2762 43.98 0.10 170 83 362

2962 4.31 0.06 131 122 343

Parallel Orientation Detected

Fine Tuning ToFArrayPID Parameters (Kp,Ki,Kd,DIR) = (400.00,0.00,0.00,1)

3162 -13.02 0.02 113 140 344

3362 -13.02 0.04 113 140 342

3562 -5.96 0.04 121 132 362

3762 -2.32 0.03 124 129 349

3963 -6.25 0.03 120 133 361

4162 -8.92 0.01 118 135 361

4362 -17.78 0.01 109 144 357

4563 -3.19 0.04 123 130 369

4762 -8.08 0.03 118 135 369

4962 -13.44 0.02 113 140 366

5164 -11.16 0.02 115 138 362

5362 -8.04 0.03 118 135 368

5562 6.16 0.07 133 120 367

From the telemetry it can be seen that the ‘find parallel’ stage results in the robot oriented parallel to the wall at about 360 mm or so. Then in the ‘track’ stage, the ‘Steer’ value is held to within a few hundredths (0.01 to 0.07 right at the end), and the offset distance stays practically constant at 361 to 369 mm – wow!

The results of the above experiments show without a doubt that the three VL53L0X linear array is quite accurate parallel orientation and wall tracking operations – much better than anything I’ve been able to do with the ‘ping’ sensors.

The last step in this process has yet to be accomplished – showing that the setup can be used to capture a desired offset after the parallel find operation but before the wall tracking stage. Based on the work to date, I think this will be straightforward, but…..

21 June 2020 Update:

I wasn’t happy with the small range of values resulting from the above steering value computation, especially the way the value range decreased for larger offsets. So, I went back to my original data in Excel and re-plotted the data using (Front-Rear)/100 instead of (Front-Rear)/Center. This produced a much nicer set of curves, as shown in the following plot

Steering values vs relative pointing angle using (Front-Rear)/100 instead of (Front-Rear)/Center

Using the above curve, I modified the program to demonstrate basic wall offset capture, as shown in the following short video

The video demonstrates a three stage wall offset capture algorithm, with delays inserted between the stages for debugging purposes. In the first stage, a PID engine is used with a very high Kp value to rotate the robot until the steering value from the VL53L0X array is near zero, denoting the parallel condition. After a 5 second delay, the PID engine Kp value is reduced to about 1/4 the ‘parallel rotate’ value, and the PID setpoint is changed to maintain a steering value that produces an approximately 20º ‘cut’ toward the desired wall offset value. In the final stage, the robot is rotated back to parallel, and the robot is stopped. In the above demonstration, the robot started out oriented about 30-40º toward the wall, about 60-70 cm from the wall. After the initial parallel rotation, the robot is located about 50 cm from the wall. In the offset capture stage, the robot moves slowly until the center VL53L0X reports about 36 cm, and then the robot rotates to parallel again, and stops the motors. At this point the robot is oriented parallel to the wall at an offset that is approximately 28 cm – not quite the desired 30 cm, but pretty close!

The next step will be to eliminate the inter-stage pauses, and instead of stopping after the third (rotate to parallel) stage, the PID engine will be used to track the resulting offset by driving the VL53L0X array steering value to zero.

23 June 2020 Update:

After nailing down the initial parallel-find, offset capture and return-to-parallel steps, I had some difficulty getting the wall tracking portion to work well. Initially I was trying to track the wall offset using the measured distance after the return-to-parallel step, and this was blowing up pretty much regardless of the PID Kp setting. After thinking about this for a while, I realized I had fallen back into the same trap I was trying to escape by going to an array of sensors rather than just one – when the robot rotates due to a tracking correction, a single distance measurement will always cause problems.

So, I went back to what I knew really worked – using all three sensor measurements to form a ‘steering value’ as follows:

Steering Value = Front - Rear + Center

1	Steering Value = Front - Rear + Center

To complete the setup, the PID setpoint is made equal to the measured center sensor distance at the conclusion of the ‘return-to-parallel’ step. When the robot is parallel to the wall, Front = Rear = 0, so the Steering value is just the Center distance, as desired, and the PID output will drive the motors to maintain the offset setpoint. When the robot rotates, the front and rear sensors develop a difference which either adds to or subtracts from the center reading in a way that forms a reliable ‘steering value’ for the PID.

Here are a couple of short videos demonstrating all four steps; parallel-find, approach-and-capture, return-to-parallel, and wall-tracking. In the first video, the PID is set for (5,0,0) and it tracks pretty nicely. In the second video, I tried a PID set for (25,0,0) and this didn’t seem to work as well.

At this point I’m pretty satisfied with the way the 3-element VL53L0X ToF sensor array is working as a replacement for the single ultrasonic ‘ping’ sensor. The robot now has the capability to capture and track a specific offset from a nearby wall – just what I started out to do lo these many months ago.

25 June 2020 Update:

Here are a couple of short videos in my ‘outdoor’ (AKA entry hallway) range. The first video shows the response using a PID of (25,0,0) while the second one shows the same thing but with a PID value of (5,0,0).

The following Excel plot shows the steering value (in this case, just Fdist – Rdist), the corresponding PID response, and the center sensor distance measurement

I Googled around a bit and found some information on PID tuning, including this blog post. I tried the recommended heuristic method, and wound up with a PID tuning set of (10,0,1), resulting in the following Excel plot and video

Stay tuned!

Frank

Replacing HC-SRO4 Ultrasonic Sensors with VL53L0X Arrays

7 Replies

Posted 20 May 2020

In a recent post, I described the issue I had discovered with the HC-SR04 ultrasonic ‘Ping’ distance sensors – namely that they don’t provide reliable distance information for off-perpendicular orientations, making them unusable for determining when Wall-E2, my autonomous wall-following robot, is oriented parallel to the nearest wall.

In the same post, I described my discovery of the STMicroelectronics VL53L0X infrared laser ‘Time-of-Flight’ distance sensor, and the thought that this might be a replacement for the HC-SR04. After running some basic experiments with one device, I became convinced that I should be able to use an array of three sensors oriented at 30 degree angles to each other to provide an accurate relative orientation to a nearby wall.

So, this post is intended to document my attempt to integrate two 3-sensor arrays into Wall-E2, starting with just the right side. If the initial results look promising, then I’ll add them to the left side as well.

Physical layout:

The VL53L0X sensors can be found in several different packages. The one I’m trying to use is the GY530/VL53L0X module available from multiple vendors. Here’s a shot from eBay

GY530/VL53L0X test setup with Arduino. Note size relative to U.S. Dime coin

The basic idea is to mount three of the above sensors in an array oriented to cover about 60 degrees. So, I designed and printed a bracket that would fit in the same place as the HC-SR04 ‘ping’ sensor, as shown below:

TinkerCad design for triple sensor mounting bracket

And here is the finished bracket with the center sensor installed (I haven’t received the others yet)

VL53L0X module mounted at the center position on triple-sensor bracket

System Integration:

The VL53L0X chip has a default I2C address of 0x29. While this address can be changed in software to allow multiple V53L0X modules to be used, the chips don’t remember the last address used, and so must be reprogrammed at startup each time. The procedure requires the use of the chip’s XSHUT input, which means that a digital I/O line must be dedicated to each of the six modules planned for this configuration, in addition to the power, ground and I2C SCL/SDA lines. This doesn’t pose an insurmountable obstacle, as there are still plenty of unused I/O lines on the Arduino Mega 2560 controlling the robot, but since I want to mount the sensor arrays on the robot’s second deck in place of the existing HC-SR04 ‘ping’ sensors, those six additional wires will have to be added through the multiple-pin connector pair I installed some time ago. Again, not a deal-breaker, but a PITA all the same.

So, I started thinking about some alternate ideas for managing the sensor arrays. I already have a separate micro-controller (a Teensy 3.5) managing the IR sensor array used for homing to charging stations, so I started thinking I could mount a second Teensy 3.2 on the second deck, and cut down on the requirement for intra-deck wiring. Something like the following:

The Teensy would manage startup I2C address assignments for each of the six VL53L0X sensor modules via a secondary I2C bus, and would communicate distance and steering values to the Arduino Mega 2560 main controller via the system I2C bus.

22 May 2020 Update:

After some fumbling around and some quality time with the great folks on the Teensy forum, I managed to get a 3-sensor array working on a Teensy 3.5’s Wire1 I2C bus (SCL1/SDA1), using the Adafruit VL53L0X library. The code as it stands capitalizes on the little-known fact that when a Teensy 3.x is the compile target, there are three more ‘TwoWire’ objects availble – Wire1, Wire2, and Wire3. So, I was able to use the following code to instantiate and configure three VL53L0X objects:

January 18 2022 Note:

The ‘magic’ that creates the ‘Wire1’, ‘Wire2’, and ‘Wire3’ objects occurs in WireKinetis.h/cpp in the Arduino\hardware\teensy\avr\libraries\Wire folder.

/*
    Name:       Triple_VL53L0X_Demo.ino
    Created:	5/19/2020 9:56:56 PM
    Author:     FRANKNEWXPS15\Frank
*/

/* This code copied from the following post on the Adafruit forum:
https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5
*/


#include <Wire.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)


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

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;

void setup() 
{
    Serial.begin(115200); //Teensy ignores rate parameter

    pinMode(RF_XSHUT_PIN, OUTPUT);
    pinMode(RC_XSHUT_PIN, OUTPUT);
    pinMode(RR_XSHUT_PIN, OUTPUT);

    // wait until serial port opens for native USB devices
    while (!Serial) {
        delay(1);
    }

    Serial.println("Teensy Adafruit Triple VL53L0X test");

    //initialize 3 LIDAR sensors
  // 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
  //    all XSHUT high to bring out of reset
    digitalWrite(RF_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RC_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RR_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RF_XSHUT_PIN, HIGH);
    digitalWrite(RC_XSHUT_PIN, HIGH);
    digitalWrite(RR_XSHUT_PIN, HIGH);

    // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
    //    shutdown by pulling XSHUT pins low
    digitalWrite(RC_XSHUT_PIN, LOW);
    digitalWrite(RR_XSHUT_PIN, LOW);

    // 4. Initialize sensor #1 with lox.begin(new_i2c_address) Pick any number but 0x29 and it
    //    must be under 0x7F. Going with 0x30 to 0x3F is probably OK.
    if (!lidar_RF.begin(VL53L0X_I2C_ADDR+1, false, &Wire1)) 
    {
        Serial.println(F("Failed to boot lidar_RF"));
        while (1);
    }

    // 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
    // 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
    //    you set the first sensor to
    digitalWrite(RC_XSHUT_PIN, HIGH);
    if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RC"));
        while (1);
    }

    // 7. Repeat for each sensor, turning each one on, setting a unique address.
    digitalWrite(RR_XSHUT_PIN, HIGH);
    if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RR"));
        while (1);
    }

    Serial.println(F("Init complete\n\nVL53L0X API Simple Ranging example\n\n"));

    Serial.printf("Time\tFront\tCenter\tRear\n");
}

void loop() 
{
    VL53L0X_RangingMeasurementData_t measure;

    lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    { 
        RF_Dist_mm = measure.RangeMilliMeter;
        if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RF_Dist_mm = 0;

    //delay(100);

    lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RC_Dist_mm = measure.RangeMilliMeter;
        if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RC_Dist_mm = 0;

    //delay(100);

    lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RR_Dist_mm = measure.RangeMilliMeter;
        if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RR_Dist_mm = 0;

    Serial.printf("%lu\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

    delay(100);
}

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

Name: Triple_VL53L0X_Demo.ino

Created: 5/19/2020 9:56:56 PM

Author: FRANKNEWXPS15\Frank

/* This code copied from the following post on the Adafruit forum:

https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5

#include <Wire.h>

#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

void setup()

{

Serial.begin(115200); //Teensy ignores rate parameter

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

// wait until serial port opens for native USB devices

while (!Serial) {

delay(1);

}

Serial.println("Teensy Adafruit Triple VL53L0X test");

//initialize 3 LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

// 4. Initialize sensor #1 with lox.begin(new_i2c_address) Pick any number but 0x29 and it

// must be under 0x7F. Going with 0x30 to 0x3F is probably OK.

if (!lidar_RF.begin(VL53L0X_I2C_ADDR+1, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RF"));

while (1);

}

// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(RC_XSHUT_PIN, HIGH);

if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 7. Repeat for each sensor, turning each one on, setting a unique address.

digitalWrite(RR_XSHUT_PIN, HIGH);

if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RR"));

while (1);

}

Serial.println(F("Init complete\n\nVL53L0X API Simple Ranging example\n\n"));

Serial.printf("Time\tFront\tCenter\tRear\n");

}

void loop()

{

VL53L0X_RangingMeasurementData_t measure;

lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RF_Dist_mm = measure.RangeMilliMeter;

if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RF_Dist_mm = 0;

//delay(100);

lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RC_Dist_mm = measure.RangeMilliMeter;

if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RC_Dist_mm = 0;

//delay(100);

lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RR_Dist_mm = measure.RangeMilliMeter;

if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

Serial.printf("%lu\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

delay(100);

}

Here’s some sample output:

Opening port
Port open
Teensy Adafruit Triple VL53L0X test
Init complete
VL53L0X API Simple Ranging example
Time	Front	Center	Rear
29366	446	566	133
29584	453	126	123
29802	449	83	111
30020	442	59	91
30237	435	60	85
30455	431	58	85
30673	427	53	86
30890	416	56	85
31108	413	57	83
31325	406	56	83
31543	400	54	84
31761	388	54	82
31978	366	51	84
32196	367	50	81

Opening port

Port open

Teensy Adafruit Triple VL53L0X test

Init complete

VL53L0X API Simple Ranging example

Time Front Center Rear

29366 446 566 133

29584 453 126 123

29802 449 83 111

30020 442 59 91

30237 435 60 85

30455 431 58 85

30673 427 53 86

30890 416 56 85

31108 413 57 83

31325 406 56 83

31543 400 54 84

31761 388 54 82

31978 366 51 84

32196 367 50 81

And the physical setup:

Triple VL53L0X LIDAR array. The two outside sensors are angled at 30 degrees. Teensy 3.5 in background

The next big questions are whether or not this 3-sensor array can be used to determine when Wall_E2 is oriented parallel to the nearest wall, and whether or not the steering value proposed in my previous post, namely

S = (Ml – Mr)/Mc

1	S = (Ml – Mr)/Mc

where the ‘l’, ‘r’ and ‘c’ subscripts denote the left/forward, right/rearward, and center sensor measured distances respectively.

23 May Update:

I’ve been improving my understanding of my triple-VL53L0X setup, and I think I’m to the point where I can try out the steering idea. Here’s the experimental setup:

I have a wood barrier set up with a 40 cm offset from the center of the compass rose. Before getting started, I checked each sensor’s measured distance from the barrier by manually placing each sensor at the center of the compass rose, aligned as closely as possible with the perpendicular to the barrier. When I did this, the measured offsets were within a few mm of the actual distance as measured by the tape measure.

The plan is to rotate the sensor bracket from -30 degrees to +30 degrees relative to the normal perpendicular orientation, while recording the left, center, and right measured distances. Then I should be able to determine if the steering idea is going to work. In this experiment, I manually rotated the array from 0 (i.e. the center sensor aligned with the perpendicular to the barrier) to -30 degrees, and then in 10 degree steps from -30 to 30 degrees. The rotation was paused for a few seconds at each orientation. As shown in the plot below, the Steering Value looks very symmetric, and surprisingly linear – yay!

Steering values resulting from manualy rotating the triple VL53L0X array from -30 to 30 degrees

However, the above experiment exposed a significant problem with my array design. The 30 degree angular offset between the center sensor and the outer two sensors is too large; when the center sensor is oriented 30 degrees off perpendicular, the line-of-sight distance from the ‘off’ sensor to the wall is very near the range limit for the sensors, with the 40 cm nominal offset chosen for the test. I could reduce the problem by reducing the initial offset, but in the intended application the initial offset might be more than 40 cm, not less.

So, back to TinkerCad for yet another bracket rev. Isn’t having a 3D printer (or two, in my case) WONDERFUL! The new version has a 20 deg relative angle rather than 30, so when the center sensor is at 30 degrees relative to the wall, the outside one will be at 50 deg. Hopefully this will still allow good steering while not going ‘off the wall’ literally.

Revised sensor carrier with 20-deg offsets vs 30-deg

04 June 2020 Update:

In order to make accurate measurements of the triple-VL53L0X array performance, I needed a way to accurately and consistently rotate the array with respect to a nearby wall, while recording the distance measurements from each array element. A few years ago I had created a rotary table program to run a stepper motor for this purpose, but I hadn’t documented it very well, so I had to take some time away from this project to rebuild the tool I needed. The result was a nice, well-documented (this time) rotary scan table controlled by a Teensy 3.2, and a companion measurement program. The rotary scan table program can be synchronized with the measurement program via control pins, and the current step # and relative pointing angle can be retrieved from the scan table via I2C.

Here’s the test setup:

Test setup for triple VL53L0X angle sweeps. Board in background extends about .5m left and right of center.

And here’s a short video showing one scan (-20 to +20 deg)

I ran two scans – one from -30 to +30 deg, and another from -20 to +20 deg. Unfortunately, my test ‘wall’ isn’t quite long enough for the -30/+30 scan, and in addition the left-most sensor started picking up clutter from my PC in the -30 position. In any case, both scans showed a predictable ‘steering’ value transition from positive to negative at about +10 deg.

-30 to +30 deg scan. Note the steering value crosses zero at about +10 deg

-20 to +20 deg scan. Note the steering value crosses zero at about +10 deg

09 June 2020 Update:

After this first series of scans, I discovered that my scan setup was not producing consistent results, and so all the data taken to this point was suspect. So, back to the drawing board.

Based on a comment on an earlier experiment made by john kvam of ST Microlectronics, regarding the 27 degree cone coverage of the photo beam from the LIDAR unit, I thought at least part of the problem was that the beam might be picking up the ‘floor’ (actually my desk surface in these first experiments). As a way of illuminating (literally) the issue, I created a new stepper motor mount assembly with holes for mounting three laser diodes – one straight ahead, one tilted down at 14 degrees, and one tilted up at 14 degrees. The combination of all three allows me to visualize the extents of the VL53L0X beam coverage on the target surface, as shown in the following photos.

The second photo shows the situation with the ‘wall’ moved away until the beam extent indicator dots are just captured, with the distance measured to be about 220 mm. The following short video shows how the beam coverage changes as the relative angle between the center sensor and the wall changes.

As shown in the video, the beam extents are fully intercepted by the 6″ wall only during the center portion of the scan, so the off-axis results start to get a little suspect after about 50 degrees off bore-sight. However, for +/- 30 to 40 degrees, the beam extents are fully on the ‘wall’, so those measurements should be OK. However, the actual measurements have a couple of serious problems, as follows:

As shown in the above Excel plots, the ‘steering’ value, calculated as (Front-Rear)/Center crosses the zero line well to the right of center, even though the sensors themselves are clearly symmetric with the ‘wall’ at 0 degrees
The scans aren’t repeatable; when I placed a pencil mark on the ‘wall’ at the center laser dot, ran a scan, and then looked at the laser dot placement after the ‘return to start position’ movement, the laser dot was well to the left of the pencil mark. After each scan, the start position kept moving left, farther and farther from the original start position.

So, I started over with the rotary table program; After some more research on the DRV8825, I became convinced that some or all of the problem was due to micro-stepping errors – or more accurately, to the user (me) not correctly adjusting the DRV8825 current-limiting potentiometer for proper micro-stepping operation. According to Pololu’s description of DRV8825 operation, if the current limit isn’t set properly, then micro-stepping may not operate properly. To investigate this more thoroughly, I revised my rotary table program to use full steps rather than micro-steps by setting the microstepping parameter to ‘1’. Then I carefully set the scan parameters so the scan would traverse from -30 to +30 steps (not degrees). When I did this, the scan was completely repeatable, with the ‘return to starting position’ maneuver always returning to the same place, as shown in the following short video

The above experiment was conducted with the DRV8825 current limit set for about 1A (VREF = 0.5V). According to information obtained from a Pololu support post on a related subject, I came to believe this current limit should be much lower – around 240-250 mA for proper microstepping operation, so I re-adjusted the current limiting pot for VREF = 0.126V.

After making this adjustment, I redid the full-step experiment and confirmed that I hadn’t screwed anything up with the current limit change – Yay!

Then I changed the micro-stepping parameter to 2, for ‘half-step’ operation, and re-ran the above experiment with the same parameters. The ‘2’ setting for micro-stepping should enable micro-stepping operation. As shown in the following video, the scan performed flawlessly, covering the -54 to 54 degree span using 60 micro-steps per scan step instead of the 30 previously, and returning precisely to the starting position – double Yay!

Next I tried a microstepping value of 8, with the same (positive) results. Then I tried a stepping value of 32, the value I started with originally. This also worked fine.

So, at this point I’m convinced that microstepping is working fine, with the current limit set to about 240 mA as noted above. This seems to fully address the second bullet above, but as shown in the plot below taken with microstepping = 32 and with data shown from -30 to +54 degrees), I still have a problem with the ‘steering’ value not being synchronized with the actual pointing angle of the array sensor.

Next I manually acquired sensor data from -30 to +30 degrees by rotating the de-energized stepper motor shaft by hand and recording the data from all three sensors. After recording them in Excel and plotting the result, I got the following chart.

This looks pretty good, except for two potentially serious problems:

The ‘steering’ value, defined as (Front Distance – Rear Distance)/Center Distance, is again skewed to the right of center, this time about 8 degrees. Not a killer, but definitely unwanted.
Rotation beyond 30 degrees left of center is not possible without getting nonsense data from the Front (left-hand) sensor, but I can rotate 60 degrees to the right while still getting good data, and this is with much more ‘wall’ available to the left than to the right, as shown in the following photo

-30 deg relative to center

+30 deg relative to center

-60 deg relative to center

+60 deg relative to center

After finishing this, I received a reply to a question I had asked on the Pololu support forum about micro-stepping and current limiting with the Pololu DRV8825. The Pololu guy recommended that the current limit be set to the coil current rating (350 mA for the Adafruit NEMA 17 stepper motor I’m using), or 0.175V on VREF. So, I changed VREF from 0.122V to 0.175V and re-verified proper micro-stepping performance. Here’s a plot using the new setup, micro-stepping set to 32, -54 to +54 deg sweep, 18 steps of 6 deg each.

and a short video showing the sweep action.

In the video, note that at the end, the red wire compass heading pointer and the laser dots return precisely to their starting points – yay!

So, at this point everything is working nicely, except I still can’t figure out why the steering value zero doesn’t occur when the sensor array is oriented parallel to the ‘wall’. I’m hoping John, the ST Micro guy, can shed some light (pun intended) on this 😉

11 June 2020 Update.

I think I figured out why the steering value zero didn’t occur when the sensor array was oriented parallel to the wall, and it was, as is usually the case, a failure of the gray matter between my ears ;-).

The problem was the way I was collecting the data with the scanner program that runs the rotary table program. The scanner program wasn’t properly synchronizing the sensor measurements with the rotary table pointing angle. Once I corrected this software error, I got the following plot (the test setup for this plot still has the left & right sensors physically reversed).

As can be seen in the above plot, the steering value y-axis crossover now occurs very close to zero degrees, where the sensor array is oriented parallel to the wall.

12 June 2020 Update

After numerous additional steering value vs angle scans, I’m reaching the conclusion that the VL53L0X sensors have the same sort of weakness as the ‘ping’ sensors – they aren’t particularly accurate off-perpendicular. To verify this, I removed the sensors from my 3-sensor array bracket and very carefully measured the distance from each sensor position to the target ‘wall’ using a tape measure and a right-angle draftsman’s triangle, with the results shown in the following Excel plot. Measuring the off-perpendicular distances turned out to be surprisingly difficult due to the very small baseline presented by the individual sensor mounting surfaces and the width of the tape measure tape. I sure wish I had a better way to make these measurements – oh, wait – that’s what I was trying to do with the VL53L0X sensors! ;-).

With just the physical measurements, the steering value crosses the y-axis very close to zero degrees relative to parallel, plus/minus measurement error as expected. One would expect even better accuracy when using a sensor that can measure distances with milometer accuracy, but that doesn’t seem to be the case. In the following Excel plot, the above tape measure values are compared to an automatic scan using the VL53L0X sensors. As can be seen, there are significant differences between what the sensors report and the actual distance, and these errors aren’t constant with angular orientation (otherwise they could be compensated out).

For example, in the above plot the difference between the solid green (rear sensor distance) and dashed green (rear sensor mounting surface tape measure distance) is about 80 mm at -30 degrees, but only about 30 mm at +30 degrees. The difference between the measurements for the blue (front) sensor and the tape measure numbers is even more dramatic.

So it is clear that my idea of orienting the sensors at angles to each other is fundamentally flawed, as this arrangement exacerbates the relative angle between the ‘outside’ sensor orientations and the target wall when the robot isn’t oriented parallel to the wall. For instance, with the robot oriented at 30 degrees to the wall, one of the sensors will have an orientation of at least 50 degrees relative to the wall.

So my next idea is to try a three sensor array again, but this time they will all be oriented at the same angle, as shown below:

The idea here is that this will reduce the maximum relative angle between any sensor and the target wall to just the robot’s orientation.

3-sensor linear array

I attached the three VL53L0X sensors to the linear array mount and ran an automatic scan from -30 to +30 degrees, with much better results than with the angled-off array, as shown in the Excel plot below:

VL53L0X sensors attached to the linear array mount. Note the nomeclature change

Triple sensor linear array automatic scan. Left/Right curve are very symmetric

So, the linear array performance is much better than the previous ‘angled-off’ arrangement, probably because the off-perpendicular ‘look’ angle of the outside sensors is now never more than the scan angle; at -30 and +30 degrees, the look angle is still only -30 and +30 degrees. Its clear that keeping the off-perpendicular angle as low as possible provides significantly greater accuracy. As an aside, the measured perpendicular distance from the sensor surface to the target ‘wall’ was almost exactly 250 mm, and the scan values at 0 degrees were 275, 238, and 266 mm for the left, center, and right sensors respectively. so the absolute accuracy isn’t great, but I suspect most of that error can be calibrated out.

13 June 2020 Update:

So, now that I have a decently-performing VL53L0X sensor array, the next step is to verify that I can actually use it to orient the robot parallel to the nearest wall, and to track the wall at a specified offset using a PID engine.

My plan is to create a short Arduino/Teensy program that will combine the automated scan program and the motor driver program to drive the stepper motor to maintain the steering value at zero, using a PID engine. On the actual robot, this algorithm will be used to track the wall at a desired offset. For my test program, I plan to skew the ‘wall’ with respect to the stepper motor and verify that the array orientation changes to maintain a wall-parallel orientation.

14 June 2020 Update:

I got the tracking program running last night, and was able to demonstrate effective ‘parallel tracking’ with my new triple VL53L0X linear sensor array, as shown in the following video

If anyone is interested in the tracking code, here it is: It’s pretty rough and has lots of bugs, but it shows the method.

/*
    Name:       TriVL53L0XAutoOrient.ino
    Created:	6/13/2020 5:40:27 PM
    Author:     FRANKNEWXPS15\Frank

    This program is intended to verify that I can use my new triple VL53L0X
    linear array to track angular orientation changes in a nearby wall. The
    idea is to calculate a steering value from the array values and use the
    value to drive a stepper motor to zero the steering value.

    The program uses the sensor measurment code from TeensyTripleVL53L0XAngleScanV1.ino 
    and the stepper motor control code from Teensy_DRV8825_Rotary_Scan_Table.ino
*/

#include <Wire.h>
#include "Adafruit_VL53L0X.h"
#include "I2C_Anything.h"
#include <PID.h> //moved here 02/09/19 
#include "DRV8825.h"//From Teensy_DRV8825_Rotary_Scan_Table.ino

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)



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

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;

//experiment with PID control for wall tracking
double Setpoint, SteeringVal, Output;//10/06/17 chg input variable name to something more descriptive
PID myPID(&SteeringVal, &Output, &Setpoint, 200, 0.0, 0.0, REVERSE);//10/15/17 'final' setup
const int STEP_MULT_FACTOR = 10;
int motorStepsToMove = 0; //used to steer array for parallel orientation
//const int PID_OUTPUT_LIMIT = 100; //06/13/20 just a guess for now
//const int PID_OUTPUT_LIMIT = 500; //06/13/20 just a guess for now
const int PID_OUTPUT_LIMIT = 1000; //06/13/20 just a guess for now

//From Teensy_DRV8825_Rotary_Scan_Table.ino
#pragma region MOTOR_CONSTANTS
 // Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step
//06/02/20 now using Pololu DRV8825
const int MOTOR_STEPS_PER_REV = 200;
//const int MICROSTEPS_PER_STEP = 32;
const int MICROSTEPS_PER_STEP = 8; //have to reduce microstepping to get max RPM up past 20
//const int MICROSTEPS_PER_STEP = 4; //have to reduce microstepping to get max RPM up past 20
const int MICROSTEPS_PER_REV = MICROSTEPS_PER_STEP * MOTOR_STEPS_PER_REV;
const float DEG_PER_MICROSTEP = 360 / (float)MICROSTEPS_PER_REV;
//const int DEFAULT_RPM = 6;
//const int DEFAULT_RPM = 20;
const int DEFAULT_RPM = 40;
//const int DEFAULT_RPM = 60;
const int POSITIONING_SPEED_RPM = 20;
const int DEFAULT_MOTOR_STEPS_TO_MOVE = 10;
#pragma endregion Motor Constants

//const int SENSOR_MEASUREMENT_INTERVAL_MSEC = 100;
//const int SENSOR_MEASUREMENT_INTERVAL_MSEC = 10;
const int SENSOR_MEASUREMENT_INTERVAL_MSEC = 1;

#pragma region PIN_ASSIGNMENTS
const int DIR = 8;
const int STEP = 9;
const int SLEEP = 13; // optional (just delete SLEEP from everywhere if not used)

const int MODE0 = 10;
const int MODE1 = 11;
const int MODE2 = 12;
#pragma endregion Pin Assignments

DRV8825 stepper(MOTOR_STEPS_PER_REV, DIR, STEP, SLEEP, MODE0, MODE1, MODE2);
bool bGoingUp = true;
int dirSign = 1;
elapsedMillis mSecSinceLastMeasurement = 0;


void setup()
{
    //from scan program
    Wire.begin();
    Wire.setClock(100000);
    Wire.setSCL(18);
    Wire.setSDA(19);

    Serial.begin(115200); //Teensy ignores rate parameter


    // wait until serial port opens for native USB devices
    while (!Serial)
    {
        delay(1);
    }

    delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

    Serial.printf("Teensy Triple VL53L0X Angle Scan Program\n");

    pinMode(RF_XSHUT_PIN, OUTPUT);
    pinMode(RC_XSHUT_PIN, OUTPUT);
    pinMode(RR_XSHUT_PIN, OUTPUT);

    pinMode(RED_LASER_PIN, OUTPUT);


    //initialize 3 LIDAR sensors
    // 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
    //    all XSHUT high to bring out of reset
    digitalWrite(RF_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RC_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RR_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RF_XSHUT_PIN, HIGH);
    digitalWrite(RC_XSHUT_PIN, HIGH);
    digitalWrite(RR_XSHUT_PIN, HIGH);

    // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
    //    shutdown by pulling XSHUT pins low
    digitalWrite(RC_XSHUT_PIN, LOW);
    digitalWrite(RR_XSHUT_PIN, LOW);

    // 3. Initialize sensor #1 
    if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RF"));
        while (1);
    }

    // 4. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
    // 5. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
    //    you set the first sensor to
    digitalWrite(RC_XSHUT_PIN, HIGH);
    if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RC"));
        while (1);
    }

    // 6. Repeat for last sensor.
    digitalWrite(RR_XSHUT_PIN, HIGH);
    if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RR"));
        while (1);
    }

    //From Teensy_DRV8825_Rotary_Scan_Table.ino
    stepper.begin(DEFAULT_RPM);
    // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line
    // stepper.setEnableActiveState(LOW);
    stepper.enable();

    /*
     * Microstepping mode: 1, 2, 4, 8, 16 or 32 (where supported by driver)
     * Mode 1 is full speed.
     * Mode 32 is 32 microsteps per step.
     * The motor should rotate just as fast (at the set RPM),
     * but movement precision is increased, which may become visually apparent at lower RPMs.
     */
    stepper.setMicrostep(MICROSTEPS_PER_STEP);   // Set microstep mode

    //PID initialization

//Step1: Initialize PID for homing
    //set the target value
    Setpoint = 0.05; //10/15/17 this seems to be the best value for now

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

    //set the limits
    myPID.SetOutputLimits(-PID_OUTPUT_LIMIT, PID_OUTPUT_LIMIT);
    //myPID.SetSampleTime(50); //decrease sampling interval from default 100 mSec
    myPID.SetSampleTime(10); //decrease sampling interval from default 100 mSec
    //DEBUG!!
        //print out PID parameters
    Serial.printf("myPID Parameters (Kp,Ki,Kd,DIR) = (%2.2f,%2.2f,%2.2f,%d)\n",
        myPID.GetKp(), myPID.GetKi(), myPID.GetKd(), myPID.GetDirection());
    //DEBUG!!


    //turn the laser OFF
    digitalWrite(RED_LASER_PIN, LOW);

    Serial.printf("\nInit complete\n \n");

    Serial.printf("Left\tCenter\tRight\tSteer\n"); 


}

void loop()
{
    //experiment with using PID for auto-orientation
    //By default, computes new output approx 10 times/sec (use SetSampleTime() to change)
    //if (myPID.Compute())//if Compute returns TRUE, Output has new value
    //{
    //    motorStepsToMove = STEP_MULT_FACTOR * (int)Output;
    //    stepper.move(motorStepsToMove);
    //}

    if (mSecSinceLastMeasurement >= SENSOR_MEASUREMENT_INTERVAL_MSEC)
    {
        mSecSinceLastMeasurement -= SENSOR_MEASUREMENT_INTERVAL_MSEC;
        TakeAndDisplayMeasurement();
        if (myPID.Compute())//if Compute returns TRUE, Output has new value
        {
            //Serial.printf("SteeringVal = %2.3f, New output value = %2.3f\n", SteeringVal, Output);
            stepper.move((int)Output);
        }



        ////motor movement test
        //dirSign = -dirSign;
        //stepper.move(dirSign * motorStepsToMove);
        //if (bGoingUp)
        //{
        //    if (motorStepsToMove < 1000)
        //    {
        //        motorStepsToMove += 100;
        //    }
        //    else
        //    {
        //        bGoingUp = false;
        //    }
        //}
        //else
        //{
        //    if (motorStepsToMove > 0)
        //    {
        //        motorStepsToMove -= 100;
        //    }
        //    else
        //    {
        //        bGoingUp = true;
        //    }
        //}

        //Serial.printf("%lu: motorStepsToMove = %d\n", millis(), motorStepsToMove);

    }

}

void GetVL53L0XValues()
{
    //Serial.printf("In GetVL53L0XValues()\n");
    VL53L0X_RangingMeasurementData_t measure;

    //turn the laser ON
    digitalWrite(RED_LASER_PIN, HIGH);

    lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RF_Dist_mm = measure.RangeMilliMeter;
        if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RF_Dist_mm = 0;

    lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RC_Dist_mm = measure.RangeMilliMeter;
        if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RC_Dist_mm = 0;

    lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RR_Dist_mm = measure.RangeMilliMeter;
        if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RR_Dist_mm = 0;

    SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

    //turn the laser OFF
    digitalWrite(RED_LASER_PIN, LOW);

    //delay(100);
}

void TakeAndDisplayMeasurement()
{
    //take measurement
    GetVL53L0XValues();

    //05/30/20 get current step# & relative pointing angle
    //Wire.requestFrom(SLAVE_ADDR, sizeof(curStepVal) + sizeof(curAngleDeg));
    //I2C_readAnything(curStepVal);
    //I2C_readAnything(curAngleDeg);

    //Serial.printf("%lu\t%d\t%3.2f\t%d\t%d\t%d\t%3.2f\n",
    //    millis(), curStepVal, curAngleDeg,
    //    RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);
    //Serial.printf("%lu\t%d\t%d\t%d\t%3.2f\n",
    //    millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

}

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

Name: TriVL53L0XAutoOrient.ino

Created: 6/13/2020 5:40:27 PM

Author: FRANKNEWXPS15\Frank

This program is intended to verify that I can use my new triple VL53L0X

linear array to track angular orientation changes in a nearby wall. The

idea is to calculate a steering value from the array values and use the

value to drive a stepper motor to zero the steering value.

The program uses the sensor measurment code from TeensyTripleVL53L0XAngleScanV1.ino

and the stepper motor control code from Teensy_DRV8825_Rotary_Scan_Table.ino

#include <Wire.h>

#include "Adafruit_VL53L0X.h"

#include "I2C_Anything.h"

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

#include "DRV8825.h"//From Teensy_DRV8825_Rotary_Scan_Table.ino

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

//experiment with PID control for wall tracking

double Setpoint, SteeringVal, Output;//10/06/17 chg input variable name to something more descriptive

PID myPID(&SteeringVal, &Output, &Setpoint, 200, 0.0, 0.0, REVERSE);//10/15/17 'final' setup

const int STEP_MULT_FACTOR = 10;

int motorStepsToMove = 0; //used to steer array for parallel orientation

//const int PID_OUTPUT_LIMIT = 100; //06/13/20 just a guess for now

//const int PID_OUTPUT_LIMIT = 500; //06/13/20 just a guess for now

const int PID_OUTPUT_LIMIT = 1000; //06/13/20 just a guess for now

//From Teensy_DRV8825_Rotary_Scan_Table.ino

#pragma region MOTOR_CONSTANTS

// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step

//06/02/20 now using Pololu DRV8825

const int MOTOR_STEPS_PER_REV = 200;

//const int MICROSTEPS_PER_STEP = 32;

const int MICROSTEPS_PER_STEP = 8; //have to reduce microstepping to get max RPM up past 20

//const int MICROSTEPS_PER_STEP = 4; //have to reduce microstepping to get max RPM up past 20

const int MICROSTEPS_PER_REV = MICROSTEPS_PER_STEP * MOTOR_STEPS_PER_REV;

const float DEG_PER_MICROSTEP = 360 / (float)MICROSTEPS_PER_REV;

//const int DEFAULT_RPM = 6;

//const int DEFAULT_RPM = 20;

const int DEFAULT_RPM = 40;

//const int DEFAULT_RPM = 60;

const int POSITIONING_SPEED_RPM = 20;

const int DEFAULT_MOTOR_STEPS_TO_MOVE = 10;

#pragma endregion Motor Constants

//const int SENSOR_MEASUREMENT_INTERVAL_MSEC = 100;

//const int SENSOR_MEASUREMENT_INTERVAL_MSEC = 10;

const int SENSOR_MEASUREMENT_INTERVAL_MSEC = 1;

#pragma region PIN_ASSIGNMENTS

const int DIR = 8;

const int STEP = 9;

const int SLEEP = 13; // optional (just delete SLEEP from everywhere if not used)

const int MODE0 = 10;

const int MODE1 = 11;

const int MODE2 = 12;

#pragma endregion Pin Assignments

DRV8825 stepper(MOTOR_STEPS_PER_REV, DIR, STEP, SLEEP, MODE0, MODE1, MODE2);

bool bGoingUp = true;

int dirSign = 1;

elapsedMillis mSecSinceLastMeasurement = 0;

void setup()

{

//from scan program

Wire.begin();

Wire.setClock(100000);

Wire.setSCL(18);

Wire.setSDA(19);

Serial.begin(115200); //Teensy ignores rate parameter

// wait until serial port opens for native USB devices

while (!Serial)

{

delay(1);

}

delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

Serial.printf("Teensy Triple VL53L0X Angle Scan Program\n");

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(RED_LASER_PIN, OUTPUT);

//initialize 3 LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

// 3. Initialize sensor #1

if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RF"));

while (1);

}

// 4. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 5. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(RC_XSHUT_PIN, HIGH);

if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 6. Repeat for last sensor.

digitalWrite(RR_XSHUT_PIN, HIGH);

if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RR"));

while (1);

}

//From Teensy_DRV8825_Rotary_Scan_Table.ino

stepper.begin(DEFAULT_RPM);

// if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line

// stepper.setEnableActiveState(LOW);

stepper.enable();

* Microstepping mode: 1, 2, 4, 8, 16 or 32 (where supported by driver)

* Mode 1 is full speed.

* Mode 32 is 32 microsteps per step.

* The motor should rotate just as fast (at the set RPM),

* but movement precision is increased, which may become visually apparent at lower RPMs.

stepper.setMicrostep(MICROSTEPS_PER_STEP); // Set microstep mode

//PID initialization

//Step1: Initialize PID for homing

//set the target value

Setpoint = 0.05; //10/15/17 this seems to be the best value for now

//turn the PID on

myPID.SetMode(AUTOMATIC);

//set the limits

myPID.SetOutputLimits(-PID_OUTPUT_LIMIT, PID_OUTPUT_LIMIT);

//myPID.SetSampleTime(50); //decrease sampling interval from default 100 mSec

myPID.SetSampleTime(10); //decrease sampling interval from default 100 mSec

//DEBUG!!

//print out PID parameters

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

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

//DEBUG!!

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

Serial.printf("\nInit complete\n \n");

Serial.printf("Left\tCenter\tRight\tSteer\n");

}

void loop()

{

//experiment with using PID for auto-orientation

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

//if (myPID.Compute())//if Compute returns TRUE, Output has new value

//{

// motorStepsToMove = STEP_MULT_FACTOR * (int)Output;

// stepper.move(motorStepsToMove);

//}

if (mSecSinceLastMeasurement >= SENSOR_MEASUREMENT_INTERVAL_MSEC)

{

mSecSinceLastMeasurement -= SENSOR_MEASUREMENT_INTERVAL_MSEC;

TakeAndDisplayMeasurement();

if (myPID.Compute())//if Compute returns TRUE, Output has new value

{

//Serial.printf("SteeringVal = %2.3f, New output value = %2.3f\n", SteeringVal, Output);

stepper.move((int)Output);

}

////motor movement test

//dirSign = -dirSign;

//stepper.move(dirSign * motorStepsToMove);

//if (bGoingUp)

//{

// if (motorStepsToMove < 1000)

// {

// motorStepsToMove += 100;

// }

// else

// {

// bGoingUp = false;

// }

//}

//else

//{

// if (motorStepsToMove > 0)

// {

// motorStepsToMove -= 100;

// }

// else

// {

// bGoingUp = true;

// }

//}

//Serial.printf("%lu: motorStepsToMove = %d\n", millis(), motorStepsToMove);

}

void GetVL53L0XValues()

{

//Serial.printf("In GetVL53L0XValues()\n");

VL53L0X_RangingMeasurementData_t measure;

//turn the laser ON

digitalWrite(RED_LASER_PIN, HIGH);

lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RF_Dist_mm = measure.RangeMilliMeter;

if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RF_Dist_mm = 0;

lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RC_Dist_mm = measure.RangeMilliMeter;

if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RC_Dist_mm = 0;

lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RR_Dist_mm = measure.RangeMilliMeter;

if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

//delay(100);

}

void TakeAndDisplayMeasurement()

{

//take measurement

GetVL53L0XValues();

//05/30/20 get current step# & relative pointing angle

//Wire.requestFrom(SLAVE_ADDR, sizeof(curStepVal) + sizeof(curAngleDeg));

//I2C_readAnything(curStepVal);

//I2C_readAnything(curAngleDeg);

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

// millis(), curStepVal, curAngleDeg,

// RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

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

// millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

}

At this point, I’m pretty confident I can use the linear array arrangement and a PID engine to track a nearby wall at a specified distance, so the next step will be to replace the ‘ping’ ultrasonic sonar sensor on Wall-E2, my autonomous wall-following robot, with the 3-sensor array, and see if I can successfully integrate it into the ‘real world’ environment.

Stay tuned!

Frank

Teensy NEMA 17 Stepper Motor Rotary Scanner Program

1 Reply

Posted 26 May 2020,

This post describes a small Teensy program developed to drive a NEMA 17 stepper motor to perform angular scans that can be used in conjunction with another program to obtain angle-synchronized performance data.

In a post several years ago I mentioned that I had developed a small Teensy program to drive a NEMA 17 stepper motor to perform angular scans to test Wall-E2’s IR Homing detection performance. Unfortunately, I didn’t do a very good job of documenting the setup, so when I wanted to use the same capability for my new VLX53L0X ‘Time-of-Flight’ sensor project, I was unable to easily put the pieces back together again. So, I decided to create a post dedicated to the software/hardware setup and usage, so the next time I need this capability it won’t be so hard to access.

The original rotary scanner program worked OK, but there was no way to synchronize the rotary table with the measurement program. So I decided this time around I was going to add some features to make it more usable:

It should allow the measurement program to trigger the start of the scan, and trigger each subsequent rotation to the next position, in a measure-move-measure… sequence.
It should provide notification when each scan position has been reached, and when the entire scan is complete
It should allow for repeated scans without restarting the program
It should be capable of reporting each position step number and calculated angle relative to the set zero position to the measurement program via I2C.

Here’s the wiring diagram:

Wiring diagram for Rotary Table program

And the software

/*
    Name:       Teensy_NEMA17_L298N_RotaryTable.ino
    Created:	5/27/2020 11:00:40 AM
    Author:     FRANKNEWXPS15\Frank
*/

/*
Teensy NEMA 17 L298N Rotary Table Program

This program controls a NEMA 17 stepper motor using an L298N motor driver. It
Takes user input to define the boresight (zero) position and the start and stop angles
in degrees relative to the zero position.  Then it moves to the start position and waits 
for a HIGH level on SCAN_START_PIN.  After this signal is received, the motor is 
stepped through to the stop position. At the stop position, the SCAN_COMPLETE_PIN is 
driven HIGH.

The Stepper motor is connected to an L298N motor driver with one coil on Out1/2, and the
other coil on Out3/4.  It may turn out that one coil's leads have to be reversed to match
the motor rotation direction with the program direction.  

The Teensy 3.2 is connect to In1/2/3/4 via pins 8,9,10,11 although this can be changed in the
constructor below.

The NEMA 17 motor is assumed to execute 200 steps/revolution.  Change the StepsPerRevolution
constant for other values.

05/27/20:  Performs an angular scan over user-supplied range of angles. By default a complete scan
from start position to stop position is performed, followed by a return to the start position
However, if the STEP_ENABLE_PIN (held HIGH by default) is pulled LOW, the program will wait 
until it goes HIGH before performing the next step.  In this case, a scan iteration would go
as follows

	Step 1 - Scan paramaters entered, motor moves to start position, waits for LOW on SCAN_START_PIN
	Step 2 - Measurement program pulls SCAN_START_PIN LOW for 1 sec, then back HIGH 
	Step 3 - Measurement program takes a measurement & retrieves step# & angle value via I2C request
	Step 4 - Measurement program pulls SCAN_STEP_ENABLE_PIN LOW
	Step 5 - Motor moves to first/next step, and outputs HIGH on SCAN_STEP_COMPLETE_PIN
	Step 6 - Measurement program waits for HIGH on SCAN_STEP_COMPLETE_PIN
	Step 7 - Measurement program takes a measurement & retrieves step# & angle value via I2C request
	Step 8 - Repeat Steps 4-7 until scan complete
	Step 9 - SCAN_COMPLETE_PIN goes HIGH
*/

//O5/30/20 Added I2C connection to pass step# & relative angle values to measurement program

#include <Stepper.h>
#include <Wire.h>
#include "I2C_Anything.h"


// change this to fit the number of steps per revolution for your motor
const int stepsPerRevolution = 200;

// initialize the stepper library on pins 8 through 11:
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);

//08/26/17 added for scan support
const int SCAN_START_PIN = 2;
const int SCAN_STEP_ENABLE_PIN = 3;
const int SCAN_STEP_COMPLETE_PIN = 4;
const int SCAN_COMPLETE_PIN = 5;

const float MOTOR_STEPS_PER_REV = 200;
const float MOTOR_STEPS_PER_DEG = MOTOR_STEPS_PER_REV / 360;
const float DEG_PER_MOTOR_STEP = 360/MOTOR_STEPS_PER_REV; //added 05/30/20
const int DEFAULT_MOTOR_STEPS_TO_MOVE = 10;
char Instr[20];
//const int POSITIONING_SPEED_RPM = 20;  
const int POSITIONING_SPEED_RPM = 100;

int zeroPos;
int scanStartDeg;
int scanStartSteps; //scanStartDeg converted to steps
int scanStopDeg;
int scanStopSteps; //scanStopDeg converted to steps
int scanNumberofSteps; //added 05/27/20
int stepsToMove; //steps to move to initial position
int curMotorStepVal; //keeps track of current position, in steps
float curRelPointingAngleDeg; //keeps track of current relative pointing angle, in degrees
int curAngleStepVal; //keeps track of current table step iteration value
bool bStopPosDone = false;
int scanSpeedRpm = 6;

const int SLAVE_ADDR = 8; //added 05/30/20

void setup() 
{
	//added 05/30/20 to provide step# & angle values to Angle Scan program
	Wire.begin(SLAVE_ADDR);
	Wire.setSCL(18);
	Wire.setSCL(19);
	Wire.setClock(100000);
	Wire.onRequest(requestEvent);

	Serial.begin(115200);
	unsigned long now = millis();
	while (!Serial)
	{
		delay(10);
	}

	delay(5000); //05/28/20 delay AFTER Serial object instantiation is NECESSARY!

	Serial.print("Serial available after "); Serial.print(millis() - now); Serial.print(" millisec");

	pinMode(SCAN_START_PIN, INPUT_PULLUP);
	pinMode(SCAN_STEP_ENABLE_PIN, INPUT_PULLUP);
	pinMode(SCAN_STEP_COMPLETE_PIN, OUTPUT);
	pinMode(SCAN_COMPLETE_PIN, OUTPUT);

	//init SCAN_COMPLETE_PIN, SCAN_STEP_COMPLETE to LOW (not complete) state
	digitalWrite(SCAN_COMPLETE_PIN, LOW);
	digitalWrite(SCAN_STEP_COMPLETE_PIN, LOW);

	//added 05/26/20
	Serial.printf("Serial.printf: NEMA-17 Stepper Rotary Table Program\n");

	//get motor speed to be used
	scanSpeedRpm = GetIntegerParameter("Motor speed RPM", 6);
	myStepper.setSpeed(scanSpeedRpm);


	//set zero position
	Serial.println("Set stepper zero position.  Enter +steps for CW, -steps for CCW, Q to exit");
	while (!bStopPosDone)
	{
		while (Serial.available() == 0); //waits for input
		if (toascii(Serial.peek()) == 'Q' || toascii(Serial.peek()) == 'q')
		{
			bStopPosDone = true;
			curMotorStepVal = 0;
		}
		else
		{
			stepsToMove = GetIntegerParameter("Steps to move (- = CCW, + = CW)?", DEFAULT_MOTOR_STEPS_TO_MOVE);
			Serial.print("Steps to move = "); Serial.println(stepsToMove);
			myStepper.setSpeed(POSITIONING_SPEED_RPM);
			myStepper.step(stepsToMove);
		}
	}

	Serial.readString(); //clear the input buffer - could still have chars in it
	Serial.print("curMotorStepVal = "); Serial.println(curMotorStepVal);

	//set scan start/stop positions in degrees
	scanStartDeg = GetIntegerParameter("Scan Start Pos in Degrees Relative to Zero", -90);
	scanStopDeg = GetIntegerParameter("Scan Stop Pos in Degrees Relative to Zero", 90);
	scanNumberofSteps = GetIntegerParameter("Number of Steps", 18);
	curRelPointingAngleDeg = scanStartDeg; //added 05/30/20

	Serial.println("Run Parameters:");
	Serial.print("Speed: "); Serial.print(scanSpeedRpm); Serial.println(" RPM");
	Serial.print("Scan Start: "); Serial.print(scanStartDeg); Serial.println(" Deg");
	Serial.print("Scan Stop: "); Serial.print(scanStopDeg); Serial.println(" Deg");
	Serial.print("Scan Steps: "); Serial.println(scanNumberofSteps);
	Serial.println();

	//go to scan start pos
	scanStartSteps = (int)scanStartDeg * MOTOR_STEPS_PER_DEG;
	Serial.print("steps to move = "); Serial.println(scanStartSteps);
	Serial.printf("Before move, curMotorStepVal = %d, scanStartSteps = %d\n", curMotorStepVal, scanStartSteps);
	myStepper.step(scanStartSteps);
	curMotorStepVal += scanStartSteps;
	Serial.print("Done - curMotorStepVal = "); Serial.println(curMotorStepVal);
	Serial.printf("After move, curMotorStepVal = %d, scanStartSteps = %d\n", curMotorStepVal, scanStartSteps);

	int motorStepsPerScanStep = (int)(((int)scanStopDeg * MOTOR_STEPS_PER_DEG - curMotorStepVal) / (float)scanNumberofSteps); //05/27/20
	Serial.print("Motor Steps Per Scan Step = "); Serial.println(motorStepsPerScanStep);

	while (1)  //user has opportunity to exit after each scan
	{
		//wait for trigger
		Serial.printf("waiting for LOW on SCAN_START_PIN (pin %d) ...\n", SCAN_START_PIN);
		while (digitalRead(SCAN_START_PIN) == HIGH)
		{
			delay(100);
		}

		Serial.println("Scan Triggered - starting scan, setting SCAN_COMPLETE_PIN to LOW\n");
		digitalWrite(SCAN_COMPLETE_PIN, LOW);

		//for (curAngleStepVal = 0; curAngleStepVal < scanNumberofSteps; curAngleStepVal++)
		for (curAngleStepVal = 0; curAngleStepVal <= scanNumberofSteps; curAngleStepVal++)
		{
			curRelPointingAngleDeg = curAngleStepVal * motorStepsPerScanStep * DEG_PER_MOTOR_STEP; //added 05/30/20

			//when SCAN_STEP_ENABLE_PIN goes LOW, go to first/next step.
			while (digitalRead(SCAN_STEP_ENABLE_PIN))
			{

			}

			unsigned long now = millis();
			digitalWrite(SCAN_STEP_COMPLETE_PIN, LOW);
			Serial.printf("Scan Step %d Triggered: motor steps to move = %d\n", curAngleStepVal, motorStepsPerScanStep);
			myStepper.setSpeed(scanSpeedRpm);
			myStepper.step(motorStepsPerScanStep);
			curMotorStepVal += motorStepsPerScanStep;
			Serial.printf("Done in %lu Msec - curMotorStepVal = %d\n", millis() - now, curMotorStepVal);
			//delay(200);

			//output HIGH on SCAN_STEP_COMPLETE_PIN
			Serial.printf("Outputting HIGH on SCAN_STEP_COMPLETE_PIN\n");
			digitalWrite(SCAN_STEP_COMPLETE_PIN, HIGH);
			//delay(50);  //make the HIGH period visible on scope
		}

		//quit
		Serial.printf("Outputting HIGH on SCAN_COMPLETE_PIN (pin %d)\n", SCAN_COMPLETE_PIN);
		digitalWrite(SCAN_COMPLETE_PIN, HIGH);

		//return to start position
		stepsToMove = curMotorStepVal - scanStartSteps;
		Serial.println("Resetting for next scan.");
		Serial.printf("Before move, curMotorStepVal = %d, scanStartSteps = %d, steps to move = %d\n", 
			curMotorStepVal, scanStartSteps, stepsToMove);
		myStepper.setSpeed(POSITIONING_SPEED_RPM);
		myStepper.step(-stepsToMove);
		curMotorStepVal = scanStartSteps;
		Serial.print("Done - curMotorStepVal = "); Serial.println(curMotorStepVal);
		myStepper.setSpeed(scanSpeedRpm);

		Serial.printf("Enter any key to continue\n");
		while (!Serial.available())
		{

		}

		//consume bytes until Serial buffer is empty to avoid unintended triggers
		int incomingByte;
		while (Serial.available())
		{
			incomingByte = Serial.read();
			Serial.printf("Consuming 0x%x\n", incomingByte);
		}

	}
}


void loop()
{
}

int GetIntegerParameter(String prompt, int defaultval)
{
	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;
}

//05/30/20 added to report current angle step# & relative pointing angle to measurement program
void requestEvent()
{
	//send the current step# and current relative pointing angle
	Serial.printf("In requestEvent: curAngleStep = %d, curRelAngle = %2.2f\n", 
		curAngleStepVal, curAngleStepVal, curRelPointingAngleDeg, curRelPointingAngleDeg);

	I2C_writeAnything(curAngleStepVal);
	I2C_writeAnything(curRelPointingAngleDeg);
}

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

Name: Teensy_NEMA17_L298N_RotaryTable.ino

Created: 5/27/2020 11:00:40 AM

Author: FRANKNEWXPS15\Frank

Teensy NEMA 17 L298N Rotary Table Program

This program controls a NEMA 17 stepper motor using an L298N motor driver. It

Takes user input to define the boresight (zero) position and the start and stop angles

in degrees relative to the zero position. Then it moves to the start position and waits

for a HIGH level on SCAN_START_PIN. After this signal is received, the motor is

stepped through to the stop position. At the stop position, the SCAN_COMPLETE_PIN is

driven HIGH.

The Stepper motor is connected to an L298N motor driver with one coil on Out1/2, and the

other coil on Out3/4. It may turn out that one coil's leads have to be reversed to match

the motor rotation direction with the program direction.

The Teensy 3.2 is connect to In1/2/3/4 via pins 8,9,10,11 although this can be changed in the

constructor below.

The NEMA 17 motor is assumed to execute 200 steps/revolution. Change the StepsPerRevolution

constant for other values.

05/27/20: Performs an angular scan over user-supplied range of angles. By default a complete scan

from start position to stop position is performed, followed by a return to the start position

However, if the STEP_ENABLE_PIN (held HIGH by default) is pulled LOW, the program will wait

until it goes HIGH before performing the next step. In this case, a scan iteration would go

as follows

Step 1 - Scan paramaters entered, motor moves to start position, waits for LOW on SCAN_START_PIN

Step 2 - Measurement program pulls SCAN_START_PIN LOW for 1 sec, then back HIGH

Step 3 - Measurement program takes a measurement & retrieves step# & angle value via I2C request

Step 4 - Measurement program pulls SCAN_STEP_ENABLE_PIN LOW

Step 5 - Motor moves to first/next step, and outputs HIGH on SCAN_STEP_COMPLETE_PIN

Step 6 - Measurement program waits for HIGH on SCAN_STEP_COMPLETE_PIN

Step 7 - Measurement program takes a measurement & retrieves step# & angle value via I2C request

Step 8 - Repeat Steps 4-7 until scan complete

Step 9 - SCAN_COMPLETE_PIN goes HIGH

//O5/30/20 Added I2C connection to pass step# & relative angle values to measurement program

#include <Stepper.h>

#include <Wire.h>

#include "I2C_Anything.h"

// change this to fit the number of steps per revolution for your motor

const int stepsPerRevolution = 200;

// initialize the stepper library on pins 8 through 11:

Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);

//08/26/17 added for scan support

const int SCAN_START_PIN = 2;

const int SCAN_STEP_ENABLE_PIN = 3;

const int SCAN_STEP_COMPLETE_PIN = 4;

const int SCAN_COMPLETE_PIN = 5;

const float MOTOR_STEPS_PER_REV = 200;

const float MOTOR_STEPS_PER_DEG = MOTOR_STEPS_PER_REV / 360;

const float DEG_PER_MOTOR_STEP = 360/MOTOR_STEPS_PER_REV; //added 05/30/20

const int DEFAULT_MOTOR_STEPS_TO_MOVE = 10;

char Instr[20];

//const int POSITIONING_SPEED_RPM = 20;

const int POSITIONING_SPEED_RPM = 100;

int zeroPos;

int scanStartDeg;

int scanStartSteps; //scanStartDeg converted to steps

int scanStopDeg;

int scanStopSteps; //scanStopDeg converted to steps

int scanNumberofSteps; //added 05/27/20

int stepsToMove; //steps to move to initial position

int curMotorStepVal; //keeps track of current position, in steps

float curRelPointingAngleDeg; //keeps track of current relative pointing angle, in degrees

int curAngleStepVal; //keeps track of current table step iteration value

bool bStopPosDone = false;

int scanSpeedRpm = 6;

const int SLAVE_ADDR = 8; //added 05/30/20

void setup()

{

//added 05/30/20 to provide step# & angle values to Angle Scan program

Wire.begin(SLAVE_ADDR);

Wire.setSCL(18);

Wire.setSCL(19);

Wire.setClock(100000);

Wire.onRequest(requestEvent);

Serial.begin(115200);

unsigned long now = millis();

while (!Serial)

{

delay(10);

}

delay(5000); //05/28/20 delay AFTER Serial object instantiation is NECESSARY!

Serial.print("Serial available after "); Serial.print(millis() - now); Serial.print(" millisec");

pinMode(SCAN_START_PIN, INPUT_PULLUP);

pinMode(SCAN_STEP_ENABLE_PIN, INPUT_PULLUP);

pinMode(SCAN_STEP_COMPLETE_PIN, OUTPUT);

pinMode(SCAN_COMPLETE_PIN, OUTPUT);

//init SCAN_COMPLETE_PIN, SCAN_STEP_COMPLETE to LOW (not complete) state

digitalWrite(SCAN_COMPLETE_PIN, LOW);

digitalWrite(SCAN_STEP_COMPLETE_PIN, LOW);

//added 05/26/20

Serial.printf("Serial.printf: NEMA-17 Stepper Rotary Table Program\n");

//get motor speed to be used

scanSpeedRpm = GetIntegerParameter("Motor speed RPM", 6);

myStepper.setSpeed(scanSpeedRpm);

//set zero position

Serial.println("Set stepper zero position. Enter +steps for CW, -steps for CCW, Q to exit");

while (!bStopPosDone)

{

while (Serial.available() == 0); //waits for input

if (toascii(Serial.peek()) == 'Q' || toascii(Serial.peek()) == 'q')

{

bStopPosDone = true;

curMotorStepVal = 0;

}

else

{

stepsToMove = GetIntegerParameter("Steps to move (- = CCW, + = CW)?", DEFAULT_MOTOR_STEPS_TO_MOVE);

Serial.print("Steps to move = "); Serial.println(stepsToMove);

myStepper.setSpeed(POSITIONING_SPEED_RPM);

myStepper.step(stepsToMove);

}

Serial.readString(); //clear the input buffer - could still have chars in it

Serial.print("curMotorStepVal = "); Serial.println(curMotorStepVal);

//set scan start/stop positions in degrees

scanStartDeg = GetIntegerParameter("Scan Start Pos in Degrees Relative to Zero", -90);

scanStopDeg = GetIntegerParameter("Scan Stop Pos in Degrees Relative to Zero", 90);

scanNumberofSteps = GetIntegerParameter("Number of Steps", 18);

curRelPointingAngleDeg = scanStartDeg; //added 05/30/20

Serial.println("Run Parameters:");

Serial.print("Speed: "); Serial.print(scanSpeedRpm); Serial.println(" RPM");

Serial.print("Scan Start: "); Serial.print(scanStartDeg); Serial.println(" Deg");

Serial.print("Scan Stop: "); Serial.print(scanStopDeg); Serial.println(" Deg");

Serial.print("Scan Steps: "); Serial.println(scanNumberofSteps);

Serial.println();

//go to scan start pos

scanStartSteps = (int)scanStartDeg * MOTOR_STEPS_PER_DEG;

Serial.print("steps to move = "); Serial.println(scanStartSteps);

Serial.printf("Before move, curMotorStepVal = %d, scanStartSteps = %d\n", curMotorStepVal, scanStartSteps);

myStepper.step(scanStartSteps);

curMotorStepVal += scanStartSteps;

Serial.print("Done - curMotorStepVal = "); Serial.println(curMotorStepVal);

Serial.printf("After move, curMotorStepVal = %d, scanStartSteps = %d\n", curMotorStepVal, scanStartSteps);

int motorStepsPerScanStep = (int)(((int)scanStopDeg * MOTOR_STEPS_PER_DEG - curMotorStepVal) / (float)scanNumberofSteps); //05/27/20

Serial.print("Motor Steps Per Scan Step = "); Serial.println(motorStepsPerScanStep);

while (1) //user has opportunity to exit after each scan

{

//wait for trigger

Serial.printf("waiting for LOW on SCAN_START_PIN (pin %d) ...\n", SCAN_START_PIN);

while (digitalRead(SCAN_START_PIN) == HIGH)

{

delay(100);

}

Serial.println("Scan Triggered - starting scan, setting SCAN_COMPLETE_PIN to LOW\n");

digitalWrite(SCAN_COMPLETE_PIN, LOW);

//for (curAngleStepVal = 0; curAngleStepVal < scanNumberofSteps; curAngleStepVal++)

for (curAngleStepVal = 0; curAngleStepVal <= scanNumberofSteps; curAngleStepVal++)

{

curRelPointingAngleDeg = curAngleStepVal * motorStepsPerScanStep * DEG_PER_MOTOR_STEP; //added 05/30/20

//when SCAN_STEP_ENABLE_PIN goes LOW, go to first/next step.

while (digitalRead(SCAN_STEP_ENABLE_PIN))

{

}

unsigned long now = millis();

digitalWrite(SCAN_STEP_COMPLETE_PIN, LOW);

Serial.printf("Scan Step %d Triggered: motor steps to move = %d\n", curAngleStepVal, motorStepsPerScanStep);

myStepper.setSpeed(scanSpeedRpm);

myStepper.step(motorStepsPerScanStep);

curMotorStepVal += motorStepsPerScanStep;

Serial.printf("Done in %lu Msec - curMotorStepVal = %d\n", millis() - now, curMotorStepVal);

//delay(200);

//output HIGH on SCAN_STEP_COMPLETE_PIN

Serial.printf("Outputting HIGH on SCAN_STEP_COMPLETE_PIN\n");

digitalWrite(SCAN_STEP_COMPLETE_PIN, HIGH);

//delay(50); //make the HIGH period visible on scope

}

//quit

Serial.printf("Outputting HIGH on SCAN_COMPLETE_PIN (pin %d)\n", SCAN_COMPLETE_PIN);

digitalWrite(SCAN_COMPLETE_PIN, HIGH);

//return to start position

stepsToMove = curMotorStepVal - scanStartSteps;

Serial.println("Resetting for next scan.");

Serial.printf("Before move, curMotorStepVal = %d, scanStartSteps = %d, steps to move = %d\n",

curMotorStepVal, scanStartSteps, stepsToMove);

myStepper.setSpeed(POSITIONING_SPEED_RPM);

myStepper.step(-stepsToMove);

curMotorStepVal = scanStartSteps;

Serial.print("Done - curMotorStepVal = "); Serial.println(curMotorStepVal);

myStepper.setSpeed(scanSpeedRpm);

Serial.printf("Enter any key to continue\n");

while (!Serial.available())

{

}

//consume bytes until Serial buffer is empty to avoid unintended triggers

int incomingByte;

while (Serial.available())

{

incomingByte = Serial.read();

Serial.printf("Consuming 0x%x\n", incomingByte);

}

void loop()

{

}

int GetIntegerParameter(String prompt, int defaultval)

{

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;

}

//05/30/20 added to report current angle step# & relative pointing angle to measurement program

void requestEvent()

{

//send the current step# and current relative pointing angle

Serial.printf("In requestEvent: curAngleStep = %d, curRelAngle = %2.2f\n",

curAngleStepVal, curAngleStepVal, curRelPointingAngleDeg, curRelPointingAngleDeg);

I2C_writeAnything(curAngleStepVal);

I2C_writeAnything(curRelPointingAngleDeg);

}

And the hardware layout:

I couldn’t find any information about winding polarity for this particular NEMA 17 stepper, so it was sort of a crapshoot as to which way the motor was going to turn for a nominal CW or CCW input to the program. As it turned out, I had to reverse one set of wires on the L298N to get the stepper to turn in the correct direction.

Companion Measurement System

For the companion measurement system, I used a Teensy 3.5 micro-controller, as I needed to connect to the VL53L0X ‘Time of Flight’ sensors and the Rotary Table program/micro-controller via I2C, and The Teensy 3.5 has provisions for up to three separate I2C busses (Wire, Wire1 & Wire2). Although it would be possible to put everything on one I2C bus, I wanted to isolate the two ‘sides’ (the rotary table control side, and the sensor control side). This dual-IC2 bus arrangement is also the way I plan to integrate the VL53L0X sensor arrays into Wall-E2, my autonomous wall-following robot, so this will give me a chance to work out the bugs on a smaller scale.

Here’s the wiring diagram:

Wiring diagram for the companion measurement system, with triple VL53L0X ToF sensors

And the software:

/*
    Name:       TeensyTripleVL53L0XAngleScanV1.ino
    Created:	5/28/2020 12:15:50 PM
    Author:     FRANKNEWXPS15\Frank
*/


/*
This program is derived from Teensy_Triple_VL53L0X_V2.ino.  It uses the 
Teensy_NEMA17_L298N_RotaryTable program to perform triple VL53L0X
measurements over a range of angles. 

Teensy_NEMA17_L298N_RotaryTable program:
Performs an angular scan over user-supplied range of angles.  By default a complete scan
from start position to stop position is performed, followed by a return to the start position
However, if the STEP_ENABLE_PIN (held HIGH by default) is pulled LOW, the program will wait
until it goes HIGH before performing the next step.  In this case, a scan iteration would go
as follows

    Step 1 - Scan paramaters entered, motor moves to start position, waits for LOW on SCAN_START_PIN
    Step 2 - Measurement program pulls SCAN_START_PIN LOW for 1 sec, then back HIGH
    Step 3 - Measurement program takes a measurement & retrieves step# & angle value via I2C request
    Step 4 - Measurement program pulls SCAN_STEP_ENABLE_PIN LOW
    Step 5 - Motor moves to first/next step, and outputs HIGH on SCAN_STEP_COMPLETE_PIN
    Step 6 - Measurement program waits for HIGH on SCAN_STEP_COMPLETE_PIN
    Step 7 - Measurement program takes a measurement & retrieves step# & angle value via I2C request
    Step 8 - Repeat Steps 4-7 until scan complete
    Step 9 - SCAN_COMPLETE_PIN goes HIGH

At present (05/28/20) the rotary table pin assignments are:
SCAN_START_PIN = 2
SCAN_STEP_ENABLE_PIN = 3
SCAN_STEP_COMPLETE_PIN = 4
SCAN_COMPLETE_PIN = 5
*/


/* This code copied from the following post on the Adafruit forum:
https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5
*/


#include <Wire.h>
#include "Adafruit_VL53L0X.h"
#include "I2C_Anything.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)


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

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;
float SteeringVal; //mvd here 05/30/20

//Rotary Scan Table Connections:
const int SCAN_START_PIN = 2; //connect to table SCAN_START_PIN
const int SCAN_STEP_ENABLE_PIN = 3;//connect to table SCAN_STEP_ENABLE_PIN
const int SCAN_STEP_COMPLETE_PIN = 4;//connect to table SCAN_STEP_COMPLETE_PIN
const int SCAN_COMPLETE_PIN = 5;//connect to table SCAN_COMPLETE_PIN

 //added 05/30/20 to retrieve values from rotary table program
const int SLAVE_ADDR = 8;
int curStepVal = 0;
float curAngleDeg = 0;


void setup()
{

    //added 05/30/20 to retrieve step# & angle values from rotary table program
    Wire.begin();
    Wire.setClock(100000);
    Wire.setSCL(18);
    Wire.setSDA(19);

    Serial.begin(115200); //Teensy ignores rate parameter


    // wait until serial port opens for native USB devices
    while (!Serial)
    {
        delay(1);
    }

    delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

    Serial.printf("Teensy Triple VL53L0X Angle Scan Program\n");

    pinMode(RF_XSHUT_PIN, OUTPUT);
    pinMode(RC_XSHUT_PIN, OUTPUT);
    pinMode(RR_XSHUT_PIN, OUTPUT);

    pinMode(RED_LASER_PIN, OUTPUT);

    //Rotary Scan Table Connections:
    pinMode(SCAN_START_PIN, OUTPUT);
    pinMode(SCAN_STEP_ENABLE_PIN, OUTPUT);
    pinMode(SCAN_STEP_COMPLETE_PIN, INPUT_PULLDOWN);
    pinMode(SCAN_COMPLETE_PIN, INPUT_PULLUP);

    //Initialize table connection states
    digitalWrite(SCAN_START_PIN, HIGH); //disables scans
    digitalWrite(SCAN_STEP_ENABLE_PIN, HIGH); //disables steps  

    //initialize 3 LIDAR sensors
    // 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
    //    all XSHUT high to bring out of reset
    digitalWrite(RF_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RC_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RR_XSHUT_PIN, LOW);
    delay(10);
    digitalWrite(RF_XSHUT_PIN, HIGH);
    digitalWrite(RC_XSHUT_PIN, HIGH);
    digitalWrite(RR_XSHUT_PIN, HIGH);

    // 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
    //    shutdown by pulling XSHUT pins low
    digitalWrite(RC_XSHUT_PIN, LOW);
    digitalWrite(RR_XSHUT_PIN, LOW);

    // 3. Initialize sensor #1 
    if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RF"));
        while (1);
    }

    // 4. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
    // 5. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
    //    you set the first sensor to
    digitalWrite(RC_XSHUT_PIN, HIGH);
    if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RC"));
        while (1);
    }

    // 6. Repeat for last sensor.
    digitalWrite(RR_XSHUT_PIN, HIGH);
    if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
    {
        Serial.println(F("Failed to boot lidar_RR"));
        while (1);
    }

    //turn the laser OFF
    digitalWrite(RED_LASER_PIN, LOW);

    Serial.printf("\nInit complete\n \n");

    Serial.printf("\nWhen the Rotary Table is set up and ready to go, send any key\n \n");


    while (!Serial.available())
    {
        Serial.printf("Nothing yet...\n");
        delay(1000);
    }

    //consume bytes until Serial buffer is empty to avoid unintended triggers
    int incomingByte;
    while (Serial.available())
    {
        incomingByte = Serial.read();
        Serial.printf("Consuming 0x%x\n",incomingByte);
    }

    ////05/23/20 experiment with accessing API functions
    //VL53L0X_Dev_t RightFrontDevice = lidar_RF.GetDeviceHandle();
    //VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();
    //Serial.printf("Device Info for Right Front Sensor\n");
    //Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",
    //    RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,
    //    RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,
    //    RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

    ////get measurement timing budget?
    //prog_uint32_t MeasBudgetUSec;
    //VL53L0X_Error ApiError = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);
    //Serial.printf("Right Front Measurement Budget (uSec) = %d\n", MeasBudgetUSec);
    //delay(1000);
    //Serial.printf("Changing budget from %d to %d....\n", MeasBudgetUSec, 2 * MeasBudgetUSec);
    //delay(1000);
    //MeasBudgetUSec = 2 * MeasBudgetUSec;
    //ApiError = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);
    //Serial.printf("Right Front Measurement Budget (uSec) Now = %d\n \n", MeasBudgetUSec);

    bool bDone = false; 
    while (!bDone)
    {
        //init controls to 'disabled' state
        digitalWrite(SCAN_STEP_ENABLE_PIN, HIGH); //init to 'steps disabled'
        digitalWrite(SCAN_START_PIN, HIGH); //init to 'scan disabled'


        Serial.printf("Starting Angle Scan...\n \n");
        Serial.printf("Msec\tStep#\tRelDeg\tFront\tCenter\tRear\tSteer\n"); //rev 5/30/20 to include step# & rel ptng angle

        //Tell the table to start the scan
        digitalWrite(SCAN_START_PIN, LOW); //trigger the scan
        delay(1000); //long enough for table to see it
        digitalWrite(SCAN_START_PIN, HIGH); //reset the trigger


        //do until the scan is complete
        while (!digitalRead(SCAN_COMPLETE_PIN))
        {
            //take measurement
            GetVL53L0XValues();

            //05/30/20 get current step# & relative pointing angle
            Wire.requestFrom(SLAVE_ADDR, sizeof(curStepVal) + sizeof(curAngleDeg));
            I2C_readAnything(curStepVal);
            I2C_readAnything(curAngleDeg);

            Serial.printf("%lu\t%d\t%3.2f\t%d\t%d\t%d\t%3.2f\n", 
                millis(),curStepVal,curAngleDeg,
                RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

            digitalWrite(SCAN_STEP_ENABLE_PIN, LOW); //trigger table to step to next position
            delay(100); //give rotary table time to reset SCAN_STEP_COMPLETE_PIN state

            //wait for step to complete
            while (!digitalRead(SCAN_STEP_COMPLETE_PIN))
            {
                //Serial.printf("Waiting for SCAN_STEP_COMPLETE_PIN to go HIGH\n");
                //delay(1000);

            }

            //disable steps
            digitalWrite(SCAN_STEP_ENABLE_PIN, HIGH); //disable further steps
            delay(50); //05/31/20 give SCAN_COMPLETE_PIN a chance to go HIGH before next measurement
        }

        Serial.printf("SCAN_COMPLETE_PIN HIGH Received - Scan is Complete!\n");

        Serial.printf("Enter Q to quit, anything else for another scan\n");
        while (!Serial.available())
        {
            Serial.printf("nothing yet....\n");
            delay(1000);
        }

        incomingByte = Serial.read();
        Serial.printf("I received: 0x%0x\n",incomingByte);

        if (incomingByte == 0x51 || incomingByte == 0x71)
        {
            Serial.printf("Stopping Program - Goodbye!\n");
            while (true)
            {

            }
        }
        else
        {
            //consume any remaining bytes in buffer
            while (Serial.available() > 0)
            {
                Serial.read();
            }
            Serial.printf("Starting another scan...\n");
        }
    }
}

void loop()
{
}

void GetVL53L0XValues()
{
    //Serial.printf("In GetVL53L0XValues()\n");
    VL53L0X_RangingMeasurementData_t measure;

    //turn the laser ON
    digitalWrite(RED_LASER_PIN, HIGH);

    lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RF_Dist_mm = measure.RangeMilliMeter;
        if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RF_Dist_mm = 0;

    lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RC_Dist_mm = measure.RangeMilliMeter;
        if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RC_Dist_mm = 0;

    lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
    if (measure.RangeStatus != 4)  // phase failures have incorrect data
    {
        RR_Dist_mm = measure.RangeMilliMeter;
        if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
    }
    else RR_Dist_mm = 0;

    SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

    //turn the laser OFF
    digitalWrite(RED_LASER_PIN, LOW);

    //delay(100);
}

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

Name: TeensyTripleVL53L0XAngleScanV1.ino

Created: 5/28/2020 12:15:50 PM

Author: FRANKNEWXPS15\Frank

This program is derived from Teensy_Triple_VL53L0X_V2.ino. It uses the

Teensy_NEMA17_L298N_RotaryTable program to perform triple VL53L0X

measurements over a range of angles.

Teensy_NEMA17_L298N_RotaryTable program:

Performs an angular scan over user-supplied range of angles. By default a complete scan

from start position to stop position is performed, followed by a return to the start position

However, if the STEP_ENABLE_PIN (held HIGH by default) is pulled LOW, the program will wait

until it goes HIGH before performing the next step. In this case, a scan iteration would go

as follows

Step 1 - Scan paramaters entered, motor moves to start position, waits for LOW on SCAN_START_PIN

Step 2 - Measurement program pulls SCAN_START_PIN LOW for 1 sec, then back HIGH

Step 3 - Measurement program takes a measurement & retrieves step# & angle value via I2C request

Step 4 - Measurement program pulls SCAN_STEP_ENABLE_PIN LOW

Step 5 - Motor moves to first/next step, and outputs HIGH on SCAN_STEP_COMPLETE_PIN

Step 6 - Measurement program waits for HIGH on SCAN_STEP_COMPLETE_PIN

Step 7 - Measurement program takes a measurement & retrieves step# & angle value via I2C request

Step 8 - Repeat Steps 4-7 until scan complete

Step 9 - SCAN_COMPLETE_PIN goes HIGH

At present (05/28/20) the rotary table pin assignments are:

SCAN_START_PIN = 2

SCAN_STEP_ENABLE_PIN = 3

SCAN_STEP_COMPLETE_PIN = 4

SCAN_COMPLETE_PIN = 5

/* This code copied from the following post on the Adafruit forum:

https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693

And adapted to use it on the Wire1 I2C bus on a Teensy 3.5

#include <Wire.h>

#include "Adafruit_VL53L0X.h"

#include "I2C_Anything.h"

Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)

Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Center LIDAR

Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)

//XSHUT required for I2C address init

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

const int RED_LASER_PIN = 0;

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

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

float SteeringVal; //mvd here 05/30/20

//Rotary Scan Table Connections:

const int SCAN_START_PIN = 2; //connect to table SCAN_START_PIN

const int SCAN_STEP_ENABLE_PIN = 3;//connect to table SCAN_STEP_ENABLE_PIN

const int SCAN_STEP_COMPLETE_PIN = 4;//connect to table SCAN_STEP_COMPLETE_PIN

const int SCAN_COMPLETE_PIN = 5;//connect to table SCAN_COMPLETE_PIN

//added 05/30/20 to retrieve values from rotary table program

const int SLAVE_ADDR = 8;

int curStepVal = 0;

float curAngleDeg = 0;

void setup()

{

//added 05/30/20 to retrieve step# & angle values from rotary table program

Wire.begin();

Wire.setClock(100000);

Wire.setSCL(18);

Wire.setSDA(19);

Serial.begin(115200); //Teensy ignores rate parameter

// wait until serial port opens for native USB devices

while (!Serial)

{

delay(1);

}

delay(5000); //05/28/20 - this delay is necessary AFTER Serial instantiation!

Serial.printf("Teensy Triple VL53L0X Angle Scan Program\n");

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

pinMode(RED_LASER_PIN, OUTPUT);

//Rotary Scan Table Connections:

pinMode(SCAN_START_PIN, OUTPUT);

pinMode(SCAN_STEP_ENABLE_PIN, OUTPUT);

pinMode(SCAN_STEP_COMPLETE_PIN, INPUT_PULLDOWN);

pinMode(SCAN_COMPLETE_PIN, INPUT_PULLUP);

//Initialize table connection states

digitalWrite(SCAN_START_PIN, HIGH); //disables scans

digitalWrite(SCAN_STEP_ENABLE_PIN, HIGH); //disables steps

//initialize 3 LIDAR sensors

// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set

// all XSHUT high to bring out of reset

digitalWrite(RF_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RC_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RR_XSHUT_PIN, LOW);

delay(10);

digitalWrite(RF_XSHUT_PIN, HIGH);

digitalWrite(RC_XSHUT_PIN, HIGH);

digitalWrite(RR_XSHUT_PIN, HIGH);

// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into

// shutdown by pulling XSHUT pins low

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

// 3. Initialize sensor #1

if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RF"));

while (1);

}

// 4. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.

// 5. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever

// you set the first sensor to

digitalWrite(RC_XSHUT_PIN, HIGH);

if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RC"));

while (1);

}

// 6. Repeat for last sensor.

digitalWrite(RR_XSHUT_PIN, HIGH);

if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))

{

Serial.println(F("Failed to boot lidar_RR"));

while (1);

}

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

Serial.printf("\nInit complete\n \n");

Serial.printf("\nWhen the Rotary Table is set up and ready to go, send any key\n \n");

while (!Serial.available())

{

Serial.printf("Nothing yet...\n");

delay(1000);

}

//consume bytes until Serial buffer is empty to avoid unintended triggers

int incomingByte;

while (Serial.available())

{

incomingByte = Serial.read();

Serial.printf("Consuming 0x%x\n",incomingByte);

}

////05/23/20 experiment with accessing API functions

//VL53L0X_Dev_t RightFrontDevice = lidar_RF.GetDeviceHandle();

//VL53L0X_DeviceInfo_t RightFrontDeviceInfo = lidar_RF.GetDeviceInfo();

//Serial.printf("Device Info for Right Front Sensor\n");

//Serial.printf("\tName = %s\n\tType = %s\n\tProdID = %s\n\tType Number = %d\n\tMajor Rev = %d\n\tMinor Rev = %d\n \n",

// RightFrontDeviceInfo.Name, RightFrontDeviceInfo.Type,

// RightFrontDeviceInfo.ProductId, RightFrontDeviceInfo.ProductType,

// RightFrontDeviceInfo.ProductRevisionMajor, RightFrontDeviceInfo.ProductRevisionMinor);

////get measurement timing budget?

//prog_uint32_t MeasBudgetUSec;

//VL53L0X_Error ApiError = VL53L0X_GetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, &MeasBudgetUSec);

//Serial.printf("Right Front Measurement Budget (uSec) = %d\n", MeasBudgetUSec);

//delay(1000);

//Serial.printf("Changing budget from %d to %d....\n", MeasBudgetUSec, 2 * MeasBudgetUSec);

//delay(1000);

//MeasBudgetUSec = 2 * MeasBudgetUSec;

//ApiError = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&RightFrontDevice, MeasBudgetUSec);

//Serial.printf("Right Front Measurement Budget (uSec) Now = %d\n \n", MeasBudgetUSec);

bool bDone = false;

while (!bDone)

{

//init controls to 'disabled' state

digitalWrite(SCAN_STEP_ENABLE_PIN, HIGH); //init to 'steps disabled'

digitalWrite(SCAN_START_PIN, HIGH); //init to 'scan disabled'

Serial.printf("Starting Angle Scan...\n \n");

Serial.printf("Msec\tStep#\tRelDeg\tFront\tCenter\tRear\tSteer\n"); //rev 5/30/20 to include step# & rel ptng angle

//Tell the table to start the scan

digitalWrite(SCAN_START_PIN, LOW); //trigger the scan

delay(1000); //long enough for table to see it

digitalWrite(SCAN_START_PIN, HIGH); //reset the trigger

//do until the scan is complete

while (!digitalRead(SCAN_COMPLETE_PIN))

{

//take measurement

GetVL53L0XValues();

//05/30/20 get current step# & relative pointing angle

Wire.requestFrom(SLAVE_ADDR, sizeof(curStepVal) + sizeof(curAngleDeg));

I2C_readAnything(curStepVal);

I2C_readAnything(curAngleDeg);

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

millis(),curStepVal,curAngleDeg,

RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, SteeringVal);

digitalWrite(SCAN_STEP_ENABLE_PIN, LOW); //trigger table to step to next position

delay(100); //give rotary table time to reset SCAN_STEP_COMPLETE_PIN state

//wait for step to complete

while (!digitalRead(SCAN_STEP_COMPLETE_PIN))

{

//Serial.printf("Waiting for SCAN_STEP_COMPLETE_PIN to go HIGH\n");

//delay(1000);

}

//disable steps

digitalWrite(SCAN_STEP_ENABLE_PIN, HIGH); //disable further steps

delay(50); //05/31/20 give SCAN_COMPLETE_PIN a chance to go HIGH before next measurement

}

Serial.printf("SCAN_COMPLETE_PIN HIGH Received - Scan is Complete!\n");

Serial.printf("Enter Q to quit, anything else for another scan\n");

while (!Serial.available())

{

Serial.printf("nothing yet....\n");

delay(1000);

}

incomingByte = Serial.read();

Serial.printf("I received: 0x%0x\n",incomingByte);

if (incomingByte == 0x51 || incomingByte == 0x71)

{

Serial.printf("Stopping Program - Goodbye!\n");

while (true)

{

}

else

{

//consume any remaining bytes in buffer

while (Serial.available() > 0)

{

Serial.read();

}

Serial.printf("Starting another scan...\n");

}

void loop()

{

}

void GetVL53L0XValues()

{

//Serial.printf("In GetVL53L0XValues()\n");

VL53L0X_RangingMeasurementData_t measure;

//turn the laser ON

digitalWrite(RED_LASER_PIN, HIGH);

lidar_RF.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RF_Dist_mm = measure.RangeMilliMeter;

if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RF_Dist_mm = 0;

lidar_RC.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RC_Dist_mm = measure.RangeMilliMeter;

if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RC_Dist_mm = 0;

lidar_RR.rangingTest(&measure, false); // pass in 'true' to get debug data printout!

if (measure.RangeStatus != 4) // phase failures have incorrect data

{

RR_Dist_mm = measure.RangeMilliMeter;

if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;

}

else RR_Dist_mm = 0;

SteeringVal = (RF_Dist_mm - RR_Dist_mm) / (float)RC_Dist_mm;

//turn the laser OFF

digitalWrite(RED_LASER_PIN, LOW);

//delay(100);

}

and the hardware setup:

Rotary table Teensy 3.2 in foreground with L298N motor controller. Teensy 3.5 measurement controller in middle, with plugboard , NEMA 17 stepper motor and triple VL53L0X array in background

Here’s a short video showing a typical measurement scan:

And here’s a typical output:

Starting Angle Scan...
 
Msec	Step#	RelDeg	Front	Center	Rear	Steer
94128	0	0.00	361	416	507	-0.35
94498	1	9.00	372	418	507	-0.32
94868	2	18.00	372	413	510	-0.33
95237	3	27.00	368	418	506	-0.33
95607	4	36.00	368	420	510	-0.34
95976	5	45.00	376	420	508	-0.31
96346	6	54.00	365	418	502	-0.33
SCAN_COMPLETE_PIN HIGH Received - Scan is Complete!
Enter Q to quit, anything else for another scan

Starting Angle Scan...

Msec Step# RelDeg Front Center Rear Steer

94128 0 0.00 361 416 507 -0.35

94498 1 9.00 372 418 507 -0.32

94868 2 18.00 372 413 510 -0.33

95237 3 27.00 368 418 506 -0.33

95607 4 36.00 368 420 510 -0.34

95976 5 45.00 376 420 508 -0.31

96346 6 54.00 365 418 502 -0.33

SCAN_COMPLETE_PIN HIGH Received - Scan is Complete!

Enter Q to quit, anything else for another scan

In the above output, the ‘Step#’ and ‘RelDeg’ values were obtained from the rotary scanner program via the primary I2C bus, while the ‘Front’, ‘Center’, and ‘Rear’ distance values were obtained from the three VL53L0X ToF sensors via the secondary I2C bus. The ‘Steer’ value was calculated locally by the measurement controller.

Upgrade to Micro-step capable motor driver:

After getting everything to work properly, I was a bit puzzled why I wasn’t getting the correct relative angle values back from the rotary table subsystem. After a while I figured out that the reason was that the rotary table program calculates an integer number of steps based on the total angle change divided by the number of scan steps, and, in general, the result won’t be exact. When moving from one scan step to the next the motor moves an integer number of steps, which in general will not be the desired angle change. For a 60 degree scan arc with 6 steps, the desired angle/scan step is 10 deg, and at 1.8 deg/step this would result in 10/1.8 = 5.5555… motor steps/scan step. This value gets truncated to 5 motor steps/scan step, which results in each scan step being 1.8 deg/step * 5 steps = 9 deg/scan step. So, instead of the scan steps occurring at 0, 10, 20, 30, 40, 50, 60 deg, they are at 0, 9, 18, 27, 36, 45, and 54 deg, and the last 6 deg of scan is never covered.

The answer to this problem is to use a motor with more than 200 steps/rev, or to use a driver that can generate micro-steps, effectively increasing the angular resolution of the scan. After some Googling, and some rooting around in my parts box, I came up with the Pololu DRV8825 Stepper Motor Driver part. This driver supports up to 32 micro-steps/step and is considerably smaller than the L298N – such a deal!

Pololu Micro-stepping Driver

At 32 microsteps/step, a full motor rev would take 200*32 = 6400 microsteps or 0.05625 deg/microstep . Now the above calculation would result in 10 deg/0.05625/10 = 177.777 –> 177 microsteps per scan step. This would result in step angles of 0, 177*0.05625 = 9.95802, 19.916, 29.874, 39.83, 49.79, and 59.748. So now we lose only the last 0.252 deg of scan – much nicer!

So, I put together a quick test, using an Arduino Uno, a spare NEMA 17 stepper motor, a Pololu DRV8825 from my parts box, and the Pololu Stepper library. Here’s the program:

/*
    Name:       PololuMicroStepperDemoV2.ino
    Created:	5/31/2020 11:05:30 PM
    Author:     FRANKNEWXPS15\Frank
*/

/*
 * Microstepping demo
 *
 * This requires that microstep control pins be connected in addition to STEP,DIR
 *
 * Copyright (C)2015 Laurentiu Badea
 *
 * This file may be redistributed under the terms of the MIT license.
 * A copy of this license has been included with this distribution in the file LICENSE.
 */
#include <Arduino.h>

 // Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step
#define MOTOR_STEPS 200
//#define RPM 120
#define RPM 12

#define DIR 8
#define STEP 9
#define SLEEP 13 // optional (just delete SLEEP from everywhere if not used)

/*
 * Choose one of the sections below that match your board
 */

//#include "DRV8834.h"
//#define M0 10
//#define M1 11
//DRV8834 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1);

// #include "A4988.h"
// #define MS1 10
// #define MS2 11
// #define MS3 12
// A4988 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MS1, MS2, MS3);

 #include "DRV8825.h"
 #define MODE0 10
 #define MODE1 11
 #define MODE2 12
 DRV8825 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MODE0, MODE1, MODE2);

// #include "DRV8880.h"
// #define M0 10
// #define M1 11
// #define TRQ0 6
// #define TRQ1 7
// DRV8880 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1, TRQ0, TRQ1);

// #include "BasicStepperDriver.h" // generic
// BasicStepperDriver stepper(DIR, STEP);

void setup() {
    /*
     * Set target motor RPM.
     */
    stepper.begin(RPM);
    // if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line
    // stepper.setEnableActiveState(LOW);
    stepper.enable();

    // set current level (for DRV8880 only). 
    // Valid percent values are 25, 50, 75 or 100.
    // stepper.setCurrent(100);
}

void loop() {
    delay(1000);

    /*
     * Moving motor in full step mode is simple:
     */
    stepper.setMicrostep(1);  // Set microstep mode to 1:1

    // One complete revolution is 360°
    stepper.rotate(360);     // forward revolution
    stepper.rotate(-360);    // reverse revolution

    // One complete revolution is also MOTOR_STEPS steps in full step mode
    stepper.move(MOTOR_STEPS);    // forward revolution
    stepper.move(-MOTOR_STEPS);   // reverse revolution

    /*
     * Microstepping mode: 1, 2, 4, 8, 16 or 32 (where supported by driver)
     * Mode 1 is full speed.
     * Mode 32 is 32 microsteps per step.
     * The motor should rotate just as fast (at the set RPM),
     * but movement precision is increased, which may become visually apparent at lower RPMs.
     */
    stepper.setMicrostep(8);   // Set microstep mode to 1:8

    // In 1:8 microstepping mode, one revolution takes 8 times as many microsteps
    stepper.move(8 * MOTOR_STEPS);    // forward revolution
    stepper.move(-8 * MOTOR_STEPS);   // reverse revolution

    // One complete revolution is still 360° regardless of microstepping mode
    // rotate() is easier to use than move() when no need to land on precise microstep position
    stepper.rotate(360);
    stepper.rotate(-360);

    delay(5000);
}

100

101

102

103

104

105

106

107

108

Name: PololuMicroStepperDemoV2.ino

Created: 5/31/2020 11:05:30 PM

Author: FRANKNEWXPS15\Frank

* Microstepping demo

* This requires that microstep control pins be connected in addition to STEP,DIR

* This file may be redistributed under the terms of the MIT license.

* A copy of this license has been included with this distribution in the file LICENSE.

#include <Arduino.h>

// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step

#define MOTOR_STEPS 200

//#define RPM 120

#define RPM 12

#define DIR 8

#define STEP 9

#define SLEEP 13 // optional (just delete SLEEP from everywhere if not used)

* Choose one of the sections below that match your board

//#include "DRV8834.h"

//#define M0 10

//#define M1 11

//DRV8834 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1);

// #include "A4988.h"

// #define MS1 10

// #define MS2 11

// #define MS3 12

// A4988 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MS1, MS2, MS3);

#include "DRV8825.h"

#define MODE0 10

#define MODE1 11

#define MODE2 12

DRV8825 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, MODE0, MODE1, MODE2);

// #include "DRV8880.h"

// #define M0 10

// #define M1 11

// #define TRQ0 6

// #define TRQ1 7

// DRV8880 stepper(MOTOR_STEPS, DIR, STEP, SLEEP, M0, M1, TRQ0, TRQ1);

// #include "BasicStepperDriver.h" // generic

// BasicStepperDriver stepper(DIR, STEP);

void setup() {

* Set target motor RPM.

stepper.begin(RPM);

// if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line

// stepper.setEnableActiveState(LOW);

stepper.enable();

// set current level (for DRV8880 only).

// Valid percent values are 25, 50, 75 or 100.

// stepper.setCurrent(100);

}

void loop() {

delay(1000);

* Moving motor in full step mode is simple:

stepper.setMicrostep(1); // Set microstep mode to 1:1

// One complete revolution is 360°

stepper.rotate(360); // forward revolution

stepper.rotate(-360); // reverse revolution

// One complete revolution is also MOTOR_STEPS steps in full step mode

stepper.move(MOTOR_STEPS); // forward revolution

stepper.move(-MOTOR_STEPS); // reverse revolution

* Microstepping mode: 1, 2, 4, 8, 16 or 32 (where supported by driver)

* Mode 1 is full speed.

* Mode 32 is 32 microsteps per step.

* The motor should rotate just as fast (at the set RPM),

* but movement precision is increased, which may become visually apparent at lower RPMs.

stepper.setMicrostep(8); // Set microstep mode to 1:8

// In 1:8 microstepping mode, one revolution takes 8 times as many microsteps

stepper.move(8 * MOTOR_STEPS); // forward revolution

stepper.move(-8 * MOTOR_STEPS); // reverse revolution

// One complete revolution is still 360° regardless of microstepping mode

// rotate() is easier to use than move() when no need to land on precise microstep position

stepper.rotate(360);

stepper.rotate(-360);

delay(5000);

}

And the hardware test setup:

Pololu DRV8825 Microstep demo setup.

And a short video showing the stepper motor action. Note that close attention to the end of the red pointer wire shows the difference between ‘full-step’ and ‘micro-stepped’ behavior; the micro-stepped rotations are much smoother. Also, if you look very carefully, you can see the compass rose platform ‘counter-rotating’ in full-step mode, as the torque is high enough to rotate the stepper motor in the opposite direction to the shaft.

So now the idea is to incorporate micro-stepping capability into the Teensy NEMA 17 Rotary Table program, so that angle scans can be performed much more accurately than before.

02 June 2020 Update:

My original Pololu Microstep Demo program used an Arduino Uno to control the Pololu DRV8825 motor driver, but my Teensy Rotary Table setup obviously uses a Teensy and an L298N. To change the Rotary Table setup to utilize the DRV8825, I’ll need to make some adjustments to pin configurations.

The first step in the process was to change out the Arduino Uno for a Teensy (a Teensy 3.5 in this case) to verify that there were no problem with the Pololu microstep demo program running on a Teensy – done.

The next step was to change out the L298N driver on the rotary table setup with the Pololu DRV8825 driver, and modify the rotary table program to use the new driver with microstepping.

Here’s the new combined hardware layout, with both the rotary table and measurement sub-systems shown

Both measurement and rotary table sub-systems shown. Rotary table Teensy 3.2 at left foreground, followed by Teensy 3.5 measurement controller, the connection plugboard, and the triple VL53L0X sensors in the left background. The new DRV8825 is mounted on the center plugboard, and the NEMA 17 stepper motor with compass rose test platter at the right

Here’s the updated software using the Pololu DRV8825

/*
	Name:       Teensy_DRV8825_Rotary_Scan_Table.ino
	Created:	6/4/2020 12:59:57 AM
	Author:     FRANKNEWXPS15\Frank
*/

/*
Teensy DRV8825 Rotary Table Program

This program controls a NEMA 17 stepper motor using a Pololu DRV8825 motor driver. It
Takes user input to define the boresight (zero) position and the start and stop angles
in degrees relative to the zero position.  Then it moves to the start position and waits
for a HIGH level on SCAN_START_PIN.  After this signal is received, the motor is
stepped through to the stop position. At the stop position, the SCAN_COMPLETE_PIN is
driven HIGH.

The Stepper motor is connected to a DRV8825 motor driver with one coil on A1/A2, and the
other coil on B1/B2.  It may turn out that one coil's leads have to be reversed to match
the motor rotation direction with the program direction.

The NEMA 17 motor is assumed to execute 200 steps/revolution.  Change the StepsPerRevolution
constant for other values.

05/27/20:  Performs an angular scan over user-supplied range of angles. By default a complete scan
from start position to stop position is performed, followed by a return to the start position
However, if the STEP_ENABLE_PIN (held HIGH by default) is pulled LOW, the program will wait
until it goes HIGH before performing the next step.  In this case, a scan iteration would go
as follows

	Step 1 - Scan parameters entered, motor moves to start position, waits for LOW on SCAN_START_PIN
	Step 2 - Measurement program pulls SCAN_START_PIN LOW for 1 sec, then back HIGH
	Step 3 - Measurement program takes a measurement & retrieves step# & angle value via I2C request
	Step 4 - Measurement program pulls SCAN_STEP_ENABLE_PIN LOW
	Step 5 - Motor moves to first/next step, and outputs HIGH on SCAN_STEP_COMPLETE_PIN
	Step 6 - Measurement program waits for HIGH on SCAN_STEP_COMPLETE_PIN
	Step 7 - Measurement program takes a measurement & retrieves step# & angle value via I2C request
	Step 8 - Repeat Steps 4-7 until scan complete
	Step 9 - SCAN_COMPLETE_PIN goes HIGH
*/

//O5/30/20 Added I2C connection to pass step# & relative angle values to measurement program
//05/02/20 Rev to use Pololu DRV8825 & micro-stepping

#include <Wire.h>
#include "I2C_Anything.h"
#include "DRV8825.h"

const int SLAVE_ADDR = 8; //added 05/30/20

#pragma region MOTOR_CONSTANTS
 // Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step
//06/02/20 now using Pololu DRV8825
const int MOTOR_STEPS_PER_REV = 200;
const int MICROSTEPS_PER_STEP = 32;
const int MICROSTEPS_PER_REV = MICROSTEPS_PER_STEP * MOTOR_STEPS_PER_REV;
const int DEG_PER_MICROSTEP = 360 / MICROSTEPS_PER_REV;
const int MICROSTEPS_PER_DEG = MICROSTEPS_PER_REV / 360;
const int DEFAULT_RPM = 12;
const int POSITIONING_SPEED_RPM = 100;
const int DEFAULT_MOTOR_STEPS_TO_MOVE = 10;
#pragma endregion Motor Constants

#pragma region PIN_ASSIGNMENTS
const int DIR = 8;
const int STEP = 9;
const int SLEEP = 13; // optional (just delete SLEEP from everywhere if not used)

const int MODE0 = 10;
const int MODE1 = 11;
const int MODE2 = 12;

const int SCAN_START_PIN = 2;
const int SCAN_STEP_ENABLE_PIN = 3;
const int SCAN_STEP_COMPLETE_PIN = 4;
const int SCAN_COMPLETE_PIN = 5;
#pragma endregion Pin Assignments

DRV8825 stepper(MOTOR_STEPS_PER_REV, DIR, STEP, SLEEP, MODE0, MODE1, MODE2);

char Instr[20];

//DRV8825
int zeroPos;
int scanStartDeg;
int scanStartMicroSteps; //scanStartDeg converted to steps
int scanStopDeg;
int scanStopSteps; //scanStopDeg converted to steps
int scanNumberofSteps; //added 05/27/20
int stepsToMove; //steps to move to initial position
int curMotorStepVal; //keeps track of current position, in steps
float curRelPointingAngleDeg; //keeps track of current relative pointing angle, in degrees
int curAngleStepVal; //keeps track of current table step iteration value
bool bStopPosDone = false;
int scanSpeedRpm = 6;



void setup()
{
	//added 05/30/20 to provide step# & angle values to Angle Scan program
	Wire.begin(SLAVE_ADDR);
	Wire.setSCL(18);
	Wire.setSCL(19);
	Wire.setClock(100000);
	Wire.onRequest(requestEvent);

	//06/02/20 now using Pololu DRV8825 microstepping motor driver
	stepper.begin(DEFAULT_RPM);
	// if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line
	// stepper.setEnableActiveState(LOW);
	stepper.enable();

	/*
	 * Microstepping mode: 1, 2, 4, 8, 16 or 32 (where supported by driver)
	 * Mode 1 is full speed.
	 * Mode 32 is 32 microsteps per step.
	 * The motor should rotate just as fast (at the set RPM),
	 * but movement precision is increased, which may become visually apparent at lower RPMs.
	 */
	stepper.setMicrostep(MICROSTEPS_PER_STEP);   // Set microstep mode to 1:32



	Serial.begin(115200);
	unsigned long now = millis();
	while (!Serial)
	{
		delay(10);
	}

	delay(5000); //05/28/20 delay AFTER Serial object instantiation is NECESSARY!

	Serial.print("Serial available after "); Serial.print(millis() - now); Serial.print(" millisec");

	pinMode(SCAN_START_PIN, INPUT_PULLUP);
	pinMode(SCAN_STEP_ENABLE_PIN, INPUT_PULLUP);
	pinMode(SCAN_STEP_COMPLETE_PIN, OUTPUT);
	pinMode(SCAN_COMPLETE_PIN, OUTPUT);

	//init SCAN_COMPLETE_PIN, SCAN_STEP_COMPLETE to LOW (not complete) state
	digitalWrite(SCAN_COMPLETE_PIN, LOW);
	digitalWrite(SCAN_STEP_COMPLETE_PIN, LOW);

	//added 05/26/20
	Serial.printf("Serial.printf: NEMA-17 Stepper Rotary Table Program\n");

	//get motor speed to be used
	scanSpeedRpm = GetIntegerParameter("Motor speed RPM", 6);


	//set zero position
	Serial.println("Set stepper zero position.  Enter +steps for CW, -steps for CCW, Q to exit");
	while (!bStopPosDone)
	{
		while (Serial.available() == 0); //waits for input
		if (toascii(Serial.peek()) == 'Q' || toascii(Serial.peek()) == 'q')
		{
			bStopPosDone = true;
			curMotorStepVal = 0;
		}
		else
		{
			stepsToMove = GetIntegerParameter("Steps to move (- = CCW, + = CW)?", DEFAULT_MOTOR_STEPS_TO_MOVE);
			Serial.print("Steps to move = "); Serial.println(stepsToMove);
			stepper.setRPM(POSITIONING_SPEED_RPM);
			stepper.move(stepsToMove);
		}
	}

	Serial.readString(); //clear the input buffer - could still have chars in it
	Serial.print("curMotorStepVal = "); Serial.println(curMotorStepVal);

	//set scan start/stop positions in degrees
	scanStartDeg = GetIntegerParameter("Scan Start Pos in Degrees Relative to Zero", -90);
	scanStopDeg = GetIntegerParameter("Scan Stop Pos in Degrees Relative to Zero", 90);
	scanNumberofSteps = GetIntegerParameter("Number of Steps", 18);
	curRelPointingAngleDeg = scanStartDeg; //added 05/30/20

	Serial.println("Run Parameters:");
	Serial.print("Speed: "); Serial.print(scanSpeedRpm); Serial.println(" RPM");
	Serial.print("Scan Start: "); Serial.print(scanStartDeg); Serial.println(" Deg");
	Serial.print("Scan Stop: "); Serial.print(scanStopDeg); Serial.println(" Deg");
	Serial.print("Scan Steps: "); Serial.println(scanNumberofSteps);

	//06/02/20 Now using Pololu DRV8825 driver & microstepping
	int scandeg = abs(scanStopDeg - scanStartDeg);
	double scanstepdeg = scandeg / scanNumberofSteps;

	//go to scan start pos
	stepper.setRPM(POSITIONING_SPEED_RPM);
	stepper.rotate(scanStartDeg);

	while (1)  //user has opportunity to exit after each scan
	{
		//wait for trigger
		Serial.printf("waiting for LOW on SCAN_START_PIN (pin %d) ...\n", SCAN_START_PIN);
		while (digitalRead(SCAN_START_PIN) == HIGH)
		{
			delay(100);
		}

		Serial.println("Scan Triggered - starting scan, setting SCAN_COMPLETE_PIN to LOW\n");
		digitalWrite(SCAN_COMPLETE_PIN, LOW);

		stepper.setRPM(scanSpeedRpm);

		for (curAngleStepVal = 1; curAngleStepVal <= scanNumberofSteps; curAngleStepVal++)
		{
			curRelPointingAngleDeg = scanStartDeg + (curAngleStepVal)*scanstepdeg;

			//when SCAN_STEP_ENABLE_PIN goes LOW, go to first/next step.
			while (digitalRead(SCAN_STEP_ENABLE_PIN))
			{

			}

			unsigned long now = millis();
			digitalWrite(SCAN_STEP_COMPLETE_PIN, LOW);
			Serial.printf("Scan Step %d Triggered: moving to %3.2f deg\n", curAngleStepVal, curRelPointingAngleDeg);

			//DRV8825
			stepper.rotate(scanstepdeg); //rotate to next scan angle

			//output HIGH on SCAN_STEP_COMPLETE_PIN
//DEBUG!!
			//Serial.printf("Outputting HIGH on SCAN_STEP_COMPLETE_PIN\n");
			//Serial.printf("Done in %lu Msec - Current Angle = %3.2f\n", millis() - now, curRelPointingAngleDeg);
//DEBUG!!

			digitalWrite(SCAN_STEP_COMPLETE_PIN, HIGH);
			//delay(50);  //make the HIGH period visible on scope
		}

		//quit
		Serial.printf("Outputting HIGH on SCAN_COMPLETE_PIN (pin %d)\n", SCAN_COMPLETE_PIN);

		digitalWrite(SCAN_COMPLETE_PIN, HIGH);

		//return to start position
		//DRV8825
		stepper.setRPM(POSITIONING_SPEED_RPM);
		stepper.rotate(-scandeg);

		//re-initialize step counter and current pointing angle
		curAngleStepVal = 0;
		curRelPointingAngleDeg = scanStartDeg;

		Serial.printf("Enter any key to continue\n");
		while (!Serial.available())
		{

		}

		//consume bytes until Serial buffer is empty to avoid unintended triggers
		int incomingByte;
		while (Serial.available())
		{
			incomingByte = Serial.read();
			Serial.printf("Consuming 0x%x\n", incomingByte);
		}

	}
}


void loop()
{
}

int GetIntegerParameter(String prompt, int defaultval)
{
	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;
}

//05/30/20 added to report current angle step# & relative pointing angle to measurement program
void requestEvent()
{
	//send the current step# and current relative pointing angle
//DEBUG!!
	//Serial.printf("In requestEvent: curAngleStep = %d, curRelAngle = %2.2f\n", 
	//	curAngleStepVal, curAngleStepVal, curRelPointingAngleDeg, curRelPointingAngleDeg);
//DEBUG!!

	I2C_writeAnything(curAngleStepVal);
	I2C_writeAnything(curRelPointingAngleDeg);
}

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

Name: Teensy_DRV8825_Rotary_Scan_Table.ino

Created: 6/4/2020 12:59:57 AM

Author: FRANKNEWXPS15\Frank

Teensy DRV8825 Rotary Table Program

This program controls a NEMA 17 stepper motor using a Pololu DRV8825 motor driver. It

Takes user input to define the boresight (zero) position and the start and stop angles

in degrees relative to the zero position. Then it moves to the start position and waits

for a HIGH level on SCAN_START_PIN. After this signal is received, the motor is

stepped through to the stop position. At the stop position, the SCAN_COMPLETE_PIN is

driven HIGH.

The Stepper motor is connected to a DRV8825 motor driver with one coil on A1/A2, and the

other coil on B1/B2. It may turn out that one coil's leads have to be reversed to match

the motor rotation direction with the program direction.

The NEMA 17 motor is assumed to execute 200 steps/revolution. Change the StepsPerRevolution

constant for other values.

05/27/20: Performs an angular scan over user-supplied range of angles. By default a complete scan

from start position to stop position is performed, followed by a return to the start position

However, if the STEP_ENABLE_PIN (held HIGH by default) is pulled LOW, the program will wait

until it goes HIGH before performing the next step. In this case, a scan iteration would go

as follows

Step 1 - Scan parameters entered, motor moves to start position, waits for LOW on SCAN_START_PIN

Step 2 - Measurement program pulls SCAN_START_PIN LOW for 1 sec, then back HIGH

Step 3 - Measurement program takes a measurement & retrieves step# & angle value via I2C request

Step 4 - Measurement program pulls SCAN_STEP_ENABLE_PIN LOW

Step 5 - Motor moves to first/next step, and outputs HIGH on SCAN_STEP_COMPLETE_PIN

Step 6 - Measurement program waits for HIGH on SCAN_STEP_COMPLETE_PIN

Step 7 - Measurement program takes a measurement & retrieves step# & angle value via I2C request

Step 8 - Repeat Steps 4-7 until scan complete

Step 9 - SCAN_COMPLETE_PIN goes HIGH

//O5/30/20 Added I2C connection to pass step# & relative angle values to measurement program

//05/02/20 Rev to use Pololu DRV8825 & micro-stepping

#include <Wire.h>

#include "I2C_Anything.h"

#include "DRV8825.h"

const int SLAVE_ADDR = 8; //added 05/30/20

#pragma region MOTOR_CONSTANTS

// Motor steps per revolution. Most steppers are 200 steps or 1.8 degrees/step

//06/02/20 now using Pololu DRV8825

const int MOTOR_STEPS_PER_REV = 200;

const int MICROSTEPS_PER_STEP = 32;

const int MICROSTEPS_PER_REV = MICROSTEPS_PER_STEP * MOTOR_STEPS_PER_REV;

const int DEG_PER_MICROSTEP = 360 / MICROSTEPS_PER_REV;

const int MICROSTEPS_PER_DEG = MICROSTEPS_PER_REV / 360;

const int DEFAULT_RPM = 12;

const int POSITIONING_SPEED_RPM = 100;

const int DEFAULT_MOTOR_STEPS_TO_MOVE = 10;

#pragma endregion Motor Constants

#pragma region PIN_ASSIGNMENTS

const int DIR = 8;

const int STEP = 9;

const int SLEEP = 13; // optional (just delete SLEEP from everywhere if not used)

const int MODE0 = 10;

const int MODE1 = 11;

const int MODE2 = 12;

const int SCAN_START_PIN = 2;

const int SCAN_STEP_ENABLE_PIN = 3;

const int SCAN_STEP_COMPLETE_PIN = 4;

const int SCAN_COMPLETE_PIN = 5;

#pragma endregion Pin Assignments

DRV8825 stepper(MOTOR_STEPS_PER_REV, DIR, STEP, SLEEP, MODE0, MODE1, MODE2);

char Instr[20];

//DRV8825

int zeroPos;

int scanStartDeg;

int scanStartMicroSteps; //scanStartDeg converted to steps

int scanStopDeg;

int scanStopSteps; //scanStopDeg converted to steps

int scanNumberofSteps; //added 05/27/20

int stepsToMove; //steps to move to initial position

int curMotorStepVal; //keeps track of current position, in steps

float curRelPointingAngleDeg; //keeps track of current relative pointing angle, in degrees

int curAngleStepVal; //keeps track of current table step iteration value

bool bStopPosDone = false;

int scanSpeedRpm = 6;

void setup()

{

//added 05/30/20 to provide step# & angle values to Angle Scan program

Wire.begin(SLAVE_ADDR);

Wire.setSCL(18);

Wire.setSCL(19);

Wire.setClock(100000);

Wire.onRequest(requestEvent);

//06/02/20 now using Pololu DRV8825 microstepping motor driver

stepper.begin(DEFAULT_RPM);

// if using enable/disable on ENABLE pin (active LOW) instead of SLEEP uncomment next line

// stepper.setEnableActiveState(LOW);

stepper.enable();

* Microstepping mode: 1, 2, 4, 8, 16 or 32 (where supported by driver)

* Mode 1 is full speed.

* Mode 32 is 32 microsteps per step.

* The motor should rotate just as fast (at the set RPM),

* but movement precision is increased, which may become visually apparent at lower RPMs.

stepper.setMicrostep(MICROSTEPS_PER_STEP); // Set microstep mode to 1:32

Serial.begin(115200);

unsigned long now = millis();

while (!Serial)

{

delay(10);

}

delay(5000); //05/28/20 delay AFTER Serial object instantiation is NECESSARY!

Serial.print("Serial available after "); Serial.print(millis() - now); Serial.print(" millisec");

pinMode(SCAN_START_PIN, INPUT_PULLUP);

pinMode(SCAN_STEP_ENABLE_PIN, INPUT_PULLUP);

pinMode(SCAN_STEP_COMPLETE_PIN, OUTPUT);

pinMode(SCAN_COMPLETE_PIN, OUTPUT);

//init SCAN_COMPLETE_PIN, SCAN_STEP_COMPLETE to LOW (not complete) state

digitalWrite(SCAN_COMPLETE_PIN, LOW);

digitalWrite(SCAN_STEP_COMPLETE_PIN, LOW);

//added 05/26/20

Serial.printf("Serial.printf: NEMA-17 Stepper Rotary Table Program\n");

//get motor speed to be used

scanSpeedRpm = GetIntegerParameter("Motor speed RPM", 6);

//set zero position

Serial.println("Set stepper zero position. Enter +steps for CW, -steps for CCW, Q to exit");

while (!bStopPosDone)

{

while (Serial.available() == 0); //waits for input

if (toascii(Serial.peek()) == 'Q' || toascii(Serial.peek()) == 'q')

{

bStopPosDone = true;

curMotorStepVal = 0;

}

else

{

stepsToMove = GetIntegerParameter("Steps to move (- = CCW, + = CW)?", DEFAULT_MOTOR_STEPS_TO_MOVE);

Serial.print("Steps to move = "); Serial.println(stepsToMove);

stepper.setRPM(POSITIONING_SPEED_RPM);

stepper.move(stepsToMove);

}

Serial.readString(); //clear the input buffer - could still have chars in it

Serial.print("curMotorStepVal = "); Serial.println(curMotorStepVal);

//set scan start/stop positions in degrees

scanStartDeg = GetIntegerParameter("Scan Start Pos in Degrees Relative to Zero", -90);

scanStopDeg = GetIntegerParameter("Scan Stop Pos in Degrees Relative to Zero", 90);

scanNumberofSteps = GetIntegerParameter("Number of Steps", 18);

curRelPointingAngleDeg = scanStartDeg; //added 05/30/20

Serial.println("Run Parameters:");

Serial.print("Speed: "); Serial.print(scanSpeedRpm); Serial.println(" RPM");

Serial.print("Scan Start: "); Serial.print(scanStartDeg); Serial.println(" Deg");

Serial.print("Scan Stop: "); Serial.print(scanStopDeg); Serial.println(" Deg");

Serial.print("Scan Steps: "); Serial.println(scanNumberofSteps);

//06/02/20 Now using Pololu DRV8825 driver & microstepping

int scandeg = abs(scanStopDeg - scanStartDeg);

double scanstepdeg = scandeg / scanNumberofSteps;

//go to scan start pos

stepper.setRPM(POSITIONING_SPEED_RPM);

stepper.rotate(scanStartDeg);

while (1) //user has opportunity to exit after each scan

{

//wait for trigger

Serial.printf("waiting for LOW on SCAN_START_PIN (pin %d) ...\n", SCAN_START_PIN);

while (digitalRead(SCAN_START_PIN) == HIGH)

{

delay(100);

}

Serial.println("Scan Triggered - starting scan, setting SCAN_COMPLETE_PIN to LOW\n");

digitalWrite(SCAN_COMPLETE_PIN, LOW);

stepper.setRPM(scanSpeedRpm);

for (curAngleStepVal = 1; curAngleStepVal <= scanNumberofSteps; curAngleStepVal++)

{

curRelPointingAngleDeg = scanStartDeg + (curAngleStepVal)*scanstepdeg;

//when SCAN_STEP_ENABLE_PIN goes LOW, go to first/next step.

while (digitalRead(SCAN_STEP_ENABLE_PIN))

{

}

unsigned long now = millis();

digitalWrite(SCAN_STEP_COMPLETE_PIN, LOW);

Serial.printf("Scan Step %d Triggered: moving to %3.2f deg\n", curAngleStepVal, curRelPointingAngleDeg);

//DRV8825

stepper.rotate(scanstepdeg); //rotate to next scan angle

//output HIGH on SCAN_STEP_COMPLETE_PIN

//DEBUG!!

//Serial.printf("Outputting HIGH on SCAN_STEP_COMPLETE_PIN\n");

//Serial.printf("Done in %lu Msec - Current Angle = %3.2f\n", millis() - now, curRelPointingAngleDeg);

//DEBUG!!

digitalWrite(SCAN_STEP_COMPLETE_PIN, HIGH);

//delay(50); //make the HIGH period visible on scope

}

//quit

Serial.printf("Outputting HIGH on SCAN_COMPLETE_PIN (pin %d)\n", SCAN_COMPLETE_PIN);

digitalWrite(SCAN_COMPLETE_PIN, HIGH);

//return to start position

//DRV8825

stepper.setRPM(POSITIONING_SPEED_RPM);

stepper.rotate(-scandeg);

//re-initialize step counter and current pointing angle

curAngleStepVal = 0;

curRelPointingAngleDeg = scanStartDeg;

Serial.printf("Enter any key to continue\n");

while (!Serial.available())

{

}

//consume bytes until Serial buffer is empty to avoid unintended triggers

int incomingByte;

while (Serial.available())

{

incomingByte = Serial.read();

Serial.printf("Consuming 0x%x\n", incomingByte);

}

void loop()

{

}

int GetIntegerParameter(String prompt, int defaultval)

{

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;

}

//05/30/20 added to report current angle step# & relative pointing angle to measurement program

void requestEvent()

{

//send the current step# and current relative pointing angle

//DEBUG!!

//Serial.printf("In requestEvent: curAngleStep = %d, curRelAngle = %2.2f\n",

// curAngleStepVal, curAngleStepVal, curRelPointingAngleDeg, curRelPointingAngleDeg);

//DEBUG!!

I2C_writeAnything(curAngleStepVal);

I2C_writeAnything(curRelPointingAngleDeg);

}

And the updated hardware schematic:

Here’s the output from a typical 180-degree scan

The triple VL53L0X scanner program:

Starting Angle Scan...
 
Msec	Step#	RelDeg	Front	Center	Rear	Steer
23815846	0	-90.00	121	30	17	3.47
23816965	1	-80.00	120	28	19	3.61
23817408	2	-70.00	118	29	18	3.45
23817852	3	-60.00	120	30	20	3.33
23818296	4	-50.00	123	32	19	3.25
23818740	5	-40.00	120	28	20	3.57
23819183	6	-30.00	122	30	19	3.43
23819627	7	-20.00	119	29	22	3.34
23820071	8	-10.00	122	29	19	3.55
23820515	9	0.00	121	31	20	3.26
23820958	10	10.00	122	31	17	3.39
23821402	11	20.00	120	30	16	3.47
23821846	12	30.00	120	27	22	3.63
23822290	13	40.00	120	31	20	3.23
23822733	14	50.00	124	29	19	3.62
23823177	15	60.00	120	30	19	3.37
23823621	16	70.00	123	29	23	3.45
23824065	17	80.00	120	31	18	3.29
23824508	18	90.00	116	31	20	3.10
SCAN_COMPLETE_PIN HIGH Received - Scan is Complete!
Enter Q to quit, anything else for another scan

Starting Angle Scan...

Msec Step# RelDeg Front Center Rear Steer

23815846 0 -90.00 121 30 17 3.47

23816965 1 -80.00 120 28 19 3.61

23817408 2 -70.00 118 29 18 3.45

23817852 3 -60.00 120 30 20 3.33

23818296 4 -50.00 123 32 19 3.25

23818740 5 -40.00 120 28 20 3.57

23819183 6 -30.00 122 30 19 3.43

23819627 7 -20.00 119 29 22 3.34

23820071 8 -10.00 122 29 19 3.55

23820515 9 0.00 121 31 20 3.26

23820958 10 10.00 122 31 17 3.39

23821402 11 20.00 120 30 16 3.47

23821846 12 30.00 120 27 22 3.63

23822290 13 40.00 120 31 20 3.23

23822733 14 50.00 124 29 19 3.62

23823177 15 60.00 120 30 19 3.37

23823621 16 70.00 123 29 23 3.45

23824065 17 80.00 120 31 18 3.29

23824508 18 90.00 116 31 20 3.10

SCAN_COMPLETE_PIN HIGH Received - Scan is Complete!

Enter Q to quit, anything else for another scan

The DRV8825 Rotary Table program:

waiting for LOW on SCAN_START_PIN (pin 2) ...
Scan Triggered - starting scan, setting SCAN_COMPLETE_PIN to LOW

Scan Step 1 Triggered: moving to -80.00 deg
Scan Step 2 Triggered: moving to -70.00 deg
Scan Step 3 Triggered: moving to -60.00 deg
Scan Step 4 Triggered: moving to -50.00 deg
Scan Step 5 Triggered: moving to -40.00 deg
Scan Step 6 Triggered: moving to -30.00 deg
Scan Step 7 Triggered: moving to -20.00 deg
Scan Step 8 Triggered: moving to -10.00 deg
Scan Step 9 Triggered: moving to 0.00 deg
Scan Step 10 Triggered: moving to 10.00 deg
Scan Step 11 Triggered: moving to 20.00 deg
Scan Step 12 Triggered: moving to 30.00 deg
Scan Step 13 Triggered: moving to 40.00 deg
Scan Step 14 Triggered: moving to 50.00 deg
Scan Step 15 Triggered: moving to 60.00 deg
Scan Step 16 Triggered: moving to 70.00 deg
Scan Step 17 Triggered: moving to 80.00 deg
Scan Step 18 Triggered: moving to 90.00 deg
Outputting HIGH on SCAN_COMPLETE_PIN (pin 5)
Enter any key to continue

waiting for LOW on SCAN_START_PIN (pin 2) ...

Scan Triggered - starting scan, setting SCAN_COMPLETE_PIN to LOW

Scan Step 1 Triggered: moving to -80.00 deg

Scan Step 2 Triggered: moving to -70.00 deg

Scan Step 3 Triggered: moving to -60.00 deg

Scan Step 4 Triggered: moving to -50.00 deg

Scan Step 5 Triggered: moving to -40.00 deg

Scan Step 6 Triggered: moving to -30.00 deg

Scan Step 7 Triggered: moving to -20.00 deg

Scan Step 8 Triggered: moving to -10.00 deg

Scan Step 9 Triggered: moving to 0.00 deg

Scan Step 10 Triggered: moving to 10.00 deg

Scan Step 11 Triggered: moving to 20.00 deg

Scan Step 12 Triggered: moving to 30.00 deg

Scan Step 13 Triggered: moving to 40.00 deg

Scan Step 14 Triggered: moving to 50.00 deg

Scan Step 15 Triggered: moving to 60.00 deg

Scan Step 16 Triggered: moving to 70.00 deg

Scan Step 17 Triggered: moving to 80.00 deg

Scan Step 18 Triggered: moving to 90.00 deg

Outputting HIGH on SCAN_COMPLETE_PIN (pin 5)

Enter any key to continue

For anyone interested in using this program, it is available on my GitHub site at https://github.com/paynterf/Teensy-DRV8825-Rotary-Scan-Table