Turn Rate PID Tuning, Part III

1 Reply

In my previous post on this subject, I described my efforts to control the turn rate (in deg/sec) of my two-wheel robot, in preparation for doing the same thing on Wall-E2, four wheel drive autonomous wall following robot.

As noted previously, I have a TIMER5 Interrupt Service Routine (ISR) set up on my four wheel robot to provide updates to the various sensor values every 100 mSec, but was unable to figure out a robust way of synchronizing the PID library’s Compute() timing with the ISR timing. So, I decided to bag the PID library entirely, at least for turn rate control, and insert the PID algorithm directly into the turn rate control, and removing the extraneous stuff that caused divide-by-zero errors when the setSampleTime() function was modified to accept a zero value.

To facilitate more rapid test cycles, I created a new program that contained just enough code to initialize and read the MP6050 IMU module, and a routine called ‘SpinTurnForever() that accepts PID parameters and causes the robot to ‘spin’ turn forever (or at least until I stop it with a keyboard command. Here’s the entire program.

/*
    Name:       FourWD_PulseTurnRateTest.ino
    Created:	5/24/2021 4:49:52 PM
    Author:     FRANKNEWXPS15\Frank
*/

#pragma region INCLUDES
#include <elapsedMillis.h>
#include <PrintEx.h> //allows printf-style printout syntax
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/05/19
#include "I2Cdev.h" //02/19/19: this includes SBWire.h
#include "I2C_Anything.h" //needed for sending float data over I2C
#pragma endregion INCLUDES

#pragma region DEFINES
#define MPU6050_I2C_ADDR 0x69
#pragma endregion DEFINES

#pragma region PRE_SETUP
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
MPU6050 mpu(MPU6050_I2C_ADDR); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0)
#pragma endregion PRE_SETUP

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

const int In1_Right = 8;
const int In2_Right = 9;
#pragma endregion Motor Pin Assignments

#pragma region MOTOR_PARAMETERS
//drive wheel speed parameters
const int MOTOR_SPEED_FULL = 200; //range is 0-255
const int MOTOR_SPEED_MAX = 255; //range is 0-255
const int MOTOR_SPEED_HALF = 127; //range is 0-255
const int MOTOR_SPEED_QTR = 75; //added 09/25/20
const int MOTOR_SPEED_LOW = 50; //added 01/22/15
const int MOTOR_SPEED_OFF = 0; //range is 0-255
//drive wheel direction constants
const boolean FWD_DIR = true;
const boolean REV_DIR = !FWD_DIR;

//Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;
bool bIsForwardDir = true; //default is foward direction
#pragma endregion Motor Parameters

#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

            // orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

//MPU6050 status flags
bool bMPU6050Ready = true;
bool dmpReady = false;  // set true if DMP init was successful
volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR
const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param
bool bFirstTime = true;
#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19
#pragma endregion MPU6050 Support

const int CHG_CONNECT_LED_PIN = 50; //12/16/20 added for debug

elapsedMillis MsecSinceLastTurnRateUpdate;

//const int MAX_PULSE_WIDTH_MSEC = 100;
const int MAX_PULSE_WIDTH_MSEC = 50;
const int MIN_PULSE_WIDTH_MSEC = 0;

//const int MAX_SPIN_MOTOR_SPEED = 200;
const int MAX_SPIN_MOTOR_SPEED = 250;
const int MIN_SPIN_MOTOR_SPEED = 50;

const int TURN_RATE_UPDATE_INTERVAL_MSEC = 20;
//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 100;

double TurnRateOutput;
double TurnRateSetPoint_DPS;
double Ival = 0; double lastErr = 0;
double lastTurnRateVal_DPS;
double TurnRateVal_DPS;


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

#pragma region MPU6050
	mySerial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);
	mpu.initialize();

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

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

	// make sure it worked (returns 0 if successful)
	if (devStatus == 0)
	{
		// turn on the DMP, now that it's ready
		Serial.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

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

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

		bMPU6050Ready = false;
	}
#pragma endregion MPU6050

	mySerial.printf("End of test - Stopping!\n");
	while (true)
	{
		CheckForUserInput();
	}
}

void loop()
{


}

void CheckForUserInput()
{
	const int bufflen = 80;
	char buff[bufflen];
	memset(buff, 0, bufflen);
	float degPerSec;
	float Kp, Ki, Kd;
	const char s[2] = ",";
	char* token;

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

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

		//02/18/20 experiment with multiple commands
		switch (incomingByte)
		{
		case 0x50: //ASCII 'P'
		case 0x70: //ASCII 'p'
			StopBothMotors();
			Serial.println(F("Setting Kp value"));

			//consume CR/LF chars
			while (Serial.available() > 0)
			{
				int byte = Serial.read();
				mySerial.printf("consumed %d\n", byte);
			}

			while (!Serial.available())
			{
			}

			Serial.readBytes(buff, bufflen);
			mySerial.printf("%s\n", buff);

			/* extract Kp */
			token = strtok(buff, s);
			Kp = atof(token);

			/* extract Ki */
			token = strtok(NULL, s);
			Ki = atof(token);

			/* extract Kd */
			token = strtok(NULL, s);
			Kd = atof(token);

			/* extract degPerSec */
			token = strtok(NULL, s);
			degPerSec = atof(token);

			mySerial.printf("Kp,Ki,Kd,degPerSec =  %2.2f, %2.2f, %2.2f, %2.2f\n",
				Kp, Ki, Kd, degPerSec);

			PulseSpinTurnForever(true, Kp, Ki, Kd, degPerSec);

			break;
		case 0x51: //ASCII 'Q'
		case 0x71: //ASCII 'q'
			StopBothMotors();
			Serial.println(F("Setting Kp value"));

			//consume CR/LF chars
			while (Serial.available() > 0)
			{
				int byte = Serial.read();
				mySerial.printf("consumed %d\n", byte);
			}

			while (!Serial.available())
			{
			}

			Serial.readBytes(buff, bufflen);
			mySerial.printf("%s\n", buff);

			/* extract Kp */
			token = strtok(buff, s);
			Kp = atof(token);

			/* extract Ki */
			token = strtok(NULL, s);
			Ki = atof(token);

			/* extract Kd */
			token = strtok(NULL, s);
			Kd = atof(token);

			/* extract degPerSec */
			token = strtok(NULL, s);
			degPerSec = atof(token);

			mySerial.printf("Kp,Ki,Kd,degPerSec =  %2.2f, %2.2f, %2.2f, %2.2f\n",
				Kp, Ki, Kd, degPerSec);

			SpinTurnForever(true, Kp, Ki, Kd, degPerSec);

			break;
		Default:
						Serial.println(F("In Default Case: Stopping Motors!"));
						MoveAhead(0, 0);
						while (true)
						{

						}
					}
				}
			}

void PulseSpinTurnForever(bool b_ccw, float Kp, float Ki, float Kd, float degPersec)
{
	float tgt_deg;
	double prev_hdg = 0;

	//DEBUG!!
	mySerial.printf("PulseTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",
		Kp, Ki, Kd, degPersec);
	//DEBUG!!


	mySerial.printf("Msec\tHdg\tDhdg\tRate\tSet\terror\tDerr\tIval\tKp*err\tKi*Ival\tKd*Derr\tOut\n");

	//init some stuff
	TurnRateOutput = 0;
	TurnRateSetPoint_DPS = degPersec;
	Ival = 0; lastErr = 0;
	lastTurnRateVal_DPS;
	TurnRateVal_DPS;

	UpdateIMUHdgValDeg();
	prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR
	MsecSinceLastTurnRateUpdate = 0;

	//05/03/21 - back to the PWM technique
	//Step4: Pulse the motors to get things started
	digitalWrite(CHG_CONNECT_LED_PIN, HIGH);
	SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);
	SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);
	delay(50);
	StopBothMotors();
	digitalWrite(CHG_CONNECT_LED_PIN, LOW);

	while (true)
	{
		//11/06/20 now just loops between ISR hits
		CheckForUserInput();

		//if (bTimeForNavUpdate) //set true in ISR
		//{
		//	bTimeForNavUpdate = false;
		if (MsecSinceLastTurnRateUpdate >= 100)
		{
			MsecSinceLastTurnRateUpdate -= 100;

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;

			//11/14/20 need to handle -179 to +179 transition
			deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

			//05/20/21 gfp can occasionally get one or two bad readings
			while (abs(deltaDeg) > TurnRateSetPoint_DPS)
			{
				mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",
					IMUHdgValDeg, prev_hdg, deltaDeg);

				prev_hdg = IMUHdgValDeg; //05/23/21 bugfix
				CheckForUserInput();

				deltaDeg = IMUHdgValDeg - prev_hdg;

				deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
				deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

				prev_hdg = IMUHdgValDeg;
				UpdateIMUHdgValDeg();
			}

			TurnRateVal_DPS = 10 * abs(deltaDeg); //05/23/21 4wd uses fixed 100mSec TIMER5 interval

			double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;
			double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);
			Ival += (error);
			double dErr = (error - lastErr);

			/*Compute PID Output*/
			TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

			/*Remember some variables for next time*/
			lastErr = error;
			lastTurnRateVal_DPS = TurnRateVal_DPS;

			if (TurnRateOutput > MAX_PULSE_WIDTH_MSEC)
				TurnRateOutput = MAX_PULSE_WIDTH_MSEC;
			else if (TurnRateOutput < MIN_PULSE_WIDTH_MSEC)
				TurnRateOutput = MIN_PULSE_WIDTH_MSEC;

			//05/03/21 - back to the PWM technique
			//Step4: Pulse the motors to full speed for the duration specified by the PID output
			digitalWrite(CHG_CONNECT_LED_PIN, HIGH);
			SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_MAX);
			SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_MAX);
			//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);
			//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);
			//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);
			//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);
			delay(TurnRateOutput);
			StopBothMotors();
			digitalWrite(CHG_CONNECT_LED_PIN, LOW);


			//DEBUG!!
			mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",
				millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,
				Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);
			//DEBUG

			prev_hdg = IMUHdgValDeg;
		}
	}
}

#pragma region MOTOR SUPPORT
//09/08/20 modified for DRV8871 motor driver
void MoveReverse(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move in reverse direction continuously - companion to MoveAhead()
	//ProvEnA_Pinnce: G. Frank Paynter 09/08/18
	//Inputs:  
	//    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
	//    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
	//Outputs: both drive Motors energized with the specified speed
	//Plan:
	//    Step 1: Set reverse direction for both wheels
	//    Step 2: Run both Motors at specified speeds
	//Notes:
	//	01/22/20 now using Adafruit DRV8871 drivers

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

//09/08/20 modified for DRV8871 motor driver
void MoveAhead(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move ahead continuously
	//ProvEnA_Pinnce: G. Frank Paynter and Danny Frank 01/24/2014
	//Inputs:  
	//    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
	//    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
	//Outputs: both drive Motors energized with the specified speed
	//Plan:
	//    Step 1: Set forward direction for both wheels
	//    Step 2: Run both Motors at specified speeds
	//Notes:
	//	01/22/20 now using Adafruit DRV8871 drivers

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

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

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

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

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

//DEBUG!!
	//mySerial.printf("In RunBothMotors(%s, %d,%d)\n", bisFwd? "FWD":"REV", leftspeednum, rightspeednum);
//DEBUG!!

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

void RunBothMotorsBidirectional(int leftspeed, int rightspeed)
{
	//Purpose:  Accommodate the need for independent bidirectional wheel motion
	//Inputs:
	//	leftspeed - integer denoting left wheel speed.  Positive value is fwd, negative is rev
	//	rightspeed - integer denoting right wheel speed.  Positive value is fwd, negative is rev
	//Outputs:
	//	left/right wheel motors direction and speed set as appropriate
	//Plan:
	//	Step1: Set left wheel direction and speed
	//	Step2: Set right wheel direction and speed

//Step1: Set left wheel direction and speed

	//DEBUG!!
	//mySerial.printf("In RunBothMotorsBidirectional(%d, %d)\n", leftspeed, rightspeed);
	if (leftspeed < 0)
	{
		SetLeftMotorDirAndSpeed(false, -leftspeed); //neg value ==> reverse
	}
	else
	{
		SetLeftMotorDirAndSpeed(true, leftspeed); //pos or zero value ==> fwd
	}

	//Step2: Set right wheel direction and speed
	if (rightspeed < 0)
	{
		SetRightMotorDirAndSpeed(false, -rightspeed); //neg value ==> reverse
	}
	else
	{
		SetRightMotorDirAndSpeed(true, rightspeed); //pos or zero value ==> fwd
	}
}

//09/08/20 added bool bisFwd param for DRV8871 motor driver
void RunBothMotorsMsec(bool bisFwd, int timeMsec, int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors for timesec seconds at speednum speed
	//Inputs:
	//    timesec = time in seconds to run the Motors
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//    Step 1: Apply drive to both wheels
	//    Step 2: Delay timsec seconds
	//    Step 3: Remove drive from both wheels.
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//	11/25/15 - rev to use motor driver class object
	//	09/08/20 added bool bisFwd param for DRV8871 motor driver

	RunBothMotors(bisFwd, leftspeednum, rightspeednum);

	//Step 2: Delay timsec seconds
	delay(timeMsec);

	//Step3: Stop motors added 04/12/21
	StopBothMotors();
}

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

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

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

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

#ifndef NO_MOTORS

	if (bIsFwd)
	{
		//mySerial.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
		digitalWrite(In1_Right, LOW);
		analogWrite(In2_Right, speed);
	}
	else
	{
		//mySerial.printf("In FALSE block of SetRightMotorDirAndSpeed(%s, %d)\n",
		//	(bIsFwd == true) ? "true" : "false", speed);
		digitalWrite(In2_Right, LOW);
		analogWrite(In1_Right, speed);
	}

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

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

	bool retval = false;

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

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

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

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

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

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

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

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

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

	//if we get to here, there should be exactly one packet in the FIFO
	mpu.getFIFOBytes(data, packetSize);
	return 1;
}
#pragma endregion MPU5060 Support

void SpinTurnForever(bool b_ccw, float Kp, float Ki, float Kd, float degPersec)
{
	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point
	double prev_hdg = 0;
	unsigned long prev_uSec; //added 09/02/20

	//DEBUG!!
	mySerial.printf("SpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",
		Kp, Ki, Kd, degPersec);
	//DEBUG!!



	UpdateIMUHdgValDeg();//get new heading information from the MPU6050
	prev_hdg = IMUHdgValDeg;

	mySerial.printf("Msec\tHdg\tDhdg\tRate\tSet\terror\tDerr\tIval\tKp*err\tKi*Ival\tKd*Derr\tOut\n");

	//init some stuff
	TurnRateOutput = 0;
	TurnRateSetPoint_DPS = degPersec;
	MsecSinceLastTurnRateUpdate = 0;
	Ival = 0; lastErr = 0;


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

	while (true)
	{
		CheckForUserInput();

		//5/12/21 now time interval is constant at ~100mSec
		if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)
		{
			MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;

			//11/14/20 need to handle -179 to +179 transition
			deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

			//05/20/21 gfp can occasionally get one or two bad readings
			while (abs(deltaDeg) > TurnRateSetPoint_DPS)
			{
				mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",
					IMUHdgValDeg, prev_hdg, deltaDeg);

				UpdateIMUHdgValDeg();//get new heading information from the MPU6050
				deltaDeg = IMUHdgValDeg - prev_hdg;
				prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

				deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
				deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;
			}

			TurnRateVal_DPS = (1000 / TURN_RATE_UPDATE_INTERVAL_MSEC) * abs(deltaDeg);

			double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;
			double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);
			Ival += (error);
			double dErr = (error - lastErr);

			/*Compute PID Output*/
			TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

			/*Remember some variables for next time*/
			lastErr = error;
			lastTurnRateVal_DPS = TurnRateVal_DPS;

			if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)
				TurnRateOutput = MAX_SPIN_MOTOR_SPEED;
			else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)
				TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

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

			//DEBUG!!
			mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",
				millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,
				Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);
			//DEBUG

			prev_hdg = IMUHdgValDeg;
		}
	}
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

Name: FourWD_PulseTurnRateTest.ino

Created: 5/24/2021 4:49:52 PM

Author: FRANKNEWXPS15\Frank

#pragma region INCLUDES

#include <elapsedMillis.h>

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

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

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

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

#pragma endregion INCLUDES

#pragma region DEFINES

#define MPU6050_I2C_ADDR 0x69

#pragma endregion DEFINES

