chassis | Paynter's Palace

Posted 20 October 2021

I have been struggling for years now with Wall-E2’s reluctance to turn smoothly, and I have been looking for ways to improve the situation. I have even thought of going back to the 3-wheel form-factor with two driven wheels and a 3rd caster wheel, just for the ability to make smooth angle adjustments. However, the 3-wheel layout has its own problems, like a tendency to get stuck when one or both drive wheel loses traction, and a tendency for the caster wheel axle to accumulate carpet fibers to the point where it won’t turn.

Recently I ran across a blog post called ‘4-wheeled robot design basics and challenges‘ that seemed to describe Wall-E2’s turning difficulties pretty accurately. In addition, this post went on to suggest how the 4-wheel form-factor could be modified to minimize the high side-slip friction exhibited by most 4-wheel drive robots. The author suggested that by reducing the front-back dimension and increasing the side-side dimension, the side-slip friction exhibited during turns could be minimized. The author showed (literally) that the problem with a traditional 4-wheel form factor is that the front and back wheel pairs have non-intersecting turn radii, as shown in the following animated GIF from the RonRobotics site:

Traditional 4-wheel robot form-factor exhibiting high side-slip friction during turns

If the form-factor is changed from the traditional ‘long and narrow’ style to a more ‘short and wide’ style, then the turning situation is more like the following animated GIF:

4-wheel drive form factor to minimize side-slip friction during turns

The author didn’t provide any numerical guidelines for how short and how wide, but the above animated GIF provides a pretty good idea.

I decided to see if I could use TinkerCad to model a form factor that might be more like the ‘short and wide’ form-factor. From previous work I already had most of the individual elements (wheels, motors, battery compartment, charger module, etc), so I did a ‘FrankenRobot’ copy as shown below:

traditional (left) and ‘short/wide’ form-factor (right). Orange disk represents turning circle

The orange disk shown in the above image was intended to model the turning circle. I manually adjusted the form factor until the turning circle more-or-less matched the layout in the ‘design 3’ image from the RonRobitics site.

After playing with this for a while, I came up with the following ‘final’ design for my new Wall-E3 short/wide robot:

Short-Wide Robot Version. Battery pack in green, TPS5100 charger in red, charging probe in orange

With the above form factor, there is plenty of room for the battery pack (4 x 18650 Li-ion) and the TPS51000 charging module. After arriving at the above layout, I started working fabrication details. At first I thought this was a perfect opportunity to use some Adafruit 20x20mm slotted extrusions and associated hardware I have had hanging around for years looking for a project. However, as I got into the design in TinkerCad, I realized I had a problem, as shown in the following screenshot:

Short-Wide Robot chassis using Adafruit 20x20mm slotted extrusions

The motors must be mounted either above or below the extrusion. This could be effected using the Adafruit ‘T’ plate, as shown above, but then the question becomes – “what to do about a bottom plate”? I could put in a second set of extrusions, but that would severely reduce ground clearance. I could maybe bend the bottom part of the ‘T’ plate to support a bottom plate, but bending a cast aluminum piece didn’t seem like a good idea. In addition, an even bigger problem was how to mount the top plate. The obvious solution would be to drill 4 holes in the top plate, and use the slotted channel threaded insert hardware to mount the plate. But, how does that work – the threaded insert pieces are made to slide freely in the channel until tightening the mounting screw causes them to bind to the sides of the channel – all fine and good, but that won’t work for a blind mounting arrangement; the inserts could be anywhere in (or out of) the channel – oops! I thought I might be able to find a way of fixing the inserts into place, but that didn’t seem like a good idea either. I could simply drill and tap 4 holes around the periphery of the extrusions, but then I’m using expensive extrusions just like flat plate – bummer!

Eventually I realized that a standard U-channel from McMaster Carr should do the trick. The motors can be mounted through the ‘bottom’ of the U, and the top/bottom plates can be mounted to the top/bottom of the U-channel. Much simpler, and because I can mount the motors low in the U-channel, I can maximize ground clearance – yay!

27 October 2021 Update:

The first try at McMaster-Carr U-channel wasn’t too successful – it was much thicker and heavier than I realized from the drawings, so I went back to their site and re-ordered a much lighter (and nicer) U-channel style. The following photo shows the difference.

First order on the left, second on the right

I got 2ea 24″ lengths of the lighter U-channel, and as an experiment I tried cutting 45º bevels on both pieces to see how I might form right angles for the chassis. This worked out pretty nicely. In addition, I cut a 1/2″ section from the thicker U-channel, and then cut it in half to produce 2ea right-angle brackets for use in connecting two pieces of a right-angle corner. The following photos show the results:

I was very pleased with my first try at forming a right angle corner for my new chassis – it looks like using pieces of the thicker U-channel for connecting brackets will work. I don’t think I’ll need more than two screws per corner, as I will also have top & bottom plates that will add tremendously to the overall structure rigidity. Now I just have to get the dimensions correct so that my already-cut top/bottom plates will fit, and then do all the other stuff to produce a finished ‘Wall-E3’ chassis.

02 November 2021 Update

After the usual trials and tribulations, I think I have produced a decent 200 x 230 mm chassis, as shown in the following photos:

One minor ‘gotcha’ to using the thinner sidewall U-channel is that I managed to strip the threads on a couple of the threaded holes as I was installing the top plate. Fortunately I had some 3mm press-fit nuts left over from Wall-E2 and was able to convert the threaded hole to a press-fit nut installation without too much problem. Right now I have two holes converted to press-fit, but I suspect all sixteen holes will need to be treated at some point. So, back to McMaster-Carr where I ordered another 100 press-fit nuts, along with some additional 3x6mm flathead screws.

By the way, just as a plug for McMaster-Carr: I ordered and received a 90º countersink bit so I can use flat-head screws on the chassis, but when I received it I realized the tip of the bit wouldn’t fit into my 3mm holes – bummer! So, I contacted McMaster-Carr and asked if I could return it for a refund, and (well within the 30 minute reply guarantee) I got a note back that they would refund the cost of the order, but I didn’t have to return the item – wow! I have been a loyal MC customer for decades, and they continue to impress me with their responsiveness and speedy deliveries. The next time you have a need for hardware, think of McMaster-Carr.

The next step is to add the motor drivers and motors to the prototype setup to wring out any mechanical issues there. As can be seen in the following photo, the current Wall-E2 setup has the motors mounted through the side walls equidistant from top and bottom. However, I plan to mount the motors as close as possible to the lower edge of the U-channel, so as to achieve the largest possible terrain clearance.

To facilitate this I designed and created a ‘motor mounting template’ with motor axle location holes that fits inside the sidewall U-channel, as shown below:

