Author Archives: paynterf

Another Try at Wall Offset Tracking

Posted 22 June 2021

About nine months ago (October 2020) I made a run at getting offset tracking to work (see here and here). This post describes yet another attempt at getting this right, taking advantage of recent work on controlled-rate turns. I constructed a short single-task program to do just the wall tracking task, hopefully simplifying things to the point where I can understand what is happening.

One of the big issues that arose in previous work was the inability to synch my TIMER5 ISR with the PID library’s ‘Compute()’ function. The PID library insists on managing the update timing internally, which meant there was no way to ensure that Compute() would be called every time the ISR ran. I eventually came to the conclusion that I simply could not use the PID library version, and instead wrote my own small function that did the Compute() function, but with the timing value passed in as an argument rather than being managed internally. This forces the PID calculations to actually update in synch with the TIMER5 interrupt. Here’s the new PIDCalcs() function:

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)
{
	//Purpose:  Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR
	//Inputs:
	//	input = double denoting current input value (turn rate, speed, whatever)
	//	setpoint = double denoting desired setpoint in same units as input
	//	sampleTime = int denoting sample time to be used in calcs.
	//	lastError = ref to double denoting error saved from prev calc
	//	lastInput = ref to double denoting input saved from prev calc
	//	Kp/Ki/Kd = doubles denoting PID values to be used for calcs
	//	Output = ref to double denoting output from calc

	double error = setpoint - input;
	double dInput = (input - lastInput);
	lastIval += (error);
	double dErr = (error - lastError);

	//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr,
	//	Kp,Ki,Kd);

	/*Compute PID Output*/
	output = Kp * error + Ki * lastIval + Kd * dErr;

	/*Remember some variables for next time*/
	lastError = error;
	lastInput = input;
	lastDerror = dErr;
}

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)

{

//Purpose: Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR

//Inputs:

// input = double denoting current input value (turn rate, speed, whatever)

// setpoint = double denoting desired setpoint in same units as input

// sampleTime = int denoting sample time to be used in calcs.

// lastError = ref to double denoting error saved from prev calc

// lastInput = ref to double denoting input saved from prev calc

// Kp/Ki/Kd = doubles denoting PID values to be used for calcs

// Output = ref to double denoting output from calc

double error = setpoint - input;

double dInput = (input - lastInput);

lastIval += (error);

double dErr = (error - lastError);

//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr,

// Kp,Ki,Kd);

/*Compute PID Output*/

output = Kp * error + Ki * lastIval + Kd * dErr;

/*Remember some variables for next time*/

lastError = error;

lastInput = input;

lastDerror = dErr;

}

As can be seen from the above, this is a very simple routine that just does one thing, and doesn’t incorporate any of the improvements (windup suppression, sample time changes, etc) available in the PID library. The calling function has to manage the persistent parameters, but that’s a small price to pay for clarity and the assurance that the output value will indeed be updated every time the function is called.

With this function in hand, I worked on getting the robot to reliably track a specified offset, but was initially stymied because while I could get it to control the motors so that the robot stayed parallel to the nearest wall, I still couldn’t get it to track a specific offset. I solved this problem by leveraging my new-found ability to make accurate, rate-controlled turns; the robot first turns by an amount proportional to it’s distance from the desired offset, moves straight ahead until the offset is met, and then turns the same number of degrees in the other direction. Assuming the robot started parallel to the wall, this results in it facing the direction of travel, at the desired offset and parallel to the nearest wall. Here is the code for this algorithm.

void TrackLeftWallOffset(double kp, double ki, double kd, double offsetCm)
{
	//Notes:
	//	06/21/21 modified to do a turn, then straight, then track 0 steer val

	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;
	bool bFirstTime = true;

	WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5

	//at 30mm from tgt offset, setpoint will be +/-0.3
	WallTrackSetPoint = (double)(10 * WALL_OFFSET_TGTDIST_CM - Lidar_LeftCenter) / 100.f; //10/04/20 positive value drives robot toward wall

	//limit approach angle to reasonable values
	if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;
	if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

	mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);
	mySerial.printf("Msec\tFdir\tCdir\ttRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

	//06/21/21 modified to do a turn, then straight, then track 0 steer val
	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_LeftCenter / 10.f);
	mySerial.printf("dist = %d, cutAngleDeg = %2.2f\n", Lidar_LeftCenter, cutAngleDeg);
	SpinTurn((cutAngleDeg < 0), cutAngleDeg, 30);

	while (abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM) > 10)
	{
		MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);
		GetRequestedVL53l0xValues(VL53L0X_LEFT);
		mySerial.printf("%lu \t%d\t%d\t%d\t%d\n",
			millis(),
			Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
			abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM));
		delay(100);
	}

	//now turn back the same amount
	SpinTurn(!(cutAngleDeg < 0), cutAngleDeg, 30);

	//sinceLastComputeTime = 0;
		WallTrackSetPoint = 0; //moved here 6/22/21

	while (true)
	{
		CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

		//now using TIMER5 100 mSec interrupt for timing
		if (bTimeForNavUpdate)
		{
			//sinceLastComputeTime -= WALL_TRACK_UPDATE_INTERVAL_MSEC;
			bTimeForNavUpdate = false;

			//GetRequestedVL53l0xValues(VL53L0X_LEFT); //now done in TIMER5 ISR

			//have to weight value by both angle and wall offset
			WallTrackSteerVal = LeftSteeringVal + (Lidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

			//update motor speeds, skipping bad values
			if (!isnan(WallTrackSteerVal))
			{
				//10/12/20 now this executes every time, with interval controlled by timer ISR
				PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
					kp, ki, kd, WallTrackOutput);
				//PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, WALL_TRACK_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
				//	kp, ki, kd, WallTrackOutput);

				int speed = 0;

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

				rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL;
				rightspeednum = (rightspeednum > 0) ? rightspeednum : 0;
				leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL;
				leftspeednum = (leftspeednum > 0) ? leftspeednum : 0;

				MoveAhead(leftspeednum, rightspeednum);

				mySerial.printf("%lu \t%d\t%d\t%d \t%2.2f\t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%d\t%d\n",
					millis(),
					Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,
					WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,
					kp * lastError, ki * lastIval, kd * lastDerror,
					WallTrackOutput, leftspeednum, rightspeednum);
			}
		}
	}
}

