Tag Archives: robots

Turn Rate PID Tuning, Part III

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

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.

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

Solving the Teensy VL53L0X Array Controller Reset Problem

Leave a reply

Posted 16 November 2020

Back in May of this year, I converted Wall-E2, my autonomous wall-following robot, from using HC-SR04 ultrasonic ‘ping’ sensors to VL53L0X infrared time-of-flight sensors for left & right (and now rear) distance measurement and obstacle detection, as described in this and follow-on posts. Since then, I have been successfully integrating the new sensing capability into Wall-E2’s wall-tracking and obstacle avoidance algorithms, as described in this post among others.

In recent ‘sandbox’ runs, however, I started to notice that the VL53L0X controller (a Teensy 3.5) wasn’t always providing proper distance measurements. Sometimes it would return ‘-1’ for some or all seven measurements. Eventually I figured out that the problem only occurred when I restarted the main controller via the wireless serial connection; when I restarted by cycling the power, VL53L0X measurements were always proper. After looking into this a bit, I realized that the problem occurred because the VL53L0X array controller wasn’t being restarted when the main controller was, except when everything was power cycled.

So, I needed a way to ensure that the VL53L0X array controller got restarted, even with a serial-port reset of the main controller. The Teensy 3.5 actually has a RESET function exposed on a pin pad (although internal to the PCB, not on the periphery) so I added a pin to this pad, and connected it to the wire formerly used as the ‘left ping’ control line. Then I modified the setup code to pull this line LOW for a few 10’s of milliseconds and then back HIGH again, to restart the Teensy.

To test the modification, I modified the main controller code to send a HIGH to an unused digital pin as the first instruction in setup() and set that pin back LOW again as the last instruction. Immediately after setting this pin high, the RESET signal is sent to the Teensy. The Teensy program was modified to set an unused pin HIGH at the start of it’s setup() program, and LOW at the end. By monitoring these two pins with my wondrous Hanmatek DOS1102 DSO (see below) I was able to definitively confirm that the Teensy restarts every time the main controller does – yay!

Yellow trace is main controller setup() timing, blue is Teensy VL53L0X array controller setup() timing

In the above scope photo, the horizontal scale is 1 sec/div. The yellow trace shows the main controller setup() function timing, and the blue is the Teensy VL53L0X array controller setup() function timing. The Teensy gets reset about 500 mSec after the main controller setup() function starts, and it ends about 5.5 Sec later, about 500 mSec before the main controller setup() function ends. The relative timing shown above is the same whether the main controller is restarted via a power switch cycle or a serial port re-open restart.

Stay Tuned,

Frank

Wall Tracking Trials Using Office ‘Sandbox’ Part I

1 Reply

Posted 12 November 2020

Back in October I added a TIMER5 timer interrupt to my autonomous wall-following robot (WAll-E2) code to manage sensor updates. Since then I have made the timer interrupt the sole timing source for all sensor and tracking updates, and upped the update rate from 5Hz to 10Hz. In addition, I’ve been making some improvements to Wall-E2’s obstacle detection/response abilities, and this post describes the results of these enhancements.

Wall-E2’s job is to autonomously track walls forever. This implies the ability not only track walls, but to deal with obstacles as they occur, and recharge its batteries at one or more provided charging stations as needed. Wall-tracking per se has been the subject of several previous posts, and is now reasonably well managed using the ‘find parallel’ technique described here. This post deals with the effort to detect and respond to obstacles as they occur. Here’s a recent run in my office ‘sandbox’

