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

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

Off-perpendicular measurement problems with HC-SRO4 Ping Measurements

1 Reply

Posted 14 May 2020,

For the last couple of months I have been working lately on updating my four-wheel autonomous wall-following robot. Unfortunately, I have been unable to really nail down an effective algorithm for capturing and then holding a specified offset distance from the nearest wall. If the robot starts with its body oriented parallel to the wall, it can successfully capture and then hold the desired distance. Unfortunately, it has turned out to more difficult than I thought to consistently obtain the required parallel orientation starting position.

So, came up with an idea that was sure to work; instead of making a sweeping turn looking for an inflection point in the distance reported by the HC-SR04 ‘ping’ sensor, I would instead make a series of N-degree turns. I wouldn’t know the absolute orientation of the robot with respect to the wall, but I would know the total angle subtended by the robot, and (I thought) it should be much easier to determine the inflection point from the resulting Angle/Distance table.

Unfortunately, when I tested this idea, it failed because the distances reported by the ping sensor didn’t vary significantly, even though I could plainly see that the actual distance between the robot’s ping sensor and where the sensor was pointing was changing significantly – what the heck? The following short video clip and Excel plot show the situation.

If you were to believe the Excel plot, the inflection point denoting parallel orientation would actually be at about 63 degrees off the perpendicular – clearly not right.

To further investigate the issue, I ran some simple manual ping vs tape measure and LIDAR vs tape measure tests. The following photo shows the setup, and the Excel plots show the results.

Setup for LIDAR vs tape vs angle measurements. The ‘Ping’ vs tape measurements were set up the same way

As the above plots show, the ping sensor measurements are basically useless for anything more than a few degrees off perpendicular; as the ‘ping diff’ plot in the upper chart shows, the inflection point could be anywhere. In contrast, the manual tape measurements show a distinct curve, and the point-point differential changes sign at very close to 0 deg off perpendicular.

The lower plot shows the same measurements but using LIDAR rather than the ultrasonic ping sensors. As the plot shows, the LIDAR measurements would actually be reasonably accurate; the inflection point for both the LIDAR measurement (the ‘LIDAR diff’ line above) and the tape measurement (‘Tape Diff’ line above) is at approximately 0 degrees off perpendicular. Unfortunately, the Pulsed Light ‘LIDAR-lite’ units are much more expensive than the ubiquitous HC-SRO4 ‘ping’ sensor.

After noodling around on the web for a while, I found some references to a GY-530/VL53L0X ‘Time of Flight’ (ToF) sensor that looks like it might do the job. From the Adafruit description of this neat device:

3 to 120 cm in ‘default’ mode
As far as 1.5 to 2 meters on a nice white reflective surface in ‘long range’ mode
3-5V compatible
Control via I2C (default addr = 0x29, but can be configured during setup)
Less than 40mA current draw

The big question, of course, is whether or not this device will be any better at off-perpendicular measurements than the ‘ping’ sensors. I have some on order so hopefully I’ll be able to answer this question shortly.

18 May 2020 Update:

I got my GL530/VLX53L0X sensor in yesterday and I’ve now had a chance to play with it a bit. It’s super small and pretty responsive. Here’s a photo of the setup with an Arduino UNO.

GL530/VL53L0X ToF sensor test setup. Note the sensor could fit on a U.S. dime with room to spare

I got it to play and produce basic distance data, but haven’t done much else with it yet. However, while noodling around looking at data sheets, I ran across this demo video by an ST engineer, and it really started me thinking about different ways to use this device as one of an array of sensors; maybe this would make the ‘RotateToParallelOrientation()’ function easier – or even unnecessary!

19 May Update:

I was able to make some reasonably precise measurements today with the GY530/VL53L0X Time-of-Flight sensor. Here’s the setup:

Testing setup for GY530/VL53LOX ToF Sensor. Note wood board test surface in background

And here’s a plot of the measured distance vs off-perpendicular angle, along with actual tape measure value for accuracy comparisons

While the measuring tape values and sensor distance values track very well over the -50 to +40 deg range, they aren’t the same. The sensor measurements are consistently lower, by 10 mm or so. That’s not a big deal, and there may be some calibration techniques available to zero out any constant error term.

I also noticed that the sensor value returned were occasionally dead wrong, reporting a value of 20 mm when the real value was more like 300-350. Again, this might be addressable by averaging, but…

And lastly, as shown by the above plot, above about 40 degrees off-perpendicular, the sensor measurements become unreliable, probably due to low SNR return signal.

The good news is that the off-perpendicular plot does seem to behave fairly well in the -30 to +30 degree range, and has the expected quadratic bowl shape, so it should be usable for finding the parallel point, and maybe even for providing a steering value to a PID engine for offset hold operations once the proper offset has been captured.

To explore the ‘steering value’ idea, I simulated a 3-sensor setup by picking values out of the above plots 3 at a time. For instance assuming the -40, -10, and +20 degree values were taken at the same time by 3 different sensors. The following plot shows the results of the following calculation:

(M_l – M_r)/M_c

where, ‘l’, ‘r’ and ‘c’ subscripts refer to the left, right, and center sensors in a 3-sensor array angled at -30, 0 and +30 degrees respectively. Here’s the plot

simulation of a 3-sensor array created by picking values from single-sensor sweep

This plot is quite exciting, as it shows a clear linear relationship between off-perpendicular orientation and steering values that could be used as the input to a PID engine.

Stay tuned,

Frank

I2C between an Arduino Mega and a Teensy 3.x

3 Replies

posted 18 March 2020

This post describes my efforts to troubleshoot an I2C communications problem between an Arduino Mega control board and a Teensy 3.2 IR beam demodulation module.

Background:

My Wall-E2 autonomous wall-following robot homes in on a modulated IR beacon to connect to a charging station. The IR beacon modulation is decoded by a dedicated Teensy 3.2 and provides left/right and combined steering values to an Arduino Mega main controller over an I2C link.

During my recent work to update the robot after some enhancements, I discovered that the main controller was no longer receiving steering information from the Teensy, even though both seemed to be operating properly. Initial efforts to troubleshoot the problem did not bear fruit, so I was forced to back up and start over from scratch.

Troubleshooting:

Initially I thought the problem was a loose connection, as the system was working before. However, I am now pretty sure that I have eliminated all the obvious culprits, so I am left with the non-obvious ones.

To start with, I resurrected an old example project to demonstrate I2C master/slave operation between two Teensy modules.

Teensy 3.2 Slave (left) and Teensy 3.5 Master (right)

Here’s the Teensy ‘Master’ code:

// -------------------------------------------------------------------------------------------
// Basic Master
// -------------------------------------------------------------------------------------------
//
// This creates a simple I2C Master device which when triggered will send/receive a text 
// string to/from a Slave device.  It is intended to pair with a Slave device running the 
// basic_slave sketch.
//
// Pull pin12 input low to send.
// Pull pin11 input low to receive.
//
// This example code is in the public domain.
//
// -------------------------------------------------------------------------------------------

#include <i2c_t3.h>
#include <T3_I2C_Anything.h>

// Memory
#define MEM_LEN 256
char databuf[MEM_LEN];
int count;
float xmtfloat = 1.2344;

void setup()
{
	pinMode(LED_BUILTIN, OUTPUT);    // LED
	digitalWrite(LED_BUILTIN, LOW);  // LED off
	pinMode(12, INPUT_PULLUP);       // Control for Send
	pinMode(11, INPUT_PULLUP);       // Control for Receive

									 // Setup for Master mode, pins 18/19, external pullups, 400kHz, 200ms default timeout
	//Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);
	Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);
	Wire.setDefaultTimeout(200000); // 200ms

	// Data init
	memset(databuf, 0, sizeof(databuf));
	count = 0;

	//Serial.begin(115200);
	unsigned long now = millis();
	Serial.begin(1); //rate value ignored
	int idx = 0;
	while (!Serial && (millis() - now) < 3000)
	{
		delay(500);
		idx++;
	}
	Serial.printf("Serial available after %lu mSec\n", millis() - now);
	Serial.printf("loop idx = %d\n", idx);

	Serial.printf("I2C_Master Example\n");
	Serial.printf("\nGround Pin 12 to transmit a float value to the slave\n");
	Serial.printf("\nGround Pin 11 to request a float value from the slave\n\n");
	
}

void loop()
{
	count++;
	uint8_t target = 0x66; // target Slave address
	//size_t idx;

	// Send string to Slave
	//
	if (digitalRead(12) == LOW)
	{
		digitalWrite(LED_BUILTIN, HIGH);   // LED on

		// Transmit to Slave
		//Serial.printf("Sending to Slave: %3.3f\n", xmtfloat*count);
		float a = xmtfloat * count;
		Serial.printf("Sending to Slave: %3.6f (0x%x)\n", a , *(unsigned int*)&a);
		Wire.beginTransmission(target);   // Slave address
		I2C_writeAnything(xmtfloat*count);
		Wire.endTransmission();           // Transmit to Slave

		// Check if error occured
		if (Wire.getError())
			Serial.print("FAIL\n");
		else
			Serial.print("OK\n");

		digitalWrite(LED_BUILTIN, LOW);    // LED off
		delay(100);                       // Delay to space out tests
	}

	// Read string from Slave
	if (digitalRead(11) == LOW)
	{
		digitalWrite(LED_BUILTIN, HIGH);   // LED on
										   
		Serial.print("Reading from Slave: ");// Print message

		// Read from Slave
		Wire.requestFrom(target, (size_t)MEM_LEN); // Read from Slave (string len unknown, request full buffer)

		if (Wire.getError())
			Serial.print("FAIL\n");
		else
		{
			float slave_val = 0;
			I2C_readAnything(slave_val);
			Serial.printf("Received %f from Slave\n", slave_val);
		}

		digitalWrite(LED_BUILTIN, LOW);    // LED off
		delay(100);                       // Delay to space out tests
	}
}

100

101

102

103

104

105

106

107

108

109

110

111

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

// Basic Master

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

// This creates a simple I2C Master device which when triggered will send/receive a text

// string to/from a Slave device. It is intended to pair with a Slave device running the

// basic_slave sketch.

// Pull pin12 input low to send.

// Pull pin11 input low to receive.

// This example code is in the public domain.

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

#include <i2c_t3.h>

#include <T3_I2C_Anything.h>

// Memory

#define MEM_LEN 256

char databuf[MEM_LEN];

int count;

float xmtfloat = 1.2344;

void setup()

{

pinMode(LED_BUILTIN, OUTPUT); // LED

digitalWrite(LED_BUILTIN, LOW); // LED off

pinMode(12, INPUT_PULLUP); // Control for Send

pinMode(11, INPUT_PULLUP); // Control for Receive

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

//Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);

Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);

Wire.setDefaultTimeout(200000); // 200ms

// Data init

memset(databuf, 0, sizeof(databuf));

count = 0;

//Serial.begin(115200);

unsigned long now = millis();

Serial.begin(1); //rate value ignored

int idx = 0;

while (!Serial && (millis() - now) < 3000)

{

delay(500);

idx++;

}

Serial.printf("Serial available after %lu mSec\n", millis() - now);

Serial.printf("loop idx = %d\n", idx);

Serial.printf("I2C_Master Example\n");

Serial.printf("\nGround Pin 12 to transmit a float value to the slave\n");

Serial.printf("\nGround Pin 11 to request a float value from the slave\n\n");

}

void loop()

{

count++;

uint8_t target = 0x66; // target Slave address

//size_t idx;

// Send string to Slave

if (digitalRead(12) == LOW)

{

digitalWrite(LED_BUILTIN, HIGH); // LED on

// Transmit to Slave

//Serial.printf("Sending to Slave: %3.3f\n", xmtfloat*count);

float a = xmtfloat * count;

Serial.printf("Sending to Slave: %3.6f (0x%x)\n", a , *(unsigned int*)&a);

Wire.beginTransmission(target); // Slave address

I2C_writeAnything(xmtfloat*count);

Wire.endTransmission(); // Transmit to Slave

// Check if error occured

if (Wire.getError())

Serial.print("FAIL\n");

else

Serial.print("OK\n");

digitalWrite(LED_BUILTIN, LOW); // LED off

delay(100); // Delay to space out tests

}

// Read string from Slave

if (digitalRead(11) == LOW)

{

digitalWrite(LED_BUILTIN, HIGH); // LED on

Serial.print("Reading from Slave: ");// Print message

// Read from Slave

Wire.requestFrom(target, (size_t)MEM_LEN); // Read from Slave (string len unknown, request full buffer)

if (Wire.getError())

Serial.print("FAIL\n");

else

{

float slave_val = 0;

I2C_readAnything(slave_val);

Serial.printf("Received %f from Slave\n", slave_val);

}

digitalWrite(LED_BUILTIN, LOW); // LED off

delay(100); // Delay to space out tests

}

And the Teensy ‘Slave’ code:

	// -------------------------------------------------------------------------------------------
	// Basic Slave
	// -------------------------------------------------------------------------------------------
	//
	// This creates a simple I2C Slave device which will print whatever text string is sent to it.
	// It will retain the text string in memory and will send it back to a Master device if 
	// requested.  It is intended to pair with a Master device running the basic_master sketch.
	//
	// This example code is in the public domain.
	//
	// -------------------------------------------------------------------------------------------

/*
	03/07/20 Modified to be Teensy-specific, and to use 'I2C_Anything' for I2C communications
*/
#include <i2c_t3.h>
#include <T3_I2C_Anything.h>

// Function prototypes
void receiveEvent(size_t count);
void requestEvent(void);

// Memory
volatile uint8_t received;
volatile float rcvfloat = 0;

//
// Setup
//
void setup()
{
	pinMode(LED_BUILTIN, OUTPUT); // LED

	// Setup for Slave mode, address 0x66, pins 18/19, internal pullups, 400kHz
	//Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);
	//Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_INT, 100000);
	Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_EXT, 100000);

	// register events
	Wire.onReceive(receiveEvent);
	Wire.onRequest(requestEvent);

	//Serial.begin(115200);
	unsigned long now = millis();
	Serial.begin(1); //rate value ignored
	int idx = 0;
	while (!Serial && (millis() - now) < 3000)
	{
		delay(500);
		idx++;
	}

	Serial.printf("Serial available after %lu mSec\n", millis() - now);
	Serial.printf("I2C Slave Example\n");
}

void loop()
{
	// print received data - this is done in main loop to keep time spent in I2C ISR to minimum
	if (received)
	{
		digitalWrite(LED_BUILTIN, HIGH);
		Serial.printf("Slave received: %f\n", rcvfloat);
		received = 0;
		digitalWrite(LED_BUILTIN, LOW);
	}
}

//
// handle Rx Event (incoming I2C data)
//
void receiveEvent(size_t count)
{
	received = count; // set received flag to count, this triggers print in main loop
	I2C_readAnything(rcvfloat);
}

//
// handle Tx Event (outgoing I2C data)
//
void requestEvent(void)
{
	Serial.printf("Sending %f to Master\n", rcvfloat);

	const byte* p = (const byte*)&rcvfloat;
	unsigned int i;
	for (i = 0; i < sizeof rcvfloat; i++)
		Wire.write(*p++);
	delay(5); //experiment
	//I2C_writeAnything(rcvfloat);
}

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

// Basic Slave

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

// This creates a simple I2C Slave device which will print whatever text string is sent to it.

// It will retain the text string in memory and will send it back to a Master device if

// requested. It is intended to pair with a Master device running the basic_master sketch.

// This example code is in the public domain.

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

03/07/20 Modified to be Teensy-specific, and to use 'I2C_Anything' for I2C communications

#include <i2c_t3.h>

#include <T3_I2C_Anything.h>

// Function prototypes

void receiveEvent(size_t count);

void requestEvent(void);

// Memory

volatile uint8_t received;

volatile float rcvfloat = 0;

// Setup

void setup()

{

pinMode(LED_BUILTIN, OUTPUT); // LED

// Setup for Slave mode, address 0x66, pins 18/19, internal pullups, 400kHz

//Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);

//Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_INT, 100000);

Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_EXT, 100000);

// register events

Wire.onReceive(receiveEvent);

Wire.onRequest(requestEvent);

//Serial.begin(115200);

unsigned long now = millis();

Serial.begin(1); //rate value ignored

int idx = 0;

while (!Serial && (millis() - now) < 3000)

{

delay(500);

idx++;

}

Serial.printf("Serial available after %lu mSec\n", millis() - now);

Serial.printf("I2C Slave Example\n");

}

void loop()

{

// print received data - this is done in main loop to keep time spent in I2C ISR to minimum

if (received)

{

digitalWrite(LED_BUILTIN, HIGH);

Serial.printf("Slave received: %f\n", rcvfloat);

received = 0;

digitalWrite(LED_BUILTIN, LOW);

}

// handle Rx Event (incoming I2C data)

void receiveEvent(size_t count)

{

received = count; // set received flag to count, this triggers print in main loop

I2C_readAnything(rcvfloat);

}

// handle Tx Event (outgoing I2C data)

void requestEvent(void)

{

Serial.printf("Sending %f to Master\n", rcvfloat);

const byte* p = (const byte*)&rcvfloat;

unsigned int i;

for (i = 0; i < sizeof rcvfloat; i++)

Wire.write(*p++);

delay(5); //experiment

//I2C_writeAnything(rcvfloat);

}

With this configuration I got the following output:

Master:

Opening port
Port open
Sending to Slave: 812935680.000000 (0x4e41d1a8)
OK
Sending to Slave: 812935680.000000 (0x4e41d1a8)
OK
Sending to Slave: 812935680.000000 (0x4e41d1a8)
OK
Sending to Slave: 812935680.000000 (0x4e41d1a8)
OK
...
...
Reading from Slave: Received 812935744.000000 from Slave
Reading from Slave: Received 812935744.000000 from Slave
Reading from Slave: Received 812935744.000000 from Slave
Reading from Slave: Received 812935744.000000 from Slave
Reading from Slave: Received 812935744.000000 from Slave

Opening port

Port open

Sending to Slave: 812935680.000000 (0x4e41d1a8)

...

Reading from Slave: Received 812935744.000000 from Slave

Slave:

Opening port
Port open
Slave received: 812935680.000000
Slave received: 812935680.000000
Slave received: 812935680.000000
Slave received: 812935680.000000
...
...
Sending 812935744.000000 to Master
Sending 812935744.000000 to Master
Sending 812935744.000000 to Master
Sending 812935744.000000 to Master

Opening port

Port open

Slave received: 812935680.000000

...

Sending 812935744.000000 to Master

So this seems to be working OK.

Then I added in a third Teensy module (T3.5) running my newly developed I2C-Sniffer code, so that I could ‘sniff’ the I2C traffic between the two Teensy’s. This will give me a ‘known-good’ baseline for when I move back to the non-working Arduino Mega Master and Teensy Slave condition that is causing me problems.

Teensy 3.2 Slave (left), Teensy 3.5 I2C Sniffer (middle), Teensy 3.5 Master (right)

Here’s the Teensy ‘Sniffer’ code:

/*
    Name:       Teensy_I2C_Sniffer_V11.ino
    Created:	1/18/2020 10:55:55 AM
    Author:     FRANKNEWXPS15\Frank
*/
/* 'Notes:

    A typical I2C sentence when communicating with a MPU6050 IMU module goes something like:
        "I2C(68) wrote 1 byte to 75 - C0 Done."
        "I2C(68) wrote 3 bytes to 72 - C0 0C 10 Done."
        "I2C(68) read 5 bytes from 6A - C0 0C 10 14 03 Done."

    To form a sentence, we need:
        Device addr: 68 in the above examples
        Read/Write direction
        To/From register address:  75, 72 and 6A in the above examples
        Data:  C0, C0 0C 10, and C0 0C 10 14 03 in the above examples
        number of bytes written/read:  1,3 & 5 in the above examples

     Each I2C communication proceeds as follows (assuming a START from an IDLE condition):
         A START or RESTART condition, denoted by SDA & SCL HIGH, followed by SDA LOW, SCL HIGH
         A 7-bit device address, MSB first (0x8/0xC = 1, 0x0/0x4 = 0)
         A R/W bit (0x8/0xC = read, 0x0/0x4 = write)
         An ACK bit (0x8/0xC = NAK, 0x0/0x4 = ACK)
         If the bus direction is WRITE, then
             A register address for read/write
             zero or more additional data bytes
         Else (the bus direction is READ)
            One or more additional data bytes
         Endif

    This version uses a fixed-size (2048 bytes) array instead of tonton81's circular buffer library.

    To generalize for any I2C slave device rather than just the MPU6050 IMU, comment out the 
    "#define MPU6050_SPECIFIC line below. This will remove all MPU6050 specific code
*/


//#define MPU6050_SPECIFIC

#include <TimerOne.h> //needed for ISR
#ifdef MPU6050_SPECIFIC
#include "helper_3dmath.h" //Arduino\Libraries\i2cdevlib\Arduino\MPU6050\ needed to compute yaw from MPU6050 DMP packet
#endif


#define PARSE_LOOP_DEBUG

const uint16_t CAPTURE_ARRAY_SIZE = 2048;
const uint16_t VALID_DATA_ARRAY_SIZE = 2048;
const int WAITING_PRINT_INTERVAL_MSEC = 200;//interval timer for 'Waiting for data...' printout

#define MONITOR_OUT1 2 //so can monitor ISR activity with O'scope
#define MONITOR_OUT2 3 //so can monitor ISR activity with O'scope
#define MONITOR_OUT3 4 //so can monitor ISR activity with O'scope
#define SDA_PIN 18 
#define SCL_PIN 19

#pragma region PROCESSING_VARIABLES
uint8_t devAddr;
uint8_t regAddr;
uint8_t databytes[2048]; //holds multiple databytes for later output sentence construction
uint16_t numbytes = 0; //number of data bytes extracted from data stream
int ACKNAKFlag; //can be negative
uint16_t databyte_idx = 0; //index into databyte_array
uint8_t killbuff[2]; //used to consume start/stop bytes
elapsedMillis mSecSinceLastWaitingPrint;
uint8_t valid_data[2048];
uint16_t numvalidbytes = 0; //number of valid bytes in this burst
uint16_t read_idx = 0; //pointer to next byte pair to be processed

//added for bus direction labels
enum BUSDIR
{
    WRITE,
    READ,
    UNKNOWN = -1
} RWDir;
BUSDIR BusDir = BUSDIR::UNKNOWN;
#pragma endregion ProcVars


#pragma region ISR_SUPPORT
uint8_t raw_data[CAPTURE_ARRAY_SIZE]; //holds data captured from I2C bus
volatile uint16_t  write_idx = 0;
volatile uint8_t   current_portb = 0xFF;
volatile uint8_t   last_portb = 0xFF;
volatile uint16_t mult0xCCount = 0;
const uint16_t MAX_IDLE_COUNT = 2500;

volatile bool bDone = false;
volatile bool bWaitingForStart = true;
volatile bool bIsData = true;
volatile bool bIsStart = false;
volatile bool bIsStop = false;
volatile uint8_t last_current;
#pragma endregion ISR Support

#ifdef MPU6050_SPECIFIC
//01/21/20 these forward declarations are required to force the preprocessor 
//to handle #ifdef MPU6050_SPECIFIC properly
uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet);
uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet);
uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity);
uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q);

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 ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
float global_yawval = 0; //updated by GetIMUHeadingDeg()
#endif // MPU6050_SPECIFIC

void setup()
{
    unsigned long now = millis();
    Serial.begin(1); //rate value ignored
    int idx = 0;
    while (!Serial && (millis() - now) < 3000)
    {
        delay(500);
        idx++;
    }
    Serial.printf("Serial available after %lu mSec\n", millis() - now);

    pinMode(MONITOR_OUT1, OUTPUT);
    digitalWrite(MONITOR_OUT1, LOW);
    pinMode(MONITOR_OUT2, OUTPUT);
    digitalWrite(MONITOR_OUT2, LOW);
    pinMode(MONITOR_OUT3, OUTPUT);
    digitalWrite(MONITOR_OUT3, LOW);

    pinMode(SCL_PIN, INPUT);
    pinMode(SDA_PIN, INPUT);

    //reset port byte vars & start timer
    last_portb = current_portb = 0;
    write_idx = 0;
    memset(raw_data, 255, CAPTURE_ARRAY_SIZE);
    //PrintNextArrayBytes(raw_data, 255, 20);
    Timer1.initialize(1); // run every mico second
    Timer1.attachInterrupt(capture_data);


    mSecSinceLastWaitingPrint = 0;
}
//-------------------------------------------------------------------------------
//--------------------------------    ISR    ------------------------------------
//-------------------------------------------------------------------------------
FASTRUN void capture_data()
//void capture_data()
{
    last_portb = current_portb;
    current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

    if (!bDone && last_portb != current_portb)
    {
        mult0xCCount = 0; //reset IDLE counter
        digitalWriteFast(MONITOR_OUT1, HIGH);

        //01/17/20: joepasquariello suggestion
        last_current = (last_portb << 4) | (current_portb);
        bIsStart = (last_current == 0xC4);
        bIsStop = (last_current == 0x4C);
        bIsData = (last_current == 0x04) || (last_current == 0x8C);

        if (bIsStart) //START  
        {
            digitalWriteFast(MONITOR_OUT2, HIGH);
            if (bWaitingForStart)
            {
                digitalWriteFast(MONITOR_OUT3, HIGH); //start of entire capture
                bWaitingForStart = false;
            }
        }
        else if (bIsStop) //STOP
        {
            digitalWriteFast(MONITOR_OUT2, LOW);
        }

        if (!bWaitingForStart && (bIsData || bIsStart || bIsStop))
        {
            //digitalWriteFast(MONITOR_OUT3, HIGH);
            raw_data[write_idx] = last_portb;
            write_idx++;
            raw_data[write_idx] = current_portb;
            write_idx++;
            if (write_idx >= CAPTURE_ARRAY_SIZE)
            {
                bDone = true;
                digitalWriteFast(MONITOR_OUT3, LOW);
            }
        }
        digitalWriteFast(MONITOR_OUT1, LOW);
    }
    else if (!bDone && mult0xCCount < MAX_IDLE_COUNT && last_portb == 0xc && current_portb == 0xc)
    {
        mult0xCCount++;
        if (mult0xCCount >= MAX_IDLE_COUNT)
        {
            digitalWriteFast(MONITOR_OUT3, LOW);
            bDone = true;
        }
    }
}
//-------------------------------------------------------------------------------
//-------------------------------- END ISR    ---------------------------------
//-------------------------------------------------------------------------------

void loop()
{
    if (bDone)
    { 
        if (write_idx > 14)
        {
            //OK, we have some data to process. IDLE detection must have been EOM
            Timer1.stop();

            unsigned long startMsec = millis();

            //Serial.printf("%lu\t %d\t", millis(), write_idx);
            //PrintNextArrayBytes(raw_data, 0, 50);
            //Serial.printf(" - %lu\n", millis());
            uint16_t numprocessed = DecodeAndPrintValidData(raw_data); //decode and print everything captured so far
            unsigned long endMsec = millis();
            Serial.printf("%lu: processed = %d elements in %lu mSec\n\n", startMsec, numprocessed, endMsec-startMsec);

            Timer1.start();
        }

        read_idx = 0;
        bDone = false;
        mult0xCCount = 0;
        write_idx = 0;
        bWaitingForStart = true;
    }    
    else
    {
        //no data to process, but don't blow prints out every mSec...
        if (mSecSinceLastWaitingPrint > WAITING_PRINT_INTERVAL_MSEC)
        {
            mSecSinceLastWaitingPrint -= WAITING_PRINT_INTERVAL_MSEC;
            Serial.printf("%lu: Waiting for Data...\n", millis());
        }
    }
}

void PrintNextArrayBytes(uint8_t* data, uint16_t startidx, uint16_t numbytes)
{
    Serial.printf("%d bytes starting at %d: ", numbytes, startidx);
    for (uint16_t i = 0; i < numbytes; i++)
    {
        Serial.printf("%x ", data[i + startidx]);
    }
}