void TrackLeftWallOffset(double kp, double ki, double kd, double offsetCm)

{

//Notes:

// 06/21/21 modified to do a turn, then straight, then track 0 steer val

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

bool bFirstTime = true;

WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5

//at 30mm from tgt offset, setpoint will be +/-0.3

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

//limit approach angle to reasonable values

if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;

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

mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

mySerial.printf("Msec\tFdir\tCdir\ttRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

//06/21/21 modified to do a turn, then straight, then track 0 steer val

GetRequestedVL53l0xValues(VL53L0X_LEFT);

double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_LeftCenter / 10.f);

mySerial.printf("dist = %d, cutAngleDeg = %2.2f\n", Lidar_LeftCenter, cutAngleDeg);

SpinTurn((cutAngleDeg < 0), cutAngleDeg, 30);

while (abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM) > 10)

{

MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);

GetRequestedVL53l0xValues(VL53L0X_LEFT);

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

millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM));

delay(100);

}

//now turn back the same amount

SpinTurn(!(cutAngleDeg < 0), cutAngleDeg, 30);

//sinceLastComputeTime = 0;

WallTrackSetPoint = 0; //moved here 6/22/21

while (true)

{

CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

//now using TIMER5 100 mSec interrupt for timing

if (bTimeForNavUpdate)

{

//sinceLastComputeTime -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

bTimeForNavUpdate = false;

//GetRequestedVL53l0xValues(VL53L0X_LEFT); //now done in TIMER5 ISR

//have to weight value by both angle and wall offset

WallTrackSteerVal = LeftSteeringVal + (Lidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

//10/12/20 now this executes every time, with interval controlled by timer ISR

PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

kp, ki, kd, WallTrackOutput);

//PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, WALL_TRACK_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

// kp, ki, kd, WallTrackOutput);

int speed = 0;

int leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

int rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL;

rightspeednum = (rightspeednum > 0) ? rightspeednum : 0;

leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL;

leftspeednum = (leftspeednum > 0) ? leftspeednum : 0;

MoveAhead(leftspeednum, rightspeednum);

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

millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear,

WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,

kp * lastError, ki * lastIval, kd * lastDerror,

WallTrackOutput, leftspeednum, rightspeednum);

}

Here are a couple of videos showing the ability to capture and track a desired offset from either side.

In both of the above videos, the desired wall offset is 30 cm.

Stay Tuned,

Frank

Turn Rate PID Tuning, Part IV

Leave a reply

Posted 10 June 2021,

In my last post on this issue, I described using a small test program to explore an in-line version of the PID (Proportional-Integral-Differential) algorithm for turn rate control with Wall-E2, my autonomous wall-following robot. This post describes some follow-on work on this same subject.

The fundamental problem with all the available Arduino PID libraries is they all require the user to wait in a loop for the PID::Compute() function to decide when to actually produce a new output value, and since this computation is inside the function, it is difficult or impossible to synchronize any other related timed element with the PID function. In my case where I want to control the turn rate of a robot, the input to the PID engine is, obviously, the turn rate, in degrees/sec. But, calculation of the turn rate is necessarily a timed function, i.e. (curent_heading – last_heading) / elapsed_time, where the ‘elapsed_time’ parameter is usually a constant. But, all the Arduino PID libraries use an internal private class member that defines the measurement period (in milliseconds), and this value isn’t available externally (well, it is, but only because the user can set the sample time – it can’t be read). So, the best one can do with the current libraries is to use the same constant for PID::SetSampleTime() and for any external time-based calculations, and hope there aren’t any synchronization issues. With this setup, it would be quite possible (and inevitable IMHO) for the PID::Compute() function to skip a step, or to be ‘phase-locked’ to producing an output that is one time constant off from the input.

The solution to this problem is to not use a PID library at all, and instead place the PID algorithm in-line with the rest of the code. This ensures that the PID calculation any related time-based calculations are operating on the same schedule. The downside of this arrangement is loss of generality; all the cool enhancements described by Brett Beauregard in his wonderful PID tutorial go away or have to be implemented in-line as well. From Brett’s tutorial, here’s ‘the beginner’s PID algorithm’:

The difficulty with all the current PID libraries is the ‘dt’ parameter in the above expression; the implementation becomes much easier if ‘dt’ is a constant – i.e. the time between calculations is a constant. However, this constraint also requires that the library, not the user program, controls the timing. This just doesn’t work when the ‘Input’ parameter above also requires a constant time interval for calculation. In my case of turn rate control, the turn rate calculation requires knowledge of the time interval between calculations, and the calculation itself should be done immediately after the turn rate is determined, using the same time interval. So, the turn rate is calculated and then PID::Compute() is called, but Compute() may or may not generate a new output value, because it can return without action if it’s internal time duration criteria isn’t met; see the problem? It may, or even might, generate a new output value each time, but there is no way to ensure that it will!

After figuring this out the hard way (by trying and failing to make the library work), I finally decided to forget the library – at least for my turn rate problem, and in-line all the needed code. Once I had it all running, I abstracted just the PID algorithm to its own function so I could use it elsewhere. This function is shown below:

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)
{
	//Purpose:  Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR
	//Inputs:
	//	input = double denoting current input value (turn rate, speed, whatever)
	//	setpoint = double denoting desired setpoint in same units as input
	//	sampleTime = int denoting sample time to be used in calcs.
	//	lastError = ref to double denoting error saved from prev calc
	//	lastInput = ref to double denoting input saved from prev calc
	//	Kp/Ki/Kd = doubles denoting PID values to be used for calcs
	//	Output = ref to double denoting output from calc


	double error = setpoint - input;
	double dInput = (input - lastInput);
	lastIval += (error);
	double dErr = (error - lastError);

	//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr = %2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr);

	/*Compute PID Output*/
	output = Kp * error + Ki * lastIval + Kd * dErr;

	/*Remember some variables for next time*/
	lastError = error;
	lastInput = input;
	lastDerror = dErr;
}

void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output)

{

//Purpose: Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR

//Inputs:

// input = double denoting current input value (turn rate, speed, whatever)

// setpoint = double denoting desired setpoint in same units as input

// sampleTime = int denoting sample time to be used in calcs.

// lastError = ref to double denoting error saved from prev calc

// lastInput = ref to double denoting input saved from prev calc

// Kp/Ki/Kd = doubles denoting PID values to be used for calcs

// Output = ref to double denoting output from calc

double error = setpoint - input;

double dInput = (input - lastInput);

lastIval += (error);

double dErr = (error - lastError);

//mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr = %2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr);

/*Compute PID Output*/

output = Kp * error + Ki * lastIval + Kd * dErr;

/*Remember some variables for next time*/

lastError = error;

lastInput = input;

lastDerror = dErr;

}

As you can see, the explanatory comments are much bigger than the function itself, which is really just eight lines long. Also, it has a huge number of arguments, five of which are references that are updated by the function. This function wouldn’t win any awards for good design, as it has too many arguments (wide coupling), but it does have high cohesion (does just one thing), and the coupling is at least ‘data’ coupling only.

Once this function was implemented, the calling function (in this case ‘SpinTurn()’ looks like this:

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)
{
	//Purpose: Make a numDeg CW or CCW 'spin' turn
	//Inputs:
	//	b_ccw - True if turn is to be ccw, false otherwise
	//	numDeg - angle to be swept out in the turn
	//	ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg
	//	IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR
	//	degPerSec = float value denoting desired turn rate
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Compute new target value
	//	Step3: 11/14/20 activate turn rate PID
	//	Step4: Set motor speeds based on b_ccw
	//	Step5: Run motors until target reached, using PID to control turn rate
	//Notes:
	//	06/27/20 copied from RollingTurn & adapted
	//	09/02/20 experiment with adjustable turn rate
	//	11/02/20 rev to use ISR-updated IMUHdgValDeg vs call to UpdateIMUHdgValDeg
	//	11/06/20 rev to use bTimeForNavUpdate flag to space out hdg/dist updates
	//	11/06/20 rev to make target turn rate variable for slowdown portion of spin
	//	11/14/20 rev to use PID for turn rate managaement
	//	04/05/21 rewritten to use ISR for timing and simplified
	//	04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)
	//	05/29/21 replaced TurnRatePID with SetTurnRateMotorSpeeds()
	//	06/06/21 added re-try for 180.00 return from IMU - could be bad value

	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
	//unsigned long prev_uSec; //added 09/02/20

	//DEBUG!!
	mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);
	//DEBUG!!
	mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",
		TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

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

	UpdateIMUHdgValDeg(); //refreshes IMHHdgValDeg

	//06/06/21 it is possible for IMU to return 180.00 on failure
	//so try one more time in this case.  If it really IS 180, then 
	//the second try should confirm this.
	if (IMUHdgValDeg == 180.f)
	{
		mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");
		UpdateIMUHdgValDeg();
	}

	//Step2: Compute new target value & timeout value
	timeout_sec = 2 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

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

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

#endif // MPU6050_CCW_INCREASES_YAWVAL

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

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

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

	float curHdgMatchVal = 0;

	//09/08/18 added to bolster end-of-turn detection
	float prevHdgMatchVal = 0;
	float matchSlope = 0;
	TIMSK5 = 0; //disable timer compare interrupt

	//Step5: Run motors until target reached, using PID to control turn rate
	Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

	elapsedMillis sinceLastTimeCheck = 0;
	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;

	while (!bDoneTurning && !bTimedOut)
	{
		//11/06/20 now just loops between ISR hits
		CheckForUserInput();
		UpdateIMUHdgValDeg(); //now Prev_HdgDeg & IMUHdgValDeg can be different (but probably aren't the first time through)

			//runs every 30mSec
		if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)
		{
			sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

			//here Prev_HdgDeg & IMUHdgValDeg should be different, as TURN_RATE_UPDATE_INTERVAL_MSEC should have elapsed since last synch
			double dHdg = IMUHdgValDeg - Prev_HdgDeg;
			double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);
			Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

			PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

			int speed = 0;

			if (TurnRatePIDOutput > MOTOR_SPEED_MAX) speed = MOTOR_SPEED_MAX;
			else if (TurnRatePIDOutput <= MOTOR_SPEED_LOW) speed = MOTOR_SPEED_LOW;
			else speed = (int)TurnRatePIDOutput;

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

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

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

		bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

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

			bResult = false;

		}
	}

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

	//04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding
	StopBothMotors();
	delay(1000); //added 04/27/21 for debug
	
	return bResult;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)

{

//Purpose: Make a numDeg CW or CCW 'spin' turn

//Inputs:

// b_ccw - True if turn is to be ccw, false otherwise

// numDeg - angle to be swept out in the turn

// ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg

// IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR

// degPerSec = float value denoting desired turn rate

//Plan:

// Step1: Get current heading as starting point

// Step2: Compute new target value

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

// Step4: Set motor speeds based on b_ccw

// Step5: Run motors until target reached, using PID to control turn rate

//Notes:

// 06/27/20 copied from RollingTurn & adapted

// 09/02/20 experiment with adjustable turn rate

// 11/02/20 rev to use ISR-updated IMUHdgValDeg vs call to UpdateIMUHdgValDeg

// 11/06/20 rev to use bTimeForNavUpdate flag to space out hdg/dist updates

// 11/06/20 rev to make target turn rate variable for slowdown portion of spin

// 11/14/20 rev to use PID for turn rate managaement

// 04/05/21 rewritten to use ISR for timing and simplified

// 04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)

// 05/29/21 replaced TurnRatePID with SetTurnRateMotorSpeeds()

// 06/06/21 added re-try for 180.00 return from IMU - could be bad value

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

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

//DEBUG!!

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

//DEBUG!!

mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",

TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

//no need to continue if the IMU isn't available

if (!dmpReady)

{

return false;

}

UpdateIMUHdgValDeg(); //refreshes IMHHdgValDeg

//06/06/21 it is possible for IMU to return 180.00 on failure

//so try one more time in this case. If it really IS 180, then

//the second try should confirm this.

if (IMUHdgValDeg == 180.f)

{

mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");

UpdateIMUHdgValDeg();

}

//Step2: Compute new target value & timeout value

timeout_sec = 2 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

//05/17/20 limit timeout_sec to 1 sec or more

timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

//12/05/19 added #define back in to manage which direction increases yaw values

#ifdef MPU6050_CCW_INCREASES_YAWVAL

tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;

#else

tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

//correct for -180/180 transition

if (tgt_deg < -180)

{

tgt_deg += 360;

}

//07/29/19 bugfix

if (tgt_deg > 180)

{

tgt_deg -= 360;

}

//DEBUG!!

mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",

IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);