Initializing Left & Right Distance Arrays...
Left	Right
340	8190
329	8190
330	8190
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 3453 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
3464	326	334	316	175	 4474.00	508	0.01	34.93	-0.30	40	109
3473	331	341	323	175	 4474.00	508	-0.01	34.93	-0.30	40	109
3554	329	342	326	172	 4286.29	8190	0.03	34.93	-0.30	40	109
3651	326	354	325	181	 4164.85	1192	-0.01	24.80	-0.30	50	99
3750	329	354	349	127	 4520.47	1114	-0.20	24.80	-0.30	50	99
3850	307	387	336	102	 5186.04	1197	-0.29	-75.00	-0.30	150	0
3946	306	342	361	96	 5933.30	1156	-0.55	-75.00	-0.30	150	0
4043	293	325	335	105	 6564.30	1272	-0.29	-2.29	-0.25	77	72
4141	282	310	305	139	 6862.29	8190	-0.23	-2.29	-0.10	77	72
4235	272	296	287	174	 6912.94	914	-0.19	8.02	0.04	66	83
4330	267	284	285	172	 6889.58	920	-0.18	8.02	0.16	66	83
4427	260	283	267	165	 6772.68	967	-0.06	16.33	0.17	58	91
4521	249	286	270	147	 6565.69	1069	-0.21	16.33	0.14	58	91
4620	248	277	268	134	 6196.07	1056	-0.20	-75.00	0.23	150	0
4717	238	261	248	133	 5544.52	1166	-0.10	-75.00	0.30	150	0
4812	230	243	249	126	 4582.67	1046	-0.19	-48.43	0.30	123	26
4910	220	243	236	122	 3231.83	974	-0.16	-48.43	0.30	123	26
5006	218	232	213	121	 1415.66	1029	0.05	43.82	0.30	31	118
5105	218	246	205	119	 1363.33	1169	0.13	43.82	0.30	31	118
5200	215	237	221	115	 1303.73	1136	-0.06	-73.26	0.30	148	1
5300	217	227	218	113	 1215.34	1190	-0.01	-21.19	0.30	96	53
5396	219	235	205	110	 1098.33	1228	0.14	-21.19	0.30	96	53
5491	228	244	215	106	  974.86	8190	0.19	43.48	0.30	31	118
5587	239	260	218	103	  868.73	8190	0.21	43.48	0.30	31	118
5688	244	254	235	97	  807.43	8190	0.09	-57.15	0.30	132	17
5783	253	268	246	94	  766.78	8190	0.07	-57.15	0.30	132	17
5882	260	260	246	92	  731.99	8190	0.14	-7.63	0.30	82	67
5982	282	278	257	88	  697.85	8190	0.25	-7.63	0.22	82	67
6081	287	279	256	85	  608.15	8190	0.30	50.64	0.21	24	125
6178	294	297	276	80	  664.40	8190	0.18	50.64	0.03	24	125
6277	316	305	287	77	  719.44	8190	0.11	-46.88	-0.05	121	28
6373	311	315	289	74	  774.14	8190	0.22	-46.88	-0.15	121	28
6471	311	334	292	73	  840.70	8190	0.25	75.00	-0.30	0	150
6569	327	344	311	68	  894.32	8190	0.16	75.00	-0.30	0	150
6670	340	348	308	62	  816.19	8190	0.32	75.00	-0.30	0	150
6772	342	360	333	54	  737.65	8190	0.09	-31.20	-0.30	106	43
6868	354	350	333	53	  657.72	8190	0.21	-31.20	-0.30	106	43
6967	338	358	326	51	  634.07	8190	0.12	51.06	-0.30	23	126
7063	346	368	333	50	  636.22	8190	0.13	51.06	-0.30	23	126
7161	334	346	345	44	  638.78	8190	-0.11	-49.37	-0.30	124	25
7260	334	371	333	39	  658.55	8190	0.01	-49.37	-0.30	124	25
7360	336	350	330	32	  693.11	8190	0.06	75.00	-0.30	0	150
7457	332	357	319	25	  732.13	8190	0.13	75.00	-0.30	0	150
7556	344	355	332	23	  757.01	8191	0.12	62.97	-0.30	12	137
bObstacleAhead TRUE in TRACKLEFT at 7656 with frontdist 18, variance = 787.12
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
7751	6.06	1.38	52.1	20.0	87
7848	1.23	6.06	51.0	20.0	50
7947	-5.41	1.23	68.5	20.0	50
8045	-12.14	-5.41	70.7	20.0	50
8144	-17.56	-12.14	55.5	20.0	50
8244	-21.25	-17.56	38.0	20.0	50
8341	-23.40	-21.25	22.6	20.0	50
8438	-24.34	-23.40	9.9	20.0	90
8537	-25.11	-24.34	8.0	20.0	130
8636	-28.17	-25.11	31.4	20.0	90
8735	-35.01	-28.17	71.1	20.0	50
8835	-41.34	-35.01	65.0	20.0	50
8935	-44.53	-41.34	32.6	20.0	50
9036	-46.09	-44.53	15.8	20.0	90
9135	-47.11	-46.09	10.6	20.0	130
9233	-50.33	-47.11	33.3	20.0	90
9333	-56.48	-50.33	63.0	20.0	50
Slowing down to 10.00 deg/sec at -56.48 deg
9430	-62.64	-56.48	65.1	10.0	50
9529	-65.95	-62.64	34.4	10.0	50
9626	-66.79	-65.95	8.7	10.0	90
9723	-67.14	-66.79	3.7	10.0	130
9823	-68.64	-67.14	15.5	10.0	90
9919	-72.84	-68.64	44.8	10.0	50
10018	-76.92	-72.84	42.1	10.0	50
10116	-79.19	-76.92	23.8	10.0	50
10213	-79.30	-79.19	1.2	10.0	90
10313	-79.37	-79.30	0.7	10.0	130
10414	-80.87	-79.37	15.3	10.0	90
10417: Exiting SpinTurn() at -80.87 deg
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 11293 with SteeringVal = 0.00

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
11304	49	69	49	101	  808.15	574	0.00	-63.93	0.30	138	11
11313	45	66	46	101	  808.15	574	-0.01	-63.93	0.30	138	11
11395	44	64	47	109	  891.88	627	-0.03	-63.93	0.30	138	11
11492	43	65	43	162	 1291.60	673	0.00	-28.83	0.30	103	46
11591	47	100	44	216	 2143.29	728	0.03	-28.83	0.30	103	46
11689	47	70	81	94	 2055.42	758	-0.34	-75.00	0.30	150	0
11786	52	71	45	69	 1960.80	749	0.07	-75.00	0.30	150	0
11882	56	75	45	67	 1851.13	701	0.11	75.00	0.30	0	150
11979	63	76	43	63	 1762.12	489	0.20	75.00	0.30	0	150
12079	62	84	56	58	 1678.46	534	0.06	-41.04	0.30	116	33
12176	73	89	71	54	 1594.01	745	0.02	-41.04	0.30	116	33
12275	79	90	73	50	 1519.21	790	0.06	-26.76	0.30	101	48
12372	85	97	74	47	 1480.52	774	0.11	-26.76	0.30	101	48
12473	95	109	78	42	 1477.60	619	0.17	16.85	0.30	58	91
12569	108	113	87	35	 1515.29	570	0.21	16.85	0.30	58	91
12666	113	120	98	33	 1561.57	639	0.15	-24.60	0.30	99	50
Hit Wall Offset Limit (30 cm) at 1769472 with frontdist = 29, Setpoint = 0.00
In StepTurn(Tracking Left)
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
12862	-109.12	-107.73	15.9	20.0	167
12961	-116.70	-109.12	78.1	20.0	127
13061	-127.75	-116.70	113.9	20.0	87
13158	-142.57	-127.75	155.6	20.0	50
13257	-155.69	-142.57	136.5	20.0	50
13355	-165.76	-155.69	106.5	20.0	50
Slowing down to 10.00 deg/sec at -165.76 deg
13453	-172.39	-165.76	69.2	10.0	50
13551	-176.13	-172.39	38.6	10.0	50
13650	-176.98	-176.13	8.9	10.0	90
13747	-177.02	-176.98	0.4	10.0	130
13845	-178.80	-177.02	18.5	10.0	90
13946	175.39	-178.80	3604.6	10.0	50
14041	169.71	175.39	61.9	10.0	50
14043: Exiting SpinTurn() at 169.71 deg
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 14687 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
14698	191	208	190	251	10269.92	289	0.01	-75.00	0.30	150	0
14706	196	205	195	251	10269.92	289	-0.06	-75.00	0.30	150	0
14788	198	213	196	246	10313.22	355	0.02	-75.00	0.30	150	0
14884	207	219	198	240	10143.41	402	0.09	-2.10	0.30	77	72
14983	210	226	197	232	 9731.76	451	0.13	-2.10	0.30	77	72
15078	216	228	209	231	 9169.12	488	0.07	-34.79	0.30	109	40
15174	223	240	214	226	 8389.68	547	0.09	-34.79	0.30	109	40
15270	225	243	215	226	 7483.51	576	0.10	-17.39	0.30	92	57
15366	223	244	229	224	 6445.87	624	-0.06	-17.39	0.30	92	57
15462	228	245	232	221	 5279.39	657	-0.01	-71.32	0.30	146	3
15558	234	254	229	217	 4146.39	705	0.05	-71.32	0.30	146	3
15655	248	260	220	218	 3116.80	748	0.28	75.00	0.30	0	150
15749	272	266	238	218	 2207.54	794	0.34	75.00	0.30	0	150
15843	282	288	248	211	 1580.34	839	0.30	17.16	0.12	57	92
15938	293	287	264	200	 1231.91	877	0.29	17.16	0.13	57	92
16038	301	303	266	194	  947.68	895	0.27	15.06	-0.03	59	90
16137	304	307	281	192	  667.96	916	0.23	15.06	-0.07	59	90
16232	305	320	297	187	  675.37	984	0.08	-34.10	-0.20	109	40
16329	321	327	294	183	  719.94	1012	0.27	-34.10	-0.27	109	40
16429	326	344	301	181	  748.10	1030	0.25	75.00	-0.30	0	150
16525	328	345	318	175	  773.36	1061	0.10	75.00	-0.30	0	150
16623	340	350	323	171	  785.28	1139	0.17	20.96	-0.30	54	95
16719	344	351	327	165	  797.68	1142	0.17	20.96	-0.30	54	95
16820	355	364	357	161	  809.43	1160	-0.01	-26.17	-0.30	101	48
16914	356	372	351	157	  816.35	1215	0.05	-26.17	-0.30	101	48
17007	371	376	338	153	  826.46	1232	0.33	75.00	-0.30	0	150
17104	365	378	350	151	  828.07	1275	0.15	75.00	-0.30	0	150
17200	362	363	359	146	  837.66	8190	0.03	-55.29	-0.30	130	19
17295	360	371	353	143	  851.33	8190	0.07	-55.29	-0.30	130	19
17392	361	387	354	136	  892.80	8190	0.07	51.82	-0.30	23	126
17488	364	368	354	134	  916.39	8190	0.10	51.82	-0.30	23	126
17584	352	380	353	129	  948.67	8190	-0.01	8.69	-0.30	66	83
17680	349	368	360	128	  954.63	8190	-0.11	8.69	-0.30	66	83
17776	370	381	356	126	  945.54	8190	0.06	61.77	-0.30	13	136
17872	362	375	362	123	  929.23	8190	0.00	61.77	-0.30	13	136
17972	347	377	363	122	  903.09	8190	-0.04	1.55	-0.30	73	76
18070	348	395	372	122	  843.49	8190	-0.24	1.55	-0.30	73	76
18167	352	370	362	120	  760.07	8190	-0.17	-20.06	-0.30	95	54
18264	332	368	353	115	  694.64	8190	-0.21	-20.06	-0.30	95	54
18362	312	336	339	111	  661.03	8190	-0.20	7.24	-0.30	67	82
18460	297	323	327	105	  644.80	8190	-0.30	7.24	-0.23	67	82
18560	283	312	313	100	  627.13	8190	-0.28	-34.24	-0.12	109	40
18655	285	302	299	96	  615.78	8190	-0.14	-34.24	-0.02	109	40
18754	264	298	287	90	  613.99	8190	-0.19	11.13	0.02	63	86
18850	259	276	255	86	  606.39	8190	0.04	11.13	0.24	63	86
18950	252	284	259	84	  601.15	8190	-0.09	9.38	0.16	65	84
19046	240	280	260	81	  595.10	8190	-0.20	9.38	0.20	65	84
19144	238	252	248	78	  595.94	8190	-0.10	-39.82	0.30	114	35
19240	217	253	242	76	  593.76	8190	-0.13	-39.82	0.30	114	35
19338	218	246	230	71	  598.99	8190	-0.09	-33.99	0.30	108	41
19438	216	236	221	67	  608.08	8190	-0.10	-42.19	0.30	117	32
19535	205	225	214	61	  621.04	8190	-0.09	-42.19	0.30	117	32
19634	208	222	203	55	  647.83	8190	0.06	24.09	0.30	50	99
19733	208	221	203	50	  677.59	8190	0.05	24.09	0.30	50	99
19832	210	230	201	48	  711.95	8190	0.18	23.73	0.30	51	98
19932	224	232	214	45	  739.29	8190	0.10	-44.87	0.30	119	30
20032	224	231	213	40	  776.88	8190	0.10	-21.47	0.30	96	53
20130	228	235	210	37	  803.67	8190	0.18	-21.47	0.30	96	53
20231	240	244	216	32	  829.84	8190	0.15	-1.92	0.30	76	73
Hit Wall Offset Limit 20330 at 1835008 with frontdist = -26214, Setpoint = 0.00
In StepTurn(Tracking Left)
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
20427	170.01	171.69	18.9	20.0	167
20528	162.73	170.01	74.4	20.0	127
20628	149.44	162.73	136.8	20.0	87
20724	133.31	149.44	171.6	20.0	50
20824	118.14	133.31	156.0	20.0	50
20921	107.84	118.14	108.3	20.0	50
Slowing down to 10.00 deg/sec at 107.84 deg
21018	100.75	107.84	75.0	10.0	50
21114	96.88	100.75	41.2	10.0	50
21211	95.61	96.88	13.7	10.0	50
21304	95.40	95.61	2.2	10.0	90
21401	95.27	95.40	1.4	10.0	130
21500	92.77	95.27	26.0	10.0	90
21595	86.00	92.77	73.4	10.0	50
21597: Exiting SpinTurn() at 86.00 deg
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 21983 with SteeringVal = -0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
21994	161	185	162	108	 5323.48	265	-0.01	-75.00	0.30	150	0
22002	164	182	160	108	 5323.48	265	0.05	-75.00	0.30	150	0
22086	166	184	159	78	 5270.27	295	0.07	-75.00	0.30	150	0
22184	175	185	165	58	 5243.36	352	0.10	9.55	0.30	65	84
22280	183	190	172	175	 5373.00	388	0.11	9.55	0.30	65	84
22375	197	204	176	91	 5247.15	436	0.21	20.28	0.30	54	95
22470	197	215	182	108	 5081.33	467	0.15	20.28	0.30	54	95
22564	207	226	189	151	 4952.56	518	0.18	-25.08	0.30	100	49
22663	218	233	204	52	 4842.48	555	0.14	-25.08	0.30	100	49
22761	236	245	213	141	 4580.68	601	0.18	-16.44	0.30	91	58
bObstacleAhead TRUE in TRACKLEFT at 22858 with frontdist 13, variance = 4694.59
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
22952	71.13	73.50	26.9	20.0	87
23052	63.70	71.13	76.5	20.0	50
23147	54.57	63.70	98.3	20.0	50
23246	48.04	54.57	67.2	20.0	50
23343	43.69	48.04	46.2	20.0	50
23441	40.42	43.69	34.1	20.0	50
23540	38.54	40.42	19.5	20.0	90
23638	37.63	38.54	9.7	20.0	130
23734	34.63	37.63	31.7	20.0	90
23833	32.15	34.63	25.5	20.0	50
23931	30.65	32.15	15.7	20.0	90
24031	30.52	30.65	1.4	20.0	130
24127	29.88	30.52	6.8	20.0	170
24225	28.04	29.88	19.3	20.0	210
24324	24.16	28.04	40.3	20.0	170
24419	20.53	24.16	39.3	20.0	130
24515	11.17	20.53	98.9	20.0	90
Slowing down to 10.00 deg/sec at 11.17 deg
24611	0.33	11.17	115.7	10.0	50
24711	-8.59	0.33	92.5	10.0	50
24713: Exiting SpinTurn() at -8.59 deg
In RotateToParallelOrientation(Left)
Parallel Orientation Achieved at 27962 with SteeringVal = -79.86

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
27973	313	8191	8190	230	   14.32	369	-78.77	-75.00	-0.30	150	0
bStuck TRUE in TRACKLEFT at 27981 with frontdist 230, variance =  14.32
---- ExecuteStuckRecoveryManeuver
Using front distance for backup with front/reardist  = 230/36
28059	231	369
28153	248	350
28249	250	327
28346	254	282
28443	255	232
finished backup with frontdist/reardist = 28444	 255/23
In SpinTurn(CW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
30450	20.77	20.77	0.0	20.0	167
30543	19.58	20.77	13.1	20.0	207
30640	9.56	19.58	106.9	20.0	167
30736	-2.24	9.56	125.2	20.0	127
30830	-14.60	-2.24	134.4	20.0	87
30922	-22.53	-14.60	88.6	20.0	50
31019	-33.56	-22.53	118.1	20.0	50
Slowing down to 10.00 deg/sec at -33.56 deg
31117	-43.11	-33.56	99.6	10.0	50
31214	-49.46	-43.11	66.5	10.0	50
31310	-52.74	-49.46	35.0	10.0	50
31409	-53.79	-52.74	10.8	10.0	50
31510	-53.85	-53.79	0.6	10.0	90
31607	-54.34	-53.85	5.2	10.0	130
31704	-56.91	-54.34	27.5	10.0	90
31802	-62.72	-56.91	60.3	10.0	50
31805: Exiting SpinTurn() at -62.72 deg
Using front distance for fwd travel with front/reardist  = 62/34
33807	62	340
33862	61	329
33959	59	346
34058	57	354
34157	52	377
34254	49	434
34354	41	453
finished forward travel with frontdist/reardist = 41/45
In SpinTurn(CCW, 90.00)
Msec	Hdg	pHdg	Rate	TRate	Spd
36356	-64.92	-64.92	0.0	20.0	167
36419	-64.63	-64.92	4.8	20.0	207
36517	-56.67	-64.63	83.0	20.0	167
36616	-41.04	-56.67	163.3	20.0	127
36712	-25.33	-41.04	166.6	20.0	87
36808	-8.73	-25.33	177.6	20.0	50
Slowing down to 10.00 deg/sec at -8.73 deg
36902	7.93	-8.73	182.0	10.0	50
36998	19.87	7.93	128.6	10.0	50
37000: Exiting SpinTurn() at 19.87 deg
I received: 63
ENTERING COMMAND MODE:
0 = 180 deg CCW Turn
1 = 180 deg CW Turn
A = Back to Auto Mode
S = Stop
F = Forward
R = Reverse

       Faster
	8
Left 4	5  6 Right
	2
       Slower

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

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

Initializing Left & Right Distance Arrays...

Left Right

340 8190

329 8190

330 8190

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 3453 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

3464 326 334 316 175 4474.00 508 0.01 34.93 -0.30 40 109

3473 331 341 323 175 4474.00 508 -0.01 34.93 -0.30 40 109

3554 329 342 326 172 4286.29 8190 0.03 34.93 -0.30 40 109

3651 326 354 325 181 4164.85 1192 -0.01 24.80 -0.30 50 99

3750 329 354 349 127 4520.47 1114 -0.20 24.80 -0.30 50 99

3850 307 387 336 102 5186.04 1197 -0.29 -75.00 -0.30 150 0

3946 306 342 361 96 5933.30 1156 -0.55 -75.00 -0.30 150 0

4043 293 325 335 105 6564.30 1272 -0.29 -2.29 -0.25 77 72

4141 282 310 305 139 6862.29 8190 -0.23 -2.29 -0.10 77 72

4235 272 296 287 174 6912.94 914 -0.19 8.02 0.04 66 83

4330 267 284 285 172 6889.58 920 -0.18 8.02 0.16 66 83

4427 260 283 267 165 6772.68 967 -0.06 16.33 0.17 58 91

4521 249 286 270 147 6565.69 1069 -0.21 16.33 0.14 58 91

4620 248 277 268 134 6196.07 1056 -0.20 -75.00 0.23 150 0

4717 238 261 248 133 5544.52 1166 -0.10 -75.00 0.30 150 0

4812 230 243 249 126 4582.67 1046 -0.19 -48.43 0.30 123 26

4910 220 243 236 122 3231.83 974 -0.16 -48.43 0.30 123 26

5006 218 232 213 121 1415.66 1029 0.05 43.82 0.30 31 118

5105 218 246 205 119 1363.33 1169 0.13 43.82 0.30 31 118

5200 215 237 221 115 1303.73 1136 -0.06 -73.26 0.30 148 1

5300 217 227 218 113 1215.34 1190 -0.01 -21.19 0.30 96 53

5396 219 235 205 110 1098.33 1228 0.14 -21.19 0.30 96 53

5491 228 244 215 106 974.86 8190 0.19 43.48 0.30 31 118

5587 239 260 218 103 868.73 8190 0.21 43.48 0.30 31 118

5688 244 254 235 97 807.43 8190 0.09 -57.15 0.30 132 17

5783 253 268 246 94 766.78 8190 0.07 -57.15 0.30 132 17

5882 260 260 246 92 731.99 8190 0.14 -7.63 0.30 82 67

5982 282 278 257 88 697.85 8190 0.25 -7.63 0.22 82 67

6081 287 279 256 85 608.15 8190 0.30 50.64 0.21 24 125

6178 294 297 276 80 664.40 8190 0.18 50.64 0.03 24 125

6277 316 305 287 77 719.44 8190 0.11 -46.88 -0.05 121 28

6373 311 315 289 74 774.14 8190 0.22 -46.88 -0.15 121 28

6471 311 334 292 73 840.70 8190 0.25 75.00 -0.30 0 150

6569 327 344 311 68 894.32 8190 0.16 75.00 -0.30 0 150

6670 340 348 308 62 816.19 8190 0.32 75.00 -0.30 0 150

6772 342 360 333 54 737.65 8190 0.09 -31.20 -0.30 106 43

6868 354 350 333 53 657.72 8190 0.21 -31.20 -0.30 106 43

6967 338 358 326 51 634.07 8190 0.12 51.06 -0.30 23 126

7063 346 368 333 50 636.22 8190 0.13 51.06 -0.30 23 126

7161 334 346 345 44 638.78 8190 -0.11 -49.37 -0.30 124 25

7260 334 371 333 39 658.55 8190 0.01 -49.37 -0.30 124 25

7360 336 350 330 32 693.11 8190 0.06 75.00 -0.30 0 150

7457 332 357 319 25 732.13 8190 0.13 75.00 -0.30 0 150

7556 344 355 332 23 757.01 8191 0.12 62.97 -0.30 12 137

bObstacleAhead TRUE in TRACKLEFT at 7656 with frontdist 18, variance = 787.12

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

7751 6.06 1.38 52.1 20.0 87

7848 1.23 6.06 51.0 20.0 50

7947 -5.41 1.23 68.5 20.0 50

8045 -12.14 -5.41 70.7 20.0 50

8144 -17.56 -12.14 55.5 20.0 50

8244 -21.25 -17.56 38.0 20.0 50

8341 -23.40 -21.25 22.6 20.0 50

8438 -24.34 -23.40 9.9 20.0 90

8537 -25.11 -24.34 8.0 20.0 130

8636 -28.17 -25.11 31.4 20.0 90

8735 -35.01 -28.17 71.1 20.0 50

8835 -41.34 -35.01 65.0 20.0 50

8935 -44.53 -41.34 32.6 20.0 50

9036 -46.09 -44.53 15.8 20.0 90

9135 -47.11 -46.09 10.6 20.0 130

9233 -50.33 -47.11 33.3 20.0 90

9333 -56.48 -50.33 63.0 20.0 50

Slowing down to 10.00 deg/sec at -56.48 deg

9430 -62.64 -56.48 65.1 10.0 50

9529 -65.95 -62.64 34.4 10.0 50

9626 -66.79 -65.95 8.7 10.0 90

9723 -67.14 -66.79 3.7 10.0 130

9823 -68.64 -67.14 15.5 10.0 90

9919 -72.84 -68.64 44.8 10.0 50

10018 -76.92 -72.84 42.1 10.0 50

10116 -79.19 -76.92 23.8 10.0 50

10213 -79.30 -79.19 1.2 10.0 90

10313 -79.37 -79.30 0.7 10.0 130

10414 -80.87 -79.37 15.3 10.0 90

10417: Exiting SpinTurn() at -80.87 deg

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 11293 with SteeringVal = 0.00

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

11304 49 69 49 101 808.15 574 0.00 -63.93 0.30 138 11

11313 45 66 46 101 808.15 574 -0.01 -63.93 0.30 138 11

11395 44 64 47 109 891.88 627 -0.03 -63.93 0.30 138 11

11492 43 65 43 162 1291.60 673 0.00 -28.83 0.30 103 46

11591 47 100 44 216 2143.29 728 0.03 -28.83 0.30 103 46

11689 47 70 81 94 2055.42 758 -0.34 -75.00 0.30 150 0

11786 52 71 45 69 1960.80 749 0.07 -75.00 0.30 150 0

11882 56 75 45 67 1851.13 701 0.11 75.00 0.30 0 150

11979 63 76 43 63 1762.12 489 0.20 75.00 0.30 0 150

12079 62 84 56 58 1678.46 534 0.06 -41.04 0.30 116 33

12176 73 89 71 54 1594.01 745 0.02 -41.04 0.30 116 33

12275 79 90 73 50 1519.21 790 0.06 -26.76 0.30 101 48

12372 85 97 74 47 1480.52 774 0.11 -26.76 0.30 101 48

12473 95 109 78 42 1477.60 619 0.17 16.85 0.30 58 91

12569 108 113 87 35 1515.29 570 0.21 16.85 0.30 58 91

12666 113 120 98 33 1561.57 639 0.15 -24.60 0.30 99 50

Hit Wall Offset Limit (30 cm) at 1769472 with frontdist = 29, Setpoint = 0.00

In StepTurn(Tracking Left)

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

12862 -109.12 -107.73 15.9 20.0 167

12961 -116.70 -109.12 78.1 20.0 127

13061 -127.75 -116.70 113.9 20.0 87

13158 -142.57 -127.75 155.6 20.0 50

13257 -155.69 -142.57 136.5 20.0 50

13355 -165.76 -155.69 106.5 20.0 50

Slowing down to 10.00 deg/sec at -165.76 deg

13453 -172.39 -165.76 69.2 10.0 50

13551 -176.13 -172.39 38.6 10.0 50

13650 -176.98 -176.13 8.9 10.0 90

13747 -177.02 -176.98 0.4 10.0 130

13845 -178.80 -177.02 18.5 10.0 90

13946 175.39 -178.80 3604.6 10.0 50

14041 169.71 175.39 61.9 10.0 50

14043: Exiting SpinTurn() at 169.71 deg

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 14687 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

14698 191 208 190 251 10269.92 289 0.01 -75.00 0.30 150 0

14706 196 205 195 251 10269.92 289 -0.06 -75.00 0.30 150 0

14788 198 213 196 246 10313.22 355 0.02 -75.00 0.30 150 0

14884 207 219 198 240 10143.41 402 0.09 -2.10 0.30 77 72

14983 210 226 197 232 9731.76 451 0.13 -2.10 0.30 77 72

15078 216 228 209 231 9169.12 488 0.07 -34.79 0.30 109 40

15174 223 240 214 226 8389.68 547 0.09 -34.79 0.30 109 40

15270 225 243 215 226 7483.51 576 0.10 -17.39 0.30 92 57

15366 223 244 229 224 6445.87 624 -0.06 -17.39 0.30 92 57

15462 228 245 232 221 5279.39 657 -0.01 -71.32 0.30 146 3

15558 234 254 229 217 4146.39 705 0.05 -71.32 0.30 146 3

15655 248 260 220 218 3116.80 748 0.28 75.00 0.30 0 150

15749 272 266 238 218 2207.54 794 0.34 75.00 0.30 0 150

15843 282 288 248 211 1580.34 839 0.30 17.16 0.12 57 92

15938 293 287 264 200 1231.91 877 0.29 17.16 0.13 57 92

16038 301 303 266 194 947.68 895 0.27 15.06 -0.03 59 90

16137 304 307 281 192 667.96 916 0.23 15.06 -0.07 59 90

16232 305 320 297 187 675.37 984 0.08 -34.10 -0.20 109 40

16329 321 327 294 183 719.94 1012 0.27 -34.10 -0.27 109 40

16429 326 344 301 181 748.10 1030 0.25 75.00 -0.30 0 150

16525 328 345 318 175 773.36 1061 0.10 75.00 -0.30 0 150

16623 340 350 323 171 785.28 1139 0.17 20.96 -0.30 54 95

16719 344 351 327 165 797.68 1142 0.17 20.96 -0.30 54 95

16820 355 364 357 161 809.43 1160 -0.01 -26.17 -0.30 101 48

16914 356 372 351 157 816.35 1215 0.05 -26.17 -0.30 101 48

17007 371 376 338 153 826.46 1232 0.33 75.00 -0.30 0 150

17104 365 378 350 151 828.07 1275 0.15 75.00 -0.30 0 150

17200 362 363 359 146 837.66 8190 0.03 -55.29 -0.30 130 19

17295 360 371 353 143 851.33 8190 0.07 -55.29 -0.30 130 19

17392 361 387 354 136 892.80 8190 0.07 51.82 -0.30 23 126

17488 364 368 354 134 916.39 8190 0.10 51.82 -0.30 23 126

17584 352 380 353 129 948.67 8190 -0.01 8.69 -0.30 66 83

17680 349 368 360 128 954.63 8190 -0.11 8.69 -0.30 66 83

17776 370 381 356 126 945.54 8190 0.06 61.77 -0.30 13 136

17872 362 375 362 123 929.23 8190 0.00 61.77 -0.30 13 136

17972 347 377 363 122 903.09 8190 -0.04 1.55 -0.30 73 76

18070 348 395 372 122 843.49 8190 -0.24 1.55 -0.30 73 76

18167 352 370 362 120 760.07 8190 -0.17 -20.06 -0.30 95 54

18264 332 368 353 115 694.64 8190 -0.21 -20.06 -0.30 95 54

18362 312 336 339 111 661.03 8190 -0.20 7.24 -0.30 67 82

18460 297 323 327 105 644.80 8190 -0.30 7.24 -0.23 67 82

18560 283 312 313 100 627.13 8190 -0.28 -34.24 -0.12 109 40

18655 285 302 299 96 615.78 8190 -0.14 -34.24 -0.02 109 40

18754 264 298 287 90 613.99 8190 -0.19 11.13 0.02 63 86

18850 259 276 255 86 606.39 8190 0.04 11.13 0.24 63 86

18950 252 284 259 84 601.15 8190 -0.09 9.38 0.16 65 84

19046 240 280 260 81 595.10 8190 -0.20 9.38 0.20 65 84

19144 238 252 248 78 595.94 8190 -0.10 -39.82 0.30 114 35

19240 217 253 242 76 593.76 8190 -0.13 -39.82 0.30 114 35

19338 218 246 230 71 598.99 8190 -0.09 -33.99 0.30 108 41

19438 216 236 221 67 608.08 8190 -0.10 -42.19 0.30 117 32

19535 205 225 214 61 621.04 8190 -0.09 -42.19 0.30 117 32

19634 208 222 203 55 647.83 8190 0.06 24.09 0.30 50 99

19733 208 221 203 50 677.59 8190 0.05 24.09 0.30 50 99

19832 210 230 201 48 711.95 8190 0.18 23.73 0.30 51 98

19932 224 232 214 45 739.29 8190 0.10 -44.87 0.30 119 30

20032 224 231 213 40 776.88 8190 0.10 -21.47 0.30 96 53

20130 228 235 210 37 803.67 8190 0.18 -21.47 0.30 96 53

20231 240 244 216 32 829.84 8190 0.15 -1.92 0.30 76 73

Hit Wall Offset Limit 20330 at 1835008 with frontdist = -26214, Setpoint = 0.00

In StepTurn(Tracking Left)

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

20427 170.01 171.69 18.9 20.0 167

20528 162.73 170.01 74.4 20.0 127

20628 149.44 162.73 136.8 20.0 87

20724 133.31 149.44 171.6 20.0 50

20824 118.14 133.31 156.0 20.0 50

20921 107.84 118.14 108.3 20.0 50

Slowing down to 10.00 deg/sec at 107.84 deg

21018 100.75 107.84 75.0 10.0 50

21114 96.88 100.75 41.2 10.0 50

21211 95.61 96.88 13.7 10.0 50

21304 95.40 95.61 2.2 10.0 90

21401 95.27 95.40 1.4 10.0 130

21500 92.77 95.27 26.0 10.0 90

21595 86.00 92.77 73.4 10.0 50

21597: Exiting SpinTurn() at 86.00 deg

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 21983 with SteeringVal = -0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

21994 161 185 162 108 5323.48 265 -0.01 -75.00 0.30 150 0

22002 164 182 160 108 5323.48 265 0.05 -75.00 0.30 150 0

22086 166 184 159 78 5270.27 295 0.07 -75.00 0.30 150 0

22184 175 185 165 58 5243.36 352 0.10 9.55 0.30 65 84

22280 183 190 172 175 5373.00 388 0.11 9.55 0.30 65 84

22375 197 204 176 91 5247.15 436 0.21 20.28 0.30 54 95

22470 197 215 182 108 5081.33 467 0.15 20.28 0.30 54 95

22564 207 226 189 151 4952.56 518 0.18 -25.08 0.30 100 49

22663 218 233 204 52 4842.48 555 0.14 -25.08 0.30 100 49

22761 236 245 213 141 4580.68 601 0.18 -16.44 0.30 91 58

bObstacleAhead TRUE in TRACKLEFT at 22858 with frontdist 13, variance = 4694.59

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

22952 71.13 73.50 26.9 20.0 87

23052 63.70 71.13 76.5 20.0 50

23147 54.57 63.70 98.3 20.0 50

23246 48.04 54.57 67.2 20.0 50

23343 43.69 48.04 46.2 20.0 50

23441 40.42 43.69 34.1 20.0 50

23540 38.54 40.42 19.5 20.0 90

23638 37.63 38.54 9.7 20.0 130

23734 34.63 37.63 31.7 20.0 90

23833 32.15 34.63 25.5 20.0 50

23931 30.65 32.15 15.7 20.0 90

24031 30.52 30.65 1.4 20.0 130

24127 29.88 30.52 6.8 20.0 170

24225 28.04 29.88 19.3 20.0 210

24324 24.16 28.04 40.3 20.0 170

24419 20.53 24.16 39.3 20.0 130

24515 11.17 20.53 98.9 20.0 90

Slowing down to 10.00 deg/sec at 11.17 deg

24611 0.33 11.17 115.7 10.0 50

24711 -8.59 0.33 92.5 10.0 50

24713: Exiting SpinTurn() at -8.59 deg

In RotateToParallelOrientation(Left)

Parallel Orientation Achieved at 27962 with SteeringVal = -79.86

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

27973 313 8191 8190 230 14.32 369 -78.77 -75.00 -0.30 150 0

bStuck TRUE in TRACKLEFT at 27981 with frontdist 230, variance = 14.32

---- ExecuteStuckRecoveryManeuver

Using front distance for backup with front/reardist = 230/36

28059 231 369

28153 248 350

28249 250 327

28346 254 282

28443 255 232

finished backup with frontdist/reardist = 28444 255/23

In SpinTurn(CW, 90.00)

Msec Hdg pHdg Rate TRate Spd

30450 20.77 20.77 0.0 20.0 167

30543 19.58 20.77 13.1 20.0 207

30640 9.56 19.58 106.9 20.0 167

30736 -2.24 9.56 125.2 20.0 127

30830 -14.60 -2.24 134.4 20.0 87

30922 -22.53 -14.60 88.6 20.0 50

31019 -33.56 -22.53 118.1 20.0 50

Slowing down to 10.00 deg/sec at -33.56 deg

31117 -43.11 -33.56 99.6 10.0 50

31214 -49.46 -43.11 66.5 10.0 50

31310 -52.74 -49.46 35.0 10.0 50

31409 -53.79 -52.74 10.8 10.0 50

31510 -53.85 -53.79 0.6 10.0 90

31607 -54.34 -53.85 5.2 10.0 130

31704 -56.91 -54.34 27.5 10.0 90

31802 -62.72 -56.91 60.3 10.0 50

31805: Exiting SpinTurn() at -62.72 deg

Using front distance for fwd travel with front/reardist = 62/34

33807 62 340

33862 61 329

33959 59 346

34058 57 354

34157 52 377

34254 49 434

34354 41 453

finished forward travel with frontdist/reardist = 41/45

In SpinTurn(CCW, 90.00)

Msec Hdg pHdg Rate TRate Spd

36356 -64.92 -64.92 0.0 20.0 167

36419 -64.63 -64.92 4.8 20.0 207

36517 -56.67 -64.63 83.0 20.0 167

36616 -41.04 -56.67 163.3 20.0 127

36712 -25.33 -41.04 166.6 20.0 87

36808 -8.73 -25.33 177.6 20.0 50

Slowing down to 10.00 deg/sec at -8.73 deg

36902 7.93 -8.73 182.0 10.0 50

36998 19.87 7.93 128.6 10.0 50

37000: Exiting SpinTurn() at 19.87 deg

I received: 63

ENTERING COMMAND MODE:

0 = 180 deg CCW Turn

1 = 180 deg CW Turn

A = Back to Auto Mode

S = Stop

F = Forward

R = Reverse

Faster

Left 4 5 6 Right

Slower

In the above telemetry printout, the first obstacle encounter occurs at 7.65 sec, corresponding to about 4 sec into the video. The obstacle is recognized at 18 cm, well inside the desired offset distance of 30 cm. I believe this occurred because the robot had just started turning back toward the near wall with a target steering value of -WALL_OFFSET_TRACK_SETPOINT_LIMIT (-0.3, the maximum toward-wall steering value) which meant that the normal forward obstacle detection limit of WALL_OFFSET_TGTDIST_CM (30cm in this case) wasn’t in force and the backup limit of MIN_FRONT_OBSTACLE_DIST_CM (20cm in this case) triggered instead. This causes the following code block to execute:

if (bObstacleAhead)//bObstacleAheada updated in timer1 ISR
{
	//mySerial.printf("bStuck or bObstacleAhead TRUE in TRACKLEFT with frontdist %d, variance = %6.2f\n", frontdistval, frontvar);
	mySerial.printf("bObstacleAhead TRUE in TRACKLEFT at %lu with frontdist %d, variance = %6.2f\n", 
		millis(), frontdistval, frontvar);
	InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

	SpinTurn(false, 90);//turn 90 deg right
	PrevOpMode = MODE_NONE; //this will force wall offset recapture

	break; //don't execute the rest - just go back to GetOpMode()
}

if (bObstacleAhead)//bObstacleAheada updated in timer1 ISR

{

//mySerial.printf("bStuck or bObstacleAhead TRUE in TRACKLEFT with frontdist %d, variance = %6.2f\n", frontdistval, frontvar);

mySerial.printf("bObstacleAhead TRUE in TRACKLEFT at %lu with frontdist %d, variance = %6.2f\n",

millis(), frontdistval, frontvar);

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

SpinTurn(false, 90);//turn 90 deg right

PrevOpMode = MODE_NONE; //this will force wall offset recapture

break; //don't execute the rest - just go back to GetOpMode()

}

As can be seen in the above code snippet, this causes the robot to make a 90º ‘spin turn’ to the right, and then restart wall tracking.

At about 12.6 sec (about 10 sec into the movie) we see it detect the upcoming wall at about 30 cm (due to a bug in the code, the printed values are incorrect). This causes the following code block to execute:

else if (WallTrackSetPoint > -WALL_OFFSET_TRACK_SETPOINT_LIMIT && frontdistval <= WALL_OFFSET_TGTDIST_CM)
{
	//DEBUG!!
	mySerial.printf("Hit Wall Offset Limit (%d cm) at %lu with frontdist = %d, Setpoint = %2.2f\n", 
		WALL_OFFSET_TGTDIST_CM, millis(), frontdistval, WallTrackSetPoint);
	//DEBUG!!

											//02/26/20 StepTurn() is now a blocking fcn - doesn't return until step-turn is complete
	StepTurn(TRACKING_LEFT);
	PrevOpMode = MODE_NONE; //this will force wall offset recapture
	break; //skip rest of processing
}

else if (WallTrackSetPoint > -WALL_OFFSET_TRACK_SETPOINT_LIMIT && frontdistval <= WALL_OFFSET_TGTDIST_CM)

{

//DEBUG!!

mySerial.printf("Hit Wall Offset Limit (%d cm) at %lu with frontdist = %d, Setpoint = %2.2f\n",

WALL_OFFSET_TGTDIST_CM, millis(), frontdistval, WallTrackSetPoint);

//DEBUG!!

//02/26/20 StepTurn() is now a blocking fcn - doesn't return until step-turn is complete

StepTurn(TRACKING_LEFT);

PrevOpMode = MODE_NONE; //this will force wall offset recapture

break; //skip rest of processing

}

This code executes a 90º ‘step turn’ (identical to a ‘spin turn’) to the right, and drops back into wall tracking mode.

At about 16 sec into the movie and 20 sec after program start, the robot again detects an upcoming obstacle at about 30cm, and again executes a 90º ‘step turn’ to the right to follow the new wall.

About 1.5 sec later, the robot detects one of the chair legs (I think it was the one nearest the wall in the movie) and tries to get away using another 90º ‘spin turn’, but then exhibits some abnormal behavior. When it attempts to find the parallel orientation to the new (non-existent) wall, it exits RotateToParallelOrientation(Left) with SteeringVal = -79.86, a very strange result. I believe this is because Wall-E2 detected the ‘stuck’ condition while it was attempting to complete the parallel orientation procedure, in this ‘while’ loop

while (abs(ToFSteeringVal) > ToFArray_PARALLEL_FIND_SETPOINT && !bIsStuck && !bObstacleAhead)
{

1 2	while (abs(ToFSteeringVal) > ToFArray_PARALLEL_FIND_SETPOINT && !bIsStuck && !bObstacleAhead) {

So, it exited abnormally, thus the odd SteeringVal number, and then re-detected it in the main tracking loop because the front distance history array isn’t re-initialized after the first detection. This, apparently, is a ‘feature’, not a bug – who knew! ;-).

After the second ‘stuck’ condition detection, the robot attempts to disengage using the ExecuteStuckRecoveryManeuver(), which, in this case tries to back up and then execute an ‘end-around’ maneuver to get past the chair leg. It finished the backup portion of the maneuver successfully with 23cm remaining rearward, and then executed a 90º ‘spin turn’. Then it went forward 21cm using the front distance sensor (not shown in the video), and halted when I took over manual control.

All in all, this was a very successful ‘sandbox’ run. Lots of good data with clear indications of where things are working well and where things need to be modified/fixed.

A bug in the telemetry display code for the ‘Wall Offset Limit’ detection printout (fixed).
In the situation at 7.65 sec where the obstacle detection occurred at 18cm vs 30cm, the robot should recognize that it needs to back up to the wall offset target before making the spin turn (done).
And, of course, porting all this new stuff to the right-side tracking sections

16 November 2020 Update:

Tonight I got the first cut done at porting the TRACKNG_LEFT algorithms over to the TRACKING_RIGHT case, and made what appears to be a successful right-side sandbox run, as shown in the following short video

And here is the telemetry from the run:

In RotateToParallelOrientation(Right)
6266	298	352	372	-0.67	127.00	0	254
6466	280	307	306	-0.26	59.72	67	186
Parallel Orientation Achieved at 6538 with SteeringVal = 0.01
WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	RFront	RCtr	RRear	Front	FrontVarSteer	Output	SetPt	LSpd	RSpd
6549	281	294	280	227	8728.96	0.01	-2.15	0.06	72	77
6554	281	294	280	227	8728.96	0.01	-2.15	0.06	72	77
6635	310	311	276	240	8520.67	0.34	-2.15	-0.11	72	77
6729	327	325	292	244	8381.43	0.35	75.00	-0.25	150	0
6828	342	337	295	240	8290.90	0.47	75.00	-0.30	150	0
6921	357	355	316	230	8240.07	0.41	75.00	-0.30	150	0
7018	360	338	328	222	8224.66	0.32	75.00	-0.30	150	0
7114	352	359	326	213	8233.74	0.26	16.46	-0.30	91	58
7211	349	356	335	206	8247.12	0.14	16.46	-0.30	91	58
7305	336	357	341	202	8234.86	-0.05	-61.79	-0.30	13	136
7402	334	349	342	199	8165.47	-0.08	-61.79	-0.30	13	136
7499	338	371	343	197	8004.15	-0.14	-4.31	-0.30	70	79
7593	345	353	335	195	7715.00	0.10	-4.31	-0.30	70	79
7695	344	359	332	192	7260.35	0.13	75.00	-0.30	150	0
7786	350	359	329	189	6597.09	0.21	75.00	-0.30	150	0
7881	347	374	338	182	5682.62	0.03	11.97	-0.30	86	63
7979	348	355	352	177	4460.83	-0.04	11.97	-0.30	86	63
8078	340	353	349	172	2874.73	-0.09	-5.40	-0.30	69	80
8170	343	362	352	170	2504.54	-0.09	-5.40	-0.30	69	80
8266	343	345	360	165	2110.81	-0.17	-1.01	-0.30	73	76
8360	352	364	342	162	1719.27	0.10	-1.01	-0.30	73	76
8460	341	369	353	158	1311.16	-0.12	43.53	-0.30	118	31
8557	354	370	351	156	926.95	0.03	43.53	-0.30	118	31
8651	344	361	359	152	772.95	-0.15	16.98	-0.30	91	58
8748	333	359	367	150	853.82	-0.34	16.98	-0.30	91	58
8845	334	342	360	147	895.27	-0.26	-17.90	-0.30	57	92
8940	329	362	351	143	927.53	-0.22	-17.90	-0.30	57	92
9037	317	357	342	139	900.06	-0.25	19.25	-0.30	94	55
9134	317	347	343	134	837.40	-0.26	19.25	-0.30	94	55
9231	315	338	332	131	767.64	-0.17	48.64	-0.30	123	26
9329	299	331	327	128	720.97	-0.28	48.64	-0.30	123	26
9426	287	334	334	125	688.38	-0.34	-43.48	-0.30	31	118
9526	288	322	334	110	719.70	-0.46	-43.48	-0.22	31	118
9621	274	313	315	109	749.19	-0.37	-22.20	-0.13	52	97
9720	264	299	301	115	743.06	-0.37	-22.20	0.01	52	97
9818	253	279	283	111	736.87	-0.26	-4.61	0.21	70	79
9914	245	270	268	110	716.83	-0.23	-4.61	0.30	70	79
10012	232	266	265	104	699.06	-0.37	-75.00	0.30	0	150
10109	224	253	248	100	679.92	-0.24	-75.00	0.30	0	150
10205	218	244	243	96	659.09	-0.25	-13.27	0.30	61	88
10301	204	221	225	91	655.60	-0.21	-13.27	0.30	61	88
10397	198	221	212	89	651.66	-0.15	-10.62	0.30	64	85
10496	199	213	207	84	658.13	-0.08	-10.62	0.30	64	85
10595	169	214	204	82	653.91	-0.23	-74.21	0.30	0	149
10691	196	198	203	81	646.92	-0.07	-74.21	0.30	0	149
10789	202	215	201	81	628.52	0.00	40.89	0.30	115	34
10889	214	226	199	79	609.31	0.15	31.44	0.30	106	43
10985	208	228	202	75	588.31	0.06	31.44	0.30	106	43
11083	225	233	206	70	575.19	0.19	2.11	0.30	77	72
11183	222	225	219	67	556.60	0.06	-62.61	0.30	12	137
11281	232	236	223	64	535.88	0.05	-62.61	0.30	12	137
11377	253	238	221	62	513.85	0.34	75.00	0.30	150	0
11475	281	278	231	61	488.39	0.43	75.00	0.22	150	0
11573	279	277	245	57	471.88	0.36	19.90	0.23	94	55
11672	287	291	261	51	463.43	0.26	19.90	0.09	94	55
11769	299	299	273	46	459.76	0.15	-47.68	0.01	27	122
11870	301	295	288	42	457.37	0.01	-44.80	0.05	30	119
11969	302	300	280	36	498.81	0.23	-44.80	0.00	30	119
12065	303	313	283	33	535.29	0.20	75.00	-0.13	150	0
12164	325	318	307	30	544.00	0.23	75.00	-0.18	150	0
12260	338	332	315	24	563.76	0.23	65.78	-0.30	140	9
bObstacleAhead TRUE in TRACKRIGHT at 12358 with frontdist 19, variance = 580.36
backing up to 30 using front distance sensor
backed up successfully to 33 using front distance sensor
In SpinTurn(CCW, 90.00)
Msec	Hdg	PrevHdg	Rate	Set	Out	Spd
13039	0.17	-1.08	12.95	45.00	200.00	0
13136	5.51	0.17	56.96	45.00	200.00	0
13234	11.65	5.51	64.88	45.00	200.00	0
13331	15.61	11.65	42.26	45.00	200.00	0
13428	16.21	15.61	6.30	45.00	-200.00	200
13528	18.80	16.21	26.83	45.00	200.00	0
13626	28.55	18.80	102.85	45.00	200.00	0
13725	33.06	28.55	46.58	45.00	200.00	0
13825	33.04	33.06	0.16	45.00	-200.00	200
13921	35.82	33.04	29.69	45.00	-200.00	255
14021	48.68	35.82	132.80	45.00	200.00	55
14117	64.01	48.68	165.27	45.00	200.00	0
14213	75.93	64.01	128.49	45.00	-13.74	13
14310	84.24	75.93	88.53	45.00	-13.74	26
14313: Exiting SpinTurn() at 84.24 deg
In RotateToParallelOrientation(Right)
Parallel Orientation Achieved at 14504 with SteeringVal = 0.10
WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
14516	391	398	376	108	11084.51	0.10	5.98	-0.30	80	69
14521	391	398	376	108	11084.51	0.10	5.98	-0.30	80	69
14605	388	400	384	106	8356.08	0.04	5.98	-0.30	80	69
14704	380	395	384	105	5067.11	0.15	66.33	-0.30	141	8
14801	402	394	383	100	1142.00	0.19	66.33	-0.30	141	8
14883	398	404	393	98	1092.24	0.04	8.35	-0.30	83	66
14980	377	402	397	97	1019.28	-0.20	8.35	-0.30	83	66
15076	384	412	399	95	935.28	-0.15	-34.20	-0.30	40	109
15174	378	392	395	92	837.84	-0.17	-34.20	-0.30	40	109
15271	370	402	395	90	748.36	-0.15	23.25	-0.30	98	51
15369	384	391	387	87	664.72	-0.03	23.25	-0.30	98	51
15464	378	394	389	86	579.21	-0.15	23.70	-0.30	98	51
15563	374	394	388	82	504.21	-0.14	23.70	-0.30	98	51
15662	360	392	393	79	448.40	-0.29	-32.27	-0.30	42	107
15760	347	391	383	75	398.56	-0.36	-32.27	-0.30	42	107
15859	325	375	376	71	354.21	-0.40	-34.57	-0.30	40	109
15958	325	341	365	68	324.42	-0.40	-34.57	-0.30	40	109
16056	331	337	347	65	296.47	-0.14	75.00	-0.30	150	0
16154	332	343	327	62	284.88	0.05	75.00	-0.30	150	0
16253	330	353	332	60	275.08	0.07	75.00	-0.30	150	0
16351	329	337	341	58	284.24	-0.12	75.00	-0.30	150	0
16447	324	345	350	55	326.19	-0.28	-75.00	-0.30	0	150
16545	317	352	367	54	343.76	-0.40	-75.00	-0.30	0	150
16648	314	349	354	51	364.72	-0.38	-28.16	-0.30	46	103
16746	306	334	343	43	390.02	-0.34	-28.16	-0.30	46	103
16843	297	328	325	39	412.51	-0.28	39.84	-0.28	114	35
16941	286	321	314	33	442.75	-0.21	39.84	-0.21	114	35
17039	287	310	312	28	476.95	-0.25	3.39	-0.10	78	71
Hit Wall Offset Limit (30 cm) at 17138 with frontdist = 25, Setpoint = -0.10
In StepTurn(Tracking Right)
In SpinTurn(CCW, 90.00)
Msec	Hdg	PrevHdg	Rate	Set	Out	Spd
17236	67.82	67.35	5.02	45.00	-200.00	255
17335	75.00	67.82	75.00	45.00	-200.00	255
17438	91.46	75.00	164.54	45.00	200.00	55
17537	113.79	91.46	235.10	45.00	200.00	0
17633	134.03	113.79	217.56	45.00	200.00	0
17732	146.96	134.03	134.52	45.00	200.00	0
17831	151.31	146.96	45.43	45.00	-200.00	200
17835: Exiting SpinTurn() at 151.31 deg
In RotateToParallelOrientation(Right)
18134	126	157	159	-0.38	86.02	40	213
Parallel Orientation Achieved at 18321 with SteeringVal = -0.02
WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec	LFront	LCtr	LRear	Front	FrontVar	Rear	Steer	Output	SetPt	LSpd	RSpd
18332	131	154	128	321	5983.33	-0.02	45.43	0.30	120	29
18337	131	154	128	321	5983.33	-0.02	45.43	0.30	120	29
18418	135	158	133	206	6668.69	0.02	45.43	0.30	120	29
18512	144	154	140	208	7308.87	-0.01	-20.50	0.30	54	95
18611	146	157	140	313	9298.40	0.06	-20.50	0.30	54	95
18706	150	161	143	311	11046.16	0.01	-16.37	0.30	58	91
18800	153	163	146	308	12536.56	0.07	-16.37	0.30	58	91
18897	157	165	144	270	13305.75	0.13	25.12	0.30	100	49
18991	162	179	156	268	13898.79	0.06	25.12	0.30	100	49
19087	162	177	159	266	14304.01	0.03	-51.69	0.30	23	126
19182	162	173	161	261	14450.51	0.01	-51.69	0.30	23	126
19278	173	180	162	213	14096.38	0.11	9.74	0.30	84	65
19376	179	189	162	191	13513.93	0.17	9.74	0.30	84	65
19472	186	203	174	187	12789.90	0.12	-10.80	0.30	64	85
19569	194	196	183	184	11946.82	0.11	-10.80	0.30	64	85
19665	197	201	184	177	11022.66	0.13	-10.31	0.30	64	85
19761	204	202	185	174	9907.81	0.19	-10.31	0.30	64	85
19858	211	215	198	172	8758.23	0.13	-13.82	0.30	61	88
19953	212	216	207	166	7632.49	0.05	-13.82	0.30	61	88
20050	218	228	205	179	6549.61	0.13	-14.33	0.30	60	89
20147	232	232	213	178	5714.13	0.19	-14.33	0.30	60	89
20242	234	238	221	176	4935.26	0.13	-14.84	0.30	60	89
20336	249	243	227	172	4144.19	0.13	-14.84	0.30	60	89
20432	252	260	235	171	3394.58	0.17	0.77	0.30	75	74
20531	265	269	238	171	3167.90	0.27	28.68	0.30	103	46
20626	270	280	247	165	2857.96	0.23	28.68	0.20	103	46
20722	280	275	255	160	2505.05	0.25	-4.32	0.25	70	79
20818	281	281	269	153	2629.37	0.12	-4.32	0.19	70	79
20918	286	290	269	147	2771.09	0.17	-15.11	0.10	59	90
21017	289	273	275	144	2412.00	0.14	-15.11	0.27	59	90
21114	303	304	283	141	1992.57	0.20	35.61	-0.04	110	39
21209	307	313	289	138	1521.46	0.18	35.61	-0.13	110	39
21306	302	314	298	131	1317.75	0.04	-26.85	-0.14	48	101
21402	310	321	303	129	1074.44	0.07	-26.85	-0.21	48	101
21500	326	310	308	127	791.55	0.18	73.99	-0.10	148	1
21599	319	341	309	123	499.21	0.10	73.99	-0.30	148	1
21696	317	335	315	112	484.99	0.02	-11.05	-0.30	63	86
21790	319	334	318	109	531.44	0.01	-11.05	-0.30	63	86
21890	317	328	322	105	582.61	-0.05	7.64	-0.28	82	67
21985	325	322	326	102	631.94	-0.01	7.64	-0.22	82	67
22080	328	341	327	99	690.09	0.01	55.57	-0.30	130	19
22176	329	335	328	98	740.19	-0.02	55.57	-0.30	130	19
22275	333	343	324	176	750.18	-0.04	18.35	-0.30	93	56
22368	320	348	335	175	769.94	-0.18	18.35	-0.30	93	56
22465	316	352	329	174	756.59	-0.09	13.98	-0.30	88	61
22563	313	331	337	170	736.67	-0.28	13.98	-0.30	88	61
22658	309	328	332	167	715.82	-0.32	-65.14	-0.28	9	140
22753	305	329	328	164	699.31	-0.30	-65.14	-0.29	9	140
22851	286	314	320	159	677.04	-0.22	29.62	-0.14	104	45
22944	287	302	308	155	649.19	-0.18	29.62	-0.02	104	45
23043	285	306	310	151	629.75	-0.25	-20.95	-0.06	54	95
23141	282	312	296	145	614.81	-0.14	-20.95	-0.12	54	95
23236	269	291	295	141	607.63	-0.16	8.30	0.09	83	66
23333	278	299	287	137	605.30	-0.08	8.30	0.01	83	66
23429	307	286	284	134	605.04	0.11	75.00	0.14	150	0
23527	502	292	280	128	608.91	1.64	75.00	0.08	150	0
23626	781	316	285	127	613.53	5.28	75.00	-0.16	150	0
23721	791	326	290	128	615.44	4.93	75.00	-0.26	150	0
23821	564	319	308	131	614.25	3.09	-75.00	-0.19	0	150
23918	402	325	329	109	641.78	0.77	-75.00	-0.25	0	150
24013	558	352	317	107	669.23	2.03	-45.64	-0.30	29	120
24113	703	467	317	126	649.74	3.33	75.00	-0.30	150	0
24213	850	709	481	118	632.92	3.69	75.00	-0.30	150	0
24311	811	842	622	115	611.13	1.99	75.00	-0.30	150	0
24407	797	835	649	94	636.27	1.87	-75.00	-0.30	0	150
24502	817	732	591	89	670.60	2.44	-75.00	-0.30	0	150
24601	830	801	642	83	725.71	1.88	75.00	-0.30	150	0
24696	248	281	290	78	  781.84	8190	-0.42	75.00	0.19	0	150
24795	272	264	266	75	  823.44	8190	0.06	-75.00	0.30	150	0
24892	327	273	258	71	  853.43	8191	0.69	-75.00	0.27	150	0
24988	542	379	269	68	  878.24	8190	2.73	75.00	-0.30	0	150
25086	676	673	348	63	  902.54	8190	3.28	75.00	-0.30	0	150
25185	696	713	571	93	  822.56	8190	1.25	-75.00	-0.30	150	0
25280	696	711	633	89	  753.12	8191	0.63	-75.00	-0.30	150	0
25379	667	717	667	88	  685.37	574	0.33	-75.00	-0.30	150	0
25479	673	650	667	60	  686.83	8191	0.06	30.00	-0.30	45	105
25577	742	736	643	61	  681.45	8190	0.99	30.00	-0.30	45	105
25674	796	782	673	54	  690.36	8190	1.23	75.00	-0.30	0	150
25773	830	810	694	48	  710.01	8190	1.29	75.00	-0.30	0	150
25869	582	643	721	37	756.88	-1.08	-75.00	-0.30	0	150
25966	545	579	615	34	807.42	-0.70	-75.00	-0.30	0	150
26066	493	523	553	33	837.20	-0.56	75.00	-0.30	150	0
26166	468	488	499	22	879.26	-0.31	75.00	-0.30	150	0
26262	446	499	502	25	859.92	-0.64	75.00	-0.30	150	0
26361	441	523	508	23	900.38	-0.67	-74.26	-0.30	0	149
26459	467	519	544	22	927.31	-0.90	-74.26	-0.30	0	149
bObstacleAhead TRUE in TRACKRIGHT at 26556 with frontdist 15, variance = 874.38
backing up to 30 using front distance sensor
backed up successfully to 35 using front distance sensor
In SpinTurn(CCW, 90.00)
Msec	Hdg	PrevHdg	Rate	Set	Out	Spd

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

In RotateToParallelOrientation(Right)

6266 298 352 372 -0.67 127.00 0 254

6466 280 307 306 -0.26 59.72 67 186

Parallel Orientation Achieved at 6538 with SteeringVal = 0.01

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec RFront RCtr RRear Front FrontVarSteer Output SetPt LSpd RSpd

6549 281 294 280 227 8728.96 0.01 -2.15 0.06 72 77

6554 281 294 280 227 8728.96 0.01 -2.15 0.06 72 77

6635 310 311 276 240 8520.67 0.34 -2.15 -0.11 72 77

6729 327 325 292 244 8381.43 0.35 75.00 -0.25 150 0

6828 342 337 295 240 8290.90 0.47 75.00 -0.30 150 0

6921 357 355 316 230 8240.07 0.41 75.00 -0.30 150 0

7018 360 338 328 222 8224.66 0.32 75.00 -0.30 150 0

7114 352 359 326 213 8233.74 0.26 16.46 -0.30 91 58

7211 349 356 335 206 8247.12 0.14 16.46 -0.30 91 58

7305 336 357 341 202 8234.86 -0.05 -61.79 -0.30 13 136

7402 334 349 342 199 8165.47 -0.08 -61.79 -0.30 13 136

7499 338 371 343 197 8004.15 -0.14 -4.31 -0.30 70 79

7593 345 353 335 195 7715.00 0.10 -4.31 -0.30 70 79

7695 344 359 332 192 7260.35 0.13 75.00 -0.30 150 0

7786 350 359 329 189 6597.09 0.21 75.00 -0.30 150 0

7881 347 374 338 182 5682.62 0.03 11.97 -0.30 86 63

7979 348 355 352 177 4460.83 -0.04 11.97 -0.30 86 63

8078 340 353 349 172 2874.73 -0.09 -5.40 -0.30 69 80

8170 343 362 352 170 2504.54 -0.09 -5.40 -0.30 69 80

8266 343 345 360 165 2110.81 -0.17 -1.01 -0.30 73 76

8360 352 364 342 162 1719.27 0.10 -1.01 -0.30 73 76

8460 341 369 353 158 1311.16 -0.12 43.53 -0.30 118 31

8557 354 370 351 156 926.95 0.03 43.53 -0.30 118 31

8651 344 361 359 152 772.95 -0.15 16.98 -0.30 91 58

8748 333 359 367 150 853.82 -0.34 16.98 -0.30 91 58

8845 334 342 360 147 895.27 -0.26 -17.90 -0.30 57 92

8940 329 362 351 143 927.53 -0.22 -17.90 -0.30 57 92

9037 317 357 342 139 900.06 -0.25 19.25 -0.30 94 55

9134 317 347 343 134 837.40 -0.26 19.25 -0.30 94 55

9231 315 338 332 131 767.64 -0.17 48.64 -0.30 123 26

9329 299 331 327 128 720.97 -0.28 48.64 -0.30 123 26

9426 287 334 334 125 688.38 -0.34 -43.48 -0.30 31 118

9526 288 322 334 110 719.70 -0.46 -43.48 -0.22 31 118

9621 274 313 315 109 749.19 -0.37 -22.20 -0.13 52 97

9720 264 299 301 115 743.06 -0.37 -22.20 0.01 52 97

9818 253 279 283 111 736.87 -0.26 -4.61 0.21 70 79

9914 245 270 268 110 716.83 -0.23 -4.61 0.30 70 79

10012 232 266 265 104 699.06 -0.37 -75.00 0.30 0 150

10109 224 253 248 100 679.92 -0.24 -75.00 0.30 0 150

10205 218 244 243 96 659.09 -0.25 -13.27 0.30 61 88

10301 204 221 225 91 655.60 -0.21 -13.27 0.30 61 88

10397 198 221 212 89 651.66 -0.15 -10.62 0.30 64 85

10496 199 213 207 84 658.13 -0.08 -10.62 0.30 64 85

10595 169 214 204 82 653.91 -0.23 -74.21 0.30 0 149

10691 196 198 203 81 646.92 -0.07 -74.21 0.30 0 149

10789 202 215 201 81 628.52 0.00 40.89 0.30 115 34

10889 214 226 199 79 609.31 0.15 31.44 0.30 106 43

10985 208 228 202 75 588.31 0.06 31.44 0.30 106 43

11083 225 233 206 70 575.19 0.19 2.11 0.30 77 72

11183 222 225 219 67 556.60 0.06 -62.61 0.30 12 137

11281 232 236 223 64 535.88 0.05 -62.61 0.30 12 137

11377 253 238 221 62 513.85 0.34 75.00 0.30 150 0

11475 281 278 231 61 488.39 0.43 75.00 0.22 150 0

11573 279 277 245 57 471.88 0.36 19.90 0.23 94 55

11672 287 291 261 51 463.43 0.26 19.90 0.09 94 55

11769 299 299 273 46 459.76 0.15 -47.68 0.01 27 122

11870 301 295 288 42 457.37 0.01 -44.80 0.05 30 119

11969 302 300 280 36 498.81 0.23 -44.80 0.00 30 119

12065 303 313 283 33 535.29 0.20 75.00 -0.13 150 0

12164 325 318 307 30 544.00 0.23 75.00 -0.18 150 0

12260 338 332 315 24 563.76 0.23 65.78 -0.30 140 9

bObstacleAhead TRUE in TRACKRIGHT at 12358 with frontdist 19, variance = 580.36

backing up to 30 using front distance sensor

backed up successfully to 33 using front distance sensor

In SpinTurn(CCW, 90.00)

Msec Hdg PrevHdg Rate Set Out Spd

13039 0.17 -1.08 12.95 45.00 200.00 0

13136 5.51 0.17 56.96 45.00 200.00 0

13234 11.65 5.51 64.88 45.00 200.00 0

13331 15.61 11.65 42.26 45.00 200.00 0

13428 16.21 15.61 6.30 45.00 -200.00 200

13528 18.80 16.21 26.83 45.00 200.00 0

13626 28.55 18.80 102.85 45.00 200.00 0

13725 33.06 28.55 46.58 45.00 200.00 0

13825 33.04 33.06 0.16 45.00 -200.00 200

13921 35.82 33.04 29.69 45.00 -200.00 255

14021 48.68 35.82 132.80 45.00 200.00 55

14117 64.01 48.68 165.27 45.00 200.00 0

14213 75.93 64.01 128.49 45.00 -13.74 13

14310 84.24 75.93 88.53 45.00 -13.74 26

14313: Exiting SpinTurn() at 84.24 deg

In RotateToParallelOrientation(Right)

Parallel Orientation Achieved at 14504 with SteeringVal = 0.10

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

14516 391 398 376 108 11084.51 0.10 5.98 -0.30 80 69

14521 391 398 376 108 11084.51 0.10 5.98 -0.30 80 69

14605 388 400 384 106 8356.08 0.04 5.98 -0.30 80 69

14704 380 395 384 105 5067.11 0.15 66.33 -0.30 141 8

14801 402 394 383 100 1142.00 0.19 66.33 -0.30 141 8

14883 398 404 393 98 1092.24 0.04 8.35 -0.30 83 66

14980 377 402 397 97 1019.28 -0.20 8.35 -0.30 83 66

15076 384 412 399 95 935.28 -0.15 -34.20 -0.30 40 109

15174 378 392 395 92 837.84 -0.17 -34.20 -0.30 40 109

15271 370 402 395 90 748.36 -0.15 23.25 -0.30 98 51

15369 384 391 387 87 664.72 -0.03 23.25 -0.30 98 51

15464 378 394 389 86 579.21 -0.15 23.70 -0.30 98 51

15563 374 394 388 82 504.21 -0.14 23.70 -0.30 98 51

15662 360 392 393 79 448.40 -0.29 -32.27 -0.30 42 107

15760 347 391 383 75 398.56 -0.36 -32.27 -0.30 42 107

15859 325 375 376 71 354.21 -0.40 -34.57 -0.30 40 109

15958 325 341 365 68 324.42 -0.40 -34.57 -0.30 40 109

16056 331 337 347 65 296.47 -0.14 75.00 -0.30 150 0

16154 332 343 327 62 284.88 0.05 75.00 -0.30 150 0

16253 330 353 332 60 275.08 0.07 75.00 -0.30 150 0

16351 329 337 341 58 284.24 -0.12 75.00 -0.30 150 0

16447 324 345 350 55 326.19 -0.28 -75.00 -0.30 0 150

16545 317 352 367 54 343.76 -0.40 -75.00 -0.30 0 150

16648 314 349 354 51 364.72 -0.38 -28.16 -0.30 46 103

16746 306 334 343 43 390.02 -0.34 -28.16 -0.30 46 103

16843 297 328 325 39 412.51 -0.28 39.84 -0.28 114 35

16941 286 321 314 33 442.75 -0.21 39.84 -0.21 114 35

17039 287 310 312 28 476.95 -0.25 3.39 -0.10 78 71

Hit Wall Offset Limit (30 cm) at 17138 with frontdist = 25, Setpoint = -0.10

In StepTurn(Tracking Right)

In SpinTurn(CCW, 90.00)

Msec Hdg PrevHdg Rate Set Out Spd

17236 67.82 67.35 5.02 45.00 -200.00 255

17335 75.00 67.82 75.00 45.00 -200.00 255

17438 91.46 75.00 164.54 45.00 200.00 55

17537 113.79 91.46 235.10 45.00 200.00 0

17633 134.03 113.79 217.56 45.00 200.00 0

17732 146.96 134.03 134.52 45.00 200.00 0

17831 151.31 146.96 45.43 45.00 -200.00 200

17835: Exiting SpinTurn() at 151.31 deg

In RotateToParallelOrientation(Right)

18134 126 157 159 -0.38 86.02 40 213

Parallel Orientation Achieved at 18321 with SteeringVal = -0.02

WallOffsetTrackPID Parameters (Kp,Ki,Kd) = (100,30,30)

Msec LFront LCtr LRear Front FrontVar Rear Steer Output SetPt LSpd RSpd

18332 131 154 128 321 5983.33 -0.02 45.43 0.30 120 29

18337 131 154 128 321 5983.33 -0.02 45.43 0.30 120 29

18418 135 158 133 206 6668.69 0.02 45.43 0.30 120 29

18512 144 154 140 208 7308.87 -0.01 -20.50 0.30 54 95

18611 146 157 140 313 9298.40 0.06 -20.50 0.30 54 95

18706 150 161 143 311 11046.16 0.01 -16.37 0.30 58 91

18800 153 163 146 308 12536.56 0.07 -16.37 0.30 58 91

18897 157 165 144 270 13305.75 0.13 25.12 0.30 100 49

18991 162 179 156 268 13898.79 0.06 25.12 0.30 100 49

19087 162 177 159 266 14304.01 0.03 -51.69 0.30 23 126

19182 162 173 161 261 14450.51 0.01 -51.69 0.30 23 126

19278 173 180 162 213 14096.38 0.11 9.74 0.30 84 65

19376 179 189 162 191 13513.93 0.17 9.74 0.30 84 65

19472 186 203 174 187 12789.90 0.12 -10.80 0.30 64 85

19569 194 196 183 184 11946.82 0.11 -10.80 0.30 64 85

19665 197 201 184 177 11022.66 0.13 -10.31 0.30 64 85

19761 204 202 185 174 9907.81 0.19 -10.31 0.30 64 85

19858 211 215 198 172 8758.23 0.13 -13.82 0.30 61 88

19953 212 216 207 166 7632.49 0.05 -13.82 0.30 61 88

20050 218 228 205 179 6549.61 0.13 -14.33 0.30 60 89

20147 232 232 213 178 5714.13 0.19 -14.33 0.30 60 89

20242 234 238 221 176 4935.26 0.13 -14.84 0.30 60 89

20336 249 243 227 172 4144.19 0.13 -14.84 0.30 60 89

20432 252 260 235 171 3394.58 0.17 0.77 0.30 75 74

20531 265 269 238 171 3167.90 0.27 28.68 0.30 103 46

20626 270 280 247 165 2857.96 0.23 28.68 0.20 103 46

20722 280 275 255 160 2505.05 0.25 -4.32 0.25 70 79

20818 281 281 269 153 2629.37 0.12 -4.32 0.19 70 79

20918 286 290 269 147 2771.09 0.17 -15.11 0.10 59 90

21017 289 273 275 144 2412.00 0.14 -15.11 0.27 59 90

21114 303 304 283 141 1992.57 0.20 35.61 -0.04 110 39

21209 307 313 289 138 1521.46 0.18 35.61 -0.13 110 39

21306 302 314 298 131 1317.75 0.04 -26.85 -0.14 48 101

21402 310 321 303 129 1074.44 0.07 -26.85 -0.21 48 101

21500 326 310 308 127 791.55 0.18 73.99 -0.10 148 1

21599 319 341 309 123 499.21 0.10 73.99 -0.30 148 1

21696 317 335 315 112 484.99 0.02 -11.05 -0.30 63 86

21790 319 334 318 109 531.44 0.01 -11.05 -0.30 63 86

21890 317 328 322 105 582.61 -0.05 7.64 -0.28 82 67

21985 325 322 326 102 631.94 -0.01 7.64 -0.22 82 67

22080 328 341 327 99 690.09 0.01 55.57 -0.30 130 19

22176 329 335 328 98 740.19 -0.02 55.57 -0.30 130 19

22275 333 343 324 176 750.18 -0.04 18.35 -0.30 93 56

22368 320 348 335 175 769.94 -0.18 18.35 -0.30 93 56

22465 316 352 329 174 756.59 -0.09 13.98 -0.30 88 61

22563 313 331 337 170 736.67 -0.28 13.98 -0.30 88 61

22658 309 328 332 167 715.82 -0.32 -65.14 -0.28 9 140

22753 305 329 328 164 699.31 -0.30 -65.14 -0.29 9 140

22851 286 314 320 159 677.04 -0.22 29.62 -0.14 104 45

22944 287 302 308 155 649.19 -0.18 29.62 -0.02 104 45

23043 285 306 310 151 629.75 -0.25 -20.95 -0.06 54 95

23141 282 312 296 145 614.81 -0.14 -20.95 -0.12 54 95

23236 269 291 295 141 607.63 -0.16 8.30 0.09 83 66

23333 278 299 287 137 605.30 -0.08 8.30 0.01 83 66

23429 307 286 284 134 605.04 0.11 75.00 0.14 150 0

23527 502 292 280 128 608.91 1.64 75.00 0.08 150 0

23626 781 316 285 127 613.53 5.28 75.00 -0.16 150 0

23721 791 326 290 128 615.44 4.93 75.00 -0.26 150 0

23821 564 319 308 131 614.25 3.09 -75.00 -0.19 0 150

23918 402 325 329 109 641.78 0.77 -75.00 -0.25 0 150

24013 558 352 317 107 669.23 2.03 -45.64 -0.30 29 120

24113 703 467 317 126 649.74 3.33 75.00 -0.30 150 0

24213 850 709 481 118 632.92 3.69 75.00 -0.30 150 0

24311 811 842 622 115 611.13 1.99 75.00 -0.30 150 0

24407 797 835 649 94 636.27 1.87 -75.00 -0.30 0 150

24502 817 732 591 89 670.60 2.44 -75.00 -0.30 0 150

24601 830 801 642 83 725.71 1.88 75.00 -0.30 150 0

24696 248 281 290 78 781.84 8190 -0.42 75.00 0.19 0 150

24795 272 264 266 75 823.44 8190 0.06 -75.00 0.30 150 0

24892 327 273 258 71 853.43 8191 0.69 -75.00 0.27 150 0

24988 542 379 269 68 878.24 8190 2.73 75.00 -0.30 0 150

25086 676 673 348 63 902.54 8190 3.28 75.00 -0.30 0 150

25185 696 713 571 93 822.56 8190 1.25 -75.00 -0.30 150 0

25280 696 711 633 89 753.12 8191 0.63 -75.00 -0.30 150 0

25379 667 717 667 88 685.37 574 0.33 -75.00 -0.30 150 0

25479 673 650 667 60 686.83 8191 0.06 30.00 -0.30 45 105

25577 742 736 643 61 681.45 8190 0.99 30.00 -0.30 45 105

25674 796 782 673 54 690.36 8190 1.23 75.00 -0.30 0 150

25773 830 810 694 48 710.01 8190 1.29 75.00 -0.30 0 150

25869 582 643 721 37 756.88 -1.08 -75.00 -0.30 0 150

25966 545 579 615 34 807.42 -0.70 -75.00 -0.30 0 150

26066 493 523 553 33 837.20 -0.56 75.00 -0.30 150 0

26166 468 488 499 22 879.26 -0.31 75.00 -0.30 150 0

26262 446 499 502 25 859.92 -0.64 75.00 -0.30 150 0

26361 441 523 508 23 900.38 -0.67 -74.26 -0.30 0 149

26459 467 519 544 22 927.31 -0.90 -74.26 -0.30 0 149

bObstacleAhead TRUE in TRACKRIGHT at 26556 with frontdist 15, variance = 874.38

backing up to 30 using front distance sensor

backed up successfully to 35 using front distance sensor

In SpinTurn(CCW, 90.00)

Msec Hdg PrevHdg Rate Set Out Spd

Here’s an Excel plot of the Right side center distance and L/R motor speeds vs time.

Comparing the times from the video and the telemetry, it appears the video time is about 3-3.5 sec lower than the telemetry values. In the video, Wall-E2 detects the first upcoming wall at about 9 sec, and this corresponds with the telemetry at 12.358 where the wall is detected with front distance of 19cm. The reason the wall didn’t get detected earlier is the robot was currently tracking back toward the wall with a steering value of -0.3, and this causes the wall detection value to be reduced to suppress false positives.

The robot then takes 2 sec to back up to 33 cm and turn 90 deg CCW, and then it starts tracking the right-side wall again. It detects the next wall at 17.1 sec (14 sec in video) and 30 cm (the steering value at that point was 0.10, so no reduction in upcoming obstacle detection distance). Because the detection occurred at 30 cm, the robot doesn’t need to back up; it just makes another 90 deg spin turn CCW and starts tracking the right-side wall again. The last segment clearly shows that Wall-E2 is capable of tracking to and capturing the desired offset of 30 cm.

All in all, a very successful right-side sandbox run.

Stay tuned!

Frank

Adafruit DS3231 Module vs generic ZS-042 Module

Leave a reply

Posted 30 October 2020,

Back in May of 2018, well over 2 years ago, I posted about adding an Adafruit DS3231 RTC module to Wall-E2, my autonomous wall-following robot project. This addition went swimmingly until about 6 months later in September of 2018 when I posted to the Adafruit support forum, saying that I was having trouble with the ‘lostPower()’ function return values; it seems like it was returning FALSE (no power loss) even though I had removed the battery and turned off the power to the system. As described in the post, I eventually gave up on this in February of 2019after discovering that I was getting radically different results when I used a different Arduino Mega and two different Adafruit DS3231 modules. Eventually I wound up in the situation where both DS3231 modules appeared to work correctly no matter what I did – strange!

Fast-forward to the present. In the process of adding a rear distance sensor to Wall-E2, I once again ran across the same anomalous behavior by the Adafruit DS3231 RTC module; The ‘lostPower()’ function stubbornly refused to declare a loss of power, even with the battery removed and the main power turned off. After a lot more investigation, including a dedicated test program and some more back-and-forth on the Adafruit forum, I (and the Adafruit support guys) still was unable to resolve the issue.

In desperation, I fished a generic ‘ZS-042’ DS3231 RTC module out of my parts bin and started working with it, thinking maybe I could use it to get a clue why the Adafruit modules were failing. As it turned out, the ZS-042 module worked perfectly from the get-go with the Adafruit RTC library, and the ‘lostPower()’ function correctly returned TRUE when main power was lost with the battery removed, and FALSE when power was lost but the battery was in place.

Here are some photos of the Adafruit and ZS-042 modules:

As can be readily seen, the ZS-042 module is considerably larger, due almost entirely to the decision to use the LIR-2032 Li-ion rechargeable cell instead of the smaller non-rechargeable CR1220 type. Other differences:

The ZS-042 module includes a power LED. This LED illuminates when main power is available on the VCC pin, but not when the RTC module is running from the battery
The Adafruit module exposes the RST (reset) line. If you need this, the ZS-042 won’t work for you.
When used with the supplied LIR2032, the battery is recharged and/or float-charged from VCC through a 1N4148 diode. This works fine if VCC is 5V, but doesn’t work at all if VCC is 3.3V.
The 32KHz output is open-drain, without a pullup on both the Adafruit module, but the ZS-042 module has a pullup to VCC. What this means in practice is you can’t easily monitor this output when operating off the battery, so it is hard to tell if the RTC module is still running. My solution to that was to attach a completely separate power supply to the 32KHz output via a 10K pullup resistor. The Adafruit module needs this to see the 32KHz output for both battery power and mains power. The ZS-042 module only needs it for battery power.

Adafruit module with temporary 10K pullup resistor installed. Note clock scope trace in background

ZS-042 module with main power applied to USB connector. 32KHz output is present even without an external pullup

Same setup but with USB connector removed. Now need a 10K external pullup to an external supply to monitor 32KHz clock

So, there you have it. The Adafruit module is smaller, has an additional output (RST) and uses a smaller, non-rechargeable CR2210 button cell. However, in my testing and use over a two-year period, I came to distrust its ability to reliably detect and report on complete power loss situations that would require a forced date/time update.

The ZS-042 module is significantly larger due to its use of the rechargeable Li-ion LIR2032 button cell, and doesn’t have the RST output. It is also considerably cheaper and widely available. Lastly, it appears to more reliably report complete power loss occurrences, allowing proper date/time updates.

For my money, I have replaced the Adafruit DS3231 module in my system with the ZS-042 module. In practice, complete RTC power failure events are very rare, so in all probability there would be no appreciable difference between the two choices. However, for those applications (like mine) where you really do want to know if the RTC loses its sense of time, I don’t feel comfortable with the Adafruit module.

If anyone has a better understanding of the Adafruit module, please feel free to comment.

30 October 2020 Update

I replaced the Adafruit DS3231 RTC module on my Wall-E2 autonomous wall-following robot with the ZS-042 DS3231 RTC module. As shown in the following photos, I had to re-arrange the I2C FRAM and I2C MPU6050 IMU modules in order to make room for the significantly larger ZS-042 module.

Original layout. Adafruit RTC module on left, MPU6050 IMU in center, FRAM on right

Straight replacement not going to work – oops!

Stay tuned,

Frank

Adding a VL53L0X Rear Distance Sensor to Wall-E2

1 Reply

posted 24 October 2020

After documenting left-side wall-tracking success with Wall-E2, my autonomous wall-tracking robot (see this post and this post), I started thinking about improving Wall-E2’s obstacle avoidance performance.

Wall-E2 can encounter several distinct obstacle situations during wall tracking operations. In the simplest case, Wall-E2 approaches an upcoming corner while tracking a wall, and needs to know how to transition from tracking the current wall to tracking the upcoming wall. A more difficult situation arises when Wall-E2 is ‘stuck’ – prevented from moving forward by an obstacle that isn’t detected by its front LIDAR distance sensor; a shoe, or the curved foot of a coat rack. A third situation arises when Wall-E2 encounters an obstacle that just wasn’t there a second ago; a cat or a human foot or a bag of groceries.

In the simple wall-to-wall transition case, all Wall-E2 has to do is make a right-angle turn away from the current wall and start following the next wall; this was successfully demonstrated several times in the previous posts. This maneuver utilizes a ‘spin-turn’ technique intended to minimize the backward movement of the robot while turning. This is done to prevent Wall-E2 from backing into the currently-tracked wall while attempting to turn toward and track the upcoming wall. Unfortunately, this maneuver is not always successful, whereupon Wall-E2 tries to climb backwards up the current wall, often with disastrous results.

In the ‘stuck’ case, Wall-E2 has to first recognize that it is no longer moving forward (or in any other direction for that matter), and then figure out what to do about it. Detection is accomplished by looking at the variance of front distance measurements over time; the ‘stuck’ condition is declared when the front-distance variance falls below a pre-determined value. A typical ‘stuck’ recovery maneuver is to back up slightly, and then make a right-angle turn away from the wall currently being tracked. This maneuver, while usually successful, has the same problem as the simple wall-to-wall transition; it sometimes results in the same backward-up-the-wall climb, with similar results.

The ‘suddenly appearing obstacle’ case can be handled in a manner similar to ‘stuck’ detection, but bypassing the variance measurement stage. and the resulting avoidance maneuver is similar to the ‘stuck’ case

Wall-E2 currently handles all of the above cases fairly well, except when it backs into something while maneuvering to avoid the detected obstacle. So, my challenge was to find a way to avoid running into something while backup up from something else. The easy answer to this problem was to add a rear-distance sensor to Wall-E2, and then use that information to modify obstacle-avoidance behavior as necessary.

During the changeover from ‘ping’ style distance sensors to left and right 3-element arrays of VL53L0X time-of-flight sensors I learned quite a bit about the care and feeding of the VL53L0X, and also wound up with quite a few spares. So, I took one of the spares and installed it on the rear ‘bumper’ plate on Wall-E2, as shown in the following photo:

Since the 2nd-deck Teensy 3.5 was already handling both 3-element VL53L0X arrays, I simply added the rear sensor to the left-hand array ‘Wire2’ daisy-chain, and connected its XSHUT pin to Teensy pin 8. Then I modified the Teensy’s program to initialize and poll the rear sensor in the same manner as all the others, and tested it to make sure it was responding properly to rear-aspect obstacles.

The next step is to incorporate rear-aspect distance information into the various obstacle avoidance algorithms in the main program.

‘Stuck’ case:

The ‘stuck’ case by definition occurs when the mathematical variance of the last 3-5 seconds of forward distance measurements fall below a set value, indicating that the robot is no longer moving forward or backward. When this happens while wall tracking, the robot has to decide what to do. The current response is to back up for 1 second at half speed, execute a 90 deg ‘spin turn’ away from the nearest wall and then go back to normal operations.

I think I would like to enhance this algorithm as follows:

If the measured front distance is less than MAX_FRONT_DISTANCE_CM (currently set at 400 cm) by at least STUCK_BACKUP_DISTANCE_CM (currently set at 25), then back up by STUCK_BACKUP_DISTANCE_CM using front distance measurements as the primary means of terminating the backup maneuver. If the front distance measurement cannot be used, but the rear distance measurement is valid (less than MAX_REAR_DISTANCE_CM, currently set at 100), then back up using the rear sensor measurement. If neither measurement is available, then revert back to a 1 second half-speed movement. In all cases, use the rear distance measurement to prevent ‘reverse wall climb’ by stopping the motors if the robot gets too close to an obstacle while backing up.
Execute a ‘spin turn’ away from the nearest wall – this is the same as the current algorithm.
Execute a ‘rolling turn’ back toward the original direction of travel. This should offset the robot further away from the nearest wall, and hopefully allow it to bypass the obstacle.