uint16_t DecodeAndPrintValidData(byte* data)
{
    //Purpose:  decode and print I2C conversation held in raw_data array
    //Inputs:  
    //  cb = 2048 element FIFO
    //Outputs:
    //  returns number of bytes processed, or -1 for failure
    //  outputs structured I2C sentence to serial monitor
    //Plan:
    //  Step1: Cull out invalid bytes
    //  Step2: Determine if there is anything to do (have to have more than one transition in FIFO)
    //  Step3: Parse transitions into I2C sentence structure
    //  Step4: Output sentence to serial monitor

    memset(valid_data, 0, VALID_DATA_ARRAY_SIZE);
#ifdef PARSE_LOOP_DEBUG
    PrintNextArrayBytes(valid_data, 0, 20); //print out first 20 bytes for verification
#endif
    numvalidbytes = RemoveInvalidBytes(raw_data, valid_data);
#ifdef PARSE_LOOP_DEBUG
    Serial.printf("Removed %d invalid bytes, leaving %d remaining\n", write_idx + 1 - numvalidbytes, numvalidbytes);
    PrintNextArrayBytes(valid_data, 0, numvalidbytes); //print out first 20 bytes of valid_data array
#endif


    if (numvalidbytes < 2)
    {
        return 0;
    }

    while (read_idx < numvalidbytes)
    {
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("At top of while (read_idx < numvalidbytes): read_idx = %d\n", read_idx);
        Serial.printf("Next two bytes in valid_data are %x, %x\n", valid_data[read_idx], valid_data[read_idx + 1]);
#endif
        //Find a START sequence (0xC followed by 0x4)
        while (!IsStart(valid_data, read_idx) && read_idx < numvalidbytes)
        {
            //Serial.printf("looking for start...\n");
            read_idx++;
        }
        //at this point, read_idx should point to next valid byte pair

#ifdef PARSE_LOOP_DEBUG
        Serial.printf("Start sequence found at %d\n", read_idx - 2);
        //PrintNextFIFOBytes(valid_data, 20);
#endif

        if (numvalidbytes - read_idx > 14)//14 entries required for 7-bit address
        {
            //Get 7-bit device address
            devAddr = Get7BitDeviceAddr(valid_data, read_idx);
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("devAddr = %x\n", devAddr);
#endif
        }
        else
        {

#ifdef PARSE_LOOP_DEBUG
            Serial.printf("ran out of data at readidx = %d - exiting!\n", read_idx);
#endif
            break;
        }

        //get read/write flag  1 = Read, 0 = Write, -1 = error
        BusDir = (BUSDIR)GetReadWriteFlag(valid_data, read_idx);

#ifdef PARSE_LOOP_DEBUG
        Serial.printf("BusDir = %s\n", ((BusDir == BUSDIR::WRITE) ? "WRITE" : "READ"));
        //PrintNextFIFOBytes(valid_data, 20);
#endif

    //get ACK/NAK flag
        ACKNAKFlag = GetACKNAKFlag(valid_data, read_idx);
        numbytes = GetDataBytes(valid_data, read_idx, databytes); //terminates on a START, but the start bytes are not consumed
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("Got %d bytes from GetDataBytes() --> ", numbytes);
        for (size_t i = 0; i < numbytes; i++)
        {
            Serial.printf(" %x ", databytes[i]);
        }
        Serial.printf("\n");

        //PrintNextFIFOBytes(cb_trans, 20);
#endif
    //If the bus direction is WRITE, then extract
    //    A register address for read / write
    //    zero or more additional data bytes
        if (BusDir == BUSDIR::WRITE)
        {
            regAddr = databytes[0];
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("regAddr = %x, read_idx = %d\n", regAddr, read_idx);
#endif

            //check for additional data
            if (numbytes > 1)
            {
#ifdef PARSE_LOOP_DEBUG
                Serial.printf("Additional data found!\n");
                for (size_t i = 0; i < numbytes; i++)
                {
                    Serial.printf("data[%d] = %x\n", i, databytes[i]);
                }
#endif
                //1st byte is register addr, subsequent bytes are data
                OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databytes, 1);
            }
        }
        else  //all bytes are data
        {
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("In data block:  got %d bytes of data\n", numbytes);
            for (size_t i = 0; i < numbytes; i++)
            {
                Serial.printf("data[%d] = %x\n", i, databytes[i]);
            }
#endif
            OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databytes, 0);
        }
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("At end of while (read_idx < numvalidbytes): read_idx = %d\n", read_idx);
#endif

    }//while (read_idx < numvalidbytes)
    return numvalidbytes;
}


#pragma region Support Functions
bool IsStart(byte* data, uint16_t& readidx)
{
    bool result = false;

    //Serial.printf("IsStart[%d] = %x, IsStart[%d] = %x\n",
    //    readidx, data[readidx], readidx + 1, data[readidx + 1]);

    if (data[readidx] == 0xC && data[readidx + 1] == 0x4)
    {
        result = true;
        readidx += 2; //bump to next byte pair
    }
    return result;
}

bool IsStop(byte* data, uint16_t& readidx)
{
    bool result = false;

    //Serial.printf("IsStop[%d] = %x, IsStop[%d] = %x\n",
        //readidx, data[readidx], readidx + 1, data[readidx + 1]);

    if (data[readidx] == 0x4 && data[readidx + 1] == 0xC)
    {
        result = true;
        readidx += 2; //bump to next byte pair
    }
    return result;
}

uint8_t Get7BitDeviceAddr(byte* data, uint16_t& readidx)
{
    //Purpose: Construct a 7-bit address starting from dataidx
    //Inputs:
    //  data = pointer to valid data array
    //  readidx = starting index of 7-bit address sequence (MSB first)
    //Outputs:
    //  returns the address as an 8-bit value with the MSB = 0, or 0x0 if unsuccessful
    //  dataidx = pointer to next data entry
    //Plan:
    //  Step1: Convert a pair of data entries into a 0 or 1
    //  Step2: Add the appropriate value to an ongoing sum
    //  Step3: return the total.
    //Notes:
    //  A '0' is coded as a 0x0 followed by a 0x4
    //  A '1' is coded as a 0x8 followed by a 0xC

    uint8_t devAddr = 0x0; //failure return value

    //Serial.printf("Get7BitDeviceAddr: readidx = %d\n",readidx);

    //devAddr is exactly 7 bits long, so 8 bits with MSB = 0
    for (size_t i = 0; i < 7; i++)
    {
        if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)
        {
            readidx += 2; //advance the pointer, but don't add to sum
        }

        else if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)
        {
            //Serial.printf("Get7BitDeviceAddr: '1' found at i = %d, adding %x to devAddr to get %x\n",
            //    i, 1 << (7 - i), devAddr + (1 << (7-i)));

            readidx += 2; //advance the pointer
            devAddr += (1 << (7 - i)); //add 2^(7-i) to sum
        }
    }

    devAddr = devAddr >> 1; //divide result by 2 to get 7-bit addr from 8 bits
    return devAddr;
}

int Get8BitDataByte(byte* data, uint16_t& readidx)
{
    //Purpose: Construct a 8-bit data byte starting from dataidx
    //Inputs:
    //  data = pointer to valid data array
    //  readidx = starting index of 8-bit data byte (MSB first)
    //Outputs:
    //  returns the address as an 8-bit value, or 0x0 if unsuccessful
    //  dataidx = pointer to next data entry
    //Plan:
    //  Step1: Convert a pair of data entries into a 0 or 1
    //  Step2: Add the appropriate value to an ongoing sum
    //  Step3: return the total.
    //Notes:
    //  A '0' is coded as a 0x0 followed by a 0x4
    //  A '1' is coded as a 0x8 followed by a 0xC
    //  12/29/19 - changed return val to int, so can return -1 when a 'short byte' is detected

    int dataval = 0x0; //failure return value

#ifdef GET_8BIT_DATABYTE_DEBUG
    Serial.printf("Get8BitDataByte: data[%d] = %x, data[%d] = %x\n",
        readidx, data[readidx], readidx + 1, data[readidx + 1]);
#endif

    //8 bits with MSB = 0
    int numbytes = 0;
    for (size_t i = 0; i < 8; i++)
    {
        if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)
        {
            readidx += 2; //advance the pointer, but don't add to sum
            numbytes++;
        }

        else if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)
        {
#ifdef GET_8BIT_DATABYTE_DEBUG
            Serial.printf("Get8BitDataByte: '1' found at i = %d, adding %x to devAddr to get %x\n",
                i, 1 << (7 - i), dataval + (1 << (7 - i)));
#endif
            readidx += 2; //advance the pointer
            dataval += (1 << (7 - i)); //add 2^(8-i) to sum
            numbytes++;
        }
    }

#ifdef GET_8BIT_DATABYTE_DEBUG
    Serial.printf("Get8BitDataByte: numbytes = %d\n", numbytes);
#endif
    if (numbytes != 8)
    {
        dataval = -1; //error return value
    }

    return dataval;
}

int GetReadWriteFlag(byte* data, uint16_t& readidx)
{
    //Purpose: decode R/W byte pair
    //Inputs:
    //  data = pointer to valid data array
    //  readidx = index into data to start of R/W byte pair
    //Outputs:
    //  readidx = if successful, points to next byte pair in data
    //  returns 1 for Read (0x8/0xC), 0 for Write (0x0/0x4), -1 for failure
    //Notes:
    //  

    //Serial.printf("GetReadWriteFlag: readidx = %d, data[readidx] = %x, data[readidx+1]= %x\n",
    //    readidx, data[readidx], data[readidx + 1]);
    int result = 0;
    if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)
    {
        result = 1; //read detected
        readidx += 2; //point to next byte pair
    }

    else if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)
    {
        result = 0; //write detected
        readidx += 2; //point to next byte pair
    }
    else
    {
        result = -1; //failed to detect read or write
    }

    return result;
}

int GetACKNAKFlag(byte* data, uint16_t& readidx)
{
    //Purpose: decode ACK/NAK byte pair
    //Inputs:
    //  data = pointer to valid data array
    //  readidx = index into data to start of ACK/NAK byte pair
    //Outputs:
    //  readidx = if successful, points to next byte pair in data
    //  returns 1 for NAK (0x8/0xC), 0 for ACK (0x0/0x4), -1 for failure
    //Notes:
    //  

    //Serial.printf("GetACKNAKFlag: readidx = %d, data[readidx] = %x, data[readidx+1]= %x\n",
    //    readidx, data[readidx], data[readidx + 1]);
    int result = 0;
    if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)
    {
        result = 1; //NAK detected
        readidx += 2; //point to next byte pair
    }

    else if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)
    {
        result = 0; //ACK detected
        readidx += 2; //point to next byte pair
    }
    else
    {
        result = -1; //failed to detect ACK or NAK
    }

    return result;
}

int GetDataBytes(uint8_t* data, uint16_t& readidx, uint8_t* databytes)
{
    //Notes:
    //  01/01/2020: removed databyteidx from sig - always starts at zero

    uint16_t numbytes = 0;
    uint16_t databyte_idx = 0;

    bool StartFlag = false;
    bool StopFlag = false;

    do
    {
        int dataval = Get8BitDataByte(data, readidx);

        //watch out for 'short byte' reads
        if (dataval >= 0)
        {
            uint8_t databyte = (uint8_t)dataval;
            databytes[databyte_idx] = databyte;
            databyte_idx++;
            numbytes++;
        }

        ACKNAKFlag = GetACKNAKFlag(data, readidx);
        StartFlag = IsStart(data, readidx);
        StopFlag = IsStop(data, readidx);

#ifdef PARSE_LOOP_DEBUG
        Serial.printf("IsStart returned %d, IsStop returned %d, dataidx = %d\n",
            StartFlag, StopFlag, readidx);
#endif

    } while (!StartFlag && !StopFlag && readidx < numvalidbytes);


    readidx -= 2;//back readidx up so loop top is positioned correctly.

    return numbytes;
}

void OutputFormattedSentence(int RW, uint8_t dev, uint8_t reg, uint8_t numbytes, uint8_t* bytearray, uint16_t startidx)
{
    Serial.printf("%lu I2C(%x) %s %d bytes %s %x... ",
        millis(), dev, (RW == 0 ? "writing" : "reading"),  numbytes - startidx, (RW == 0 ? "to" : "from"), reg);
    //for (size_t i = startidx; i < numbytes; i++)
    //{
    //    Serial.printf("%x ", bytearray[i]);
    //}
    //Serial.printf(". Done\n");

#ifdef MPU6050_SPECIFIC


    //01/18/20 experiment to decode 28-byte packet into yaw value
    if (numbytes == 28)
    {
        dmpGetQuaternion(&q, bytearray);
        dmpGetGravity(&gravity, &q);
        dmpGetYawPitchRoll(ypr, &q, &gravity);

        //compute the yaw value
        global_yawval = ypr[0] * 180 / M_PI;
        Serial.printf("yawval = %3.2f\n", global_yawval);
    }
    else
    {
        Serial.printf(". Done\n");
    }
#endif // MPU6050_SPECIFIC



}

uint16_t RemoveInvalidBytes(uint8_t* rawdata, uint8_t* validdata)
{
    uint16_t numvalid = 0;
    uint16_t valididx = 0;

    //Serial.printf("raw data array contains %d bytes\n", write_idx + 1);
    //PrintNextArrayBytes(raw_data, 0, 20);

    //OK, now go back through the array, excising invalid sequences
    for (uint16_t rawidx = 0; rawidx < write_idx;/*rawidx incremented internally*/)
    {
        uint8_t firstByte = raw_data[rawidx]; //get the first byte
        uint8_t secondByte = raw_data[rawidx + 1]; //get the next byte
        bool validpair =
            (
            (firstByte == 0xC && secondByte == 0x4) //START or RESTART
                || (firstByte == 0x4 && secondByte == 0xC) //STOP
                || (firstByte == 0x0 && secondByte == 0x4) //0 OR ACK
                || (firstByte == 0x8 && secondByte == 0xC) //1 or NAK
                );

        //Serial.printf("rawidx %d: Considering %x and %x: validity = %d\n",
            //rawidx, firstByte, secondByte, validpair);
        if (validpair)
        {
            //save valid bytes to valid_bytes array
            validdata[valididx] = firstByte;
            validdata[valididx + 1] = secondByte;
            numvalid += 2;
            //Serial.printf("Added %x & %x at idx = %d & %d\n", firstByte, secondByte, valididx, valididx + 1);
            //PrintNextArrayBytes(validdata,0,numvalid);
            rawidx += 2;
            valididx += 2;
        }
        else
        {
            rawidx++; //on invalid, just go to next byte
        }
    }

    return numvalid;
}
#pragma endregion Support Functions

#ifdef MPU6050_SPECIFIC

//#pragma region YAW_COMPUTATIONS
////01/18/2020: I copied these functions from MPU6050_6Axis_MotionApps_V6_12.h and
////  modified them to be called directly instead of from an 'mpu' object
//    
uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet) 
{
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    if (packet != 0) 
    {
        data[0] = ((packet[0] << 8) | packet[1]);
        data[1] = ((packet[4] << 8) | packet[5]);
        data[2] = ((packet[8] << 8) | packet[9]);
        data[3] = ((packet[12] << 8) | packet[13]);
    }
    return 0;
}

uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet) 
{
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    int16_t qI[4]{ 0,0,0,0 };
    uint8_t status = dmpGetQuaternion(qI, packet);
    if (status == 0) {
        q->w = (float)qI[0] / 16384.0f;
        q->x = (float)qI[1] / 16384.0f;
        q->y = (float)qI[2] / 16384.0f;
        q->z = (float)qI[3] / 16384.0f;
        return 0;
    }
    return status; // int16 return value, indicates error if this line is reached
}

uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity) 
{
    // yaw: (about Z axis)
    data[0] = atan2(2 * q->x * q->y - 2 * q->w * q->z, 2 * q->w * q->w + 2 * q->x * q->x - 1);
    // pitch: (nose up/down, about Y axis)
    data[1] = atan2(gravity->x, sqrt(gravity->y * gravity->y + gravity->z * gravity->z));
    // roll: (tilt left/right, about X axis)
    data[2] = atan2(gravity->y, gravity->z);
    if (gravity->z < 0) {
        if (data[1] > 0) {
            data[1] = PI - data[1];
        }
        else {
            data[1] = -PI - data[1];
        }
    }
    return 0;
}


uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q) 
{
    v->x = 2 * (q->x * q->z - q->w * q->y);
    v->y = 2 * (q->w * q->x + q->y * q->z);
    v->z = q->w * q->w - q->x * q->x - q->y * q->y + q->z * q->z;
    return 0;
}
//#pragma endregion YAW_COMPUTATIONS
#endif //MPU6050_SPECIFIC

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

Name: Teensy_I2C_Sniffer_V11.ino

Created: 1/18/2020 10:55:55 AM

Author: FRANKNEWXPS15\Frank

/* 'Notes:

A typical I2C sentence when communicating with a MPU6050 IMU module goes something like:

"I2C(68) wrote 1 byte to 75 - C0 Done."

"I2C(68) wrote 3 bytes to 72 - C0 0C 10 Done."

"I2C(68) read 5 bytes from 6A - C0 0C 10 14 03 Done."

To form a sentence, we need:

Device addr: 68 in the above examples

Read/Write direction

To/From register address: 75, 72 and 6A in the above examples

Data: C0, C0 0C 10, and C0 0C 10 14 03 in the above examples

number of bytes written/read: 1,3 & 5 in the above examples

Each I2C communication proceeds as follows (assuming a START from an IDLE condition):

A START or RESTART condition, denoted by SDA & SCL HIGH, followed by SDA LOW, SCL HIGH

A 7-bit device address, MSB first (0x8/0xC = 1, 0x0/0x4 = 0)

A R/W bit (0x8/0xC = read, 0x0/0x4 = write)

An ACK bit (0x8/0xC = NAK, 0x0/0x4 = ACK)

If the bus direction is WRITE, then

A register address for read/write

zero or more additional data bytes

Else (the bus direction is READ)

One or more additional data bytes

Endif

This version uses a fixed-size (2048 bytes) array instead of tonton81's circular buffer library.

To generalize for any I2C slave device rather than just the MPU6050 IMU, comment out the

"#define MPU6050_SPECIFIC line below. This will remove all MPU6050 specific code

//#define MPU6050_SPECIFIC

#include <TimerOne.h> //needed for ISR

#ifdef MPU6050_SPECIFIC

#include "helper_3dmath.h" //Arduino\Libraries\i2cdevlib\Arduino\MPU6050\ needed to compute yaw from MPU6050 DMP packet

#endif

#define PARSE_LOOP_DEBUG

const uint16_t CAPTURE_ARRAY_SIZE = 2048;

const uint16_t VALID_DATA_ARRAY_SIZE = 2048;

const int WAITING_PRINT_INTERVAL_MSEC = 200;//interval timer for 'Waiting for data...' printout

#define MONITOR_OUT1 2 //so can monitor ISR activity with O'scope

#define MONITOR_OUT2 3 //so can monitor ISR activity with O'scope

#define MONITOR_OUT3 4 //so can monitor ISR activity with O'scope

#define SDA_PIN 18

#define SCL_PIN 19

#pragma region PROCESSING_VARIABLES

uint8_t devAddr;

uint8_t regAddr;

uint8_t databytes[2048]; //holds multiple databytes for later output sentence construction

uint16_t numbytes = 0; //number of data bytes extracted from data stream

int ACKNAKFlag; //can be negative

uint16_t databyte_idx = 0; //index into databyte_array

uint8_t killbuff[2]; //used to consume start/stop bytes

elapsedMillis mSecSinceLastWaitingPrint;

uint8_t valid_data[2048];

uint16_t numvalidbytes = 0; //number of valid bytes in this burst

uint16_t read_idx = 0; //pointer to next byte pair to be processed

//added for bus direction labels

enum BUSDIR

{

WRITE,

READ,

UNKNOWN = -1

} RWDir;

BUSDIR BusDir = BUSDIR::UNKNOWN;

#pragma endregion ProcVars

#pragma region ISR_SUPPORT

uint8_t raw_data[CAPTURE_ARRAY_SIZE]; //holds data captured from I2C bus

volatile uint16_t write_idx = 0;

volatile uint8_t current_portb = 0xFF;

volatile uint8_t last_portb = 0xFF;

volatile uint16_t mult0xCCount = 0;

const uint16_t MAX_IDLE_COUNT = 2500;

volatile bool bDone = false;

volatile bool bWaitingForStart = true;

volatile bool bIsData = true;

volatile bool bIsStart = false;

volatile bool bIsStop = false;

volatile uint8_t last_current;

#pragma endregion ISR Support

#ifdef MPU6050_SPECIFIC

//01/21/20 these forward declarations are required to force the preprocessor

//to handle #ifdef MPU6050_SPECIFIC properly

uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet);

uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet);

uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity);

uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q);

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 ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

float global_yawval = 0; //updated by GetIMUHeadingDeg()

#endif // MPU6050_SPECIFIC

void setup()

{

unsigned long now = millis();

Serial.begin(1); //rate value ignored

int idx = 0;

while (!Serial && (millis() - now) < 3000)

{

delay(500);

idx++;

}

Serial.printf("Serial available after %lu mSec\n", millis() - now);

pinMode(MONITOR_OUT1, OUTPUT);

digitalWrite(MONITOR_OUT1, LOW);

pinMode(MONITOR_OUT2, OUTPUT);

digitalWrite(MONITOR_OUT2, LOW);

pinMode(MONITOR_OUT3, OUTPUT);

digitalWrite(MONITOR_OUT3, LOW);

pinMode(SCL_PIN, INPUT);

pinMode(SDA_PIN, INPUT);

//reset port byte vars & start timer

last_portb = current_portb = 0;

write_idx = 0;

memset(raw_data, 255, CAPTURE_ARRAY_SIZE);

//PrintNextArrayBytes(raw_data, 255, 20);

Timer1.initialize(1); // run every mico second

Timer1.attachInterrupt(capture_data);

mSecSinceLastWaitingPrint = 0;

}

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

//-------------------------------- ISR ------------------------------------

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

FASTRUN void capture_data()

//void capture_data()

{

last_portb = current_portb;

current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

if (!bDone && last_portb != current_portb)

{

mult0xCCount = 0; //reset IDLE counter

digitalWriteFast(MONITOR_OUT1, HIGH);

//01/17/20: joepasquariello suggestion

last_current = (last_portb << 4) | (current_portb);

bIsStart = (last_current == 0xC4);

bIsStop = (last_current == 0x4C);

bIsData = (last_current == 0x04) || (last_current == 0x8C);

if (bIsStart) //START

{

digitalWriteFast(MONITOR_OUT2, HIGH);

if (bWaitingForStart)

{

digitalWriteFast(MONITOR_OUT3, HIGH); //start of entire capture

bWaitingForStart = false;

}

else if (bIsStop) //STOP

{

digitalWriteFast(MONITOR_OUT2, LOW);

}

if (!bWaitingForStart && (bIsData || bIsStart || bIsStop))

{

//digitalWriteFast(MONITOR_OUT3, HIGH);

raw_data[write_idx] = last_portb;

write_idx++;

raw_data[write_idx] = current_portb;

write_idx++;

if (write_idx >= CAPTURE_ARRAY_SIZE)

{

bDone = true;

digitalWriteFast(MONITOR_OUT3, LOW);

}

digitalWriteFast(MONITOR_OUT1, LOW);

}

else if (!bDone && mult0xCCount < MAX_IDLE_COUNT && last_portb == 0xc && current_portb == 0xc)

{

mult0xCCount++;

if (mult0xCCount >= MAX_IDLE_COUNT)

{

digitalWriteFast(MONITOR_OUT3, LOW);

bDone = true;

}

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

//-------------------------------- END ISR ---------------------------------

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

void loop()

{

if (bDone)

{

if (write_idx > 14)

{

//OK, we have some data to process. IDLE detection must have been EOM

Timer1.stop();

unsigned long startMsec = millis();

//Serial.printf("%lu\t %d\t", millis(), write_idx);

//PrintNextArrayBytes(raw_data, 0, 50);

//Serial.printf(" - %lu\n", millis());

uint16_t numprocessed = DecodeAndPrintValidData(raw_data); //decode and print everything captured so far

unsigned long endMsec = millis();

Serial.printf("%lu: processed = %d elements in %lu mSec\n\n", startMsec, numprocessed, endMsec-startMsec);

Timer1.start();

}

read_idx = 0;

bDone = false;

mult0xCCount = 0;

write_idx = 0;

bWaitingForStart = true;

}

else

{

//no data to process, but don't blow prints out every mSec...

if (mSecSinceLastWaitingPrint > WAITING_PRINT_INTERVAL_MSEC)

{

mSecSinceLastWaitingPrint -= WAITING_PRINT_INTERVAL_MSEC;

Serial.printf("%lu: Waiting for Data...\n", millis());

}

void PrintNextArrayBytes(uint8_t* data, uint16_t startidx, uint16_t numbytes)

{

Serial.printf("%d bytes starting at %d: ", numbytes, startidx);

for (uint16_t i = 0; i < numbytes; i++)

{

Serial.printf("%x ", data[i + startidx]);

}

uint16_t DecodeAndPrintValidData(byte* data)

{

//Purpose: decode and print I2C conversation held in raw_data array

//Inputs:

// cb = 2048 element FIFO

//Outputs:

// returns number of bytes processed, or -1 for failure

// outputs structured I2C sentence to serial monitor

//Plan:

// Step1: Cull out invalid bytes

// Step2: Determine if there is anything to do (have to have more than one transition in FIFO)

// Step3: Parse transitions into I2C sentence structure

// Step4: Output sentence to serial monitor

memset(valid_data, 0, VALID_DATA_ARRAY_SIZE);

#ifdef PARSE_LOOP_DEBUG

PrintNextArrayBytes(valid_data, 0, 20); //print out first 20 bytes for verification

#endif

numvalidbytes = RemoveInvalidBytes(raw_data, valid_data);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Removed %d invalid bytes, leaving %d remaining\n", write_idx + 1 - numvalidbytes, numvalidbytes);

PrintNextArrayBytes(valid_data, 0, numvalidbytes); //print out first 20 bytes of valid_data array

#endif

if (numvalidbytes < 2)

{

return 0;

}

while (read_idx < numvalidbytes)

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("At top of while (read_idx < numvalidbytes): read_idx = %d\n", read_idx);

Serial.printf("Next two bytes in valid_data are %x, %x\n", valid_data[read_idx], valid_data[read_idx + 1]);

#endif

//Find a START sequence (0xC followed by 0x4)

while (!IsStart(valid_data, read_idx) && read_idx < numvalidbytes)

{

//Serial.printf("looking for start...\n");

read_idx++;

}

//at this point, read_idx should point to next valid byte pair

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Start sequence found at %d\n", read_idx - 2);

//PrintNextFIFOBytes(valid_data, 20);

#endif

if (numvalidbytes - read_idx > 14)//14 entries required for 7-bit address

{

//Get 7-bit device address

devAddr = Get7BitDeviceAddr(valid_data, read_idx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("devAddr = %x\n", devAddr);

#endif

}

else

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("ran out of data at readidx = %d - exiting!\n", read_idx);

#endif

break;

}

//get read/write flag 1 = Read, 0 = Write, -1 = error

BusDir = (BUSDIR)GetReadWriteFlag(valid_data, read_idx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("BusDir = %s\n", ((BusDir == BUSDIR::WRITE) ? "WRITE" : "READ"));

//PrintNextFIFOBytes(valid_data, 20);

#endif

//get ACK/NAK flag

ACKNAKFlag = GetACKNAKFlag(valid_data, read_idx);

numbytes = GetDataBytes(valid_data, read_idx, databytes); //terminates on a START, but the start bytes are not consumed

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Got %d bytes from GetDataBytes() --> ", numbytes);

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

{

Serial.printf(" %x ", databytes[i]);

}

Serial.printf("\n");

//PrintNextFIFOBytes(cb_trans, 20);

#endif

//If the bus direction is WRITE, then extract

// A register address for read / write

// zero or more additional data bytes

if (BusDir == BUSDIR::WRITE)

{

regAddr = databytes[0];

#ifdef PARSE_LOOP_DEBUG

Serial.printf("regAddr = %x, read_idx = %d\n", regAddr, read_idx);

#endif

//check for additional data

if (numbytes > 1)

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Additional data found!\n");

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

{

Serial.printf("data[%d] = %x\n", i, databytes[i]);

}

#endif

//1st byte is register addr, subsequent bytes are data

OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databytes, 1);

}

else //all bytes are data

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("In data block: got %d bytes of data\n", numbytes);

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

{

Serial.printf("data[%d] = %x\n", i, databytes[i]);

}

#endif

OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databytes, 0);

}

#ifdef PARSE_LOOP_DEBUG

Serial.printf("At end of while (read_idx < numvalidbytes): read_idx = %d\n", read_idx);

#endif

}//while (read_idx < numvalidbytes)

return numvalidbytes;

}

#pragma region Support Functions

bool IsStart(byte* data, uint16_t& readidx)

{

bool result = false;

//Serial.printf("IsStart[%d] = %x, IsStart[%d] = %x\n",

// readidx, data[readidx], readidx + 1, data[readidx + 1]);

if (data[readidx] == 0xC && data[readidx + 1] == 0x4)

{

result = true;

readidx += 2; //bump to next byte pair

}

return result;

}

bool IsStop(byte* data, uint16_t& readidx)

{

bool result = false;

//Serial.printf("IsStop[%d] = %x, IsStop[%d] = %x\n",

//readidx, data[readidx], readidx + 1, data[readidx + 1]);

if (data[readidx] == 0x4 && data[readidx + 1] == 0xC)

{

result = true;

readidx += 2; //bump to next byte pair

}

return result;

}

uint8_t Get7BitDeviceAddr(byte* data, uint16_t& readidx)

{

//Purpose: Construct a 7-bit address starting from dataidx

//Inputs:

// data = pointer to valid data array

// readidx = starting index of 7-bit address sequence (MSB first)

//Outputs:

// returns the address as an 8-bit value with the MSB = 0, or 0x0 if unsuccessful

// dataidx = pointer to next data entry

//Plan:

// Step1: Convert a pair of data entries into a 0 or 1

// Step2: Add the appropriate value to an ongoing sum

// Step3: return the total.

//Notes:

// A '0' is coded as a 0x0 followed by a 0x4

// A '1' is coded as a 0x8 followed by a 0xC

uint8_t devAddr = 0x0; //failure return value

//Serial.printf("Get7BitDeviceAddr: readidx = %d\n",readidx);

//devAddr is exactly 7 bits long, so 8 bits with MSB = 0

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

{

if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)

{

readidx += 2; //advance the pointer, but don't add to sum

}

else if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)

{

//Serial.printf("Get7BitDeviceAddr: '1' found at i = %d, adding %x to devAddr to get %x\n",

// i, 1 << (7 - i), devAddr + (1 << (7-i)));

readidx += 2; //advance the pointer

devAddr += (1 << (7 - i)); //add 2^(7-i) to sum

}

devAddr = devAddr >> 1; //divide result by 2 to get 7-bit addr from 8 bits

return devAddr;

}

int Get8BitDataByte(byte* data, uint16_t& readidx)

{

//Purpose: Construct a 8-bit data byte starting from dataidx

//Inputs:

// data = pointer to valid data array

// readidx = starting index of 8-bit data byte (MSB first)

//Outputs:

// returns the address as an 8-bit value, or 0x0 if unsuccessful

// dataidx = pointer to next data entry

//Plan:

// Step1: Convert a pair of data entries into a 0 or 1

// Step2: Add the appropriate value to an ongoing sum

// Step3: return the total.

//Notes:

// A '0' is coded as a 0x0 followed by a 0x4

// A '1' is coded as a 0x8 followed by a 0xC

// 12/29/19 - changed return val to int, so can return -1 when a 'short byte' is detected

int dataval = 0x0; //failure return value

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: data[%d] = %x, data[%d] = %x\n",

readidx, data[readidx], readidx + 1, data[readidx + 1]);

#endif

//8 bits with MSB = 0

int numbytes = 0;

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

{

if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)

{

readidx += 2; //advance the pointer, but don't add to sum

numbytes++;

}

else if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)

{

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: '1' found at i = %d, adding %x to devAddr to get %x\n",

i, 1 << (7 - i), dataval + (1 << (7 - i)));

#endif

readidx += 2; //advance the pointer

dataval += (1 << (7 - i)); //add 2^(8-i) to sum

numbytes++;

}

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: numbytes = %d\n", numbytes);

#endif

if (numbytes != 8)

{

dataval = -1; //error return value

}

return dataval;

}

int GetReadWriteFlag(byte* data, uint16_t& readidx)

{

//Purpose: decode R/W byte pair

//Inputs:

// data = pointer to valid data array

// readidx = index into data to start of R/W byte pair

//Outputs:

// readidx = if successful, points to next byte pair in data

// returns 1 for Read (0x8/0xC), 0 for Write (0x0/0x4), -1 for failure

//Notes:

//Serial.printf("GetReadWriteFlag: readidx = %d, data[readidx] = %x, data[readidx+1]= %x\n",

// readidx, data[readidx], data[readidx + 1]);

int result = 0;

if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)

{

result = 1; //read detected

readidx += 2; //point to next byte pair

}

else if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)

{

result = 0; //write detected

readidx += 2; //point to next byte pair

}

else

{

result = -1; //failed to detect read or write

}

return result;

}

int GetACKNAKFlag(byte* data, uint16_t& readidx)

{

//Purpose: decode ACK/NAK byte pair

//Inputs:

// data = pointer to valid data array

// readidx = index into data to start of ACK/NAK byte pair

//Outputs:

// readidx = if successful, points to next byte pair in data

// returns 1 for NAK (0x8/0xC), 0 for ACK (0x0/0x4), -1 for failure

//Notes:

//Serial.printf("GetACKNAKFlag: readidx = %d, data[readidx] = %x, data[readidx+1]= %x\n",

// readidx, data[readidx], data[readidx + 1]);

int result = 0;

if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)

{

result = 1; //NAK detected

readidx += 2; //point to next byte pair

}

else if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)

{

result = 0; //ACK detected

readidx += 2; //point to next byte pair

}

else

{

result = -1; //failed to detect ACK or NAK

}

return result;

}

int GetDataBytes(uint8_t* data, uint16_t& readidx, uint8_t* databytes)

{

//Notes:

// 01/01/2020: removed databyteidx from sig - always starts at zero

uint16_t numbytes = 0;

uint16_t databyte_idx = 0;

bool StartFlag = false;

bool StopFlag = false;

{

int dataval = Get8BitDataByte(data, readidx);

//watch out for 'short byte' reads

if (dataval >= 0)

{

uint8_t databyte = (uint8_t)dataval;

databytes[databyte_idx] = databyte;

databyte_idx++;

numbytes++;

}

ACKNAKFlag = GetACKNAKFlag(data, readidx);

StartFlag = IsStart(data, readidx);

StopFlag = IsStop(data, readidx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("IsStart returned %d, IsStop returned %d, dataidx = %d\n",

StartFlag, StopFlag, readidx);

#endif

} while (!StartFlag && !StopFlag && readidx < numvalidbytes);

readidx -= 2;//back readidx up so loop top is positioned correctly.

return numbytes;

}

void OutputFormattedSentence(int RW, uint8_t dev, uint8_t reg, uint8_t numbytes, uint8_t* bytearray, uint16_t startidx)

{

Serial.printf("%lu I2C(%x) %s %d bytes %s %x... ",

millis(), dev, (RW == 0 ? "writing" : "reading"), numbytes - startidx, (RW == 0 ? "to" : "from"), reg);

//for (size_t i = startidx; i < numbytes; i++)

//{

// Serial.printf("%x ", bytearray[i]);

//}

//Serial.printf(". Done\n");

#ifdef MPU6050_SPECIFIC

//01/18/20 experiment to decode 28-byte packet into yaw value

if (numbytes == 28)

{

dmpGetQuaternion(&q, bytearray);

dmpGetGravity(&gravity, &q);

dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

global_yawval = ypr[0] * 180 / M_PI;

Serial.printf("yawval = %3.2f\n", global_yawval);

}

else

{

Serial.printf(". Done\n");

}

#endif // MPU6050_SPECIFIC

}

uint16_t RemoveInvalidBytes(uint8_t* rawdata, uint8_t* validdata)

{

uint16_t numvalid = 0;

uint16_t valididx = 0;

//Serial.printf("raw data array contains %d bytes\n", write_idx + 1);

//PrintNextArrayBytes(raw_data, 0, 20);

//OK, now go back through the array, excising invalid sequences

for (uint16_t rawidx = 0; rawidx < write_idx;/*rawidx incremented internally*/)