#pragma region PRE_SETUP

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

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

#pragma endregion PRE_SETUP

#pragma region MOTOR_PINS

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

const int In1_Left = 10;

const int In2_Left = 11;

const int In1_Right = 8;

const int In2_Right = 9;

#pragma endregion Motor Pin Assignments

#pragma region MOTOR_PARAMETERS

//drive wheel speed parameters

const int MOTOR_SPEED_FULL = 200; //range is 0-255

const int MOTOR_SPEED_MAX = 255; //range is 0-255

const int MOTOR_SPEED_HALF = 127; //range is 0-255

const int MOTOR_SPEED_QTR = 75; //added 09/25/20

const int MOTOR_SPEED_LOW = 50; //added 01/22/15

const int MOTOR_SPEED_OFF = 0; //range is 0-255

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

bool bIsForwardDir = true; //default is foward direction

#pragma endregion Motor Parameters

#pragma region MPU6050_SUPPORT

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float euler[3]; // [psi, theta, phi] Euler angle container

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

int GetPacketLoopCount = 0;

int OuterGetPacketLoopCount = 0;

//MPU6050 status flags

bool bMPU6050Ready = true;

bool dmpReady = false; // set true if DMP init was successful

volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR

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

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

bool bFirstTime = true;

#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19

#pragma endregion MPU6050 Support

const int CHG_CONNECT_LED_PIN = 50; //12/16/20 added for debug

elapsedMillis MsecSinceLastTurnRateUpdate;

//const int MAX_PULSE_WIDTH_MSEC = 100;

const int MAX_PULSE_WIDTH_MSEC = 50;

const int MIN_PULSE_WIDTH_MSEC = 0;

//const int MAX_SPIN_MOTOR_SPEED = 200;

const int MAX_SPIN_MOTOR_SPEED = 250;

const int MIN_SPIN_MOTOR_SPEED = 50;

const int TURN_RATE_UPDATE_INTERVAL_MSEC = 20;

//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 100;

double TurnRateOutput;

double TurnRateSetPoint_DPS;

double Ival = 0; double lastErr = 0;

double lastTurnRateVal_DPS;

double TurnRateVal_DPS;

void setup()

{

Serial.begin(115200);

#pragma region MPU6050

mySerial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);

mpu.initialize();

// verify connection

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

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

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

devStatus = mpu.dmpInitialize();

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

if (devStatus == 0)

{

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

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

mpu.setDMPEnabled(true);

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

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

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

bMPU6050Ready = true;

StartSec = millis() / 1000.f;

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

}

else

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

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

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

Serial.print(devStatus);

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

bMPU6050Ready = false;

}

#pragma endregion MPU6050

mySerial.printf("End of test - Stopping!\n");

while (true)

{

CheckForUserInput();

}

void loop()

{

}

void CheckForUserInput()

{

const int bufflen = 80;

char buff[bufflen];

memset(buff, 0, bufflen);

float degPerSec;

float Kp, Ki, Kd;

const char s[2] = ",";

char* token;

if (Serial.available() > 0)

{

// read the incoming byte:

int incomingByte = Serial.read();

// say what you got:

Serial.print("I received: ");

//Serial.println(incomingByte, DEC);

Serial.println(incomingByte, HEX); //chg to HEX 02/18/20

//02/18/20 experiment with multiple commands

switch (incomingByte)

{

case 0x50: //ASCII 'P'

case 0x70: //ASCII 'p'

StopBothMotors();

Serial.println(F("Setting Kp value"));

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

mySerial.printf("consumed %d\n", byte);

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

mySerial.printf("%s\n", buff);

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract degPerSec */

token = strtok(NULL, s);

degPerSec = atof(token);

mySerial.printf("Kp,Ki,Kd,degPerSec = %2.2f, %2.2f, %2.2f, %2.2f\n",

Kp, Ki, Kd, degPerSec);

PulseSpinTurnForever(true, Kp, Ki, Kd, degPerSec);

break;

case 0x51: //ASCII 'Q'

case 0x71: //ASCII 'q'

StopBothMotors();

Serial.println(F("Setting Kp value"));

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

mySerial.printf("consumed %d\n", byte);

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

mySerial.printf("%s\n", buff);

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract degPerSec */

token = strtok(NULL, s);

degPerSec = atof(token);

mySerial.printf("Kp,Ki,Kd,degPerSec = %2.2f, %2.2f, %2.2f, %2.2f\n",

Kp, Ki, Kd, degPerSec);

SpinTurnForever(true, Kp, Ki, Kd, degPerSec);

break;

Default:

Serial.println(F("In Default Case: Stopping Motors!"));

MoveAhead(0, 0);

while (true)

{

}

void PulseSpinTurnForever(bool b_ccw, float Kp, float Ki, float Kd, float degPersec)

{

float tgt_deg;

double prev_hdg = 0;

//DEBUG!!

mySerial.printf("PulseTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",

Kp, Ki, Kd, degPersec);

//DEBUG!!

mySerial.printf("Msec\tHdg\tDhdg\tRate\tSet\terror\tDerr\tIval\tKp*err\tKi*Ival\tKd*Derr\tOut\n");

//init some stuff

TurnRateOutput = 0;

TurnRateSetPoint_DPS = degPersec;

Ival = 0; lastErr = 0;

lastTurnRateVal_DPS;

TurnRateVal_DPS;

UpdateIMUHdgValDeg();

prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

MsecSinceLastTurnRateUpdate = 0;

//05/03/21 - back to the PWM technique

//Step4: Pulse the motors to get things started

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

delay(50);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

while (true)

{

//11/06/20 now just loops between ISR hits

CheckForUserInput();

//if (bTimeForNavUpdate) //set true in ISR

//{

// bTimeForNavUpdate = false;

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

//05/20/21 gfp can occasionally get one or two bad readings

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",

IMUHdgValDeg, prev_hdg, deltaDeg);

prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

CheckForUserInput();

deltaDeg = IMUHdgValDeg - prev_hdg;

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

prev_hdg = IMUHdgValDeg;

UpdateIMUHdgValDeg();

}

TurnRateVal_DPS = 10 * abs(deltaDeg); //05/23/21 4wd uses fixed 100mSec TIMER5 interval

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_PULSE_WIDTH_MSEC)

TurnRateOutput = MAX_PULSE_WIDTH_MSEC;

else if (TurnRateOutput < MIN_PULSE_WIDTH_MSEC)

TurnRateOutput = MIN_PULSE_WIDTH_MSEC;

//05/03/21 - back to the PWM technique

//Step4: Pulse the motors to full speed for the duration specified by the PID output

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_MAX);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_MAX);

//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);

//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);

delay(TurnRateOutput);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

//DEBUG!!

mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,

Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);

//DEBUG

prev_hdg = IMUHdgValDeg;

}

#pragma region MOTOR SUPPORT

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

void MoveReverse(int leftspeednum, int rightspeednum)

{

//Purpose: Move in reverse direction continuously - companion to MoveAhead()

//ProvEnA_Pinnce: G. Frank Paynter 09/08/18

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Plan:

// Step 1: Set reverse direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

// 01/22/20 now using Adafruit DRV8871 drivers

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

SetLeftMotorDirAndSpeed(REV_DIR, leftspeednum);

SetRightMotorDirAndSpeed(REV_DIR, rightspeednum);

}

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

void MoveAhead(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

//ProvEnA_Pinnce: G. Frank Paynter and Danny Frank 01/24/2014

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Plan:

// Step 1: Set forward direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

// 01/22/20 now using Adafruit DRV8871 drivers

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

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

SetLeftMotorDirAndSpeed(true, leftspeednum);

SetRightMotorDirAndSpeed(true, rightspeednum);

}

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

void StopLeftMotors()

{

analogWrite(In1_Left, MOTOR_SPEED_OFF);

analogWrite(In2_Left, MOTOR_SPEED_OFF);

}

void StopRightMotors()

{

analogWrite(In1_Right, MOTOR_SPEED_OFF);

analogWrite(In2_Right, MOTOR_SPEED_OFF);

}

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

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

{

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

//Inputs:

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

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

//Plan:

// Step 1: Apply drive to both wheels

//Notes:

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

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

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

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

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

//Step 1: Apply drive to both wheels

//DEBUG!!

//mySerial.printf("In RunBothMotors(%s, %d,%d)\n", bisFwd? "FWD":"REV", leftspeednum, rightspeednum);

//DEBUG!!

SetLeftMotorDirAndSpeed(bisFwd, leftspeednum);

SetRightMotorDirAndSpeed(bisFwd, rightspeednum);

}

void RunBothMotorsBidirectional(int leftspeed, int rightspeed)

{

//Purpose: Accommodate the need for independent bidirectional wheel motion

//Inputs:

// leftspeed - integer denoting left wheel speed. Positive value is fwd, negative is rev

// rightspeed - integer denoting right wheel speed. Positive value is fwd, negative is rev

//Outputs:

// left/right wheel motors direction and speed set as appropriate

//Plan:

// Step1: Set left wheel direction and speed

// Step2: Set right wheel direction and speed

//Step1: Set left wheel direction and speed

//DEBUG!!

//mySerial.printf("In RunBothMotorsBidirectional(%d, %d)\n", leftspeed, rightspeed);

if (leftspeed < 0)

{

SetLeftMotorDirAndSpeed(false, -leftspeed); //neg value ==> reverse

}

else

{

SetLeftMotorDirAndSpeed(true, leftspeed); //pos or zero value ==> fwd

}

//Step2: Set right wheel direction and speed

if (rightspeed < 0)

{

SetRightMotorDirAndSpeed(false, -rightspeed); //neg value ==> reverse

}

else

{

SetRightMotorDirAndSpeed(true, rightspeed); //pos or zero value ==> fwd

}

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

void RunBothMotorsMsec(bool bisFwd, int timeMsec, int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors for timesec seconds at speednum speed

//Inputs:

// timesec = time in seconds to run the Motors

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

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

//Plan:

// Step 1: Apply drive to both wheels

// Step 2: Delay timsec seconds

// Step 3: Remove drive from both wheels.

//Notes:

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

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

// 11/25/15 - rev to use motor driver class object

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

RunBothMotors(bisFwd, leftspeednum, rightspeednum);

//Step 2: Delay timsec seconds

delay(timeMsec);

//Step3: Stop motors added 04/12/21

StopBothMotors();

}

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

void StopBothMotors()

{

StopLeftMotors();

StopRightMotors();

}

void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)

{

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

#ifndef NO_MOTORS

if (bIsFwd)

{

digitalWrite(In1_Left, LOW);

analogWrite(In2_Left, speed);

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

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

}

else

{

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

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

digitalWrite(In2_Left, LOW);

analogWrite(In1_Left, speed);

}

#endif // !NO_MOTORS

}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)

{

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

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

#ifndef NO_MOTORS

if (bIsFwd)

{

//mySerial.printf("In TRUE block of SetRighttMotorDirAndSpeed(%s, %d)\n",

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

digitalWrite(In1_Right, LOW);

analogWrite(In2_Right, speed);

}

else

{

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

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

digitalWrite(In2_Right, LOW);

analogWrite(In1_Right, speed);

}

#endif // !NO_MOTORS

}

#pragma endregion Motor Support Functions

#pragma region MPU5060 Support

float UpdateIMUHdgValDeg()

{

//Purpose: Get latest yaw (heading) value from IMU

//Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE

//Outputs:

// returns true if successful, otherwise false

// IMUHdgValDeg updated on success

//Plan:

//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet

//Step2: read all available packets to get to latest data

//Step3: update IMUHdgValDeg with latest value

//Notes:

// 10/08/19 changed return type to boolean

// 10/08/19 no longer need mpuIntStatus

// 10/21/19 completely rewritten to use Homer's algorithm

// 05/05/20 changed return type to float vs bool.

bool retval = false;

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

if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow

{

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

IMUHdgValDeg = ypr[0] * 180 / M_PI;

retval = true;

}

//return retval;

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

}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)

{

mpu.resetFIFO();

delay(1);

//int countloop = 0;

fifoCount = mpu.getFIFOCount();

GetPacketLoopCount = 0;

//mySerial.printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);

while (fifoCount < packetSize && GetPacketLoopCount < max_loops)

{

GetPacketLoopCount++;

fifoCount = mpu.getFIFOCount();

delay(2);

}

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

if (GetPacketLoopCount >= max_loops)

{

return 0;

}

//if we get to here, there should be exactly one packet in the FIFO

mpu.getFIFOBytes(data, packetSize);

return 1;

}

#pragma endregion MPU5060 Support

void SpinTurnForever(bool b_ccw, float Kp, float Ki, float Kd, float degPersec)

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

double prev_hdg = 0;

unsigned long prev_uSec; //added 09/02/20

//DEBUG!!

mySerial.printf("SpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",

Kp, Ki, Kd, degPersec);

//DEBUG!!

UpdateIMUHdgValDeg();//get new heading information from the MPU6050

prev_hdg = IMUHdgValDeg;

mySerial.printf("Msec\tHdg\tDhdg\tRate\tSet\terror\tDerr\tIval\tKp*err\tKi*Ival\tKd*Derr\tOut\n");

//init some stuff

TurnRateOutput = 0;

TurnRateSetPoint_DPS = degPersec;

MsecSinceLastTurnRateUpdate = 0;

Ival = 0; lastErr = 0;

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

while (true)

{

CheckForUserInput();

//5/12/21 now time interval is constant at ~100mSec

if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

//05/20/21 gfp can occasionally get one or two bad readings

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",

IMUHdgValDeg, prev_hdg, deltaDeg);

UpdateIMUHdgValDeg();//get new heading information from the MPU6050

deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

}

TurnRateVal_DPS = (1000 / TURN_RATE_UPDATE_INTERVAL_MSEC) * abs(deltaDeg);

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)

TurnRateOutput = MAX_SPIN_MOTOR_SPEED;

else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)

TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,

Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);

//DEBUG

prev_hdg = IMUHdgValDeg;

}

This program includes a function called ‘CheckForUserInput()’ that, curiously enough, monitors the serial port for user input, and uses a ‘switch’ statement to execute different commands. One of these commands (‘q’ or ‘Q’) causes ‘SpinTurnForever()’ to execute, which in turn accepts a 4-paremeter input that specifies the three PID parameters plus the desired turn rage, in deg/sec. This routine then starts and manages a CCW turn ‘forever’, in the ‘while()’ block shown below:

while (true)
{
	CheckForUserInput();

	//5/12/21 now time interval is constant at ~100mSec
	if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)
	{
		MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

		UpdateIMUHdgValDeg();
		float deltaDeg = IMUHdgValDeg - prev_hdg;

		//11/14/20 need to handle -179 to +179 transition
		deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
		deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

		//05/20/21 gfp can occasionally get one or two bad readings
		while (abs(deltaDeg) > TurnRateSetPoint_DPS)
		{
			mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",
				IMUHdgValDeg, prev_hdg, deltaDeg);

			UpdateIMUHdgValDeg();//get new heading information from the MPU6050
			deltaDeg = IMUHdgValDeg - prev_hdg;
			prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

			deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;
		}

		TurnRateVal_DPS = (1000 / TURN_RATE_UPDATE_INTERVAL_MSEC) * abs(deltaDeg);

		double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;
		double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);
		Ival += (error);
		double dErr = (error - lastErr);

		/*Compute PID Output*/
		TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

		/*Remember some variables for next time*/
		lastErr = error;
		lastTurnRateVal_DPS = TurnRateVal_DPS;

		if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)
			TurnRateOutput = MAX_SPIN_MOTOR_SPEED;
		else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)
			TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

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

		//DEBUG!!
		mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",
			millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,
			Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);
		//DEBUG

		prev_hdg = IMUHdgValDeg;
	}
}

while (true)

{

CheckForUserInput();

//5/12/21 now time interval is constant at ~100mSec

if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

//05/20/21 gfp can occasionally get one or two bad readings

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

mySerial.printf("bad value detected: Hdg %2.2f, prevHdg %2.2f, delta %2.2f\n",

IMUHdgValDeg, prev_hdg, deltaDeg);

UpdateIMUHdgValDeg();//get new heading information from the MPU6050

deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg; //05/23/21 bugfix

deltaDeg = (deltaDeg >= 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg <= -180) ? deltaDeg + 360 : deltaDeg;

}

TurnRateVal_DPS = (1000 / TURN_RATE_UPDATE_INTERVAL_MSEC) * abs(deltaDeg);

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

TurnRateOutput = Kp * error + Ki * Ival + Kd * dErr;

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)

TurnRateOutput = MAX_SPIN_MOTOR_SPEED;

else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)

TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

mySerial.printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.0f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, deltaDeg, TurnRateVal_DPS, degPersec, error, dErr, Ival,

Kp * error, Ki * Ival, Kd * dErr, TurnRateOutput);

//DEBUG

prev_hdg = IMUHdgValDeg;

}

This routine mimics the PID library computations without suffering from library’s synchronization problems, and also allows me to fully instrument the contribution of each PID term to the output. This program also allows me to vary the computational interval independently of the rest of the program, bounded only by the ability of the MPU6050 to produce reliable readings.

After a number of trials, I started getting some reasonable results on my benchtop (hard surface with a thin electrostatic mat), as shown below:

As can be seen in the above plot, the turn rate is controlled pretty well around the 90 deg/sec turn rate, with an average turn rate of 89.6 deg/sec.

The plot below shows the same parameter set, but run on carpet rather than my bench.

Comparing these two plots it is obvious that a lot more motor current is required to make the robot turn on carpet, due to the much higher sideways friction on the wheels.

The next step was to see if the PID parameters for 90 deg/sec would also handle different turn rates. Here are the plots for 45 deg/sec on my benchtop and on carpet:

And then 30 deg/sec on benchtop and carpet

It is clear from the above plots that the PID values (5,0.8,0.1) do fairly well for the four wheel robot, both on hard surfaces and carpet.

Having this kind of control over turn rate is pretty nice. I might even be able to do turns by setting the turn rate appropriately and just timing the turn, or even vary the turn rate during the turn. For a long turn (say 180 deg) I could do the first 90-120 at 90 deg/sec, and then do the last 90-60 at 30 deg/sec; might make for a much more precise turn.

All of the above tests were done with a 20 mSec time interval, which is 5x smaller than the current 100mSec time interval used for the master timer in Wall-E2. So, my next set of tests will keep the turn rate constant and slowly increase the time interval to see if I can get back to 100 mSec without any major sacrifice in performance.

28 May 2021 Update:

I went back through the tests using a 100 mSec interval instead of 20 mSec, and was gratified to see that there was very little degradation in performance. The turn performance was a bit more ‘jerky’ than with a 20 mSec interval, but still quite acceptable, and very well controlled, both on the benchtop and carpet surfaces – Yay! Here are some plots to show the performance.

31 May 2021 Update:

I made some additional runs on benchtop and carpet, thinking I might be able to reduce the turn-rate oscillations a bit. I found that by reducing the time interval back to 20 mSec and increase the ‘D’ (differential) parameter. After some tweaking back and forth, I wound up with a PID set of (5, 0.8, 3). Using these parameters, I got the following performance plots.

PID = (5,0.8,3), 20mSec interval, 90 deg/sec

As can be seen in the Excel plot and the movie, the turn performance is much smoother – yay!

Stay tuned!

Frank

New Batteries for Wall-E2

Leave a reply

I have been using a set of four Panasonic 18650 LiPo batteries in a 2-cell stack configuration in Wall-E2, my autonomous wall following robot. for a little over three years now, and they are starting to show their age. So, I decided to replace these

with these

Panasonic 18650G-A 3500 mAH 3.7V LiPo battery

Three years younger, and with another 100 mAH (rated, at least).

Here’s a photo record of the process of changing out the batteries

Old batteries still installed (can tell by the green showing through the ‘V5’ cutout)

Stay tuned,

Frank

Turn Rate PID Tuning, Part II

1 Reply

Posted 14 May 2021,

In my previous post on this topic, I described my efforts to use the Arduino PID library to manage turns with Wall-E2, my autonomous wall following robot. This post talks about a problem I encountered with the PID library when used in a system that uses an external timing source, like the TIMER5 ISR in my system and a PID input that depends on accurate timing, such as my turn-rate input.

In my autonomous wall-following robot project, I use TIMER5 on the Arduino Mega 2560 to generate an interrupt ever 100 mSec, and update all time-sensitive parameters in the ISR. These include results from all seven VL53L0X ToF distance sensors, the front-mounted LIDAR, and heading information from a MP6050 IMU. This simplifies the software immensely, as now the latest information is available throughout the code, and encapsulates all sensor-related calls to a single routine.

In my initial efforts at turn-rate tuning using the Arduino PID library, I computed the turn rate in the ISR by simply using

turn_rate = 10*curr_hdg - prev_hdg //appropriately adjusted for 0-180 transitions, etc.

1	turn_rate = 10*curr_hdg - prev_hdg //appropriately adjusted for 0-180 transitions, etc.

This actually worked because, the ISR frequency and the PID::Compute() frequency were more or less the same. However, since the two time intervals are independent of each other there could be a phase shift, which might drift slowly over time. Also, if either timer interval is changed sometime down the road, the system behavior could change dramatically. I thought I had figured out how to handle this issue by moving the turn-rate computation inside the PID::Compute() function block, as shown below

In a typical PID use case, you see code like the following:

if(myPID.Compute())
{
    //compute the new turn rate
   //use the PID output term to control something
   ...
   ...
}

if(myPID.Compute())

{

//compute the new turn rate

//use the PID output term to control something

...

}

After making the above change, I started getting really weird behavior, and all my efforts at PID tuning failed miserably. After a LOT of troubleshooting and head-scratching, I finally figured out what was happening. In the above code configuration, the PID generates a new output value BEFORE the new turn rate is computed, so the PID is always operating on information that is at least 100mSec old – not a good way to run a railroad!

Some of the PID documentation I researched said (or at least implied) that by setting the PID’s sample time to zero using PID::SetSampleTime(0), that Compute() would actually produce a new output value every time it was called. This meant that I could do something like the following:

if (bTimeForNavUpdate) //set true in ISR
{
	bTimeForNavUpdate = false;

	//4/28/21 now time interval is constant at ~100mSec
	//11/14/20 need to handle -179 to +179 transition
	float deltaDeg = IMUHdgValDeg - prev_hdg;
	deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;
	deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;
	TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

	TurnRatePID.Compute();//04/10/21 SampleTime == 0 so now this updates every time

	SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);
	SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);
	prev_hdg = IMUHdgValDeg;
}

if (bTimeForNavUpdate) //set true in ISR

{

bTimeForNavUpdate = false;

//4/28/21 now time interval is constant at ~100mSec

//11/14/20 need to handle -179 to +179 transition

float deltaDeg = IMUHdgValDeg - prev_hdg;

deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;

TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

TurnRatePID.Compute();//04/10/21 SampleTime == 0 so now this updates every time

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);

prev_hdg = IMUHdgValDeg;

}

Great idea, but it didn’t work! After some more troubleshooting and head-scratching, I finally realized that the PID::SetSampleTime() function specifically disallows a value of zero, as it would cause the ‘D’ term to go to infinity – oops! Here’s the relevant code

void PID::SetSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;
   }
}

void PID::SetSampleTime(int NewSampleTime)

{

if (NewSampleTime > 0)

{

double ratio = (double)NewSampleTime

/ (double)SampleTime;

ki *= ratio;

kd /= ratio;

SampleTime = (unsigned long)NewSampleTime;

}

As can be seen from the above, an argument of zero is simply ignored, and the sample time remains unchanged. When I pointed this out to the developer, he said this was by design, as the ‘ratio’ calculation above would be undefined for an input argument of zero. This is certainly a valid point, but makes it impossible to synch the PID to an external master clock – bummer!

After some more thought, I modified my copy of PID.cpp as follows:

void PID::SetSampleTime(int NewSampleTime)
{
  Serial.println("In PID::SetSampleTime with NewSampleTime = "); Serial.println(NewSampleTime);
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      //SampleTime = (unsigned long)NewSampleTime;
   }

    SampleTime = (unsigned long)NewSampleTime;
}

void PID::SetSampleTime(int NewSampleTime)

{

Serial.println("In PID::SetSampleTime with NewSampleTime = "); Serial.println(NewSampleTime);

if (NewSampleTime > 0)

{

double ratio = (double)NewSampleTime

/ (double)SampleTime;

ki *= ratio;

kd /= ratio;

//SampleTime = (unsigned long)NewSampleTime;

}

SampleTime = (unsigned long)NewSampleTime;

}

By moving the SampleTime = (unsigned long)NewSampleTime; line out of the ‘if’ block, I can now set the sample time to zero without causing problems with the value of ‘ratio’. Now PID::Compute() will generate a new output value every time it is called, which synchs the PID engine with the program’s master timing source – yay!

I tried out a slightly modified version of this technique on my small 2-wheel robot. The two-wheeler uses an Arduino Uno instead of a Mega, so I didn’t use a TIMER interrupt. Instead I used the ‘elapsedMillisecond’ library and set up an elapsed time of 100 mSec, and also modified the program to turn indefinitely at the desired turn rate in deg/sec.

I experimented with two different methods for controlling the turn rate – a ‘PWM’ method where the wheel motors are pulsed at full speed for a variable pulse width, and a ‘direct’ method where the wheel motor speeds are varied directly to achieve the desired turn rate. I thought the PWM method might work better on a heavier robot for smaller angle turns as there is quite a bit of inertia to overcome, but the ‘direct’ method might be more accurate.

Here’s the code for the ‘direct’ method, where the wheel speeds are varied with

SetLeftMotorDirAndSpeed

1	SetLeftMotorDirAndSpeed

void SpinTurnForever(bool b_ccw, float kp, float ki, float kd, float degPersec)
{
	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point
	double prev_hdg = 0;
	unsigned long prev_uSec; //added 09/02/20

	//DEBUG!!
	mySerial.printf("SpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",
		kp, ki, kd, degPersec);
	//DEBUG!!

//Step3: 11/14/20 activate turn rate PID
	TurnRateOutput = 0;
	TurnRatePID.SetMode(AUTOMATIC);
	TurnRatePID.SetTunings(kp, ki, kd);
	TurnRateSetPoint = degPersec;

	//set the limits
	//TurnRatePID.SetOutputLimits(10, 100);
	TurnRatePID.SetOutputLimits(MOTOR_SPEED_LOW, MOTOR_SPEED_MAX);
	TurnRatePID.SetSampleTime(0); //Compute() runs every time it is called

	UpdateIMUHdgValDeg();
	prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

	mySerial.printf("Msec\tHdg\tPrevHdg\tRate\tSet\tOut\n");

	MsecSinceLastTurnRateUpdate = 0;

	while (true)
	{
		CheckForUserInput();

		//5/12/21 now time interval is constant at ~100mSec
		if (MsecSinceLastTurnRateUpdate >= 100)
		{
			MsecSinceLastTurnRateUpdate -= 100;

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;
			prev_hdg = IMUHdgValDeg;

			//11/14/20 need to handle -179 to +179 transition
			deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;
			TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

			TurnRatePID.Compute();  //this is where everything is computed

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

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

			prev_hdg = IMUHdgValDeg;
		}
	}
}

void SpinTurnForever(bool b_ccw, float kp, float ki, float kd, float degPersec)

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

double prev_hdg = 0;

unsigned long prev_uSec; //added 09/02/20

//DEBUG!!

mySerial.printf("SpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",

kp, ki, kd, degPersec);

//DEBUG!!

//Step3: 11/14/20 activate turn rate PID

TurnRateOutput = 0;

TurnRatePID.SetMode(AUTOMATIC);

TurnRatePID.SetTunings(kp, ki, kd);

TurnRateSetPoint = degPersec;

//set the limits

//TurnRatePID.SetOutputLimits(10, 100);

TurnRatePID.SetOutputLimits(MOTOR_SPEED_LOW, MOTOR_SPEED_MAX);

TurnRatePID.SetSampleTime(0); //Compute() runs every time it is called

UpdateIMUHdgValDeg();

prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

mySerial.printf("Msec\tHdg\tPrevHdg\tRate\tSet\tOut\n");

MsecSinceLastTurnRateUpdate = 0;

while (true)

{

CheckForUserInput();

//5/12/21 now time interval is constant at ~100mSec

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;

TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

TurnRatePID.Compute(); //this is where everything is computed

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

mySerial.printf("%lu\t%4.2f\t%4.2f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, prev_hdg, TurnRateVal, degPersec, TurnRateOutput);

//DEBUG

prev_hdg = IMUHdgValDeg;

}

Here’s the code for the PWM method: the only difference is that is the duration of the pulse that is varied, not the wheel speed.

void PulseSpinTurnForever(bool b_ccw, float kp, float ki, float kd, float degPersec)
{
	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;
	bool bResult = true; //04/21/20 added so will be only one exit point
	double prev_hdg = 0;
	unsigned long prev_uSec; //added 09/02/20

	//DEBUG!!
	mySerial.printf("PulseSpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",
		kp, ki, kd, degPersec);
	//DEBUG!!

//Step3: 11/14/20 activate turn rate PID
	TurnRateOutput = 0;
	TurnRatePID.SetMode(AUTOMATIC);
	TurnRatePID.SetTunings(kp, ki, kd);
	TurnRateSetPoint = degPersec;

	//set the limits
	TurnRatePID.SetOutputLimits(10, 100);
	TurnRatePID.SetSampleTime(0); //Compute() runs every time it is called

	UpdateIMUHdgValDeg();
	prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

	mySerial.printf("Msec\tHdg\tPrevHdg\tRate\tSet\tOut\n");
	uint32_t prevUsec = micros();//initialize
	uint32_t nowUsec = prevUsec;

	MsecSinceLastTurnRateUpdate = 0;

	while (true)
	{
		CheckForUserInput();

		//5/12/21 now time interval is constant at ~100mSec
		if (MsecSinceLastTurnRateUpdate >= 100)
		{
			MsecSinceLastTurnRateUpdate -= 100;

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;
			prev_hdg = IMUHdgValDeg;

			//11/14/20 need to handle -179 to +179 transition
			deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;
			deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;
			TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

			TurnRatePID.Compute();  //this is where everything is computed

			//05/03/21 - back to the PWM technique
		//Step4: Pulse the motors to full speed for the duration specified by the PID output
			digitalWrite(CHG_CONNECT_LED_PIN, HIGH);
			SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);
			SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);
			delay(TurnRateOutput);
			StopBothMotors();
			digitalWrite(CHG_CONNECT_LED_PIN, LOW);

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

			StopBothMotors();

			prev_hdg = IMUHdgValDeg;
		}
	}
}

void PulseSpinTurnForever(bool b_ccw, float kp, float ki, float kd, float degPersec)

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

bool bResult = true; //04/21/20 added so will be only one exit point

double prev_hdg = 0;

unsigned long prev_uSec; //added 09/02/20

//DEBUG!!

mySerial.printf("PulseSpinTurnForever TurnRatePID parameters Kp/Ki/Kd/Setpoint = %2.2f/%2.2f/%2.2f/%2.2f\n",

kp, ki, kd, degPersec);

//DEBUG!!

//Step3: 11/14/20 activate turn rate PID

TurnRateOutput = 0;

TurnRatePID.SetMode(AUTOMATIC);

TurnRatePID.SetTunings(kp, ki, kd);

TurnRateSetPoint = degPersec;

//set the limits

TurnRatePID.SetOutputLimits(10, 100);

TurnRatePID.SetSampleTime(0); //Compute() runs every time it is called

UpdateIMUHdgValDeg();

prev_hdg = IMUHdgValDeg; //11/06/20 now IMUHdgValDeg updated in ISR

mySerial.printf("Msec\tHdg\tPrevHdg\tRate\tSet\tOut\n");

uint32_t prevUsec = micros();//initialize

uint32_t nowUsec = prevUsec;

MsecSinceLastTurnRateUpdate = 0;

while (true)

{

CheckForUserInput();

//5/12/21 now time interval is constant at ~100mSec

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg;

//11/14/20 need to handle -179 to +179 transition

deltaDeg = (deltaDeg > 180) ? deltaDeg - 360 : deltaDeg;

deltaDeg = (deltaDeg < -180) ? deltaDeg + 360 : deltaDeg;

TurnRateVal = 10 * abs(deltaDeg); //now time interval is constant 1/10 sec

TurnRatePID.Compute(); //this is where everything is computed

//05/03/21 - back to the PWM technique

//Step4: Pulse the motors to full speed for the duration specified by the PID output

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);

delay(TurnRateOutput);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

//DEBUG!!

mySerial.printf("%lu\t%4.2f\t%4.2f\t%2.1f\t%2.1f\t%2.1f\n",

millis(), IMUHdgValDeg, prev_hdg, TurnRateVal, degPersec, TurnRateOutput);

//DEBUG

StopBothMotors();

prev_hdg = IMUHdgValDeg;

}

Here’s a short video showing the two-wheel robot doing a spin turn using the PWM technique with a desired turn rate of 90 deg/sec, using PID = (1,0.5,0).

The average turn rate for the entire run was about 85 deg/sec.

Here’s another run, this time on carpet:

Average turn rate for the entire run was about 85 deg/sec

Here’s some data from the ‘direct’ method, on hard flooring

And on carpet

So, it appears that either the PWM or ‘direct’ methods are effective in controlling the turn rate, and I don’t really see any huge difference between them. I guess the PWM method might be a little more effective with the 4-wheel robot caused by the wheels having to slide sideways while turning.

Stay Tuned!

Frank

Wall Parallel Find PID Tuning

1 Reply

Posted 10 April 2021

In addition to using PID for homing to its charging station and for turn rate control, Wall-E2 also uses PID for finding the parallel orientation to a nearby wall. After successfully tuning the turn rate and IR Homing PID controllers using the Ziegler-Nichols method for PID tuning, I decided to see what I could do with the PID controller for parallel orientation finding

Wall-E2 uses two 3-element VL53L0X Time-of-Flight distance sensors for parallel orientation finding. The idea is that when all three sensors report the same distance, then the robot must be oriented parallel to the wall. The Teensy 3.5 Array Controller MCU calculates a ‘steering value’ using the expression (shown for the left side array):

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

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

This value is fed to the PID engine, which drives the motors to zero it out – thus arriving at a parallel orientation. Originally I just basically ‘winged it’ in choosing PID Kp, Ki & Kd values, arriving emperically at Kp = 200, Ki = 50, Kd = 0. However, after going through the K-N process with Wall-E2’s other two PID control setups, I decided to try it with this one as well.

The first step is to determine Kc, the Kp value for which the system oscillates in a reasonably stable fashion. To accomplish this I started with Kp = 20 and worked my way up in stages, plotting the ‘steering value’ each time. The last three trials (as shown in the following plots) were for Kp = 400, Kp = 500 and Kp = 600:

Looking at the above plots, it looks like Kp = 600 will work for Kc. Using the K-N formula, we get

Kp = 0.5Kc = 300
Ki = 0.45Kc = 270
Kd = 0.6Kc = 360

Kp = 0.5Kc = 300

Ki = 0.45Kc = 270

Kd = 0.6Kc = 360

Using the above values for the Parallel Find PID, we get the following plot:

Which is not exactly what I thought it would be – it looks like my guess for Kc must be off. Trying again with a Kc = 400 –> PID = 200,180,240, we get:

which, to my eye at least, seems a bit better.

To test how this worked with ‘real’ parallel finding, I incorporated these parameter values into my ‘RotateToParallelOrientation()’ routine and ran a couple of tests. Here’s one where Wall-E2 starts in the ‘toed-out’ position:

And here’s the Excel plot from this same run

As can be seen, the robot takes less than two seconds to converge on a pretty decent parallel orientation, starting from a 30-40º angle to the near wall.

Here’s another run where the robot starts in the ‘toed-in’ orientation.

And here’s the Excel plot for the run

Again, the robot gets to a pretty decent parallel orientation within 2 seconds of the start of the run. The only concern I have with this run is that it winds up pretty close to the wall.

Serial Bridge over WiFi using ESP32 DevKitC

3 Replies

Posted 09 April 2021

I have been wanting to upgrade my robot’s brains from the old Arduino Mega 2560 to a more modern Teensy 3.x or 4.x MCU for some time now, but have been stymied by the lack of Over-The-Air (OTA) programming support. My current Mega-based setup uses a pair of Pololu Wixels to form a transparent wireless bi-directional serial link between my PC and the robot. My Microsoft Visual Studio IDE sees the link as just another COM port, and the Mega thinks it’s Tx/Rx0 lines are connected directly to the PC; couldn’t be simpler and more effective. The range of the Wixel connection is on the order of a few tens of meters, but that’s all I need for managing my autonomous wall-following robot.

Unfortunately, the Teensy world seems a bit short on effective wireless OTA solutions, or at least my searches have come up mostly dry. Some users report they have been able to use a Raspberry Pi Zero-W for this purpose, but that seems like a major overkill

After yet another Google search, I started seeing some posts about the ability to form a wireless serial data bridge using an ESP32 wireless-enabled development module, and I since I happened to have a ESP32 DevKitC hanging around Idecided to try my hand at that, maybe as a first step to achieving OTA nirvana with a Teensy 3/4.x.

The starting point for this project was this tutorial (the bottom ‘Serial Bridge Using ESP8266 (Simpler)’ part), based on this module sold on the AliExpress site. The AliExpress module wasn’t exactly the same as the DevKitC I had on hand, but it was close enough and AFAICT the pinouts are identical.

The first step was getting the Arduino/VS2019 IDE to recognize the ESP32 hardware. This was a semi-major PITA, but I eventually found some posts showing how the board information could be added to the system.

The next step was getting PuTTY downloaded and installed on my Win10 system. This went fairly easily.

Next I downloaded the ESP32 serial bridge software from the GitHub site and set it up as a project in my VS2019/Visual Micro IDE. This all seemed to work, and I was able to upload the program to my DevKitC module

I also happened to have an old CKdevices FTDI module, so I was able to connect to the required serial lines on the ESP32 module. However, I discovered that the tutorial didn’t use the standard Tx/Rx lines shown in the DevKitC pinout diagram; evidently they had to be moved to allow all three UART/USB modules to be connected at the same time. The actual pinouts used by the tutorial are:

here is the wiring diagram recomendation: https://raw.githubusercontent.com/AlphaLima/ESP32-Serial-Bridge/master/ESP32-SerialBridge.jpg
Pinning
COM0 Rx <-> GPIO21
COM0 Tx <-> GPIO01
COM1 Rx <-> GPIO16
COM1 Tx <-> GPIO17
COM2 Rx <-> GPIO15
COM2 Tx <-> GPIO04

NOTE: The PIN assignment has changed and may not look straigt forward (other PINs are marke as Rx/Tx), but this assignment allows to flash via USB also with hooked MAX3232 serial drivers.

here is the wiring diagram recomendation: https://raw.githubusercontent.com/AlphaLima/ESP32-Serial-Bridge/master/ESP32-SerialBridge.jpg

Pinning

COM0 Rx <-> GPIO21

COM0 Tx <-> GPIO01

COM1 Rx <-> GPIO16

COM1 Tx <-> GPIO17

COM2 Rx <-> GPIO15

COM2 Tx <-> GPIO04

NOTE: The PIN assignment has changed and may not look straigt forward (other PINs are marke as Rx/Tx), but this assignment allows to flash via USB also with hooked MAX3232 serial drivers.

After some fumbling around, I finally realized that the ‘GPIOxx’ labels in the above pinout list is the same as the numbers printed on the actual module, i.e. ‘GPIO21′ <==> ’21’ on the module PCB (the lone exception to this is ‘GPIO1’, which corresponds to ‘TX’ on the module). Here’s a picture from the GitHub documentation showing the actual layout expected by the sofware.

After working my way though these problems, here’s the physical setup I wound up with:

CkDevices FTDI UART/USB connected to COM5 on USB side, Tx2/Rx2 on ESP32 side

Detail showing the pin connections to ESP32 module. Yellow is connected to ESP32 Rx2, Green to Tx2

With the above hardware setup, I was able to pass serial data from one PuTTY terminal connected to the 192.168.4.1/8882 port, and another connected to COM5 on the PC (corresponding to COM2 on the ESP32).

Having accomplished this, I’m still unsure how to go about using this capability to program a Teensy 3/4.x using the wireless link. More study required!

Frank

Turn Rate PID Tuning

1 Reply

Posted 05 April 2021

Wall-E2, my autonomous wall-following robot, does a lot of turns to follow walls. Originally Wall-E2 used a simple timing algorithm to make turns, but this wasn’t very accurate. On a hard surface a 5-second turn at half motor speed could result in a 360º turn, while the same 5 seconds on carpet my only cover 90º. After installing the MPU6050 IMU about 18 months ago, turns could be controlled much more accurately, but the turn rate still varied widely on carpet vs hard flooring. Some time ago, I revised Wall-E2’s program to use a PID engine for turn rate control, but this resulted in a low-frequency ‘motorboating’ movement as the robot ramped up motor current until the angle started changing, followed by ramping the motor current down again because the turn rate target had been exceeded.

After having some success with improving the robot’s performance in homing to the charging station by utilizing the Zeigler-Nichols tuning method, I decided to try using it to improve turn rate control.

The method starts with setting the PID variables to (Kp, 0, 0) and then varying the Kp values to determine Kc, the value at which the setpoint variable (in this case, the turn rate in deg/sec) exhibits a steady oscillation. The value of Kc is then used to calculate all three PID parameters using

The existing setting for Kp, Ki, and Kd was Kp = 5, Ki = 0, Kd = 10, and this resulted in the following plot for a single 270º turn.

To start the process of determining Kc, I first zeroed out the Kd factor, resulting in the following plot:

So the system is clearly exhibiting a ‘constant amplitude oscillation’, but Kc is the minimum Kp value that produces oscillation. So, I started reducing Kp looking for the point at which the oscillation stopped, producing the following plots:

Comparing the above plots, it seems the value of Kc is probably around 0.1. Using the Z-N formula above, I get

Kp = 0.5Kc = 0.05
Ki = 0.45Kc = 0.045
Kd = 0.6Kc = 0.06

Kp = 0.5Kc = 0.05

Ki = 0.45Kc = 0.045

Kd = 0.6Kc = 0.06

These values are almost two orders of magnitude smaller than the values I had been using – ouch! Looking back on the original work, I had declared the Kp, Ki, & Kd variables as ‘const int’, which means the lowest positive value I could use was ‘1’, which might explain why I never tried anything smaller. Of course, I could have also done what I did with the home-to-charger PID engine and arranged the turn rate calculation so that instead of Kc = 0.1, it would have been more like 100, resulting in (Kp,Ki,Kd) values of (50,45,60).

Anyway, using values of (0.05, 0.045, 0.06) results in the following plot

And here is a short video showing the robot executing the 270º turn plotted above

I was quite impressed by the difference between what I had before and the current performance after implementing the Z-N tuning method; the turn rate was an almost constant 40º/sec (an average of 42.9º according to Excel). And, now that I have a semi-constant rate to look at, it appears to me that I should probably crank up the turn rate to something more like 90º/sec.

With the turn rate cranked up to 90º/sec and using the same values for Kp, Ki, Kd, I get the following plot and video.

The plot of turn rate vs time shows considerably more variation with a 90º/sec turn rate, as compared to 45º/sec; maybe Kc is different due to the different physical dynamics of the robot? The average turn rate from the Excel data is 80º/sec; not the 90º/sec I was looking for, but still not bad. Even so, this result is much better than I had before, with extreme motorboating between 0º/sec and something much higher. I may or may not try to re-determine Kc, but in the meantime I think I’ll run with this for a while.

10 April 2021 Update:

Some of the above data were collected at a pretty low charge level. When I had the opportunity to recharge Wall-E2 I re-took the 90º/sec run with the following results:

As the above plot shows, the turn rate was held almost constant (average of 87.9º) throughout the turn – very nice!

Here’s another run, this time on carpet with a fully charged battery. As expected, the robot has a harder time getting started on carpet due to the added sideways friction on the wheels. However, once the turn gets started, it stabilizes fairly well on the 90º turn rate target

Stay tuned,

Frank

Another Try at Charging Station Homing PID Tuning

Leave a reply

Posted 04 April 2021

Lately I have been working on improving the performance of Wall-E2, my autonomous wall-following robot, when homing in and connecting to it’s charging station. The robot uses the PID (Proportional-Integral-Derivative) library to drive the motors to home in on an IR beacon, and this ‘mostly’ works, but still occasionally hangs up on the lead-in rails. I have made several attempts to get this right (see this post and earlier work), but have never really gotten it zeroed in. After yet another web search for tuning help, I ran across this post dealing with the Ziegler-Nichols method for PID tuning. Basically the method starts by setting the proportional (Kp), integral (Ki) and derivative (Kd) terms to zero, and then slowly increasing the proportional (Kp) term until a ‘stable oscillation condition’ is achieved (Kp = Kc). Then the Ki & Kd terms can be calculated using the following relationships:

Getting to the ‘Kc’ (Kp-critical) value for my setup is a bit more difficult than normal, as the PID engine only operates for a few seconds, from the time the IR homing beacon is detected, to the time the robot actually connects (or doesn’t) to the charging probe. Here’s a short video showing a typical run (Kd = 150 in this case), and an Excel plot of the steering value from the same run.

As can be seen from the above, there really isn’t much of an ‘oscillation’ to go on – there is basically only one full cycle from about 10.5 sec to around 12.0 sec.

Here’s another run, this time with Kd set to 200. As can be seen, this is much more like what I was expecting to see, with several full cycles of oscillation. The amplitude trails off a bit toward the end, but this may have been due to a low battery level – I’ll have to repeat this experiment after getting a full charge into the robot.

Kp = 200, Kd = Ki = 0. Period is approximately 1.3 sec

However, using the above data with Kc = 200, we get

Kp = 0.5Kc = 100
Ki = 0.45Kc = 90
Kd = 0.6Kc = 120

Kp = 0.5Kc = 100

Ki = 0.45Kc = 90

Kd = 0.6Kc = 120

I revised my program to incorporate the Z-N numbers from the above calculations, and this resulted in the homing runs shown below; in the first one, Wall-E2 was oriented directly at the charging station beacon, and the robot’s track was pretty much direct, with no side-side oscillation at all. In the second one I oriented the robot a bit off-axis to excite a more active homing response. In both runs, the LEDs on the rear of the robot show the current relative wheel speed commands – LED’s to the left of center indicate higher wheel speed on the left, and vice versa. In the first run, the LEDs show that there is some oscillation of wheel speed commands, but it is relatively small, leading to an almost serene homing performance. In the second run the initial orientation offset forces the robot to more actively manage the wheel speeds to stay ‘on the beam’.

Stay tuned,

Frank

Charging Station Initial Approach Algorithm Improvement

Leave a reply

Posted 20 March, 2021

In order to realize my long-term goal of a fully autonomous wall-following robot, Wall-E2 has to be able to reliably mate to its charging station when it gets low on go-juice. Unfortunately, Wall-E2 occasionally fails to mate properly, usually due to an initial misalignment with the center of the IR homing beam. I haven’t worried too much about this, as there have been more pressing problems, but as these have been solved, the mating problem has risen to the top of the to-do list.

The basic geometry for the charging station is shown below:

Tilted gate option. The tilt decreases the minimum required IR beam capture distance from about 1.7m to about 1.0m

As long as the robot starts its approach on or near the boresight of the IR beam, all goes swimmingly. However, if Wall-E2 detects the IR beam while tracking the wall at right angles to the one depicted above, it can easily start its approach before getting to the center of the beam, resulting in it getting stuck on the outside guide-in rail (upper rail in the above diagram). In addition, if Wall-E2 is tracking too close to the wall above, it can actually get stuck on the inside guide-in rail (lower rail in the above diagram).

So, what is needed here is a way to force the robot to line up on the IR beam centerline before committing to the mating approach. To investigate this, I created a part-task version of Wall-E2’s operating system that does just one thing; it detects the IR homing signal, and then takes action to position itself in the center of the IR homing beam, aligned with the beam’s boresight. In the aviation instrument (blind) flying world, this is known as the ‘IAP’ (Initial Approach Point), so I needed to create an algorithm so Wall-E2 could navigate to the Charging Station IAP, and start it’s final approach from the same place every time.

In previous work I have gotten Wall-E2 smart enough to track walls at a constant offset. so this is where I started with the current effort. When Wall-E2 starts to track a wall, the first thing it does is use the near-side array of VL53L0X IR laser TOF sensors to orient parallel to the wall, without regard to the absolute offset. It then angles toward or away from the wall to achieve tracking at the desired offset.

The starting position for the current effort is with the robot placed close to the wall leading to the charging station, pointed generally toward the charging station. When the robot wakes up, it sees that there is an active IR homing beacon, and takes action to navigate to the IAP.

First it uses the parallel orientation algorithm to align itself parallel to the near wall so it can measure it’s offset from the wall, and also to ensure that the front distance measurement accurately reflects the distance from the robot to the charging station.
Next, it compares the wall offset and front distance measurements to the known values for the IAP, i.e. a 50 cm offset at a distance of 180 cm. It then calculates how much additional offset it needs to place itself in the center of the beam.
If necessary, the robot turns 90º away from the wall and moves away to achieve the desired offset. If the robot is already far enough away from the wall, it skips this step
After getting far enough away, the robot turns in place (a ‘Spin Turn’) until the signal strength of the received IR homing beacon rises above a set threshold. This gets the robot oriented generally in the direction of the charging station
The last step is to fine-tune the robot’s orientation so that it is centered in the beam and also well aligned with the beam boresight.

The following photograph shows the robot at the IAP, ready to start the final approach to the charging station.

Wall-E2 at the Initial Approach Point, ready to start the final approach to the charging station.

And the following video shows the entire process, up to the point where the robot would actually start the final approach.

15 April 2021 Update:

One of the issues with the current initial approach algorithm is the lack of accuracy in achieving the desired wall offset, due to Wall-E2’s tendency to ‘coast’ past the desired distance. I could just lower the offset target by a fixed amount to account for the ‘coast’ effect, but since that changes significantly depending on whether Wall-E2 is on carpet or hard flooring, that doesn’t sound like a good ide.

Instead, I decided to use yet another PID object to manage offset distance acquisition, using the following algorithm:

//04/12/21 Wall offset testing
	const double OffsetDistKp = 5.f;
	const double OffsetDistKi = 3.f; 
	const double OffsetDistKd = 0.0f;
	double OffsetDistSetpointCm, OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive
	volatile double OffsetDistVal = 0; //11/07/20 needs to be volatile as it is updated in ISR (and has to be 'double')

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

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

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

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

//04/12/21 Wall offset testing

const double OffsetDistKp = 5.f;

const double OffsetDistKi = 3.f;

const double OffsetDistKd = 0.0f;

double OffsetDistSetpointCm, OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive

volatile double OffsetDistVal = 0; //11/07/20 needs to be volatile as it is updated in ISR (and has to be 'double')

PID OffsetDistPID(&OffsetDistVal, &OffsetDistOutput, &OffsetDistSetpointCm,

OffsetDistKp, OffsetDistKi, OffsetDistKd, DIRECT);//12/14/20 re-doing this from scratch

OffsetDistPID.SetMode(AUTOMATIC);

OffsetDistPID.SetSampleTime(0);

OffsetDistPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

StopBothMotors();

int speed;

OffsetDistSetpointCm = 30;

mySerial.printf("Msec\tDist\tdiff\tspeed\tvar\tavg\n");

uint16_t avg = CalcRearDistAvgMM(Lidar_Rear, Rear_Dist_PrevMean);

while (abs(Lidar_Rear - 10*OffsetDistSetpointCm) > 10 || abs(avg - 10*OffsetDistSetpointCm) > 10)

{

CheckForUserInput();

if (bTimeForNavUpdate)

{

bTimeForNavUpdate = false;

OffsetDistVal = Lidar_Rear / 10;

OffsetDistPID.Compute();

speed = (int)OffsetDistOutput;

uint16_t rear_var = CalcRearDistArrayVariance((unsigned long)Lidar_Rear);

avg = CalcRearDistAvgMM(Lidar_Rear, Rear_Dist_PrevMean);

mySerial.printf("%lu\t%2.2f\t%2.2f\t%d\t%d\t%d\n", millis(), OffsetDistVal, OffsetDistOutput, speed, rear_var, avg);

if (speed >= 0)

{

RunBothMotors(true, speed, speed);

}

else

{

RunBothMotors(false, -speed, -speed);

}

mySerial.printf("Stopped at %2.2f cm with avg = %d\n", (float)Lidar_Rear / 10, avg);

StopBothMotors();

Using this code, I got the following output:

And a short video showing the offset acquisition process:

Here’s the same process, but starting from farther away than desired

21 April 21 Update:

After some additional work on the initial approach algorithm, I arrived at a pretty nice spot; Wall-E2 will reliably detect the IR homing beacon, offset the proper amount from the wall using a 90º turn and a PID engine-driven rear-distance controlled movement, and then rotate to orient to the IR beam boresight. The ‘rotate-to-boresight’ operation takes place in two stages. In the first stage, the robot turns toward the beacon in 10deg steps until the beacon is re-acquired (this is necessary because the robot loses the beam signal when it turns 90º to the wall) and then uses another PID-driven algorithm to center up on the beam boresight. Here’s a short video showing the process.

IR Homing with Initial Approach Phase added. 2-sec pauses are inserted to delineate sub-phases

As can be seen from the above video, the robot successfully navigates to the initial approach point (IAP), rotates to orient with the homing beacon boresight, and then homes to the charging station. This all works, but it is pretty clunky and inelegant. The initial 90º turn away from the wall is in itself a bit problematic, as it can easily overshoot, and then the robot loses the beacon signal, which means that after the appropriate wall offset has been reached, the robot has to turn back toward the charging station to re-acquire the signal, and it has to do so ‘gently’ so as not to overshoot.

I think it would be much better if the initial turn away from the wall was just 45º so the robot won’t lose the beacon signal while navigating to the IAP, and potentially eliminating the first part of the ‘rotate to boresight’ phase. Here is the relevant geometry:

Charging station initial approach and homing geometry

In the above figure, the robot currently makes a 90º utilizes the line labelled ‘Offset =…’ to offset out to the IR beam boresight. I’m thinking that the line labelled ‘x = …’ would work better, as the robot only has to make a 45º turn initially, and then the robot might not lose the beam signal as it offsets out to the IR boresight line. Here’s the supporting math.

In the above figure, an example is worked out for d = 120cm, where the perpendicular offset is found to be 34.4cm and the 45º turn distance is found to be 1.09*Offset = 37.8cm.

23 April 2021 Update:

The change from 90º to 45º IAP approach angle turned out to be pretty easy to do – really just a matter of changing ‘SpinTurn(90)’ ‘SpinTurn(45)’ and the offset value to 1.09 x offset. Here’s a short video showing the result.

After a few more runs (with some failures due to the robot hanging up on the outside rail), I realized my basic beam geometry estimate was significantly off. Instead of a beam angle of about 16deg, it was more like 11, yielding a distance::offset ratio of about 0.18 instead of 0.27. Revising the program to use the more accurate ratio resulted in the following much nicer homing run.

Homing run using a distance::offset ratio of 0.18 vs 0.27

And here is the telemetry from the run:

In RotateToParallelOrientation(Right)
ToFArrayPID Parameters (Kp,Ki,Kd,DIR) = (200.00,180.00,240.00,0)
ToFArrayPID output max/min = 127/-127
Time	Fdist	Cdist	Rdist	Fdist	Steer	Cur*Pr	PIDout	Lspd	RSpd
8785	258	252	286	116	-0.28	0.07	127.00	0	254
8881	260	256	281	128	-0.21	0.06	-114.82	241	12
8979	255	251	276	158	-0.21	0.04	57.14	69	184
9073	255	245	282	151	-0.27	0.06	127.00	0	254
9169	255	245	282	211	-0.27	0.07	79.22	47	206
9268	242	228	259	218	-0.17	0.05	-127.00	254	0
9361	230	202	231	215	-0.17	0.03	65.70	61	192
Parallel Orientation Achieved at 9364 with SteeringVal = -0.01 & bIRBeamAvail = TRUE
OK we have a IR Beacon, with front distance = 215!
In IRHomingNavigateToIAP with LF/LR/LS/RF/RR/RS = 8190	679	75.11	243	232	0.11
IP is at 32 cm offset, 180cm away from charging station, a ratio of 0.18
Robot is at 196 from charger, so we need a 35 cm offset
Current wall offset (cm) is 21.40 or 61% of required offset
offset is too small or too large - turn away from wall to adjust
After SpinTurn(), rear dist (cm) is 33
Msec	Dist	diff	speed	var	avg
10999	26.00	47.70	47	27841	491
11097	22.00	71.60	71	7108	261
11194	22.00	75.50	75	5802	234
...
13015	39.00	-8.60	-8	5622	396
13112	39.00	-9.80	-9	5621	396
13207	40.00	-16.30	-16	5632	399
13305	39.00	-12.50	-12	5642	397
13402	39.00	-13.70	-13	5642	396
13497	40.00	-20.20	-20	5632	397
13591	40.00	-21.70	-21	5622	400
13685	39.00	-17.90	-17	5622	399
...
15313	38.00	-33.90	-33	5631	390
15411	39.00	-40.10	-40	5631	391
15506	38.00	-36.00	-36	5632	391
15602	38.00	-36.90	-36	5629	390
15697	39.00	-43.10	-43	5632	392
15794	38.00	-39.00	-39	5635	391
15889	40.00	-50.50	-50	5644	394
15985	40.00	-52.00	-52	5653	398
16078	39.00	-48.20	-48	5628	398
16180	39.00	-49.40	-49	5630	396
16276	38.00	-45.30	-45	5626	392
16371	38.00	-46.20	-46	5626	389
16468	37.00	-41.80	-41	5670	381
16564	36.00	-37.10	-37	5681	372
16659	36.00	-37.40	-37	5633	365
16757	35.00	-32.40	-32	5642	359
Stopped at 35.40 cm with avg = 359
Wall offset (cm) now 35, with IRHomingValTotalAvg = 24714
Now rotating to achieve avg value > 15000
Msec	ValAvg
TurnToHomingBeacon with Steering = 0.99 & IRRotate Kp,Ki,Kd = (75,68,90)
TurnToHomingBeacon(RIGHT)
Msec	Signal	Steer	Val
16855	25031	0.99	77.42
16950	25309	0.99	85.94
17049	25742	0.98	87.80
17143	26667	0.97	93.47
17241	28398	0.94	70.95
17339	30002	0.90	68.24
17436	31002	0.88	94.82
17532	31554	0.84	83.08
17629	32456	0.81	88.08
17723	34123	0.75	66.22
17821	36159	0.66	32.28
17920	37863	0.55	7.43
18015	38913	0.51	79.82
18110	39181	0.51	115.91
18204	39323	0.50	104.22
18301	39952	0.36	-13.31
18400	41434	0.11	-127.00
18494	42963	0.04	22.60
TurnToHomingBeacon exited with Steering = 0.04
Should be close to beacon centerline val avg = 42963, SteeringVal = 0.041
IRHomeToChgStn with detection value = 42963, Steering = 0.04 & IRHOMING Kp,Ki,Kd = (100,90,120)
front dist = 192, lastHomingTelemetryMsec = 12
Time	BattV	Fin1	Fin2	Steer	PID_Out		LSpd	RSpd	FrontD	RearD
18785	7.71	25482	17878	0.18	-118.68		8	245	186	1186
18976	7.70	14622	31040	-0.36	-127.00		0	254	164	1020
19171	7.69	30166	21000	0.18	127.00		254	0	174	8190
19360	7.77	31456	26846	0.08	-127.00		0	254	156	8190
19552	7.75	26844	39502	-0.19	-27.31		99	154	144	8190
19748	7.75	44104	36840	0.09	127.00		254	0	140	8190
19941	7.73	39250	54988	-0.17	-127.00		0	254	129	8190
20137	7.71	54326	60540	-0.05	127.00		254	0	115	8190
20331	7.75	74378	66494	0.06	-36.61		90	163	99	8190
20525	7.74	72640	101500	-0.17	-92.89		34	219	85	8190
20719	7.70	127128	101718	0.11	127.00		254	0	82	8190
20914	7.85	150490	157470	-0.02	-127.00		0	254	70	8190
21106	7.78	204068	227704	-0.05	64.30		191	62	66	8190
21401	7.74	430668	395454	0.04	-127.00		0	254	36	8191
21597	7.74	687992	776442	-0.06	43.76		170	83	23	8191
21795	7.74	1570870	1145658	0.16	53.77		180	73	15	8191
Accelerating to Contact with frontdist = 15
Accelerating to Contact with frontdist = 4
21993	7.75	2447090	2476002	-0.01	-7.92		119	134	5	-1
Accelerating to Contact with frontdist = 5
Accelerating to Contact with frontdist = 3
22194	7.60	2469022	2490074	-0.00	2.01		129	124	4	-1
Accelerating to Contact with frontdist = 4
Accelerating to Contact with frontdist = 4
22394	7.72	2461754	2490566	-0.01	-0.65		126	127	5	-1
Accelerating to Contact with frontdist = 5
Charger Connected at 22398

100

101

102

103

104

105

106

In RotateToParallelOrientation(Right)

ToFArrayPID Parameters (Kp,Ki,Kd,DIR) = (200.00,180.00,240.00,0)

ToFArrayPID output max/min = 127/-127

Time Fdist Cdist Rdist Fdist Steer Cur*Pr PIDout Lspd RSpd

8785 258 252 286 116 -0.28 0.07 127.00 0 254

8881 260 256 281 128 -0.21 0.06 -114.82 241 12

8979 255 251 276 158 -0.21 0.04 57.14 69 184

9073 255 245 282 151 -0.27 0.06 127.00 0 254

9169 255 245 282 211 -0.27 0.07 79.22 47 206

9268 242 228 259 218 -0.17 0.05 -127.00 254 0

9361 230 202 231 215 -0.17 0.03 65.70 61 192

Parallel Orientation Achieved at 9364 with SteeringVal = -0.01 & bIRBeamAvail = TRUE

OK we have a IR Beacon, with front distance = 215!

In IRHomingNavigateToIAP with LF/LR/LS/RF/RR/RS = 8190 679 75.11 243 232 0.11

IP is at 32 cm offset, 180cm away from charging station, a ratio of 0.18

Robot is at 196 from charger, so we need a 35 cm offset

Current wall offset (cm) is 21.40 or 61% of required offset

offset is too small or too large - turn away from wall to adjust

After SpinTurn(), rear dist (cm) is 33

Msec Dist diff speed var avg

10999 26.00 47.70 47 27841 491

11097 22.00 71.60 71 7108 261

11194 22.00 75.50 75 5802 234

...

13015 39.00 -8.60 -8 5622 396

13112 39.00 -9.80 -9 5621 396

13207 40.00 -16.30 -16 5632 399

13305 39.00 -12.50 -12 5642 397

13402 39.00 -13.70 -13 5642 396

13497 40.00 -20.20 -20 5632 397

13591 40.00 -21.70 -21 5622 400

13685 39.00 -17.90 -17 5622 399

...

15313 38.00 -33.90 -33 5631 390

15411 39.00 -40.10 -40 5631 391

15506 38.00 -36.00 -36 5632 391

15602 38.00 -36.90 -36 5629 390

15697 39.00 -43.10 -43 5632 392

15794 38.00 -39.00 -39 5635 391

15889 40.00 -50.50 -50 5644 394

15985 40.00 -52.00 -52 5653 398

16078 39.00 -48.20 -48 5628 398

16180 39.00 -49.40 -49 5630 396

16276 38.00 -45.30 -45 5626 392

16371 38.00 -46.20 -46 5626 389

16468 37.00 -41.80 -41 5670 381

16564 36.00 -37.10 -37 5681 372

16659 36.00 -37.40 -37 5633 365

16757 35.00 -32.40 -32 5642 359

Stopped at 35.40 cm with avg = 359

Wall offset (cm) now 35, with IRHomingValTotalAvg = 24714

Now rotating to achieve avg value > 15000

Msec ValAvg

TurnToHomingBeacon with Steering = 0.99 & IRRotate Kp,Ki,Kd = (75,68,90)

TurnToHomingBeacon(RIGHT)

Msec Signal Steer Val

16855 25031 0.99 77.42

16950 25309 0.99 85.94

17049 25742 0.98 87.80

17143 26667 0.97 93.47

17241 28398 0.94 70.95

17339 30002 0.90 68.24

17436 31002 0.88 94.82

17532 31554 0.84 83.08

17629 32456 0.81 88.08

17723 34123 0.75 66.22

17821 36159 0.66 32.28

17920 37863 0.55 7.43

18015 38913 0.51 79.82

18110 39181 0.51 115.91

18204 39323 0.50 104.22

18301 39952 0.36 -13.31

18400 41434 0.11 -127.00

18494 42963 0.04 22.60

TurnToHomingBeacon exited with Steering = 0.04

Should be close to beacon centerline val avg = 42963, SteeringVal = 0.041

IRHomeToChgStn with detection value = 42963, Steering = 0.04 & IRHOMING Kp,Ki,Kd = (100,90,120)

front dist = 192, lastHomingTelemetryMsec = 12

Time BattV Fin1 Fin2 Steer PID_Out LSpd RSpd FrontD RearD

18785 7.71 25482 17878 0.18 -118.68 8 245 186 1186

18976 7.70 14622 31040 -0.36 -127.00 0 254 164 1020

19171 7.69 30166 21000 0.18 127.00 254 0 174 8190

19360 7.77 31456 26846 0.08 -127.00 0 254 156 8190

19552 7.75 26844 39502 -0.19 -27.31 99 154 144 8190

19748 7.75 44104 36840 0.09 127.00 254 0 140 8190

19941 7.73 39250 54988 -0.17 -127.00 0 254 129 8190

20137 7.71 54326 60540 -0.05 127.00 254 0 115 8190

20331 7.75 74378 66494 0.06 -36.61 90 163 99 8190

20525 7.74 72640 101500 -0.17 -92.89 34 219 85 8190

20719 7.70 127128 101718 0.11 127.00 254 0 82 8190

20914 7.85 150490 157470 -0.02 -127.00 0 254 70 8190

21106 7.78 204068 227704 -0.05 64.30 191 62 66 8190

21401 7.74 430668 395454 0.04 -127.00 0 254 36 8191

21597 7.74 687992 776442 -0.06 43.76 170 83 23 8191

21795 7.74 1570870 1145658 0.16 53.77 180 73 15 8191

Accelerating to Contact with frontdist = 15

Accelerating to Contact with frontdist = 4

21993 7.75 2447090 2476002 -0.01 -7.92 119 134 5 -1

Accelerating to Contact with frontdist = 5

Accelerating to Contact with frontdist = 3

22194 7.60 2469022 2490074 -0.00 2.01 129 124 4 -1

Accelerating to Contact with frontdist = 4

22394 7.72 2461754 2490566 -0.01 -0.65 126 127 5 -1

Accelerating to Contact with frontdist = 5

Charger Connected at 22398

Much nicer!

Stay tuned!

Frank

Wall-E2 Gets a VL53L0X Array Upgrade

3 Replies

Posted 13 March 2021

Back in May of last year I started the process of replacing the ultrasonic distance sensors on Wall-E2, my autonomous wall-following robot, with two side-looking arrays of STMicroelectronics’ VL53L0X infrared laser ‘Time-of-Flight’ distance sensor. Later that same year I upgraded the installation by adding a rear distance sensor as well, as shown in the following photo:

left-hand VL53L0X array and rear sensor shown. Identical 3-elelment array on the other side

All went swimmingly until I started having problems when Wall-E2 connected up to its charger. Wall-E2 connecting to its charger is a pretty dynamic event, as it has to build up enough speed to make sure the charging probe seats into the charging jack properly. I discovered that about every other time Wall-E2 connected, the distance sensors would start reporting ‘-1’ instead of the actual distances. This meant that when Wall-E2 disconnected from charging, it had no idea what to do or which way to turn. This didn’t happen all the time, but often enough to be very worrisome.

After some head-scratching and program instrumentation I became convinced this was a real issue, and I posted on the ST Micro’s forum about the issue. ST Micro’s VL53L0X expert John E. Kvam answered with this post:

The VL53L0X sensor will not return a -1. The user manual does define a -1 error, but it states that this cannot happen. What does return a -1 is an I2C timeout.
And I think that is what is killing you. The I2C is notorious for being basically unreliable. And the major symptom of a dropped bit (the most likely failure), is that the bus is stuck low. (When nothing is being transmitted both the clock and data lines should be high.) Philips designed the I2C bus and NXP bought Philips. So the NXP site has some documentation on how to tweak the bus. I’d read and understand that.
One other possibility is that one or more of the sensors rebooted – perhaps due to power glitch. If this happens the sensor will revert to it’s base I2C address.
If you’ve changed all your addresses there should be nothing at address 0x29 (0x52/0x53). You could occasionally ping the base address – and if you ever got an answer, you would would know you had a reboot of some kind.

After thinking about this some more, I realized that there is a good chance that the I2C ‘daisy-chain’ wiring and/or the power/ground connections are getting interrupted due to the impulse generated when Wall-E2 hits the charging station stop, and this is causing one or more of the sensors to drop out. I thought I had done a very careful job with the I2C/power ‘daisy-chain’, but I had thought that about a previous effort where I found a faulty ground connection, so I knew it was a possibility.

After thinking about this some more, it occurred to me that I could eliminate most of the point-point wiring issues by creating a PCB to house the VL53L0X modules, with a single 4-pin connector for I2C and power. I had made a PCB some years before using DipTrace that had turned out petty well, so making one for the very simple VL53L0X wiring scheme should be a piece of cake.

Well, as it turned out, designing the PCB was the easy part. However, when it came to the part about having the boards actually manufactured, I was in for a bit of a shock. For my previous board I used DipTrace’s ‘baked-in’ manufacturer Bay Area Circuits to purchase 5 boards for around $30, but when I tried this same trick with my new board, the minimum charge for boards from BAC was $150 – ouch!

After a lot of web searching and research, I eventually found the Chinese company JLCPCB and discovered they have a very nice document that shows how to export the required files from DipTrace and upload them to their site. Took me about 30 minutes to go through the process the first time, and now I have my 5ea boards ordered for a grand total of about $15USD. Only a 10:1 ratio from BAC to JLCPCB. I can’t imagine how BAC can stay in business, and I can’t imagine why DipTrace has that company ‘baked in’ and not JLCPCB. DipTrace must be getting a heck of a kickback from BAC!

Less than 10 days later, I had the finished PCBs in my hand – wow! Here’s a photo showing two PCB’s installed on Wall-E2. I installed 4-pin headers on both ends of the near PCB in order to daisy-chain the I2C and power connections to the rear distance sensor (hidden behind the red ‘rear bumper’ block in the right background).

New VL53L0X array PCB to replace all the hand-wired I2C/Power connections.

Comparing the ‘before’ and ‘after’ photos, it is easy to see that the PCB installation eliminates two 4-pin connectors and a three-loop daisy chain on one side of the robot, and two 4-pin connectors and a four-loop daisy chain on the other side (the one that also connects to the rear distance sensor. Moreover, now none of the 3 connectors used for all seven sensors has more than one wire per pin.

Hopefully this upgrade will eliminate (or at least significantly suppress) wiring and/or connector issues associated with the charger-connect ‘impulse’ – we’ll see

25 March 2021 Update:

After getting the new PCBs installed and some other connection issues solved, Wall-E2’s sensors seem to be a lot more reliable. However, I decided to take this opportunity to study John Kvam’s idea of re-initializing all seven sensors if one of them happens to show up at the default VL53L0X I2C address.

So, I modified the Teensy 3.5 VL53L0X array management code to abstract the sensor init code from setup() to its own function so it can be called if the program detects a sensor at the default I2C address, and then added code in loop() to do just that. Here’s the detection code:

  //03/25/21 see if any VL53L0X responds to a query at the default address
  Wire1.beginTransmission(DEFAULT_VL53L0X_ADDR);
  if (!Wire1.endTransmission() != 0)
  {
    Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire1!\n", DEFAULT_VL53L0X_ADDR);
    InitAllSensors();
  }
  Wire2.beginTransmission(DEFAULT_VL53L0X_ADDR);
  if (!Wire2.endTransmission() != 0)
  {
    Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire2!\n", DEFAULT_VL53L0X_ADDR);
    InitAllSensors();
  }

//03/25/21 see if any VL53L0X responds to a query at the default address

Wire1.beginTransmission(DEFAULT_VL53L0X_ADDR);

if (!Wire1.endTransmission() != 0)

{

Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire1!\n", DEFAULT_VL53L0X_ADDR);

InitAllSensors();

}

Wire2.beginTransmission(DEFAULT_VL53L0X_ADDR);

if (!Wire2.endTransmission() != 0)

{

Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire2!\n", DEFAULT_VL53L0X_ADDR);

InitAllSensors();

}

and here’s the new ‘InitAllSensors()’ routine:

bool InitAllSensors()
{
  //Purpose: Initialize all seven VL53L0X sensors
  //Provenance: 03/22/21

  Serial.printf("Initializing VL53L0X Sensor Array Elements\n");
 
  //Put all sensors in reset mode by pulling XSHUT low
  digitalWrite(RF_XSHUT_PIN, LOW);
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

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

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


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

  bool retval = true;

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

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


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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  return retval;
}

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

bool InitAllSensors()

{

//Purpose: Initialize all seven VL53L0X sensors

//Provenance: 03/22/21

Serial.printf("Initializing VL53L0X Sensor Array Elements\n");

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

digitalWrite(RF_XSHUT_PIN, LOW);

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

//added 10/23/20 for rear sensor

pinMode(Rear_XSHUT_PIN, OUTPUT);

digitalWrite(Rear_XSHUT_PIN, LOW);

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

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

pinMode(RF_XSHUT_PIN, INPUT);

delay(10);

bool retval = true;

if (!lidar_RF.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RF.setMeasurementTimingBudget(20000);

lidar_RF.startContinuous();

lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);

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

delay(100);

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

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

pinMode(RC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RC.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RC.setMeasurementTimingBudget(20000);

lidar_RC.startContinuous();

lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);

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

delay(100);

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

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

pinMode(RR_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RR.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RR.setMeasurementTimingBudget(20000);

lidar_RR.startContinuous();

lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);

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

delay(100);

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

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

pinMode(LF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LF.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LF.setMeasurementTimingBudget(20000);

lidar_LF.startContinuous();

lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);

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

delay(100);

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

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

pinMode(LC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LC.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LC.setMeasurementTimingBudget(20000);

lidar_LC.startContinuous();

lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);

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

delay(100);

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

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

pinMode(LR_XSHUT_PIN, INPUT);

if (!lidar_LR.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LR.setMeasurementTimingBudget(20000);

lidar_LR.startContinuous();

lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);

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

delay(100);

//added 10/23/20 for rear sensor

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

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

pinMode(Rear_XSHUT_PIN, INPUT);

if (!lidar_Rear.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_Rear.setMeasurementTimingBudget(20000);

lidar_Rear.startContinuous();

lidar_Rear.setAddress(DEFAULT_VL53L0X_ADDR + 7);

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

return retval;

}

And here is the complete Teensy program for managing the VL53L0X arrays:

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

    This is the 'final' version of the Teensy 3.5 project to manage all 
    seven VL53L0X lidar distance sensors, three on left, three on right, 
    and now including a rear distance sensor.  As of this date it was an
    identical copy of the TeensyHexVL53L0XHighSpeedDemo project, just
    renamed for clarity.  This program was uploaded to the Teensy on 
    Wall-E2's second deck on 1/24/21

    Notes:
      02/01/21 Added code in requestEvent() to blink LED_BUILTIN when distance data is requested
      03/22/21 Abstracted sensor init code from setup() to InitAllSensors() so can use elsewhere

*/

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

volatile uint8_t request_type;

const int SETUP_DURATION_OUTPUT_PIN = 33;
const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;
bool bTeensyReady = false;


enum VL53L0X_REQUEST
{
  VL53L0X_READYCHECK, //added 11/10/20 to prevent bad reads during Teensy setup()
  VL53L0X_CENTERS_ONLY,
  VL53L0X_RIGHT,
  VL53L0X_LEFT,
  VL53L0X_ALL, //added 08/05/20, chg 'BOTH' to 'ALL' 10/31/20
  VL53L0X_REAR_ONLY //added 10/24/20
};


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

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

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

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

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

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

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


float RightSteeringVal;
float LeftSteeringVal;

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

//10/23/20 added rear distance sensor
VL53L0X lidar_Rear(&Wire2); //added to Wire2 daisy-chain
const int Rear_XSHUT_PIN = 8;
uint16_t Rear_Dist_mm;

void setup()
{
  // wait until serial port opens ... For 5 seconds max
  Serial.begin(115200);
  delay(5000);

  pinMode(LED_BUILTIN, OUTPUT);
  bTeensyReady = false; //11/10/20 added to prevent bad data reads by main controller

  pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);
  pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);
  digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);

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

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


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

  //03/22/21 sensor init abstracted to function so can call from loop()
  bool bSensorsOK = InitAllSensors();
  Serial.printf("Just after InitAllSensors()\n");

  if (!bSensorsOK)
  {
    Serial.printf("Sensor initialization failed - quitting!\n");
    while (true)
    {

    }
  }

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

  //02/01/21 now using LED_BUILTIN to provide visible indication of I2C data requests
  pinMode(LED_BUILTIN, OUTPUT);

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

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

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

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

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

  //added 10/23/20 for rear sensor
  Rear_Dist_mm = lidar_Rear.readRangeContinuousMillimeters();


  Serial.printf("%lu\t%d\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, Rear_Dist_mm);

  delay(100);

  //03/25/21 see if any VL53L0X responds to a query at the default address
  Wire1.beginTransmission(DEFAULT_VL53L0X_ADDR);
  if (!Wire1.endTransmission() != 0)
  {
    Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire1!\n", DEFAULT_VL53L0X_ADDR);
    InitAllSensors();
  }
  Wire2.beginTransmission(DEFAULT_VL53L0X_ADDR);
  if (!Wire2.endTransmission() != 0)
  {
    Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire2!\n", DEFAULT_VL53L0X_ADDR);
    InitAllSensors();
  }

}

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


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

  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);
  digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));

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

  case VL53L0X_CENTERS_ONLY:

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

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

  case VL53L0X_RIGHT:

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

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

  case VL53L0X_LEFT:

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

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

  case VL53L0X_ALL:
    //added 08/05/20 to get data from both sides at once
    //10/31/20 chg to VL53L0X_ALL & report all 7 sensor values

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

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

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


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

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

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

    //10/31/20 now also reporting rear sensor value
    I2C_writeAnything(Rear_Dist_mm);

    break;

  case VL53L0X_REAR_ONLY:

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

    I2C_writeAnything(Rear_Dist_mm);
    break;

  default:
    break;
  }

  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);
}

//
// 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!!
}
bool InitAllSensors()
{
  //Purpose: Initialize all seven VL53L0X sensors
  //Provenance: 03/22/21

  Serial.printf("Initializing VL53L0X Sensor Array Elements\n");
 
  //Put all sensors in reset mode by pulling XSHUT low
  digitalWrite(RF_XSHUT_PIN, LOW);
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

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

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


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

  bool retval = true;

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

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


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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  delay(100);

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

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

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

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

  return retval;
}

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

Name: Teensy_7VL53L0X_I2C_Slave_V3.ino

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

Author: FRANKNEWXPS15\Frank

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

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

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

identical copy of the TeensyHexVL53L0XHighSpeedDemo project, just

renamed for clarity. This program was uploaded to the Teensy on

Wall-E2's second deck on 1/24/21

Notes:

02/01/21 Added code in requestEvent() to blink LED_BUILTIN when distance data is requested

03/22/21 Abstracted sensor init code from setup() to InitAllSensors() so can use elsewhere

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

#include <VL53L0X.h>

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

volatile uint8_t request_type;

const int SETUP_DURATION_OUTPUT_PIN = 33;

const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;

bool bTeensyReady = false;

enum VL53L0X_REQUEST

{

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

VL53L0X_CENTERS_ONLY,

VL53L0X_RIGHT,

VL53L0X_LEFT,

VL53L0X_ALL, //added 08/05/20, chg 'BOTH' to 'ALL' 10/31/20

VL53L0X_REAR_ONLY //added 10/24/20

};

//right side array

VL53L0X lidar_RF(&Wire1);

VL53L0X lidar_RC(&Wire1);

VL53L0X lidar_RR(&Wire1);

//left side array

VL53L0X lidar_LF(&Wire2);

VL53L0X lidar_LC(&Wire2);

VL53L0X lidar_LR(&Wire2);

//XSHUT required for I2C address init

//right side array

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

//left side array

const int LF_XSHUT_PIN = 5;

const int LC_XSHUT_PIN = 6;

const int LR_XSHUT_PIN = 7;

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

//right side array

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

//left side array

uint16_t LF_Dist_mm;

uint16_t LC_Dist_mm;

uint16_t LR_Dist_mm;

float RightSteeringVal;

float LeftSteeringVal;

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

const int DEFAULT_VL53L0X_ADDR = 0x29;

//10/23/20 added rear distance sensor

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

const int Rear_XSHUT_PIN = 8;

uint16_t Rear_Dist_mm;

void setup()

{

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

Serial.begin(115200);

delay(5000);

pinMode(LED_BUILTIN, OUTPUT);

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

pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);

pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);

digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);

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

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

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

Wire1.begin();

Wire2.begin();

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

//03/22/21 sensor init abstracted to function so can call from loop()