//DEBUG!!

float curHdgMatchVal = 0;

//09/08/18 added to bolster end-of-turn detection

float prevHdgMatchVal = 0;

float matchSlope = 0;

TIMSK5 = 0; //disable timer compare interrupt

//Step5: Run motors until target reached, using PID to control turn rate

Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

elapsedMillis sinceLastTimeCheck = 0;

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

while (!bDoneTurning && !bTimedOut)

{

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

CheckForUserInput();

UpdateIMUHdgValDeg(); //now Prev_HdgDeg & IMUHdgValDeg can be different (but probably aren't the first time through)

//runs every 30mSec

if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

//here Prev_HdgDeg & IMUHdgValDeg should be different, as TURN_RATE_UPDATE_INTERVAL_MSEC should have elapsed since last synch

double dHdg = IMUHdgValDeg - Prev_HdgDeg;

double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

int speed = 0;

if (TurnRatePIDOutput > MOTOR_SPEED_MAX) speed = MOTOR_SPEED_MAX;

else if (TurnRatePIDOutput <= MOTOR_SPEED_LOW) speed = MOTOR_SPEED_LOW;

else speed = (int)TurnRatePIDOutput;

SetLeftMotorDirAndSpeed(!b_ccw, speed);

SetRightMotorDirAndSpeed(b_ccw, speed);

//07/31/19 now running as fast as we can get valid hdgs from IMU

//check for nearly there and all the way there

curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);

matchSlope = curHdgMatchVal - prevHdgMatchVal;

prevHdgMatchVal = curHdgMatchVal;

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start

}

bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

if (bTimedOut)

{

//DEBUG!!

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

//DEBUG!!

bResult = false;

}

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

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

StopBothMotors();

delay(1000); //added 04/27/21 for debug

return bResult;

}

In the above code, the lines dealing with ‘TIMSK5’ are there to disable and then re-enable the TIMER5 interrupt I have set up to update external sensor values every 100 mSec. I’m not really sure that this HAS to be done, but once I learned how to do it I figured it wouldn’t hurt, either ☺

Now that I have this ‘PIDCalcs()’ function working properly, I plan to use it in several other places where I currently use the PID library; it’s just so much simpler now, and because all the relevant parameters are visible to the calling program, debugging is now a piece of cake where before it was just an opaque black box.

12 June 2021 Update:

After chasing down and eliminating a number of bugs and edge-case issues, I think I now have a pretty stable/working version of the ‘SpinTurn’ function, as shown below:

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)
{
	//Purpose: Make a numDeg CW or CCW 'spin' turn
	//Inputs:
	//	b_ccw - True if turn is to be ccw, false otherwise
	//	numDeg - angle to be swept out in the turn
	//	ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg
	//	IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR
	//	degPerSec = float value denoting desired turn rate
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Disable TIMER5 interrupts
	//	Step3: Compute new target value & timeout value
	//	Step4: Run motors until target reached, using inline PID algorithm to control turn rate
	//	Step5: Re-enable TIMER5 interrupts
	//Notes:
	//	06/06/21 we-written to remove PID library - now uses custom 'PIDCalcs()' function
	//	06/06/21 added re-try for 180.00 return from IMU - could be bad value
	//	06/11/21 added code to correct dHdg errors due to 179/-179 transition & bad IMU values
	//	06/12/21 cleaned up & commented out debug code

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

	////DEBUG!!
	//mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec);
	////DEBUG!!
	//mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",
	//	TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

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

//Step1: Get current heading as starting point
	//06/06/21 it is possible for IMU to return 180.00 on failure
	//so try again.  If it really IS 180, then 
	//it will eventually time out and go on
	int retries = 0;
	if (IMUHdgValDeg == 180.f && retries < 5)
	{
		//DEBUG!!
		//mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");
		//DEBUG!!
		UpdateIMUHdgValDeg();
		retries++;
		delay(30);
	}

//Step2: Compute new target value & timeout value
	timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

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

	UpdateIMUHdgValDeg();

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

#endif // MPU6050_CCW_INCREASES_YAWVAL

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

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

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

	float curHdgMatchVal = 0;

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


//Step2: Disable TIMER5 interrupts
	TIMSK5 = 0; //disable timer compare interrupt

	//Step3: Run motors until target reached, using PID algorithm to control turn rate
	Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

	//DEBUG!!
	//mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg);

	//mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n");
	//DEBUG!!

	elapsedMillis sinceLastTimeCheck = 0;
	elapsedMillis sinceLastComputeTime = 0;
	double lastError = 0;
	double lastInput = 0;
	double lastIval = 0;
	double lastDerror = 0;
	bool bFirstIMUHdg = true;

	while (!bDoneTurning && !bTimedOut)
	{
		//11/06/20 now just loops between ISR hits
		CheckForUserInput();

			//runs every 30mSec
		if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)
		{
			sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

			UpdateIMUHdgValDeg(); //update IMUHdgValDeg

			double dHdg = IMUHdgValDeg - Prev_HdgDeg;
			if (dHdg > 180)
			{
				dHdg -= 360;
				mySerial.printf("dHdg > 180 - subtracting 360\n");
			}
			else if (dHdg < -180)
			{
				dHdg += 360;
				mySerial.printf("dHdg < -180 - adding 360\n");
			}

			//watch for turn rates that are wildly off
			double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);
			if (rate > 3*degPersec)
			{
				//DEBUG!!
				//mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec);
				//DEBUG!!
				rate = degPersec;
			}

			//06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet
			if (bFirstIMUHdg)
			{
				bFirstIMUHdg = false;
				Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time
			}
			else
			{
				PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

				int speed = 0;

				(TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput;
				(TurnRatePIDOutput <= MOTOR_SPEED_LOW)? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput;

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

				//check for nearly there and all the way there
				curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);
				matchSlope = curHdgMatchVal - prevHdgMatchVal;
				prevHdgMatchVal = curHdgMatchVal;

				//DEBUG!!
				//mySerial.printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\n", 
				//	millis(), 
				//	IMUHdgValDeg,
				//	Prev_HdgDeg,
				//	dHdg,
				//	rate,
				//	degPersec, 
				//	lastError, 
				//	TurnRate_Kp * lastError,
				//	TurnRate_Ki * lastIval,
				//	TurnRate_Kd * lastDerror,
				//	speed);
				//DEBUG!!

				Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

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

		bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

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

			bResult = false;
		}
	}

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