{

uint8_t firstByte = raw_data[rawidx]; //get the first byte

uint8_t secondByte = raw_data[rawidx + 1]; //get the next byte

bool validpair =

(

(firstByte == 0xC && secondByte == 0x4) //START or RESTART

|| (firstByte == 0x4 && secondByte == 0xC) //STOP

|| (firstByte == 0x0 && secondByte == 0x4) //0 OR ACK

|| (firstByte == 0x8 && secondByte == 0xC) //1 or NAK

);

//Serial.printf("rawidx %d: Considering %x and %x: validity = %d\n",

//rawidx, firstByte, secondByte, validpair);

if (validpair)

{

//save valid bytes to valid_bytes array

validdata[valididx] = firstByte;

validdata[valididx + 1] = secondByte;

numvalid += 2;

//Serial.printf("Added %x & %x at idx = %d & %d\n", firstByte, secondByte, valididx, valididx + 1);

//PrintNextArrayBytes(validdata,0,numvalid);

rawidx += 2;

valididx += 2;

}

else

{

rawidx++; //on invalid, just go to next byte

}

return numvalid;

}

#pragma endregion Support Functions

#ifdef MPU6050_SPECIFIC

//#pragma region YAW_COMPUTATIONS

////01/18/2020: I copied these functions from MPU6050_6Axis_MotionApps_V6_12.h and

//// modified them to be called directly instead of from an 'mpu' object

uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet)

{

// TODO: accommodate different arrangements of sent data (ONLY default supported now)

if (packet != 0)

{

data[0] = ((packet[0] << 8) | packet[1]);

data[1] = ((packet[4] << 8) | packet[5]);

data[2] = ((packet[8] << 8) | packet[9]);

data[3] = ((packet[12] << 8) | packet[13]);

}

return 0;

}

uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet)

{

// TODO: accommodate different arrangements of sent data (ONLY default supported now)

int16_t qI[4]{ 0,0,0,0 };

uint8_t status = dmpGetQuaternion(qI, packet);

if (status == 0) {

q->w = (float)qI[0] / 16384.0f;

q->x = (float)qI[1] / 16384.0f;

q->y = (float)qI[2] / 16384.0f;

q->z = (float)qI[3] / 16384.0f;

return 0;

}

return status; // int16 return value, indicates error if this line is reached

}

uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity)

{

// yaw: (about Z axis)

data[0] = atan2(2 * q->x * q->y - 2 * q->w * q->z, 2 * q->w * q->w + 2 * q->x * q->x - 1);

// pitch: (nose up/down, about Y axis)

data[1] = atan2(gravity->x, sqrt(gravity->y * gravity->y + gravity->z * gravity->z));

// roll: (tilt left/right, about X axis)

data[2] = atan2(gravity->y, gravity->z);

if (gravity->z < 0) {

if (data[1] > 0) {

data[1] = PI - data[1];

}

else {

data[1] = -PI - data[1];

}

return 0;

}

uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q)

{

v->x = 2 * (q->x * q->z - q->w * q->y);

v->y = 2 * (q->w * q->x + q->y * q->z);

v->z = q->w * q->w - q->x * q->x - q->y * q->y + q->z * q->z;

return 0;

}

//#pragma endregion YAW_COMPUTATIONS

#endif //MPU6050_SPECIFIC

With this setup with the master sending data to the slave, I got the following outputs:

Master

Sending to Slave: 1048.005615 (0x4483002e)
i = 0: I2C_WriteAnything() sending 2E
i = 1: I2C_WriteAnything() sending 0
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK
Sending to Slave: 1049.239990 (0x448327ae)
i = 0: I2C_WriteAnything() sending AE
i = 1: I2C_WriteAnything() sending 27
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK
Sending to Slave: 1050.474487 (0x44834f2f)
i = 0: I2C_WriteAnything() sending 2F
i = 1: I2C_WriteAnything() sending 4F
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK
Sending to Slave: 1051.708862 (0x448376af)
i = 0: I2C_WriteAnything() sending AF
i = 1: I2C_WriteAnything() sending 76
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK
Sending to Slave: 1052.943237 (0x44839e2f)
i = 0: I2C_WriteAnything() sending 2F
i = 1: I2C_WriteAnything() sending 9E
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK

Sending to Slave: 1048.005615 (0x4483002e)

i = 0: I2C_WriteAnything() sending 2E

i = 1: I2C_WriteAnything() sending 0

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Sending to Slave: 1049.239990 (0x448327ae)

i = 0: I2C_WriteAnything() sending AE

i = 1: I2C_WriteAnything() sending 27

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Sending to Slave: 1050.474487 (0x44834f2f)

i = 0: I2C_WriteAnything() sending 2F

i = 1: I2C_WriteAnything() sending 4F

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Sending to Slave: 1051.708862 (0x448376af)

i = 0: I2C_WriteAnything() sending AF

i = 1: I2C_WriteAnything() sending 76

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Sending to Slave: 1052.943237 (0x44839e2f)

i = 0: I2C_WriteAnything() sending 2F

i = 1: I2C_WriteAnything() sending 9E

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Slave:

Slave received: 1048.005615
Slave received: 1049.239990
Slave received: 1050.474487
Slave received: 1051.708862
Slave received: 1052.943237

Slave received: 1048.005615

Slave received: 1049.239990

Slave received: 1050.474487

Slave received: 1051.708862

Slave received: 1052.943237

I2C Sniffer:

Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  2e  0  83  44 
Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  ae  27  83  44 
Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  2f  4f  83  44 
Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  af  76  83  44 
Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  2f  9e  83  44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> 2e 0 83 44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> ae 27 83 44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> 2f 4f 83 44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> af 76 83 44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> 2f 9e 83 44

In the other direction, with the slave sending data to the master, I got the following:

Slave:

Sending 28398426.000000 to Master
Sending 28398426.000000 to Master
Sending 28398426.000000 to Master
Sending 28398426.000000 to Master

Sending 28398426.000000 to Master

Master:

Reading from Slave: Received 28398426.000000 from Slave
Reading from Slave: Received 28398426.000000 from Slave
Reading from Slave: Received 28398426.000000 from Slave

Reading from Slave: Received 28398426.000000 from Slave

I2C Sniffer:

In data block:  got 112 bytes of data
data[0] = ad
data[1] = a9
data[2] = d8
data[3] = 4b
data[4] = 0
data[5] = 0
....
In data block:  got 112 bytes of data
data[0] = ad
data[1] = a9
data[2] = d8
data[3] = 4b
data[4] = 0
data[5] = 0
.....
In data block:  got 112 bytes of data
data[0] = ad
data[1] = a9
data[2] = d8
data[3] = 4b
data[4] = 0
data[5] = 0

In data block: got 112 bytes of data

data[0] = ad

data[1] = a9

data[2] = d8

data[3] = 4b

data[4] = 0

data[5] = 0

....

In data block: got 112 bytes of data

data[0] = ad

data[1] = a9

data[2] = d8

data[3] = 4b

data[4] = 0

data[5] = 0

.....

In data block: got 112 bytes of data

data[0] = ad

data[1] = a9

data[2] = d8

data[3] = 4b

data[4] = 0

data[5] = 0

Where the HEX sequence 4B D8 A9 AD converted to a IEEE float = 2.83984e+07

So, the Teensy-to-Teensy I2C connection is clearly working in both directions, and the Teensy I2C Sniffer is successfully capturing and decoding the I2C traffic between the two modules – cool!

So, the next step is to replace the ‘Master’ Teensy with an Arduino Mega and repeat the process. Hopefully this will allow me to figure out why the slave-to-master data transfer isn’t working

21 March 2020 Update:

Based on the above results, I formed a hypothesis that the problem with sending I2C data from a Teensy slave to an Arduino Mega master might be due to the Mega not being able to properly interpret 0-3.3V transitions from the Teensy. So, I decided to construct a low-to-high level converter to make sure that SDA data from the Teensy was presented to the Mega as 0-5V transitions rather than the Teensy’s ‘raw’ 0-3.3V ones. To do this I used the circuit shown below, except with a 1K instead of 10K resistor for R2 to clean up the transitions at 100KHz.

Bidirectional 3.3-5V level shifter using 2N7000 MosFet

Here’s a scope photo of the 3.3V input to and the 5V output from the 2N7000-based level shifter. The top trace is the Teensy 0-3.3V output, and the bottom trace is the 0-5V level-shifted version. Both traces are 1V/cm, and the horizontal time scale is 20 uSec/cm.

Teensy slave, Mega master: Top: 0-3.3V input. Bottom: 0-5V output. Both traces are 1V/cm vertical, 20 uSec cm horizontally

To verify that the above circuit worked properly, I used it in the SDA line (the SCL line doesn’t require any level shifting as it is from the 5V Mega to the 3.3V Teensy) with a Teensy slave and a Teensy master. This is OK, as Teensy 3.x’s can handle 5V inputs, and it worked properly in both directions. Here’s a shot of the SDA line

Teensy slave, Teensy master: Top: 0-3.3V input. Bottom: 0-5V output. Both traces are 1V/cm vertical, 20 uSec cm horizontally

However, after replacing the Teensy master with the Mega one, I had the same problem as before – I can successfully transfer data from Mega master to Teensy slave, but not in the other direction. There is still a problem somewhere, and I no longer think it’s due to level-shifting problems. From the above scope photos, it is clear that only a few bytes are transmitted by the Teensy slave each time it services the OnRequest() interrupt, while that same function transmits MUCH more data when servicing the same request from the Teensy 3.5 master.

Stay tuned!

23 March 2020 Update:

OK, back to the basics: I downloaded the code for a very basic Arduino-Arduino I2C tutorial, and verified that it worked OK. I also took a scope shot of SDA/SCL activity during the two-way data transfers, as shown below:

Basic Arduino-Arduino tutorial layout

I2C line activity for the ‘Basic Arduino – Arduino I2C Tutorial’

So now that I have a working baseline for the Arduino-Arduino I2C case, I plan to incrementally modify it toward duplicating the non-working Arduino-Arduino I2C case until it breaks, and then I’ll know that the last incremental modification is the culprit. At the moment, I’m leaning toward the use of the I2CDev & SBWIRE libraries as they are the only significant difference between the working and non-working setups. We’ll see….

24 March 2020 Update:

I created a new Arduino project called ‘I2C_Master_Tut_Mod1’ initially identical to ‘I2C_Master_Tutorial’ and verified that it worked properly with the unmodified ‘I2C_Slave_Tutorial’ project, and then started modifying it.

Added #include <PrintEx.h> //allows printf-style printout syntax
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing, and changed all occurrences of ‘Serial’ to ‘mySerial’ and modified print statements to ‘printf’ format. All worked fine.
Added #include <I2C_Anything.h> and changed loop() to write a float value to the slave. However, this caused a compile error. When I started tracking it down I realized that I had previously modified ‘I2C_Anything.h’ to #include SBWIRE.h rather than the original #include Wire.h. So, this could be the culprit, assuming that there is something about SBWIRE that causes Arduino I2C slaves to misbehave.
Copied I2C_Anything.h/cpp from the Libraries folder to my new ‘I2C_Master_Tut_Mod1’ folder, and un-modified it to reference back Wire.h vs SBWIRE.h.

25 March 2020 Update:

As it turns out, I was never able to get the ‘I2C_Master_Tut_Mod1’ project working with #include <I2C_Anything.h> , no matter what I did, including copying the #include <I2C_Anything.h> to the local folder and including it as a resource int the VS2019 project, build cleans, removing the _vm folder created by Visual Micro, etc. To make it even stranger, I created a new Arduino project in VS2019 called ‘I2C_Master_Tut_Mod2’, copied the ‘I2C_Anything.h’ file to the local folder, and it compiles without error! So now I have two almost completely identical Arduino projects, one of which compiles fine and the other of which blows a bunch of errors about twi.c. I’m currently working with Tim Leek at VisualMicro to sort out what I did wrong.

27 March 2020 Update:

Ok, I now have the Arduino Mega master to Arduino Uno slave setup working, with I2C_Anything. I can transfer a float value in either direction with no problem. The master and slave code sets and the corresponding outputs are shown below.

Master Sketch:

/*
    Name:       I2C_Master_Tut_Mod2.ino
    Created:	3/25/2020 8:54:55 AM
    Author:     FRANKNEWXPS15\Frank
*/


/*
    Name:       I2C_Master_Tutorial.ino
    Created:	3/23/2020 1:20:08 PM
    Author:     FRANKNEWXPS15\Frank
*/
/*
  I2C Master Demo
  i2c-master-demo.ino
  Demonstrate use of I2C bus
  Master sends character and gets reply from Slave
  DroneBot Workshop 2019
  https://dronebotworkshop.com
*/

// Include Arduino Wire library for I2C
#include <Wire.h>
#include "PrintEx.h"
#include "I2C_Anything.h"
// Define Slave I2C Address
#define SLAVE_ADDR 9
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

// Define Slave answer size
#define ANSWERSIZE 5
const float sendval = 3.14159f;
float rcvval = 0;

void setup() {

    // Initialize I2C communications as Master
    Wire.begin();

    // Setup serial monitor
    Serial.begin(115200);
    Serial.println("I2C Master Demonstration");
}

void loop() {
    delay(50);
    //Serial.println("Write data to slave");
    mySerial.printf("Sent %3.5f: ",sendval);

    // Write a charatre to the Slave
    Wire.beginTransmission(SLAVE_ADDR);
    //Wire.write(0);
    I2C_writeAnything(sendval);
    Wire.endTransmission();

    //Serial.println("Receive data");

    //// Read response from Slave
    //// Read back 5 characters
    //Wire.requestFrom(SLAVE_ADDR, ANSWERSIZE);

    //// Add characters to string
    //String response = "";
    //while (Wire.available()) {
    //    char b = Wire.read();
    //    response += b;
    //}

    //// Print to Serial Monitor
    //Serial.println(response);

    //03/26/2020 gfp rev to retrieve float value
    Wire.requestFrom(SLAVE_ADDR, sizeof(float));
    I2C_readAnything(rcvval);
    mySerial.printf("Received %4.1f\n",rcvval);


}

Name: I2C_Master_Tut_Mod2.ino

Created: 3/25/2020 8:54:55 AM

Author: FRANKNEWXPS15\Frank

Name: I2C_Master_Tutorial.ino

Created: 3/23/2020 1:20:08 PM

Author: FRANKNEWXPS15\Frank

I2C Master Demo

i2c-master-demo.ino

Demonstrate use of I2C bus

Master sends character and gets reply from Slave

DroneBot Workshop 2019

https://dronebotworkshop.com

// Include Arduino Wire library for I2C

#include <Wire.h>

#include "PrintEx.h"

#include "I2C_Anything.h"

// Define Slave I2C Address

#define SLAVE_ADDR 9

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

// Define Slave answer size

#define ANSWERSIZE 5

const float sendval = 3.14159f;

float rcvval = 0;

void setup() {

// Initialize I2C communications as Master

Wire.begin();

// Setup serial monitor

Serial.begin(115200);

Serial.println("I2C Master Demonstration");

}

void loop() {

delay(50);

//Serial.println("Write data to slave");

mySerial.printf("Sent %3.5f: ",sendval);

// Write a charatre to the Slave

Wire.beginTransmission(SLAVE_ADDR);

//Wire.write(0);

I2C_writeAnything(sendval);

Wire.endTransmission();

//Serial.println("Receive data");

//// Read response from Slave

//// Read back 5 characters

//Wire.requestFrom(SLAVE_ADDR, ANSWERSIZE);

//// Add characters to string

//String response = "";

//while (Wire.available()) {

// char b = Wire.read();

// response += b;

//}

//// Print to Serial Monitor

//Serial.println(response);

//03/26/2020 gfp rev to retrieve float value

Wire.requestFrom(SLAVE_ADDR, sizeof(float));

I2C_readAnything(rcvval);

mySerial.printf("Received %4.1f\n",rcvval);

}

Slave Sketch:

/*
    Name:       I2C_Slave_Tutorial.ino
    Created:	3/23/2020 1:23:40 PM
    Author:     FRANKNEWXPS15\Frank
*/


/*
  I2C Slave Demo
  i2c-slave-demo.ino
  Demonstrate use of I2C bus
  Slave receives character from Master and responds
  DroneBot Workshop 2019
  https://dronebotworkshop.com
*/

// Include Arduino Wire library for I2C
#include <Wire.h>
#include "PrintEx.h"
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

//03/26/2020 experiment with I2C_Anything
#include "I2C_Anything.h"
float rcvval = 0;
float sendval = 95141.3; //3.14159 written backwards


// Define Slave I2C Address
#define SLAVE_ADDR 9

// Define Slave answer size
#define ANSWERSIZE 5

// Define string with response to Master
String answer = "Hello";

void setup() {

    // Initialize I2C communications as Slave
    Wire.begin(SLAVE_ADDR);

    // Function to run when data requested from master
    Wire.onRequest(requestEvent);

    // Function to run when data received from master
    Wire.onReceive(receiveEvent);

    // Setup Serial Monitor 
    Serial.begin(115200);
    Serial.println("I2C Slave Demonstration");
}

void receiveEvent() {

    //Serial.println("Receive event");
    // Read while data received
    //while (0 < Wire.available()) {
    //    byte x = Wire.read();
    //}

    //03/26/2020 experiment with I2C_Anything
    I2C_readAnything(rcvval);

    // Print to Serial Monitor
    //Serial.print("Received "); Serial.print(rcvval, 6); Serial.println(" From Master");
    mySerial.printf("Received %3.6f: ", rcvval);
}

void requestEvent() {

    //// Setup byte variable in the correct size
    //byte response[ANSWERSIZE];

    //// Format answer as array
    //for (byte i = 0; i < ANSWERSIZE; i++) {
    //    response[i] = (byte)answer.charAt(i);
    //}

    //// Send response back to Master
    //Wire.write(response, sizeof(response));

    //// Print to Serial Monitor
    //Serial.println("Request event");

    //03/26/2020 experiment with I2C_Anything
    //Serial.print("Sending "); Serial.print(sendval, 6); Serial.println(" to Master");
    mySerial.printf("Sent %6.1f\n",sendval);
    I2C_writeAnything(sendval);
}

void loop() {

    // Time delay in loop
    delay(50);
}

Name: I2C_Slave_Tutorial.ino

Created: 3/23/2020 1:23:40 PM

Author: FRANKNEWXPS15\Frank

I2C Slave Demo

i2c-slave-demo.ino

Demonstrate use of I2C bus

Slave receives character from Master and responds

DroneBot Workshop 2019

https://dronebotworkshop.com

// Include Arduino Wire library for I2C

#include <Wire.h>

#include "PrintEx.h"

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

//03/26/2020 experiment with I2C_Anything

#include "I2C_Anything.h"

float rcvval = 0;

float sendval = 95141.3; //3.14159 written backwards

// Define Slave I2C Address

#define SLAVE_ADDR 9

// Define Slave answer size

#define ANSWERSIZE 5

// Define string with response to Master

String answer = "Hello";

void setup() {

// Initialize I2C communications as Slave

Wire.begin(SLAVE_ADDR);

// Function to run when data requested from master

Wire.onRequest(requestEvent);

// Function to run when data received from master

Wire.onReceive(receiveEvent);

// Setup Serial Monitor

Serial.begin(115200);

Serial.println("I2C Slave Demonstration");

}

void receiveEvent() {

//Serial.println("Receive event");

// Read while data received

//while (0 < Wire.available()) {

// byte x = Wire.read();

//}

//03/26/2020 experiment with I2C_Anything

I2C_readAnything(rcvval);

// Print to Serial Monitor

//Serial.print("Received "); Serial.print(rcvval, 6); Serial.println(" From Master");

mySerial.printf("Received %3.6f: ", rcvval);

}

void requestEvent() {

//// Setup byte variable in the correct size

//byte response[ANSWERSIZE];

//// Format answer as array

//for (byte i = 0; i < ANSWERSIZE; i++) {

// response[i] = (byte)answer.charAt(i);

//}

//// Send response back to Master

//Wire.write(response, sizeof(response));

//// Print to Serial Monitor

//Serial.println("Request event");

//03/26/2020 experiment with I2C_Anything

//Serial.print("Sending "); Serial.print(sendval, 6); Serial.println(" to Master");

mySerial.printf("Sent %6.1f\n",sendval);

I2C_writeAnything(sendval);

}

void loop() {

// Time delay in loop

delay(50);

}

Master & Slave Output:

Opening port
Port open
I2C Master Demonstration
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3


Opening port
Port open
I2C Slave Demonstration
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3

Opening port

Port open

I2C Master Demonstration

Sent 3.14159: Received 95141.3

Opening port

Port open

I2C Slave Demonstration

Received 3.141590: Sent 95141.3

So now I have a working Arduino-Arduino I2C master/slave setup, using I2C_Anything.h to transfer float values back and forth, and a working Teensy-Teensy I2C master/slave setup, using I2C_Anything.h to transfer float values back and forth.

The next step was to replace the Arduino Uno I2C slave with the Teensy 3.2 I2C slave and see if I can get that combination working. This turned out to be surprisingly easy – basically plug-and-pray (oops, I meant ‘play’).

Now that I have a simple Arduiono Mega Master – Teensy 3.2 Slave setup working, I can start to explore why my 4WD robot setup doesn’t work. Some possibilities:

It could be that the SBWire version of the Wire library has some hidden bugs in the slave-related code. The SBWire library eliminates the well-known and well-hated ‘hang’ bug in the Arduino wire library.
It could be that some other library I am using, like the RTC library or the Adafruit FRAM library is interfering with the Arduino-Teensy interrupts needed for master-slave communication.
It could be that the IR demod program running on the slave Teensy is somehow interfering with master-slave communications (this is very unlikely as this same program had been working flawlessly for several months).
Something else…

To start with, I copied the current ‘I2C_Master_Tut2’ VS2019 project to a new one – ‘I2C_Master_Tut3’ before making any modifications. I plan to keep a ‘breadcrumb trail’ of incrementally modified projects so that I can go backwards in case I get lost at some point (and, in my fairly long 50+ years as a practicing engineer, I have learned that ‘getting lost’ is part and parcel of any non-trivial troubleshooting project).

Once I verified that ‘Tut3’ properly communicated with the Teensy slave, I started making modifications.

Replaced #include <Wire.h> with #include SBWire.h. This failed in compile with a linker error, but succeeded after I did a ‘Build -> Clean’. After uploading the new ‘Tut3’ version to the Mega, I found that master-slave communications still worked properly. This is actually a welcome development, as that now eliminates SBWire as the culprit for lost Mega-Teensy I2C capability on the robot.
Replaced ‘#include “SBWire” with #include “MPU6050_6Axis_MotionApps_V6_12.h” and include “I2Cdev.h” (this includes SBWire.h). These two libraries are required for interfacing to the MPU6050 IMU module.

29 March 2020 Update:

So I created a new copy of the I2C master, ‘I2C_Master_Tut4’ and started adding things from the original non-working robot program.

Added all the remaining #include’s. Still works fine with the Teensy 3.2 slave
Added all the #defines. Still works fine
Added all the IRHOMING parameters. OK
Added all ENUMs, Battery constants, distance measurement support constants & array declarations, motor parameters, wheel direction constants and variables, and heading based turn parameters. No problem
Added pin assignments. No problem.
Added all the other pre-setup stuff. No problem
Added ‘ Serial1.begin(9600); //03/04/16 bugfix’. This isn’t actually used anymore in the robot project, but it is there, so it could be the culprit. Nope – master/slave comms still OK.
Added RTC initialization and support function. Still no problem
Added FRAM initialization. No problem.
Added MPU6050 initialization. No problem
Added PID distance array and incremental variance initialization. No problem
Added I/O pin initialization. No problem
Added the rest of Setup(), and all the support functions required for the POST check to run. Compiles and runs fine. Of course, no peripherals are attached, so not much happens.

At this point I have all the pre-setup and setup code incorporated into the master/slave example, and everything is still happily plugging away. So obviously the culprit hasn’t yet been identified. However, before going any farther, I think I’ll drop ‘Tut4’ into the safe-deposit box and create a ‘Tut5’ to continue on into the loop() function.

OK, so I have a ‘Tut5’ project now, with everything up to loop() incorporated from the ‘FourWD_WallE2_V2’ robot project. Now the question is, ‘What next?’. I need to figure out what is causing I2C comms between the Mega master and the Teensy 3.2 slave to fail, so I need a way to faithfully replicate/simulate/emulate the actual robot code for this function.

The current robot algorithm as pertaining to the Teensy IR Demod module

Each time through the loop, the current operating mode is determined.
- If IsIRBeamAvail() returns TRUE, the IR HOMING mode is activated
  - IsIRBeamAvail() gets three float values from the Teensy 3.2, and returns TRUE if the total of the first two values (Fin1 & Fin2) is above a set threshold. It is this function that is failing to communicate with the Teensy. More specifically, it is this function that is not successfully acquiring the three float values, and subsequently always returns FALSE.

So, it may be that all I have to do is to call IsIRBeamAvail() by itself, and modify the slave code to send back three floats as expected. If this works, then I’ll have to start suspecting the robot’s Teensy or the I2C wiring, or something else entirely.

2 April 2020 Update:

After some additional fumbling around with I2C_Anything and the PrintEx library printf() formats, I now have a working ‘IsIRBeamAvail()’ function in ‘Tut5’, as follows:

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

	//get latest info from IR Demod Module
	long Fin1 = 0; //03/30/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)
	long Fin2 = 23; //03/30/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)
	float SteeringValue;

	mySerial.printf("\n<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>\n\n");

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

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

	return false;
}

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

//get latest info from IR Demod Module

long Fin1 = 0; //03/30/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)

long Fin2 = 23; //03/30/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)

float SteeringValue;

mySerial.printf("\n<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>\n\n");

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

I2C_readAnything(Fin1);

I2C_readAnything(Fin2);

I2C_readAnything(SteeringValue);

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

return false;

}

Which, when connected to my basic Teensy 3.2 I2C slave program produces the following (correct) output:

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 469, Fin2 = 938, SteeringValue = 1473.41

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 470, Fin2 = 940, SteeringValue = 1476.55

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 471, Fin2 = 942, SteeringValue = 1479.69

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 469, Fin2 = 938, SteeringValue = 1473.41

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 470, Fin2 = 940, SteeringValue = 1476.55

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 471, Fin2 = 942, SteeringValue = 1479.69

At this point we have a working robot code emulation that communicates successfully with a basic Teensy 3.2 slave. So, the available culprits have been reduced significantly to

Something about the Teensy IR demod code
The I2C Wiring
A hardware problem at either the robot’s Mega controller or the robot’s Teensy 3.2 controller
Something else.

For the next step, I modified the Teensy IR Demod code to emulate the IR detector response and loaded this code into the Teensy 3.2 slave connected to the Mega master. After some initial mis-steps, it started working nicely, with the following (correct) output from the Mega master:

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

Port closed

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

Port closed

So this eliminates the ‘Something about the Teensy IR demod code’ possibility from the above list.

Next, I replaced the direct SCL/SDA jumpers with the daisy-chain wiring from the robot, and miracle of miracles, I2C comms failed – YAY!!. Now hopefully I can figure out (and fix) the problem.

First, I un-replaced the daisy-chain wiring to confirm that I2C comms were still working, and they were.
Unplugged the daisy-chain from the FRAM, 6050IMU, and RTC modules – now working OK
Plugged back in to FRAM unit (now have FRAM and Teensy 3.2 in chain)- – still OK
Plugged back in to 6050 IMU (now have IMU, FRAM, and Teensy 3.2 in chain)- failed
Plugged back in to RTC (now have RTC, FRAM, and Teensy 3.2 in chain)- failed

3 April 2020 Update:

The next step in the saga was to load the ‘Tut5’ code onto the robot’s Mega 2560 controller, with the slave code still running on the original (off-robot) Teensy 3.2. Setting things up in this fashion allowed the sensors to receive power from the robot’s Mega controller as in normal operation.

In the photo above, the Teensy 3.2 slave is shown to the left of the robot. The I2C ‘daisy-chain’ cable starts at the Mega controller (just to the right of the front left wheel), and goes through three ‘hops’ to the Teensy. With this setup, the I2C comms code still ran fine, with direct I2C jumper wires or with the daisy-chain wiring setup (with no connections to the other I2C sensors) as shown above.

The next step was to connect the robot’s Mega controller to the robot’s Teensy 3.2 running the unmodified IR Demodulation code, with the I2C daisy-chain cable, but without anything else connected. This actually worked! So now I have a working robot system again, so something about the connections with the other sensors must be killing the I2C link to the Teensy. Here’s a scope photo of the SDA line showing the data activity. The vertical scale is 1V/cm, showing the HIGH value is about 3.8V, so the Mega must be comfortable with that value as a HIGH logic level

In the above photo, notice the business card for Probe Master, Inc (www.probemaster.com). These folks were kind enough to replace my ten year old scope probe with a new upgraded version at no cost – thank you Probe Master!

At this point I have the ‘Tut5’ program running on the Mega, and the unmodified IR Demod code running on the robot’s Teensy 3.2, and the Mega appears to be acquiring valid demod data from the Teensy. To further validate the data, I fired up my charging station with it’s square-wave modulated IR beam, and was pleased to see that the acquired data from the Teensy varied appropriately as I manually rotated the robot, as shown in the following Excel chart.

So, after all this work, the whole thing came down to a bad I2C ‘daisy-chain’ cable. This particular cable has been on the robot for a while, and was the soldering graduation exercise of my grandson Danny when he was here a couple of summers ago. It wasn’t the best job I’ve ever seen, but it was pretty good for a 15 year old ;-). In any case, I took the opportunity to build a new cable with smaller diameter wire so things would fit a little bit better into the Pololu pins, sockets, and 2-pin header sleeves as shown in the following photo.

New I2C ‘daisy-chain’ cable. Run starts from the Mega connector at bottom left, then to the RTC, IMU and FRAM modules in that order, then last to the Teensy 3.2 IR Demod module.

05 April 2020 Epilogue:

Well, there was one last ‘gotcha’ in all this. When I loaded the original ‘FourWD-WallE2_V2.ino’ program back onto the Mega, it still refused to acquire valid data from the Teensy IR Demod module. So, I compared the ‘Tut5’ code to the ‘_V2’ code, and noticed three significant differences:

In the ‘_V2’ code, the ‘Fin1/2’ variables had at some time been changed from ‘long’ to ‘int’. While this sounds reasonable, it isn’t – because a Teensy ‘int’ is 4 bytes – the same as a Mega ‘long’.
In the ‘_V2’ code, the Wire.requestFrom() call asks for 32 bytes, but the Teensy only sends 12.
In the ‘_V2’ code there is a loop that waits for either a timeout or receiving an entire 32-byte buffer from the Teensy. Apparently the Teensy (and/or the Mega) is slow enough so that Wire.Available() never reports a non-zero buffer size, so the loop times out.

Replacing the line

	uint8_t numbytes = Wire.requestFrom(slaveAddr, (size_t)32); //this works, with Wire.Available() reporting 32

1	uint8_t numbytes = Wire.requestFrom(slaveAddr, (size_t)32); //this works, with Wire.Available() reporting 32

with the line

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

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

did the trick, but I have no idea why.

Replacing the debug printout lines

Serial.print("in IsIRBeamAvail(): got ");
Serial.print(Fin1); Serial.print(", ");
Serial.print(Fin2); Serial.print(", total =  ");
//Serial.print(SteeringValue); Serial.println(" from slave");
Serial.println(total);

Serial.print("in IsIRBeamAvail(): got ");

Serial.print(Fin1); Serial.print(", ");

Serial.print(Fin2); Serial.print(", total = ");

//Serial.print(SteeringValue); Serial.println(" from slave");

Serial.println(total);

with the line

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

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

Produced lines like this one from the ‘_V2’ program

Fin1 = 390556, Fin2 = 153386, SteeringValue = 0.44
IR Beam Detected with Battery Voltage = 7.32DateTime	Batt	FinVal1

1 2	Fin1 = 390556, Fin2 = 153386, SteeringValue = 0.44 IR Beam Detected with Battery Voltage = 7.32DateTime Batt FinVal1

Which appear to be the correct values for the given robot orientation with respect to the charging station.

With no other changed except removing power from the charging station, the output became

Fin1 = 14, Fin2 = 28, SteeringValue = -0.33
Fin1 = 66, Fin2 = 42, SteeringValue = 0.22
Fin1 = 26, Fin2 = 48, SteeringValue = -0.30
Fin1 = 54, Fin2 = 30, SteeringValue = 0.29

Fin1 = 14, Fin2 = 28, SteeringValue = -0.33

Fin1 = 66, Fin2 = 42, SteeringValue = 0.22

Fin1 = 26, Fin2 = 48, SteeringValue = -0.30

Fin1 = 54, Fin2 = 30, SteeringValue = 0.29

showing definitively that the ‘_V2’ program correctly identified and decoded the square-wave modulated IR beam.

So, after a two-week off-road trip to I2C comms hell and back, I think I finally have that particular issue put to bed. It wasn’t exactly what I wanted to do, but it was at least interesting, and more important – consumed a lot of coronavirus quarantine time ;-).

Stay tuned!

Frank

Arduino SPI Data Exchange Between Two Arduinos in a Master/Slave Configuration

2 Replies

posted 13 March 2020,

While updating my four wheel autonomous wall-following robot (aka Wall-E2), I ran into a roadblock when I couldn’t get my Teensy 3.2 IR demodulator/tracker module to communicate with the Mega 2560 main microcontroller over I2C. This worked fine when I last tested it, but now it seems to have taken a vacation. It’s been a while, and I have added some functionality since I originally installed and tested IR homing to the charging station, but it still should all work, right?

In any case, after trying (and failing) to get the I2C bus connection working, I thought I might try an alternate solution and just use SPI between the main controller (Mega 2560) and the IR homing controller (Teensy 3.2). This would have the advantage of making the Teensy independent of the I2C bus, and also give me the chance to play with a part of the Arduino ecosystem that I haven’t used before.

As usual, I started this process with a lot of Googling, and quickly ran across Australian Nick Gammon’s “SPI – Serial Peripheral Interface – for Arduino“ . This post was way more than I ever wanted to know about SPI, but it sure is complete!

Then I moved on to trying to get SPI working for myself. As noted above, my intended application is to transfer steering values from my IR detector/demodulator/homing module to the main Mega 2560 microcontroller. The Mega uses the steering data to adjust wheel speeds to home in on a charging station, so Wall-E2 can continue to roam our house autonomously.

It turns out that SPI between two Arduinos isn’t entirely straightforward, at least not at first. I went through a bunch of iterations, and even more passes through the available documentation. In the end though, I think I wrassled it into a reasonable facsimile of a working solution, as shown below. The program repeatedly transfers a fixed string (“Hello World!”) from the UNO (master) to the Mega 2560 (slave), and then transfers the string version of a float value (3.159) from the slave to the master.

The Circuit:

The circuit and layout is just about as basic as it gets. The UNO (master) connections are the default pinouts:

UNO (master) connected to Mega 2560 (slave) using default SPI pins on both ends

The Master:

/*
    Name:       SPI_EX3_Master.ino
    Created:	3/13/2020 4:19:35 PM
    Author:     FRANKNEWXPS15\Frank
*/


#include <SPI.h>
#include "pins_arduino.h"
#include <PrintEx.h> //allows printf-style printout syntax
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

volatile byte rcvbyte = 0;
char rcvbuf[100];

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

    // Put SCK, MOSI, SS pins into output mode
    // also put SCK, MOSI into LOW state, and SS into HIGH state.
    // Then put SPI hardware into Master mode and turn SPI on
    SPI.begin();

    Serial.println("SPI Example Master");

    memset(rcvbuf, 0, sizeof(rcvbuf));

}  // end of setup

void loop(void)
{

    char c;

    // send test string
    Serial.println("Sending Test String");

    //tell slave to get ready
    digitalWrite(SS, LOW);    // SS is pin 10
    delayMicroseconds(200); //give slave some time 

    // send test string
    for (const char* p = "Hello, world!\n"; c = *p; p++)
    {
        mySerial.printf("Sending %x to Slave\n", c);
        SPI.transfer(c);
        delayMicroseconds(20); //required for arduino-arduino xfrs
    }

    //receive data from slave
    delay(500);

    //get float value from slave
    Serial.println("Receiving Data String\n");
    int pos = 0;
    memset(rcvbuf, sizeof(rcvbuf), 0);

    c = 1;
    while (c != 0x0D)
    {
        c = SPI.transfer(0);
        mySerial.printf("got %x from Slave\n", c);
        rcvbuf[pos++] = c;
        delayMicroseconds(20);
    }

    //remove any non-numeric characters
    char buf2[20];
    int buf2pos = 0;
    for (size_t i = 0; i < sizeof(rcvbuf); i++)
    {
        char p = rcvbuf[i];  // search for space
        if (p > 0x20 )
        {
            buf2[buf2pos++] = p;
        }
        buf2[buf2pos] = 0;
    }

    rcvbuf[pos] = 0; //add NULL to terminate string

    if (pos > 0)
    {
        mySerial.printf("received %s from slave\n", buf2);
    }

    // tell slave to get ready for next data transfer
    digitalWrite(SS, HIGH);

    delay(1000);  // 1 seconds delay
}  // end of loop

Name: SPI_EX3_Master.ino

Created: 3/13/2020 4:19:35 PM

Author: FRANKNEWXPS15\Frank

#include <SPI.h>

#include "pins_arduino.h"

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

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

volatile byte rcvbyte = 0;

char rcvbuf[100];

void setup(void)

{

Serial.begin(115200);

// Put SCK, MOSI, SS pins into output mode

// also put SCK, MOSI into LOW state, and SS into HIGH state.

// Then put SPI hardware into Master mode and turn SPI on

SPI.begin();

Serial.println("SPI Example Master");

memset(rcvbuf, 0, sizeof(rcvbuf));

} // end of setup

void loop(void)

{

char c;

// send test string

Serial.println("Sending Test String");

//tell slave to get ready

digitalWrite(SS, LOW); // SS is pin 10

delayMicroseconds(200); //give slave some time

// send test string

for (const char* p = "Hello, world!\n"; c = *p; p++)

{

mySerial.printf("Sending %x to Slave\n", c);

SPI.transfer(c);

delayMicroseconds(20); //required for arduino-arduino xfrs

}

//receive data from slave

delay(500);

//get float value from slave

Serial.println("Receiving Data String\n");

int pos = 0;

memset(rcvbuf, sizeof(rcvbuf), 0);

c = 1;

while (c != 0x0D)

{

c = SPI.transfer(0);

mySerial.printf("got %x from Slave\n", c);

rcvbuf[pos++] = c;

delayMicroseconds(20);

}

//remove any non-numeric characters

char buf2[20];

int buf2pos = 0;

for (size_t i = 0; i < sizeof(rcvbuf); i++)

{

char p = rcvbuf[i]; // search for space

if (p > 0x20 )

{

buf2[buf2pos++] = p;

}

buf2[buf2pos] = 0;

}

rcvbuf[pos] = 0; //add NULL to terminate string

if (pos > 0)

{

mySerial.printf("received %s from slave\n", buf2);

}

// tell slave to get ready for next data transfer

digitalWrite(SS, HIGH);

delay(1000); // 1 seconds delay

} // end of loop

The Slave:

/*
 Name:		SPI_EX3_Slave.ino
 Created:	3/13/2020 4:21:43 PM
 Author:	Frank
*/


#include "pins_arduino.h"
#include <SPI.h>
#include <PrintEx.h> //allows printf-style printout syntax
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

char buf[100];
char val[20];
volatile byte rcv_pos;
volatile byte xmt_pos;
volatile boolean process_it;
volatile boolean bReceivingString;

void setup(void)
{
    Serial.begin(115200);   // debugging
    mySerial.printf("SPI Example Slave\n");

    // have to send on master in, *slave out*
    pinMode(MISO, OUTPUT);
    pinMode(PIN_SPI_SS, INPUT_PULLUP); //SS is INPUT for slave device

    // turn on SPI in slave mode
    SPCR |= _BV(SPE);

    // turn on interrupts
    SPCR |= _BV(SPIE);

    rcv_pos = 0;
    process_it = false;

    //use dstrtof to produce string version of float value
    float x = 3.14159;
    memset(val, 0x0D, sizeof(val));
    dtostrf(x, 7, 5, val);
    mySerial.printf("string representation of %2.5f is %s\n", x, val);

    for (size_t i = 0; i < sizeof(val) - 1; i++)
    {
        mySerial.printf("%x", val[i]);
    }
    mySerial.printf("%x\n", val[sizeof(val) - 1]);

    xmt_pos = 0;

}  // end of setup


// SPI interrupt routine
ISR(SPI_STC_vect)
{
    if (bReceivingString)
    {
        byte c = SPDR;

        // add to buffer if room
        if (rcv_pos < sizeof buf)
        {
            buf[rcv_pos++] = c;

            //newline means time to process buffer
            if (c == '\n')
                process_it = true;

        }  // end of room available
    }
    else
    {
        SPDR = val[xmt_pos];
        xmt_pos++;
    }
}

// main loop - wait for flag set in interrupt routine
void loop(void)
{
    if (process_it)
    {
        mySerial.printf("In process_it with rcv_pos = %d\n",
            rcv_pos);
        buf[rcv_pos] = 0;
        mySerial.printf("Received %s from Master\n", buf);
        rcv_pos = 0;
        process_it = false;
        bReceivingString = false;
    }  // end of flag set

    if (digitalRead(PIN_SPI_SS) == HIGH)
    {
        xmt_pos = 0; //get ready for next iteration
        SPDR = 0;
        bReceivingString = true;
        process_it = false;

    }

}  // end of loop

100

101

102

103

Name: SPI_EX3_Slave.ino

Created: 3/13/2020 4:21:43 PM

Author: Frank

#include "pins_arduino.h"

#include <SPI.h>

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

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

char buf[100];

char val[20];

volatile byte rcv_pos;

volatile byte xmt_pos;

volatile boolean process_it;

volatile boolean bReceivingString;

void setup(void)

{

Serial.begin(115200); // debugging

mySerial.printf("SPI Example Slave\n");

// have to send on master in, *slave out*

pinMode(MISO, OUTPUT);

pinMode(PIN_SPI_SS, INPUT_PULLUP); //SS is INPUT for slave device

// turn on SPI in slave mode

SPCR |= _BV(SPE);

// turn on interrupts

SPCR |= _BV(SPIE);

rcv_pos = 0;

process_it = false;

//use dstrtof to produce string version of float value

float x = 3.14159;

memset(val, 0x0D, sizeof(val));

dtostrf(x, 7, 5, val);

mySerial.printf("string representation of %2.5f is %s\n", x, val);

for (size_t i = 0; i < sizeof(val) - 1; i++)

{

mySerial.printf("%x", val[i]);

}

mySerial.printf("%x\n", val[sizeof(val) - 1]);

xmt_pos = 0;

} // end of setup

// SPI interrupt routine

ISR(SPI_STC_vect)

{

if (bReceivingString)

{

byte c = SPDR;

// add to buffer if room

if (rcv_pos < sizeof buf)

{

buf[rcv_pos++] = c;

//newline means time to process buffer

if (c == '\n')

process_it = true;

} // end of room available

}

else

{

SPDR = val[xmt_pos];

xmt_pos++;

}

// main loop - wait for flag set in interrupt routine

void loop(void)

{

if (process_it)

{

mySerial.printf("In process_it with rcv_pos = %d\n",

rcv_pos);

buf[rcv_pos] = 0;

mySerial.printf("Received %s from Master\n", buf);

rcv_pos = 0;

process_it = false;

bReceivingString = false;

} // end of flag set

if (digitalRead(PIN_SPI_SS) == HIGH)

{

xmt_pos = 0; //get ready for next iteration

SPDR = 0;

bReceivingString = true;

process_it = false;

}

} // end of loop

The above configuration produced the following output:

Master:

Opening port
Port open
SPI Example Master
Sending Test String
Sending 48 to Slave
Sending 65 to Slave
Sending 6C to Slave
Sending 6C to Slave
Sending 6F to Slave
Sending 2C to Slave
Sending 20 to Slave
Sending 77 to Slave
Sending 6F to Slave
Sending 72 to Slave
Sending 6C to Slave
Sending 64 to Slave
Sending 21 to Slave
Sending A to Slave
Receiving Data String

got A from Slave
got 33 from Slave
got 2E from Slave
got 31 from Slave
got 34 from Slave
got 31 from Slave
got 35 from Slave
got 39 from Slave
got 0 from Slave
got D from Slave
received 3.14159 from slave

Opening port

Port open

SPI Example Master

Sending Test String

Sending 48 to Slave

Sending 65 to Slave

Sending 6C to Slave

Sending 6F to Slave

Sending 2C to Slave

Sending 20 to Slave

Sending 77 to Slave

Sending 6F to Slave

Sending 72 to Slave

Sending 6C to Slave

Sending 64 to Slave

Sending 21 to Slave

Sending A to Slave

Receiving Data String

got A from Slave

got 33 from Slave

got 2E from Slave

got 31 from Slave

got 34 from Slave

got 31 from Slave

got 35 from Slave

got 39 from Slave

got 0 from Slave

got D from Slave

received 3.14159 from slave

Slave:

Opening port
Port open
SPI Example Slave
string representation of 3.14159 is 3.14159
332E31343135390DDDDDDDDDDDD
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master

Port closed

Opening port

Port open

SPI Example Slave

string representation of 3.14159 is 3.14159

332E31343135390DDDDDDDDDDDD

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

Port closed

Now that I’ve had my fun and games with SPI, I’m still not at all sure I want to use it to connect the Mega to the Teensy IR Demodulator/Homing module. While it is almost certain to work (eventually), it has some drawbacks

It requires 4 additional wires
It requires that I add an ISR and other code to the Teensy module, and companion code to the Mega
More things to go wrong.

So, I think I’ll try again to get the original I2C based code working, as it worked before, so it should work again. If I can’t get it working after another good try, I’ll go with door #2 (SPI)

Stay tuned!

Frank

Updating the Four Wheel Robot

1 Reply

Posted 29 February 2020,

Happy Leap Day!

For the last several months I have been using my older 2 wheel robot to investigate improved wall following techniques using relative heading from the onboard MPU6050 IMU module. As the reader may recall (and if you can’t, look at this summarizing post) I had a heck of a time achieving reliable operation with the MPU6050 module mounted on the two wheel robot. In the process of figuring all that out, and in collaboration with Homer Creutz, we also developed a nifty polling algorithm for obtaining heading information from the MPU6050, a method that has now been incorporated into Jeff Rowberg’s fantastic I2CDev ecosystem.

After getting the MPU6050 (and the metal-geared motors on the 2 wheel robot) to behave, I was also able to significantly enhance wall-following performance (at least for the 2 wheel robot). Now it can start from any orientation relative to a nearby wall, figure out an approximate parallel heading, and then acquire and then track a specified offset distance from the wall – pretty cool, huh?

So, now it is time to integrate all this new stuff back into the 4-wheel robot, and see if it will translate to better autonomous wall-tracking, charging station acquisition, obstacle avoidance, and doing the laundry (well, maybe not the last one). The major changes are:

Update the project with the newest MPU6050 libraries:
Revise the original 4 wheel code to use polling vs interrupt for heading values
Installation of the ‘FindParallelHeading()’ function and all its support routines
Integration of the parallel heading determination step into current wall tracking routines
Verification of improved functionality
Verification that the new work hasn’t degraded any existing functionality
Incorporating heading and heading-based turn capabilities into obstacle avoidance

To implement all the above, while attempting to insulate myself from the possibility of a major screwup, I created a brand-new Arduino project called ‘FourWD_WallE2_V2’ and started integrating the original code from ‘FourWD_WallE2_V1’ and ‘TwoWheelRobot_V2’

Update the project with the newest MPU6050 libraries:

The original FourWD_WallE2_V1 project used the older MPU6050_6Axis_MotionApps20.h library, but the two wheel robot uses the newer MPU6050_6Axis_MotionApps_V6_12.h one. In addition, Homer Creutz had updated the new library even further since its incorporation into the two wheel robot. The first step in updating the 4 wheel robot was to re-synchronize the library on my PC with the newer version on GitHub. This was accomplished very easily – yay! The next step was to copy most of the #includes and program constants from the original 4 wheel project into the new one, and then get the resulting skeleton program to compile. This took a few tries and the addition of several files into the project folder as ‘local’ resource and header files, but it got done. At the conclusion of this step, the project had empty setup() and loop() functions and no auxiliary/support functions, but it did compile – yay!

Revise the original 4 wheel code to use polling vs interrupt for heading values

The original project uses a flag modified by an ISR (Interrupt Service Routine) to mediate heading value acquisition. The two wheel robot uses a polling based routine to do the same thing. However, the algorithm used by the two wheel robot isn’t exactly the same as the one provided by Homer Creutz as part of the new MPU6050_6Axis_MotionApps_V6_12.h library. In addition, the two wheel robot uses a different naming convention for the current heading value retrieved from the IMU. The 4-wheel robot uses ‘global_yawval’, and the 2-wheel one uses ‘IMUHdgValDeg’. The 4-wheel robot uses ‘bool GetIMUHeadingDeg()’ to retrieve heading values, but the 2-wheel robot uses ‘bool UpdateIMUHdgValDeg()’ to better indicate it’s function. So, all instances of ‘global_yawval’ will need to be changed to ‘IMUHdgValDeg’, and references to ‘GetIMUHeadingDeg()’ will have to instead reference ‘UpdateIMUHdgValDeg()’.

I started this step by copying the entire ‘setup()’ and ‘loop()’ function contents from the old 4-wheel robot project to the new one, and then working through the laborious process of getting everything to compile with the new variable and function names. First I just started with ‘setup()’, and kept copying over the required support functions until I’d gotten everything. For each support function I checked the corresponding function in the 2 wheel project to make sure I wasn’t missing an update or enhancement. BTW, the combination of Microsoft’s Visual Studio and Visual Micro’s wonderful Arduino extension made this much easier, as the non-compiling code is highlighted in red in the margins of the VS edit window, reducing the need for multiple compiles The affected functions were:

GetDayDateTimeStringFromDateTime(now, buffer): not in 2 wheel project
GetLeft/RightMedianDistCm(): Eliminated – these were never really used. Replaced where necessary with GetAvgLeft/RightDistCm() from the two wheel project.
GetFrontDistCm(): Not used in 2 wheel project
dmpDataReady(): ISR for MPU6050 interrupts. Replaced with polling strategy
StopLeft/RightMotors(): Not used in 2 wheel robot – copied unchanged
SetLeft/RightMotorDir(): Not used in 2 wheel robot – copied unchanged
RunBothMotors(), RunBothMotorsMsec(): Not used in 2 wheel robot – copied unchanged
IsCharging(): Not used in 2 wheel robot – copied unchanged.

At this point, the entire setup() function compiles without error, and the setup() code runs properly. The next step is to add in the loop() functionality and then modify as necessary to replace interrupt-based heading acquisition with polling-based, replace ‘global_yawval’ with ‘IMUHdgValDeg’, and to replace ‘GetIMUHeadingDeg()’ with ‘UpdateIMUHdgValDeg()’

notes:

Revise UpdateWallFollowMotorspeeds as necessary to incorporate heading-based offset tracking
Revise/Replace RollingTurn() & GetIMUHeadingDeg() as necessary – done
Global replace of global_yawval with IMUHdgValDeg showed 25 replacements
Replaced ‘GetIMUHeadingDeg()’ with ‘UpdateIMUHdgValDeg()’ from 2 wheel robot project
Added GetCurrentFIFOPacket() from 2 wheel robot project
Replaced ‘if(devStatus == 0)’ block with the one from 2 wheel project
Had to comment out PrintWallFollowTelemetry(frontvar), FillPacketFromCurrentState(CFRAMStatePacket* pkt), and DisplayHumanReadablePacket(CFRAMStatePacket* pkt) to get everything to compile.

At the conclusion of all the above, the _V2 project now compiles completely.

1 March 2020 Update:

After getting the entire program to compile, I decided to try some simple tests of heading-based turn capability, so I modified setup() to have the robot do some simple S-turns, and then a backup-and-turn procedure. As the accompanying video shows, this seemed to work quite well. This is very encouraging, as it demonstrates polling-based rather than interrupt-based MPU6050 heading value acquisition and verifies that the latest MPU6050 libraries work properly.

The next step was to incorporate the ‘command mode’ facility from the two wheel robot. This facility allows a user within range of the Wixel RF link to take over the robot and issue movement commands, like a crude RC controller. After making these changes, I was able to take control of the robot and manually command some simple maneuvers as shown in the following video.

As shown above, the left 180 degree turn as currently implemented for the 4-wheel robot takes forever! I’ll need to work on that.

04 March 2020 Update:

I lowered the value of the OFFSIDE_MOTOR_SPEED constant while leaving the DRIVESIDE_MOTOR_SPEED constant unchanged to make turns more aggressive, as shown in the following set of three video clips. In the first two, the OFFSIDE_MOTOR_SPEED is 0, while in the last one, it is 25. I think I’ll leave it set to 25 for the foreseeable future.

Stay tuned,

Frank

06 April 2020 Update:

After a two-week trip to I2C hell and back, I’m ready to continue the project to update my autonomous wall-following robot with new heading-based turn and tracking capability. The off-road trip was caused by (I now believe) the combination of a couple of software bugs and an intermittent I2C ‘daisy-chain’ cable connecting the Arduino Mega controller to four I2C peripherals. See this post for the gory details.

Installation of the ‘FindParallelHeading()’ function and all its support routines:

In the TwoWheelRobot project, the ‘FindParallelHdg()’ function is used to orient the robot parallel to the nearest wall in preparation to approaching and then tracking a specified offset distance. The algorithm first determines the parallel heading by turning the robot and monitoring the distance to the near wall. Once the parallel heading is determined, the robot turns toward or away from the wall as necessary to capture and then track the desired offset distance.

Here’s a short video and telemetry from a representative run in my ‘indoor range’.

Opening port
Port open
Two Wheel Robot
Initializing MPU6050 ...
Testing device connections...
MPU6050 connection successful
Initializing DMP...
Enabling DMP...
DMP ready! Waiting for MPU6050 drift rate to settle...

MPU6050 Ready at 1.21 Sec
mSec	Yaw	TurnDeg	PrevDeg	LeftD	RightD	Error
1347	0.04	0	0	200	200	0
turndeg = 0; Removing Entire Cut
In FindParallelHdg()
mSec	meas#	Hdg	PrevD	CurD
setting prevmindist to 49
bTrackingRight = TRUE
starting CCW turn (away from right wall)
1487	1	-0.86	49	49	1
1517	2	-1.28	49	49	2
1537	3	-1.54	49	49	3
1556	4	-1.70	49	49	4
1577	5	-1.75	49	48	2
1596	6	-1.56	48	49	3
1615	7	-1.25	48	49	4
1639	8	-0.92	48	48	5
1669	9	-0.46	48	49	6
1698	10	0.11	48	47	4
1728	11	0.77	47	48	5
1757	12	1.34	47	47	6
1789	13	1.92	47	48	7
1818	14	2.60	47	47	8
1848	15	3.38	47	47	9
1876	16	4.36	47	47	10
1909	17	5.38	47	47	11
1938	18	6.47	47	47	12
1968	19	7.50	47	47	13
1997	20	8.43	47	47	14
2026	21	9.46	47	47	15
2045	22	10.14	47	48	16
2065	23	10.82	47	48	17
2088	24	11.50	47	46	15
2118	25	12.52	46	47	16
2147	26	13.58	46	47	17
2177	27	14.64	46	48	18
2208	28	15.70	46	46	19
2238	29	16.77	46	45	17
2267	30	17.89	45	47	18
2296	31	18.96	45	48	19
2328	32	20.01	45	48	20
Tracking Case 2, Parallel Heading = 20.01
3345	26.54	20	0	200	49	19
turndeg > m_PrevHdgCutDeg; Increasing Cut
3555	15.09	20	10	200	45	15
turndeg > m_PrevHdgCutDeg; Increasing Cut
3785	3.29	20	20	200	43	13
3813	1.93	20	20	200	44	14
3843	1.16	20	20	200	44	14
not converging - increasing cut
4114	-9.10	20	30	200	43	13
Converging with turndeg 20 & PrevCut 30. Reducing Cut
4463	-0.36	10	20	200	39	9
Converging with turndeg 10 & PrevCut 20. Reducing Cut
4731	11.06	10	10	200	36	6
4761	12.67	10	10	200	36	6
4792	13.18	10	10	200	36	6
not converging - increasing cut
5123	2.97	5	20	200	34	4
Converging with turndeg 5 & PrevCut 20. Reducing Cut
5472	11.71	10	10	200	33	3
5502	13.07	5	10	200	32	2
Converging with turndeg 5 & PrevCut 10. Reducing Cut
5810	24.21	5	0	200	32	2
turndeg > m_PrevHdgCutDeg; Increasing Cut
6181	14.03	0	10	200	31	1
turndeg = 0; Removing Entire Cut
6531	23.51	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6560	24.95	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6590	25.04	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6620	25.09	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6649	25.17	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6678	25.14	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6711	25.05	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6739	24.84	0	0	200	29	-1
turndeg = 0; Removing Entire Cut
6769	24.74	0	0	200	29	-1
turndeg = 0; Removing Entire Cut
6800	24.60	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6830	24.35	0	0	200	30	0
turndeg = 0; Removing Entire Cut
6859	24.12	0	0	200	31	1
turndeg = 0; Removing Entire Cut
6888	23.96	0	0	200	31	1
turndeg = 0; Removing Entire Cut
6920	23.68	5	0	200	27	-3
turndeg > m_PrevHdgCutDeg; Increasing Cut
7249	34.05	0	10	200	31	1
turndeg = 0; Removing Entire Cut
7519	45.60	5	0	200	34	4
turndeg > m_PrevHdgCutDeg; Increasing Cut
7888	35.61	10	10	200	34	4
not converging - increasing cut
I received: 35
8138	23.63	5	20	200	33	3
Converging with turndeg 5 & PrevCut 20. Reducing Cut
I received: D
8485	33.30	10	10	200	34	4
not converging - increasing cut
I received: A
8857	24.37	5	20	200	34	4
8887	22.76	10	20	200	34	4
8916	21.88	10	20	200	34	4
8946	21.46	10	20	200	34	4
8976	21.09	10	20	200	33	3
Converging with turndeg 10 & PrevCut 20. Reducing Cut
9307	30.78	10	10	200	34	4
not converging - increasing cut
9675	21.56	5	20	200	34	4
9705	19.96	10	20	200	33	3
Converging with turndeg 10 & PrevCut 20. Reducing Cut
10055	30.10	20	10	200	36	6
turndeg > m_PrevHdgCutDeg; Increasing Cut
10343	21.16	10	20	200	26	-4
10385	18.89	20	20	200	100	70
not converging - increasing cut
I received: 63
ENTERING COMMAND MODE:
0 = 180 deg CCW Turn
1 = 180 deg CW Turn
A = Back to Auto Mode
S = Stop

       Faster
	8
Left 4	5  6 Right
	2
       Slower

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

Opening port

Port open

Two Wheel Robot

Initializing MPU6050 ...

Testing device connections...

MPU6050 connection successful

Initializing DMP...

Enabling DMP...

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

MPU6050 Ready at 1.21 Sec

mSec Yaw TurnDeg PrevDeg LeftD RightD Error

1347 0.04 0 0 200 200 0

turndeg = 0; Removing Entire Cut

In FindParallelHdg()

mSec meas# Hdg PrevD CurD

setting prevmindist to 49

bTrackingRight = TRUE

starting CCW turn (away from right wall)

1487 1 -0.86 49 49 1

1517 2 -1.28 49 49 2

1537 3 -1.54 49 49 3

1556 4 -1.70 49 49 4

1577 5 -1.75 49 48 2

1596 6 -1.56 48 49 3

1615 7 -1.25 48 49 4

1639 8 -0.92 48 48 5

1669 9 -0.46 48 49 6

1698 10 0.11 48 47 4

1728 11 0.77 47 48 5

1757 12 1.34 47 47 6

1789 13 1.92 47 48 7

1818 14 2.60 47 47 8

1848 15 3.38 47 47 9

1876 16 4.36 47 47 10

1909 17 5.38 47 47 11

1938 18 6.47 47 47 12

1968 19 7.50 47 47 13

1997 20 8.43 47 47 14

2026 21 9.46 47 47 15

2045 22 10.14 47 48 16

2065 23 10.82 47 48 17

2088 24 11.50 47 46 15

2118 25 12.52 46 47 16

2147 26 13.58 46 47 17

2177 27 14.64 46 48 18

2208 28 15.70 46 46 19

2238 29 16.77 46 45 17

2267 30 17.89 45 47 18

2296 31 18.96 45 48 19

2328 32 20.01 45 48 20

Tracking Case 2, Parallel Heading = 20.01

3345 26.54 20 0 200 49 19

turndeg > m_PrevHdgCutDeg; Increasing Cut

3555 15.09 20 10 200 45 15

turndeg > m_PrevHdgCutDeg; Increasing Cut

3785 3.29 20 20 200 43 13

3813 1.93 20 20 200 44 14

3843 1.16 20 20 200 44 14

not converging - increasing cut

4114 -9.10 20 30 200 43 13

Converging with turndeg 20 & PrevCut 30. Reducing Cut

4463 -0.36 10 20 200 39 9

Converging with turndeg 10 & PrevCut 20. Reducing Cut

4731 11.06 10 10 200 36 6

4761 12.67 10 10 200 36 6

4792 13.18 10 10 200 36 6

not converging - increasing cut

5123 2.97 5 20 200 34 4

Converging with turndeg 5 & PrevCut 20. Reducing Cut

5472 11.71 10 10 200 33 3

5502 13.07 5 10 200 32 2

Converging with turndeg 5 & PrevCut 10. Reducing Cut

5810 24.21 5 0 200 32 2

turndeg > m_PrevHdgCutDeg; Increasing Cut

6181 14.03 0 10 200 31 1

turndeg = 0; Removing Entire Cut

6531 23.51 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6560 24.95 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6590 25.04 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6620 25.09 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6649 25.17 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6678 25.14 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6711 25.05 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6739 24.84 0 0 200 29 -1

turndeg = 0; Removing Entire Cut

6769 24.74 0 0 200 29 -1

turndeg = 0; Removing Entire Cut

6800 24.60 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6830 24.35 0 0 200 30 0

turndeg = 0; Removing Entire Cut

6859 24.12 0 0 200 31 1

turndeg = 0; Removing Entire Cut

6888 23.96 0 0 200 31 1

turndeg = 0; Removing Entire Cut

6920 23.68 5 0 200 27 -3

turndeg > m_PrevHdgCutDeg; Increasing Cut

7249 34.05 0 10 200 31 1

turndeg = 0; Removing Entire Cut

7519 45.60 5 0 200 34 4

turndeg > m_PrevHdgCutDeg; Increasing Cut

7888 35.61 10 10 200 34 4

not converging - increasing cut

I received: 35

8138 23.63 5 20 200 33 3

Converging with turndeg 5 & PrevCut 20. Reducing Cut

I received: D

8485 33.30 10 10 200 34 4

not converging - increasing cut

I received: A

8857 24.37 5 20 200 34 4

8887 22.76 10 20 200 34 4

8916 21.88 10 20 200 34 4

8946 21.46 10 20 200 34 4

8976 21.09 10 20 200 33 3

Converging with turndeg 10 & PrevCut 20. Reducing Cut

9307 30.78 10 10 200 34 4

not converging - increasing cut

9675 21.56 5 20 200 34 4

9705 19.96 10 20 200 33 3

Converging with turndeg 10 & PrevCut 20. Reducing Cut

10055 30.10 20 10 200 36 6

turndeg > m_PrevHdgCutDeg; Increasing Cut

10343 21.16 10 20 200 26 -4

10385 18.89 20 20 200 100 70

not converging - increasing cut

I received: 63

ENTERING COMMAND MODE:

0 = 180 deg CCW Turn

1 = 180 deg CW Turn

A = Back to Auto Mode

S = Stop

Faster

Left 4 5 6 Right

Slower

So, now to port this capability to the FourWD_WallE_V2 project:

08 April 2020 Update:

The current operating algorithm for WallE2 is pretty simple. Every 200 mSec or so it assesses the current situation and decides on a new operating mode. This in turn allows the main code in loop() to decide what to do.

Here’s the code for GetOpMode()

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

//Step1: If batt voltage too low, return MODE_DEADBATTERY (4)
	if (batv <= DEAD_BATT_THRESH_VOLTS) return MODE_DEADBATTERY;

	//Step2: If Charger is connected, return MODE_CHARGING (1)
	else if (IsChargerConnected() && IsStillCharging()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor

//Step3: Else If IR Homing beam detected, return MODE_IRHOMING (2)
	else if (IsIRBeamAvail()) return MODE_IRHOMING; //this handles 'hungry/not-hungry' cases internally

//03/04/2020 added to facilitate battery discharge
#ifdef BATTERY_DISCHARGE
	while (GetBattVoltage() > DEAD_BATT_THRESH_VOLTS)
	{
		return MODE_DISCHARGE;
	}
	StopBothMotors();
	mySerial.printf("Battery Discharge Complete at %3.2f Minutes and %3.2f Volts", millis() / 60000.f, GetBattVoltage);

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

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

//Step1: If batt voltage too low, return MODE_DEADBATTERY (4)

if (batv <= DEAD_BATT_THRESH_VOLTS) return MODE_DEADBATTERY;

//Step2: If Charger is connected, return MODE_CHARGING (1)

else if (IsChargerConnected() && IsStillCharging()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor

//Step3: Else If IR Homing beam detected, return MODE_IRHOMING (2)

else if (IsIRBeamAvail()) return MODE_IRHOMING; //this handles 'hungry/not-hungry' cases internally

//03/04/2020 added to facilitate battery discharge

#ifdef BATTERY_DISCHARGE

while (GetBattVoltage() > DEAD_BATT_THRESH_VOLTS)

{

return MODE_DISCHARGE;

}

StopBothMotors();

mySerial.printf("Battery Discharge Complete at %3.2f Minutes and %3.2f Volts", millis() / 60000.f, GetBattVoltage);

#else

//Step4: Else return MODE_WALLFOLLOW (3)

else return MODE_WALLFOLLOW;

#endif // BATTERY_DISCHARGE

}

In terms of the project to port heading-based specified-offset wall tracking to WallE2, the only pertinent result from GetOpMode() is the default MODE_WALLFOLLOW result.

In the main loop(), a switch(CurrentOpMode) decides what actions, if any need to occur. Here’s the relevant section of the code. In the MODE_WALLFOLLOW case, the first thing that happens is an update of the TRACKING CASE via

‘TrackingCase = (WallTrackingCases)GetTrackingDir()’

which returns one of several tracking cases; TRACKING_RIGHT, TRACKING_LEFT, or TRACKING_NEITHER. A switch(TrackingCase) handles each case separately.

		CurrentOpMode = (OpModes)GetOpMode(batVoltage); //04/28/19 bat volts added to sig

////DEBUG!!
//		//CurrentOpMode = MODE_IRHOMING; //forced to MODE_IRHOMING for testing
//		Serial.print("OpMode = ");
//		Serial.println(CurrentOpMode);
////DEBUG!!

		//Step2: Switch to appropriate operating case block
		String trackstr = "Unknown"; //used for telemetry printouts

		switch (CurrentOpMode)
		{
		case MODE_WALLFOLLOW:
			//Purpose:  Follow nearest wall, responding to obstacles as necessary
			//Provenance: G. Frank Paynter and Danny Frank 12/20/2015
			//Inputs:  Distances from left/right/front sensors
			//Outputs: updated left/right motor speeds in wall-follow case, or appropriate avoidance maneuver
			//Plan:
			//	Step 1: Update motor speeds & NAV mode based on current TRACK/NAV states and front distance
			//	Step 2: Print out telemetry info
			//Notes:
			//	03/30/14 Step1 code moved to top of loop() - compiler directives cause errors here
			//	09/11/18 revised plan to reflect reality

			// Update motor speeds & NAV mode based on current TRACK/NAV states and front distance
			// See https://fpaynter.com/2016/01/making-wall-e2-smarter-using-karnaugh-maps/ for details

////DEBUG!!
//			mySerial.printf("In MODE_WALLFOLLOW before GetTrackingDir() at time %lu\n", millis());
////DEBUG!!

			TrackingCase = (WallTrackingCases)GetTrackingDir();

			////DEBUG!!
			//			mySerial.printf("In MODE_WALLFOLLOW at time %lu with Tracking Case = %d\n", millis(), TrackingCase);
			////DEBUG!!

			if (PrevOpMode != MODE_WALLFOLLOW)
			{
				Serial.println(WallFollowTelemStr); //header for wall-follow telemetry data

				//09/09/19 maybe use a 'InApproachState' state variable here, changing to 'InTrackingState'
				//			once the proper standoff distance has been achieved?
			}

			PrevOpMode = MODE_WALLFOLLOW;

			switch (TrackingCase)
			{
			case TRACKING_LEFT:
				trackstr = "Left"; //used for telemetry printouts

				//Bkp & Rot Right = Tracking Left && (Near || Stuck)
				if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))
				{
					Serial.println("---- Backup & Turn to the Right");

					//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'
					BackupAndTurn(true, MOTOR_SPEED_HALF); //backup and turn 90 deg right //c/o 02/23/17 for IR Track debug

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				//Step-turn Right = Tracking Left && Step && !Stuck
				//this section *starts* the step-turn.  It is terminated as soon as frontdistval exits step-turn window
				else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)
				{
					//if we aren't already in Step-turn mode, start it.  Otherwise, skip
					if (NavCase != NAV_STEPTURN)
					{
						Serial.println("---- Starting Stepturn to the Right");
						NavCase = NAV_STEPTURN;
						leftspeednum = MOTOR_SPEED_MAX;
						rightspeednum = MOTOR_SPEED_LOW;
						MoveAhead(leftspeednum, rightspeednum);
					}
				}

				//Wall-tracking Left = Tracking Left && Far away from obstacles
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					if (NavCase == NAV_STEPTURN)
					{
						mySerial.printf("Exiting Step-turn to the right\n");
					}

					NavCase = NAV_WALLTRK;
					UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);
					MoveAhead(leftspeednum, rightspeednum);
				}

				/*				PrintWallFollowTelemetry(frontvar); *///added 04/15/19
				break;
			case TRACKING_RIGHT:
				trackstr = "Right"; //used for telemetry printouts

////DEBUG!!
//				mySerial.printf("In WALL_TRACKING_RIGHT top at %lu\n", millis());
////DEBUG!!

				if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))
				{
					Serial.println("---- Backup & Turn to the Left");
					//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'
					BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				////02/23/19 temporarily implemented to stop robot at end of TrkPID tuning run
				//if (frontdistval <= MIN_OBS_DIST_CM)
				//{
				//	Serial.println("---- End of run - stopping motors");
				//	MoveAhead(0, 0);
				//	delay(500);
				//	cli();
				//	sleep_enable();
				//	sleep_cpu();
				//}


				//Step-turn Left = Tracking Right && Step && !Stuck
				//this section *starts* the step-turn.  It is terminated as soon as frontdistval exits step-turn window
				//02/23/19 temporarily commented out for TrkPID tuning 
				else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)
				{
					//if we aren't already in Step-turn mode, start it.  Otherwise, skip
					if (NavCase != NAV_STEPTURN)
					{
						Serial.println("---- Starting Stepturn to the Left");
						NavCase = NAV_STEPTURN;
						rightspeednum = MOTOR_SPEED_MAX;
						leftspeednum = MOTOR_SPEED_LOW;
						MoveAhead(leftspeednum, rightspeednum);
					}
				}

				//Wall-tracking Right = Tracking Right && Far
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					if (NavCase == NAV_STEPTURN)
					{
						mySerial.printf("Exiting Step-turn to the left\n");
					}

					NavCase = NAV_WALLTRK;
					UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);
					MoveAhead(leftspeednum, rightspeednum);
				}


				////DEBUG!!
				//				mySerial.printf("In WALL_TRACKING_RIGHT just before printout at %lu\n", millis());
				////DEBUG!!


				//PrintWallFollowTelemetry(frontvar); //added 04/15/19
				break;
			case TRACKING_NEITHER:
				trackstr = "Neither"; //used for telemetry printouts
				mySerial.printf("time(ms) in TRACKNEITHER = %lu\n", millis());

				//added 01/20/16: turn direction arbitrary
				if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))
				{
					Serial.println("---- Backup & Turn to the Left");
					//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'
					BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				//Open Corner case
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					//OK, this is the Open Corner case - turn toward the last-tracked wall

#ifndef NO_MOTORS
					delay(1000); //01/20/16 - delay to give Wall-E2 time to clear the corner a bit
#endif

					if (PrevTrackingCase == TRACKING_LEFT)
					{
						//RotateCWDeg(false, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);
					}
					else if (PrevTrackingCase == TRACKING_RIGHT)
					{
						//RotateCWDeg(true, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);
					}
					else //oops!  not tracking either wall - out in middle of room somewhere?
					{
						//not open corner, but not tracking a wall, either - go straight
						leftspeednum = rightspeednum = MOTOR_SPEED_HALF;
						MoveAhead(leftspeednum, rightspeednum);
					}
				}
				break;
			default:
				break;
			} //switch (TrackingCase)

			digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
			UpdateRearLEDPanel(leftspeednum, rightspeednum); //using LEDs to visualize motor speeds
			break; //MODE_WALLFOLLOW
		}//switch (CurrentOpMode)

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

CurrentOpMode = (OpModes)GetOpMode(batVoltage); //04/28/19 bat volts added to sig

////DEBUG!!

// //CurrentOpMode = MODE_IRHOMING; //forced to MODE_IRHOMING for testing

// Serial.print("OpMode = ");

// Serial.println(CurrentOpMode);

////DEBUG!!

//Step2: Switch to appropriate operating case block

String trackstr = "Unknown"; //used for telemetry printouts

switch (CurrentOpMode)

{

case MODE_WALLFOLLOW:

//Purpose: Follow nearest wall, responding to obstacles as necessary

//Provenance: G. Frank Paynter and Danny Frank 12/20/2015

//Inputs: Distances from left/right/front sensors

//Outputs: updated left/right motor speeds in wall-follow case, or appropriate avoidance maneuver

//Plan:

// Step 1: Update motor speeds & NAV mode based on current TRACK/NAV states and front distance

// Step 2: Print out telemetry info

//Notes:

// 03/30/14 Step1 code moved to top of loop() - compiler directives cause errors here

// 09/11/18 revised plan to reflect reality

// Update motor speeds & NAV mode based on current TRACK/NAV states and front distance

// See https://fpaynter.com/2016/01/making-wall-e2-smarter-using-karnaugh-maps/ for details

////DEBUG!!

// mySerial.printf("In MODE_WALLFOLLOW before GetTrackingDir() at time %lu\n", millis());

////DEBUG!!

TrackingCase = (WallTrackingCases)GetTrackingDir();

////DEBUG!!

// mySerial.printf("In MODE_WALLFOLLOW at time %lu with Tracking Case = %d\n", millis(), TrackingCase);

////DEBUG!!

if (PrevOpMode != MODE_WALLFOLLOW)

{

Serial.println(WallFollowTelemStr); //header for wall-follow telemetry data

//09/09/19 maybe use a 'InApproachState' state variable here, changing to 'InTrackingState'

// once the proper standoff distance has been achieved?

}

PrevOpMode = MODE_WALLFOLLOW;

switch (TrackingCase)

{

case TRACKING_LEFT:

trackstr = "Left"; //used for telemetry printouts

//Bkp & Rot Right = Tracking Left && (Near || Stuck)

if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))

{

Serial.println("---- Backup & Turn to the Right");

//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'

BackupAndTurn(true, MOTOR_SPEED_HALF); //backup and turn 90 deg right //c/o 02/23/17 for IR Track debug

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

//Step-turn Right = Tracking Left && Step && !Stuck

//this section *starts* the step-turn. It is terminated as soon as frontdistval exits step-turn window

else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)

{

//if we aren't already in Step-turn mode, start it. Otherwise, skip

if (NavCase != NAV_STEPTURN)

{

Serial.println("---- Starting Stepturn to the Right");

NavCase = NAV_STEPTURN;

leftspeednum = MOTOR_SPEED_MAX;

rightspeednum = MOTOR_SPEED_LOW;

MoveAhead(leftspeednum, rightspeednum);

}

//Wall-tracking Left = Tracking Left && Far away from obstacles

else if (frontdistval > STEPTURN_DIST_CM)

{

if (NavCase == NAV_STEPTURN)

{

mySerial.printf("Exiting Step-turn to the right\n");

}

NavCase = NAV_WALLTRK;

UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);

MoveAhead(leftspeednum, rightspeednum);

}

/* PrintWallFollowTelemetry(frontvar); *///added 04/15/19

break;

case TRACKING_RIGHT:

trackstr = "Right"; //used for telemetry printouts

////DEBUG!!

// mySerial.printf("In WALL_TRACKING_RIGHT top at %lu\n", millis());

////DEBUG!!

if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))

{

Serial.println("---- Backup & Turn to the Left");

//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'

BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

////02/23/19 temporarily implemented to stop robot at end of TrkPID tuning run

//if (frontdistval <= MIN_OBS_DIST_CM)

//{

// Serial.println("---- End of run - stopping motors");

// MoveAhead(0, 0);

// delay(500);

// cli();

// sleep_enable();

// sleep_cpu();

//}

//Step-turn Left = Tracking Right && Step && !Stuck

//this section *starts* the step-turn. It is terminated as soon as frontdistval exits step-turn window

//02/23/19 temporarily commented out for TrkPID tuning

else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)

{

//if we aren't already in Step-turn mode, start it. Otherwise, skip

if (NavCase != NAV_STEPTURN)

{

Serial.println("---- Starting Stepturn to the Left");

NavCase = NAV_STEPTURN;

rightspeednum = MOTOR_SPEED_MAX;

leftspeednum = MOTOR_SPEED_LOW;

MoveAhead(leftspeednum, rightspeednum);

}

//Wall-tracking Right = Tracking Right && Far

else if (frontdistval > STEPTURN_DIST_CM)

{

if (NavCase == NAV_STEPTURN)

{

mySerial.printf("Exiting Step-turn to the left\n");

}

NavCase = NAV_WALLTRK;

UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);

MoveAhead(leftspeednum, rightspeednum);

}

////DEBUG!!

// mySerial.printf("In WALL_TRACKING_RIGHT just before printout at %lu\n", millis());

////DEBUG!!

//PrintWallFollowTelemetry(frontvar); //added 04/15/19

break;

case TRACKING_NEITHER:

trackstr = "Neither"; //used for telemetry printouts

mySerial.printf("time(ms) in TRACKNEITHER = %lu\n", millis());

//added 01/20/16: turn direction arbitrary

if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))

{

Serial.println("---- Backup & Turn to the Left");

//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'

BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

//Open Corner case

else if (frontdistval > STEPTURN_DIST_CM)

{

//OK, this is the Open Corner case - turn toward the last-tracked wall

#ifndef NO_MOTORS

delay(1000); //01/20/16 - delay to give Wall-E2 time to clear the corner a bit

#endif

if (PrevTrackingCase == TRACKING_LEFT)

{

//RotateCWDeg(false, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);

}

else if (PrevTrackingCase == TRACKING_RIGHT)

{

//RotateCWDeg(true, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);

}

else //oops! not tracking either wall - out in middle of room somewhere?

{

//not open corner, but not tracking a wall, either - go straight

leftspeednum = rightspeednum = MOTOR_SPEED_HALF;

MoveAhead(leftspeednum, rightspeednum);

}

break;

default:

break;

} //switch (TrackingCase)

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

UpdateRearLEDPanel(leftspeednum, rightspeednum); //using LEDs to visualize motor speeds

break; //MODE_WALLFOLLOW

}//switch (CurrentOpMode)

The two-wheel robot code uses a very simple left/right distance check to determine which wall to track. In the four-wheel code, the ‘GetTrackingDir()’ function uses a LR_PING_AVG_WINDOW_SIZE-point running average for left & right distances and returns TRACKING_LEFT, TRACKING_RIGHT, or TRACKING_NEITHER enum value.

09 April 2020 Update:

It looks like the two-wheel code actually checks the L/R distances twice; once in GetParallelHdg() and again in the main loop() code. Once it determines which wall to track, then it uses the same code for both tracking directions, with a ‘turndirection’ boolean to control which way the robot actually turns (CW or CCW) to effect capture and tracking.

The four-wheel code uses two LR_PING_DIST_ARRAY_SIZE buffers – aRightDist & aLeftDist – to hold ‘ping’ measurements. These arrays are updated every MIN_PING_INTERVAL_MSEC with the latest left/right distances, pushing older measurements down in the stack. The ‘GetTrackingDir()’ function computes the average of the LR_PING_AVG_WINDOW_SIZE latest measurements for tracking direction (left/right/neither) determination.

There are also two utility functions ‘GetAvgLeftDistCm()’ and ‘GetAvgRightDistCm()’ that are used in several places, but they don’t do a running average of the last LR_PING_AVG_WINDOW_SIZE measurements; instead they do an average of the first LR_PING_AVG_WINDOW_SIZE ones! Fortunately for the program right now the LR_PING_AVG_WINDOW_SIZE and LR_PING_DIST_ARRAY_SIZE values are the same — 3.

So, I think part of the port needs to involve normalizing the distance measurement situation. I think the proper way to do this is to revise the ‘GetAvgLeftDistCm()’ & ‘GetAvgRightDistCm()’ functions to compute the running average as it done currently now in GetTrackingDir(), and then call those functions there. This considerably simplifies GetTrackingDir() and increases its cohesion (in the software engineering sense) as it no longer contains any computations not directly related to its purpose. DONE in FourWD_WallE2_V3 project

The ‘GetTrackingDir()’ function is called in only one place – at the top of the ‘case MODE_WALLFOLLOW:’ block of the ‘ switch (CurrentOpMode)’ switch statement. The ‘TrackingCase’ enum value returned by ‘GetTrackingDir()’ is then used within the MODE_WALLFOLLOW:’ block in a new ‘switch (TrackingCase)’ switch statement to determine the appropriate action to be taken. Here’s the relevant code section:

			switch (TrackingCase)
			{
			case TRACKING_LEFT:
#pragma region TRACKLEFT
				trackstr = "Left"; //used for telemetry printouts

				//Bkp & Rot Right = Tracking Left && (Near || Stuck)
				if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))
				{
					Serial.println("---- Backup & Turn to the Right");

					//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'
					BackupAndTurn(true, MOTOR_SPEED_HALF); //backup and turn 90 deg right //c/o 02/23/17 for IR Track debug

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				//Step-turn Right = Tracking Left && Step && !Stuck
				//this section *starts* the step-turn.  It is terminated as soon as frontdistval exits step-turn window
				else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)
				{
					//if we aren't already in Step-turn mode, start it.  Otherwise, skip
					if (NavCase != NAV_STEPTURN)
					{
						Serial.println("---- Starting Stepturn to the Right");
						NavCase = NAV_STEPTURN;
						leftspeednum = MOTOR_SPEED_MAX;
						rightspeednum = MOTOR_SPEED_LOW;
						MoveAhead(leftspeednum, rightspeednum);
					}
				}

				//Wall-tracking Left = Tracking Left && Far away from obstacles
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					if (NavCase == NAV_STEPTURN)
					{
						mySerial.printf("Exiting Step-turn to the right\n");
					}

					NavCase = NAV_WALLTRK;
					UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);
					MoveAhead(leftspeednum, rightspeednum);
				}

				/*				PrintWallFollowTelemetry(frontvar); *///added 04/15/19
#pragma endregion Tracking Left Wall Case
				break;
			case TRACKING_RIGHT:
#pragma region TRACKRIGHT
				trackstr = "Right"; //used for telemetry printouts

////DEBUG!!
//				mySerial.printf("In WALL_TRACKING_RIGHT top at %lu\n", millis());
////DEBUG!!

				if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))
				{
					Serial.println("---- Backup & Turn to the Left");
					//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'
					BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				////02/23/19 temporarily implemented to stop robot at end of TrkPID tuning run
				//if (frontdistval <= MIN_OBS_DIST_CM)
				//{
				//	Serial.println("---- End of run - stopping motors");
				//	MoveAhead(0, 0);
				//	delay(500);
				//	cli();
				//	sleep_enable();
				//	sleep_cpu();
				//}


				//Step-turn Left = Tracking Right && Step && !Stuck
				//this section *starts* the step-turn.  It is terminated as soon as frontdistval exits step-turn window
				//02/23/19 temporarily commented out for TrkPID tuning 
				else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)
				{
					//if we aren't already in Step-turn mode, start it.  Otherwise, skip
					if (NavCase != NAV_STEPTURN)
					{
						Serial.println("---- Starting Stepturn to the Left");
						NavCase = NAV_STEPTURN;
						rightspeednum = MOTOR_SPEED_MAX;
						leftspeednum = MOTOR_SPEED_LOW;
						MoveAhead(leftspeednum, rightspeednum);
					}
				}

				//Wall-tracking Right = Tracking Right && Far
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					if (NavCase == NAV_STEPTURN)
					{
						mySerial.printf("Exiting Step-turn to the left\n");
					}

					NavCase = NAV_WALLTRK;
					UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);
					MoveAhead(leftspeednum, rightspeednum);
				}


				////DEBUG!!
				//				mySerial.printf("In WALL_TRACKING_RIGHT just before printout at %lu\n", millis());
				////DEBUG!!


				//PrintWallFollowTelemetry(frontvar); //added 04/15/19
#pragma endregion Tracking Right Wall Case
				break;
			case TRACKING_NEITHER:
#pragma region TRACKNEITHER
				trackstr = "Neither"; //used for telemetry printouts
				mySerial.printf("time(ms) in TRACKNEITHER = %lu\n", millis());

				//added 01/20/16: turn direction arbitrary
				if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))
				{
					Serial.println("---- Backup & Turn to the Left");
					//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'
					BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				//Open Corner case
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					//OK, this is the Open Corner case - turn toward the last-tracked wall

#ifndef NO_MOTORS
					delay(1000); //01/20/16 - delay to give Wall-E2 time to clear the corner a bit
#endif

					if (PrevTrackingCase == TRACKING_LEFT)
					{
						//RotateCWDeg(false, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);
					}
					else if (PrevTrackingCase == TRACKING_RIGHT)
					{
						//RotateCWDeg(true, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);
					}
					else //oops!  not tracking either wall - out in middle of room somewhere?
					{
						//not open corner, but not tracking a wall, either - go straight
						leftspeednum = rightspeednum = MOTOR_SPEED_HALF;
						MoveAhead(leftspeednum, rightspeednum);
					}
				}
#pragma endregion Not Tracking Either Wall
				break;
			default:
				break;
			} //switch (TrackingCase)

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

switch (TrackingCase)

{

case TRACKING_LEFT:

#pragma region TRACKLEFT

trackstr = "Left"; //used for telemetry printouts

//Bkp & Rot Right = Tracking Left && (Near || Stuck)

if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))

{

Serial.println("---- Backup & Turn to the Right");

//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'

BackupAndTurn(true, MOTOR_SPEED_HALF); //backup and turn 90 deg right //c/o 02/23/17 for IR Track debug

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

//Step-turn Right = Tracking Left && Step && !Stuck

//this section *starts* the step-turn. It is terminated as soon as frontdistval exits step-turn window

else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)

{

//if we aren't already in Step-turn mode, start it. Otherwise, skip

if (NavCase != NAV_STEPTURN)

{

Serial.println("---- Starting Stepturn to the Right");

NavCase = NAV_STEPTURN;

leftspeednum = MOTOR_SPEED_MAX;

rightspeednum = MOTOR_SPEED_LOW;

MoveAhead(leftspeednum, rightspeednum);

}

//Wall-tracking Left = Tracking Left && Far away from obstacles

else if (frontdistval > STEPTURN_DIST_CM)

{

if (NavCase == NAV_STEPTURN)

{

mySerial.printf("Exiting Step-turn to the right\n");

}

NavCase = NAV_WALLTRK;

UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);

MoveAhead(leftspeednum, rightspeednum);

}

/* PrintWallFollowTelemetry(frontvar); *///added 04/15/19

#pragma endregion Tracking Left Wall Case

break;

case TRACKING_RIGHT:

#pragma region TRACKRIGHT

trackstr = "Right"; //used for telemetry printouts

////DEBUG!!

// mySerial.printf("In WALL_TRACKING_RIGHT top at %lu\n", millis());

////DEBUG!!

if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))

{

Serial.println("---- Backup & Turn to the Left");

//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'

BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

////02/23/19 temporarily implemented to stop robot at end of TrkPID tuning run

//if (frontdistval <= MIN_OBS_DIST_CM)

//{

// Serial.println("---- End of run - stopping motors");

// MoveAhead(0, 0);

// delay(500);

// cli();

// sleep_enable();

// sleep_cpu();

//}

//Step-turn Left = Tracking Right && Step && !Stuck

//this section *starts* the step-turn. It is terminated as soon as frontdistval exits step-turn window

//02/23/19 temporarily commented out for TrkPID tuning

else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)

{

//if we aren't already in Step-turn mode, start it. Otherwise, skip

if (NavCase != NAV_STEPTURN)

{

Serial.println("---- Starting Stepturn to the Left");

NavCase = NAV_STEPTURN;

rightspeednum = MOTOR_SPEED_MAX;

leftspeednum = MOTOR_SPEED_LOW;

MoveAhead(leftspeednum, rightspeednum);

}

//Wall-tracking Right = Tracking Right && Far

else if (frontdistval > STEPTURN_DIST_CM)

{

if (NavCase == NAV_STEPTURN)

{

mySerial.printf("Exiting Step-turn to the left\n");

}

NavCase = NAV_WALLTRK;

UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);

MoveAhead(leftspeednum, rightspeednum);

}

////DEBUG!!

// mySerial.printf("In WALL_TRACKING_RIGHT just before printout at %lu\n", millis());

////DEBUG!!

//PrintWallFollowTelemetry(frontvar); //added 04/15/19

#pragma endregion Tracking Right Wall Case

break;

case TRACKING_NEITHER:

#pragma region TRACKNEITHER

trackstr = "Neither"; //used for telemetry printouts

mySerial.printf("time(ms) in TRACKNEITHER = %lu\n", millis());

//added 01/20/16: turn direction arbitrary

if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))

{

Serial.println("---- Backup & Turn to the Left");

//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'

BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

//Open Corner case

else if (frontdistval > STEPTURN_DIST_CM)

{

//OK, this is the Open Corner case - turn toward the last-tracked wall

#ifndef NO_MOTORS

delay(1000); //01/20/16 - delay to give Wall-E2 time to clear the corner a bit

#endif

if (PrevTrackingCase == TRACKING_LEFT)

{

//RotateCWDeg(false, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);

}

else if (PrevTrackingCase == TRACKING_RIGHT)

{

//RotateCWDeg(true, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);

}

else //oops! not tracking either wall - out in middle of room somewhere?

{

//not open corner, but not tracking a wall, either - go straight

leftspeednum = rightspeednum = MOTOR_SPEED_HALF;

MoveAhead(leftspeednum, rightspeednum);

}

#pragma endregion Not Tracking Either Wall

break;

default:

break;

} //switch (TrackingCase)

looking at just the ‘TRACKING_LEFT’ section,

			case TRACKING_LEFT:
				trackstr = "Left"; //used for telemetry printouts

				//Bkp & Rot Right = Tracking Left && (Near || Stuck)
				if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))
				{
					Serial.println("---- Backup & Turn to the Right");

					//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'
					BackupAndTurn(true, MOTOR_SPEED_HALF); //backup and turn 90 deg right //c/o 02/23/17 for IR Track debug

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				//Step-turn Right = Tracking Left && Step && !Stuck
				//this section *starts* the step-turn.  It is terminated as soon as frontdistval exits step-turn window
				else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)
				{
					//if we aren't already in Step-turn mode, start it.  Otherwise, skip
					if (NavCase != NAV_STEPTURN)
					{
						Serial.println("---- Starting Stepturn to the Right");
						NavCase = NAV_STEPTURN;
						leftspeednum = MOTOR_SPEED_MAX;
						rightspeednum = MOTOR_SPEED_LOW;
						MoveAhead(leftspeednum, rightspeednum);
					}
				}

				//Wall-tracking Left = Tracking Left && Far away from obstacles
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					if (NavCase == NAV_STEPTURN)
					{
						mySerial.printf("Exiting Step-turn to the right\n");
					}

					NavCase = NAV_WALLTRK;
					UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);
					MoveAhead(leftspeednum, rightspeednum);
				}

				break;

case TRACKING_LEFT:

trackstr = "Left"; //used for telemetry printouts

//Bkp & Rot Right = Tracking Left && (Near || Stuck)

if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))

{

Serial.println("---- Backup & Turn to the Right");

//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'

BackupAndTurn(true, MOTOR_SPEED_HALF); //backup and turn 90 deg right //c/o 02/23/17 for IR Track debug

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

//Step-turn Right = Tracking Left && Step && !Stuck

//this section *starts* the step-turn. It is terminated as soon as frontdistval exits step-turn window

else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)

{

//if we aren't already in Step-turn mode, start it. Otherwise, skip

if (NavCase != NAV_STEPTURN)

{

Serial.println("---- Starting Stepturn to the Right");

NavCase = NAV_STEPTURN;

leftspeednum = MOTOR_SPEED_MAX;

rightspeednum = MOTOR_SPEED_LOW;

MoveAhead(leftspeednum, rightspeednum);

}

//Wall-tracking Left = Tracking Left && Far away from obstacles

else if (frontdistval > STEPTURN_DIST_CM)

{

if (NavCase == NAV_STEPTURN)

{

mySerial.printf("Exiting Step-turn to the right\n");

}

NavCase = NAV_WALLTRK;

UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum);

MoveAhead(leftspeednum, rightspeednum);

}

break;

There are three potential actions available in this section; a ‘back up and turn’ obstacle avoidance maneuver, a ‘step-turn to the right’ “upcoming obstacle” avoidance maneuver, and a “continue wall tracking” motor speed adjustment action.

It’s the “continue wall tracking” action that is of interest for porting the new tracking algorithm. At this point, if this is the first time for the TRACKING_LEFT mode, the robot needs to execute the FindParallelHdg() routine, then capture the offset distance, and then start tracking. If the previous mode was TRACKING_LEFT, then just continue tracking.

A potential problem with the port idea is that the ‘FindParallelHdg()’ and offset capture routines are ‘blocking’ functions, so if something else happens (like the robot runs into an obstacle), it might not ever recover. In the current four wheel code, this is handled by checking for obstacle clearance each time through the MIN_PING_INTERVAL_MSEC interval check. Maybe I can incorporate this idea into the ‘capture’ and ‘maintenance’ phases of the angle-based wall tracking algorithm. Maybe something like the following state diagram?

Possible state diagram for the TRK_RIGHT case

11 April 2020 Update:

I’m concentrating on the TRACKING_RIGHT sub-case in MODE_WALLFOLLOW, because my ‘local’ (in my office) test range is optimized for tracking the right-hand wall, and I figure I should work our the bugs on one side first.

Ported the ‘FindParallelHdg()’ code from the two wheel to the four wheel project, and in the process I changed the name to ‘RotateToParallelOrientation()’ to more accurately describe what the function does.
In porting over the actual code that decides what ‘cut’ to use to capture and maintain the desired offset, I realized this should be it’s own function so it can be called from both the left and righthand tracking algorithms. Then I discovered it already was a function in the two wheel program – but wasn’t being used that way for some unknown reason. Ported the ‘MakeTrackingTurn()’ function to FourWD_WallE_V3

26 April 2020 Update – Charging Station:

While trying (and failing so far) to work out the wall-following ‘capture/maintain’ algorithms for the four wheel robot, the battery voltage got down to the point where the ‘GetOpMode()’ function was starting to return DEAD_BATTERY. So, I decided this was a good time to complete the required software & hardware modifications to the charging station to work with the new 90 mm x 10 mm wheels I recently added to the robot. To accommodate the larger diameter wheels I raised the entire charging station electronics platform up by some 14 mm. To accommodate the much narrower wheel width, I had to completely redo the wheel guard geometry, which also required re-aligning the charging station approach guides.

When I was all done with the required physical mods, I discovered that although the robot would still home to the charging station, it wouldn’t shut off its motors when it finally connected to the charging probe. I could see from telemetry that the probe plug had successfully engaged the probe jack’s integrated switch, but the motors continued to run. However, if I lifted the robot slightly off its wheels from the back while keeping it plugged in, the motors stopped immediately, and the charging operation proceeded normally. It appeared for all the world like the plug was only partially engaged into the jack. So, I tried the same trick with the robot on its stand (this raises the robot up slightly so the motors can turn without the robot running off on me) and a second probe plug tied to the power supply but physically separate from the charging station, and this worked fine; as soon as the plug was pushed into the jack, the motors turned off and life was good.

So, back to the charging station; I thought maybe the plug wasn’t making full contact due to misalignment and after critically examining the geometry I made some adjustments. However, this did not solve the problem, even when the plug was perfectly aligned with the jack. But, with the plug alignment cone attached to the robot it is hard to see whether or not the plug is fully inserted into the jack, so I still thought that maybe I just needed to have the robot plug in with a bit more authority. To this end I modified the software to monitor the LIDAR distance measurement as the robot approached the charging station, and have the robot speed up to max wheel speed when it got within 20 cm. I also printed up a ‘target panel’ for the charging station so the LIDAR would have a consistent target to work with. This worked great, but still didn’t solve the problem; the robot clearly sped up at the end of the approach maneuver, but also still literally “spun its wheels” after hitting the charging station stops. Lifting the back slightly still caused the motors to stop and for charging to proceed normally. However, I was now convinced this phenomenon wasn’t due to plug/jack misalignment, and I had already confirmed that all electrical connections were correct. So, having eliminated the hardware, the software must be at fault

So, now I was forced to dissect the software controlling the transition from wall-following to IR homing to battery charge monitoring. In order for the robot to transition from the IR Homing mode to the charge monitoring mode, the following conditions had to exist:

The physical charging jack switch must be OPEN, causing the voltage read by the associated MEGA pin to go HIGH.
12V must be present at the charging jack +V pin
The Difference between the Total current (measured by the 1NA169 ‘high-side’ current sensor located between the charging jack and the battery charger) and the Run current (measured by the 1NA169 between the power switch and the rest of the robot) must be positive and greater than 0.5A.

Of the above conditions, I was able to directly measure the first two in both the ‘working’ (with the robot on its stand and using the auxiliary charging probe) and ‘non-working’ (when the robot engaged the charging plug automatically) conditions and verified that they were both met in both cases. That left the third condition – the I_Total – I_Run condition.

The reason for the I_Total – I_Run condition is to properly manage charge cutoff at the 80% battery capacity point. The robot has a resting (idle) current of about 0.2A, so a 0.5A difference would mean that the battery charge current has decreased to 0.3A, which is slightly above the recommended 90% capacity level (see this post for a more detailed discussion). So, this condition is included in the ‘GetOpMode()’ algorithm for determining when the battery is charging, and when the battery is fully charged. In normal operation, I_Total = 0 (nothing connected to the charger) and I_Run = Robot running current, so I_Total – I_Run < 0 and the IsCharging () function returns FALSE. When the robot is connected to a charger, I_Total is usually around 1.5A initially, and I_Total – I_Run > 0.5, causing IsCharging() to return TRUE and the robot to enter the CHARGING mode, which disables the motors. What I didn’t realize though is that the larger diameter wheels and better motors cause I_Run to be a lot higher than I had anticipated, which means that when the robot plugs into the charging station, the I_Run value goes over 2A with all four motors stalled. This in turn means that the and I_Total – I_Run > 0.5 condition is never met, IsCharging() continues to return FALSE, and the motors never turn off – OOPS!

So, how to fix this problem? It appears that I don’t want to use the I_Total – I_Run > 0.5 condition as part of the IR Homing –> CHARGING mode transition, but I do want to use it as part of the CHARGING –> CHARGE_DISCONNECT mode transition. This should work, but this exercise got me thinking about how I the charge operation relates to the rest of the robot’s behavior.

The basic idea is for the robot to autonomously seek out and connect to on of a number of charging stations whenever it is ‘hungry’, defined by a battery voltage less than some set threshold, and to avoid those same stations when it isn’t. When the robot is ‘hungry’ and a charging station signal is detected, the robot should home in on the station and connect to a charging probe, stop its motors, wait until the battery is 80% or so charged, and then back out of the station and go on its merry way.

As currently programmed, the robot operates in one of several modes as shown below.

	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

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

The program calls a function called ‘GetOpMode’ every 400 msec to determine the proper mode based on sensor input and (to some degree) past history). The GetOpMode() function is shown below:

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

//Step1: If batt voltage too low, return MODE_DEADBATTERY (4)
	if (batv <= DEAD_BATT_THRESH_VOLTS) return MODE_DEADBATTERY;

	//Step2: If Charger is connected, return MODE_CHARGING (1)
	//04/26/20 only  use IsStillCharging() if previous op mode is MODE_CHARGING
	//else if (IsChargerConnected() && IsStillCharging()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor
	else if (PrevOpMode != MODE_CHARGING) //first time through with MODE_CHARGING
	{
		if (IsChargerConnected()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor
		PrevOpMode = MODE_CHARGING; //this causes next 'elseif' to run until charge mode exits
		mySerial.printf("PrevOpMode = %s, IsChargerConnected() returns %s\n",
			LongModeStrArray[PrevOpMode], IsChargerConnected() ? "TRUE" : "FALSE");
	}
	else if (PrevOpMode == MODE_CHARGING) //subsequent times through with MODE_CHARGING
	{
		if (IsChargerConnected() && IsStillCharging()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor
	}

	//else if (IsChargerConnected() && IsStillCharging()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor

//Step3: Else If IR Homing beam detected, return MODE_IRHOMING (2)
	else if (IsIRBeamAvail()) return MODE_IRHOMING; //this handles 'hungry/not-hungry' cases internally

//03/04/2020 added to facilitate battery discharge
#ifdef BATTERY_DISCHARGE
	while (GetBattVoltage() > DEAD_BATT_THRESH_VOLTS)
	{
		return MODE_DISCHARGE;
	}
	StopBothMotors();
	mySerial.printf("Battery Discharge Complete at %3.2f Minutes and %3.2f Volts", millis() / 60000.f, GetBattVoltage);

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

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

//Step1: If batt voltage too low, return MODE_DEADBATTERY (4)

if (batv <= DEAD_BATT_THRESH_VOLTS) return MODE_DEADBATTERY;

//Step2: If Charger is connected, return MODE_CHARGING (1)

//04/26/20 only use IsStillCharging() if previous op mode is MODE_CHARGING

//else if (IsChargerConnected() && IsStillCharging()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor

else if (PrevOpMode != MODE_CHARGING) //first time through with MODE_CHARGING

{

if (IsChargerConnected()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor

PrevOpMode = MODE_CHARGING; //this causes next 'elseif' to run until charge mode exits

mySerial.printf("PrevOpMode = %s, IsChargerConnected() returns %s\n",

LongModeStrArray[PrevOpMode], IsChargerConnected() ? "TRUE" : "FALSE");

}

else if (PrevOpMode == MODE_CHARGING) //subsequent times through with MODE_CHARGING

{

if (IsChargerConnected() && IsStillCharging()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor

}

//else if (IsChargerConnected() && IsStillCharging()) return MODE_CHARGING; //02/24/19 now using 1NA619 charge current sensor

//Step3: Else If IR Homing beam detected, return MODE_IRHOMING (2)

else if (IsIRBeamAvail()) return MODE_IRHOMING; //this handles 'hungry/not-hungry' cases internally

//03/04/2020 added to facilitate battery discharge

#ifdef BATTERY_DISCHARGE

while (GetBattVoltage() > DEAD_BATT_THRESH_VOLTS)

{

return MODE_DISCHARGE;

}

StopBothMotors();

mySerial.printf("Battery Discharge Complete at %3.2f Minutes and %3.2f Volts", millis() / 60000.f, GetBattVoltage);

#else

//Step4: Else return MODE_WALLFOLLOW (3)

else return MODE_WALLFOLLOW;

#endif // BATTERY_DISCHARGE

}

The return value from GetOpMode() is used in a SWITCH block to determine the appropriate actions to be taken, as shown below, with the MODE_WALLFOLLOW case reduced to one line for clarity:

		switch (CurrentOpMode)
		{
#pragma region MODE_CHARGING
		case MODE_CHARGING:
			digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
			chgStartMsec = millis(); //used for charge duration telemetry
			Serial.print("Starting Charge Mode at "); Serial.println(chgStartMsec);
			StopBothMotors(); //added 03/13/17
			PrevOpMode = MODE_CHARGING;
			//Serial.println(ChargingTelemStr); 02/28/19 moved into MonitorChargeUntilDone()
			MonitorChargeUntilDone(); //10/16/17 removed chgStartMsec from call sig
			Serial.print("Charging Complete. Executing disconnect maneuver");
			ExecDisconManeuver(); //disconnect, backup, and turn 90deg from near wall
			Serial.println("After ExexDisconManeuver");
			break;
#pragma endregion MODE_CHARGING

#pragma region MODE_IRHOMING
		case MODE_IRHOMING:
			digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
			mySerial.printf("IR Beam Detected with Battery Voltage = %1.2f\n", batVoltage);
			PrevOpMode = MODE_IRHOMING;

			if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry
			{
				Serial.println("Not Hungry - Avoiding Charger");
				curMinObstacleDistance = CHG_STN_AVOIDANCE_DIST_CM; //hopefully this will cause avoidance
				IRHomeToChgStn(CHG_STN_AVOIDANCE_DIST_CM, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
			}
			else //hungry - try to connect
			{
				Serial.println("Low battery - homing to Charger");
				IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
			}
			break;
#pragma endregion IR Homing Case

#pragma region MODE_WALLFOLLOW
		case MODE_WALLFOLLOW:
#pragma endregion Wall Following Case

#pragma region MODE_DEADBATTERY
		case MODE_DEADBATTERY:
			Serial.println("Dead Battery Detected!");
			digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

			YellForHelp(); //this function never returns
			break;
#pragma endregion Dead Battery Case

#pragma region MODE_DISCHARGE
		case MODE_DISCHARGE:

			//have to explicitely set the direction, as StopBothMotors() disables everything
			SetLeftMotorDir(FWD_DIR);
			SetRightMotorDir(FWD_DIR);

			RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);
			mySerial.printf("Discharge Mode %2.2f:  BattV, IRun = %2.3f, %2.3f\n",
				millis() / 60000.f, GetBattVoltage(), GetRunningAmps());
			break;
#pragma endregion MODE_DISCHARGE
		default:
			break;
		}//switch (CurrentOpMode)
#pragma endregion OP_MODE SWITCH

switch (CurrentOpMode)

{

#pragma region MODE_CHARGING

case MODE_CHARGING:

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

chgStartMsec = millis(); //used for charge duration telemetry

Serial.print("Starting Charge Mode at "); Serial.println(chgStartMsec);

StopBothMotors(); //added 03/13/17

PrevOpMode = MODE_CHARGING;

//Serial.println(ChargingTelemStr); 02/28/19 moved into MonitorChargeUntilDone()

MonitorChargeUntilDone(); //10/16/17 removed chgStartMsec from call sig

Serial.print("Charging Complete. Executing disconnect maneuver");

ExecDisconManeuver(); //disconnect, backup, and turn 90deg from near wall

Serial.println("After ExexDisconManeuver");

break;

#pragma endregion MODE_CHARGING

#pragma region MODE_IRHOMING

case MODE_IRHOMING:

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

mySerial.printf("IR Beam Detected with Battery Voltage = %1.2f\n", batVoltage);

PrevOpMode = MODE_IRHOMING;

if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry

{

Serial.println("Not Hungry - Avoiding Charger");

curMinObstacleDistance = CHG_STN_AVOIDANCE_DIST_CM; //hopefully this will cause avoidance

IRHomeToChgStn(CHG_STN_AVOIDANCE_DIST_CM, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

}

else //hungry - try to connect

{

Serial.println("Low battery - homing to Charger");

IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

}

break;

#pragma endregion IR Homing Case

#pragma region MODE_WALLFOLLOW

case MODE_WALLFOLLOW:

#pragma endregion Wall Following Case

#pragma region MODE_DEADBATTERY

case MODE_DEADBATTERY:

Serial.println("Dead Battery Detected!");

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

YellForHelp(); //this function never returns

break;

#pragma endregion Dead Battery Case

#pragma region MODE_DISCHARGE

case MODE_DISCHARGE:

//have to explicitely set the direction, as StopBothMotors() disables everything

SetLeftMotorDir(FWD_DIR);

SetRightMotorDir(FWD_DIR);

RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

mySerial.printf("Discharge Mode %2.2f: BattV, IRun = %2.3f, %2.3f\n",

millis() / 60000.f, GetBattVoltage(), GetRunningAmps());

break;

#pragma endregion MODE_DISCHARGE

default:

break;

}//switch (CurrentOpMode)

#pragma endregion OP_MODE SWITCH

In the normal sequence of events, the MODE_IRHOMING case will be executed first. If a charging station signal is detected, the robot will call IRHomeToChgStn() whether or not it is ‘hungry’. However, if it is ‘not hungry’ IRHomeToChgStn() will return FALSE with the robot oriented 90 degrees to the charging station, which should cause the program to re-enter WALL_FOLLOW mode. If the robot is ‘hungry’ and successfully connects to the charging probe, IRHomeToChgStn() returns TRUE. In either case, the program exits the SWITCH block and goes back the top of loop() to start all over again. The return value of IRHomeToChgStn() is not actually ever used.

The next time GetOpMode() runs, if the robot is indeed connected to the charging station, GetOpMode() returns MODE_CHARGING. When this section of the SWITCH statement is executed, the motors are stopped and MonitorChargeUntilDone() is called. This function is a blocking call, and doesn’t exit until the robot is fully charged or the timeout value has been reached. When MonitorChargeUntilDone() returns, the robot backs away from the charging station, turns 90 deg, and returns to wall-following.

When looking back through the above paragraphs, it becomes clear that managing the charging process is broken into two separate but interdependent parts; GetOpMode() recognizes the conditions for entering charging and returns with MODE_CHARGING. The MODE_CHARGING section of the SWITCH block in loop() actually executes the steps required to begin charging (like stopping the motors, turning off the red laser) and the steps required to disconnect at end-of-charge.

It also becomes clear that once the MODE_CHARGING section of the SWITCH statement is entered, it stays there until MonitorChargeUntilDone() returns at end-of-charge, and the robot disconnects from the charging station. I think this means the GetOpMode() code can be significantly simplified and made much more readable. Here’s the new version of GetOpMode():

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)

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

//Step1: If batt voltage too low, return MODE_DEADBATTERY (4)
	if (batv <= DEAD_BATT_THRESH_VOLTS) mode = MODE_DEADBATTERY;

	//Step2: If Charger is connected, return MODE_CHARGING (1)
	else if (IsChargerConnected())
	{
		mode = MODE_CHARGING;
		mySerial.printf("Robot has successfully connected to charger!\n");
	}

//Step3: Else If IR Homing beam detected, return MODE_IRHOMING (2)
	else if (IsIRBeamAvail()) mode = MODE_IRHOMING; //this handles 'hungry/not-hungry' cases internally

//03/04/2020 added to facilitate battery discharge
#ifdef BATTERY_DISCHARGE
	while (GetBattVoltage() > DEAD_BATT_THRESH_VOLTS)
	{
		mode = MODE_DISCHARGE;
	}
	StopBothMotors();
	mySerial.printf("Battery Discharge Complete at %3.2f Minutes and %3.2f Volts", millis() / 60000.f, GetBattVoltage);

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

	return mode;
}

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)

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

//Step1: If batt voltage too low, return MODE_DEADBATTERY (4)

if (batv <= DEAD_BATT_THRESH_VOLTS) mode = MODE_DEADBATTERY;

//Step2: If Charger is connected, return MODE_CHARGING (1)

else if (IsChargerConnected())

{

mode = MODE_CHARGING;

mySerial.printf("Robot has successfully connected to charger!\n");

}

//Step3: Else If IR Homing beam detected, return MODE_IRHOMING (2)

else if (IsIRBeamAvail()) mode = MODE_IRHOMING; //this handles 'hungry/not-hungry' cases internally

//03/04/2020 added to facilitate battery discharge

#ifdef BATTERY_DISCHARGE

while (GetBattVoltage() > DEAD_BATT_THRESH_VOLTS)

{

mode = MODE_DISCHARGE;

}

StopBothMotors();

mySerial.printf("Battery Discharge Complete at %3.2f Minutes and %3.2f Volts", millis() / 60000.f, GetBattVoltage);

#else

//Step4: Else return MODE_WALLFOLLOW (3)

else mode = MODE_WALLFOLLOW;

#endif // BATTERY_DISCHARGE

return mode;

}

29 April 2020 Update – Tracking (Again):

While screwing around with the charging station code, I managed to charge Wall-E2’s battery to the point where it refuses to connect to the charging station – the ‘Not Hungry’ condition. Rather than just let it run down by running the motors on its stand, I decided to continue working on the wall-following code improvements ported over from the two wheel model.

The last time I worked on the tracking code was back on 11 April when I ported the ‘RotateToParallelOrientation()’ and ‘MakeTrackingTurn()’ functions from the two wheel robot.

For reference, here’s where ended up with the TRACKING_RIGHT case from last time:

#pragma region TRACKRIGHT
				trackstr = "Right"; //used for telemetry printouts

//DEBUG!!
				//mySerial.printf("%lu: In WALL_TRACKING_RIGHT top\n", millis());
//DEBUG!!
				//04/19/20 revised to disable stepturn detection during CAPTURE
				int stepturn_dist_cm = CurrentTrackingState == TRK_RIGHT_CAPTURE ? 0 : STEPTURN_DIST_CM;

				if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))
				{
					Serial.println("---- Backup & Turn to the Left");
					//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'
					BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				//Step-turn Left = Tracking Right && Step && !Stuck
				//04/10/20 revised to just call new 'StepTurn()' function
				//else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)
				else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= stepturn_dist_cm)
				{
					//if we aren't already in Step-turn mode, start it.  Otherwise, skip
					if (NavCase != NAV_STEPTURN)
					{
						Serial.println("---- Starting Stepturn to the Left");
						StepTurn(TRACKING_RIGHT);
					}
				}

				//Wall-tracking Right = Tracking Right && Far
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					NavCase = NAV_WALLTRK;

					//if first time through, rotate robot to parallel
					if (CurrentTrackingState == TRK_RIGHT_NONE) //first time through
					{
						float par_hdg = RotateToParallelOrientation(TRACKING_RIGHT, DESIRED_WALL_OFFSET_DIST_CM); //this is port of two-wheel FindParallelHgd()

						mySerial.printf("%lu: parallel heading found at %2.3f deg\n", millis(), par_hdg);
//DEBUG!!
						StopBothMotors();
						delay(2000); //04/15/20 just for debugging purposes
//DEBUG!!
						CurrentTrackingState = TRK_RIGHT_CAPTURE;
					}

					else if (CurrentTrackingState == TRK_RIGHT_CAPTURE)
					{
						//OK, still in capture mode.  Adjust heading as necessary
						//right wall is closer, so CW moves twd wall, CCW moves away

						//04/19/20 have to temporarily disable obstacle detection

						loc_disterror = loc_rightdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away'
						turndirection = loc_disterror > 0 ? TURNDIR_CW : TURNDIR_CCW;
						turndeg = GetTrackTurnDeg(loc_disterror, m_TrkErrWinMult);
						MakeTrackingTurn(turndirection, turndeg);
						MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //added 04/21/20

						////04/17/20 new experiment; use LIDAR dist to achieve capture
						//if (rightdistval > DESIRED_WALL_OFFSET_DIST_CM)
						//{
						//	//make a 90 deg right turn, then a 90 left just before desired offset
						//	RollingTurn(false, true, 45.f);
						//	StopBothMotors();
						//	delay(2000);

						//	MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
						//	int turn_threshold = 1.4 * DESIRED_WALL_OFFSET_DIST_CM ;
						//	mySerial.printf("turn threshold = %d\n", turn_threshold);
						//	while (frontdistval > turn_threshold)
						//	{
						//		frontdistval = GetFrontDistCm();
						//		mySerial.printf("Front distance = %d, threshold = %d\n", frontdistval, turn_threshold);
						//		delay(10);
						//	}

						//	//now turn back to parallel
						//	RollingTurn(true, true, 45.f);
						//	StopBothMotors();
						//	while (true)
						//	{

						//	}
							
						//}
					}
				}

				////DEBUG!!
				//				mySerial.printf("In WALL_TRACKING_RIGHT just before printout at %lu\n", millis());
				////DEBUG!!


				//PrintWallFollowTelemetry(frontvar); //added 04/15/19
#pragma endregion Tracking Right Wall Case

100

#pragma region TRACKRIGHT

trackstr = "Right"; //used for telemetry printouts

//DEBUG!!

//mySerial.printf("%lu: In WALL_TRACKING_RIGHT top\n", millis());

//DEBUG!!

//04/19/20 revised to disable stepturn detection during CAPTURE

int stepturn_dist_cm = CurrentTrackingState == TRK_RIGHT_CAPTURE ? 0 : STEPTURN_DIST_CM;

if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar))

{

Serial.println("---- Backup & Turn to the Left");

//09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction'

BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left //c/o 02/23/17 for IR Track debug

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

//Step-turn Left = Tracking Right && Step && !Stuck

//04/10/20 revised to just call new 'StepTurn()' function

//else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM)

else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= stepturn_dist_cm)

{

//if we aren't already in Step-turn mode, start it. Otherwise, skip

if (NavCase != NAV_STEPTURN)

{

Serial.println("---- Starting Stepturn to the Left");

StepTurn(TRACKING_RIGHT);

}

//Wall-tracking Right = Tracking Right && Far

else if (frontdistval > STEPTURN_DIST_CM)

{

NavCase = NAV_WALLTRK;

//if first time through, rotate robot to parallel

if (CurrentTrackingState == TRK_RIGHT_NONE) //first time through

{

float par_hdg = RotateToParallelOrientation(TRACKING_RIGHT, DESIRED_WALL_OFFSET_DIST_CM); //this is port of two-wheel FindParallelHgd()

mySerial.printf("%lu: parallel heading found at %2.3f deg\n", millis(), par_hdg);

//DEBUG!!

StopBothMotors();

delay(2000); //04/15/20 just for debugging purposes

//DEBUG!!

CurrentTrackingState = TRK_RIGHT_CAPTURE;

}

else if (CurrentTrackingState == TRK_RIGHT_CAPTURE)

{

//OK, still in capture mode. Adjust heading as necessary

//right wall is closer, so CW moves twd wall, CCW moves away

//04/19/20 have to temporarily disable obstacle detection

loc_disterror = loc_rightdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away'

turndirection = loc_disterror > 0 ? TURNDIR_CW : TURNDIR_CCW;

turndeg = GetTrackTurnDeg(loc_disterror, m_TrkErrWinMult);

MakeTrackingTurn(turndirection, turndeg);

MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //added 04/21/20

////04/17/20 new experiment; use LIDAR dist to achieve capture

//if (rightdistval > DESIRED_WALL_OFFSET_DIST_CM)

//{

// //make a 90 deg right turn, then a 90 left just before desired offset

// RollingTurn(false, true, 45.f);

// StopBothMotors();

// delay(2000);

// MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

// int turn_threshold = 1.4 * DESIRED_WALL_OFFSET_DIST_CM ;

// mySerial.printf("turn threshold = %d\n", turn_threshold);

// while (frontdistval > turn_threshold)

// {

// frontdistval = GetFrontDistCm();

// mySerial.printf("Front distance = %d, threshold = %d\n", frontdistval, turn_threshold);

// delay(10);

// }

// //now turn back to parallel

// RollingTurn(true, true, 45.f);

// StopBothMotors();

// while (true)

// {

// }

//}

}

////DEBUG!!

// mySerial.printf("In WALL_TRACKING_RIGHT just before printout at %lu\n", millis());

////DEBUG!!

//PrintWallFollowTelemetry(frontvar); //added 04/15/19

#pragma endregion Tracking Right Wall Case

The significant changes in the code from where I started are:

Disabled ‘Far obstacle’ detection while in ‘capture’ mode to avoid obstacle avoidance step-turns in the middle of trying to capture the desired offset distance
When a ‘far obstacle’ IS detected, the StepTurn() function is now called immediately, rather than just starting the turn and letting it play out over multiple passes through the loop. This is now possible due to having accurate relative heading information, and is a huge improvement over the old timed-turn method.
Tried, and then abandoned the idea of using the front LIDAR measurement to directly acquire the desired offset distance by turning 45 deg from parallel, driving until the front distance was sqrt(2) (1.414) times the desired distance, and then turning back to parallel. This failed miserably because the robot’s turning radius is WAY too large to make this work.
ported the offset capture/maintenance code directly from the two wheel robot.

So, now the capture/maintenance code in the four whee robot looks like this

else if (CurrentTrackingState == TRK_RIGHT_CAPTURE)
{
	//OK, still in capture mode.  Adjust heading as necessary
	//right wall is closer, so CW moves twd wall, CCW moves away

	loc_disterror = loc_rightdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away'
	turndirection = loc_disterror > 0 ? TURNDIR_CW : TURNDIR_CCW;
	turndeg = GetTrackTurnDeg(loc_disterror, m_TrkErrWinMult);
	MakeTrackingTurn(turndirection, turndeg);
	MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //added 04/21/20
}

else if (CurrentTrackingState == TRK_RIGHT_CAPTURE)

{

//OK, still in capture mode. Adjust heading as necessary

//right wall is closer, so CW moves twd wall, CCW moves away

loc_disterror = loc_rightdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away'

turndirection = loc_disterror > 0 ? TURNDIR_CW : TURNDIR_CCW;

turndeg = GetTrackTurnDeg(loc_disterror, m_TrkErrWinMult);

MakeTrackingTurn(turndirection, turndeg);

MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //added 04/21/20

}

I ran a couple of trials in my ‘local’ (office) test range and they looked promising, so I tried a test in my ‘field’ (hallway outside my office) range, with very good results. Starting from about 75 cm out from the right wall and oriented slightly toward the wall, the robot tracked into the 30 cm desired offset and then maintained that offset to the end of the hallway. Interesting, it also attempted a ‘step-turn’ obstacle avoidance maneuver at the end, as shown in the following video:

Here is the relevant telemetry output from this run.

OpMode = Wall Following
Left/Right Avg Dist = 98/68
2585: In MODE_WALLFOLLOW  with Tracking Case = Right
3175: parallel heading found at 30.984 deg
OpMode = Wall Following
Left/Right Avg Dist = 104/72
5222: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg > m_PrevHdgCutDeg; Increasing Cut
In RollingTurn(CW, FWD,10.00)
5399: frontdistval = 354
OpMode = Wall Following
Left/Right Avg Dist = 91/71
5415: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg > m_PrevHdgCutDeg; Increasing Cut
In RollingTurn(CW, FWD,10.00)
5591: frontdistval = 354
OpMode = Wall Following
Left/Right Avg Dist = 91/70
5607: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 71/69, slope = -2	tgtdist = 30
IsConverging reports TRUE
5635: frontdistval = 354
OpMode = Wall Following
Left/Right Avg Dist = 93/69
5651: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 70/69, slope = -1	tgtdist = 30
IsConverging reports TRUE
5680: frontdistval = 353
OpMode = Wall Following
Left/Right Avg Dist = 94/68
5696: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 69/68, slope = -1	tgtdist = 30
IsConverging reports TRUE
5722: frontdistval = 353
OpMode = Wall Following
Left/Right Avg Dist = 96/68
5737: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 69/68, slope = -1	tgtdist = 30
IsConverging reports TRUE
5762: frontdistval = 349
OpMode = Wall Following
Left/Right Avg Dist = 97/68
5778: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 68/68, slope = 0	tgtdist = 30
IsConverging reports FALSE
not converging - increasing cut
In RollingTurn(CW, FWD,10.00)
6041: frontdistval = 221
OpMode = Wall Following
Left/Right Avg Dist = 131/67
6057: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 68/65, slope = -3	tgtdist = 30
IsConverging reports TRUE
Converging with turndeg 20 & PrevCut 30. Reducing Cut
In RollingTurn(CCW, FWD,10.00)
6479: frontdistval = 318
OpMode = Wall Following
Left/Right Avg Dist = 165/64
6495: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 68/59, slope = -9	tgtdist = 30
IsConverging reports TRUE
6518: frontdistval = 315
OpMode = Wall Following
Left/Right Avg Dist = 200/60
6535: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 65/58, slope = -7	tgtdist = 30
IsConverging reports TRUE
6557: frontdistval = 315
OpMode = Wall Following
Left/Right Avg Dist = 200/58
6574: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 59/58, slope = -1	tgtdist = 30
IsConverging reports TRUE
6596: frontdistval = 313
OpMode = Wall Following
Left/Right Avg Dist = 200/57
6611: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 58/57, slope = -1	tgtdist = 30
IsConverging reports TRUE
6635: frontdistval = 311
OpMode = Wall Following
Left/Right Avg Dist = 200/57
6693: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 57/57, slope = 0	tgtdist = 30
IsConverging reports FALSE
not converging - increasing cut
In RollingTurn(CW, FWD,10.00)
OpMode = Wall Following
Left/Right Avg Dist = 200/55
7017: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 57/53, slope = -4	tgtdist = 30
IsConverging reports TRUE
Converging with turndeg 20 & PrevCut 30. Reducing Cut
In RollingTurn(CCW, FWD,10.00)
OpMode = Wall Following
Left/Right Avg Dist = 200/52
7397: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 57/48, slope = -9	tgtdist = 30
IsConverging reports TRUE
7424: frontdistval = 276
OpMode = Wall Following
Left/Right Avg Dist = 200/49
7439: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 53/48, slope = -5	tgtdist = 30
IsConverging reports TRUE
OpMode = Wall Following
Left/Right Avg Dist = 200/47
7476: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 48/47, slope = -1	tgtdist = 30
IsConverging reports TRUE
7497: frontdistval = 274
OpMode = Wall Following
Left/Right Avg Dist = 200/47
7513: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 48/47, slope = -1	tgtdist = 30
IsConverging reports TRUE
7536: frontdistval = 273
OpMode = Wall Following
Left/Right Avg Dist = 200/47
7552: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 47/47, slope = 0	tgtdist = 30
IsConverging reports FALSE
not converging - increasing cut
In RollingTurn(CW, FWD,10.00)
7852: frontdistval = 160
OpMode = Wall Following
Left/Right Avg Dist = 200/46
7867: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 47/44, slope = -3	tgtdist = 30
IsConverging reports TRUE
Converging with turndeg 20 & PrevCut 30. Reducing Cut
In RollingTurn(CCW, FWD,10.00)
8290: frontdistval = 235
OpMode = Wall Following
Left/Right Avg Dist = 200/42
8304: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 47/37, slope = -10	tgtdist = 30
IsConverging reports TRUE
Converging with turndeg 10 & PrevCut 20. Reducing Cut
In RollingTurn(CCW, FWD,10.00)
8713: frontdistval = 222
OpMode = Wall Following
Left/Right Avg Dist = 200/38
8727: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 44/34, slope = -10	tgtdist = 30
IsConverging reports TRUE
Converging with turndeg 5 & PrevCut 10. Reducing Cut
In RollingTurn(CCW, FWD,10.00)
9132: frontdistval = 202
OpMode = Wall Following
Left/Right Avg Dist = 200/35
9146: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg > m_PrevHdgCutDeg; Increasing Cut
In RollingTurn(CW, FWD,10.00)
9405: frontdistval = 193
OpMode = Wall Following
Left/Right Avg Dist = 200/33
9419: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 34/33, slope = -1	tgtdist = 30
IsConverging reports TRUE
9445: frontdistval = 192
OpMode = Wall Following
Left/Right Avg Dist = 200/33
9459: In MODE_WALLFOLLOW  with Tracking Case = Right
IsConverging:  firstd/lastd = 34/34, slope = 0	tgtdist = 30
IsConverging reports FALSE
not converging - increasing cut
In RollingTurn(CW, FWD,10.00)
9764: frontdistval = 182
OpMode = Wall Following
Left/Right Avg Dist = 200/32
9779: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
In RollingTurn(CCW, FWD,20.00)
10454: frontdistval = 151
OpMode = Wall Following
Left/Right Avg Dist = 200/30
10468: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg > m_PrevHdgCutDeg; Increasing Cut
In RollingTurn(CCW, FWD,10.00)
10766: frontdistval = 138
OpMode = Wall Following
Left/Right Avg Dist = 200/29
10780: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
In RollingTurn(CW, FWD,10.00)
11036: frontdistval = 124
OpMode = Wall Following
Left/Right Avg Dist = 200/29
11051: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11065: frontdistval = 124
OpMode = Wall Following
Left/Right Avg Dist = 200/30
11079: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11095: frontdistval = 122
OpMode = Wall Following
Left/Right Avg Dist = 200/31
11109: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11124: frontdistval = 120
OpMode = Wall Following
Left/Right Avg Dist = 200/31
11139: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11154: frontdistval = 120
OpMode = Wall Following
Left/Right Avg Dist = 159/31
11168: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11183: frontdistval = 118
OpMode = Wall Following
Left/Right Avg Dist = 159/31
11197: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11212: frontdistval = 116
OpMode = Wall Following
Left/Right Avg Dist = 119/31
11227: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11241: frontdistval = 115
OpMode = Wall Following
Left/Right Avg Dist = 118/31
11256: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11271: frontdistval = 114
OpMode = Wall Following
Left/Right Avg Dist = 118/31
11285: In MODE_WALLFOLLOW  with Tracking Case = Right
turndeg = 0; Removing Entire Cut
11299: frontdistval = 111

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

OpMode = Wall Following

Left/Right Avg Dist = 98/68

2585: In MODE_WALLFOLLOW with Tracking Case = Right

3175: parallel heading found at 30.984 deg

OpMode = Wall Following

Left/Right Avg Dist = 104/72

5222: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg > m_PrevHdgCutDeg; Increasing Cut

In RollingTurn(CW, FWD,10.00)

5399: frontdistval = 354

OpMode = Wall Following

Left/Right Avg Dist = 91/71

5415: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg > m_PrevHdgCutDeg; Increasing Cut

In RollingTurn(CW, FWD,10.00)

5591: frontdistval = 354

OpMode = Wall Following

Left/Right Avg Dist = 91/70

5607: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 71/69, slope = -2 tgtdist = 30

IsConverging reports TRUE

5635: frontdistval = 354

OpMode = Wall Following

Left/Right Avg Dist = 93/69

5651: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 70/69, slope = -1 tgtdist = 30

IsConverging reports TRUE

5680: frontdistval = 353

OpMode = Wall Following

Left/Right Avg Dist = 94/68

5696: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 69/68, slope = -1 tgtdist = 30

IsConverging reports TRUE

5722: frontdistval = 353

OpMode = Wall Following

Left/Right Avg Dist = 96/68

5737: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 69/68, slope = -1 tgtdist = 30

IsConverging reports TRUE

5762: frontdistval = 349

OpMode = Wall Following

Left/Right Avg Dist = 97/68

5778: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 68/68, slope = 0 tgtdist = 30

IsConverging reports FALSE

not converging - increasing cut

In RollingTurn(CW, FWD,10.00)

6041: frontdistval = 221

OpMode = Wall Following

Left/Right Avg Dist = 131/67

6057: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 68/65, slope = -3 tgtdist = 30

IsConverging reports TRUE

Converging with turndeg 20 & PrevCut 30. Reducing Cut

In RollingTurn(CCW, FWD,10.00)

6479: frontdistval = 318

OpMode = Wall Following

Left/Right Avg Dist = 165/64

6495: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 68/59, slope = -9 tgtdist = 30

IsConverging reports TRUE

6518: frontdistval = 315

OpMode = Wall Following

Left/Right Avg Dist = 200/60

6535: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 65/58, slope = -7 tgtdist = 30

IsConverging reports TRUE

6557: frontdistval = 315

OpMode = Wall Following

Left/Right Avg Dist = 200/58

6574: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 59/58, slope = -1 tgtdist = 30

IsConverging reports TRUE

6596: frontdistval = 313

OpMode = Wall Following

Left/Right Avg Dist = 200/57

6611: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 58/57, slope = -1 tgtdist = 30

IsConverging reports TRUE

6635: frontdistval = 311

OpMode = Wall Following

Left/Right Avg Dist = 200/57

6693: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 57/57, slope = 0 tgtdist = 30

IsConverging reports FALSE

not converging - increasing cut

In RollingTurn(CW, FWD,10.00)

OpMode = Wall Following

Left/Right Avg Dist = 200/55

7017: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 57/53, slope = -4 tgtdist = 30

IsConverging reports TRUE

Converging with turndeg 20 & PrevCut 30. Reducing Cut

In RollingTurn(CCW, FWD,10.00)

OpMode = Wall Following

Left/Right Avg Dist = 200/52

7397: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 57/48, slope = -9 tgtdist = 30

IsConverging reports TRUE

7424: frontdistval = 276

OpMode = Wall Following

Left/Right Avg Dist = 200/49

7439: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 53/48, slope = -5 tgtdist = 30

IsConverging reports TRUE

OpMode = Wall Following

Left/Right Avg Dist = 200/47

7476: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 48/47, slope = -1 tgtdist = 30

IsConverging reports TRUE

7497: frontdistval = 274

OpMode = Wall Following

Left/Right Avg Dist = 200/47

7513: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 48/47, slope = -1 tgtdist = 30

IsConverging reports TRUE

7536: frontdistval = 273

OpMode = Wall Following

Left/Right Avg Dist = 200/47

7552: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 47/47, slope = 0 tgtdist = 30

IsConverging reports FALSE

not converging - increasing cut

In RollingTurn(CW, FWD,10.00)

7852: frontdistval = 160

OpMode = Wall Following

Left/Right Avg Dist = 200/46

7867: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 47/44, slope = -3 tgtdist = 30

IsConverging reports TRUE

Converging with turndeg 20 & PrevCut 30. Reducing Cut

In RollingTurn(CCW, FWD,10.00)

8290: frontdistval = 235

OpMode = Wall Following

Left/Right Avg Dist = 200/42

8304: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 47/37, slope = -10 tgtdist = 30

IsConverging reports TRUE

Converging with turndeg 10 & PrevCut 20. Reducing Cut

In RollingTurn(CCW, FWD,10.00)

8713: frontdistval = 222

OpMode = Wall Following

Left/Right Avg Dist = 200/38

8727: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 44/34, slope = -10 tgtdist = 30

IsConverging reports TRUE

Converging with turndeg 5 & PrevCut 10. Reducing Cut

In RollingTurn(CCW, FWD,10.00)

9132: frontdistval = 202

OpMode = Wall Following

Left/Right Avg Dist = 200/35

9146: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg > m_PrevHdgCutDeg; Increasing Cut

In RollingTurn(CW, FWD,10.00)

9405: frontdistval = 193

OpMode = Wall Following

Left/Right Avg Dist = 200/33

9419: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 34/33, slope = -1 tgtdist = 30

IsConverging reports TRUE

9445: frontdistval = 192

OpMode = Wall Following

Left/Right Avg Dist = 200/33

9459: In MODE_WALLFOLLOW with Tracking Case = Right

IsConverging: firstd/lastd = 34/34, slope = 0 tgtdist = 30

IsConverging reports FALSE

not converging - increasing cut

In RollingTurn(CW, FWD,10.00)

9764: frontdistval = 182

OpMode = Wall Following

Left/Right Avg Dist = 200/32

9779: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

In RollingTurn(CCW, FWD,20.00)

10454: frontdistval = 151

OpMode = Wall Following

Left/Right Avg Dist = 200/30

10468: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg > m_PrevHdgCutDeg; Increasing Cut

In RollingTurn(CCW, FWD,10.00)

10766: frontdistval = 138

OpMode = Wall Following

Left/Right Avg Dist = 200/29

10780: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

In RollingTurn(CW, FWD,10.00)

11036: frontdistval = 124

OpMode = Wall Following

Left/Right Avg Dist = 200/29

11051: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11065: frontdistval = 124

OpMode = Wall Following

Left/Right Avg Dist = 200/30

11079: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11095: frontdistval = 122

OpMode = Wall Following

Left/Right Avg Dist = 200/31

11109: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11124: frontdistval = 120

OpMode = Wall Following

Left/Right Avg Dist = 200/31

11139: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11154: frontdistval = 120

OpMode = Wall Following

Left/Right Avg Dist = 159/31

11168: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11183: frontdistval = 118

OpMode = Wall Following

Left/Right Avg Dist = 159/31

11197: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11212: frontdistval = 116

OpMode = Wall Following

Left/Right Avg Dist = 119/31

11227: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11241: frontdistval = 115

OpMode = Wall Following

Left/Right Avg Dist = 118/31

11256: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11271: frontdistval = 114

OpMode = Wall Following

Left/Right Avg Dist = 118/31

11285: In MODE_WALLFOLLOW with Tracking Case = Right

turndeg = 0; Removing Entire Cut

11299: frontdistval = 111

From the above telemetry and video it is clear the robot was successful in capturing and maintaining the desired wall offset. Salient points are (leading numbers denote mSec from start):

parallel heading found at 30.984 deg – very close, and very efficient
Left/Right Avg Dist = 104/72. So the robot started out with a 40 cm error
5522: In RollingTurn(CW, FWD,10.00). The capture process started with two quick 10-deg CW turns toward the right wall.
5778: After about 6 times through the loop, the ‘cut’ is increased another 10 deg toward the right wall.
6057: Cut reduced by 10 deg with distance to wall = 64 cm
6693: Cut increased by 10 deg with distance = 57 cm
7017: Cut reduced by 10 deg with distance to wall = 55 cm
7536: Cut increased by 10 deg with distance = 47 cm
7867: Cut reduced by 10 deg with distance to wall = 46 cm
8304: Cut reduced by 10 deg with distance to wall = 42 cm
8727: Cut reduced by 10 deg with distance to wall = 34 cm

So, it only took about 3 seconds after the initial ‘find parallel’ maneuver to close from 72 to 34 cm and to remove the entire initial ‘cut’ of 20 degrees. Pretty successful, I would say!

30 April 2020 Update:

After the above successful test, I tried a wall-tracking run with the robot initially oriented slightly away from the wall, and this was a dismal failure. Looking through the code, I became convinced that the algorithm as currently implemented will never work for the ‘away from wall case’, so now I’m busily rewriting the whole thing so it will work for both cases.

After a couple false starts, I think I now have a working ‘turn to parallel’ algorithm that works for both the ‘toward wall’ and ‘away from wall’ starting orientations. Here’s a couple of video clips showing the operation. For purposes of this demonstration, I added short (1 sec) pauses between each step in the ‘find parallel’ operation so they can be easily identified in the video. In actual operation each step should flow smoothly into the next one.

Here’s the code for the newly re-written ‘RotateToParallelOrientation()’:

bool RotateToParallelOrientation(int trkcase, int tgtdist)
{
	//Purpose:  Rotate robot to parallel near wall
	//Inputs: 
	//	TrackingCase = int representing current WallTrackingCases enum value
	//	tgtdist = int value representing the desired wall offset
	//Outputs:
	//	Robot orientation changed to parallel nearest wall
	//Plan:
	//Step1: Start turn away from wall.  If the distance decreases, continue the turn until it 
	//	starts going up again, and record the associated heading value.
	//Step2: Reverse the turn and wait for the distance to 
	//	go down and then back up. If the heading value associated with this inflection point and 
	//	the previous inflection point are close, return TRUE
	//Step3: If the values from Step1 & Step2 aren't close enough, reverse the turn again and 
	//	find a new inflection point.  If the resulting heading is close enough to the Step1 or
	//	Step2 values, return TRUE.  Otherwise, return FALSE
	//Notes:
	//	04/11/20 Copied from two wheel robot's FindParallelHdg() & revised
	//	04/29/20 rewritten to handle away-from-wall starting orientation

	bool bTrackingRight = true;
	bool bFirstTurnDirection = TURNDIR_CCW;
	float first_par_hdg_deg = 999; //impossible value
	float second_par_hdg_deg = 999; //impossible value
	int success_count = 0;
	bool bResult = true;


//DEBUG!!
	mySerial.printf("In RotateToParallelOrientation(Tracking %s,%d)\n", TrkStrArray[TrackingCase], tgtdist);
	mySerial.printf("mSec\tmeas#\tHdg\tPrevD\tCurD\n");
//DEBUG!!

	if ((WallTrackingCases)trkcase == TRACKING_RIGHT)
	{
		//newdist = GetRightDistCm();
		bTrackingRight = true;
		bFirstTurnDirection = TURNDIR_CCW;
	}
	else if ((WallTrackingCases)trkcase == TRACKING_LEFT)
	{
		//newdist = GetLeftDistCm();
		bTrackingRight = false;
		bFirstTurnDirection = TURNDIR_CW;
	}
	else
	{
		mySerial.printf("Error determining tracking side in RotateToParallelOrientation(Tracking %s,%d)\n",
			TrkStrArray[TrackingCase]);
	}

//Step1: Start turn away from wall.  If the distance decreases, continue the turn until it 
//	starts going up again, and record the associated heading value.

	bool bInflectionPointFound = FindInflectionPoint(bFirstTurnDirection, bTrackingRight);
	if (bInflectionPointFound)
	{
		//success - save heading for comparison with verification run
		first_par_hdg_deg = IMUHdgValDeg ; //save the latest heading from IMU
		success_count++;
		mySerial.printf("Step1: Infection point found at %3.2f deg\n", first_par_hdg_deg);
	}
	else
	{
		mySerial.printf("FindInflectionPoint(%s) failed!", bFirstTurnDirection ? "CCW" : "CW");
		first_par_hdg_deg = 999; //impossible value
	}

//DEBUG!!
	StopBothMotors();
	delay(1000);
//DEBUG!!

//Step2: Reverse the turn and wait for the distance to 
//	go down and then back up. If the heading value associated with this inflection point and 
//	the previous inflection point are close, return TRUE

	bInflectionPointFound = FindInflectionPoint(!bFirstTurnDirection, bTrackingRight);

//DEBUG!!
	StopBothMotors();
	delay(1000);
//DEBUG!!

	if (bInflectionPointFound)
	{
		//success - compare heading with one from Step1:
		success_count++;
		mySerial.printf("Step2: Inflection point found at %3.2f deg\n", IMUHdgValDeg);

		if (success_count >= 2)
		{
			//compare current inflection hdg with prev one; if close enough, we're done
			//otherwise, try again
			if (abs(first_par_hdg_deg - IMUHdgValDeg) < 10.f)
			{
				mySerial.printf("Step2: 2nd inflection point at %3.2f deg matches to within 10 deg of 1st point %3.2f\n",
					IMUHdgValDeg, first_par_hdg_deg);
				bResult = true; //sucess!
			}
			else
			{
				//Step3: If the values from Step1 & Step2 aren't close enough, reverse the turn again and 
				//	find a new inflection point.  If the resulting heading is close enough to the Step1 or
				//	Step2 values, return TRUE.  Otherwise, return FALSE
				second_par_hdg_deg = IMUHdgValDeg;
				bInflectionPointFound = FindInflectionPoint(bFirstTurnDirection, bTrackingRight);

				//DEBUG!!
				StopBothMotors();
				delay(1000);
				//DEBUG!!

				if (bInflectionPointFound)
				{
					success_count++;
					if (success_count >= 2)
					{
						//compare current inflection hdg with prev one; if close enough, we're done
						//otherwise, try again
						if (abs(first_par_hdg_deg - IMUHdgValDeg) < 10.f || abs(second_par_hdg_deg - IMUHdgValDeg) < 10.f)
						{
							mySerial.printf("Step4: 2nd inflection point at %3.2f deg matches with 1st point %3.2f or second point %3.2f\n",
								IMUHdgValDeg, first_par_hdg_deg, second_par_hdg_deg);
							bResult = true; //sucess!
						}
					}
					else
					{
						mySerial.printf("Step4: Failed to find second matching inflection point - punting\n");
						bResult = false;
					}
				}
				else
				{
					mySerial.printf("Step4: Failed to find second inflection point - punting\n");
					bResult = false;
				}
			}
		}

	}
	else
	{
		mySerial.printf("Step1: FindInflectionPoint(%s) failed!", bFirstTurnDirection ? "CCW" : "CW");
		bResult = false;
	}

	return bResult;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

bool RotateToParallelOrientation(int trkcase, int tgtdist)

{

//Purpose: Rotate robot to parallel near wall

//Inputs:

// TrackingCase = int representing current WallTrackingCases enum value

// tgtdist = int value representing the desired wall offset

//Outputs:

// Robot orientation changed to parallel nearest wall

//Plan:

//Step1: Start turn away from wall. If the distance decreases, continue the turn until it

// starts going up again, and record the associated heading value.

//Step2: Reverse the turn and wait for the distance to

// go down and then back up. If the heading value associated with this inflection point and

// the previous inflection point are close, return TRUE

//Step3: If the values from Step1 & Step2 aren't close enough, reverse the turn again and

// find a new inflection point. If the resulting heading is close enough to the Step1 or

// Step2 values, return TRUE. Otherwise, return FALSE

//Notes:

// 04/11/20 Copied from two wheel robot's FindParallelHdg() & revised

// 04/29/20 rewritten to handle away-from-wall starting orientation

bool bTrackingRight = true;

bool bFirstTurnDirection = TURNDIR_CCW;

float first_par_hdg_deg = 999; //impossible value

float second_par_hdg_deg = 999; //impossible value

int success_count = 0;

bool bResult = true;

//DEBUG!!

mySerial.printf("In RotateToParallelOrientation(Tracking %s,%d)\n", TrkStrArray[TrackingCase], tgtdist);

mySerial.printf("mSec\tmeas#\tHdg\tPrevD\tCurD\n");

//DEBUG!!

if ((WallTrackingCases)trkcase == TRACKING_RIGHT)

{

//newdist = GetRightDistCm();

bTrackingRight = true;

bFirstTurnDirection = TURNDIR_CCW;

}

else if ((WallTrackingCases)trkcase == TRACKING_LEFT)

{

//newdist = GetLeftDistCm();

bTrackingRight = false;

bFirstTurnDirection = TURNDIR_CW;

}

else

{

mySerial.printf("Error determining tracking side in RotateToParallelOrientation(Tracking %s,%d)\n",

TrkStrArray[TrackingCase]);

}

//Step1: Start turn away from wall. If the distance decreases, continue the turn until it

// starts going up again, and record the associated heading value.

bool bInflectionPointFound = FindInflectionPoint(bFirstTurnDirection, bTrackingRight);

if (bInflectionPointFound)

{

//success - save heading for comparison with verification run

first_par_hdg_deg = IMUHdgValDeg ; //save the latest heading from IMU

success_count++;

mySerial.printf("Step1: Infection point found at %3.2f deg\n", first_par_hdg_deg);

}

else

{

mySerial.printf("FindInflectionPoint(%s) failed!", bFirstTurnDirection ? "CCW" : "CW");

first_par_hdg_deg = 999; //impossible value

}

//DEBUG!!

StopBothMotors();

delay(1000);

//DEBUG!!

//Step2: Reverse the turn and wait for the distance to

// go down and then back up. If the heading value associated with this inflection point and

// the previous inflection point are close, return TRUE

bInflectionPointFound = FindInflectionPoint(!bFirstTurnDirection, bTrackingRight);

//DEBUG!!

StopBothMotors();

delay(1000);

//DEBUG!!

if (bInflectionPointFound)

{

//success - compare heading with one from Step1:

success_count++;

mySerial.printf("Step2: Inflection point found at %3.2f deg\n", IMUHdgValDeg);

if (success_count >= 2)

{

//compare current inflection hdg with prev one; if close enough, we're done

//otherwise, try again

if (abs(first_par_hdg_deg - IMUHdgValDeg) < 10.f)

{

mySerial.printf("Step2: 2nd inflection point at %3.2f deg matches to within 10 deg of 1st point %3.2f\n",

IMUHdgValDeg, first_par_hdg_deg);

bResult = true; //sucess!

}

else

{

//Step3: If the values from Step1 & Step2 aren't close enough, reverse the turn again and

// find a new inflection point. If the resulting heading is close enough to the Step1 or

// Step2 values, return TRUE. Otherwise, return FALSE

second_par_hdg_deg = IMUHdgValDeg;

bInflectionPointFound = FindInflectionPoint(bFirstTurnDirection, bTrackingRight);

//DEBUG!!

StopBothMotors();

delay(1000);

//DEBUG!!

if (bInflectionPointFound)

{

success_count++;

if (success_count >= 2)

{

//compare current inflection hdg with prev one; if close enough, we're done

//otherwise, try again

if (abs(first_par_hdg_deg - IMUHdgValDeg) < 10.f || abs(second_par_hdg_deg - IMUHdgValDeg) < 10.f)

{

mySerial.printf("Step4: 2nd inflection point at %3.2f deg matches with 1st point %3.2f or second point %3.2f\n",

IMUHdgValDeg, first_par_hdg_deg, second_par_hdg_deg);

bResult = true; //sucess!

}

else

{

mySerial.printf("Step4: Failed to find second matching inflection point - punting\n");

bResult = false;

}

else

{

mySerial.printf("Step4: Failed to find second inflection point - punting\n");

bResult = false;

}

else

{

mySerial.printf("Step1: FindInflectionPoint(%s) failed!", bFirstTurnDirection ? "CCW" : "CW");

bResult = false;

}

return bResult;

}

In the above code, the actual inflection point detection routine was abstracted into its own function ‘FindInflectionPoint()’ as shown below:

bool FindInflectionPoint(bool turndir, bool bTrackingRight)
{
	int measurenum = 0;
	int hitnum = 0; //number of times inflection has been detected
	float InflectionHdgArray[3];
	int prevdist, prevmindist = 0;
	int offsidespeed, drivesidespeed = 0;
	bool bFirstTurnDirection = TURNDIR_CCW;

	bool bResult = true;

	int newdist = GetNearWallDistCm(bTrackingRight);
	prevmindist = newdist;


	if (turndir == TURNDIR_CCW)
	{
		//Start CCW turn (away from right wall)
		mySerial.printf("starting CCW turn (away from right wall)\n");
		MoveAhead(OFFSIDE_MOTOR_SPEED, DRIVESIDE_MOTOR_SPEED_HIGH);
	}
	else
	{
		//Start CW turn (away from left wall)
		mySerial.printf("starting CW turn (toward from left wall)\n");
		MoveAhead(DRIVESIDE_MOTOR_SPEED_HIGH, OFFSIDE_MOTOR_SPEED);
	}


	while (hitnum < MIN_COUNTS_PAST_INFLECTION_POINT && measurenum < MAX_INFLECTION_POINT_TRIALS)//waiting for well-defined inflection
	{
		measurenum++;
		UpdateIMUHdgValDeg();

		if (newdist < prevmindist)
		{
			prevmindist = newdist;
		}
		newdist = GetNearWallDistCm(bTrackingRight);

		if (newdist >= prevmindist)
		{
			InflectionHdgArray[hitnum] = IMUHdgValDeg;
			hitnum++;
		}
		else
		{
			//02/26/20 - rev to just decrement hit count rather than completely resetting 
			hitnum = (hitnum > INFLECTION_DETECTION_COUNT_DECREMENT) ?
				hitnum - INFLECTION_DETECTION_COUNT_DECREMENT : 0; //adj hit counter
		}

		//DEBUG!!
		mySerial.printf("%lu\t%d\t%3.2f\t%d\t%d\t%d\n", millis(),
			measurenum, IMUHdgValDeg, prevmindist, newdist, hitnum);
		//DEBUG!!
	}

	//04/29/20 at this point, we either have detected an inflection point, or something is wrong
	if (measurenum >= MAX_INFLECTION_POINT_TRIALS)
	{
		//oops - something went wrong 
		mySerial.printf("Step2: too many trials without inflection point! measurenum = %d, hitnum = %d\n", measurenum, hitnum);
		bResult = false;
	}

	return bResult;
}

bool FindInflectionPoint(bool turndir, bool bTrackingRight)

{

int measurenum = 0;

int hitnum = 0; //number of times inflection has been detected

float InflectionHdgArray[3];

int prevdist, prevmindist = 0;

int offsidespeed, drivesidespeed = 0;

bool bFirstTurnDirection = TURNDIR_CCW;

bool bResult = true;

int newdist = GetNearWallDistCm(bTrackingRight);

prevmindist = newdist;

if (turndir == TURNDIR_CCW)

{

//Start CCW turn (away from right wall)

mySerial.printf("starting CCW turn (away from right wall)\n");

MoveAhead(OFFSIDE_MOTOR_SPEED, DRIVESIDE_MOTOR_SPEED_HIGH);

}

else

{

//Start CW turn (away from left wall)

mySerial.printf("starting CW turn (toward from left wall)\n");

MoveAhead(DRIVESIDE_MOTOR_SPEED_HIGH, OFFSIDE_MOTOR_SPEED);

}

while (hitnum < MIN_COUNTS_PAST_INFLECTION_POINT && measurenum < MAX_INFLECTION_POINT_TRIALS)//waiting for well-defined inflection

{

measurenum++;

UpdateIMUHdgValDeg();

if (newdist < prevmindist)

{

prevmindist = newdist;

}

newdist = GetNearWallDistCm(bTrackingRight);

if (newdist >= prevmindist)

{

InflectionHdgArray[hitnum] = IMUHdgValDeg;

hitnum++;

}

else

{

//02/26/20 - rev to just decrement hit count rather than completely resetting

hitnum = (hitnum > INFLECTION_DETECTION_COUNT_DECREMENT) ?

hitnum - INFLECTION_DETECTION_COUNT_DECREMENT : 0; //adj hit counter

}

//DEBUG!!

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

measurenum, IMUHdgValDeg, prevmindist, newdist, hitnum);

//DEBUG!!

}

//04/29/20 at this point, we either have detected an inflection point, or something is wrong

if (measurenum >= MAX_INFLECTION_POINT_TRIALS)

{

//oops - something went wrong

mySerial.printf("Step2: too many trials without inflection point! measurenum = %d, hitnum = %d\n", measurenum, hitnum);

bResult = false;

}

return bResult;

}

At this point I think I have the wall-tracking case completely solved for the right-hand wall tracking case. Now I have to port the algorithm to the left-hand wall and ‘neither wall’ tracking cases and make sure it all works. But for now I’m pretty happy.

1 May 2020 Update: Obstacle Avoidance

Happy May Day! In Ohio USA we have now been in ‘lockdown for two entire months – All of March and all of April. According to our governor, we may be able to go out and play at least a little bit in the coming month – yay!

At the end of both the last two videos of successful ‘RotateToParallelOrientation()’ runs, the robot gets close to the end of the hallway and attempts a left/right step-turn maneuver, ending with its nose against the far wall. The maneuver occurs way too close to the obstacle to do any good as a ‘in-line obstacle’ avoidance maneuver; at this distance, a 90 degree turn to follow the new wall would be a better deal.

When I looked into the telemetry log to determine what happened, I discovered the late step-turn was a consequence of an earlier decision to significantly shorten the ‘far obstacle’ detection distance, in order to avoid step-turns in the middle of the robot’s attempt to capture and then maintain the desired wall offset distance. Unfortunately the detection distance wasn’t re-instated to its former value once the robot had captured the desired wall offset, so it didn’t detect the upcoming wall as an obstacle until too late.

And this line of thought brings up another issue; what defines the transition from the ‘approach’ state to the ‘distance hold’ state, anyway? In previous work I had sketched out a proposed state diagram for the wall-following mode, as shown below:

Possible state diagram for the TRK_RIGHT case

And I now realize it is both incomplete and poorly labelled. I now believe the ‘Capture’ state should be renamed to ‘Approach’, and the ‘Maintain’ state should be renamed ‘Offset Hold’. Moreover, there should be a third ’90-deg left turn’ state that is entered if the step-turn maneuver fails to bypass the ‘far obstacle’. Maybe something like:

02 May 2020 Update: Back to Charging Station Code

After a week of working with wall-following code, I managed to discharge Wall-E2’s battery to the point where it’s looking to be fed again, so I’m back on the charging station support code. When we last left this portion of the saga, I had discovered the problem with Wall-E2 not shutting off its motors after connecting to the charging station, and was in the process of making the changes when Wall-E2 turned up its nose at the charging station and I had to go do other stuff for a while.

Now, back on the Charging Station case, I found that I had to make some significant changes to GetOpMode; I had to move the DEAD_BATTERY condition check to after the IR Homing and Charger connected mode checks; otherwise, the robot can’t home to a charging station with a low battery because the DEAD_BATTERY mode will be detected first – oops! The new version of GetOpMode() is shown below:

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;

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

	return mode;
}

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;

//Step4: Else return MODE_WALLFOLLOW (3)

else mode = MODE_WALLFOLLOW;

return mode;

}

After these (and some other minor improvements) I was able to get Wall-E2 to successfully home to a charging station, stop its motors (yay!) and commence charging. As the following videos show, I was able to do this on my desktop ‘local’ range, and also ‘in the field’ (aka my atrium hallway).

After the robot connects to the charging station, the rear LED strip changes function to become a ‘fuel guage’, with ’empty’ on the right and ‘full’ on the left. In the videos above, note that about 6 seconds after the robot connects, the LED strip changes to show ‘almost empty’.

Stay tuned,

Frank