bool bSensorsOK = InitAllSensors();

Serial.printf("Just after InitAllSensors()\n");

if (!bSensorsOK)

{

Serial.printf("Sensor initialization failed - quitting!\n");

while (true)

{

}

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

//02/01/21 now using LED_BUILTIN to provide visible indication of I2C data requests

pinMode(LED_BUILTIN, OUTPUT);

digitalWrite(SETUP_DURATION_OUTPUT_PIN, LOW);

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

}

void loop()

{

//from Pololu forum post

RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();

RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();

RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();

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

//from Pololu forum post

LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();

LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();

LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();

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

//added 10/23/20 for rear sensor

Rear_Dist_mm = lidar_Rear.readRangeContinuousMillimeters();

Serial.printf("%lu\t%d\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, Rear_Dist_mm);

delay(100);

//03/25/21 see if any VL53L0X responds to a query at the default address

Wire1.beginTransmission(DEFAULT_VL53L0X_ADDR);

if (!Wire1.endTransmission() != 0)

{

Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire1!\n", DEFAULT_VL53L0X_ADDR);

InitAllSensors();

}

Wire2.beginTransmission(DEFAULT_VL53L0X_ADDR);

if (!Wire2.endTransmission() != 0)

{

Serial.printf("Ooops! found a VL53L0X sensor at %x on Wire2!\n", DEFAULT_VL53L0X_ADDR);

InitAllSensors();

}

// function that executes whenever data is requested by master

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

void requestEvent()

{

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

//Inputs:

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

// 0 = left & right center distances only

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

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

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

//Outputs:

// Requested data sent to master

//Notes:

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

// 10/24/20 added VL53L0X_REAR_ONLY request type

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

// 02/01/21 aded LED_PIN blink

//DEBUG!!

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

// request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//DEBUG!!

digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);

digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));

switch (request_type)

{

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

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

I2C_writeAnything(bTeensyReady);

break;

case VL53L0X_CENTERS_ONLY:

//DEBUG!!

//data_size = 2*sizeof(uint16_t);

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

//DEBUG!!

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

break;

case VL53L0X_RIGHT:

//DEBUG!!

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

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

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

//DEBUG!!

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(RightSteeringVal);

break;

case VL53L0X_LEFT:

//DEBUG!!

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

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

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

//DEBUG!!

I2C_writeAnything(LF_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

I2C_writeAnything(LR_Dist_mm);

I2C_writeAnything(LeftSteeringVal);

break;

case VL53L0X_ALL:

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

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

//DEBUG!!

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

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

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

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

// LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);

//DEBUG!!

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

//left side

I2C_writeAnything(LF_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

I2C_writeAnything(LR_Dist_mm);

I2C_writeAnything(LeftSteeringVal);

//DEBUG!!

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

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

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

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

//RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//DEBUG!!

//right side

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(RightSteeringVal);

//10/31/20 now also reporting rear sensor value

I2C_writeAnything(Rear_Dist_mm);

break;

case VL53L0X_REAR_ONLY:

//DEBUG!!

//data_size = sizeof(uint16_t);

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

//DEBUG!!

I2C_writeAnything(Rear_Dist_mm);

break;

default:

break;

}

digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);

}

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

}

bool InitAllSensors()

{

//Purpose: Initialize all seven VL53L0X sensors

//Provenance: 03/22/21

Serial.printf("Initializing VL53L0X Sensor Array Elements\n");

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

digitalWrite(RF_XSHUT_PIN, LOW);

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

//added 10/23/20 for rear sensor

pinMode(Rear_XSHUT_PIN, OUTPUT);

digitalWrite(Rear_XSHUT_PIN, LOW);

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

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

pinMode(RF_XSHUT_PIN, INPUT);

delay(10);

bool retval = true;

if (!lidar_RF.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RF.setMeasurementTimingBudget(20000);

lidar_RF.startContinuous();

lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);

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

delay(100);

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

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

pinMode(RC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RC.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RC.setMeasurementTimingBudget(20000);

lidar_RC.startContinuous();

lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);

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

delay(100);

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

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

pinMode(RR_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RR.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_RR.setMeasurementTimingBudget(20000);

lidar_RR.startContinuous();

lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);

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

delay(100);

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

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

pinMode(LF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LF.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LF.setMeasurementTimingBudget(20000);

lidar_LF.startContinuous();

lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);

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

delay(100);

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

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

pinMode(LC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LC.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LC.setMeasurementTimingBudget(20000);

lidar_LC.startContinuous();

lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);

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

delay(100);

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

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

pinMode(LR_XSHUT_PIN, INPUT);

if (!lidar_LR.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_LR.setMeasurementTimingBudget(20000);

lidar_LR.startContinuous();

lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);

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

delay(100);

//added 10/23/20 for rear sensor

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

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

pinMode(Rear_XSHUT_PIN, INPUT);

if (!lidar_Rear.init())

{

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

//while (1) {}

retval = false;

}

//from Pololu forum post

lidar_Rear.setMeasurementTimingBudget(20000);

lidar_Rear.startContinuous();

lidar_Rear.setAddress(DEFAULT_VL53L0X_ADDR + 7);

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

return retval;

}

After getting everything set up, I started experimenting. The first thing I tried was momentarily disconnecting one or the other of the two I2C bus connections at the Teensy end. This was interesting in that the affected sensor array results became invalid as soon as the I2C connection was pulled, but resumed valid output as soon as the connection was restored – nice!

Next, I tried momentarily disconnecting the +V line to the arrays. All sensor outputs immediately became invalid, and stayed invalid after +V was restored. In addition, the code in loop() designed to detect one or more VL53L0X sensors at the default address immediately triggered – nice – but the re-initialization code failed to restore valid output.

Here’s the output from a run were the +V line was momentarily disconnected and then reconnected:

Opening port
Port open
Initializing VL53L0X Sensor Array Elements
lidar_RF address is 0x2a
lidar_RC address is 0x2b
lidar_RR address is 0x2c
lidar_LF address is 0x2d
lidar_LC address is 0x2e
lidar_LR address is 0x2f
lidar_Rear address is 0x30
Just after InitAllSensors()
Msec	LFront	LCtr	LRear	RFront	RCtr	RRear	LSteer	RSteer	Rear
6448	373	350	363	544	8191	8190	996
6556	369	344	353	486	8191	8191	1006
6665	371	353	368	475	8191	8190	988
6773	379	352	365	510	512	8190	991
6881	377	346	363	489	8191	8191	1009
6990	368	353	365	541	8191	8191	997
7098	375	350	372	487	8191	8191	989
7206	356	350	366	497	535	8190	983
7315	360	358	364	483	543	8190	997
7423	370	353	363	502	633	8190	1001
7531	373	335	350	465	8191	8190	982
7639	374	349	352	483	497	8191	1017
7748	356	350	381	503	8191	8190	1000
7856	362	350	364	489	585	8191	1013
7964	359	346	380	477	557	8191	979
8073	357	350	361	482	8191	8190	1019
8181	357	347	367	463	8191	8191	1011
8289	374	349	359	459	8191	8190	996
8397	363	348	371	494	8191	8191	1030
8506	365	351	370	557	509	8191	1000
8614	367	345	368	480	445	8191	974
8722	364	354	365	500	8191	8190	987
8831	384	349	356	524	470	8190	1019
8939	376	348	363	539	534	8191	1007
9047	354	349	371	522	475	8191	1020
9156	369	353	362	758	8191	8190	1003
9264	361	347	380	489	8191	8190	981
9372	364	350	367	463	8191	8190	998
9480	366	350	375	505	498	8190	985
9589	382	346	364	445	8191	8190	1004
9697	365	346	373	484	492	8190	996
9805	375	349	353	493	616	8190	1004
9914	371	349	356	538	492	8191	1001
10022	379	346	366	496	503	8191	1016
10130	365	349	354	446	428	8190	1025
10239	381	347	366	474	8191	8191	1008
10347	372	351	362	493	647	8191	1006
10455	366	344	358	487	8191	8191	987
10563	379	346	367	487	566	8191	979
10672	370	348	366	496	8191	8191	975
10780	375	356	360	510	601	8191	1000
10888	370	343	370	497	550	8191	1006
10997	371	352	360	488	8191	8190	982
11105	369	351	353	501	8191	8191	1019
11213	370	345	363	483	8191	8190	1020
11321	372	349	361	503	561	8190	1023
11430	369	351	379	505	453	8191	976
11538	373	347	366	495	8191	8191	988
11646	364	351	372	488	8191	8191	989
11755	365	339	361	470	544	8191	1011
11863	374	358	369	483	8191	8191	1008
11971	367	343	370	490	499	8190	1013
12080	364	348	373	475	491	8191	1006
12188	367	344	363	495	8191	8190	1025
12296	376	349	362	514	580	8191	1035
12404	366	351	365	474	8191	8190	1021
12513	364	350	372	489	8191	8191	1004
12621	373	348	357	494	8191	8190	998
12729	371	352	364	523	8191	8191	1015
12838	365	355	372	500	8191	8190	987
12946	364	357	374	497	8191	8190	998
13054	365	347	364	520	436	8190	990
13163	364	351	381	535	8191	8191	1028
13271	366	345	375	499	8191	8191	1011
13379	358	350	356	482	705	8190	1008
13487	366	350	365	532	590	8191	1008
13596	370	349	355	491	563	8190	1021
13704	364	348	368	517	480	8190	1018
13812	358	348	375	481	8191	8191	1014
13921	363	346	359	503	8191	8191	1004
14029	371	353	363	458	8191	8191	983
14137	376	357	357	565	641	8191	999
14246	360	348	368	548	535	8191	1021
14354	364	347	367	494	601	8191	1002
14462	364	352	372	470	550	8191	991
14570	366	349	366	532	536	8191	1003
14679	374	348	364	493	520	8191	1000
14787	364	345	363	471	8191	8191	1000
14895	364	341	371	503	8191	8190	994
15004	360	349	366	474	605	8191	1030
15112	375	342	356	464	548	8190	986
15220	367	351	364	471	524	8190	1022
15328	365	349	372	491	656	8191	1001
15437	364	347	365	472	8191	8191	993
15545	366	348	365	520	8191	8191	980
15653	374	348	361	527	591	8191	1010
15762	363	352	361	500	609	8190	1002
15870	370	352	370	531	8191	8191	1004
15978	367	353	365	555	8191	8191	987
16087	366	348	365	501	8191	8191	992
16195	366	347	362	481	558	8190	1013
16303	368	352	364	485	8191	8190	1018
16411	372	340	360	459	8003	8191	993
16520	366	357	369	503	561	8191	1009
16628	365	345	364	416	497	8191	986
16736	363	352	362	542	8191	8191	996
16845	373	356	365	455	8191	8190	975
16953	354	342	364	488	551	8191	971
17061	368	349	364	495	8191	8191	983
17170	371	350	367	451	421	8191	1019
17278	367	345	362	465	8191	8190	977
17386	369	351	367	517	604	8190	990
17494	371	342	363	513	8191	8190	999
17603	368	350	365	498	8191	8191	986
17711	362	350	371	509	600	8190	988
17819	366	351	360	515	8191	8190	1002
17928	378	348	364	478	8191	8190	999
18036	367	346	365	436	8191	8191	1035
18144	378	349	356	515	596	8190	1017
18253	365	347	364	491	8191	8191	1018
18361	370	349	367	478	649	8191	1024
18469	370	348	366	443	8191	8190	1002
18577	374	345	366	471	8191	8190	1009
18686	370	355	372	499	8191	8191	1022
18794	362	353	364	480	8191	8191	1003
18902	368	347	365	507	510	8191	1007
19011	373	357	367	486	8191	8190	1029
19119	369	351	362	498	8191	8190	999
19227	360	347	365	445	8191	8190	995
19336	363	351	371	471	557	8190	985
19444	364	354	368	439	707	8190	985
19552	360	354	371	529	8191	8190	994
19660	358	351	373	496	438	8191	992
19769	363	348	366	457	8191	8190	1000
19877	361	358	368	435	689	8190	1005
19985	364	351	371	500	8191	8191	996
20094	368	351	363	478	8191	8190	1029
20202	365	351	370	521	8191	8191	996
20310	373	345	365	514	8191	8191	1009
20418	368	348	367	526	596	8191	959
20527	369	351	351	534	8191	8191	1011
20635	373	346	361	512	8191	8190	989
20743	367	349	367	470	8191	8190	1023
20852	364	344	354	443	586	8191	1027
20960	369	353	375	481	8191	8191	1010
21068	372	354	369	471	461	8190	999
21177	369	353	364	509	8191	8191	977
21285	372	351	366	516	517	8191	1013
21393	370	355	351	515	673	8190	1018
21501	366	351	366	481	8191	8190	1037
21610	374	353	371	547	478	8191	1006
21718	367	354	369	492	8191	8190	991
21826	368	347	359	454	664	8191	982
21935	362	347	370	530	635	8190	997
22043	368	350	368	523	488	8190	984
22151	364	352	363	539	8191	8191	1032
22260	362	348	355	552	8191	8190	995
22368	375	348	357	528	616	8191	996
22476	371	349	357	491	679	8191	1020
22584	370	352	363	539	517	8190	1007
22693	367	346	360	491	8191	8191	1015
22801	366	348	373	524	8191	8190	1003
22909	381	346	371	498	682	8191	1027
23018	373	345	367	528	587	8190	1024
23126	369	341	364	482	8191	8191	1021
23234	372	344	365	532	8191	8190	983
23343	369	352	363	457	8191	8191	1012
23451	370	345	360	448	590	8191	997
23559	364	348	380	523	570	8190	996
23667	378	346	368	509	492	8191	1002
23776	370	348	375	473	8191	8190	1004
23884	371	344	365	457	8191	8190	1001
23992	368	350	375	469	8191	8190	1027
24101	376	353	361	484	8191	8190	1009
24209	373	348	374	529	8191	8190	1015
24317	375	352	368	559	8191	8190	973
24426	369	354	370	541	8191	8191	1003
24534	361	345	366	510	8191	1143	987
24642	364	350	363	420	857	8191	982
24750	367	345	364	496	8191	8190	1021
24859	362	344	370	482	8191	8191	1010
24967	366	346	367	420	503	8191	1005
25075	361	349	372	383	447	8191	981
25184	369	343	371	393	386	8190	1003
25292	367	355	374	360	395	8191	1016
25400	369	342	367	368	392	408	1008
25508	360	352	375	374	376	431	1020
25617	364	355	371	368	352	369	1022
25725	364	346	368	364	322	328	1016
25833	362	348	376	381	320	284	1000
25942	368	343	362	402	331	279	1030
26050	367	352	361	411	333	264	1011
26158	366	349	371	399	364	279	1008
26267	361	356	378	390	367	291	997
26375	365	350	370	373	369	306	1002
26483	372	349	357	367	353	306	980
26591	369	350	371	355	340	300	1002
26700	362	348	372	339	340	297	998
26808	370	353	372	338	325	295	1015
26916	371	351	368	341	331	287	1011
27025	362	344	370	338	325	287	994
27133	369	350	357	334	319	292	1000
27241	371	347	365	328	303	293	972
27349	373	352	365	318	310	293	1013
27458	359	352	374	321	302	283	999
27566	366	347	360	311	308	277	1004
27674	370	348	368	321	303	285	1024
27783	369	349	365	320	315	282	1036
27891	371	352	364	316	307	279	1006
27999	369	346	361	326	295	282	989
28108	367	350	359	312	301	279	1001
28216	373	345	373	315	299	279	1006
28324	368	362	364	306	302	274	989
28432	367	346	365	320	303	284	1025
28541	370	355	363	313	306	277	1003
28649	378	347	369	317	295	280	997
28757	368	348	364	324	307	274	1011
28866	377	349	362	330	300	285	1007
28974	371	352	366	326	307	291	998
29104	65535	65535	65535	65535	65535	65535	65535
29224	65535	65535	65535	65535	65535	65535	65535
29418	65535	65535	65535	65535	65535	65535	65535
Ooops! found a VL53L0X sensor at 29 on Wire1!
Initializing VL53L0X Sensor Array Elements
Failed to detect and initialize lidar_RF!
lidar_RF address is 0x2a
Failed to detect and initialize lidar_RC!
lidar_RC address is 0x2b
Failed to detect and initialize lidar_RR!
lidar_RR address is 0x2c
Failed to detect and initialize lidar_LF!
lidar_LF address is 0x2d
Failed to detect and initialize lidar_LC!
lidar_LC address is 0x2e
Failed to detect and initialize lidar_LR!
lidar_LR address is 0x2f
Failed to detect and initialize lidar_Rear!
lidar_Rear address is 0x30
Ooops! found a VL53L0X sensor at 29 on Wire2!
30190	65535	65535	65535	65535	65535	65535	65535
Ooops! found a VL53L0X sensor at 29 on Wire1!
Initializing VL53L0X Sensor Array Elements
Failed to detect and initialize lidar_RF!
lidar_RF address is 0x2a
Failed to detect and initialize lidar_RC!
lidar_RC address is 0x2b
Failed to detect and initialize lidar_RR!
lidar_RR address is 0x2c
Failed to detect and initialize lidar_LF!
lidar_LF address is 0x2d
Failed to detect and initialize lidar_LC!
lidar_LC address is 0x2e
Failed to detect and initialize lidar_LR!
lidar_LR address is 0x2f
Failed to detect and initialize lidar_Rear!
lidar_Rear address is 0x30
Ooops! found a VL53L0X sensor at 29 on Wire2!
30962	65535	65535	65535	65535	65535	65535	65535
Ooops! found a VL53L0X sensor at 29 on Wire1!
Initializing VL53L0X Sensor Array Elements
Failed to detect and initialize lidar_RF!
lidar_RF address is 0x2a
Failed to detect and initialize lidar_RC!
lidar_RC address is 0x2b
Failed to detect and initialize lidar_RR!
lidar_RR address is 0x2c
Failed to detect and initialize lidar_LF!
lidar_LF address is 0x2d
Failed to detect and initialize lidar_LC!
lidar_LC address is 0x2e
Failed to detect and initialize lidar_LR!
lidar_LR address is 0x2f
Failed to detect and initialize lidar_Rear!
lidar_Rear address is 0x30

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