//Step4: Re-enable TIMER5 interrupts
	TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt

	StopBothMotors();
	delay(1000); //added 04/27/21 for debug
	return bResult;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

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

bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC)

{

//Purpose: Make a numDeg CW or CCW 'spin' turn

//Inputs:

// b_ccw - True if turn is to be ccw, false otherwise

// numDeg - angle to be swept out in the turn

// ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg

// IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR

// degPerSec = float value denoting desired turn rate

//Plan:

// Step1: Get current heading as starting point

// Step2: Disable TIMER5 interrupts

// Step3: Compute new target value & timeout value

// Step4: Run motors until target reached, using inline PID algorithm to control turn rate

// Step5: Re-enable TIMER5 interrupts

//Notes:

// 06/06/21 we-written to remove PID library - now uses custom 'PIDCalcs()' function

// 06/06/21 added re-try for 180.00 return from IMU - could be bad value

// 06/11/21 added code to correct dHdg errors due to 179/-179 transition & bad IMU values

// 06/12/21 cleaned up & commented out debug code

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

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

////DEBUG!!

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

////DEBUG!!

//mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n",

// TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC);

//no need to continue if the IMU isn't available

if (!dmpReady)

{

return false;

}

//Step1: Get current heading as starting point

//06/06/21 it is possible for IMU to return 180.00 on failure

//so try again. If it really IS 180, then

//it will eventually time out and go on

int retries = 0;

if (IMUHdgValDeg == 180.f && retries < 5)

{

//DEBUG!!

//mySerial.printf("Got 180.00 exactly from IMU - retrying...\n");

//DEBUG!!

UpdateIMUHdgValDeg();

retries++;

delay(30);

}

//Step2: Compute new target value & timeout value

timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter

//05/17/20 limit timeout_sec to 1 sec or more

timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec;

UpdateIMUHdgValDeg();

//12/05/19 added #define back in to manage which direction increases yaw values

#ifdef MPU6050_CCW_INCREASES_YAWVAL

tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg;

#else

tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg;

#endif // MPU6050_CCW_INCREASES_YAWVAL

//correct for -180/180 transition

if (tgt_deg < -180)

{

tgt_deg += 360;

}

//07/29/19 bugfix

if (tgt_deg > 180)

{

tgt_deg -= 360;

}

//DEBUG!!

//mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n",

// IMUHdgValDeg, numDeg, tgt_deg, timeout_sec);

//DEBUG!!

float curHdgMatchVal = 0;

//09/08/18 added to bolster end-of-turn detection

float prevHdgMatchVal = 0;

float matchSlope = 0;

//Step2: Disable TIMER5 interrupts

TIMSK5 = 0; //disable timer compare interrupt

//Step3: Run motors until target reached, using PID algorithm to control turn rate

Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop

//DEBUG!!

//mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg);

//mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n");

//DEBUG!!

elapsedMillis sinceLastTimeCheck = 0;

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

bool bFirstIMUHdg = true;

while (!bDoneTurning && !bTimedOut)

{

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

CheckForUserInput();

//runs every 30mSec

if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg(); //update IMUHdgValDeg

double dHdg = IMUHdgValDeg - Prev_HdgDeg;

if (dHdg > 180)

{

dHdg -= 360;

mySerial.printf("dHdg > 180 - subtracting 360\n");

}

else if (dHdg < -180)

{

dHdg += 360;

mySerial.printf("dHdg < -180 - adding 360\n");

}

//watch for turn rates that are wildly off

double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC);

if (rate > 3*degPersec)

{

//DEBUG!!

//mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec);

//DEBUG!!

rate = degPersec;

}

//06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet

if (bFirstIMUHdg)

{

bFirstIMUHdg = false;

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

}

else

{

PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput);

int speed = 0;

(TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput;

(TurnRatePIDOutput <= MOTOR_SPEED_LOW)? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput;

SetLeftMotorDirAndSpeed(!b_ccw, speed);

SetRightMotorDirAndSpeed(b_ccw, speed);

//check for nearly there and all the way there

curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg);

matchSlope = curHdgMatchVal - prevHdgMatchVal;

prevHdgMatchVal = curHdgMatchVal;

//DEBUG!!

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

// millis(),

// IMUHdgValDeg,

// Prev_HdgDeg,

// dHdg,

// rate,

// degPersec,

// lastError,

// TurnRate_Kp * lastError,

// TurnRate_Ki * lastIval,

// TurnRate_Kd * lastDerror,

// speed);

//DEBUG!!

Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

|| (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start

}

bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000);

if (bTimedOut)

{

//DEBUG!!

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

//DEBUG!!

bResult = false;

}

//DEBUG!!

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

//DEBUG!!

//Step4: Re-enable TIMER5 interrupts

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

StopBothMotors();

delay(1000); //added 04/27/21 for debug

return bResult;

}

With this code in place, I made some 180º turns at 45 & 90 deg/sec, both on my benchtop and on carpet, as shown in the plots and video below:

Stay Tuned!

Frank

Turn Rate PID Tuning, Part III

Leave a reply

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

elapsedMillis MsecSinceLastTurnRateUpdate;

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

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

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

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


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

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

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

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

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

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

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

		bMPU6050Ready = false;
	}
#pragma endregion MPU6050

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