The template has two holes per motor location to allow the template to be placed either way (side tabs toward the top plate, or side tabs toward bottom plate) and still have one set of holes in the proper vertical location.

06 November 2021 Update:

As described in the companion post to this one, I assembled a basic system on a blank aluminum plate as shown in the following photo:

Basic system assembled on blank aluminum plate

After fabricating the aluminum chassis, I started transferring pieces from the prototype to the new chassis, as shown below

After transferring everything except the Teensy module and the 5V LDO regulator module, this is what the new chassis looks like:

Still lots of work, but it is now clear that everything fits comfortably into the chassis, including both motor drivers which are mounted on top of the chassis on the Wall-E2 robot. This will clear up lots of space on the top plate, so things should be a lot neater with Wall-E3.

07 November 2021 Update:

After getting the basic component layout established, I decided to see if I could get the robot to actually move forwards and backwards under computer control via the HC-05 Bluetooth link. So, I transferred all four wheels from Wall-E2 to Wall-E3, and temporarily hot-glued the Teensy 3.5 and its accompanying 5V LDO regulator/HC-05 module to the battery pack, as shown in the following photo:

And, after fixing the problem of one set of motors turning in the opposite direction as the other set, I was able to move Wall-E3 forwards and backwards utilizing the wireless link from my PC to the robot, as shown in the following short video:

The next thing I tried was some manually controlled ‘spin’ turns. To do this I added an overload to the function that does normal rate-controlled turns using an MPU6050 IMU sensor to make a turn through a defined angle span. The overloaded function simply starts a CW or CCW turn and waits for any key to stop the turn. Here’s a short video showing the result

CW & CCW ‘spin’ turns

As can be seen from the above video, Wall-E3’s turn behavior is MUCH smoother than that of Wall-E3, and is quite close to the behavior exhibited in the ‘wide’ form factor GIF that prompted the Wall-E3 redesign (repeated below for comparison):

With such good results from the above turns on hard-surface flooring, I decided to try my luck with my test carpet patch. Turns on carpet with Wall-E2 have always been a jerky procedure, with PID algorithm having to ramp motor current way up to overcome wheel side friction, and then way back down again when the wheels broke free. With Wall-E3, the turning behavior on carpet was indistinguishable from its behavior on hard flooring – both cases exhibited very smooth turning behavior around an almost constant turning axis, as shown in the following short video.

For comparison, here is Wall-E2 making a 180⁰ turn on carpet:

And here is the plot of motor speed vs time for the turn.

As can be seen from the above, the motor speed goes from max (255) to min (50) several times per second as the wheels alternately grip and then slide. In contrast, the Wall-E3 turns on both carpet and hard flooring were done with a constant half-speed (125) motor current, resulting in an angular rate of about 70 ⁰ /sec on both carpet and hard surface.

20 November 2021 Update:

Unfortunately, when I tried to add MPU6050 capability to Wall-E3, I went down a rabbit hole due to changes in Jeff Rowberg’s I2CDevlib library for the MPU6050 that have made it incompatible with Teensy 3.x targets (see this post for the gory details). After chasing Alice for way too long, I finally figured out that I had two choices. I could use an older (as in 2 years older) version of the i2cdevlib libraries with a Teensy, or I could use the current versions with an Arduino UNO or MEGA. Because at this point I already had an Arduino UNO running with a MPU6050 and the new libraries, I decided to continue on that track and see if I could tune the PID engine for smooth(er) turns with the new form factor. Here’s the physical setup:

Wall-E3 with Arduino UNO/MPU6050 setup running the motors

Here’s the code:

/*
    Name:       Arduino_MPU6050_6Axis_MotionApps612.ino
    Created:	11/20/2021 7:15:44 AM
    Author:     FRANKNEWXPS15\Frank
*/

/*
		Name:       T35_MPU6050_Demo.ino
		Created:	11/13/2021 8:52:56 AM
		Author:     FRANKNEWXPS15\Frank
*/
#pragma region INCLUDES
#include "MPU6050_6Axis_MotionApps612.h" 
#include "I2Cdev.h" //modified so it #includes i2c_t3.h
#include "I2C_Anything.h"
#include <PrintEx.h> //allows printf-style printout syntax
#include <elapsedMillis.h>
#pragma endregion INCLUDES

//#define MPU6050_I2C_ADDR 0x68
#define MPU6050_I2C_ADDR 0x69

MPU6050 mpu(MPU6050_I2C_ADDR);
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing


#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
bool bFirstTime = true;
#pragma endregion MPU6050_SUPPORT

#pragma region MISC_PINS
const int I_RunPin = A0;
const int I_TotPin = A1;
const int ChgConnectPin = 4;
const int BattV_Pin = A2;
#pragma endregion MISC_PINS

#pragma region MOTOR_PINS
//11/04/21 Now using Pololu VNH5019 drivers for all 4 motors
//const int InA_Left = 22;
//const int InB_Left = 21;
//const int Spd_Left = 23;
//
//const int InA_Right = 34;
//const int InB_Right = 33;
//const int Spd_Right = 35;

//11/14/21 chg to UNO pinouts
const int Spd_Left = 10;
const int InA_Left = 11;
const int InB_Left = 12;

const int Spd_Right = 5;
const int InA_Right = 6;
const int InB_Right = 7;
#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;
const bool TURNDIR_CCW = true;
const bool TURNDIR_CW = false;

//Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;
bool bIsForwardDir = true; //default is foward direction


#pragma endregion Motor Parameters

#pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS
elapsedMillis MsecSinceLastTurnRateUpdate;

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

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

//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 30;
//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 60;
const int TURN_RATE_UPDATE_INTERVAL_MSEC = 100;
const int TURN_START_SPEED = MOTOR_SPEED_QTR; //added 11/14/21

double Prev_HdgDeg = 0;
float tgt_deg;
float timeout_sec;

//05/31/21 latest & greatest PID values
//double TurnRate_Kp = 2.5;
//double TurnRate_Ki = 2.0;
//double TurnRate_Kd = 0.1;
//double TurnRate_Kp = 5;

//almost perfect for 90deg/sec 
//double TurnRate_Kp = 3; //almost perfect for 90deg/sec
//double TurnRate_Ki = 1;
//double TurnRate_Kd = 0;

//11/16/21 almost perfect for 45 deg/sec, 30mSec
//double TurnRate_Kp = 1.8; 
//double TurnRate_Ki = 0.5;
//double TurnRate_Kd = 0;

//11/16/21 almost perfect for 90 deg/sec, 100mSec
//double TurnRate_Kp = 2.0; 
//double TurnRate_Ki = 1.5;
//double TurnRate_Kd = 0;