Opening port

Port open

Initializing VL53L0X Sensor Array Elements

lidar_RF address is 0x2a

lidar_RC address is 0x2b

lidar_RR address is 0x2c

lidar_LF address is 0x2d

lidar_LC address is 0x2e

lidar_LR address is 0x2f

lidar_Rear address is 0x30

Just after InitAllSensors()

Msec LFront LCtr LRear RFront RCtr RRear LSteer RSteer Rear

6448 373 350 363 544 8191 8190 996

6556 369 344 353 486 8191 8191 1006

6665 371 353 368 475 8191 8190 988

6773 379 352 365 510 512 8190 991

6881 377 346 363 489 8191 8191 1009

6990 368 353 365 541 8191 8191 997

7098 375 350 372 487 8191 8191 989

7206 356 350 366 497 535 8190 983

7315 360 358 364 483 543 8190 997

7423 370 353 363 502 633 8190 1001

7531 373 335 350 465 8191 8190 982

7639 374 349 352 483 497 8191 1017

7748 356 350 381 503 8191 8190 1000

7856 362 350 364 489 585 8191 1013

7964 359 346 380 477 557 8191 979

8073 357 350 361 482 8191 8190 1019

8181 357 347 367 463 8191 8191 1011

8289 374 349 359 459 8191 8190 996

8397 363 348 371 494 8191 8191 1030

8506 365 351 370 557 509 8191 1000

8614 367 345 368 480 445 8191 974

8722 364 354 365 500 8191 8190 987

8831 384 349 356 524 470 8190 1019

8939 376 348 363 539 534 8191 1007

9047 354 349 371 522 475 8191 1020

9156 369 353 362 758 8191 8190 1003

9264 361 347 380 489 8191 8190 981

9372 364 350 367 463 8191 8190 998

9480 366 350 375 505 498 8190 985

9589 382 346 364 445 8191 8190 1004

9697 365 346 373 484 492 8190 996

9805 375 349 353 493 616 8190 1004

9914 371 349 356 538 492 8191 1001

10022 379 346 366 496 503 8191 1016

10130 365 349 354 446 428 8190 1025

10239 381 347 366 474 8191 8191 1008

10347 372 351 362 493 647 8191 1006

10455 366 344 358 487 8191 8191 987

10563 379 346 367 487 566 8191 979

10672 370 348 366 496 8191 8191 975

10780 375 356 360 510 601 8191 1000

10888 370 343 370 497 550 8191 1006

10997 371 352 360 488 8191 8190 982

11105 369 351 353 501 8191 8191 1019

11213 370 345 363 483 8191 8190 1020

11321 372 349 361 503 561 8190 1023

11430 369 351 379 505 453 8191 976

11538 373 347 366 495 8191 8191 988

11646 364 351 372 488 8191 8191 989

11755 365 339 361 470 544 8191 1011

11863 374 358 369 483 8191 8191 1008

11971 367 343 370 490 499 8190 1013

12080 364 348 373 475 491 8191 1006

12188 367 344 363 495 8191 8190 1025

12296 376 349 362 514 580 8191 1035

12404 366 351 365 474 8191 8190 1021

12513 364 350 372 489 8191 8191 1004

12621 373 348 357 494 8191 8190 998

12729 371 352 364 523 8191 8191 1015

12838 365 355 372 500 8191 8190 987

12946 364 357 374 497 8191 8190 998

13054 365 347 364 520 436 8190 990

13163 364 351 381 535 8191 8191 1028

13271 366 345 375 499 8191 8191 1011

13379 358 350 356 482 705 8190 1008

13487 366 350 365 532 590 8191 1008

13596 370 349 355 491 563 8190 1021

13704 364 348 368 517 480 8190 1018

13812 358 348 375 481 8191 8191 1014

13921 363 346 359 503 8191 8191 1004

14029 371 353 363 458 8191 8191 983

14137 376 357 357 565 641 8191 999

14246 360 348 368 548 535 8191 1021

14354 364 347 367 494 601 8191 1002

14462 364 352 372 470 550 8191 991

14570 366 349 366 532 536 8191 1003

14679 374 348 364 493 520 8191 1000

14787 364 345 363 471 8191 8191 1000

14895 364 341 371 503 8191 8190 994

15004 360 349 366 474 605 8191 1030

15112 375 342 356 464 548 8190 986

15220 367 351 364 471 524 8190 1022

15328 365 349 372 491 656 8191 1001

15437 364 347 365 472 8191 8191 993

15545 366 348 365 520 8191 8191 980

15653 374 348 361 527 591 8191 1010

15762 363 352 361 500 609 8190 1002

15870 370 352 370 531 8191 8191 1004

15978 367 353 365 555 8191 8191 987

16087 366 348 365 501 8191 8191 992

16195 366 347 362 481 558 8190 1013

16303 368 352 364 485 8191 8190 1018

16411 372 340 360 459 8003 8191 993

16520 366 357 369 503 561 8191 1009

16628 365 345 364 416 497 8191 986

16736 363 352 362 542 8191 8191 996

16845 373 356 365 455 8191 8190 975

16953 354 342 364 488 551 8191 971

17061 368 349 364 495 8191 8191 983

17170 371 350 367 451 421 8191 1019

17278 367 345 362 465 8191 8190 977

17386 369 351 367 517 604 8190 990

17494 371 342 363 513 8191 8190 999

17603 368 350 365 498 8191 8191 986

17711 362 350 371 509 600 8190 988

17819 366 351 360 515 8191 8190 1002

17928 378 348 364 478 8191 8190 999

18036 367 346 365 436 8191 8191 1035

18144 378 349 356 515 596 8190 1017

18253 365 347 364 491 8191 8191 1018

18361 370 349 367 478 649 8191 1024

18469 370 348 366 443 8191 8190 1002

18577 374 345 366 471 8191 8190 1009

18686 370 355 372 499 8191 8191 1022

18794 362 353 364 480 8191 8191 1003

18902 368 347 365 507 510 8191 1007

19011 373 357 367 486 8191 8190 1029

19119 369 351 362 498 8191 8190 999

19227 360 347 365 445 8191 8190 995

19336 363 351 371 471 557 8190 985

19444 364 354 368 439 707 8190 985

19552 360 354 371 529 8191 8190 994

19660 358 351 373 496 438 8191 992

19769 363 348 366 457 8191 8190 1000

19877 361 358 368 435 689 8190 1005

19985 364 351 371 500 8191 8191 996

20094 368 351 363 478 8191 8190 1029

20202 365 351 370 521 8191 8191 996

20310 373 345 365 514 8191 8191 1009

20418 368 348 367 526 596 8191 959

20527 369 351 351 534 8191 8191 1011

20635 373 346 361 512 8191 8190 989

20743 367 349 367 470 8191 8190 1023

20852 364 344 354 443 586 8191 1027

20960 369 353 375 481 8191 8191 1010

21068 372 354 369 471 461 8190 999

21177 369 353 364 509 8191 8191 977

21285 372 351 366 516 517 8191 1013

21393 370 355 351 515 673 8190 1018

21501 366 351 366 481 8191 8190 1037

21610 374 353 371 547 478 8191 1006

21718 367 354 369 492 8191 8190 991

21826 368 347 359 454 664 8191 982

21935 362 347 370 530 635 8190 997

22043 368 350 368 523 488 8190 984

22151 364 352 363 539 8191 8191 1032

22260 362 348 355 552 8191 8190 995

22368 375 348 357 528 616 8191 996

22476 371 349 357 491 679 8191 1020

22584 370 352 363 539 517 8190 1007

22693 367 346 360 491 8191 8191 1015

22801 366 348 373 524 8191 8190 1003

22909 381 346 371 498 682 8191 1027

23018 373 345 367 528 587 8190 1024

23126 369 341 364 482 8191 8191 1021

23234 372 344 365 532 8191 8190 983

23343 369 352 363 457 8191 8191 1012

23451 370 345 360 448 590 8191 997

23559 364 348 380 523 570 8190 996

23667 378 346 368 509 492 8191 1002

23776 370 348 375 473 8191 8190 1004

23884 371 344 365 457 8191 8190 1001

23992 368 350 375 469 8191 8190 1027

24101 376 353 361 484 8191 8190 1009

24209 373 348 374 529 8191 8190 1015

24317 375 352 368 559 8191 8190 973

24426 369 354 370 541 8191 8191 1003

24534 361 345 366 510 8191 1143 987

24642 364 350 363 420 857 8191 982

24750 367 345 364 496 8191 8190 1021

24859 362 344 370 482 8191 8191 1010

24967 366 346 367 420 503 8191 1005

25075 361 349 372 383 447 8191 981

25184 369 343 371 393 386 8190 1003

25292 367 355 374 360 395 8191 1016

25400 369 342 367 368 392 408 1008

25508 360 352 375 374 376 431 1020

25617 364 355 371 368 352 369 1022

25725 364 346 368 364 322 328 1016

25833 362 348 376 381 320 284 1000

25942 368 343 362 402 331 279 1030

26050 367 352 361 411 333 264 1011

26158 366 349 371 399 364 279 1008

26267 361 356 378 390 367 291 997

26375 365 350 370 373 369 306 1002

26483 372 349 357 367 353 306 980

26591 369 350 371 355 340 300 1002

26700 362 348 372 339 340 297 998

26808 370 353 372 338 325 295 1015

26916 371 351 368 341 331 287 1011

27025 362 344 370 338 325 287 994

27133 369 350 357 334 319 292 1000

27241 371 347 365 328 303 293 972

27349 373 352 365 318 310 293 1013

27458 359 352 374 321 302 283 999

27566 366 347 360 311 308 277 1004

27674 370 348 368 321 303 285 1024

27783 369 349 365 320 315 282 1036

27891 371 352 364 316 307 279 1006

27999 369 346 361 326 295 282 989

28108 367 350 359 312 301 279 1001

28216 373 345 373 315 299 279 1006

28324 368 362 364 306 302 274 989

28432 367 346 365 320 303 284 1025

28541 370 355 363 313 306 277 1003

28649 378 347 369 317 295 280 997

28757 368 348 364 324 307 274 1011

28866 377 349 362 330 300 285 1007

28974 371 352 366 326 307 291 998

29104 65535 65535 65535 65535 65535 65535 65535

29224 65535 65535 65535 65535 65535 65535 65535

29418 65535 65535 65535 65535 65535 65535 65535

Ooops! found a VL53L0X sensor at 29 on Wire1!

Initializing VL53L0X Sensor Array Elements

Failed to detect and initialize lidar_RF!

lidar_RF address is 0x2a

Failed to detect and initialize lidar_RC!

lidar_RC address is 0x2b

Failed to detect and initialize lidar_RR!

lidar_RR address is 0x2c

Failed to detect and initialize lidar_LF!

lidar_LF address is 0x2d

Failed to detect and initialize lidar_LC!

lidar_LC address is 0x2e

Failed to detect and initialize lidar_LR!

lidar_LR address is 0x2f

Failed to detect and initialize lidar_Rear!

lidar_Rear address is 0x30

Ooops! found a VL53L0X sensor at 29 on Wire2!

30190 65535 65535 65535 65535 65535 65535 65535

Ooops! found a VL53L0X sensor at 29 on Wire1!

Initializing VL53L0X Sensor Array Elements

Failed to detect and initialize lidar_RF!

lidar_RF address is 0x2a

Failed to detect and initialize lidar_RC!

lidar_RC address is 0x2b

Failed to detect and initialize lidar_RR!

lidar_RR address is 0x2c

Failed to detect and initialize lidar_LF!

lidar_LF address is 0x2d

Failed to detect and initialize lidar_LC!

lidar_LC address is 0x2e

Failed to detect and initialize lidar_LR!

lidar_LR address is 0x2f

Failed to detect and initialize lidar_Rear!

lidar_Rear address is 0x30

Ooops! found a VL53L0X sensor at 29 on Wire2!

30962 65535 65535 65535 65535 65535 65535 65535

Ooops! found a VL53L0X sensor at 29 on Wire1!

Initializing VL53L0X Sensor Array Elements

Failed to detect and initialize lidar_RF!

lidar_RF address is 0x2a

Failed to detect and initialize lidar_RC!

lidar_RC address is 0x2b

Failed to detect and initialize lidar_RR!

lidar_RR address is 0x2c

Failed to detect and initialize lidar_LF!

lidar_LF address is 0x2d

Failed to detect and initialize lidar_LC!

lidar_LC address is 0x2e

Failed to detect and initialize lidar_LR!

lidar_LR address is 0x2f

Failed to detect and initialize lidar_Rear!

lidar_Rear address is 0x30

28 March 2021 Update:

I have been continuing to work on the ‘re-initialize after power interruption’ idea for my VL53L0X sensor array, and while I haven’t come up with a very good solution, I have learned some things:

The VL53L0X reverts to it’s default (0x29) I2C address immediately when power is removed, but will actually respond to a 0x29 query even after power has been removed. Apparently, the module can draw enough current from the I2C connections to continue to operate (albeit at the default address).
A ‘powered down’ (power disconnected, but I2C leads still connected) will also respond to distance measurement requests, but will return a nonsense number (65535).
In order to get a VL53L0X sensor to stop responding, the power lead must be physically grounded – just disconnecting isn’t enough.
If I use a digital output to power the array, I can turn the power on and off programmatically. Using this tool:
- Turning power OFF causes distance measurements to return ‘65535’ (because this is the same as connecting the power lead to GND). However, this isn’t the normal failure mode; the normal failure mode for power disconnections is an open-circuit – not GND
- Turning power back ON again causes the system to detect that one or more VL53L0X units has appeared at the default I2C address. This probably means I could successfully re-initialize all modules.

So, it looks like I can’t really protect against an arbitrary length power disconnect scenario, as I can’t tell when the power has been restored. All I can do is have the program detect the ‘65535’ distance value, wait for some time (a few seconds?) and then try to re-initialize the sensors. This is, unfortunately, a one-shot deal, as if power hasn’t been restored when the re-init procedure is executed, the initialization code hangs up permanently.

Stay tuned!

Frank

Wall Tracking Trials Using Office ‘Sandbox’ Part II

Leave a reply

Posted 24 January 2021,

Back in November of 2020, I posted about some wall-tracking exercises using my Office ‘sandbox’. Since then I have done some work on the charging station to make it more robust, and on Wall-E2’s ability to home in on and connect to the charging station. The following short video shows Wall-E2 making a complete circuit of the sandbox, ending with a homing run and connection to the charging station.

Wall-E2 makes a complete circuit of the ‘sandbox’, ending up connected to the charging station.

I plan to do quite a bit more work on the charging station homing algorithm, in particular how Wall-E2 reacts when it gets stuck trying to connect (which happens with somewhat disconcerting regularity).

Stay Tuned!

Frank