void loop()
{


}

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

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

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

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

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

			while (!Serial.available())
			{
			}

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

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

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

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

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

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

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

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

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

			while (!Serial.available())
			{
			}

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

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

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

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

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

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

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

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

						}
					}
				}
			}

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

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


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

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

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

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

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

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

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

				deltaDeg = IMUHdgValDeg - prev_hdg;

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

				prev_hdg = IMUHdgValDeg;
				UpdateIMUHdgValDeg();
			}

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

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

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

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

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

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


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

			prev_hdg = IMUHdgValDeg;
		}
	}
}

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

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

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

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

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

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

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

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

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

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

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

//Step1: Set left wheel direction and speed

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

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

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

	RunBothMotors(bisFwd, leftspeednum, rightspeednum);

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

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

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

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

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

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

#ifndef NO_MOTORS

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

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

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

	bool retval = false;

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

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

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

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

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

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

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

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

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

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

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

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



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

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

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


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

	while (true)
	{
		CheckForUserInput();

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

			UpdateIMUHdgValDeg();
			float deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

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

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

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

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

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

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

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

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

			prev_hdg = IMUHdgValDeg;
		}
	}
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

Name: FourWD_PulseTurnRateTest.ino

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

Author: FRANKNEWXPS15\Frank

#pragma region INCLUDES

#include <elapsedMillis.h>

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

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

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

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

#pragma endregion INCLUDES

#pragma region DEFINES

#define MPU6050_I2C_ADDR 0x69

#pragma endregion DEFINES

#pragma region PRE_SETUP

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

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

#pragma endregion PRE_SETUP

#pragma region MOTOR_PINS

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

const int In1_Left = 10;

const int In2_Left = 11;

const int In1_Right = 8;

const int In2_Right = 9;

#pragma endregion Motor Pin Assignments

#pragma region MOTOR_PARAMETERS

//drive wheel speed parameters

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

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

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

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

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

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

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

bool bIsForwardDir = true; //default is foward direction

#pragma endregion Motor Parameters

#pragma region MPU6050_SUPPORT

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

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

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

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

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

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

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

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

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

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

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

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

int GetPacketLoopCount = 0;

int OuterGetPacketLoopCount = 0;

//MPU6050 status flags

bool bMPU6050Ready = true;

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

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

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

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

bool bFirstTime = true;

#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19

#pragma endregion MPU6050 Support

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

elapsedMillis MsecSinceLastTurnRateUpdate;

//const int MAX_PULSE_WIDTH_MSEC = 100;

const int MAX_PULSE_WIDTH_MSEC = 50;

const int MIN_PULSE_WIDTH_MSEC = 0;

//const int MAX_SPIN_MOTOR_SPEED = 200;

const int MAX_SPIN_MOTOR_SPEED = 250;

const int MIN_SPIN_MOTOR_SPEED = 50;

const int TURN_RATE_UPDATE_INTERVAL_MSEC = 20;

//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 100;

double TurnRateOutput;

double TurnRateSetPoint_DPS;

double Ival = 0; double lastErr = 0;

double lastTurnRateVal_DPS;

double TurnRateVal_DPS;

void setup()

{

Serial.begin(115200);

#pragma region MPU6050

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

mpu.initialize();

// verify connection

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

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

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

devStatus = mpu.dmpInitialize();

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

if (devStatus == 0)

{

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

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

mpu.setDMPEnabled(true);

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

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

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

bMPU6050Ready = true;

StartSec = millis() / 1000.f;

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

}

else

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

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

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

Serial.print(devStatus);

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

bMPU6050Ready = false;

}

#pragma endregion MPU6050

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

while (true)

{

CheckForUserInput();

}

void loop()

{

}

void CheckForUserInput()

{

const int bufflen = 80;

char buff[bufflen];

memset(buff, 0, bufflen);

float degPerSec;

float Kp, Ki, Kd;

const char s[2] = ",";

char* token;

if (Serial.available() > 0)

{

// read the incoming byte:

int incomingByte = Serial.read();

// say what you got:

Serial.print("I received: ");

//Serial.println(incomingByte, DEC);

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

//02/18/20 experiment with multiple commands

switch (incomingByte)

{

case 0x50: //ASCII 'P'

case 0x70: //ASCII 'p'

StopBothMotors();

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

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

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

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

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

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract degPerSec */

token = strtok(NULL, s);

degPerSec = atof(token);

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

Kp, Ki, Kd, degPerSec);

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

break;

case 0x51: //ASCII 'Q'

case 0x71: //ASCII 'q'

StopBothMotors();

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

//consume CR/LF chars

while (Serial.available() > 0)

{

int byte = Serial.read();

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

}

while (!Serial.available())

{

}

Serial.readBytes(buff, bufflen);

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

/* extract Kp */

token = strtok(buff, s);

Kp = atof(token);

/* extract Ki */

token = strtok(NULL, s);

Ki = atof(token);

/* extract Kd */

token = strtok(NULL, s);

Kd = atof(token);

/* extract degPerSec */

token = strtok(NULL, s);

degPerSec = atof(token);

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

Kp, Ki, Kd, degPerSec);

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

break;

Default:

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

MoveAhead(0, 0);

while (true)

{

}

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

{

float tgt_deg;

double prev_hdg = 0;

//DEBUG!!

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

Kp, Ki, Kd, degPersec);

//DEBUG!!

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

//init some stuff

TurnRateOutput = 0;

TurnRateSetPoint_DPS = degPersec;

Ival = 0; lastErr = 0;

lastTurnRateVal_DPS;

TurnRateVal_DPS;

UpdateIMUHdgValDeg();

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

MsecSinceLastTurnRateUpdate = 0;

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

//Step4: Pulse the motors to get things started

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

delay(50);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

while (true)

{

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

CheckForUserInput();

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

//{

// bTimeForNavUpdate = false;

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

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

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

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

IMUHdgValDeg, prev_hdg, deltaDeg);

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

CheckForUserInput();

deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

prev_hdg = IMUHdgValDeg;

UpdateIMUHdgValDeg();

}

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

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

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

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_PULSE_WIDTH_MSEC)

TurnRateOutput = MAX_PULSE_WIDTH_MSEC;

else if (TurnRateOutput < MIN_PULSE_WIDTH_MSEC)

TurnRateOutput = MIN_PULSE_WIDTH_MSEC;

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

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

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_MAX);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_MAX);