//11/16/21 almost perfect for 45 deg/sec, 100mSec
//11/17/21 and pretty good for 90 deg/sec too!
double TurnRate_Kp = 1.8;
double TurnRate_Ki = 1.0;
double TurnRate_Kd = 0.4;

double TurnRatePIDSetPointDPS, TurnRatePIDOutput;
double TurnRatePIDInput = 15.f;
//PID TurnRatePID(&TurnRatePIDInput, &TurnRatePIDOutput, &TurnRatePIDSetPointDPS, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, DIRECT);

const float HDG_NEAR_MATCH_VAL = 0.8; //slow the turn down here
const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 05/17/20
const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started
#pragma endregion HEADING_AND_RATE_BASED_TURN_PARAMETERS

elapsedMillis MsecSinceLastUpdate;

void setup()
{
	Serial.begin(115200);
	//Serial.begin(57600);
	delay(2000); //10/06/21 - just use fixed delay instead

#pragma region MPU6050_INIT
	Serial.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"));

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

		Serial.printf("Just after MPU6050 Init\n");
		Serial.printf("Calibrating...\n");
		mpu.CalibrateGyro(); //using default value of 15
		Serial.printf("Retrieving Calibration Values\n\n");
		mpu.PrintActiveOffsets();

		//loop heading display until stabilized
		mySerial.printf("\nMsec\tHdg\n");

		UpdateIMUHdgValDeg();
		Prev_HdgDeg = IMUHdgValDeg;
		delay(100);
		UpdateIMUHdgValDeg();

		mySerial.printf("%lu\t%2.3f\t%2.3f\n", millis(), IMUHdgValDeg, Prev_HdgDeg);
		while (abs(IMUHdgValDeg - Prev_HdgDeg) > 0.1f)
		{
			mySerial.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg);
			Prev_HdgDeg = IMUHdgValDeg;
			delay(100);
			UpdateIMUHdgValDeg();
		}

		mySerial.printf("MPU6050 Ready at %2.2f Sec with delta = %2.3f\n", millis() / 1000.f, IMUHdgValDeg - Prev_HdgDeg);
		bMPU6050Ready = true;

		delay(1000);

	}
	else //MPU6050 Init failed for some reason
	{
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		Serial.printf("DMP Initialization failed (code %d\n", devStatus);
	}
#pragma endregion MPU6050_INIT

#pragma region MOTOR_PIN_INIT
	pinMode(Spd_Left, OUTPUT);
	pinMode(InA_Left, OUTPUT);
	pinMode(InB_Left, OUTPUT);

	pinMode(Spd_Right, OUTPUT);
	pinMode(InA_Right, OUTPUT);
	pinMode(InB_Right, OUTPUT);
#pragma endregion MOTOR_PIN_INIT

#pragma region MISC_PINS_INIT
	pinMode(I_RunPin, INPUT);
	pinMode(I_TotPin, INPUT);
	pinMode(ChgConnectPin, INPUT_PULLUP);
	pinMode(BattV_Pin, INPUT);
#pragma endregion MISC_PINS_INIT

	//mySerial.printf("Starting Turns Testing\n");
	//delay(1000);

	//mySerial.printf("CCW 45deg at 30deg/sec\n");
	//delay(1000);
	//SpinTurn(true, 45, 30);

	//mySerial.printf("CW 45deg at 30deg/sec\n");
	//delay(1000);
	//SpinTurn(false, 45, 30);

	//mySerial.printf("CCW 90deg at 30deg/sec\n");
	//delay(1000);
	//SpinTurn(true, 90, 30);

	//mySerial.printf("CW 90deg at 30deg/sec\n");
	//delay(1000);
	//SpinTurn(false, 90, 30);

	mySerial.printf("Tests Complete.  Enter 'C' for manual control\n");



	//mySerial.printf("Msec\tHdg\n", millis(), IMUHdgValDeg);
	MsecSinceLastUpdate = 0;
}

void loop()
{
	CheckForUserInput();

	//if (MsecSinceLastUpdate >= 200)
	//{
	//  MsecSinceLastUpdate -= 200;
	//  UpdateIMUHdgValDeg();
	//  mySerial.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg);
	//}
}

#pragma region HDG_BASED_TURN_SUPPORT
float GetHdgMatchVal(float tgt_deg, float cur_deg)
{
	//Purpose:  Compute the match ratio between two compass headings
	//Inputs:
	//	tgt_deg = float representing target heading in +/-180 range
	//	IMUHdgValDeg = float representing sensor yaw value in +/-180 deg range
	//Outputs:
	//	returns result of 1 - abs(Tgt_deg - Hdg_deg)/180, all angles in 0-360 deg range
	//Plan:
	//	Step1: convert both inputs to 0-360 deg range
	//	Step2: compute match ratio
	//Notes:
	//	formula from https://gis.stackexchange.com/questions/129954/comparing-compass-bearings

//Step1: convert both inputs to 0-360 deg range
	float tgthdg = (tgt_deg < 0) ? tgt_deg + 360 : tgt_deg;
	float curhdg = (cur_deg < 0) ? cur_deg + 360 : cur_deg;

	//Step2: compute match ratio
	float match_ratio = 1 - abs(tgthdg - curhdg) / 180;

	//DEBUG!!
		//mySerial.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio);
	//DEBUG!!
	return abs(match_ratio);
}

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

	bool retval = false;

	int flag = mpu.GetCurrentFIFOPacket(fifoBuffer, packetSize); //08/01/21 now using library version

	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
}

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
	//	11/14/21 removed 'first time skip' block; added motor start before entering loop

	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);
	//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);
	//DEBUG!!

	//no need to continue if the IMU isn't available
	if (!dmpReady)
	{
		mySerial.printf("DMP Failure - returning FALSE\n");
		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

	//08/26/21 re-wrote using 3-value array to make sure initial heading is a steady value
	UpdateIMUHdgValDeg();

	int retries = 0;
	if ((IMUHdgValDeg == 180.f || IMUHdgValDeg == 0.f) && retries < 5)
	{
		//DEBUG!!
		mySerial.printf("Got 180.00 or 0.00 exactly (%2.3f) from IMU - retrying %d...\n", IMUHdgValDeg, retries);
		//DEBUG!!
		UpdateIMUHdgValDeg();
		retries++;
		delay(100);
	}

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

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

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

	//11/14/21 start motors before entering loop.  For CCW turn, left motors go backwards, right ones forward
	int startSpd = (degPersec <= 45) ? TURN_START_SPEED : 2 * TURN_START_SPEED;
	SetLeftMotorDirAndSpeed(!b_ccw, startSpd);
	SetRightMotorDirAndSpeed(b_ccw, startSpd);



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

	//DEBUG!!
	mySerial.printf("Msec\tHdg\tPrvHdg\tdHdg\tRate\ttgtDPS\terr\tKp*err\tIval\tKd*Derr\tspeed\tMatch\tSlope\n");
	//DEBUG!!

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

		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
			//11/14/21 don't need this now, as motor is started at least TURN_RATE_UPDATE_INTERVAL_MSEC prior to loop entry
			//if (bFirstIMUHdg)
			//{
			//	bFirstIMUHdg = false;
			//	Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time
			//}
			//else
			//{
			TurnRatePIDOutput = PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd);

			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;

			//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\t%2.2f\t%2.2f\n",
			//	millis(),
			//	IMUHdgValDeg,
			//	Prev_HdgDeg,
			//	dHdg,
			//	rate,
			//	degPersec,
			//	lastError,
			//	TurnRate_Kp * lastError,
			//	TurnRate_Ki * lastIval,
			//	TurnRate_Kd * lastDerror,
			//	speed,
			//	curHdgMatchVal,
			//	matchSlope);
			// 
			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\t%2.2f\t%2.2f\n",
				millis(),
				IMUHdgValDeg,
				Prev_HdgDeg,
				dHdg,
				rate,
				degPersec,
				lastError,
				TurnRate_Kp * lastError,
				lastIval,
				TurnRate_Kd * lastDerror,
				speed,
				curHdgMatchVal,
				matchSlope);

			//DEBUG!!

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

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

			//mySerial.printf("curHdgMatchVal = %2.2f, prevHdgMatchVal = %2.2f, matchslope = %2.2f, bDoneTurning = %d\n",
			//	curHdgMatchVal,
			//	prevHdgMatchVal,
			//	matchSlope,
			//	bDoneTurning);

			prevHdgMatchVal = curHdgMatchVal; //07/31/21 moved below bDoneTurning chk so can use prevHdgMatchVal vs curHdgMatchVal in slope check
		//}
		}
		//
		//		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;
		//		}
		//
		//		if (bDoneTurning)
		//		{
		//			mySerial.printf("Completed turn with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal);
		//		}
	}
	//
		//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;
}