//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

//SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);

//SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);

delay(TurnRateOutput);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

//DEBUG!!

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

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

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

//DEBUG

prev_hdg = IMUHdgValDeg;

}

#pragma region MOTOR SUPPORT

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

void MoveReverse(int leftspeednum, int rightspeednum)

{

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

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

//Inputs:

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

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

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

//Plan:

// Step 1: Set reverse direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

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

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

SetLeftMotorDirAndSpeed(REV_DIR, leftspeednum);

SetRightMotorDirAndSpeed(REV_DIR, rightspeednum);

}

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

void MoveAhead(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

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

//Inputs:

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

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

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

//Plan:

// Step 1: Set forward direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

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

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

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

SetLeftMotorDirAndSpeed(true, leftspeednum);

SetRightMotorDirAndSpeed(true, rightspeednum);

}

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

void StopLeftMotors()

{

analogWrite(In1_Left, MOTOR_SPEED_OFF);

analogWrite(In2_Left, MOTOR_SPEED_OFF);

}

void StopRightMotors()

{

analogWrite(In1_Right, MOTOR_SPEED_OFF);

analogWrite(In2_Right, MOTOR_SPEED_OFF);

}

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

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

{

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

//Inputs:

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

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

//Plan:

// Step 1: Apply drive to both wheels

//Notes:

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

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

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

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

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

//Step 1: Apply drive to both wheels

//DEBUG!!

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

//DEBUG!!

SetLeftMotorDirAndSpeed(bisFwd, leftspeednum);

SetRightMotorDirAndSpeed(bisFwd, rightspeednum);

}

void RunBothMotorsBidirectional(int leftspeed, int rightspeed)

{

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

//Inputs:

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

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

//Outputs:

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

//Plan:

// Step1: Set left wheel direction and speed

// Step2: Set right wheel direction and speed

//Step1: Set left wheel direction and speed

//DEBUG!!

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

if (leftspeed < 0)

{

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

}

else

{

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

}

//Step2: Set right wheel direction and speed

if (rightspeed < 0)

{

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

}

else

{

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

}

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

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

{

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

//Inputs:

// timesec = time in seconds to run the Motors

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

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

//Plan:

// Step 1: Apply drive to both wheels

// Step 2: Delay timsec seconds

// Step 3: Remove drive from both wheels.

//Notes:

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

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

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

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

RunBothMotors(bisFwd, leftspeednum, rightspeednum);

//Step 2: Delay timsec seconds

delay(timeMsec);

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

StopBothMotors();

}

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

void StopBothMotors()

{

StopLeftMotors();

StopRightMotors();

}

void SetLeftMotorDirAndSpeed(bool bIsFwd, int speed)

{

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

#ifndef NO_MOTORS

if (bIsFwd)

{

digitalWrite(In1_Left, LOW);

analogWrite(In2_Left, speed);

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

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

}

else

{

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

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

digitalWrite(In2_Left, LOW);

analogWrite(In1_Left, speed);

}

#endif // !NO_MOTORS

}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)

{

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

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

#ifndef NO_MOTORS

if (bIsFwd)

{

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

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

digitalWrite(In1_Right, LOW);

analogWrite(In2_Right, speed);

}

else

{

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

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

digitalWrite(In2_Right, LOW);

analogWrite(In1_Right, speed);

}

#endif // !NO_MOTORS

}

#pragma endregion Motor Support Functions

#pragma region MPU5060 Support

float UpdateIMUHdgValDeg()

{

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

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

//Outputs:

// returns true if successful, otherwise false

// IMUHdgValDeg updated on success

//Plan:

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

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

//Step3: update IMUHdgValDeg with latest value

//Notes:

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

// 10/08/19 no longer need mpuIntStatus

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

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

bool retval = false;

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

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

{

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

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

//compute the yaw value

IMUHdgValDeg = ypr[0] * 180 / M_PI;

retval = true;

}

//return retval;

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

}

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

{

mpu.resetFIFO();

delay(1);

//int countloop = 0;

fifoCount = mpu.getFIFOCount();

GetPacketLoopCount = 0;

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

while (fifoCount < packetSize && GetPacketLoopCount < max_loops)

{

GetPacketLoopCount++;

fifoCount = mpu.getFIFOCount();

delay(2);

}

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

if (GetPacketLoopCount >= max_loops)

{

return 0;

}

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

mpu.getFIFOBytes(data, packetSize);

return 1;

}

#pragma endregion MPU5060 Support

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

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

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

double prev_hdg = 0;

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

//DEBUG!!

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

Kp, Ki, Kd, degPersec);

//DEBUG!!

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

prev_hdg = IMUHdgValDeg;

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

//init some stuff

TurnRateOutput = 0;

TurnRateSetPoint_DPS = degPersec;

MsecSinceLastTurnRateUpdate = 0;

Ival = 0; lastErr = 0;

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_HALF);

while (true)

{

CheckForUserInput();

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

if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

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

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

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

IMUHdgValDeg, prev_hdg, deltaDeg);

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

deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

}

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

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

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

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)

TurnRateOutput = MAX_SPIN_MOTOR_SPEED;

else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)

TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

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

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

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

//DEBUG

prev_hdg = IMUHdgValDeg;

}

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

while (true)
{
	CheckForUserInput();

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

		UpdateIMUHdgValDeg();
		float deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

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

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

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

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

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

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

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

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

		prev_hdg = IMUHdgValDeg;
	}
}

while (true)

{

CheckForUserInput();

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

if (MsecSinceLastTurnRateUpdate >= TURN_RATE_UPDATE_INTERVAL_MSEC)

{

MsecSinceLastTurnRateUpdate -= TURN_RATE_UPDATE_INTERVAL_MSEC;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

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

while (abs(deltaDeg) > TurnRateSetPoint_DPS)

{

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

IMUHdgValDeg, prev_hdg, deltaDeg);

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

deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

}

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

double error = TurnRateSetPoint_DPS - TurnRateVal_DPS;

double dInput = (TurnRateVal_DPS - lastTurnRateVal_DPS);

Ival += (error);

double dErr = (error - lastErr);

/*Compute PID Output*/

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

/*Remember some variables for next time*/

lastErr = error;

lastTurnRateVal_DPS = TurnRateVal_DPS;

if (TurnRateOutput > MAX_SPIN_MOTOR_SPEED)

TurnRateOutput = MAX_SPIN_MOTOR_SPEED;

else if (TurnRateOutput < MIN_SPIN_MOTOR_SPEED)

TurnRateOutput = MIN_SPIN_MOTOR_SPEED;

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

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

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

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

//DEBUG

prev_hdg = IMUHdgValDeg;

}

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

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

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

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

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

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

And then 30 deg/sec on benchtop and carpet

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

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

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

28 May 2021 Update:

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

31 May 2021 Update:

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

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

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

Stay tuned!

Frank

New Batteries for Wall-E2

Leave a reply

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

with these

Panasonic 18650G-A 3500 mAH 3.7V LiPo battery

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

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

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

Stay tuned,

Frank

Turn Rate PID Tuning, Part II

Leave a reply

Posted 14 May 2021,

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

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

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

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

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

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

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

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

if(myPID.Compute())

{

//compute the new turn rate

//use the PID output term to control something

...

}

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

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

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

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

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

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

if (bTimeForNavUpdate) //set true in ISR

{

bTimeForNavUpdate = false;

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

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

float deltaDeg = IMUHdgValDeg - prev_hdg;

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

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

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

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

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput + MOTOR_SPEED_HALF);

prev_hdg = IMUHdgValDeg;

}

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

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

void PID::SetSampleTime(int NewSampleTime)

{

if (NewSampleTime > 0)

{

double ratio = (double)NewSampleTime

/ (double)SampleTime;

ki *= ratio;

kd /= ratio;

SampleTime = (unsigned long)NewSampleTime;

}

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

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

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

    SampleTime = (unsigned long)NewSampleTime;
}

void PID::SetSampleTime(int NewSampleTime)

{

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

if (NewSampleTime > 0)

{

double ratio = (double)NewSampleTime

/ (double)SampleTime;

ki *= ratio;

kd /= ratio;

//SampleTime = (unsigned long)NewSampleTime;

}

SampleTime = (unsigned long)NewSampleTime;

}

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

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

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

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

SetLeftMotorDirAndSpeed

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

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

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

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

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

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

	MsecSinceLastTurnRateUpdate = 0;

	while (true)
	{
		CheckForUserInput();

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

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

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

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

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

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

			prev_hdg = IMUHdgValDeg;
		}
	}
}

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

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

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

double prev_hdg = 0;

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

//DEBUG!!

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

kp, ki, kd, degPersec);

//DEBUG!!

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

TurnRateOutput = 0;

TurnRatePID.SetMode(AUTOMATIC);

TurnRatePID.SetTunings(kp, ki, kd);

TurnRateSetPoint = degPersec;

//set the limits

//TurnRatePID.SetOutputLimits(10, 100);

TurnRatePID.SetOutputLimits(MOTOR_SPEED_LOW, MOTOR_SPEED_MAX);

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

UpdateIMUHdgValDeg();

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

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

MsecSinceLastTurnRateUpdate = 0;

while (true)

{

CheckForUserInput();

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

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg;

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

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

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

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

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

SetLeftMotorDirAndSpeed(!b_ccw, TurnRateOutput);

SetRightMotorDirAndSpeed(b_ccw, TurnRateOutput);

//DEBUG!!

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

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

//DEBUG

prev_hdg = IMUHdgValDeg;

}

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

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

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

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

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

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

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

	MsecSinceLastTurnRateUpdate = 0;

	while (true)
	{
		CheckForUserInput();

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

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

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

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

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

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

			StopBothMotors();

			prev_hdg = IMUHdgValDeg;
		}
	}
}

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

{

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

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

double prev_hdg = 0;

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

//DEBUG!!

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

kp, ki, kd, degPersec);

//DEBUG!!

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

TurnRateOutput = 0;

TurnRatePID.SetMode(AUTOMATIC);

TurnRatePID.SetTunings(kp, ki, kd);

TurnRateSetPoint = degPersec;

//set the limits

TurnRatePID.SetOutputLimits(10, 100);

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

UpdateIMUHdgValDeg();

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

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

uint32_t prevUsec = micros();//initialize

uint32_t nowUsec = prevUsec;

MsecSinceLastTurnRateUpdate = 0;

while (true)

{

CheckForUserInput();

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

if (MsecSinceLastTurnRateUpdate >= 100)

{

MsecSinceLastTurnRateUpdate -= 100;

UpdateIMUHdgValDeg();

float deltaDeg = IMUHdgValDeg - prev_hdg;

prev_hdg = IMUHdgValDeg;

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

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

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

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

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

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

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

digitalWrite(CHG_CONNECT_LED_PIN, HIGH);

SetLeftMotorDirAndSpeed(!b_ccw, MOTOR_SPEED_FULL);

SetRightMotorDirAndSpeed(b_ccw, MOTOR_SPEED_FULL);

delay(TurnRateOutput);

StopBothMotors();

digitalWrite(CHG_CONNECT_LED_PIN, LOW);

//DEBUG!!

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

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

//DEBUG

StopBothMotors();

prev_hdg = IMUHdgValDeg;

}

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

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

Here’s another run, this time on carpet:

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

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

And on carpet

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

Stay Tuned!

Frank

Wall Parallel Find PID Tuning

Leave a reply

Posted 10 April 2021

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

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

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

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

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

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

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

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

Kp = 0.5Kc = 300

Ki = 0.45Kc = 270

Kd = 0.6Kc = 360

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

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

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

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

And here’s the Excel plot from this same run

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

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

And here’s the Excel plot for the run

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

Serial Bridge over WiFi using ESP32 DevKitC

Leave a reply

Posted 09 April 2021

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

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

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

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

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

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

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

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

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

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

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

Pinning

COM0 Rx <-> GPIO21

COM0 Tx <-> GPIO01

COM1 Rx <-> GPIO16

COM1 Tx <-> GPIO17

COM2 Rx <-> GPIO15

COM2 Tx <-> GPIO04

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

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

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

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

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

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

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

Frank

Turn Rate PID Tuning

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