//09/03/21 chg fcn type from void to double to eliminate double& output param
double PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd)
{
	//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);
	lastIval += (Ki * error);

	//11/16/21 rev to clamp integral term
	lastIval = (lastIval > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : lastIval;
	lastIval = (lastIval < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : lastIval;
	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*/
	//11/16/21 rev to subtract differential term rather than add.
	//double output = Kp * error + Ki * lastIval + Kd * dErr;
	double output = Kp * error + lastIval - Kd * dErr;

	//11/16/21 now clamping output value as well as I term
	output = (output > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : output;
	output = (output < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : output;

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

	return output; //added 09/03/21
}

#pragma endregion HDG_BASED_TURN_SUPPORT

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

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

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

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

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

//09/08/10 modified for DRV8871 motor driver
//11/04/21 modified for Pololu VNH5019 motor driver
void StopLeftMotors()
{
	digitalWrite(InA_Left, LOW);
	digitalWrite(InB_Left, LOW);
	analogWrite(Spd_Left, MOTOR_SPEED_OFF);
}

//11/04/21 modified for Pololu VNH5019 motor driver
void StopRightMotors()
{
	digitalWrite(InA_Right, LOW);
	digitalWrite(InB_Right, LOW);
	analogWrite(Spd_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)
{
	speed = (speed < MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
	speed = (speed > MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;
	//mySerial.printf("In SetLeftMotorDirAndSpeed(%s, %d)\n",
	//	(bIsFwd == true) ? "true" : "false", speed);
	//11/04/21 fwd for right motors is CCW when looking at shaft

#ifndef NO_MOTORS
	if (bIsFwd)
	{
		digitalWrite(InA_Left, LOW);
		digitalWrite(InB_Left, HIGH);
		analogWrite(Spd_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(InA_Left, HIGH);
		digitalWrite(InB_Left, LOW);
		analogWrite(Spd_Left, speed);
	}
#endif // !NO_MOTORS
}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)
{
	speed = (speed < MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
	speed = (speed > MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;
	//mySerial.printf("In SetRightMotorDirAndSpeed(%s, %d)\n",
	//	(bIsFwd == true) ? "true" : "false", speed);
	//11/04/21 fwd for right motors is CW when looking at shaft

#ifndef NO_MOTORS

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

#endif // !NO_MOTORS
}

#pragma endregion Motor Support Functions

void CheckForUserInput()
{
	const int bufflen = 80;
	char buff[bufflen];
	memset(buff, 0, bufflen);
	float OffsetCm;
	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, HEX); //chg to HEX 02/18/20

		//02/18/20 experiment with multiple commands
		switch (incomingByte)
		{
		case 0x43: //ASCII 'C'
		case 0x63: //ASCII 'c'
#pragma region MANUALCONTROL
			//enter infinite loop for direct remote control
			Serial.println(F("ENTERING COMMAND MODE:"));
			Serial.println(F("0 = 180 deg CCW Turn"));
			Serial.println(F("1 = 180 deg CW Turn"));
			Serial.println(F("A = Back to Auto Mode"));
			Serial.println(F("S = Stop"));
			Serial.println(F("F = Forward"));
			Serial.println(F("R = Reverse"));
			Serial.println(F(""));
			Serial.println(F("       Faster"));
			Serial.println(F("\t8"));
			Serial.println(F("Left 4\t5  6 Right"));
			Serial.println(F("\t2"));
			Serial.println(F("       Slower"));

			StopBothMotors();
			int speed = 0;

			bool bAutoMode = false;
			while (!bAutoMode)
			{
				if (Serial.available() > 0)
				{
					// read the incoming byte:
					int incomingByte = Serial.read();

					//mySerial.printf("Got %x\n", incomingByte);

					switch (incomingByte)
					{
						//case 0x54: //ASCII 'T'
						//case 0x74: //ASCII 't'
						//	Serial.println(F("Toggle TIMER5 Enable/Disable"));
						//	if (TIMSK5 == 0)
						//	{
						//		Serial.println(F("Enable TIMER5"));
						//		//TIMSK5 |= OCIE5A;
						//		TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt
						//		mySerial.printf("TIMSK5 = %x\n", TIMSK5);
						//	}
						//	else
						//	{
						//		Serial.println(F("Disable TIMER5"));
						//		TIMSK5 = 0;
						//		mySerial.printf("TIMSK5 = %x\n", TIMSK5);
						//	}
						//	break;
					case 0x30: //Dec '0'
						Serial.println(F("CCW 180 deg Turn"));
						//RollingTurn(true, bIsForwardDir, 180);
						//SpinTurn(true, 180, 45);
						SpinTurn(true, 180, 90);
						MoveAhead(speed, speed);
						break;
					case 0x31: //Dec '1'
						Serial.println(F("CW 180 deg Turn"));
						//RollingTurn(false, bIsForwardDir, 180);
						SpinTurn(false, 180, 45);
						//MoveAhead(speed, speed);
						break;
					case 0x34: //Turn left 10 deg
						Serial.println(F("CCW 10 deg Turn"));
						//MoveAhead(speed, speed);
						//RollingTurn(true, bIsForwardDir, 10);
						//SpinTurn(true, 10, 30);
						//SpinTurn(true, 30, 30);

						//added 05/03/20
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							MoveReverse(speed, speed);
						}
						break;
					case 0x36: //Turn right 10 deg'
						mySerial.print("CW 10 deg Turn\n");
						MoveAhead(speed, speed);
						//RollingTurn(false, bIsForwardDir, 10);
						//SpinTurn(false, 10, 30);
						//SpinTurn(false, 45, 30);

						//added 05/03/20
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							//MoveReverse(speed, speed);
						}
						break;
					case 0x38: //Speed up 
						speed += 50;
						speed = (speed >= MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed;
						mySerial.printf("Speeding up: speed now %d\n", speed);
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							//MoveReverse(speed, speed);
						}
						break;
					case 0x32: //Slow down 
						speed -= 50;
						speed = (speed < 0) ? 0 : speed;
						mySerial.printf("Slowing down: speed now %d\n", speed);
						if (bIsForwardDir)
						{
							MoveAhead(speed, speed);
						}
						else
						{
							//MoveReverse(speed, speed);
						}
						break;
					case 0x35: //05/07/20 changed to use '5' vs 'S'
						Serial.println(F("Stopping Motors!"));
						StopBothMotors();
						speed = 0;
						break;
					case 0x41: //ASCII 'A'
					case 0x61: //ASCII 'a'
						StopBothMotors();
						Serial.println(F("Re-entering AUTO mode"));
						bAutoMode = true;
						break;
					case 0x52: //ASCII 'R'
					case 0x72: //ASCII 'r'
						Serial.println(F("Setting both motors to reverse"));
						bIsForwardDir = false;
						//MoveReverse(speed, speed);
						break;
					case 0x46: //ASCII 'F'
					case 0x66: //ASCII 'f'
						Serial.println(F("Setting both motors to forward"));
						bIsForwardDir = true;
						MoveAhead(speed, speed);
#pragma endregion MANUALCONTROL
						break;

#pragma region	WALL OFFSET TRACKING CASES
						//left side wall tracking
					case 0x4C: //ASCII 'L'
					case 0x6C: //ASCII 'l'
						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 Offset in cm */
						token = strtok(NULL, s);
						OffsetCm = atof(token);

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

						//TrackLeftWallOffset(Kp, Ki, Kd, OffsetCm);
						break;
					case 0x4D: //ASCII 'M'
					case 0x6D: //ASCII 'm'
						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 Offset in cm */
						token = strtok(NULL, s);
						OffsetCm = atof(token);

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

						//TrackRightWallOffset(Kp, Ki, Kd, OffsetCm);
						break;
#pragma endregion	WALL OFFSET TRACKING CASES
						Default :
						Serial.println(F("In Default Case: Stopping Motors!"));
						MoveAhead(0, 0);
						while (true)
						{

						}
					}
				}
			}
		}
	}
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

751

752

753

754

755

756

757

758

759

760

761

762

763

764

765

766

767

768

769

770

771

772

773

774

775

776

777

778

779

780

781

782

783

784

785

786

787

788

789

790

791

792

793

794

795

796

797

798

799

800

801

802

803

804

805

806

807

808

809

810

811

812

813

814

815

816

817

818

819

820

821

822

823

824

825

826

827

828

829

830

831

832

833

834

835

836

837

838

839

840

841

842

843

844

845

846

847

848

849

850

851

852

853

854

855

856

857

858

859

860

861

862

863

864

865

866

867

868

869

870

871

872

873

874

875

876

877

878

879

880

881

882

883

884

885

886

887

888

889

890

891

892

893

894

895

896

897

898

899

900

901

902

903

904

905

906

907

908

909

910

911

912

913

914

915

916

917

918

919

920

921

922

923

924

925

926

927

928

929

930

931

932

933

934

935

936

937

938

939

940

941

942

943

944

945

946

947

948

949

950

951

952

953

954

955

956

957

958

959

960

961

962

963

964

965

966

967

968

969

970

971

972

973

974

975

976

977

978

979

980

981

982

983

984

985

986

987

988

989

990

991

992

993

994

995

996

997

998

999

1000

1001

1002

1003

1004

1005

1006

1007

1008

1009

1010

1011

1012

1013

1014

1015

1016

1017

1018

1019

1020

1021

1022

1023

1024

1025

1026

1027

1028

1029

1030

1031

1032

1033

1034

1035

1036

1037

1038

1039

1040

1041

1042

1043

1044

1045

1046

1047

1048

1049

1050

1051

1052

1053

1054

1055

1056

1057

1058

1059

1060

1061

1062

1063

1064

1065

1066

1067

1068

1069

1070

1071

1072

1073

1074

1075

1076

1077

1078

1079

1080

1081

1082

1083

1084

1085

1086

1087

1088

1089

1090

1091

1092

1093

1094

1095

1096

1097

1098

1099

1100

1101

1102

1103

1104

1105

1106

1107

1108

1109

1110

1111

1112

1113

1114

1115

1116

1117

1118

1119

1120

1121

1122

1123

1124

1125

1126

1127

1128

Name: Arduino_MPU6050_6Axis_MotionApps612.ino

Created: 11/20/2021 7:15:44 AM

Author: FRANKNEWXPS15\Frank

Name: T35_MPU6050_Demo.ino

Created: 11/13/2021 8:52:56 AM

Author: FRANKNEWXPS15\Frank

#pragma region INCLUDES

#include "MPU6050_6Axis_MotionApps612.h"

#include "I2Cdev.h" //modified so it #includes i2c_t3.h

#include "I2C_Anything.h"

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

#include <elapsedMillis.h>

#pragma endregion INCLUDES

//#define MPU6050_I2C_ADDR 0x68

#define MPU6050_I2C_ADDR 0x69

MPU6050 mpu(MPU6050_I2C_ADDR);

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

#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

bool bFirstTime = true;

#pragma endregion MPU6050_SUPPORT

#pragma region MISC_PINS

const int I_RunPin = A0;

const int I_TotPin = A1;

const int ChgConnectPin = 4;

const int BattV_Pin = A2;

#pragma endregion MISC_PINS

#pragma region MOTOR_PINS

//11/04/21 Now using Pololu VNH5019 drivers for all 4 motors

//const int InA_Left = 22;

//const int InB_Left = 21;

//const int Spd_Left = 23;

//const int InA_Right = 34;

//const int InB_Right = 33;

//const int Spd_Right = 35;

//11/14/21 chg to UNO pinouts

const int Spd_Left = 10;

const int InA_Left = 11;

const int InB_Left = 12;

const int Spd_Right = 5;

const int InA_Right = 6;

const int InB_Right = 7;

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

const bool TURNDIR_CCW = true;

const bool TURNDIR_CW = false;

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

bool bIsForwardDir = true; //default is foward direction

#pragma endregion Motor Parameters

#pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS

elapsedMillis MsecSinceLastTurnRateUpdate;

const int MAX_PULSE_WIDTH_MSEC = 100;

const int MIN_PULSE_WIDTH_MSEC = 0;

const int MAX_SPIN_MOTOR_SPEED = 250;

const int MIN_SPIN_MOTOR_SPEED = 50;

//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 30;

//const int TURN_RATE_UPDATE_INTERVAL_MSEC = 60;

const int TURN_RATE_UPDATE_INTERVAL_MSEC = 100;

const int TURN_START_SPEED = MOTOR_SPEED_QTR; //added 11/14/21

double Prev_HdgDeg = 0;

float tgt_deg;

float timeout_sec;

//05/31/21 latest & greatest PID values

//double TurnRate_Kp = 2.5;

//double TurnRate_Ki = 2.0;

//double TurnRate_Kd = 0.1;

//double TurnRate_Kp = 5;

//almost perfect for 90deg/sec

//double TurnRate_Kp = 3; //almost perfect for 90deg/sec

//double TurnRate_Ki = 1;

//double TurnRate_Kd = 0;

//11/16/21 almost perfect for 45 deg/sec, 30mSec

//double TurnRate_Kp = 1.8;

//double TurnRate_Ki = 0.5;

//double TurnRate_Kd = 0;

//11/16/21 almost perfect for 90 deg/sec, 100mSec

//double TurnRate_Kp = 2.0;

//double TurnRate_Ki = 1.5;

//double TurnRate_Kd = 0;

//11/16/21 almost perfect for 45 deg/sec, 100mSec

//11/17/21 and pretty good for 90 deg/sec too!

double TurnRate_Kp = 1.8;

double TurnRate_Ki = 1.0;

double TurnRate_Kd = 0.4;

double TurnRatePIDSetPointDPS, TurnRatePIDOutput;

double TurnRatePIDInput = 15.f;

//PID TurnRatePID(&TurnRatePIDInput, &TurnRatePIDOutput, &TurnRatePIDSetPointDPS, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, DIRECT);

const float HDG_NEAR_MATCH_VAL = 0.8; //slow the turn down here

const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 05/17/20

const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started

#pragma endregion HEADING_AND_RATE_BASED_TURN_PARAMETERS

elapsedMillis MsecSinceLastUpdate;

void setup()

{

Serial.begin(115200);

//Serial.begin(57600);

delay(2000); //10/06/21 - just use fixed delay instead

#pragma region MPU6050_INIT

Serial.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"));

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

Serial.printf("Just after MPU6050 Init\n");

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

mpu.CalibrateGyro(); //using default value of 15

Serial.printf("Retrieving Calibration Values\n\n");

mpu.PrintActiveOffsets();

//loop heading display until stabilized

mySerial.printf("\nMsec\tHdg\n");

UpdateIMUHdgValDeg();

Prev_HdgDeg = IMUHdgValDeg;

delay(100);

UpdateIMUHdgValDeg();

mySerial.printf("%lu\t%2.3f\t%2.3f\n", millis(), IMUHdgValDeg, Prev_HdgDeg);

while (abs(IMUHdgValDeg - Prev_HdgDeg) > 0.1f)

{

mySerial.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg);

Prev_HdgDeg = IMUHdgValDeg;

delay(100);

UpdateIMUHdgValDeg();

}

mySerial.printf("MPU6050 Ready at %2.2f Sec with delta = %2.3f\n", millis() / 1000.f, IMUHdgValDeg - Prev_HdgDeg);

bMPU6050Ready = true;

delay(1000);

}

else //MPU6050 Init failed for some reason

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

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

Serial.printf("DMP Initialization failed (code %d\n", devStatus);

}

#pragma endregion MPU6050_INIT

#pragma region MOTOR_PIN_INIT

pinMode(Spd_Left, OUTPUT);

pinMode(InA_Left, OUTPUT);

pinMode(InB_Left, OUTPUT);

pinMode(Spd_Right, OUTPUT);

pinMode(InA_Right, OUTPUT);

pinMode(InB_Right, OUTPUT);

#pragma endregion MOTOR_PIN_INIT

#pragma region MISC_PINS_INIT

pinMode(I_RunPin, INPUT);

pinMode(I_TotPin, INPUT);

pinMode(ChgConnectPin, INPUT_PULLUP);

pinMode(BattV_Pin, INPUT);

#pragma endregion MISC_PINS_INIT

//mySerial.printf("Starting Turns Testing\n");

//delay(1000);

//mySerial.printf("CCW 45deg at 30deg/sec\n");

//delay(1000);

//SpinTurn(true, 45, 30);

//mySerial.printf("CW 45deg at 30deg/sec\n");

//delay(1000);

//SpinTurn(false, 45, 30);

//mySerial.printf("CCW 90deg at 30deg/sec\n");

//delay(1000);

//SpinTurn(true, 90, 30);

//mySerial.printf("CW 90deg at 30deg/sec\n");

//delay(1000);

//SpinTurn(false, 90, 30);

mySerial.printf("Tests Complete. Enter 'C' for manual control\n");

//mySerial.printf("Msec\tHdg\n", millis(), IMUHdgValDeg);

MsecSinceLastUpdate = 0;

}

void loop()

{

CheckForUserInput();

//if (MsecSinceLastUpdate >= 200)

//{

// MsecSinceLastUpdate -= 200;

// UpdateIMUHdgValDeg();

// mySerial.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg);

//}

}

#pragma region HDG_BASED_TURN_SUPPORT

float GetHdgMatchVal(float tgt_deg, float cur_deg)

{

//Purpose: Compute the match ratio between two compass headings

//Inputs:

// tgt_deg = float representing target heading in +/-180 range

// IMUHdgValDeg = float representing sensor yaw value in +/-180 deg range

//Outputs:

// returns result of 1 - abs(Tgt_deg - Hdg_deg)/180, all angles in 0-360 deg range

//Plan:

// Step1: convert both inputs to 0-360 deg range

// Step2: compute match ratio

//Notes:

// formula from https://gis.stackexchange.com/questions/129954/comparing-compass-bearings

//Step1: convert both inputs to 0-360 deg range

float tgthdg = (tgt_deg < 0) ? tgt_deg + 360 : tgt_deg;

float curhdg = (cur_deg < 0) ? cur_deg + 360 : cur_deg;

//Step2: compute match ratio

float match_ratio = 1 - abs(tgthdg - curhdg) / 180;

//DEBUG!!

//mySerial.printf("tgt\tcur\tmatch = %4.2f\t%4.2f\t%1.3f\n", tgthdg, curhdg, match_ratio);

//DEBUG!!

return abs(match_ratio);

}

float UpdateIMUHdgValDeg()

{

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

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

//Outputs:

// returns true if successful, otherwise false

// IMUHdgValDeg updated on success

//Plan:

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

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

//Step3: update IMUHdgValDeg with latest value

//Notes:

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

// 10/08/19 no longer need mpuIntStatus

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

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

bool retval = false;

int flag = mpu.GetCurrentFIFOPacket(fifoBuffer, packetSize); //08/01/21 now using library version

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

}

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

// 11/14/21 removed 'first time skip' block; added motor start before entering loop

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

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

//DEBUG!!

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

if (!dmpReady)

{

mySerial.printf("DMP Failure - returning FALSE\n");

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

//08/26/21 re-wrote using 3-value array to make sure initial heading is a steady value

UpdateIMUHdgValDeg();

int retries = 0;

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

{

//DEBUG!!

mySerial.printf("Got 180.00 or 0.00 exactly (%2.3f) from IMU - retrying %d...\n", IMUHdgValDeg, retries);

//DEBUG!!

UpdateIMUHdgValDeg();

retries++;

delay(100);

}

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

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

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

//11/14/21 start motors before entering loop. For CCW turn, left motors go backwards, right ones forward

int startSpd = (degPersec <= 45) ? TURN_START_SPEED : 2 * TURN_START_SPEED;

SetLeftMotorDirAndSpeed(!b_ccw, startSpd);

SetRightMotorDirAndSpeed(b_ccw, startSpd);

elapsedMillis sinceLastTimeCheck = 0;

elapsedMillis sinceLastComputeTime = 0;

double lastError = 0;

double lastInput = 0;

double lastIval = 0;

double lastDerror = 0;

bool bFirstIMUHdg = true;

//DEBUG!!

mySerial.printf("Msec\tHdg\tPrvHdg\tdHdg\tRate\ttgtDPS\terr\tKp*err\tIval\tKd*Derr\tspeed\tMatch\tSlope\n");

//DEBUG!!

while (!bDoneTurning && !bTimedOut)

{

//11/06/20 now just loops between PID calcs

CheckForUserInput();

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

//11/14/21 don't need this now, as motor is started at least TURN_RATE_UPDATE_INTERVAL_MSEC prior to loop entry

//if (bFirstIMUHdg)

//{

// bFirstIMUHdg = false;

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

//}

//else

//{

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

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;

//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\t%2.2f\t%2.2f\n",

// millis(),

// IMUHdgValDeg,

// Prev_HdgDeg,

// dHdg,

// rate,

// degPersec,

// lastError,

// TurnRate_Kp * lastError,

// TurnRate_Ki * lastIval,

// TurnRate_Kd * lastDerror,

// speed,

// curHdgMatchVal,

// matchSlope);

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

millis(),

IMUHdgValDeg,

Prev_HdgDeg,

dHdg,

rate,

degPersec,

lastError,

TurnRate_Kp * lastError,

lastIval,

TurnRate_Kd * lastDerror,

speed,

curHdgMatchVal,

matchSlope);

//DEBUG!!

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

//look for full match

bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

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

//mySerial.printf("curHdgMatchVal = %2.2f, prevHdgMatchVal = %2.2f, matchslope = %2.2f, bDoneTurning = %d\n",

// curHdgMatchVal,

// prevHdgMatchVal,

// matchSlope,

// bDoneTurning);

prevHdgMatchVal = curHdgMatchVal; //07/31/21 moved below bDoneTurning chk so can use prevHdgMatchVal vs curHdgMatchVal in slope check

//}

}

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

// }

// if (bDoneTurning)

// {

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

// }

}

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

}

//09/03/21 chg fcn type from void to double to eliminate double& output param

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

{

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

lastIval += (Ki * error);

//11/16/21 rev to clamp integral term

lastIval = (lastIval > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : lastIval;

lastIval = (lastIval < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : lastIval;

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

//11/16/21 rev to subtract differential term rather than add.

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

double output = Kp * error + lastIval - Kd * dErr;

//11/16/21 now clamping output value as well as I term

output = (output > MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : output;

output = (output < MOTOR_SPEED_OFF) ? MOTOR_SPEED_OFF : output;

/*Remember some variables for next time*/

lastError = error;

lastInput = input;

lastDerror = dErr;

return output; //added 09/03/21

}

#pragma endregion HDG_BASED_TURN_SUPPORT

#pragma region MOTOR SUPPORT

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

void MoveReverse(int leftspeednum, int rightspeednum)

{

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

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

//Inputs:

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

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

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

//Plan:

// Step 1: Set reverse direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

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

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

SetLeftMotorDirAndSpeed(REV_DIR, leftspeednum);

SetRightMotorDirAndSpeed(REV_DIR, rightspeednum);

}

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

void MoveAhead(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

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

//Inputs:

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

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

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

//Plan:

// Step 1: Set forward direction for both wheels

// Step 2: Run both Motors at specified speeds

//Notes:

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

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

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

SetLeftMotorDirAndSpeed(true, leftspeednum);

SetRightMotorDirAndSpeed(true, rightspeednum);

}

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

//11/04/21 modified for Pololu VNH5019 motor driver

void StopLeftMotors()

{

digitalWrite(InA_Left, LOW);

digitalWrite(InB_Left, LOW);

analogWrite(Spd_Left, MOTOR_SPEED_OFF);

}

//11/04/21 modified for Pololu VNH5019 motor driver

void StopRightMotors()

{

digitalWrite(InA_Right, LOW);

digitalWrite(InB_Right, LOW);

analogWrite(Spd_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)

{

speed = (speed < MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;

speed = (speed > MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

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

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

//11/04/21 fwd for right motors is CCW when looking at shaft

#ifndef NO_MOTORS

if (bIsFwd)

{

digitalWrite(InA_Left, LOW);

digitalWrite(InB_Left, HIGH);

analogWrite(Spd_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(InA_Left, HIGH);

digitalWrite(InB_Left, LOW);

analogWrite(Spd_Left, speed);

}

#endif // !NO_MOTORS

}

void SetRightMotorDirAndSpeed(bool bIsFwd, int speed)

{

speed = (speed < MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;

speed = (speed > MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

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

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

//11/04/21 fwd for right motors is CW when looking at shaft

#ifndef NO_MOTORS

if (bIsFwd)

{

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

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

digitalWrite(InA_Right, HIGH);

digitalWrite(InB_Right, LOW);

analogWrite(Spd_Right, speed);

}

else

{

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

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

digitalWrite(InA_Right, LOW);

digitalWrite(InB_Right, HIGH);

analogWrite(Spd_Right, speed);

}

#endif // !NO_MOTORS

}

#pragma endregion Motor Support Functions

void CheckForUserInput()

{

const int bufflen = 80;

char buff[bufflen];

memset(buff, 0, bufflen);

float OffsetCm;

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, HEX); //chg to HEX 02/18/20

//02/18/20 experiment with multiple commands

switch (incomingByte)

{

case 0x43: //ASCII 'C'

case 0x63: //ASCII 'c'

#pragma region MANUALCONTROL

//enter infinite loop for direct remote control

Serial.println(F("ENTERING COMMAND MODE:"));

Serial.println(F("0 = 180 deg CCW Turn"));

Serial.println(F("1 = 180 deg CW Turn"));

Serial.println(F("A = Back to Auto Mode"));

Serial.println(F("S = Stop"));

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

Serial.println(F("R = Reverse"));

Serial.println(F(""));

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

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

Serial.println(F("Left 4\t5 6 Right"));

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

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

StopBothMotors();

int speed = 0;

bool bAutoMode = false;

while (!bAutoMode)

{

if (Serial.available() > 0)

{

// read the incoming byte:

int incomingByte = Serial.read();

//mySerial.printf("Got %x\n", incomingByte);

switch (incomingByte)

{

//case 0x54: //ASCII 'T'

//case 0x74: //ASCII 't'

// Serial.println(F("Toggle TIMER5 Enable/Disable"));

// if (TIMSK5 == 0)

// {

// Serial.println(F("Enable TIMER5"));

// //TIMSK5 |= OCIE5A;

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

// mySerial.printf("TIMSK5 = %x\n", TIMSK5);

// }

// else

// {

// Serial.println(F("Disable TIMER5"));

// TIMSK5 = 0;

// mySerial.printf("TIMSK5 = %x\n", TIMSK5);

// }

// break;

case 0x30: //Dec '0'

Serial.println(F("CCW 180 deg Turn"));

//RollingTurn(true, bIsForwardDir, 180);

//SpinTurn(true, 180, 45);

SpinTurn(true, 180, 90);

MoveAhead(speed, speed);

break;

case 0x31: //Dec '1'

Serial.println(F("CW 180 deg Turn"));

//RollingTurn(false, bIsForwardDir, 180);

SpinTurn(false, 180, 45);

//MoveAhead(speed, speed);

break;

case 0x34: //Turn left 10 deg

Serial.println(F("CCW 10 deg Turn"));

//MoveAhead(speed, speed);

//RollingTurn(true, bIsForwardDir, 10);

//SpinTurn(true, 10, 30);

//SpinTurn(true, 30, 30);

//added 05/03/20

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x36: //Turn right 10 deg'

mySerial.print("CW 10 deg Turn\n");

MoveAhead(speed, speed);

//RollingTurn(false, bIsForwardDir, 10);

//SpinTurn(false, 10, 30);

//SpinTurn(false, 45, 30);

//added 05/03/20

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

//MoveReverse(speed, speed);

}

break;

case 0x38: //Speed up

speed += 50;

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

mySerial.printf("Speeding up: speed now %d\n", speed);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

//MoveReverse(speed, speed);

}

break;

case 0x32: //Slow down

speed -= 50;

speed = (speed < 0) ? 0 : speed;

mySerial.printf("Slowing down: speed now %d\n", speed);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

//MoveReverse(speed, speed);

}

break;

case 0x35: //05/07/20 changed to use '5' vs 'S'

Serial.println(F("Stopping Motors!"));

StopBothMotors();

speed = 0;

break;

case 0x41: //ASCII 'A'

case 0x61: //ASCII 'a'

StopBothMotors();

Serial.println(F("Re-entering AUTO mode"));

bAutoMode = true;

break;

case 0x52: //ASCII 'R'

case 0x72: //ASCII 'r'

Serial.println(F("Setting both motors to reverse"));

bIsForwardDir = false;

//MoveReverse(speed, speed);

break;

case 0x46: //ASCII 'F'

case 0x66: //ASCII 'f'

Serial.println(F("Setting both motors to forward"));

bIsForwardDir = true;

MoveAhead(speed, speed);

#pragma endregion MANUALCONTROL

break;

#pragma region WALL OFFSET TRACKING CASES

//left side wall tracking

case 0x4C: //ASCII 'L'

case 0x6C: //ASCII 'l'

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 Offset in cm */

token = strtok(NULL, s);

OffsetCm = atof(token);

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

Kp, Ki, Kd, OffsetCm);

//TrackLeftWallOffset(Kp, Ki, Kd, OffsetCm);

break;

case 0x4D: //ASCII 'M'

case 0x6D: //ASCII 'm'

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 Offset in cm */

token = strtok(NULL, s);

OffsetCm = atof(token);

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

Kp, Ki, Kd, OffsetCm);

//TrackRightWallOffset(Kp, Ki, Kd, OffsetCm);

break;

#pragma endregion WALL OFFSET TRACKING CASES

Default :

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

MoveAhead(0, 0);

while (true)

{

}

After a number of iterations, I arrived at the following PID values:

//11/16/21 almost perfect for 45 deg/sec, 100mSec
//11/17/21 and pretty good for 90 deg/sec too!
double TurnRate_Kp = 1.8;
double TurnRate_Ki = 1.0;
double TurnRate_Kd = 0.4;

//11/16/21 almost perfect for 45 deg/sec, 100mSec

//11/17/21 and pretty good for 90 deg/sec too!

double TurnRate_Kp = 1.8;

double TurnRate_Ki = 1.0;

double TurnRate_Kd = 0.4;

Here are the Excel plots showing the turn performance:

At this point, I think I’m pretty much done with this post. The ‘rest of the story’ will continue in the companion ‘Replacing Mega 2560 With Teensy‘ post.

Stay Tuned,

Frank

Paynter's Palace

Tag Archives: chassis

Wall-E3 – Different Form-factor for Better Turn Performance

27 October 2021 Update:

02 November 2021 Update

06 November 2021 Update:

07 November 2021 Update:

20 November 2021 Update: