Heading-based Turns Using MPU6050 and Polling vs Interrupts

Posted 06 October 2019

In previous posts I have described my efforts to integrate heading-based wall tracking into my two-wheel and four-wheel robots. I installed a MPU6050 module into Wall-E2, my primary four-wheel robot, some time ago but was never able to make heading-based turns work for one reason or another. In conjunction with some other experiments, I installed an MPU6050 module on my two-wheel robot so that I could investigate the issues with heading-based turns and heading-based wall tracking with a simpler hardware configuration.

With the two-wheel robot I was able to demonstrate successful heading-based wall tracking, but I was unable to port the capability to my four-wheel configuration. Not only that, but for some reason I started having problems getting reliable yaw/heading values from my two-wheel robot configuration. This post describes the steps I took to troubleshoot the problem, ultimately arriving at a stable polling-only (no interrupt line required) yaw/heading value retrieval algorithm suitable for both the two-wheel and four-wheel robot configurations.

Back to Basics:

As I always do when faced with a complex problem with conflicting results, I decided to simplify the problem as much as possible. In this case that meant reducing the hardware configuration to just a MPU6050 module and an Arduino Mega controller, as shown below:

Arduino Mega and MPU6050

On the software side, I started with the simplest possible Arduino sketch – Jeff Rowberg’s ‘MPU6050_DMP6.ino’ example, included in his latest I2CDevLib library and described in this post.

After getting everything running properly in this very basic configuration using an interrupt-driven algorithm, I moved on to working with the polling-driven arrangement, to confirm that polling was a viable strategy. To do this I modified the hardware to disconnect the interrupt line from the MPU6050 to the controller board, and modified the software as described in this post to use a polling arrangement vs interrupts.

After confirming that this simple example worked properly and seemed stable, it was time to work my way back into the two-wheel robot hardware and software configuration (again!). To do this I started with the basic controller/MPU6050 only hardware configuration, but running my two-wheel robot software program, modified to eliminate everything but the ‘RollingTurn()’ function that uses heading information from the MPU6050 to initiate and terminate turns. After some false starts and blind alleys, I finally arrived at a stable software configuration demonstrating consistent heading-based turn performance using polling only – no interrupts! the code is shown below:

/*
    Name:       TwoWheelRobot.ino
    Created:	6/3/2019 9:31:46 AM
    Author:     FRANKWIN10\Frank

	Notes:
		11/05/19 changed to MPU6050 DMP Version 6.12
*/

#pragma region INCLUDES
//#include <Adafruit_INA219.h>
#include <elapsedMillis.h>
#include <NewPing.h> //added 01/15/15
#include <PrintEx.h> //allows printf-style printout syntax
//#include "MPU6050_6Axis_MotionApps20.h" //06/23/18 modified for 20Hz interrupt freq
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/05/19
#include "I2Cdev.h" //02/19/19: this includes SBWire.h
#pragma endregion INCLUDES

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
elapsedMillis SinceLastDistanceCheck;
elapsedMillis sinceLastTimeCheck; //used for rolling turn timeout
elapsedMillis sinceLastHdgCheck; //rolling turn heading check timer

#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_LOW = 50; //added 01/22/15
const int MOTOR_SPEED_OFF = 0; //range is 0-255
const int MOTOR_SPEED_ADJ_FACTOR = 0; //chg to 40 at 5:55pm
const int LEFT_SPEED_COMP_VAL_FWD = 0; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_FWD = 5; //right speed compensation value
const int LEFT_SPEED_COMP_VAL_REV = 0; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_REV = 0; //right speed compensation value

//drive wheel direction constants
const boolean FWD_DIR = true;
const boolean REV_DIR = !FWD_DIR;

//Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;

const float ROLLING_TURN_MAX_SEC_PER_DEG = 1 / 15.0; //used to limit time in rolling turns
const int OFFSIDE_MOTOR_SPEED = MOTOR_SPEED_LOW;
const int DRIVESIDE_MOTOR_SPEED_HIGH = MOTOR_SPEED_MAX;
const int DRIVESIDE_MOTOR_SPEED_LOW = MOTOR_SPEED_FULL;
const float HDG_NEAR_MATCH_VAL = 0.9; //slow the turn down here
const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here
const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started

#pragma endregion Motor Parameters

#pragma region DEFINES
#define NO_MOTORS
//#define NO_LIDAR
//#define NO_PINGS
//#define NO_IRDET //added 04/05/17 for daytime in-atrium testing (too much ambient IR)
//#define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop
#pragma endregion Program Defines

#pragma region PROGRAM_CONSTANTS
const int MIN_PING_INTERVAL_MSEC = 200;
const int CURRENT_AVERAGE_NUMBER = 10; //added 07/09/19
const int MAX_AD_VALUE = 1023;
const float ADC_REF_VOLTS = 3.3; //03/27/18 now using analogReference(EXTERNAL) with Teensy 3.3V connected to AREF 
const float VOLTAGE_TO_CURRENT_RATIO = 0.75f; //Volts/Amp rev 07/09/19. 
#pragma endregion Program Constants

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

#pragma region MOTOR_PINS
//Left Motor
int enA = 2;
int in1 = 3;
int in2 = 4;

//Right Motor
int in3 = 5;
int in4 = 6;
int enB = 7;
#pragma endregion Motor Pin Assignments


#pragma region SENSOR_PINS
const int RightSensorTrigPin = 52;
const int LeftSensorTrigPin = 53;
const int LeftSensorEchoPin = LeftSensorTrigPin;
const int RightSensorEchoPin = RightSensorTrigPin;
const int TOT_CURR_PIN = A8;

//LIDAR MODE pin (continuous mode)
const int LIDAR_MODE_PIN = 4; //mvd here 01/10/18
#pragma endregion Sensor Pin Assigns

#pragma region DIST_HDG_MEASUREMENT_SUPPORT
	//misc LIDAR and Ping sensor parameters
const int MIN_OBS_DIST_CM = 20; //rev 04/28/17 for better obstacle handling
const int MAX_LR_DISTANCE_CM = 200;  //04/19/15 now using sep parameters for front and side sensors

//distance and heading running average
const int LR_PING_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg
const int LR_PING_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ
const int HDG_RUNNING_AVG_ARRAY_SIZE = 20; //1 second

byte aLeftDist[LR_PING_DIST_ARRAY_SIZE];
byte aRightDist[LR_PING_DIST_ARRAY_SIZE];
#pragma endregion Distance Measurement Support

//MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr, using INT connected to Mega pin 2 (INT0)
MPU6050 mpu(0x68); //07/28/19 use default addr for GY-521 on 2-motor robot

//added 01/16/15 for NewPing support
NewPing LeftPing(LeftSensorTrigPin, LeftSensorEchoPin, MAX_LR_DISTANCE_CM / 2);
NewPing RightPing(RightSensorTrigPin, RightSensorEchoPin, MAX_LR_DISTANCE_CM / 2);


#pragma region LOOP_VARS
int leftspeednum = MOTOR_SPEED_HALF;
int rightspeednum = MOTOR_SPEED_HALF;
elapsedMillis 	sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec
int m_FinalLeftSpeed = 0;
int m_FinalRightSpeed = 0;


float shuntvoltage = 0;
float busvoltage = 0;
float current_mA = 0;
float loadvoltage = 0;
float power_mW = 0;

float TotAmps = 0.f;
float SecSinceStart = 0.f;
float StartSec = 0;

int m_LeftDistCm = 0;
int m_RightDistCm = 0;
#pragma endregion Loop Variables

#pragma region MPU6050_SUPPORT
//uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU c/o 10/08/19 not used for polling
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t DMPpacketSize;    // 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

//RTC/FRAM/MPU6050 status flags
bool RTC_Avail = true;
bool bRTCLostPower = false; //added 10/17/18
bool bFRAMReady = true;
bool bMPU6050Ready = true;
bool dmpReady = false;  // set true if DMP init was successful
volatile float global_yawval = 0; //updated by GetIMUHeadingDeg()
const int MAX_FIFO_WAIT_LOOP_COUNT = 1000;
#pragma endregion MPU6050 Support

#pragma region WALL_FOLLOW_SUPPORT
#define TURNDIR_CCW false
#define TURNDIR_CW true

const int DESIRED_WALL_OFFSET_DIST_CM = 30;
const int WALL_APPR_ERR_WIN_MULTFACT = 2; //added 08/12/19
const int WALL_TRK_ERR_WIN_MULTFACT = 1; //added 08/12/19
const int VERY_FAR_AWAY_CM = 5;
const int FAR_AWAY_CM = 2;
const int CLOSE_CM = 1;
const int VERY_FAR_AWAY_TURN_DEG = 20;
const int FAR_AWAY_TURN_DEG = 10;
const int CLOSE_TURN_DEG = 5;
const int NAV_UPDATE_DELAY_MSEC = 1000;
const int TURN_INCREMENT_DEG = 10;
const int U_TURN_SEC = 8;

int m_PrevHdgCutDeg = 0;
bool m_PrevHdgCutDir = TURNDIR_CW; //CW
int m_PrevLeftDistCm = 0;
int m_PrevRightDistCm = 0;
elapsedMillis 	sinceLastHdgCutUpdateMsec; //added 10/15/18 to replace lastmillisec
int m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //used to increase close in response

#pragma endregion Wall Following Support

//08/03/19 experiment with using a timer interrupt for distance checking while in a turn
const int TIMER3_OUT_PIN = 31; 

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

	Serial.println("Two Wheel Robot");

	// join I2C bus (I2Cdev library doesn't do this automatically)
	Wire.begin();
	Wire.setTwiMaxLoops(15000); //08/23 needed for SBWire library
	Wire.setClock(100000); //07/25/19 slow down I2C clock for better noise immunity

	//motor control PWM pins

	//set up left motor pins as outputs
	pinMode(enA, OUTPUT);
	pinMode(in1, OUTPUT);
	pinMode(in2, OUTPUT);

	//set up right motor pins as outputs
	pinMode(in3, OUTPUT);
	pinMode(in4, OUTPUT);
	pinMode(enB, OUTPUT);

	pinMode(TOT_CURR_PIN, INPUT);//07/09/19 now connected to 'Total Current' 1NA619 charge current sensor
	digitalWrite(TOT_CURR_PIN, LOW); //turn off the internal pullup resistor
	analogReference(EXTERNAL); //07/09/18 now using external 3.3V ref
	analogRead(0); //dummy read to let the ADC circuitry settle down


	// configure LED for output
	pinMode(LED_PIN, OUTPUT);


	// initialize MPU6050 added 09/03/18
	Serial.println(F("Initializing MPU6050 ..."));
	mpu.initialize();

	// verify connection
	Serial.println(F("Testing device connections..."));
	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
		DMPpacketSize = mpu.dmpGetFIFOPacketSize();
		bMPU6050Ready = true;
		StartSec = millis() / 1000.f;
		mySerial.printf("\nMPU6050 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;
	}

	mySerial.printf("Sec\tHdg\tLdist\tRdist\tErrDeg\tPrevDeg\tTurnDir\tTurnDeg\n"); //column headers

	m_LeftDistCm = GetLeftDistCm();
	m_RightDistCm = GetRightDistCm();
	RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

	SinceLastDistanceCheck = 0;
	sinceLastTimeCheck = 0; //used for rolling turn timeout
	sinceLastHdgCheck = 0; //rolling turn heading check timer

	mySerial.printf("Starting test turns...\n");
	delay(1000);
	mpu.resetFIFO(); //07/27/19 make sure we are starting with a clean slate
	mpu.getIntStatus(); //required to make sure the FIFO overflow interrupt bit is reset
	mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount());

	mySerial.printf("Turning right 30 deg...\n");
	RollingTurn(true, true, 30);
	delay(1000);

	mySerial.printf("Turning left 30 deg...\n");
	mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount());
	mpu.resetFIFO(); //07/27/19 make sure we are starting with a clean slate
	mpu.getIntStatus(); //required to make sure the FIFO overflow interrupt bit is reset
	mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount());
	RollingTurn(false, true, 30);

	sinceLastNavUpdateMsec = 0;
}

void loop()
{

	// if programming failed, don't try to do anything
	if (dmpReady && mpu.dmpPacketAvailable())
	{
		//global_yawval = GetIMUHeadingDeg(); //retrieve the most current yaw value from IMU
		GetIMUHeadingDeg(); //updates global_yawval if successful

		// blink LED to indicate activity
		bool blinkState = !blinkState;
		digitalWrite(LED_PIN, blinkState);
	}

	//if (SecSinceStart - StartSec > U_TURN_SEC)
	//{
	//	mySerial.printf("Turning Around!\n");
	//	RollingTurn(TURNDIR_CCW,true,180);
	//	RunBothMotorsMsec(U_TURN_SEC*1000, MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);
	//	StopBothMotors();
	//	while (1);
	//}

	if (sinceLastNavUpdateMsec >= MIN_PING_INTERVAL_MSEC)
	{
		sinceLastNavUpdateMsec -= MIN_PING_INTERVAL_MSEC;

		int loc_leftdist = GetLeftDistCm();
		int loc_rightdist = GetRightDistCm();
		int loc_disterror = 0;
		int turndeg = 0;
		bool turndirection = false;

		UpdateDistArrays(loc_leftdist, loc_rightdist);

		////decide what to do
		//if (loc_leftdist < loc_rightdist)
		//{
		//	//left wall is closer, so CCW moves twd wall, CW moves away
		//	loc_disterror = loc_leftdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away'
		//	turndirection = loc_disterror > 0 ? TURNDIR_CCW : TURNDIR_CW;

		//	if (abs(loc_disterror) > VERY_FAR_AWAY_CM) //use 20-deg cut
		//	{
		//		turndeg = VERY_FAR_AWAY_TURN_DEG;
		//	}
		//	else if (abs(loc_disterror) > FAR_AWAY_CM) //use 10-deg cut
		//	{
		//		turndeg = FAR_AWAY_TURN_DEG;
		//	}
		//	else
		//	{
		//		mySerial.printf("Close to target distance - what do I do now?\n");
		//		RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);
		//	}
		//}
		//else //right wall is closer or equal...
		//{
		//	//right wall is closer, so CW moves twd wall, CCW moves away
		//	loc_disterror = loc_rightdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away'
		//	turndirection = loc_disterror > 0 ? TURNDIR_CW : TURNDIR_CCW;

		//	if (abs(loc_disterror) > m_TrkErrWinMult * VERY_FAR_AWAY_CM) //use 20-deg cut
		//	{
		//		turndeg = VERY_FAR_AWAY_TURN_DEG;
		//	}
		//	else if (abs(loc_disterror) > m_TrkErrWinMult * FAR_AWAY_CM) //use 10-deg cut
		//	{
		//		turndeg = FAR_AWAY_TURN_DEG;
		//	}
		//	else if (abs(loc_disterror) > m_TrkErrWinMult * CLOSE_CM) //use 5-deg cut
		//	{
		//		turndeg = CLOSE_TURN_DEG;
		//		m_TrkErrWinMult  = WALL_TRK_ERR_WIN_MULTFACT;
		//	}
		//	else //must be close to desired offset distance. Reduce error window size
		//	{
		//		//mySerial.printf("Close to target distance - setting turndeg to zero\n");
		//		turndeg = 0;
		//		m_TrkErrWinMult  = WALL_APPR_ERR_WIN_MULTFACT; //go back to approach window sizes
		//	}
		//}

		//if (turndeg == 0)
		//{
		//	//robot is close to target - take out any remaining cut
		//	//mySerial.printf("turndeg = 0; Removing Entire Cut\n");
		//	if (m_PrevHdgCutDeg > 0)
		//	{
		//		RollingTurn(!turndirection, true, m_PrevHdgCutDeg);
		//		m_PrevHdgCutDeg = 0;
		//	}
		//}
		//else if (turndeg > m_PrevHdgCutDeg)
		//{
		//	//cut not cutting it; increase it by 10 deg
		//	//mySerial.printf("turndeg > m_PrevHdgCutDeg; Increasing Cut\n");
		//	turndeg = TURN_INCREMENT_DEG;
		//	RollingTurn(turndirection, true, turndeg);
		//	m_PrevHdgCutDeg += TURN_INCREMENT_DEG;
		//}
		//else  if (turndeg < m_PrevHdgCutDeg)// turndeg > 0 &&  < m_PrevHdgCutDeg)
		//{
		//	if (isConverging(DESIRED_WALL_OFFSET_DIST_CM, true))
		//	{
		//		//take out some of the cut
		//		//mySerial.printf(" turndeg > 0 &&  < m_PrevHdgCutDeg: Reducing Cut\n");
		//		turndeg = TURN_INCREMENT_DEG;
		//		RollingTurn(!turndirection, true, turndeg);
		//		m_PrevHdgCutDeg -= TURN_INCREMENT_DEG;
		//	}
		//}
		//else if (turndeg == m_PrevHdgCutDeg) //this case takes more work
		//{
		//	if (isConverging(DESIRED_WALL_OFFSET_DIST_CM, true))
		//	{
		//		//do nothing - things are going smoothly
		//	}
		//	else
		//	{
		//		//cut has been maxed out, but dist & tgt not converging
		//		//increase cut in same direction as before
		//		//mySerial.printf("not converging - increasing cut\n");
		//		turndeg = TURN_INCREMENT_DEG;
		//		RollingTurn(turndirection, true, turndeg);
		//		m_PrevHdgCutDeg += TURN_INCREMENT_DEG;
		//		m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //go back to approach sensitivity
		//	}
		//}

		////regardless of what happens above, continue to run the motors straight ahead
		//RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

		//07/09/19 if GetIMUHeadingDeg() succeeds, do the rest  
		TotAmps = GetTotalAmps();
		SecSinceStart = millis() / 1000.f; //hopefully this will be a float result

		mySerial.printf("%2.2f\t%3.2f\t%d\t%d\t%d\t%d\t%s\t%d\n", 
			SecSinceStart, global_yawval,loc_leftdist,loc_rightdist, loc_disterror,m_PrevHdgCutDeg, turndirection == TURNDIR_CCW ? "CCW":"CW", turndeg);
	}
}

#pragma region MOTOR_SUPPORT_ROUTINES
void RunBothMotors(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:
	//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

////DEBUG!!
//	mySerial.printf("In RunBothMotors(%d,%d\n", leftspeednum, rightspeednum);
////DEBUG!!
	SetLeftMotorSpeed(leftspeednum);
	SetRightMotorSpeed(rightspeednum);
}

//12/26/14 added for independent motor speed
void RunBothMotorsMsec(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

	RunBothMotors(leftspeednum, rightspeednum);

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

//11/25/15 added for symmetry ;-).
void StopBothMotors()
{
	//added analogWrite calls to ensure speed set to zero
	analogWrite(enA, MOTOR_SPEED_OFF);
	analogWrite(enB, MOTOR_SPEED_OFF);

	digitalWrite(in1, LOW);
	digitalWrite(in2, LOW);
	digitalWrite(in3, LOW);
	digitalWrite(in4, LOW);
}

//01/24/15 removed time from MoveAhead sig, added MoveAheadMsec
void MoveAhead(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move ahead continuously
	//Provenance: 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

	//Step 1: Set forward direction for both wheels
	SetLeftMotorDir(true);
	SetRightMotorDir(true);

	//Step 2: Run both myMotors for timsec seconds
	RunBothMotors(leftspeednum, rightspeednum);
}

void MoveReverse(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move in reverse direction continuously - companion to MoveAhead()
	//Provenance: 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

	//Step 1: Set reverse direction for both wheels
	SetLeftMotorDir(false);
	SetRightMotorDir(false);

	//Step 2: Run both myMotors for timsec seconds
	//01/24/15 - removed timing - now continuous run
	RunBothMotors(leftspeednum, rightspeednum);
}

void SetLeftMotorDir(boolean bIsFwdDir)
{
	if (bIsFwdDir)
	{
		digitalWrite(in1, LOW);
		digitalWrite(in2, HIGH);
	}
	else //must be REV
	{
		digitalWrite(in1, HIGH);
		digitalWrite(in2, LOW);
	}

	bLeftMotorDirIsFwd = bIsFwdDir;
}

void SetRightMotorDir(boolean bIsFwdDir)
{
	if (bIsFwdDir)
	{
		digitalWrite(in3, LOW);
		digitalWrite(in4, HIGH);
	}
	else //must be REV
	{
		digitalWrite(in3, HIGH);
		digitalWrite(in4, LOW);
	}

	bRightMotorDirIsFwd = bIsFwdDir;
}

//11/25/15 added to utilize new private member variable bLeftMotorDirIsFwd
void SetLeftMotorSpeed(int speed)
{
	if (bLeftMotorDirIsFwd)
	{
		speed = speed + LEFT_SPEED_COMP_VAL_FWD;

		//check limits
		speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
		speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

	}
	else //must be REV
	{
		speed = speed + LEFT_SPEED_COMP_VAL_REV;

		//check limits
		speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
		speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;
	}


#ifndef NO_MOTORS
		//Serial.print("Left Speed = "); Serial.println(speed);
		analogWrite(enA, speed);
#endif


	//02/13/16 added for 'pause' debug
	m_FinalLeftSpeed = speed;
}

//11/25/15 added to utilize new private member variable bRightMotorDirIsFwd
void SetRightMotorSpeed(int speed)
{
	if (bRightMotorDirIsFwd)
	{
		speed = speed + LEFT_SPEED_COMP_VAL_FWD;

		//check limits
		speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
		speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

	}
	else //must be REV
	{
		speed = speed + LEFT_SPEED_COMP_VAL_REV;

		//check limits
		speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
		speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

	}

	//02/13/16 added for 'pause' debug
	m_FinalLeftSpeed = speed;

#ifndef NO_MOTORS
		//Serial.print("Right Speed = "); Serial.println(speed);
		analogWrite(enB, speed);
#endif

}
#pragma endregion Motor Support Routines


bool GetIMUHeadingDeg()
{
	//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
	//	global_yawval 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 global_yawval with latest value
	//Notes:
	//	10/08/19 changed return type to boolean
	//	10/08/19 no longer need mpuIntStatus

	//mpuIntStatus = mpu.getIntStatus(); //this resets the FIFO overflow interrupt bit
	fifoCount = mpu.getFIFOCount();// get current FIFO count

	// check for overflow (this should never happen unless our code is too inefficient)
	//if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)
	if (fifoCount >= 1024)
	{
		//if the FIFO count reset so we can continue cleanly
		mySerial.printf("FIFO overflow with FIFO count = %d\n", mpu.getFIFOCount());
		mpu.resetFIFO();

		// now have to wait for DMP data ready again
		int wait_count = 0;
		while (!mpu.dmpPacketAvailable() && wait_count < 1000)
		{
			delay(1);
			wait_count++;
		}

		//if the wait loop timed out, punt
		if (wait_count >= 1000)
		{
			mySerial.printf("wait loop timed out wait_count = %d",wait_count);
			return false; 
		}
	}

	// read all available packets from FIFO
	while (fifoCount >= DMPpacketSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!
	{
		mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);
		// track FIFO count here in case there is > 1 packet available
		// (this lets us immediately read more without waiting for an interrupt)
		fifoCount -= DMPpacketSize;
	}

	// display Euler angles in degrees
	mpu.dmpGetQuaternion(&q, fifoBuffer);
	mpu.dmpGetGravity(&gravity, &q);
	mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

	//compute the yaw value
	global_yawval = ypr[0] * 180 / M_PI;
	return  true;
}
float GetTotalAmps()
{
	//Purpose:  Get total current in amps
	//Inputs: 
	//	Voltage on TOT_CURR_PIN is approximately Ichg*2 Amps
	//	VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio
	//Outputs:
	//	returns total robot current (chg current plus running current)
	//Notes:
	//	02/28/18 chg name from GetBattChgAmps() to GetTotalAmps()

	int ITotAnalogReading = GetAverageAnalogReading(TOT_CURR_PIN, CURRENT_AVERAGE_NUMBER); //range is 0-1023
	float ITotVolts = ((float)ITotAnalogReading / (float)MAX_AD_VALUE)* ADC_REF_VOLTS;
	float ITotAmps = ITotVolts * VOLTAGE_TO_CURRENT_RATIO;
	return ITotAmps;
}

int GetAverageAnalogReading(int pin, int numavgs)
{
	long totreads = 0;
	for (int i = 0; i < numavgs; i++)
	{
		totreads += analogRead(pin);
	}

	return (int)((float)totreads / (float)numavgs); //truncation ok
}

//01/10/18 reverted to regular ping().  median distance function takes too long
int GetLeftDistCm()
{
	//Serial.print("LeftPing\t"); Serial.println(millis());
	//Notes:
	//	04/30/17 added limit detection/correction

	int timeUsec = LeftPing.ping();
	//Serial.print("Left Ping Usec = "); Serial.println(timeUsec);
	int distCm = LeftPing.convert_cm(timeUsec);

	//04/30/17 added limit detection/correction
	distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM;

	return distCm;
}

//01/10/18 reverted to regular ping().  median distance function takes too long
int GetRightDistCm()
{
	//Serial.print("RightPing\t"); Serial.println(millis());
	//Notes:
	//	04/30/17 added limit detection/correction

	int timeUsec = RightPing.ping();
	//Serial.print("Right Ping Usec = "); Serial.println(timeUsec);
	int distCm = RightPing.convert_cm(timeUsec);

	//04/30/17 added limit detection/correction
	distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM;

	return distCm;
}

bool RollingTurn(bool bIsCCW, bool bIsFwd, float numDeg)
{
	//Purpose: Make a numDeg forwards or backwards CW turn
	//Inputs:
	//	bIsFwd - True if turn is to be forward, 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
	//	global_yawval = IMU heading value updated by GetIMUHeadingDeg()
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Compute new target value
	//	Step3: Set motor speeds based on fwd/backwds flag
	//	Step4: Run motors until target reached
	//Notes:
	//	08/22/18 now using MPU6050_CCW_INCREASES_YAWVAL define to compute tgt
	//	08/29/18 removed MPU6050_CCW_INCREASES_YAWVAL - the MPU6050 DMP takes care of this
	//	09/08/18 revised end-of-turn detection to include slope calc.  Threshold alone is too fragile
	//	09/11/18 revised to handle both turn directions
	//	04/29/19 uncommented
	//	07/31/19 rev for better initial heading capture
	//	07/31/19 rev return type to bool
	//	07/31/19 rev to eliminate timed loop - now runs as fast as possible
	//	07/31/19 rev to not stop motors at end - now calling pgm is resp for this
	//	10/06/19 rev for new IMU polling setup

	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;

//DEBUG!!
	mySerial.printf("In RollingTurn(%s, %s,%4.2f)\n", bIsCCW == TURNDIR_CCW? "CCW":"CW", bIsFwd == true? "FWD":"BKWD", numDeg);
//DEBUG!!

//Step1: Get current heading as starting point
	//if (!GetIMUHeadingDeg())
	//{
	//	mySerial.printf("RollingTurn(%d,%d,%d): Initial Heading Capture Failed!",
	//		bIsCCW, bIsFwd, numDeg);
	//	return false;
	//}

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

	//Wait for packet availability (times out after one second) ...
	int wait_count = 0;
	while (!mpu.dmpPacketAvailable() && wait_count < 1000)
	{
		delay(1);
		wait_count++;
	}

	//if the wait loop timed out, punt
	if (wait_count >= 1000)
	{
		return false;
	}

	//if we get to here, the IMU is OK and at least one packet is available
	//global_yawval = GetIMUHeadingDeg();
	GetIMUHeadingDeg(); //updates global_yawval if successful

//Step2: Compute new target value & timeout value
	//timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG;
	timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG * 10000;//10/06/19 make timeout irrelevant

	tgt_deg = bIsCCW ? global_yawval + numDeg : global_yawval - numDeg;

	//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",
		global_yawval, numDeg, tgt_deg, timeout_sec);
//DEBUG!!

//Step3: Start motors
	SetRightMotorDir(bIsFwd);
	SetLeftMotorDir(bIsFwd);
	int rightspeed;
	int leftspeed;

	if (!bIsCCW) //2-motor robot is backwards from 4-motor one
	{
		rightspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED;
		leftspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH;

	}
	else
	{
		rightspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH;
		leftspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED;
	}
	SetLeftMotorSpeed(leftspeed);
	SetRightMotorSpeed(rightspeed);


	sinceLastTimeCheck = 0; //needed for watchdog timer
	//mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n");
	bool bFirstPass = true; //for 'slowdown' print control
	float curHdgMatchVal = 0;

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

//Step4: Run motors until target reached
	mpu.resetFIFO();
	while (!bDoneTurning && !bTimedOut)
	{

		//GetIMUHeadingDeg(); //07/31/19 now incorporates FIFO wait loop
		if (mpu.dmpPacketAvailable())
		{
			//global_yawval = GetIMUHeadingDeg(); //10/06/19 only do this if a new packet is avail
			GetIMUHeadingDeg();  //updates global_yawval if successful

//DEBUG!!
			mySerial.printf("%lu\t%4.2f\t%d\n", millis(), global_yawval, fifoCount);
//DEBUG!!
		}

		//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, global_yawval);
		matchSlope = curHdgMatchVal - prevHdgMatchVal;

		if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL)
		{
			if (bFirstPass)
			{
				prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs
				//mySerial.printf("Slowing down at %4.2f deg\n", global_yawval);
				bFirstPass = false;
			}
		}

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

//DEBUG!!
		if (bDoneTurning)
		{
			mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, and slope = %1.3f\n",
				global_yawval, tgt_deg, curHdgMatchVal, matchSlope);
		}
//DEBUG!!

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

		}
		mpu.resetFIFO();
	}

	return true; 
}

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
	//	global_yawval = 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);
}

bool isConverging(int tgtdist, bool bIsRightSide)
{
	//Purpose: Determine if act dist & tgt dist are converging
	//Inputs:
	//	tgtdist = it representing target distance
	//	bIsRightSide = boolean denoting side under consideration. True = right, False = left
	//	aRightDist[], aLeftDist[] = int arrays containing the LR_PING_DIST_ARRAY_SIZE previous ping measurements
	//Plan:
	//	Step1: compute avg slope = (dist[LR_PING_DIST_ARRAY_SIZE-1]-dist[0])/LR_PING_DIST_ARRAY_SIZE
	//	Step2: if slope > 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] < tgtdist, return TRUE (convergent)
	//		   if slope < 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] > tgtdist, return TRUE (convergent)
	//		   else, return FALSE (non-convergent)

//Step1: compute avg slope = (dist[LR_PING_DIST_ARRAY_SIZE-1]-dist[0])/LR_PING_DIST_ARRAY_SIZE
	int slope = 0;
	int last_dist = 0;
	int first_dist = 0;
	if (bIsRightSide)
	{
		first_dist = aRightDist[0];
		last_dist = aRightDist[LR_PING_DIST_ARRAY_SIZE - 1];
		slope = last_dist - first_dist; //don't need to divide
	}
	else
	{
		first_dist = aLeftDist[0];
		last_dist = aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1];
		slope = last_dist - first_dist; //don't need to divide
	}

////DEBUG!!
//	mySerial.printf("IsConverging:  firstd/lastd = %d/%d, slope = %d\ttgtdist = %d\n", first_dist,last_dist, slope, tgtdist);
////DEBUG!!

//Step2: if slope > 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] < tgtdist, return TRUE (convergent)
//	   if slope < 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] > tgtdist, return TRUE (convergent)
//	   else, return FALSE (non-convergent)
	if (slope > 0 && last_dist <= tgtdist) return true;
	if (slope < 0 && last_dist >= tgtdist) return true;
	else return false;
}

void UpdateDistArrays(int leftdist, int rightdist)
{
	for (size_t i = 0; i < LR_PING_DIST_ARRAY_SIZE-1; i++)
	{
		aLeftDist[i] = aLeftDist[i + 1];
		aRightDist[i] = aRightDist[i + 1];
	}

	aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1] = leftdist;
	aRightDist[LR_PING_DIST_ARRAY_SIZE - 1] = rightdist;
}

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

Name: TwoWheelRobot.ino

Created: 6/3/2019 9:31:46 AM

Author: FRANKWIN10\Frank

Notes:

11/05/19 changed to MPU6050 DMP Version 6.12

#pragma region INCLUDES

//#include <Adafruit_INA219.h>

#include <elapsedMillis.h>

#include <NewPing.h> //added 01/15/15

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

//#include "MPU6050_6Axis_MotionApps20.h" //06/23/18 modified for 20Hz interrupt freq

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

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

#pragma endregion INCLUDES

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

elapsedMillis SinceLastDistanceCheck;

elapsedMillis sinceLastTimeCheck; //used for rolling turn timeout

elapsedMillis sinceLastHdgCheck; //rolling turn heading check timer

#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_LOW = 50; //added 01/22/15

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

const int MOTOR_SPEED_ADJ_FACTOR = 0; //chg to 40 at 5:55pm

const int LEFT_SPEED_COMP_VAL_FWD = 0; //left speed compensation value

const int RIGHT_SPEED_COMP_VAL_FWD = 5; //right speed compensation value

const int LEFT_SPEED_COMP_VAL_REV = 0; //left speed compensation value

const int RIGHT_SPEED_COMP_VAL_REV = 0; //right speed compensation value

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

const float ROLLING_TURN_MAX_SEC_PER_DEG = 1 / 15.0; //used to limit time in rolling turns

const int OFFSIDE_MOTOR_SPEED = MOTOR_SPEED_LOW;

const int DRIVESIDE_MOTOR_SPEED_HIGH = MOTOR_SPEED_MAX;

const int DRIVESIDE_MOTOR_SPEED_LOW = MOTOR_SPEED_FULL;

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

const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here

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

#pragma endregion Motor Parameters

#pragma region DEFINES

#define NO_MOTORS

//#define NO_LIDAR

//#define NO_PINGS

//#define NO_IRDET //added 04/05/17 for daytime in-atrium testing (too much ambient IR)

//#define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop

#pragma endregion Program Defines

#pragma region PROGRAM_CONSTANTS

const int MIN_PING_INTERVAL_MSEC = 200;

const int CURRENT_AVERAGE_NUMBER = 10; //added 07/09/19

const int MAX_AD_VALUE = 1023;

const float ADC_REF_VOLTS = 3.3; //03/27/18 now using analogReference(EXTERNAL) with Teensy 3.3V connected to AREF

const float VOLTAGE_TO_CURRENT_RATIO = 0.75f; //Volts/Amp rev 07/09/19.

#pragma endregion Program Constants

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

#pragma region MOTOR_PINS

//Left Motor

int enA = 2;

int in1 = 3;

int in2 = 4;

//Right Motor

int in3 = 5;

int in4 = 6;

int enB = 7;

#pragma endregion Motor Pin Assignments

#pragma region SENSOR_PINS

const int RightSensorTrigPin = 52;

const int LeftSensorTrigPin = 53;

const int LeftSensorEchoPin = LeftSensorTrigPin;

const int RightSensorEchoPin = RightSensorTrigPin;

const int TOT_CURR_PIN = A8;

//LIDAR MODE pin (continuous mode)

const int LIDAR_MODE_PIN = 4; //mvd here 01/10/18

#pragma endregion Sensor Pin Assigns

#pragma region DIST_HDG_MEASUREMENT_SUPPORT

//misc LIDAR and Ping sensor parameters

const int MIN_OBS_DIST_CM = 20; //rev 04/28/17 for better obstacle handling

const int MAX_LR_DISTANCE_CM = 200; //04/19/15 now using sep parameters for front and side sensors

//distance and heading running average

const int LR_PING_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg

const int LR_PING_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ

const int HDG_RUNNING_AVG_ARRAY_SIZE = 20; //1 second

byte aLeftDist[LR_PING_DIST_ARRAY_SIZE];

byte aRightDist[LR_PING_DIST_ARRAY_SIZE];

#pragma endregion Distance Measurement Support

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

MPU6050 mpu(0x68); //07/28/19 use default addr for GY-521 on 2-motor robot

//added 01/16/15 for NewPing support

NewPing LeftPing(LeftSensorTrigPin, LeftSensorEchoPin, MAX_LR_DISTANCE_CM / 2);

NewPing RightPing(RightSensorTrigPin, RightSensorEchoPin, MAX_LR_DISTANCE_CM / 2);

#pragma region LOOP_VARS

int leftspeednum = MOTOR_SPEED_HALF;

int rightspeednum = MOTOR_SPEED_HALF;

elapsedMillis sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec

int m_FinalLeftSpeed = 0;

int m_FinalRightSpeed = 0;

float shuntvoltage = 0;

float busvoltage = 0;

float current_mA = 0;

float loadvoltage = 0;

float power_mW = 0;

float TotAmps = 0.f;

float SecSinceStart = 0.f;

float StartSec = 0;

int m_LeftDistCm = 0;

int m_RightDistCm = 0;

#pragma endregion Loop Variables

#pragma region MPU6050_SUPPORT

//uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU c/o 10/08/19 not used for polling

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

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

//RTC/FRAM/MPU6050 status flags

bool RTC_Avail = true;

bool bRTCLostPower = false; //added 10/17/18

bool bFRAMReady = true;

bool bMPU6050Ready = true;

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

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

const int MAX_FIFO_WAIT_LOOP_COUNT = 1000;

#pragma endregion MPU6050 Support

#pragma region WALL_FOLLOW_SUPPORT

#define TURNDIR_CCW false

#define TURNDIR_CW true

const int DESIRED_WALL_OFFSET_DIST_CM = 30;

const int WALL_APPR_ERR_WIN_MULTFACT = 2; //added 08/12/19

const int WALL_TRK_ERR_WIN_MULTFACT = 1; //added 08/12/19

const int VERY_FAR_AWAY_CM = 5;

const int FAR_AWAY_CM = 2;

const int CLOSE_CM = 1;

const int VERY_FAR_AWAY_TURN_DEG = 20;

const int FAR_AWAY_TURN_DEG = 10;

const int CLOSE_TURN_DEG = 5;

const int NAV_UPDATE_DELAY_MSEC = 1000;

const int TURN_INCREMENT_DEG = 10;

const int U_TURN_SEC = 8;

int m_PrevHdgCutDeg = 0;

bool m_PrevHdgCutDir = TURNDIR_CW; //CW

int m_PrevLeftDistCm = 0;

int m_PrevRightDistCm = 0;

elapsedMillis sinceLastHdgCutUpdateMsec; //added 10/15/18 to replace lastmillisec

int m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //used to increase close in response

#pragma endregion Wall Following Support

//08/03/19 experiment with using a timer interrupt for distance checking while in a turn

const int TIMER3_OUT_PIN = 31;

void setup()

{

Serial.begin(115200);

Serial.println("Two Wheel Robot");

// join I2C bus (I2Cdev library doesn't do this automatically)

Wire.begin();

Wire.setTwiMaxLoops(15000); //08/23 needed for SBWire library

Wire.setClock(100000); //07/25/19 slow down I2C clock for better noise immunity

//motor control PWM pins

//set up left motor pins as outputs

pinMode(enA, OUTPUT);

pinMode(in1, OUTPUT);

pinMode(in2, OUTPUT);

//set up right motor pins as outputs

pinMode(in3, OUTPUT);

pinMode(in4, OUTPUT);

pinMode(enB, OUTPUT);

pinMode(TOT_CURR_PIN, INPUT);//07/09/19 now connected to 'Total Current' 1NA619 charge current sensor

digitalWrite(TOT_CURR_PIN, LOW); //turn off the internal pullup resistor

analogReference(EXTERNAL); //07/09/18 now using external 3.3V ref

analogRead(0); //dummy read to let the ADC circuitry settle down

// configure LED for output

pinMode(LED_PIN, OUTPUT);

// initialize MPU6050 added 09/03/18

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

mpu.initialize();

// verify connection

Serial.println(F("Testing device connections..."));

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

DMPpacketSize = mpu.dmpGetFIFOPacketSize();

bMPU6050Ready = true;

StartSec = millis() / 1000.f;

mySerial.printf("\nMPU6050 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;

}

mySerial.printf("Sec\tHdg\tLdist\tRdist\tErrDeg\tPrevDeg\tTurnDir\tTurnDeg\n"); //column headers

m_LeftDistCm = GetLeftDistCm();

m_RightDistCm = GetRightDistCm();

RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

SinceLastDistanceCheck = 0;

sinceLastTimeCheck = 0; //used for rolling turn timeout

sinceLastHdgCheck = 0; //rolling turn heading check timer

mySerial.printf("Starting test turns...\n");

delay(1000);

mpu.resetFIFO(); //07/27/19 make sure we are starting with a clean slate

mpu.getIntStatus(); //required to make sure the FIFO overflow interrupt bit is reset

mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount());

mySerial.printf("Turning right 30 deg...\n");

RollingTurn(true, true, 30);

delay(1000);

mySerial.printf("Turning left 30 deg...\n");

mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount());

mpu.resetFIFO(); //07/27/19 make sure we are starting with a clean slate

mpu.getIntStatus(); //required to make sure the FIFO overflow interrupt bit is reset

mySerial.printf("FIFO count = %d\n", mpu.getFIFOCount());

RollingTurn(false, true, 30);

sinceLastNavUpdateMsec = 0;

}

void loop()

{

// if programming failed, don't try to do anything

if (dmpReady && mpu.dmpPacketAvailable())

{

//global_yawval = GetIMUHeadingDeg(); //retrieve the most current yaw value from IMU

GetIMUHeadingDeg(); //updates global_yawval if successful

// blink LED to indicate activity

bool blinkState = !blinkState;

digitalWrite(LED_PIN, blinkState);

}

//if (SecSinceStart - StartSec > U_TURN_SEC)

//{

// mySerial.printf("Turning Around!\n");

// RollingTurn(TURNDIR_CCW,true,180);

// RunBothMotorsMsec(U_TURN_SEC*1000, MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

// StopBothMotors();

// while (1);

//}

if (sinceLastNavUpdateMsec >= MIN_PING_INTERVAL_MSEC)

{

sinceLastNavUpdateMsec -= MIN_PING_INTERVAL_MSEC;

int loc_leftdist = GetLeftDistCm();

int loc_rightdist = GetRightDistCm();

int loc_disterror = 0;

int turndeg = 0;

bool turndirection = false;

UpdateDistArrays(loc_leftdist, loc_rightdist);

////decide what to do

//if (loc_leftdist < loc_rightdist)

//{

// //left wall is closer, so CCW moves twd wall, CW moves away

// loc_disterror = loc_leftdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away'

// turndirection = loc_disterror > 0 ? TURNDIR_CCW : TURNDIR_CW;

// if (abs(loc_disterror) > VERY_FAR_AWAY_CM) //use 20-deg cut

// {

// turndeg = VERY_FAR_AWAY_TURN_DEG;

// }

// else if (abs(loc_disterror) > FAR_AWAY_CM) //use 10-deg cut

// {

// turndeg = FAR_AWAY_TURN_DEG;

// }

// else

// {

// mySerial.printf("Close to target distance - what do I do now?\n");

// RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

// }

//}

//else //right wall is closer or equal...

//{

// //right wall is closer, so CW moves twd wall, CCW moves away

// loc_disterror = loc_rightdist - DESIRED_WALL_OFFSET_DIST_CM; //pos diff is 'too far away'

// turndirection = loc_disterror > 0 ? TURNDIR_CW : TURNDIR_CCW;

// if (abs(loc_disterror) > m_TrkErrWinMult * VERY_FAR_AWAY_CM) //use 20-deg cut

// {

// turndeg = VERY_FAR_AWAY_TURN_DEG;

// }

// else if (abs(loc_disterror) > m_TrkErrWinMult * FAR_AWAY_CM) //use 10-deg cut

// {

// turndeg = FAR_AWAY_TURN_DEG;

// }

// else if (abs(loc_disterror) > m_TrkErrWinMult * CLOSE_CM) //use 5-deg cut

// {

// turndeg = CLOSE_TURN_DEG;

// m_TrkErrWinMult = WALL_TRK_ERR_WIN_MULTFACT;

// }

// else //must be close to desired offset distance. Reduce error window size

// {

// //mySerial.printf("Close to target distance - setting turndeg to zero\n");

// turndeg = 0;

// m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //go back to approach window sizes

// }

//}

//if (turndeg == 0)

//{

// //robot is close to target - take out any remaining cut

// //mySerial.printf("turndeg = 0; Removing Entire Cut\n");

// if (m_PrevHdgCutDeg > 0)

// {

// RollingTurn(!turndirection, true, m_PrevHdgCutDeg);

// m_PrevHdgCutDeg = 0;

// }

//}

//else if (turndeg > m_PrevHdgCutDeg)

//{

// //cut not cutting it; increase it by 10 deg

// //mySerial.printf("turndeg > m_PrevHdgCutDeg; Increasing Cut\n");

// turndeg = TURN_INCREMENT_DEG;

// RollingTurn(turndirection, true, turndeg);

// m_PrevHdgCutDeg += TURN_INCREMENT_DEG;

//}

//else if (turndeg < m_PrevHdgCutDeg)// turndeg > 0 && < m_PrevHdgCutDeg)

//{

// if (isConverging(DESIRED_WALL_OFFSET_DIST_CM, true))

// {

// //take out some of the cut

// //mySerial.printf(" turndeg > 0 && < m_PrevHdgCutDeg: Reducing Cut\n");

// turndeg = TURN_INCREMENT_DEG;

// RollingTurn(!turndirection, true, turndeg);

// m_PrevHdgCutDeg -= TURN_INCREMENT_DEG;

// }

//}

//else if (turndeg == m_PrevHdgCutDeg) //this case takes more work

//{

// if (isConverging(DESIRED_WALL_OFFSET_DIST_CM, true))

// {

// //do nothing - things are going smoothly

// }

// else

// {

// //cut has been maxed out, but dist & tgt not converging

// //increase cut in same direction as before

// //mySerial.printf("not converging - increasing cut\n");

// turndeg = TURN_INCREMENT_DEG;

// RollingTurn(turndirection, true, turndeg);

// m_PrevHdgCutDeg += TURN_INCREMENT_DEG;

// m_TrkErrWinMult = WALL_APPR_ERR_WIN_MULTFACT; //go back to approach sensitivity

// }

//}

////regardless of what happens above, continue to run the motors straight ahead

//RunBothMotors(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

//07/09/19 if GetIMUHeadingDeg() succeeds, do the rest

TotAmps = GetTotalAmps();

SecSinceStart = millis() / 1000.f; //hopefully this will be a float result

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

SecSinceStart, global_yawval,loc_leftdist,loc_rightdist, loc_disterror,m_PrevHdgCutDeg, turndirection == TURNDIR_CCW ? "CCW":"CW", turndeg);

}

#pragma region MOTOR_SUPPORT_ROUTINES

void RunBothMotors(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:

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

////DEBUG!!

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

////DEBUG!!

SetLeftMotorSpeed(leftspeednum);

SetRightMotorSpeed(rightspeednum);

}

//12/26/14 added for independent motor speed

void RunBothMotorsMsec(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

RunBothMotors(leftspeednum, rightspeednum);

//Step 2: Delay timsec seconds

delay(timeMsec);

StopBothMotors();

}

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

void StopBothMotors()

{

//added analogWrite calls to ensure speed set to zero

analogWrite(enA, MOTOR_SPEED_OFF);

analogWrite(enB, MOTOR_SPEED_OFF);

digitalWrite(in1, LOW);

digitalWrite(in2, LOW);

digitalWrite(in3, LOW);

digitalWrite(in4, LOW);

}

//01/24/15 removed time from MoveAhead sig, added MoveAheadMsec

void MoveAhead(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

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

//Step 1: Set forward direction for both wheels

SetLeftMotorDir(true);

SetRightMotorDir(true);

//Step 2: Run both myMotors for timsec seconds

RunBothMotors(leftspeednum, rightspeednum);

}

void MoveReverse(int leftspeednum, int rightspeednum)

{

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

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

//Step 1: Set reverse direction for both wheels

SetLeftMotorDir(false);

SetRightMotorDir(false);

//Step 2: Run both myMotors for timsec seconds

//01/24/15 - removed timing - now continuous run

RunBothMotors(leftspeednum, rightspeednum);

}

void SetLeftMotorDir(boolean bIsFwdDir)

{

if (bIsFwdDir)

{

digitalWrite(in1, LOW);

digitalWrite(in2, HIGH);

}

else //must be REV

{

digitalWrite(in1, HIGH);

digitalWrite(in2, LOW);

}

bLeftMotorDirIsFwd = bIsFwdDir;

}

void SetRightMotorDir(boolean bIsFwdDir)

{

if (bIsFwdDir)

{

digitalWrite(in3, LOW);

digitalWrite(in4, HIGH);

}

else //must be REV

{

digitalWrite(in3, HIGH);

digitalWrite(in4, LOW);

}

bRightMotorDirIsFwd = bIsFwdDir;

}

//11/25/15 added to utilize new private member variable bLeftMotorDirIsFwd

void SetLeftMotorSpeed(int speed)

{

if (bLeftMotorDirIsFwd)

{

speed = speed + LEFT_SPEED_COMP_VAL_FWD;

//check limits

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

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

}

else //must be REV

{

speed = speed + LEFT_SPEED_COMP_VAL_REV;

//check limits

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

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

}

#ifndef NO_MOTORS

//Serial.print("Left Speed = "); Serial.println(speed);

analogWrite(enA, speed);

#endif

//02/13/16 added for 'pause' debug

m_FinalLeftSpeed = speed;

}

//11/25/15 added to utilize new private member variable bRightMotorDirIsFwd

void SetRightMotorSpeed(int speed)

{

if (bRightMotorDirIsFwd)

{

speed = speed + LEFT_SPEED_COMP_VAL_FWD;

//check limits

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

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

}

else //must be REV

{

speed = speed + LEFT_SPEED_COMP_VAL_REV;

//check limits

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

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

}

//02/13/16 added for 'pause' debug

m_FinalLeftSpeed = speed;

#ifndef NO_MOTORS

//Serial.print("Right Speed = "); Serial.println(speed);

analogWrite(enB, speed);

#endif

}

#pragma endregion Motor Support Routines

bool GetIMUHeadingDeg()

{

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

// global_yawval 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 global_yawval with latest value

//Notes:

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

// 10/08/19 no longer need mpuIntStatus

//mpuIntStatus = mpu.getIntStatus(); //this resets the FIFO overflow interrupt bit

fifoCount = mpu.getFIFOCount();// get current FIFO count

// check for overflow (this should never happen unless our code is too inefficient)

//if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)

if (fifoCount >= 1024)

{

//if the FIFO count reset so we can continue cleanly

mySerial.printf("FIFO overflow with FIFO count = %d\n", mpu.getFIFOCount());

mpu.resetFIFO();

// now have to wait for DMP data ready again

int wait_count = 0;

while (!mpu.dmpPacketAvailable() && wait_count < 1000)

{

delay(1);

wait_count++;

}

//if the wait loop timed out, punt

if (wait_count >= 1000)

{

mySerial.printf("wait loop timed out wait_count = %d",wait_count);

return false;

}

// read all available packets from FIFO

while (fifoCount >= DMPpacketSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!

{

mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);

// track FIFO count here in case there is > 1 packet available

// (this lets us immediately read more without waiting for an interrupt)

fifoCount -= DMPpacketSize;

}

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

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

//compute the yaw value

global_yawval = ypr[0] * 180 / M_PI;

return true;

}

float GetTotalAmps()

{

//Purpose: Get total current in amps

//Inputs:

// Voltage on TOT_CURR_PIN is approximately Ichg*2 Amps

// VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio

//Outputs:

// returns total robot current (chg current plus running current)

//Notes:

// 02/28/18 chg name from GetBattChgAmps() to GetTotalAmps()

int ITotAnalogReading = GetAverageAnalogReading(TOT_CURR_PIN, CURRENT_AVERAGE_NUMBER); //range is 0-1023

float ITotVolts = ((float)ITotAnalogReading / (float)MAX_AD_VALUE)* ADC_REF_VOLTS;

float ITotAmps = ITotVolts * VOLTAGE_TO_CURRENT_RATIO;

return ITotAmps;

}

int GetAverageAnalogReading(int pin, int numavgs)

{

long totreads = 0;

for (int i = 0; i < numavgs; i++)

{

totreads += analogRead(pin);

}

return (int)((float)totreads / (float)numavgs); //truncation ok

}

//01/10/18 reverted to regular ping(). median distance function takes too long

int GetLeftDistCm()

{

//Serial.print("LeftPing\t"); Serial.println(millis());

//Notes:

// 04/30/17 added limit detection/correction

int timeUsec = LeftPing.ping();

//Serial.print("Left Ping Usec = "); Serial.println(timeUsec);

int distCm = LeftPing.convert_cm(timeUsec);

//04/30/17 added limit detection/correction

distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM;

return distCm;

}

//01/10/18 reverted to regular ping(). median distance function takes too long

int GetRightDistCm()

{

//Serial.print("RightPing\t"); Serial.println(millis());

//Notes:

// 04/30/17 added limit detection/correction

int timeUsec = RightPing.ping();

//Serial.print("Right Ping Usec = "); Serial.println(timeUsec);

int distCm = RightPing.convert_cm(timeUsec);

//04/30/17 added limit detection/correction

distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM;

return distCm;

}

bool RollingTurn(bool bIsCCW, bool bIsFwd, float numDeg)

{

//Purpose: Make a numDeg forwards or backwards CW turn

//Inputs:

// bIsFwd - True if turn is to be forward, 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

// global_yawval = IMU heading value updated by GetIMUHeadingDeg()

//Plan:

// Step1: Get current heading as starting point

// Step2: Compute new target value

// Step3: Set motor speeds based on fwd/backwds flag

// Step4: Run motors until target reached

//Notes:

// 08/22/18 now using MPU6050_CCW_INCREASES_YAWVAL define to compute tgt

// 08/29/18 removed MPU6050_CCW_INCREASES_YAWVAL - the MPU6050 DMP takes care of this

// 09/08/18 revised end-of-turn detection to include slope calc. Threshold alone is too fragile

// 09/11/18 revised to handle both turn directions

// 04/29/19 uncommented

// 07/31/19 rev for better initial heading capture

// 07/31/19 rev return type to bool

// 07/31/19 rev to eliminate timed loop - now runs as fast as possible

// 07/31/19 rev to not stop motors at end - now calling pgm is resp for this

// 10/06/19 rev for new IMU polling setup

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

//DEBUG!!

mySerial.printf("In RollingTurn(%s, %s,%4.2f)\n", bIsCCW == TURNDIR_CCW? "CCW":"CW", bIsFwd == true? "FWD":"BKWD", numDeg);

//DEBUG!!

//Step1: Get current heading as starting point

//if (!GetIMUHeadingDeg())

//{

// mySerial.printf("RollingTurn(%d,%d,%d): Initial Heading Capture Failed!",

// bIsCCW, bIsFwd, numDeg);

// return false;

//}

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

if (!dmpReady)

{

return false;

}

//Wait for packet availability (times out after one second) ...

int wait_count = 0;

while (!mpu.dmpPacketAvailable() && wait_count < 1000)

{

delay(1);

wait_count++;

}

//if the wait loop timed out, punt

if (wait_count >= 1000)

{

return false;

}

//if we get to here, the IMU is OK and at least one packet is available

//global_yawval = GetIMUHeadingDeg();

GetIMUHeadingDeg(); //updates global_yawval if successful

//Step2: Compute new target value & timeout value

//timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG;

timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG * 10000;//10/06/19 make timeout irrelevant

tgt_deg = bIsCCW ? global_yawval + numDeg : global_yawval - numDeg;

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

global_yawval, numDeg, tgt_deg, timeout_sec);

//DEBUG!!

//Step3: Start motors

SetRightMotorDir(bIsFwd);

SetLeftMotorDir(bIsFwd);

int rightspeed;

int leftspeed;

if (!bIsCCW) //2-motor robot is backwards from 4-motor one

{

rightspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED;

leftspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH;

}

else

{

rightspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH;

leftspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED;

}

SetLeftMotorSpeed(leftspeed);

SetRightMotorSpeed(rightspeed);

sinceLastTimeCheck = 0; //needed for watchdog timer

//mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n");

bool bFirstPass = true; //for 'slowdown' print control

float curHdgMatchVal = 0;

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

float prevHdgMatchVal = 0;

float matchSlope = 0;

//Step4: Run motors until target reached

mpu.resetFIFO();

while (!bDoneTurning && !bTimedOut)

{

//GetIMUHeadingDeg(); //07/31/19 now incorporates FIFO wait loop

if (mpu.dmpPacketAvailable())

{

//global_yawval = GetIMUHeadingDeg(); //10/06/19 only do this if a new packet is avail

GetIMUHeadingDeg(); //updates global_yawval if successful

//DEBUG!!

mySerial.printf("%lu\t%4.2f\t%d\n", millis(), global_yawval, fifoCount);

//DEBUG!!

}

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

matchSlope = curHdgMatchVal - prevHdgMatchVal;

if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL)

{

if (bFirstPass)

{

prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs

//mySerial.printf("Slowing down at %4.2f deg\n", global_yawval);

bFirstPass = false;

}

//look for full match

//bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

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

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

//DEBUG!!

if (bDoneTurning)

{

mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, and slope = %1.3f\n",

global_yawval, tgt_deg, curHdgMatchVal, matchSlope);

}

//DEBUG!!

if (bTimedOut)

{

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

}

mpu.resetFIFO();

}

return true;

}

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

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

}

bool isConverging(int tgtdist, bool bIsRightSide)

{

//Purpose: Determine if act dist & tgt dist are converging

//Inputs:

// tgtdist = it representing target distance

// bIsRightSide = boolean denoting side under consideration. True = right, False = left

// aRightDist[], aLeftDist[] = int arrays containing the LR_PING_DIST_ARRAY_SIZE previous ping measurements

//Plan:

// Step1: compute avg slope = (dist[LR_PING_DIST_ARRAY_SIZE-1]-dist[0])/LR_PING_DIST_ARRAY_SIZE

// Step2: if slope > 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] < tgtdist, return TRUE (convergent)

// if slope < 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] > tgtdist, return TRUE (convergent)

// else, return FALSE (non-convergent)

//Step1: compute avg slope = (dist[LR_PING_DIST_ARRAY_SIZE-1]-dist[0])/LR_PING_DIST_ARRAY_SIZE

int slope = 0;

int last_dist = 0;

int first_dist = 0;

if (bIsRightSide)

{

first_dist = aRightDist[0];

last_dist = aRightDist[LR_PING_DIST_ARRAY_SIZE - 1];

slope = last_dist - first_dist; //don't need to divide

}

else

{

first_dist = aLeftDist[0];

last_dist = aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1];

slope = last_dist - first_dist; //don't need to divide

}

////DEBUG!!

// mySerial.printf("IsConverging: firstd/lastd = %d/%d, slope = %d\ttgtdist = %d\n", first_dist,last_dist, slope, tgtdist);

////DEBUG!!

//Step2: if slope > 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] < tgtdist, return TRUE (convergent)

// if slope < 0 && dist[LR_PING_DIST_ARRAY_SIZE-1] > tgtdist, return TRUE (convergent)

// else, return FALSE (non-convergent)

if (slope > 0 && last_dist <= tgtdist) return true;

if (slope < 0 && last_dist >= tgtdist) return true;

else return false;

}

void UpdateDistArrays(int leftdist, int rightdist)

{

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

{

aLeftDist[i] = aLeftDist[i + 1];

aRightDist[i] = aRightDist[i + 1];

}

aLeftDist[LR_PING_DIST_ARRAY_SIZE - 1] = leftdist;

aRightDist[LR_PING_DIST_ARRAY_SIZE - 1] = rightdist;

}

In the above code, the only relevant functions are the ‘GetIMUHeading()’ and ‘RollingTurn()’ functions, as shown below:

bool RollingTurn(bool bIsCCW, bool bIsFwd, float numDeg)
{
	//Purpose: Make a numDeg forwards or backwards CW turn
	//Inputs:
	//	bIsFwd - True if turn is to be forward, 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
	//	global_yawval = IMU heading value updated by GetIMUHeadingDeg()
	//Plan:
	//	Step1: Get current heading as starting point
	//	Step2: Compute new target value
	//	Step3: Set motor speeds based on fwd/backwds flag
	//	Step4: Run motors until target reached
	//Notes:
	//	08/22/18 now using MPU6050_CCW_INCREASES_YAWVAL define to compute tgt
	//	08/29/18 removed MPU6050_CCW_INCREASES_YAWVAL - the MPU6050 DMP takes care of this
	//	09/08/18 revised end-of-turn detection to include slope calc.  Threshold alone is too fragile
	//	09/11/18 revised to handle both turn directions
	//	04/29/19 uncommented
	//	07/31/19 rev for better initial heading capture
	//	07/31/19 rev return type to bool
	//	07/31/19 rev to eliminate timed loop - now runs as fast as possible
	//	07/31/19 rev to not stop motors at end - now calling pgm is resp for this
	//	10/06/19 rev for new IMU polling setup

	float tgt_deg;
	float timeout_sec;
	bool bDoneTurning = false;
	bool bTimedOut = false;

//DEBUG!!
	mySerial.printf("In RollingTurn(%s, %s,%4.2f)\n", bIsCCW == TURNDIR_CCW? "CCW":"CW", bIsFwd == true? "FWD":"BKWD", numDeg);
//DEBUG!!

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

	//Wait for packet availability (times out after one second) ...
	int wait_count = 0;
	while (!mpu.dmpPacketAvailable() && wait_count < 1000)
	{
		delay(1);
		wait_count++;
	}

	//if the wait loop timed out, punt
	if (wait_count >= 1000)
	{
		return false;
	}

	//if we get to here, the IMU is OK and at least one packet is available
	GetIMUHeadingDeg(); //updates global_yawval if successful

//Step2: Compute new target value & timeout value
	//timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG;
	timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG * 10000;//10/06/19 make timeout irrelevant for testing

	tgt_deg = bIsCCW ? global_yawval + numDeg : global_yawval - numDeg;

	//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",
		global_yawval, numDeg, tgt_deg, timeout_sec);
//DEBUG!!

//Step3: Start motors
	SetRightMotorDir(bIsFwd);
	SetLeftMotorDir(bIsFwd);
	int rightspeed;
	int leftspeed;

	if (!bIsCCW) //2-motor robot is backwards from 4-motor one
	{
		rightspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED;
		leftspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH;

	}
	else
	{
		rightspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH;
		leftspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED;
	}
	SetLeftMotorSpeed(leftspeed);
	SetRightMotorSpeed(rightspeed);


	sinceLastTimeCheck = 0; //needed for watchdog timer
	//mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n");
	bool bFirstPass = true; //for 'slowdown' print control
	float curHdgMatchVal = 0;

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

//Step4: Run motors until target reached
	while (!bDoneTurning && !bTimedOut)
	{

		//GetIMUHeadingDeg(); //07/31/19 now incorporates FIFO wait loop
		if (mpu.dmpPacketAvailable())
		{
			//global_yawval = GetIMUHeadingDeg(); //10/06/19 only do this if a new packet is avail
			GetIMUHeadingDeg();  //updates global_yawval if successful

//DEBUG!!
			mySerial.printf("%lu\t%4.2f\t%d\n", millis(), global_yawval, fifoCount);
//DEBUG!!
		}

		//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, global_yawval);
		matchSlope = curHdgMatchVal - prevHdgMatchVal;

		if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL)
		{
			if (bFirstPass)
			{
				prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs
				//mySerial.printf("Slowing down at %4.2f deg\n", global_yawval);
				bFirstPass = false;
			}
		}

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

//DEBUG!!
		if (bDoneTurning)
		{
			mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, and slope = %1.3f\n",
				global_yawval, tgt_deg, curHdgMatchVal, matchSlope);
		}
//DEBUG!!

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

		}
	}

	return 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

bool RollingTurn(bool bIsCCW, bool bIsFwd, float numDeg)

{

//Purpose: Make a numDeg forwards or backwards CW turn

//Inputs:

// bIsFwd - True if turn is to be forward, 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

// global_yawval = IMU heading value updated by GetIMUHeadingDeg()

//Plan:

// Step1: Get current heading as starting point

// Step2: Compute new target value

// Step3: Set motor speeds based on fwd/backwds flag

// Step4: Run motors until target reached

//Notes:

// 08/22/18 now using MPU6050_CCW_INCREASES_YAWVAL define to compute tgt

// 08/29/18 removed MPU6050_CCW_INCREASES_YAWVAL - the MPU6050 DMP takes care of this

// 09/08/18 revised end-of-turn detection to include slope calc. Threshold alone is too fragile

// 09/11/18 revised to handle both turn directions

// 04/29/19 uncommented

// 07/31/19 rev for better initial heading capture

// 07/31/19 rev return type to bool

// 07/31/19 rev to eliminate timed loop - now runs as fast as possible

// 07/31/19 rev to not stop motors at end - now calling pgm is resp for this

// 10/06/19 rev for new IMU polling setup

float tgt_deg;

float timeout_sec;

bool bDoneTurning = false;

bool bTimedOut = false;

//DEBUG!!

mySerial.printf("In RollingTurn(%s, %s,%4.2f)\n", bIsCCW == TURNDIR_CCW? "CCW":"CW", bIsFwd == true? "FWD":"BKWD", numDeg);

//DEBUG!!

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

if (!dmpReady)

{

return false;

}

//Wait for packet availability (times out after one second) ...

int wait_count = 0;

while (!mpu.dmpPacketAvailable() && wait_count < 1000)

{

delay(1);

wait_count++;

}

//if the wait loop timed out, punt

if (wait_count >= 1000)

{

return false;

}

//if we get to here, the IMU is OK and at least one packet is available

GetIMUHeadingDeg(); //updates global_yawval if successful

//Step2: Compute new target value & timeout value

//timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG;

timeout_sec = numDeg * ROLLING_TURN_MAX_SEC_PER_DEG * 10000;//10/06/19 make timeout irrelevant for testing

tgt_deg = bIsCCW ? global_yawval + numDeg : global_yawval - numDeg;

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

global_yawval, numDeg, tgt_deg, timeout_sec);

//DEBUG!!

//Step3: Start motors

SetRightMotorDir(bIsFwd);

SetLeftMotorDir(bIsFwd);

int rightspeed;

int leftspeed;

if (!bIsCCW) //2-motor robot is backwards from 4-motor one

{

rightspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED;

leftspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH;

}

else

{

rightspeed = (bIsFwd) ? OFFSIDE_MOTOR_SPEED : DRIVESIDE_MOTOR_SPEED_HIGH;

leftspeed = (bIsFwd) ? DRIVESIDE_MOTOR_SPEED_HIGH : OFFSIDE_MOTOR_SPEED;

}

SetLeftMotorSpeed(leftspeed);

SetRightMotorSpeed(rightspeed);

sinceLastTimeCheck = 0; //needed for watchdog timer

//mySerial.printf("hdg\typr\ttgt\tmatch\tslope\n");

bool bFirstPass = true; //for 'slowdown' print control

float curHdgMatchVal = 0;

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

float prevHdgMatchVal = 0;

float matchSlope = 0;

//Step4: Run motors until target reached

while (!bDoneTurning && !bTimedOut)

{

//GetIMUHeadingDeg(); //07/31/19 now incorporates FIFO wait loop

if (mpu.dmpPacketAvailable())

{

//global_yawval = GetIMUHeadingDeg(); //10/06/19 only do this if a new packet is avail

GetIMUHeadingDeg(); //updates global_yawval if successful

//DEBUG!!

mySerial.printf("%lu\t%4.2f\t%d\n", millis(), global_yawval, fifoCount);

//DEBUG!!

}

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

matchSlope = curHdgMatchVal - prevHdgMatchVal;

if (abs(curHdgMatchVal) > HDG_NEAR_MATCH_VAL)

{

if (bFirstPass)

{

prevHdgMatchVal = curHdgMatchVal; //init baseline for slope calcs

//mySerial.printf("Slowing down at %4.2f deg\n", global_yawval);

bFirstPass = false;

}

//look for full match

//bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL

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

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

//DEBUG!!

if (bDoneTurning)

{

mySerial.printf("found match with yaw = %3.2f, tgt = %3.2f, match = %1.3f, and slope = %1.3f\n",

global_yawval, tgt_deg, curHdgMatchVal, matchSlope);

}

//DEBUG!!

if (bTimedOut)

{

//DEBUG!!

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

return false;

//DEBUG!!

}

return true;

}

bool GetIMUHeadingDeg()
{
	//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
	//	global_yawval 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 global_yawval with latest value
	//Notes:
	//	10/08/19 changed return type to boolean
	//	10/08/19 no longer need mpuIntStatus

	//mpuIntStatus = mpu.getIntStatus(); //this resets the FIFO overflow interrupt bit
	fifoCount = mpu.getFIFOCount();// get current FIFO count

	// check for overflow (this should never happen unless our code is too inefficient)
	//if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)
	if (fifoCount >= 1024)
	{
		//if the FIFO count reset so we can continue cleanly
		mySerial.printf("FIFO overflow with FIFO count = %d\n", mpu.getFIFOCount());
		mpu.resetFIFO();

		// now have to wait for DMP data ready again
		int wait_count = 0;
		while (!mpu.dmpPacketAvailable() && wait_count < 1000)
		{
			delay(1);
			wait_count++;
		}

		//if the wait loop timed out, punt
		if (wait_count >= 1000)
		{
			mySerial.printf("wait loop timed out wait_count = %d",wait_count);
			return false; 
		}
	}

	// read all available packets from FIFO
	while (fifoCount >= DMPpacketSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!
	{
		mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);
		// track FIFO count here in case there is > 1 packet available
		// (this lets us immediately read more without waiting for an interrupt)
		fifoCount -= DMPpacketSize;
	}

	// display Euler angles in degrees
	mpu.dmpGetQuaternion(&q, fifoBuffer);
	mpu.dmpGetGravity(&gravity, &q);
	mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

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

bool GetIMUHeadingDeg()

{

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

// global_yawval 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 global_yawval with latest value

//Notes:

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

// 10/08/19 no longer need mpuIntStatus

//mpuIntStatus = mpu.getIntStatus(); //this resets the FIFO overflow interrupt bit

fifoCount = mpu.getFIFOCount();// get current FIFO count

// check for overflow (this should never happen unless our code is too inefficient)

//if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)

if (fifoCount >= 1024)

{

//if the FIFO count reset so we can continue cleanly

mySerial.printf("FIFO overflow with FIFO count = %d\n", mpu.getFIFOCount());

mpu.resetFIFO();

// now have to wait for DMP data ready again

int wait_count = 0;

while (!mpu.dmpPacketAvailable() && wait_count < 1000)

{

delay(1);

wait_count++;

}

//if the wait loop timed out, punt

if (wait_count >= 1000)

{

mySerial.printf("wait loop timed out wait_count = %d",wait_count);

return false;

}

// read all available packets from FIFO

while (fifoCount >= DMPpacketSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!

{

mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);

// track FIFO count here in case there is > 1 packet available

// (this lets us immediately read more without waiting for an interrupt)

fifoCount -= DMPpacketSize;

}

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

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

//compute the yaw value

global_yawval = ypr[0] * 180 / M_PI;

return true;

}

When the ‘RollingTurn()’ function is called, it waits for mpu.dmpPacketAvailable() to return TRUE, and then it calls GetIMUHeadingDeg(), which updates a global variable (subtly named ‘global_yawval’. This value is then used to determine turn completion.

GetIMUHeadingDeg() reads bytes from the FIFO and computes a yaw value using the retreived quaternion data.

After getting everything going to my satisfaction, I added code to setup() for a 30-degree turn to the right followed by a 30-degree turn to the left, followed by an infinite loop of yaw value readouts. The output from one test run is shown below.

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

MPU6050 Ready at 5.22 Sec
Sec	Hdg	Ldist	Rdist	ErrDeg	PrevDeg	TurnDir	TurnDeg
Starting test turns...
FIFO count = 0
Turning right 30 deg...
In RollingTurn(CW, FWD,30.00)
Init hdg = 2.13 deg, Turn = 30.00  deg, tgt = 32.13 deg, timeout = 20000.00 sec
6269	2.14	0
6280	2.16	0
6289	2.18	0
6299	2.20	0
6308	2.22	0
6319	2.23	0
6329	2.25	0
6339	2.27	0
6348	2.29	0
6359	2.32	0
6369	2.34	0
6379	2.35	0
6389	2.37	0
6398	2.39	0
6409	2.41	0
6418	2.42	0
6428	2.44	0
6439	2.46	0
6449	2.48	0
6459	2.51	0
6468	2.52	0
6478	2.54	0
6489	2.56	0
6499	2.58	0
6509	2.60	0
6518	2.61	0
6529	2.63	0
6539	2.66	0
6549	2.68	0
6558	2.69	0
6568	2.71	0
6578	2.73	0
6589	2.75	0
6599	2.77	0
6608	2.79	0
6619	2.80	0
6628	2.83	0
6638	2.85	0
6648	2.87	0
6659	2.89	0
6668	2.91	0
6678	2.92	0
6688	2.94	0
6699	2.96	0
6709	2.98	0
6719	3.00	0
6728	3.02	0
6737	3.05	0
6748	3.07	0
6758	3.10	0
6768	3.13	0
6778	3.15	0
6788	3.19	0
6798	3.25	0
6808	3.31	0
6818	3.39	0
6829	3.50	0
6838	3.71	0
6848	4.02	0
6857	4.40	0
6867	4.75	0
6878	5.06	0
6888	5.38	0
6898	5.69	0
6907	6.02	0
6918	6.36	0
6928	6.70	0
6938	7.00	0
6948	7.28	0
6958	7.58	0
6968	7.89	0
6977	8.22	0
6987	8.53	0
6998	8.82	0
7008	9.13	0
7017	9.44	0
7027	9.72	0
7037	9.96	0
7048	10.20	0
7058	10.43	0
7067	10.65	0
7077	10.88	0
7088	11.12	0
7098	11.33	0
7107	11.54	0
7117	11.78	0
7128	12.04	0
7138	12.25	0
7147	12.47	0
7157	12.70	0
7168	12.92	0
7177	13.13	0
7187	13.35	0
7197	13.56	0
7207	13.76	0
7217	13.99	0
7227	14.28	0
7237	14.59	0
7247	14.93	0
7257	15.26	0
7267	15.57	0
7277	15.87	0
7287	16.15	0
7297	16.39	0
7308	16.62	0
7317	16.86	0
7327	17.10	0
7336	17.33	0
7347	17.59	0
7357	17.90	0
7366	18.23	0
7377	18.55	0
7387	18.86	0
7397	19.17	0
7407	19.48	0
7417	19.77	0
7427	20.00	0
7437	20.22	0
7447	20.45	0
7456	20.68	0
7467	20.91	0
7477	21.15	0
7487	21.40	0
7496	21.65	0
7506	21.90	0
7517	22.16	0
7527	22.41	0
7536	22.65	0
7546	22.85	0
7557	23.04	0
7566	23.22	0
7576	23.43	0
7586	23.68	0
7597	23.97	0
7606	24.23	0
7617	24.46	0
7626	24.73	0
7636	25.05	0
7647	25.36	0
7657	25.63	0
7666	25.87	0
7676	26.13	0
7687	26.44	0
7697	26.78	0
7706	27.08	0
7716	27.39	0
7727	27.71	0
7736	28.03	0
7746	28.32	0
7756	28.58	0
7767	28.84	0
7776	29.03	0
7786	29.24	0
7796	29.47	0
7805	29.73	0
7816	29.99	0
7826	30.24	0
7836	30.48	0
found match with yaw = 30.48, tgt = 32.13, match = 0.991, and slope = 0.090
Turning left 30 deg...
FIFO count = 1024
FIFO count = 0
In RollingTurn(CCW, FWD,30.00)
Init hdg = 41.58 deg, Turn = 30.00  deg, tgt = 11.58 deg, timeout = 20000.00 sec
8864	41.59	0
8873	41.59	0
8885	41.54	0
8894	41.55	0
8903	41.57	0
8913	41.58	0
8925	41.58	0
8934	41.44	0
8943	41.26	0
8953	41.06	0
8964	40.78	0
8974	40.49	0
8984	40.23	0
8993	39.98	0
9004	39.72	0
9014	39.43	0
9023	39.14	0
9033	38.84	0
9044	38.53	0
9054	38.22	0
9064	37.91	0
9073	37.62	0
9083	37.42	0
9094	37.32	0
9103	37.28	0
9113	37.30	0
9124	37.32	0
9134	37.33	0
9143	37.33	0
9153	37.21	0
9163	37.08	0
9174	36.96	0
9183	36.80	0
9194	36.65	0
9203	36.51	0
9213	36.38	0
9223	36.26	0
9234	36.13	0
9243	36.01	0
9253	35.85	0
9264	35.66	0
9274	35.46	0
9283	35.25	0
9293	35.04	0
9304	34.85	0
9314	34.68	0
9323	34.53	0
9333	34.42	0
9344	34.31	0
9354	34.20	0
9363	34.09	0
9373	33.97	0
9383	33.84	0
9393	33.69	0
9403	33.52	0
9413	33.35	0
9423	33.18	0
9433	33.03	0
9443	32.88	0
9453	32.73	0
9463	32.59	0
9473	32.44	0
9483	32.26	0
9493	32.07	0
9503	31.85	0
9512	31.63	0
9523	31.42	0
9533	31.25	0
9543	31.11	0
9552	30.99	0
9563	30.91	0
9573	30.84	0
9583	30.75	0
9592	30.66	0
9603	30.55	0
9613	30.44	0
9623	30.32	0
9632	30.22	0
9643	30.10	0
9653	29.98	0
9663	29.86	0
9672	29.70	0
9682	29.54	0
9693	29.37	0
9703	29.18	0
9712	29.01	0
9722	28.82	0
9733	28.64	0
9742	28.46	0
9752	28.30	0
9762	28.13	0
9773	27.95	0
9782	27.77	0
9792	27.59	0
9802	27.39	0
9811	27.19	0
9822	26.99	0
9832	26.80	0
9842	26.62	0
9851	26.45	0
9863	26.26	0
9872	26.06	0
9882	25.85	0
9892	25.62	0
9903	25.38	0
9912	25.13	0
9922	24.87	0
9932	24.62	0
9942	24.36	0
9952	24.09	0
9962	23.85	0
9972	23.62	0
9981	23.39	0
9992	23.12	0
10002	22.84	0
10011	22.56	0
10021	22.29	0
10032	22.03	0
10042	21.76	0
10052	21.54	0
10061	21.35	0
10072	21.17	0
10082	20.97	0
10091	20.78	0
10102	20.61	0
10112	20.45	0
10122	20.28	0
10132	20.13	0
10141	19.95	0
10151	19.81	0
10162	19.68	0
10171	19.56	0
10182	19.40	0
10191	19.24	0
10202	19.06	0
10212	18.90	0
10221	18.73	0
10231	18.56	0
10242	18.39	0
10251	18.22	0
10262	18.01	0
10271	17.82	0
10281	17.65	0
10292	17.47	0
10301	17.29	0
10311	17.14	0
10321	17.02	0
10331	16.90	0
10342	16.77	0
10351	16.64	0
10361	16.48	0
10372	16.33	0
10381	16.18	0
10391	16.01	0
10401	15.84	0
10412	15.64	0
10421	15.42	0
10431	15.22	0
10441	15.01	0
10451	14.81	0
10461	14.64	0
10471	14.49	0
10481	14.36	0
10491	14.24	0
10501	14.11	0
10511	13.97	0
10521	13.81	0
10530	13.64	0
10541	13.50	0
10551	13.36	0
found match with yaw = 13.36, tgt = 11.58, match = 0.990, and slope = 0.090
10.78	10.13	200	200	0	0	CCW	0
10.98	7.50	200	200	0	0	CCW	0
11.18	7.28	200	200	0	0	CCW	0
11.38	7.67	200	200	0	0	CCW	0
11.58	7.23	200	200	0	0	CCW	0
11.78	4.82	200	200	0	0	CCW	0
11.98	5.16	200	200	0	0	CCW	0
12.18	5.56	200	200	0	0	CCW	0
12.38	5.97	200	200	0	0	CCW	0
12.58	6.73	200	200	0	0	CCW	0
12.78	7.44	200	200	0	0	CCW	0
12.98	7.96	200	200	0	0	CCW	0
13.18	10.70	200	200	0	0	CCW	0
13.38	12.16	200	200	0	0	CCW	0
13.58	12.86	200	200	0	0	CCW	0
13.78	13.98	200	200	0	0	CCW	0
13.98	14.56	200	200	0	0	CCW	0
14.18	15.57	200	200	0	0	CCW	0
14.38	16.86	200	200	0	0	CCW	0
14.58	17.39	200	200	0	0	CCW	0
14.78	17.37	200	200	0	0	CCW	0
14.98	17.60	200	200	0	0	CCW	0
15.18	17.96	200	200	0	0	CCW	0
15.38	18.34	200	200	0	0	CCW	0
15.58	18.72	200	200	0	0	CCW	0
15.78	19.10	200	200	0	0	CCW	0
15.98	19.50	200	200	0	0	CCW	0
16.18	19.88	200	200	0	0	CCW	0
16.38	20.21	200	200	0	0	CCW	0
16.58	20.99	200	200	0	0	CCW	0
16.78	22.92	200	200	0	0	CCW	0
16.98	25.77	200	200	0	0	CCW	0
17.18	26.87	200	200	0	0	CCW	0
17.38	28.11	200	200	0	0	CCW	0
17.58	29.87	200	200	0	0	CCW	0
17.78	30.60	200	200	0	0	CCW	0
17.98	30.97	200	200	0	0	CCW	0
18.18	31.40	200	200	0	0	CCW	0
18.38	31.87	200	200	0	0	CCW	0
18.58	32.27	200	200	0	0	CCW	0
18.78	33.46	200	200	0	0	CCW	0
18.98	36.06	200	200	0	0	CCW	0
19.18	39.47	200	200	0	0	CCW	0
19.38	46.26	200	200	0	0	CCW	0
19.58	53.56	200	200	0	0	CCW	0
19.78	57.95	200	200	0	0	CCW	0
19.98	62.18	200	200	0	0	CCW	0
20.18	68.34	200	200	0	0	CCW	0
20.38	74.71	200	200	0	0	CCW	0
20.58	80.05	200	200	0	0	CCW	0
20.78	84.66	200	200	0	0	CCW	0
20.98	91.72	200	200	0	0	CCW	0
21.18	100.94	200	200	0	0	CCW	0
21.38	109.92	200	200	0	0	CCW	0
21.58	116.38	200	200	0	0	CCW	0
21.78	118.67	200	200	0	0	CCW	0
21.98	119.04	200	200	0	0	CCW	0
22.18	119.33	200	200	0	0	CCW	0
22.38	117.98	200	200	0	0	CCW	0
22.58	109.04	200	200	0	0	CCW	0
22.78	99.40	200	200	0	0	CCW	0
22.98	92.47	200	200	0	0	CCW	0
23.18	85.81	200	200	0	0	CCW	0
23.38	81.29	200	200	0	0	CCW	0
23.58	74.51	200	200	0	0	CCW	0
23.78	67.58	200	200	0	0	CCW	0
23.98	59.32	200	200	0	0	CCW	0
24.18	53.99	200	200	0	0	CCW	0
24.38	50.39	200	200	0	0	CCW	0
24.58	46.73	200	200	0	0	CCW	0
24.78	42.14	200	200	0	0	CCW	0
24.98	39.26	200	200	0	0	CCW	0
25.18	39.60	200	200	0	0	CCW	0
25.38	39.97	200	200	0	0	CCW	0
25.58	40.36	200	200	0	0	CCW	0
25.78	40.73	200	200	0	0	CCW	0
25.98	41.11	200	200	0	0	CCW	0
26.18	40.66	200	200	0	0	CCW	0
26.38	39.16	200	200	0	0	CCW	0
26.58	36.13	200	200	0	0	CCW	0
26.78	35.51	200	200	0	0	CCW	0
26.98	35.92	200	200	0	0	CCW	0
27.18	36.30	200	200	0	0	CCW	0
27.38	36.67	200	200	0	0	CCW	0
27.58	37.06	200	200	0	0	CCW	0
27.78	37.46	200	200	0	0	CCW	0
27.98	37.85	200	200	0	0	CCW	0
28.18	38.22	200	200	0	0	CCW	0
28.38	38.60	200	200	0	0	CCW	0
28.58	38.97	200	200	0	0	CCW	0
28.78	39.35	200	200	0	0	CCW	0
28.98	39.72	200	200	0	0	CCW	0
29.18	40.10	200	200	0	0	CCW	0
29.38	40.48	200	200	0	0	CCW	0
29.58	40.86	200	200	0	0	CCW	0
29.78	41.23	200	200	0	0	CCW	0
29.98	41.61	200	200	0	0	CCW	0
30.18	41.98	200	200	0	0	CCW	0
30.38	42.37	200	200	0	0	CCW	0
30.58	42.74	200	200	0	0	CCW	0
30.78	43.12	200	200	0	0	CCW	0
30.98	43.50	200	200	0	0	CCW	0
31.18	43.87	200	200	0	0	CCW	0
31.38	44.25	200	200	0	0	CCW	0
31.58	44.63	200	200	0	0	CCW	0
31.78	45.01	200	200	0	0	CCW	0
31.98	45.40	200	200	0	0	CCW	0
32.18	45.78	200	200	0	0	CCW	0
32.38	46.15	200	200	0	0	CCW	0
32.58	46.53	200	200	0	0	CCW	0
32.78	46.91	200	200	0	0	CCW	0
32.98	47.29	200	200	0	0	CCW	0
33.18	47.67	200	200	0	0	CCW	0

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

Opening port

Port open

Two Wheel Robot

Initializing MPU6050 ...

Testing device connections...

MPU6050 connection successful

Initializing DMP...

Enabling DMP...

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

MPU6050 Ready at 5.22 Sec

Sec Hdg Ldist Rdist ErrDeg PrevDeg TurnDir TurnDeg

Starting test turns...

FIFO count = 0

Turning right 30 deg...

In RollingTurn(CW, FWD,30.00)

Init hdg = 2.13 deg, Turn = 30.00 deg, tgt = 32.13 deg, timeout = 20000.00 sec

6269 2.14 0

6280 2.16 0

6289 2.18 0

6299 2.20 0

6308 2.22 0

6319 2.23 0

6329 2.25 0

6339 2.27 0

6348 2.29 0

6359 2.32 0

6369 2.34 0

6379 2.35 0

6389 2.37 0

6398 2.39 0

6409 2.41 0

6418 2.42 0

6428 2.44 0

6439 2.46 0

6449 2.48 0

6459 2.51 0

6468 2.52 0

6478 2.54 0

6489 2.56 0

6499 2.58 0

6509 2.60 0

6518 2.61 0

6529 2.63 0

6539 2.66 0

6549 2.68 0

6558 2.69 0

6568 2.71 0

6578 2.73 0

6589 2.75 0

6599 2.77 0

6608 2.79 0

6619 2.80 0

6628 2.83 0

6638 2.85 0

6648 2.87 0

6659 2.89 0

6668 2.91 0

6678 2.92 0

6688 2.94 0

6699 2.96 0

6709 2.98 0

6719 3.00 0

6728 3.02 0

6737 3.05 0

6748 3.07 0

6758 3.10 0

6768 3.13 0

6778 3.15 0

6788 3.19 0

6798 3.25 0

6808 3.31 0

6818 3.39 0

6829 3.50 0

6838 3.71 0

6848 4.02 0

6857 4.40 0

6867 4.75 0

6878 5.06 0

6888 5.38 0

6898 5.69 0

6907 6.02 0

6918 6.36 0

6928 6.70 0

6938 7.00 0

6948 7.28 0

6958 7.58 0

6968 7.89 0

6977 8.22 0

6987 8.53 0

6998 8.82 0

7008 9.13 0

7017 9.44 0

7027 9.72 0

7037 9.96 0

7048 10.20 0

7058 10.43 0

7067 10.65 0

7077 10.88 0

7088 11.12 0

7098 11.33 0

7107 11.54 0

7117 11.78 0

7128 12.04 0

7138 12.25 0

7147 12.47 0

7157 12.70 0

7168 12.92 0

7177 13.13 0

7187 13.35 0

7197 13.56 0

7207 13.76 0

7217 13.99 0

7227 14.28 0

7237 14.59 0

7247 14.93 0

7257 15.26 0

7267 15.57 0

7277 15.87 0

7287 16.15 0

7297 16.39 0

7308 16.62 0

7317 16.86 0

7327 17.10 0

7336 17.33 0

7347 17.59 0

7357 17.90 0

7366 18.23 0

7377 18.55 0

7387 18.86 0

7397 19.17 0

7407 19.48 0

7417 19.77 0

7427 20.00 0

7437 20.22 0

7447 20.45 0

7456 20.68 0

7467 20.91 0

7477 21.15 0

7487 21.40 0

7496 21.65 0

7506 21.90 0

7517 22.16 0

7527 22.41 0

7536 22.65 0

7546 22.85 0

7557 23.04 0

7566 23.22 0

7576 23.43 0

7586 23.68 0

7597 23.97 0

7606 24.23 0

7617 24.46 0

7626 24.73 0

7636 25.05 0

7647 25.36 0

7657 25.63 0

7666 25.87 0

7676 26.13 0

7687 26.44 0

7697 26.78 0

7706 27.08 0

7716 27.39 0

7727 27.71 0

7736 28.03 0

7746 28.32 0

7756 28.58 0

7767 28.84 0

7776 29.03 0

7786 29.24 0

7796 29.47 0

7805 29.73 0

7816 29.99 0

7826 30.24 0

7836 30.48 0

found match with yaw = 30.48, tgt = 32.13, match = 0.991, and slope = 0.090

Turning left 30 deg...

FIFO count = 1024

FIFO count = 0

In RollingTurn(CCW, FWD,30.00)

Init hdg = 41.58 deg, Turn = 30.00 deg, tgt = 11.58 deg, timeout = 20000.00 sec

8864 41.59 0

8873 41.59 0

8885 41.54 0

8894 41.55 0

8903 41.57 0

8913 41.58 0

8925 41.58 0

8934 41.44 0

8943 41.26 0

8953 41.06 0

8964 40.78 0

8974 40.49 0

8984 40.23 0

8993 39.98 0

9004 39.72 0

9014 39.43 0

9023 39.14 0

9033 38.84 0

9044 38.53 0

9054 38.22 0

9064 37.91 0

9073 37.62 0

9083 37.42 0

9094 37.32 0

9103 37.28 0

9113 37.30 0

9124 37.32 0

9134 37.33 0

9143 37.33 0

9153 37.21 0

9163 37.08 0

9174 36.96 0

9183 36.80 0

9194 36.65 0

9203 36.51 0

9213 36.38 0

9223 36.26 0

9234 36.13 0

9243 36.01 0

9253 35.85 0

9264 35.66 0

9274 35.46 0

9283 35.25 0

9293 35.04 0

9304 34.85 0

9314 34.68 0

9323 34.53 0

9333 34.42 0

9344 34.31 0

9354 34.20 0

9363 34.09 0

9373 33.97 0

9383 33.84 0

9393 33.69 0

9403 33.52 0

9413 33.35 0

9423 33.18 0

9433 33.03 0

9443 32.88 0

9453 32.73 0

9463 32.59 0

9473 32.44 0

9483 32.26 0

9493 32.07 0

9503 31.85 0

9512 31.63 0

9523 31.42 0

9533 31.25 0

9543 31.11 0

9552 30.99 0

9563 30.91 0

9573 30.84 0

9583 30.75 0

9592 30.66 0

9603 30.55 0

9613 30.44 0

9623 30.32 0

9632 30.22 0

9643 30.10 0

9653 29.98 0

9663 29.86 0

9672 29.70 0

9682 29.54 0

9693 29.37 0

9703 29.18 0

9712 29.01 0

9722 28.82 0

9733 28.64 0

9742 28.46 0

9752 28.30 0

9762 28.13 0

9773 27.95 0

9782 27.77 0

9792 27.59 0

9802 27.39 0

9811 27.19 0

9822 26.99 0

9832 26.80 0

9842 26.62 0

9851 26.45 0

9863 26.26 0

9872 26.06 0

9882 25.85 0

9892 25.62 0

9903 25.38 0

9912 25.13 0

9922 24.87 0

9932 24.62 0

9942 24.36 0

9952 24.09 0

9962 23.85 0

9972 23.62 0

9981 23.39 0

9992 23.12 0

10002 22.84 0

10011 22.56 0

10021 22.29 0

10032 22.03 0

10042 21.76 0

10052 21.54 0

10061 21.35 0

10072 21.17 0

10082 20.97 0

10091 20.78 0

10102 20.61 0

10112 20.45 0

10122 20.28 0

10132 20.13 0

10141 19.95 0

10151 19.81 0

10162 19.68 0

10171 19.56 0

10182 19.40 0

10191 19.24 0

10202 19.06 0

10212 18.90 0

10221 18.73 0

10231 18.56 0

10242 18.39 0

10251 18.22 0

10262 18.01 0

10271 17.82 0

10281 17.65 0

10292 17.47 0

10301 17.29 0

10311 17.14 0

10321 17.02 0

10331 16.90 0

10342 16.77 0

10351 16.64 0

10361 16.48 0

10372 16.33 0

10381 16.18 0

10391 16.01 0

10401 15.84 0

10412 15.64 0

10421 15.42 0

10431 15.22 0

10441 15.01 0

10451 14.81 0

10461 14.64 0

10471 14.49 0

10481 14.36 0

10491 14.24 0

10501 14.11 0

10511 13.97 0

10521 13.81 0

10530 13.64 0

10541 13.50 0

10551 13.36 0

found match with yaw = 13.36, tgt = 11.58, match = 0.990, and slope = 0.090

10.78 10.13 200 200 0 0 CCW 0

10.98 7.50 200 200 0 0 CCW 0

11.18 7.28 200 200 0 0 CCW 0

11.38 7.67 200 200 0 0 CCW 0

11.58 7.23 200 200 0 0 CCW 0

11.78 4.82 200 200 0 0 CCW 0

11.98 5.16 200 200 0 0 CCW 0

12.18 5.56 200 200 0 0 CCW 0

12.38 5.97 200 200 0 0 CCW 0

12.58 6.73 200 200 0 0 CCW 0

12.78 7.44 200 200 0 0 CCW 0

12.98 7.96 200 200 0 0 CCW 0

13.18 10.70 200 200 0 0 CCW 0

13.38 12.16 200 200 0 0 CCW 0

13.58 12.86 200 200 0 0 CCW 0

13.78 13.98 200 200 0 0 CCW 0

13.98 14.56 200 200 0 0 CCW 0

14.18 15.57 200 200 0 0 CCW 0

14.38 16.86 200 200 0 0 CCW 0

14.58 17.39 200 200 0 0 CCW 0

14.78 17.37 200 200 0 0 CCW 0

14.98 17.60 200 200 0 0 CCW 0

15.18 17.96 200 200 0 0 CCW 0

15.38 18.34 200 200 0 0 CCW 0

15.58 18.72 200 200 0 0 CCW 0

15.78 19.10 200 200 0 0 CCW 0

15.98 19.50 200 200 0 0 CCW 0

16.18 19.88 200 200 0 0 CCW 0

16.38 20.21 200 200 0 0 CCW 0

16.58 20.99 200 200 0 0 CCW 0

16.78 22.92 200 200 0 0 CCW 0

16.98 25.77 200 200 0 0 CCW 0

17.18 26.87 200 200 0 0 CCW 0

17.38 28.11 200 200 0 0 CCW 0

17.58 29.87 200 200 0 0 CCW 0

17.78 30.60 200 200 0 0 CCW 0

17.98 30.97 200 200 0 0 CCW 0

18.18 31.40 200 200 0 0 CCW 0

18.38 31.87 200 200 0 0 CCW 0

18.58 32.27 200 200 0 0 CCW 0

18.78 33.46 200 200 0 0 CCW 0

18.98 36.06 200 200 0 0 CCW 0

19.18 39.47 200 200 0 0 CCW 0

19.38 46.26 200 200 0 0 CCW 0

19.58 53.56 200 200 0 0 CCW 0

19.78 57.95 200 200 0 0 CCW 0

19.98 62.18 200 200 0 0 CCW 0

20.18 68.34 200 200 0 0 CCW 0

20.38 74.71 200 200 0 0 CCW 0

20.58 80.05 200 200 0 0 CCW 0

20.78 84.66 200 200 0 0 CCW 0

20.98 91.72 200 200 0 0 CCW 0

21.18 100.94 200 200 0 0 CCW 0

21.38 109.92 200 200 0 0 CCW 0

21.58 116.38 200 200 0 0 CCW 0

21.78 118.67 200 200 0 0 CCW 0

21.98 119.04 200 200 0 0 CCW 0

22.18 119.33 200 200 0 0 CCW 0

22.38 117.98 200 200 0 0 CCW 0

22.58 109.04 200 200 0 0 CCW 0

22.78 99.40 200 200 0 0 CCW 0

22.98 92.47 200 200 0 0 CCW 0

23.18 85.81 200 200 0 0 CCW 0

23.38 81.29 200 200 0 0 CCW 0

23.58 74.51 200 200 0 0 CCW 0

23.78 67.58 200 200 0 0 CCW 0

23.98 59.32 200 200 0 0 CCW 0

24.18 53.99 200 200 0 0 CCW 0

24.38 50.39 200 200 0 0 CCW 0

24.58 46.73 200 200 0 0 CCW 0

24.78 42.14 200 200 0 0 CCW 0

24.98 39.26 200 200 0 0 CCW 0

25.18 39.60 200 200 0 0 CCW 0

25.38 39.97 200 200 0 0 CCW 0

25.58 40.36 200 200 0 0 CCW 0

25.78 40.73 200 200 0 0 CCW 0

25.98 41.11 200 200 0 0 CCW 0

26.18 40.66 200 200 0 0 CCW 0

26.38 39.16 200 200 0 0 CCW 0

26.58 36.13 200 200 0 0 CCW 0

26.78 35.51 200 200 0 0 CCW 0

26.98 35.92 200 200 0 0 CCW 0

27.18 36.30 200 200 0 0 CCW 0

27.38 36.67 200 200 0 0 CCW 0

27.58 37.06 200 200 0 0 CCW 0

27.78 37.46 200 200 0 0 CCW 0

27.98 37.85 200 200 0 0 CCW 0

28.18 38.22 200 200 0 0 CCW 0

28.38 38.60 200 200 0 0 CCW 0

28.58 38.97 200 200 0 0 CCW 0

28.78 39.35 200 200 0 0 CCW 0

28.98 39.72 200 200 0 0 CCW 0

29.18 40.10 200 200 0 0 CCW 0

29.38 40.48 200 200 0 0 CCW 0

29.58 40.86 200 200 0 0 CCW 0

29.78 41.23 200 200 0 0 CCW 0

29.98 41.61 200 200 0 0 CCW 0

30.18 41.98 200 200 0 0 CCW 0

30.38 42.37 200 200 0 0 CCW 0

30.58 42.74 200 200 0 0 CCW 0

30.78 43.12 200 200 0 0 CCW 0

30.98 43.50 200 200 0 0 CCW 0

31.18 43.87 200 200 0 0 CCW 0

31.38 44.25 200 200 0 0 CCW 0

31.58 44.63 200 200 0 0 CCW 0

31.78 45.01 200 200 0 0 CCW 0

31.98 45.40 200 200 0 0 CCW 0

32.18 45.78 200 200 0 0 CCW 0

32.38 46.15 200 200 0 0 CCW 0

32.58 46.53 200 200 0 0 CCW 0

32.78 46.91 200 200 0 0 CCW 0

32.98 47.29 200 200 0 0 CCW 0

33.18 47.67 200 200 0 0 CCW 0

Shown below are the yaw values plotted against time in Excel

The next step will be to port the updated software back into my two-wheel robot to confirm that heading-based turns can be accomplished automatically (this is something that I had going before, but…).

Stay tuned!

Frank

Polling vs Interrupt with MPU6050 (GY-521) and Arduino

2 Replies

Posted 04 October 2019,

In my last post I described my Arduino Mega test program to interface with the popular Invensense MPU6050 IMU and it’s GY-521 clone. In this post I describe the interface configuration for using a polling strategy rather than relying on the IMU6050’s interrupt signal. A polling strategy can be very useful as it is much simpler, and saves a pin connection from the MPU6050 to the controller; all that is required is +V, GND, SDA & SCL, as shown below:

With this wiring setup, the control program is shown below:

/*
    Name:       Arduino MPU6050 Polling Test.ino
    Created:	10/5/2019 1:31:08 PM
    Author:     FRANKWIN10\Frank

	This is a complete, working MPU6050 (GY-521) example using polling vs interrupts.
	It is based on Jeff Rowberg's MPU6050_DMP6_using_DMP_V6.12 example.  After confirming
	that the example worked properly using interrupts, I modified it to remove the need
	for the interrupt line.  
*/

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/04/19
#include "elapsedMillis.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
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

//extra stuff
int IMU_CHECK_INTERVAL_MSEC = 100;
elapsedMillis sinceLastIMUCheck;
float global_yawval = 0.0; //contains most recent yaw value from IMU
int global_fifo_count = 0; //made global so can monitor from outside GetIMUHeadingDeg() fcn

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup()
{
	// I2C bus init done in SBWIRE.h

	Serial.begin(115200);
	while (!Serial); // wait for Leonardo enumeration, others continue immediately

	// initialize device
	Serial.println(F("Initializing MPU6050..."));
	mpu.initialize();

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

	// load and configure the DMP
	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// supply your own gyro offsets here, scaled for min sensitivity
	mpu.setXGyroOffset(220);
	mpu.setYGyroOffset(76);
	mpu.setZGyroOffset(-85);
	mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

	// make sure it worked (returns 0 if so)
	if (devStatus == 0)
	{
		// Calibration Time: generate offsets and calibrate our MPU6050
		mpu.CalibrateAccel(6);
		mpu.CalibrateGyro(6);
		mpu.PrintActiveOffsets();

		// 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 first interrupt..."));
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
	}
	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(")"));
	}

	// configure LED for output
	pinMode(LED_PIN, OUTPUT);
	sinceLastIMUCheck = 0; //this manages 'other program stuff' cycle

	//print out column headers
	Serial.println("MSec\tYawDeg");
}

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
void loop()
{
	// if programming failed, don't try to do anything
	if (!dmpReady) return;

	if (mpu.dmpPacketAvailable())
	{
		global_yawval = GetIMUHeadingDeg(); //retreive the most current yaw value from IMU

		// blink LED to indicate activity
		blinkState = !blinkState;
		digitalWrite(LED_PIN, blinkState);
	}

	//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec
	//for this test program, there's nothing here except diagnostics printouts
	if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC)
	{
		sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;
		Serial.print(millis());
		Serial.print("\t");
		Serial.println(global_yawval);

		if (global_fifo_count != 0)
		{
			Serial.print("FIFO Reset!");
			mpu.resetFIFO();
		}
	}
}

float GetIMUHeadingDeg()
{
	// At least one data packet is available

	mpuIntStatus = mpu.getIntStatus();
	fifoCount = mpu.getFIFOCount();// get current FIFO count

	// check for overflow (this should never happen unless our code is too inefficient)
	if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)
	{
		// reset so we can continue cleanly
		mpu.resetFIFO();
		Serial.println(F("FIFO overflow!"));

		// otherwise, check for DMP data ready interrupt (this should happen frequently)
	}
	else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT))
	{
		// read all available packets from FIFO
		while (fifoCount >= packetSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!
		{
			mpu.getFIFOBytes(fifoBuffer, packetSize);
			// track FIFO count here in case there is > 1 packet available
			// (this lets us immediately read more without waiting for an interrupt)
			fifoCount -= packetSize;
		}
		global_fifo_count = mpu.getFIFOCount(); //should be zero here

		// display Euler angles in degrees
		mpu.dmpGetQuaternion(&q, fifoBuffer);
		mpu.dmpGetGravity(&gravity, &q);
		mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
	}

	float yawval = ypr[0] * 180 / M_PI;
	return  yawval;
}

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

Name: Arduino MPU6050 Polling Test.ino

Created: 10/5/2019 1:31:08 PM

Author: FRANKWIN10\Frank

This is a complete, working MPU6050 (GY-521) example using polling vs interrupts.

It is based on Jeff Rowberg's MPU6050_DMP6_using_DMP_V6.12 example. After confirming

that the example worked properly using interrupts, I modified it to remove the need

for the interrupt line.

#include "I2Cdev.h"

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

#include "elapsedMillis.h"

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here

// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)

// AD0 high = 0x69

MPU6050 mpu;

//MPU6050 mpu(0x69); // <-- use for AD0 high

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

bool blinkState = false;

// MPU control/status vars

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

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU

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

//extra stuff

int IMU_CHECK_INTERVAL_MSEC = 100;

elapsedMillis sinceLastIMUCheck;

float global_yawval = 0.0; //contains most recent yaw value from IMU

int global_fifo_count = 0; //made global so can monitor from outside GetIMUHeadingDeg() fcn

// ================================================================

// === INITIAL SETUP ===

// ================================================================

void setup()

{

// I2C bus init done in SBWIRE.h

Serial.begin(115200);

while (!Serial); // wait for Leonardo enumeration, others continue immediately

// initialize device

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

mpu.initialize();

// verify connection

Serial.println(F("Testing device connections..."));

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

// load and configure the DMP

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

devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity

mpu.setXGyroOffset(220);

mpu.setYGyroOffset(76);

mpu.setZGyroOffset(-85);

mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

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

if (devStatus == 0)

{

// Calibration Time: generate offsets and calibrate our MPU6050

mpu.CalibrateAccel(6);

mpu.CalibrateGyro(6);

mpu.PrintActiveOffsets();

// 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 first interrupt..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

}

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

}

// configure LED for output

pinMode(LED_PIN, OUTPUT);

sinceLastIMUCheck = 0; //this manages 'other program stuff' cycle

//print out column headers

Serial.println("MSec\tYawDeg");

}

// ================================================================

// === MAIN PROGRAM LOOP ===

// ================================================================

void loop()

{

// if programming failed, don't try to do anything

if (!dmpReady) return;

if (mpu.dmpPacketAvailable())

{

global_yawval = GetIMUHeadingDeg(); //retreive the most current yaw value from IMU

// blink LED to indicate activity

blinkState = !blinkState;

digitalWrite(LED_PIN, blinkState);

}

//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec

//for this test program, there's nothing here except diagnostics printouts

if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC)

{

sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;

Serial.print(millis());

Serial.print("\t");

Serial.println(global_yawval);

if (global_fifo_count != 0)

{

Serial.print("FIFO Reset!");

mpu.resetFIFO();

}

float GetIMUHeadingDeg()

{

// At least one data packet is available

mpuIntStatus = mpu.getIntStatus();

fifoCount = mpu.getFIFOCount();// get current FIFO count

// check for overflow (this should never happen unless our code is too inefficient)

if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)

{

// reset so we can continue cleanly

mpu.resetFIFO();

Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)

}

else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT))

{

// read all available packets from FIFO

while (fifoCount >= packetSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!

{

mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available

// (this lets us immediately read more without waiting for an interrupt)

fifoCount -= packetSize;

}

global_fifo_count = mpu.getFIFOCount(); //should be zero here

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

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

}

float yawval = ypr[0] * 180 / M_PI;

return yawval;

}

In the above program, the interrupt service routine (ISR) and the accompanying ‘attachInterrupt()’ setup function have been removed as they are no longer needed for the polling arrangement. Instead, the program calls ‘mpu.dmpPacketAvailable()’ every time through the loop, and if a packet is available, GetIMUHeadingDeg() is called to read the packet and return a yaw value. The rest of the code in the loop() function is the place holder for the ‘other stuff’ that the program does when it isn’t paying attention to the IMU.

	//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec
	//for this test program, there's nothing here except diagnostics printouts
	if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC)
	{
		sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;
		Serial.print(millis());
		Serial.print("\t");
		Serial.println(global_yawval);

		if (global_fifo_count != 0)
		{
			Serial.print("FIFO Reset!");
			mpu.resetFIFO();
		}
	}

//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec

//for this test program, there's nothing here except diagnostics printouts

if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC)

{

sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;

Serial.print(millis());

Serial.print("\t");

Serial.println(global_yawval);

if (global_fifo_count != 0)

{

Serial.print("FIFO Reset!");

mpu.resetFIFO();

}

In this test program, I have set this section up to execute every 100 Msec, but in my robot programs I usually set it up for a 200 Msec interval; 5 cycles/sec is plenty fast enough for a wheeled robot that uses only the IMU yaw information for turn management.

So far, this arrangement seems very stable; I have been running it now for several hours without a hiccup.

Stay tuned,

Frank

Basic Arduino/MPU6050 (GY-521) test

2 Replies

Posted 29 September 2019,

In my quest to figure out WTF happened to my ability to acquire real-time relative heading information on both my 2-wheel and 4-wheel robots, I have been trying to start from scratch with very simple controller/IMU hardware configurations. After succeeding with a basic functionality demonstration using a Teensy 3.2 and a Sparkfun MPU6250 IMU breakout board, I decided the next step would be to do the same thing with an Arduino Mega controller and a GY-521 )MPU6050 clone) to more closely replicate the hardware configuration on my 2-wheel and 4-wheel robots.

As usual I started this project with a web search for basic MPU6050/Arduino examples, and I found this YouTube video showing just what I was after. After going through the video several times to make sure I understood what was going on, I decided to try and duplicate it so I could compare my (hopefully) working demo code with my (currently non-working) robot code.

In my past efforts with the MPU6050, I had struggled with the complexities of using Jeff Rowberg’s wonderful (but quite massive and convoluted) I2CDevLib GitHub repository. There was always something that didn’t quite fit the situation, and making it fit invariably required a trip down the rabbit hole into Alice’s wonderland. Getting the right combination of files in the right places seemed to be more a matter of luck than skill. However, this particular video does a nice job of explicitly demonstrating what has to go where. Essentially the magic steps are:

Download Jeff Rowberg’s IC2DevLib repository from GitHub into a ZIP file.
UnZip the repository files into a temporary folder
Copy the Arduino/I2CDev and Arduino/MPU6050 folders into the Arduino/Libraries folder. This makes them available to the Arduino IDE (and the VS2017/Visual Micro setup I use).
Open a new sketch in the Arduino IDE (or a new project in the VS/VisMicro environment) and then:
- In the Arduino IDE select ‘File-Examples, and scroll down to the ‘Examples from Custom Libraries’ section. Then select ‘MPU6050->MPU6050_DMP6’ This will load the example code into the sketch.
- In the VS/VM environment, select the Visual Micro Explorer (under the vMicro tab). Then click on the Examples tab, expand the ‘MPU6050’ section and then select the MPU6050_DMP6 example. This will load the code into the edit window.

Assuming you have the wiring setup correct, the example should run ‘out of the box’ with no required modifications. However, after verifying that everything was working, I made the following changes:

The unmodified MPU6050_6Axis_MotionApps20.h file configures the MPU6050 DMP to send data packets to the controller at a fairly high rate – like 100Hz. This is way too high for my robot application, so I changed the configuration to send packets at a 10Hz rate, by changing the MPU6050_DMP_FIFO_RATE_DIVISOR constant from 0x01 to 0x09 (lines 271-274) as shown below

#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR 
//#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x09 //09/29/19 reduce FIFO rate to 20Hz vs 100Hz
#endif

#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR

//#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default

#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x09 //09/29/19 reduce FIFO rate to 20Hz vs 100Hz

#endif

The Arduino I2C library (Wire.h) has a well-known and documented flaw that causes the I2C bus to hang up on an intermittent basis, so I modified I2CDev.h lines 50-57 to use the SBWIRE library that contains timeouts to prevent this problem from happening

// -----------------------------------------------------------------------------
// I2C interface implementation setting
// -----------------------------------------------------------------------------
#ifndef I2CDEV_IMPLEMENTATION
//#define I2CDEV_IMPLEMENTATION       I2CDEV_ARDUINO_WIRE
#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_SBWIRE //09/29/19 gfp enabled this to avoid I2C hangups
//#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_FASTWIRE
#endif // I2CDEV_IMPLEMENTATION

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

// I2C interface implementation setting

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

#ifndef I2CDEV_IMPLEMENTATION

//#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE

#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE //09/29/19 gfp enabled this to avoid I2C hangups

//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE

#endif // I2CDEV_IMPLEMENTATION

And the last change I made was to disable the interrupt service routine (ISR) and use a polling technique. Instead of waiting for an interrupt, I simply poll the DMP register with

‘mpuIntStatus = mpu.getIntStatus();’

every time through the loop. If the return value indicates that a data packet is ready, it is read; otherwise it does nothing. This appears to be entirely equivalent to the interrupt technique as long as the loop is fast enough service the DMP’s FIFO.

30 September Update:

Well, something’s not equivalent, as the yaw values are fine for a few minutes, but then start showing up as ‘179.000’. From my previous work I know this means that the line

mpu.getFIFOBytes(fifoBuffer, packetSize);

is getting out of sync with the DMP and isn’t reading a complete packet. When I then changed the code back to the original interrupt-driven model, the yaw values stay valid forever.

03 October Update:

I modified the code to break the ‘put other programming stuff here’ block out of the ‘if()’ within a ‘while()’ within a ‘loop()’ structure for two reasons:

It gave me a headache every time I tried to figure out how it worked
I wanted to do ‘the programming stuff’ only once every K Msec where K was something like 100 or 200. With the above nested structure, that would never work.

After removing extraneous comments and unused code, the resulting program is shown below:

/*
    Name:       Arduino_GY521_Test.ino
    Created:	9/28/2019 6:38:12 PM
    Author:     FRANKWIN10\Frank
*/

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "elapsedMillis.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.
 * ========================================================================= */
#define OUTPUT_READABLE_YAWPITCHROLL

#define INTERRUPT_PIN 2  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
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

//extra stuff
const int GET_INT_STATUS_PIN = 53;
const int READ_BYTES_SECTION_PIN = 51;
int IMU_CHECK_INTERVAL_MSEC = 100;
elapsedMillis sinceLastIMUCheck;
float global_yawval = 0.0; //contains most recent yaw value from IMU
int global_fifo_count = 0;



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() 
{
	// I2C bus init done in SBWIRE.h

	Serial.begin(115200);
	while (!Serial); // wait for Leonardo enumeration, others continue immediately

	// initialize device
	Serial.println(F("Initializing MPU6050..."));
	mpu.initialize();
	pinMode(INTERRUPT_PIN, INPUT);

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

	// load and configure the DMP
	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// supply your own gyro offsets here, scaled for min sensitivity
	mpu.setXGyroOffset(220);
	mpu.setYGyroOffset(76);
	mpu.setZGyroOffset(-85);
	mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

	// make sure it worked (returns 0 if so)
	if (devStatus == 0) 
	{
		// Calibration Time: generate offsets and calibrate our MPU6050
		mpu.CalibrateAccel(6);
		mpu.CalibrateGyro(6);
		mpu.PrintActiveOffsets();

		// turn on the DMP, now that it's ready
		Serial.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

		// enable Arduino interrupt detection
		Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
		Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
		Serial.println(F(")..."));
		attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
		mpuIntStatus = mpu.getIntStatus();

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

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
	}
	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(")"));
	}

	// configure LED for output
	pinMode(LED_PIN, OUTPUT);
	sinceLastIMUCheck = 0; //this manages 'other program stuff' cycle

	//diagnostic pins - not needed for normal operation
	pinMode(GET_INT_STATUS_PIN, OUTPUT); 
	pinMode(READ_BYTES_SECTION_PIN, OUTPUT);

	//print out column headers
	Serial.println("MSec\tYawDeg\tGlFifoCnt\tFifoCnt");
}


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
	mpuInterrupt = true;
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
void loop()
{
	// if programming failed, don't try to do anything
	if (!dmpReady) return;

	//if (mpuInterrupt && fifoCount < packetSize)
	if (mpuInterrupt)
	{
		mpuInterrupt = false;//reset interrupt flag
		global_yawval = GetIMUHeadingDeg(); //retreive the most current yaw value from IMU

		// blink LED to indicate activity
		blinkState = !blinkState;
		digitalWrite(LED_PIN, blinkState);
	}

	//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec
	//for this test program, there's nothing here except diagnostics printouts
	if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC)
	{
		sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;
		Serial.print(millis());
		Serial.print("\t");
		Serial.print(global_yawval);
		Serial.print("\t");
		Serial.print(global_fifo_count);
		Serial.print("\t");
		Serial.println(fifoCount);

		if (global_fifo_count != 0)
		{
			Serial.print("Program halted!");
			while (1)
			{

			}
		}
	}
}

float GetIMUHeadingDeg()
{
	// An interrupt has occurred.  get INT_STATUS byte
	//pulse an output pin to measure getIntStatus() duration
	digitalWrite(GET_INT_STATUS_PIN, HIGH);
	mpuIntStatus = mpu.getIntStatus();
	digitalWrite(GET_INT_STATUS_PIN, LOW);

	// get current FIFO count
	fifoCount = mpu.getFIFOCount();

	// check for overflow (this should never happen unless our code is too inefficient)
	if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) 
	{
		// reset so we can continue cleanly
		mpu.resetFIFO();
		//  fifoCount = mpu.getFIFOCount();  // will be zero after reset no need to ask
		Serial.println(F("FIFO overflow!"));

		// otherwise, check for DMP data ready interrupt (this should happen frequently)
	}
	else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT))
	{
		// read all available packets from FIFO
		while (fifoCount >= packetSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!
		{
			mpu.getFIFOBytes(fifoBuffer, packetSize);
			// track FIFO count here in case there is > 1 packet available
			// (this lets us immediately read more without waiting for an interrupt)
			fifoCount -= packetSize;
			//digitalWrite(READ_BYTES_SECTION_PIN, !digitalRead(READ_BYTES_SECTION_PIN)); //toggle the pin

			//pulse the pin
			digitalWrite(READ_BYTES_SECTION_PIN, HIGH);
			delay(1);
			digitalWrite(READ_BYTES_SECTION_PIN, LOW);
		}
		global_fifo_count = mpu.getFIFOCount(); //should be zero here

#ifdef OUTPUT_READABLE_YAWPITCHROLL
		// display Euler angles in degrees
		mpu.dmpGetQuaternion(&q, fifoBuffer);
		mpu.dmpGetGravity(&gravity, &q);
		mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
#endif
	}
	float yawval = ypr[0] * 180 / M_PI;
	return  yawval;
}

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

Name: Arduino_GY521_Test.ino

Created: 9/28/2019 6:38:12 PM

Author: FRANKWIN10\Frank

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#include "elapsedMillis.h"

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here

// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)

// AD0 high = 0x69

MPU6050 mpu;

//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================

NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch

depends on the MPU-6050's INT pin being connected to the Arduino's

external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is

digital I/O pin 2.

* ========================================================================= */

#define OUTPUT_READABLE_YAWPITCHROLL

#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

bool blinkState = false;

// MPU control/status vars

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

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU

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

//extra stuff

const int GET_INT_STATUS_PIN = 53;

const int READ_BYTES_SECTION_PIN = 51;

int IMU_CHECK_INTERVAL_MSEC = 100;

elapsedMillis sinceLastIMUCheck;

float global_yawval = 0.0; //contains most recent yaw value from IMU

int global_fifo_count = 0;

// ================================================================

// === INITIAL SETUP ===

// ================================================================

void setup()

{

// I2C bus init done in SBWIRE.h

Serial.begin(115200);

while (!Serial); // wait for Leonardo enumeration, others continue immediately

// initialize device

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

mpu.initialize();

pinMode(INTERRUPT_PIN, INPUT);

// verify connection

Serial.println(F("Testing device connections..."));

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

// load and configure the DMP

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

devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity

mpu.setXGyroOffset(220);

mpu.setYGyroOffset(76);

mpu.setZGyroOffset(-85);

mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

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

if (devStatus == 0)

{

// Calibration Time: generate offsets and calibrate our MPU6050

mpu.CalibrateAccel(6);

mpu.CalibrateGyro(6);

mpu.PrintActiveOffsets();

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

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

mpu.setDMPEnabled(true);

// enable Arduino interrupt detection

Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));

Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));

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

attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);

mpuIntStatus = mpu.getIntStatus();

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

Serial.println(F("DMP ready! Waiting for first interrupt..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

}

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

}

// configure LED for output

pinMode(LED_PIN, OUTPUT);

sinceLastIMUCheck = 0; //this manages 'other program stuff' cycle

//diagnostic pins - not needed for normal operation

pinMode(GET_INT_STATUS_PIN, OUTPUT);

pinMode(READ_BYTES_SECTION_PIN, OUTPUT);

//print out column headers

Serial.println("MSec\tYawDeg\tGlFifoCnt\tFifoCnt");

}

// ================================================================

// === INTERRUPT DETECTION ROUTINE ===

// ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high

void dmpDataReady()

{

mpuInterrupt = true;

}

// ================================================================

// === MAIN PROGRAM LOOP ===

// ================================================================

void loop()

{

// if programming failed, don't try to do anything

if (!dmpReady) return;

//if (mpuInterrupt && fifoCount < packetSize)

if (mpuInterrupt)

{

mpuInterrupt = false;//reset interrupt flag

global_yawval = GetIMUHeadingDeg(); //retreive the most current yaw value from IMU

// blink LED to indicate activity

blinkState = !blinkState;

digitalWrite(LED_PIN, blinkState);

}

//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec

//for this test program, there's nothing here except diagnostics printouts

if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC)

{

sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;

Serial.print(millis());

Serial.print("\t");

Serial.print(global_yawval);

Serial.print("\t");

Serial.print(global_fifo_count);

Serial.print("\t");

Serial.println(fifoCount);

if (global_fifo_count != 0)

{

Serial.print("Program halted!");

while (1)

{

}

float GetIMUHeadingDeg()

{

// An interrupt has occurred. get INT_STATUS byte

//pulse an output pin to measure getIntStatus() duration

digitalWrite(GET_INT_STATUS_PIN, HIGH);

mpuIntStatus = mpu.getIntStatus();

digitalWrite(GET_INT_STATUS_PIN, LOW);

// get current FIFO count

fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)

if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)

{

// reset so we can continue cleanly

mpu.resetFIFO();

// fifoCount = mpu.getFIFOCount(); // will be zero after reset no need to ask

Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)

}

else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT))

{

// read all available packets from FIFO

while (fifoCount >= packetSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!

{

mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available

// (this lets us immediately read more without waiting for an interrupt)

fifoCount -= packetSize;

//digitalWrite(READ_BYTES_SECTION_PIN, !digitalRead(READ_BYTES_SECTION_PIN)); //toggle the pin

//pulse the pin

digitalWrite(READ_BYTES_SECTION_PIN, HIGH);

delay(1);

digitalWrite(READ_BYTES_SECTION_PIN, LOW);

}

global_fifo_count = mpu.getFIFOCount(); //should be zero here

#ifdef OUTPUT_READABLE_YAWPITCHROLL

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

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

#endif

}

float yawval = ypr[0] * 180 / M_PI;

return yawval;

}

Notes about the above program:

I used the SBWIRE library vs the normal Arduino WIRE library to avoid the well-known and well documented infinite blocking problems in the WIRE code. This was accomplished by editing the I2C interface implementation section in I2Cdev.h as follows

// -----------------------------------------------------------------------------
// I2C interface implementation setting
// -----------------------------------------------------------------------------
#ifndef I2CDEV_IMPLEMENTATION
//#define I2CDEV_IMPLEMENTATION       I2CDEV_ARDUINO_WIRE
#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_SBWIRE //09/29/19 gfp enabled this to avoid I2C hangups
//#define I2CDEV_IMPLEMENTATION       I2CDEV_BUILTIN_FASTWIRE
#endif // I2CDEV_IMPLEMENTATION

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

// I2C interface implementation setting

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

#ifndef I2CDEV_IMPLEMENTATION

//#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE

#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE //09/29/19 gfp enabled this to avoid I2C hangups

//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE

#endif // I2CDEV_IMPLEMENTATION

I lowered the MPU6050 interrupt rate to 20Hz (I don’t need anything faster for my wall-following robot by modifying MPU6050_6AxisMotionApps20.h as follows:

ifndef MPU6050_DMP_FIFO_RATE_DIVISOR 
//#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x09 //09/29/19 reduce FIFO rate to 20Hz vs 100Hz
#endif

ifndef MPU6050_DMP_FIFO_RATE_DIVISOR

//#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default

#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x09 //09/29/19 reduce FIFO rate to 20Hz vs 100Hz

#endif

The loop() function has just three blocks

if (!dmpReady) return; this bypasses everything else if the MPU6050 didn’t init correctly

	if (mpuInterrupt)
	{
		mpuInterrupt = false;//reset interrupt flag
		global_yawval = GetIMUHeadingDeg(); //retreive the most current yaw value from IMU

		// blink LED to indicate activity
		blinkState = !blinkState;
		digitalWrite(LED_PIN, blinkState);
	}

if (mpuInterrupt)

{

mpuInterrupt = false;//reset interrupt flag

global_yawval = GetIMUHeadingDeg(); //retreive the most current yaw value from IMU

// blink LED to indicate activity

blinkState = !blinkState;

digitalWrite(LED_PIN, blinkState);

}

All this section does is call GetIMUHeadingDeg() whenever an interrupt has been processed in the ISR

	//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec
	//for this test program, there's nothing here except diagnostics printouts
	if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC)
	{
		sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;
		Serial.print(millis());
		Serial.print("\t");
		Serial.print(global_yawval);
		Serial.print("\t");
		Serial.print(global_fifo_count);
		Serial.print("\t");
		Serial.println(fifoCount);

		if (global_fifo_count != 0)
		{
			Serial.print("Program halted!");
			while (1)
			{

			}
		}
	}

//other program stuff block - executes every IMU_CHECK_INTERVAL_MSEC Msec

//for this test program, there's nothing here except diagnostics printouts

if (sinceLastIMUCheck >= IMU_CHECK_INTERVAL_MSEC)

{

sinceLastIMUCheck -= IMU_CHECK_INTERVAL_MSEC;

Serial.print(millis());

Serial.print("\t");

Serial.print(global_yawval);

Serial.print("\t");

Serial.print(global_fifo_count);

Serial.print("\t");

Serial.println(fifoCount);

if (global_fifo_count != 0)

{

Serial.print("Program halted!");

while (1)

{

}

This section is the ‘everything else’ block. In my robot programs, this section runs the robot, using the yaw value output from the MPU6050 as appropriate.

I discovered that the local variable ‘fifoCount’ can become desynchronized from the actual FIFO count resulting in a situation where the line:

if (mpuInterrupt && fifoCount < packetSize)

in the loop() function fails with fifoCount == packetSize. The fix for this was to remove the fifoCount comparison from the if() statement, making it just ‘if (mpuInterrupt)’. This means the if() block will execute every time the interrupt occurs, whether or not there is data in the FIFO.

With the above modifications, the program has run for many hours with no problems, so I’m convinced I have most, if not all, the problems licked. I’m still using the interrupt-driven version rather than the polling version I would prefer, but that’s a small price to pay for the demonstrated stability of the interrupt-driven version.

Future Work:

Next I plan to try the new MotionDriver 6.12 version of the MPU6050 DMP firmware, which is reputed to be faster, better, and more stable than the present 2.0 version.

04 October Update.

As it happens, the only thing that was required to change from MotionApps V2 to MotionApps V6.12 was to change #include “MPU6050_6Axis_MotionApps20.h” to #include “MPU6050_6Axis_MotionApps_V6_12.h” in little test program. This compiled and ran fine, and the only difference I could see is that V6.12 has a fixed interrupt rate of about 200Hz, whereas V2.0 could be adjusted down to about 20Hz. According to some Invensense documentation, the newer version has better/faster calibration capabilities and (maybe?) lower drift rates??

Stay Tuned

Frank

Sparkfun MPU9250 Test with Teensy 3.2

Leave a reply

Posted 15 September 2019

After successfully demonstrating heading-based wall following with my little two motor robot, I attempted to integrate this capability back into my newly re-engined (re-motored??) 4-wheel Wall-E2 robot. Naturally the attempt was a dismal failure, for reasons I have yet to determine. For some reason, the MPU 6050 IMU on the 4-wheel robot refused to produce valid heading data, and when I then attempted to redo the previously successful experiment on my 2-wheel robot, it too failed – in the same manner! Clearly the two robots got together and decided to misbehave just to watch me tear what’s left of my hair out!

So, after trying in vain to figure out WTF with respect to the two robots and their respective IMU’s, I decided to just start all over with a different controller and a different IMU and see if I could just make something positive happen. I found a Sparkfun MPU 9250 IMU breakout board in my parts bin, left over from an older post. Because the Sparkfun board is set up for 3.3V only, I decided to use a Teensy controller instead of an Arduino Mega and see if I could just get something to work.

After the usual number of screwups and frustrations, I was finally able to get the Sparkfun MPU 9250 breakout board and the Teensy 3.2 talking to each other and to capture some valid heading (yaw) data from the MPU 9250. The reason for this post is to document the setup and the code so when I have this same problem a year from now, I can come back here and have a working, documented baseline to start with.

The Hardware:

I used a Teensy 3.2 and a Sparkfun MPU 9250 IMU breakout board, both mounted on a small ASP solderless breadboard, as shown in the following photo, along with a Fritzing view of the layout

The Software:

I wrote a short program to display heading (yaw) values from the 9250, as shown below. The program uses Brian (nox771)’s wonderful i2c_t3 I2C library for the Teensy 3.x & LC controllers, and a modified version of ‘Class_MPU9250BasicAHRS_t3.h/cpp that incorporates an adaptation of Sebastian Madgwick’s “…efficient orientation filter” for 6D0F vs 9DOF. The modification removes the magnetometer information from the calculations, as I already know that the magnetic field in my environment is corrupted with house wiring and is unreliable.

/*
    Name:       Teensy_MPU9250_Test.ino
    Created:	9/15/2019 11:46:10 AM
    Author:     FRANKWIN10\Frank
*/

/*	All this program does is display heading values from a Sparkfun MPU9250
	breakout board at the default (0x68) I2C address.  Note that it uses a 
	slightly modified version of Class_MPU9250BasicAHRS_t3.h/cpp that incorporates 
	an adaptation of Sebastian Madgwick's "...efficient orientation filter" for 
	6D0F vs 9DOF.  The modification removes the magnetometer information from the 
	calculations, as I already know that the magnetic field in my environment is 
	corrupted with house wiring and is unreliable.
*/

#include "Class_MPU9250BasicAHRS_t3.h" //extracted all #defines from MPU9250BasicAHRS_t3.ino
#include <i2c_t3.h>
#include <elapsedMillis.h>

elapsedMillis sinceLastPrint;
Class_MPU9250BasicAHRS myIMU;

int pulsePin = LED_BUILTIN;//04/10/18 added per-step pulse pin parameter
bool bFirstTime = true;
const long MSEC_PER_PRINT = 100;

void setup()
{
	Serial.begin(115200);
	delay(3000); //delay required to wait for Serial object creation
	//Wire.begin();

	pinMode(LED_BUILTIN, OUTPUT);
	Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);
	//Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);
	Wire.setDefaultTimeout(200000); // 200ms

	//IMU init & test
	Serial.println("Starting program...");
	Serial.println("Initializing 9250 IMU...");
	myIMU.init9250IMU();
	Serial.println("Done...");
}

void loop()
{
	float hdg_deg = myIMU.GetIMUHeadingDeg();
	if (bFirstTime)
	{
		bFirstTime = false;
		Serial.printf("Msec\tHdg\n");
	}

	if (sinceLastPrint > MSEC_PER_PRINT)
	{
		Serial.printf("%lu\t%4.2f\n", millis(), hdg_deg);
		sinceLastPrint -= MSEC_PER_PRINT;
	}
}

Name: Teensy_MPU9250_Test.ino

Created: 9/15/2019 11:46:10 AM

Author: FRANKWIN10\Frank

/* All this program does is display heading values from a Sparkfun MPU9250

breakout board at the default (0x68) I2C address. Note that it uses a

slightly modified version of Class_MPU9250BasicAHRS_t3.h/cpp that incorporates

an adaptation of Sebastian Madgwick's "...efficient orientation filter" for

6D0F vs 9DOF. The modification removes the magnetometer information from the

calculations, as I already know that the magnetic field in my environment is

corrupted with house wiring and is unreliable.

#include "Class_MPU9250BasicAHRS_t3.h" //extracted all #defines from MPU9250BasicAHRS_t3.ino

#include <i2c_t3.h>

#include <elapsedMillis.h>

elapsedMillis sinceLastPrint;

Class_MPU9250BasicAHRS myIMU;

int pulsePin = LED_BUILTIN;//04/10/18 added per-step pulse pin parameter

bool bFirstTime = true;

const long MSEC_PER_PRINT = 100;

void setup()

{

Serial.begin(115200);

delay(3000); //delay required to wait for Serial object creation

//Wire.begin();

pinMode(LED_BUILTIN, OUTPUT);

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

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

Wire.setDefaultTimeout(200000); // 200ms

//IMU init & test

Serial.println("Starting program...");

Serial.println("Initializing 9250 IMU...");

myIMU.init9250IMU();

Serial.println("Done...");

}

void loop()

{

float hdg_deg = myIMU.GetIMUHeadingDeg();

if (bFirstTime)

{

bFirstTime = false;

Serial.printf("Msec\tHdg\n");

}

if (sinceLastPrint > MSEC_PER_PRINT)

{

Serial.printf("%lu\t%4.2f\n", millis(), hdg_deg);

sinceLastPrint -= MSEC_PER_PRINT;

}

The modified Madgwick routine is included below

// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
void Class_MPU9250BasicAHRS::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz)
//void Class_MPU9250BasicAHRS::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float q_array[4], float delta_t)
{
	float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];         // short name local variable for readability
	//float q1 = q_array[0], q2 = q_array[1], q3 = q_array[2], q4 = q_array[3];         // short name local variable for readability
	float norm;                                               // vector norm
	float f1, f2, f3;                                         // objetive funcyion elements
	float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements
	float qDot1, qDot2, qDot3, qDot4;
	float hatDot1, hatDot2, hatDot3, hatDot4;
	float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz;        // gyro bias error

															  // Auxiliary variables to avoid repeated arithmetic
	float _halfq1 = 0.5f * q1;
	float _halfq2 = 0.5f * q2;
	float _halfq3 = 0.5f * q3;
	float _halfq4 = 0.5f * q4;
	float _2q1 = 2.0f * q1;
	float _2q2 = 2.0f * q2;
	float _2q3 = 2.0f * q3;
	float _2q4 = 2.0f * q4;

	//04/24/18 bugfix:  these vars aren't used in 6DOF version
	//float _2q1q3 = 2.0f * q1 * q3;
	//float _2q3q4 = 2.0f * q3 * q4;

	// Normalise accelerometer measurement
	norm = sqrt(ax * ax + ay * ay + az * az);
	if (norm == 0.0f) return; // handle NaN
	norm = 1.0f / norm;
	ax *= norm;
	ay *= norm;
	az *= norm;

	// Compute the objective function and Jacobian
	f1 = _2q2 * q4 - _2q1 * q3 - ax;
	f2 = _2q1 * q2 + _2q3 * q4 - ay;
	f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;
	J_11or24 = _2q3;
	J_12or23 = _2q4;
	J_13or22 = _2q1;
	J_14or21 = _2q2;
	J_32 = 2.0f * J_14or21;
	J_33 = 2.0f * J_11or24;

	// Compute the gradient (matrix multiplication)
	hatDot1 = J_14or21 * f2 - J_11or24 * f1;
	hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;
	hatDot3 = J_12or23 * f2 - J_33 * f3 - J_13or22 * f1;
	hatDot4 = J_14or21 * f1 + J_11or24 * f2;

	// Normalize the gradient
	norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
	hatDot1 /= norm;
	hatDot2 /= norm;
	hatDot3 /= norm;
	hatDot4 /= norm;

	// Compute estimated gyroscope biases
	gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;
	gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;
	gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;

	// Compute and remove gyroscope biases
	//04/24/18 bugfix: gbias vars are unitialized here
	//gbiasx += gerrx * deltat_sec * zeta;
	//gbiasy += gerry * deltat_sec * zeta;
	//gbiasz += gerrz * deltat_sec * zeta;
	gbiasx = gerrx * deltat_sec * zeta;
	gbiasy = gerry * deltat_sec * zeta;
	gbiasz = gerrz * deltat_sec * zeta;
	gx -= gbiasx;
	gy -= gbiasy;
	gz -= gbiasz;

	// Compute the quaternion derivative
	qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz;
	qDot2 = _halfq1 * gx + _halfq3 * gz - _halfq4 * gy;
	qDot3 = _halfq1 * gy - _halfq2 * gz + _halfq4 * gx;
	qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx;

	// Compute then integrate estimated quaternion derivative
	q1 += (qDot1 - (beta * hatDot1)) * deltat_sec;
	q2 += (qDot2 - (beta * hatDot2)) * deltat_sec;
	q3 += (qDot3 - (beta * hatDot3)) * deltat_sec;
	q4 += (qDot4 - (beta * hatDot4)) * deltat_sec;

	// Normalize the quaternion
	norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
	norm = 1.0f / norm;
	q[0] = q1 * norm;
	q[1] = q2 * norm;
	q[2] = q3 * norm;
	q[3] = q4 * norm;
}

float Class_MPU9250BasicAHRS::GetIMUHeadingDeg()
{
	// If intPin goes high, all data registers have new data
	//if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) 
	{  // On interrupt, check if data ready interrupt
		readAccelData(accelCount);  // Read the x/y/z adc values
		getAres();

		// Now we'll calculate the accleration value into actual g's
		ax = (float)accelCount[0] * aRes; // - accelBias[0];  // get actual g value, this depends on scale being set
		ay = (float)accelCount[1] * aRes; // - accelBias[1];   
		az = (float)accelCount[2] * aRes; // - accelBias[2];  

		readGyroData(gyroCount);  // Read the x/y/z adc values
		getGres();

		// Calculate the gyro value into actual degrees per second
		gx = (float)gyroCount[0] * gRes;  // get actual gyro value, this depends on scale being set
		gy = (float)gyroCount[1] * gRes;
		gz = (float)gyroCount[2] * gRes;

#ifndef NO_MAG
		readMagData(magCount);  // Read the x/y/z adc values
		getMres();
		magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
		magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
		magbias[2] = +125.;  // User environmental x-axis correction in milliGauss

							 // Calculate the magnetometer values in milliGauss
							 // Include factory calibration per data sheet and user environmental corrections
		mx = (float)magCount[0] * mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
		my = (float)magCount[1] * mRes*magCalibration[1] - magbias[1];
		mz = (float)magCount[2] * mRes*magCalibration[2] - magbias[2];
#endif // !NO_MAG
	}

	Now = micros();
	deltat_sec = ((Now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update
	lastUpdate = Now;

	sum += deltat_sec; // sum for averaging filter update rate
	sumCount++;

	// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer;
	// the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!
	// We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter.
	// For the MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like
	// in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention.
	// This is ok by aircraft orientation standards!  
	// Pass gyro rate as rad/s
#ifndef NO_MAG
	MadgwickQuaternionUpdate(ax, ay, az, gx*PI / 180.0f, gy*PI / 180.0f, gz*PI / 180.0f, my, mx, mz);
	//  MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
#else
	//Serial.printf("q = {%4.2f, %4.2f, %4.2f, %4.2f, deltat_ms = %2.4f\n", q[0], q[1], q[2], q[3], deltat_sec*1000);

	//experiment with multiple iterations on quat update
	//for (size_t i = 0; i < 10; i++)
	{
		MadgwickQuaternionUpdate(ax, ay, az, gx*PI / 180.0f, gy*PI / 180.0f, gz*PI / 180.0f);
	}
	//MadgwickQuaternionUpdate(ax, ay, az, gx*PI / 180.0f, gy*PI / 180.0f, gz*PI / 180.0f, q, deltat_sec);
#endif // !NO_MAG

	if (!AHRS) 
	{
		delt_t = millis() - count;
		if (delt_t > 500) 
		{

			if (SerialDebug) {
				// Print acceleration values in milligs!
				Serial.print("X-acceleration: "); Serial.print(1000 * ax); Serial.print(" mg ");
				Serial.print("Y-acceleration: "); Serial.print(1000 * ay); Serial.print(" mg ");
				Serial.print("Z-acceleration: "); Serial.print(1000 * az); Serial.println(" mg ");

				// Print gyro values in degree/sec
				Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec ");
				Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec ");
				Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec");

#ifndef NO_MAG
				// Print mag values in degree/sec
				Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG ");
				Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG ");
				Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG");

				tempCount = readTempData();  // Read the adc values
				temperature = ((float)tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade
																  // Print temperature in degrees Centigrade      
				Serial.print("Temperature is ");  Serial.print(temperature, 1);  Serial.println(" degrees C"); // Print T values to tenths of s degree C
#endif // !NO_MAG
			}
			count = millis();
		}
	}
	else 
	{

		// Serial print and/or display at 0.5 s rate independent of data rates
		delt_t = millis() - count;
		//if (delt_t > 500) 
		{ // update LCD once per half-second independent of read rate

			if (SerialDebug) {
				Serial.print("ax = "); Serial.print((int)1000 * ax);
				Serial.print(" ay = "); Serial.print((int)1000 * ay);
				Serial.print(" az = "); Serial.print((int)1000 * az); Serial.println(" mg");
				Serial.print("gx = "); Serial.print(gx, 2);
				Serial.print(" gy = "); Serial.print(gy, 2);
				Serial.print(" gz = "); Serial.print(gz, 2); Serial.println(" deg/s");
#ifndef NO_MAG
				Serial.print("mx = "); Serial.print((int)mx);
				Serial.print(" my = "); Serial.print((int)my);
				Serial.print(" mz = "); Serial.print((int)mz); Serial.println(" mG");
#endif // !NO_MAG

				Serial.print("q0 = "); Serial.print(q[0]);
				Serial.print(" qx = "); Serial.print(q[1]);
				Serial.print(" qy = "); Serial.print(q[2]);
				Serial.print(" qz = "); Serial.println(q[3]);
			}

			// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
			// In this coordinate system, the positive z-axis is down toward Earth. 
			// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
			// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
			// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
			// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
			// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
			// applied in the correct order which for this configuration is yaw, pitch, and then roll.
			// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
			yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
			pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
			roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
			pitch *= 180.0f / PI;
			yaw *= 180.0f / PI;
			//yaw -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
			roll *= 180.0f / PI;

			if (SerialDebug) {
				Serial.print("Yaw, Pitch, Roll: ");
				Serial.print(yaw, 2);
				Serial.print(", ");
				Serial.print(pitch, 2);
				Serial.print(", ");
				Serial.println(roll, 2);

				Serial.print("rate = "); Serial.print((float)sumCount / sum, 2); Serial.println(" Hz");
			}

			//Serial.printf("%3.2f\t%3.2f\t%3.2f\n", yaw, pitch, roll);

			// With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and 
			// >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz.
			// The filter update rate is determined mostly by the mathematical steps in the respective algorithms, 
			// the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:
			// an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
			// filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. 
			// This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
			// This filter update rate should be fast enough to maintain accurate platform orientation for 
			// stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
			// produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
			// The 3.3 V 8 MHz Pro Mini is doing pretty well!

			digitalWrite(myLed, !digitalRead(myLed));
			count = millis();
			sumCount = 0;
			sum = 0;
		}
	}

	return yaw;
}

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

// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"

// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)

// which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative

// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.

// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms

// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!

void Class_MPU9250BasicAHRS::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz)

//void Class_MPU9250BasicAHRS::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float q_array[4], float delta_t)

{

float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability

//float q1 = q_array[0], q2 = q_array[1], q3 = q_array[2], q4 = q_array[3]; // short name local variable for readability

float norm; // vector norm

float f1, f2, f3; // objetive funcyion elements

float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements

float qDot1, qDot2, qDot3, qDot4;

float hatDot1, hatDot2, hatDot3, hatDot4;

float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz; // gyro bias error

// Auxiliary variables to avoid repeated arithmetic

float _halfq1 = 0.5f * q1;

float _halfq2 = 0.5f * q2;

float _halfq3 = 0.5f * q3;

float _halfq4 = 0.5f * q4;

float _2q1 = 2.0f * q1;

float _2q2 = 2.0f * q2;

float _2q3 = 2.0f * q3;

float _2q4 = 2.0f * q4;

//04/24/18 bugfix: these vars aren't used in 6DOF version

//float _2q1q3 = 2.0f * q1 * q3;

//float _2q3q4 = 2.0f * q3 * q4;

// Normalise accelerometer measurement

norm = sqrt(ax * ax + ay * ay + az * az);

if (norm == 0.0f) return; // handle NaN

norm = 1.0f / norm;

ax *= norm;

ay *= norm;

az *= norm;

// Compute the objective function and Jacobian

f1 = _2q2 * q4 - _2q1 * q3 - ax;

f2 = _2q1 * q2 + _2q3 * q4 - ay;

f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;

J_11or24 = _2q3;

J_12or23 = _2q4;

J_13or22 = _2q1;

J_14or21 = _2q2;

J_32 = 2.0f * J_14or21;

J_33 = 2.0f * J_11or24;

// Compute the gradient (matrix multiplication)

hatDot1 = J_14or21 * f2 - J_11or24 * f1;

hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;

hatDot3 = J_12or23 * f2 - J_33 * f3 - J_13or22 * f1;

hatDot4 = J_14or21 * f1 + J_11or24 * f2;

// Normalize the gradient

norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);

hatDot1 /= norm;

hatDot2 /= norm;

hatDot3 /= norm;

hatDot4 /= norm;

// Compute estimated gyroscope biases

gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;

gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;

gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;

// Compute and remove gyroscope biases

//04/24/18 bugfix: gbias vars are unitialized here

//gbiasx += gerrx * deltat_sec * zeta;

//gbiasy += gerry * deltat_sec * zeta;

//gbiasz += gerrz * deltat_sec * zeta;

gbiasx = gerrx * deltat_sec * zeta;

gbiasy = gerry * deltat_sec * zeta;

gbiasz = gerrz * deltat_sec * zeta;

gx -= gbiasx;

gy -= gbiasy;

gz -= gbiasz;

// Compute the quaternion derivative

qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz;

qDot2 = _halfq1 * gx + _halfq3 * gz - _halfq4 * gy;

qDot3 = _halfq1 * gy - _halfq2 * gz + _halfq4 * gx;

qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx;

// Compute then integrate estimated quaternion derivative

q1 += (qDot1 - (beta * hatDot1)) * deltat_sec;

q2 += (qDot2 - (beta * hatDot2)) * deltat_sec;

q3 += (qDot3 - (beta * hatDot3)) * deltat_sec;

q4 += (qDot4 - (beta * hatDot4)) * deltat_sec;

// Normalize the quaternion

norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion

norm = 1.0f / norm;

q[0] = q1 * norm;

q[1] = q2 * norm;

q[2] = q3 * norm;

q[3] = q4 * norm;

}

float Class_MPU9250BasicAHRS::GetIMUHeadingDeg()

{

// If intPin goes high, all data registers have new data

//if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)

{ // On interrupt, check if data ready interrupt

readAccelData(accelCount); // Read the x/y/z adc values

getAres();

// Now we'll calculate the accleration value into actual g's

ax = (float)accelCount[0] * aRes; // - accelBias[0]; // get actual g value, this depends on scale being set

ay = (float)accelCount[1] * aRes; // - accelBias[1];

az = (float)accelCount[2] * aRes; // - accelBias[2];

readGyroData(gyroCount); // Read the x/y/z adc values

getGres();

// Calculate the gyro value into actual degrees per second

gx = (float)gyroCount[0] * gRes; // get actual gyro value, this depends on scale being set

gy = (float)gyroCount[1] * gRes;

gz = (float)gyroCount[2] * gRes;

#ifndef NO_MAG

readMagData(magCount); // Read the x/y/z adc values

getMres();

magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated

magbias[1] = +120.; // User environmental x-axis correction in milliGauss

magbias[2] = +125.; // User environmental x-axis correction in milliGauss

// Calculate the magnetometer values in milliGauss

// Include factory calibration per data sheet and user environmental corrections

mx = (float)magCount[0] * mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set

my = (float)magCount[1] * mRes*magCalibration[1] - magbias[1];

mz = (float)magCount[2] * mRes*magCalibration[2] - magbias[2];

#endif // !NO_MAG

}

Now = micros();

deltat_sec = ((Now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update

lastUpdate = Now;

sum += deltat_sec; // sum for averaging filter update rate

sumCount++;

// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer;

// the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!

// We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter.

// For the MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like

// in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention.

// This is ok by aircraft orientation standards!

// Pass gyro rate as rad/s

#ifndef NO_MAG

MadgwickQuaternionUpdate(ax, ay, az, gx*PI / 180.0f, gy*PI / 180.0f, gz*PI / 180.0f, my, mx, mz);

// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);

#else

//Serial.printf("q = {%4.2f, %4.2f, %4.2f, %4.2f, deltat_ms = %2.4f\n", q[0], q[1], q[2], q[3], deltat_sec*1000);

//experiment with multiple iterations on quat update

//for (size_t i = 0; i < 10; i++)

{

MadgwickQuaternionUpdate(ax, ay, az, gx*PI / 180.0f, gy*PI / 180.0f, gz*PI / 180.0f);

}

//MadgwickQuaternionUpdate(ax, ay, az, gx*PI / 180.0f, gy*PI / 180.0f, gz*PI / 180.0f, q, deltat_sec);

#endif // !NO_MAG

if (!AHRS)

{

delt_t = millis() - count;

if (delt_t > 500)

{

if (SerialDebug) {

// Print acceleration values in milligs!

Serial.print("X-acceleration: "); Serial.print(1000 * ax); Serial.print(" mg ");

Serial.print("Y-acceleration: "); Serial.print(1000 * ay); Serial.print(" mg ");

Serial.print("Z-acceleration: "); Serial.print(1000 * az); Serial.println(" mg ");

// Print gyro values in degree/sec

Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec ");

Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec ");

Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec");

#ifndef NO_MAG

// Print mag values in degree/sec

Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG ");

Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG ");

Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG");

tempCount = readTempData(); // Read the adc values

temperature = ((float)tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade

// Print temperature in degrees Centigrade

Serial.print("Temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C

#endif // !NO_MAG

}

count = millis();

}

else

{

// Serial print and/or display at 0.5 s rate independent of data rates

delt_t = millis() - count;

//if (delt_t > 500)

{ // update LCD once per half-second independent of read rate

if (SerialDebug) {

Serial.print("ax = "); Serial.print((int)1000 * ax);

Serial.print(" ay = "); Serial.print((int)1000 * ay);

Serial.print(" az = "); Serial.print((int)1000 * az); Serial.println(" mg");

Serial.print("gx = "); Serial.print(gx, 2);

Serial.print(" gy = "); Serial.print(gy, 2);

Serial.print(" gz = "); Serial.print(gz, 2); Serial.println(" deg/s");

#ifndef NO_MAG

Serial.print("mx = "); Serial.print((int)mx);

Serial.print(" my = "); Serial.print((int)my);

Serial.print(" mz = "); Serial.print((int)mz); Serial.println(" mG");

#endif // !NO_MAG

Serial.print("q0 = "); Serial.print(q[0]);

Serial.print(" qx = "); Serial.print(q[1]);

Serial.print(" qy = "); Serial.print(q[2]);

Serial.print(" qz = "); Serial.println(q[3]);

}

// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.

// In this coordinate system, the positive z-axis is down toward Earth.

// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.

// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.

// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.

// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.

// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be

// applied in the correct order which for this configuration is yaw, pitch, and then roll.

// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.

yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);

pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));

roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);

pitch *= 180.0f / PI;

yaw *= 180.0f / PI;

//yaw -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04

roll *= 180.0f / PI;

if (SerialDebug) {

Serial.print("Yaw, Pitch, Roll: ");

Serial.print(yaw, 2);

Serial.print(", ");

Serial.print(pitch, 2);

Serial.print(", ");

Serial.println(roll, 2);

Serial.print("rate = "); Serial.print((float)sumCount / sum, 2); Serial.println(" Hz");

}

//Serial.printf("%3.2f\t%3.2f\t%3.2f\n", yaw, pitch, roll);

// With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and

// >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz.

// The filter update rate is determined mostly by the mathematical steps in the respective algorithms,

// the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:

// an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces

// filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively.

// This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.

// This filter update rate should be fast enough to maintain accurate platform orientation for

// stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz

// produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.

// The 3.3 V 8 MHz Pro Mini is doing pretty well!

digitalWrite(myLed, !digitalRead(myLed));

count = millis();

sumCount = 0;

sum = 0;

}

return yaw;

}

Note to self; after reviewing the extensive email thread with Kris Winer (tleracorp@gmail.com) I now believe I encapsulated all the required modifications to the AHRS code into a new class called “Class_MPU9250BasicAHRS_t3” with Class_MPU9250BasicAHRS_t3.h & .cpp files, and then referenced this new class in my MPU9250 work.

The results:

After getting everything working (and figuring out the history), I finally started getting reliable heading data from the MPU9250 as shown in the following Excel plot, where the breadboard was manually rotated back and forth.

Stay tuned,

Frank

Accessing the Internet with an ESP32 Dev Board

Leave a reply

Posted 27 August 2019

During my recent investigation of problems associated with the MPU6050 IMU on my 2-motor robot (which I eventually narrowed down to I2C bus susceptibility to motor driver noise), one poster suggested that the Espressif ESP32 wifi & bluetooth enabled microcontroller might be a good alternative to Arduino boards because the ESP32 chip is ‘shielded’ (not sure what that means, but…). In any case, I was intrigued by the possibility that I might be able to replace my current HC-05 bluetooth module (on the 2-motor robot) and/or the Wixel shield (on the 4-motor robot) with an integrated wifi link that would be able to send back telemetry from anywhere in my house via the existing wifi network. So, I decided to buy a couple (I got the Espressif ESP32 Dev Board from Adafruit) and see if I could get the wifi capability to work.

As usual, this turned out to be a much bigger deal than I thought. My first clue was the fact that Adafruit went to significant pains on their website to note that the ESP Dev Board was ‘for developers only” as shown below:

Please note: The ESP32 is still targeted to developers. Not all of the peripherals are fully documented with example code, and there are some bugs still being found and fixed. We got many sensors and displays working under Arduino IDE, so you can expect things like I2C and SPI and analog reads to work. But other elements are still under dev

Undaunted, I got two boards, and set about connecting my ESP32 dev board to the internet. I found several examples on the internet, but none of them worked (or were even understandable, at least to me). That’s when I realized that I was basically clueless about the entire IoT world in general, and the ESP32’s place in that world in particular – bummer!

So, after lots of screaming, yelling, and hair-pulling (well, not the last because I don’t have much left), I finally got my ESP32 to talk to the internet and actually retrieve part of a web page without crashing. In order to consolidate my new-found knowledge (and maybe help other ESP32 newbies), I decided to create this post as a ‘how to’ for ESP32 internet connections.

General Strategy

Here’s the general strategy I followed in getting my ESP Dev Board connected to the internet and capable of downloading data from a website.

Install ESP32 libraries and tools into either the Arduino IDE or the Visual Micro extension to Microsoft Visual Studio (I have the VS 2019 Community Edition).
Install and run a localhost server. This was a great troubleshooting tool, as with it I could monitor website requests to the server.
Install ‘curl’, the wonderful open-source tool for internet protocol data transfers. This was absolutely essential for verifying the proper http request syntax needed to elicit the proper response from the server.
Use curl to figure out the proper HTTP ‘GET’ string syntax.
Modify the WiFiClientBasic example program to successfully retrieve a document from my localhost server.

Install ESP32 libraries and tools

This step by itself was not entirely straightforward; I wound up installing the libraries & tools using the Arduino IDE rather than in the VS2019/Visual Micro environment. I’m sure it can be done either way, but it seemed much easier in the Arduino IDE. Once this is done, then the ESP32 Dev Board can be selected (in either the Arduino IDE or the VS/VM environment) as a compile target.

Install and run a localhost server

This step is probably not absolutely necessary, as there are a number of ‘mock’ sites on the internet that purport to help with IoT app development. However, I found having a ‘localhost’ web server on my laptop very convenient, as this gave me a self-contained test suite for working through the myriad problems I encountered. I used the Node.js setup for Win10, as described in this post. The cool thing about this approach is the console window used to start the server also shows all the request activity directed to the server, allowing me to directly monitor what the ESP32 is actually sending to the site. Here are two screenshots showing some recent activity.

The first log fragment above shows the server starting up, and the first set of http requests. The first half dozen or so requests are from another PC; I did this to initially confirm I could actually reach my localhost server. This first test failed miserably until I figured out I had to disable my PC’s firewall – oops! The next set of lines are from my curl app showing what is actually received by the server when I send a ‘GET’ request from curl.

The screenshot above shows some more curl-generated requests, and then a bunch of requests from ‘undefined’. These requests were generated by my ‘WiFiClientBasic’ program running on the ESP32 – success!!

Install ‘curl’

Curl is a wonderful command-line program to generate http (and any other web protocol you can imagine) requests. You can get the executable from this site, and unzip and run it from a command window – no installation required. Using curl, I was able to determine the exact syntax for an http ‘GET’ request to a website, as shown in the screenshot below

The screenshot above shows curl being used from the command line. The first line C:\Users\Frank>curl -v http://192.168.1.90:1337/index.html generates a ‘GET’ request for the file ‘index.html’ to the site ‘192.168.1.90’ (my localhost server address on the local network), and the -v (verbose) option displays what is actually sent to the server, i.e.

GET /index.html HTTP/1.1
> Host: 192.168.1.90:1337
> User-Agent: curl/7.55.1
> Accept: */*

This was actually a huge revelation to me, as I had no idea that a simple ‘GET’ request was a multi-line deal – wow! Up to this point, I had been trying to use the ‘client.send()’ command in the WiFiClientBasic example program to just send the ‘GET /index.html HTTP/1.1’ string, with a commensurate lack of success – oops!

Modify the WifiClientBasic example program

Armed with the knowledge of the exact syntax required, I was now able to modify the ‘WifiClientBasic’ example program to emit the proper ‘GET’ syntax so that the localhost server would respond appropriately. The final program (minus my network login credentials) is shown below.

/*
 *  This sketch sends a message to a TCP server
 *
 */

//#include <WiFi.h>
#include <WiFiMulti.h>

WiFiMulti WiFiMulti;

char outbuf[100] = { "GET /index.html HTTP/1.1\nHost: 192.168.1.90:1337\nUser-Agent: ESP32\nAccept: */*\n\n" };
//char outbuf[100] = { "GET /index.html HTTP/1.1\n\n" };
char inbuf[200] = { "input buffer" };

void setup()
{
    Serial.begin(115200);
    delay(10);
	Serial.print("outbuf = "); Serial.println(outbuf);
	Serial.print("inbuf = "); Serial.println(inbuf);

    // We start by connecting to a WiFi network
    WiFiMulti.addAP("your_SSID", "your_password");

    Serial.println();
    Serial.println();
    Serial.print("Waiting for WiFi... ");

    while(WiFiMulti.run() != WL_CONNECTED) {
        Serial.print(".");
        delay(500);
    }

    Serial.println("");
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());

    delay(500);
}

void loop()
{
    const uint16_t port = 1337;
    const char * host = "192.168.1.10"; // ip or dns

    Serial.print("Connecting to ");
    Serial.println(host);

    // Use WiFiClient class to create TCP connections
    WiFiClient client;

    if (!client.connect(host, port)) {
        Serial.println("Connection failed.");
        Serial.println("Waiting 5 seconds before retrying...");
        delay(5000);
        return;
    }

    // This will send a request to the server
	Serial.print("Sending "); Serial.println(outbuf);
	client.println(outbuf);

	int maxloops = 0;
	int numbytes = 0;

	while (!client.available() && maxloops < 1000)
	{
		maxloops++;
		delay(1);
	}
	if (client.available() > 0)
	{
		numbytes = client.readBytes(inbuf, sizeof(inbuf)); // read chars from stream into outbuf
		Serial.print("got "); Serial.print(numbytes); Serial.print(" from server after "); 
		Serial.print(maxloops); Serial.println(" Msec");
		Serial.println(inbuf);
	}
	else
	{
		Serial.print("client.available() returned "); Serial.println(numbytes);
	}


    Serial.println("Closing connection.");
    client.stop();

    Serial.println("Waiting 5 seconds before restarting...");
    delay(5000);
}

* This sketch sends a message to a TCP server

//#include <WiFi.h>

#include <WiFiMulti.h>

WiFiMulti WiFiMulti;

char outbuf[100] = { "GET /index.html HTTP/1.1\nHost: 192.168.1.90:1337\nUser-Agent: ESP32\nAccept: */*\n\n" };

//char outbuf[100] = { "GET /index.html HTTP/1.1\n\n" };

char inbuf[200] = { "input buffer" };

void setup()

{

Serial.begin(115200);

delay(10);

Serial.print("outbuf = "); Serial.println(outbuf);

Serial.print("inbuf = "); Serial.println(inbuf);

// We start by connecting to a WiFi network

WiFiMulti.addAP("your_SSID", "your_password");

Serial.println();

Serial.print("Waiting for WiFi... ");

while(WiFiMulti.run() != WL_CONNECTED) {

Serial.print(".");

delay(500);

}

Serial.println("");

Serial.println("WiFi connected");

Serial.println("IP address: ");

Serial.println(WiFi.localIP());

delay(500);

}

void loop()

{

const uint16_t port = 1337;

const char * host = "192.168.1.10"; // ip or dns

Serial.print("Connecting to ");

Serial.println(host);

// Use WiFiClient class to create TCP connections

WiFiClient client;

if (!client.connect(host, port)) {

Serial.println("Connection failed.");

Serial.println("Waiting 5 seconds before retrying...");

delay(5000);

return;

}

// This will send a request to the server

Serial.print("Sending "); Serial.println(outbuf);

client.println(outbuf);

int maxloops = 0;

int numbytes = 0;

while (!client.available() && maxloops < 1000)

{

maxloops++;

delay(1);

}

if (client.available() > 0)

{

numbytes = client.readBytes(inbuf, sizeof(inbuf)); // read chars from stream into outbuf

Serial.print("got "); Serial.print(numbytes); Serial.print(" from server after ");

Serial.print(maxloops); Serial.println(" Msec");

Serial.println(inbuf);

}

else

{

Serial.print("client.available() returned "); Serial.println(numbytes);

}

Serial.println("Closing connection.");

client.stop();

Serial.println("Waiting 5 seconds before restarting...");

delay(5000);

}

This produced the following output:

Conclusion:

After all was said and done, most of the problems I had getting the ESP32 to connect to the internet and successfully retrieve some contents from a website were due to my almost complete ignorance of HTTP protocol syntax. However, some of the blame must be laid at the foot of the WiFiClientBasic example program, as the lack of any error checking caused multiple ‘Guru Meditation Errors’ (which I believe is Espressif-speak for ‘segmentation fault’) when I was trying to get everything to work. In particular, the original example code assumes the website response will be available immediately after the request and tries to read an invalid buffer, crashing the ESP32. My modified code waits in a 1 Msec delay loop for client.available() to return a non-zero result. As shown in the above output, this usually happens after 5-7 Msec.

In addition, I found that either the full syntax:

GET /index.html HTTP/1.1
Host: 192.168.1.90:1337
User-Agent: ESP32
Accept: */* {newline}

or just

GET /index.html HTTP/1.1{newline}

worked fine to retrieve the contents of ‘index.html’ on the localhost server, because the ‘host’ information is already present in the connection, and the defaults for the remaining two lines are reasonable. However, I believe the trailing {newline} is still required for both cases.

So, now that I can successfully use the ESP32 to connect to my local wireless network and perform internet functions, my plan is to try and use some of the IoT support facilities available on the internet (like Adafruit’s io.adafruit.com) to see if I can get the ESP32 to upload simulated robot telemetry data to a cloud-based data store. If I can pull that off, then I’ll be one step closer to replacing my current HC-05 bluetooth setup (on the 2-motor robot) and/or my Wixel setup (on the 4-motor robot).

Stay tuned!

Frank

Back to the future with Wall-E2. Wall-following Part VI

3 Replies

Posted 13 August 2019

In my last post on this subject, I discussed the idea of using orientation information to compensate raw wall offset distance values to account for the errors associated with robot orientation. The idea was that if I could do that, then Wall-E2 would know how far he was away from the wall regardless of orientation, and would be able to make appropriate corrections to get to and stay at a predetermined offset from the wall.

Well, it didn’t really work out that way. After getting through the geometry analysis and the math, it turned out that in order to use the compensation algorithm, I have to know the initial robot orientation with respect to the wall, and I don’t :-(. Without knowing this, it is basically impossible to apply the correct compensation. For example, if the robot is originally oriented 30º away from the wall, then a ‘toward-wall’ rotation will cause the measured distance to go down, and an upward compensation is required. However, if the robot is initially oriented toward the wall, then that same ‘toward-wall’ rotation will cause the measured distance to go up and a downward compensation is required – bummer!

However, all is not lost; the ability to perform relatively precise angular rotations means that I can use incremental rotations for acquiring and then tracking a predetermined offset distance. In the acquisition phase, the robot orientation is changed in 10º increments in the appropriate direction, and an N-point slope calculation is performed to determine whether or not the current ‘cut angle’ will allow the robot to eventually reach the predetermined offset distance. As the robot approaches the offset line, the cut angle is reduced until it is zero, in theory resulting in the robot travelling parallel to the wall at the offset distance. At this point the robot transitions from ‘capture’ to ‘track’ mode, and the response to distance deviations becomes more robust.

This strategy was implemented using my 2-motor robot, and seems to work well once the normal crop of bugs was eradicated. The following Excel plots show the results of two short runs where the robot first captured and then tracked a 30cm offset setting.

Capture and track a 30cm wall offset starting from the outside

Capture and track a 30cm wall offset starting from the inside

So far I have only implemented this completely for the right side, but as the left side is identical, I anticipate no problems in this regard.

Future Work:

So far I have demonstrated the ability to capture and then track a predetermined wall offset distance, starting from either inside or outside the desired offset distance. This represents a quantum leap in performance, as Wall-E2 currently can only track whatever distance it first measures – it has no capability to capture a desired offset distance. However, there are still some ‘edge’ cases that need to be dealt with one way or the other. For instance, if the robot orientation is too far away from parallel, the current algorithm won’t be able to rotate it enough to capture the desired offset or the measured distance will exceed the max range gate of the ping sensors (currently set at 200cm). These conditions may not be all that deleterious, as eventually Wall-E2 will get close enough to something to trigger an avoidance response, thereby resetting the entire orientation picture (hopefully to something a little more parallel).

In addition to the wall tracking problem, the new capability to make reasonably precise angular rotations should significantly improve Wall-E2’s performance in handling ‘open-corner’ and ‘closed-corner’ situations; currently these cases are handled with timed turns, which are only correct for one floor covering type (hard vs soft) and battery state. With the heading measurement capability, a 90º corner turn will always be (approximately) 90º whether it is on carpet or hard flooring. In addition, now I can program in obstacle avoidance step-turns for approaching obstacles instead of relying entirely the ‘backup-and-turn’ approach.

Stay tuned!

Frank

Back to the future with Wall-E2. Wall-following Part V

Leave a reply

Posted 08 August 2019

In my last post on this subject, I described some ideas for improving Wall-E2’s wall following performance by compensating for distance-to-wall errors caused by Wall-E2 not being oriented perfectly parallel to the wall. The situation is shown in the diagram below:

When the robot is parallel to the wall, as shown in light purple, the ping sensor measures distance d1 to the wall. However, when it rotates to make a wall-following adjustment, the ping sensor now measures distance d2, even though the robot’s center of rotation (CR) hasn’t moved at all. If the wall-following algorithm is based strictly on ping distance, the robot tends to wander back and forth, chasing ping measurements that don’t reflect (no pun intended) reality. I need a way of relating the measured distance to the distance from the robot’s CR to the wall, so that wall-following adjustments can be made referenced to the CR, not to the ping sensor position on the robot.

Given the above geometry, an expression can be developed to relate the perpendicular distance d1 and the measured distance d2, as shown below:

Expression relating perpendicular distance to measured distance for any rotation angle

I set up an experiment where the robot was placed on a platform about 16cm away from an obstacle. I measured the ‘ping’ distance to the obstacle as the robot was manually rotated +/- 20 deg. Then I plotted the data in Excel as shown below:

In the above plot, the heading values (blue line) have been normalized to the initial heading and any linear drift removed. After correction, the robot changes heading almost exactly +/- 20 deg. Similarly, the measured distances (orange line) values were normalized to the nominal distance of 16cm. As can be seen, the measured distance varied about +4 to -2 cm, even though the robot center of rotation (CR) remained fixed. Then the distance compensation expression shown above was applied, resulting in the gray line. This shows that the compensation expression is effective in reducing angle-induced distance changes.

Next, I set up a ‘live’ experiment with the 2-motor robot to more closely emulate the normal operating environment. I set up a section of ‘wall’ and had the robot make a single 60 deg turn, starting with the robot angled about 30 deg toward the wall, and ending with the robot angled about 30 deg away from the wall. Distance measurements were taken as rapidly as possible during the turn, but not before or after the turn started.

Here’s a short video of the 2-motor robot approaching a ‘wall’ at an angle of about 30º and making a single turn of about 60º. The entire sequence is about 3 seconds long. The robot runs straight for about 1 sec, then turns for about 1 sec, then goes straight again for about 1 sec.

The measured ‘ping’ distances for the 1-second turn portion of the run is shown in the Excel plot below

The above plot starts when the robot starts turning at about 1.2 sec into the video (the approach to the wall is not shown). When the turn starts, the measured distance to the wall is about 20 cm. The measured distance decreases rapidly to about 16 cm at about 0.4 sec into the turn (about 1.6 sec into the video), and stays there for about 0.4 sec and then starts climbing rapidly to about 23 cm when the turn finishes. However, the distance from the center of rotation (CR) of the robot to the wall changes hardly at all. The blue painter’s tape in the background of the video has black markings each 5 cm, and it is possible to estimate the distance from the CR to the wall throughout the turn. My estimate is that the robot’s CR starts at about 25 cm, decreases to about 22 cm at the apex of the turn, and then goes back to about 25 cm at the end of the turn. The measured distance decreases 4 cm and then increases 8 cm while the robot’s CR decreases 3 cm and increases 3 – quite a difference, due entirely to the angle change between the robot and the wall during the turn. After normalizing the heading values so that they reflect the angle off parallel and applying the distance compensation expression above, I got the following plot:

In the above plot, the gray line shows the corrected distance from the robot CR to the wall. As estimated from the video earlier, the CR varies only about 1cm during the turn. This is pretty strong evidence that the proposed distance correction scheme is correct and effective in removing distance measurement errors due to robot heading changes.

With the technique demonstrated above, I am optimistic that I can now not only improve wall tracking, but also can implement wall-following at a specific distance, say 25 cm. The difficulty with trying to displace laterally to acquire and then lock to a specific distance is the large changes in measured difference due to the angle change needed to move toward or away from the wall made it impossible to determine where the robot’s CR actually was relative to the desired offset distance. By (mostly) removing this orientation-induced error term, it should be feasible to determine the actions needed to approach and then track the desired offset distance.

Stay tuned!

Frank

08 February 2020 Update:

As I continued my campaign to integrate heading information into my wall-following robot algorithm, my efforts to compensate ‘ping’ distances for off-parallel robot orientations with respect to the nearest wall kept failing, and I didn’t know why. I had gone through the math several times and was convinced it was valid, and as the plot above showed, it should work.

So, I made another run at it, completely redoing the math from the ground up – and running some more test in my ‘local range’ (aka my office). Still no joy – no matter what I did, the math seemed to be overcompensating, as shown in the plot below:

Ping Distance vs Calc Distance for two heading changes

This plot (and others like it) convinced me that I was still missing something fundamental. As I often do, I was thinking about this in bed while drifting off to sleep, and I realized that I might be able to determine the culprit by cheating; I would place the robot at a set distance from the wall, and carefully rotate it manually over a compass rose. At each heading I would manually measure the distance from the ping sensor to the wall, perpendicular to the plane of the sensor (i.e. I would physically measure the distance I would expect the ping sensor to report), and also record the ‘ping’ distance reported by the sensor. With just a few measurements the problem became obvious; the ‘ping’ distance for slant angles to the wall do not even remotely resemble the actual physical distance – it is much less, as shown below.

As can be seen , the compensation algorithm actually works quite well, when dealing with the physically measured slant range. However, because the ‘ping’ distance loses accuracy very rapidly off-parallel angles beyond about 20 degrees, the compensation algorithm is ineffective. A classic case of ‘GIGO’.

After performing the above experiment, I was still left with the mystery of why the compensation algorithm appeared to work so well before – WTF? So, I went back and very carefully examined the previous plot and the underlying data, and discovered I’d made another classic experimental error – The ‘Calculated Distance’ data was plotted on the wrong scale. When plotted on the correct scale, the plot changes to the one shown below:

Previous plot with ‘Calc Distance’ plotted on the correct scale

Now it is clear that the calculated compensation using ‘ping’ distances is not at all useful.

So, the bottom line on all of this it that the effort to apply a heading-based ping distance compensation was doomed to failure from the start, because the distance reported by the ping sensor is wildly inaccurate for off-perpendicular geometries. The good news is that now at least I know why the compensation effort was doomed to fail!

In the meantime, I independently developed a technique for determining the heading required for orienting the robot parallel to the wall as the heading associated with the minimum ping distance achieved by swinging the robot back and forth. This technique utilizes the ping sensors in the realm where they are most accurate, and does away entirely with the need for compensation.

Stay tuned!

Frank

Back to the future with Wall-E2. Wall-following Part IV

Leave a reply

Posted 30 April 2019

In two previous posts (here & here) I described my efforts to upgrade Wall-E2’s wall following performance using a PID control algorithm. The results of my efforts to date in this area have not been very spectacular – a better description might actually be ‘dreadful’ :-(.

After some additional analysis, I came to believe that the reason the PID approach doesn’t work very well is a fundamental feature of the way Wall-E2 measures distance to the nearest wall. Wall-E2 has two acoustic sonar units fixed to its upper deck, and they measure the distance perpendicular to the robot’s longitudinal axis. What this means, however, is that when the robot is angled with respect to the nearest wall, the distance measured isn’t the perpendicular distance, but rather the hypotenuse of the right triangle with the right angle at the wall. So, when Wall-E2 turns toward or away from the wall, the measured distance increases even though the robot hasn’t actually moved toward or away. Conversely, if the robot is angled in toward the wall and then turns to be parallel, the measured distance decreases even if the robot hasn’t moved at all relative to that wall. The situation is shown in the sketch below:

Using Excel, I ran a simulation of the ping distance versus the actual distance for a range of angle offsets from 0 to 30 degrees, as shown below:

As shown above, the ping distance for a constant 25 cm offset ranges from 25 (robot longitudinal axis parallel to the wall) to almost 29 cm for a 30 degree off-axis heading. These values translate to a percentage error of zero to approximately 15%, independent of the initial parallel distance.

So, it becomes obvious why a standard PID algorithm has trouble; if the ping distance goes up slightly, the PID algorithm attempts to compensate by turning toward the wall. However, this causes the ping distance to increase rather than decrease, causing the algorithm to command an even greater angle toward the wall, which in turn causes a further increase in ping distance – entirely backward. The reverse happens for an initial decrease in the ping distance starting from a parallel orientation. The algorithm commands a turn away from the wall, which causes the ping distance to increase immediately, even though the actual distance hasn’t changed. This causes the algorithm to seriously overcorrect in one case, and seriously undercorrect in the other. Not good.

What I need is a way to compensate for the changes in ping distance caused by Wall-E2’s angular orientation with respect to the wall being tracked. If Wall-E2 is oriented parallel to the wall, then no correction is needed; if not,then a correction is required. Fortunately for the good guys, Wall-E2 now has a way of providing the needed heading information, with the integration of the MPU6050-based 6DOF IMU module described in this post from last September.

To investigate this idea, I modified an old test program to have Wall-E2 perform a series of mild S-turns in my test hallway while capturing heading and ping distance data. The S-turns were tweaked so that Wall-E2 stayed a fairly constant 50 cm from the right-hand wall, as shown in the following movie clip.

Start of test area showing tape measure for offset distance measurement

Using Excel, I plotted the reported ping distance, the commanded heading, and the actual heading versus time, as shown below:

In the above plot, the initial CCW turn (away from the wall) was a 10° change, and all the rest were approximately 20° to maintain a more-or-less straight line. At the end of the second (the first CW turn) and subsequent heading changes, there is an approximately 0.5 sec straight period, during which no data was captured. As can be seen, the ping distance (gray curve) goes up slightly as the first CCW turn starts, then levels off during the changeover from CCW to CW turns, and then precipitously declines as the CW turn sweeps the ping sensor toward the perpendicular point. Part of this decline is actual distance change caused by the 0.5 sec straight period that moves the robot toward the wall. After the next (CCW) heading change is commanded, the robot starts to turn away from the wall causing the ping distance to increase, but this is partially cancelled by the fact that the robot continues to travel toward the wall during the S-turn. As soon as the robot gets parallel to the wall, then the ping distance goes up quickly as the heading continues to change in a CCW direction. This behavior repeats for each S-turn until the end of the run.

As an exercise, I added another column to the spreadsheet – “perpendicular distance”, and set up a formula to compute the adjusted distance from the robot to the wall, using the recorded angular offset. This computation presumes that the robot started off parallel to the wall (confirmed via the video clip). The result is shown on the yellow line in the plot below:

Ping distance and heading vs time, with calculated perpendicular distance added

As can be seen from the above plot and video, the compensated distance looks like it might be a good match with the perpendicular distance estimated from the video. For instance, at 17 sec into the video, the robot has just finished the first clockwise turn and straight run, and is just starting the second counter-clockwise turn. At this point the robot is oriented parallel to the wall, and the ping distance and the perpendicular distance should match. The video shows that distance should be about 33-35 cm, and the recorded ping distance at this point is 36 cm. However, the calculated distance went directly from 45 cm at point 11 to 34 cm at point 12 and basically stayed at that value before changing rapidly from 34 to 45 over points 19 & 20. Again at 19 seconds into the video, the robot is approximately 42-44 cm from the wall and parallel to it; both the actual ping distance and the calculated perpendicular distance agree at this point at 45 cm – a close match to the estimate from the video.

So now the question is – can I use the calculated perpendicular wall distance to assist wall-following operations? A significant issue may be knowing when the robot is actually parallel to the wall, to establish a heading baseline for compensation calcs.

When is the robot parallel to the wall?

A unique feature of the point or points where the robot is parallel to the wall is that the ping distance and the calculated distance are equal. However, that’s a bit of ‘chicken and the egg’ as one has to know the robot is parallel in order to use an offset angle of 0 degrees for the compensation calc to work out. Since the heading information available from the MPU6050 IMU is only relative, the heading value for the parallel condition can be anything, and can vary arbitrarily from run to run. So, what to do? One thought would be to have the robot make a short S-turn at the start of any tracking run to establish the heading for which the ping distance goes through a minimum or maximum – the heading for the max/min point would be the parallel heading. From there on, that heading should be reliably constant until the next time the robot’s power is cycled. Of course, a new parallel heading value would be required each and every time Wall-E2’s tracking situation changes (obstacle recovery, step-turns and reversals at the end of a hallway, changing from the left wall to the right one, etc). Maybe a hybrid mode would be feasible, whereby the robot uses uncompensated heading-based S-turns instead of the current ‘bang-bang’ system for initial wall tracking, shifting to a compensation algorithm after a suitable parallel heading is determined.

Looking at the above plots, it may not be all that useful to look for maxima and/or minima, as there are multiple headings for which the ping distance is constant, so which one is the parallel heading? Thinking about ways to rapidly find the parallel heading, it occurred to me that my previous work on quickly finding the mathematical variance of a set of values might be useful here. I plugged the above ping distance numbers into the Excel spreadsheet I used before, and got the following plot of ping distance and 3-element running variance vs time.

So, looking at the above plot, it is encouraging that a 3-point running variance calculation shows near-zero values when the robot is most probably parallel or nearly parallel to the wall. Adding the heading information to the spreadsheet gives the plot shown below

and now it is clear that the large variance values are associated with the changes from one heading to another, and the low variance values are associated with the middle section of each linear heading change (S-turn) segment. If I further enhance the plot by putting the variance plot on a secondary scale and zooming in on the low variance sections, we get the plot shown below:

Variance scale modified to show 0-5 range only

In the above plot, the variance line is zoomed in to the 0-5 range only, emphasizing the 0-0.5 unit variance range. In this plot it can be seen that the variance actually has a distinct minimum very near zero at time points 7, 16, 22, 28-30, and 35-38. These time values correspond to robot heading values of 64, 61, 63, 61-67. and 70-65. Discarding the last set as bad data (this is where the robot literally ‘hit the wall’ at the end of the run), we can compute an approximate parallel heading value as the average of all these values, or the average of 64, 61, 63, 64 (average of 61-67) = 63 degrees. From the video we can see that the robot started out parallel to the wall, and the first heading reading was 62 degrees – a very good match to the calculated parallel heading value.

The next step, I think, is to run some more field tests against a wall, with wall-following and heading assist integrated into the code.

Frank

MPU6050 IMU Motor Noise Troubleshooting

3 Replies

Posted 24 July 2019

For a while now I’ve been investigating ways of improving the wall following performance of my autonomous wall-following robot Wall-E2. At the heart of the plan is the use of a MPU6050 IMU to sense relative angle changes of the robot so that changes in the distance to the nearest wall due only to the angle change itself can be compensated out, leaving only the actual offset distance to be used for tracking.

As the test vehicle for this project, I am using my old 2-motor robot, fitted with new Pololu 125:1 metal-geared DC motors and Adafruit DRV8871 motor drivers, as shown in the photo below.

2-motor test vehicle on left, Wall-E2 on right

The DFRobots MPU6050 IMU module is mounted on the green perfboard assembly near the right wheel of the 2-motor test robot, along with an Adafruit INA169 high-side current sensor and an HC-05 Bluetooth module used for remote programming and telemetry.

This worked great at first, but then I started experiencing anomalous behavior where the robot would lose track of the relative heading and start turning in circles. After some additional testing, I determined that this problem only occurred when the motors were running. It would work fine as long as the motors weren’t running, but since the robot had to move to do its job, not having the ability to run the motors was a real ‘buzz-kill’. I ran some experiments on the bench to demonstrate the problem, as shown in the Excel plots below:

Troubleshooting:

There were a number of possibilities for the observed behavior:

The extra computing load required to run the motors was causing heading sensor readings to get missed (not likely, but…)
Motor noise of some sort was feeding back into the power & ground lines
RFI created by the motors was getting into the MPU6050 interrupt line to the Arduino Mega and causing interrupt processing to overwhelm the Mega
RFI created by the motors was interfering with I2C communications between the Mega and the MPU6050
Something else

Extra Computing Load:

This one was pretty easy to eliminate. The main loop does nothing most of the time, and only updates system parameters every 200 mSec. If the extra computing load was the problem, I would expect to see no ‘dead time’ between adjacent adjustment function blocks. I had some debug printing code in the program that displayed the result of the ‘millis()’ function at various points in the program, and it was clear that there was still plenty of ‘dead time’ between each 200 mSec adjustment interval.

Motor noise feeding back into power/ground:

I poked around on the power lines with my O’scope with the motors running and not running, but didn’t find anything spectacular; there was definitely some noise, but IMHO not enough to cause the problems I was seeing. So, in an effort to completely eliminate this possibility, I removed the perfboard sub-module from the robot entirely, and connected it to a separate Mega microcontroller. Since this setup used completely different power circuits (the onboard battery for the robot, PC USB cable for the second Mega), power line feedback could not possibly be a factor. With this setup I was able to demonstrate that the MPU6050 output was accurate and reasonable until I placed the perfboard sub-module in close proximity to the robot; then it started acting up just as it did when mounted on the robot.

So it was clear that the interference is RFI, not conducted through any wiring.

RFI created by the motors was getting into the MPU6050 interrupt line to the Arduino Mega and causing interrupt processing to overwhelm the Mega

This one seemed very possible. The MPU6050 generates interrupts at a 20Hz rate, but I only use measurements at a 5Hz (200mSec) rate. Each interrupt causes the Interrupt Service Routine (ISR) to fire, but the actual heading measurement only occurs every 200 mSec. I reasoned that if motor-generated RFI was causing the issue, I should see many more activations of the ISR than could be explained by the 20Hz MPU6050 interrupt generation rate. To test this theory, I placed code in the ISR that pulsed a digital output pin, and then monitored this pin with my O’scope. When I did this, I saw many extra ISR activations, and was convinced I had found the problem. In the following short video clip, the top trace is the normal interrupt line pulse frequency, and the bottom trace is the ISR-generated pulse train. In normal operation, these two traces would be identical, but as can be seen, many extra ISR activations are occurring when the motors are running.

So now I had to figure out what to do with this information. After Googling around for a while, I ran across some posts that described using the MPU6050/DMP setup without using the interrupt output line from the module; instead, the MPU6050 was polled whenever a new reading was required. As long as this polling takes place at a rate greater than the normal DMP measurement frequency, the DMP’s internal FIFO shouldn’t overflow. If the polling rate is less than the normal rate, then FIFO management is required. After thinking about this for a while, I realized I could easily poll the MPU/DMP at a higher rate than the configured 20Hz rate by simply polling it each time through the main loop – not waiting for the 200mSec/5Hz motor speed adjustment interval. I would simply poll the MPU/DMP as fast as possible, and whenever new data was ready I would pull it off the FIFO and put it into a global variable. The next time the motor adjustment function ran, it would use the latest relative heading value and everyone would be happy.

So, I implemented this change and tested it off the robot, and everything worked OK, as shown in the following Excel plot.

And then I put it on the robot and ran the motors

Crap! I was back to the same problem! So, although I had found evidence that the motor RFI was causing additional ISP activations, that clearly wasn’t the entire problem, as the polling method completely eliminates the ISP.

RFI created by the motors was interfering with I2C communications between the Mega and the MPU6050

I knew that the I2C control channel could experience corruption due to noise, especially with ‘weak’ pullup resistor values and long wire runs. However, I was using short (15cm) runs and 2.2K pullups on the MPU6050 end of the run, so I didn’t think that was an issue. However, since I now knew that the problem wasn’t related to wiring issues or ISR overload, this was the next item on the list. So, I shortened the I2C runs from 15cm to about 3cm, and found that this did indeed suppress (but not eliminate) the interference. However, even with this modification and with the MPU6050 module located as far away from the motors as possible, the interference was still present.

Something else

So, now I was down to the ‘something else’ item on my list, having run out of ideas for suppressing the interference. After letting this sit for a few days, I realized that I didn’t have this problem (or at least didn’t notice it) on my 4-motor Wall-E2 robot, so I started wondering about the differences between the two robot configurations.

Wall-E2 uses plastic-geared 120:1 ‘red cap’ motors, while the 2-motor robot uses pololu 125:1 metal-geared motors
Wall-E2 uses L298N linear drivers while the 2-motor version uses the Adafruit DRV8871 switching drivers.

So, I decided to see if I could isolate these two factors and see if it was the motors, or the drivers (or both/neither?) responsible for the interference. To do this, I used my new DPS5005 power supply to generate a 6V DC source, and connected the power supply directly to the motors, bypassing the drivers entirely. When I did this, all the interference went away! The motors aren’t causing the interference – it’s the drivers!

In the first plot above, I used a short (3cm) I2C wire pair and the module was located near, but not on, the robot. As can be seen, no interference occurred when the motors were run. In the second plot I used a long (15cm) I2C wire pair and mounted the module directly on the robot in its original position. Again, no interference when the motors were run.

So, at this point it was pretty definite that the main culprit in the MPU6050 interference issue is the Adafruit DRV8871 switch-mode driver. Switch-mode drivers are much more efficient than L298N linear-mode drivers, but the cost is high switching transients and debilitating interference to any I2C peripherals.

As an experiment, I tried reducing the cable length from the drivers to the motors, reasoning that the cables must be acting like antennae, and reducing their length should reduce the strength of the RFI. I re-positioned the drivers from the top surface of the robot to the bottom right next to the motors, thereby reducing the drive cable length from about 15cm to about 3 (a 5:1 reduction). Unfortunately, this did not significantly reduce the interference.

So, at this point I’m running out of ideas for eliminating the MPU6050 interference due to switch-mode driver use.

I read at least one post where the poster had eliminated motor interference by eliminating the I2C wiring entirely – he used a MPU6050 ‘shield’ where the I2C pins on the MPU6050 were connected directly to the I2C pins on the Arduino microcontroller. The poster didn’t mention what type of motor driver (L298N linear-mode style or DRV8871 switch-mode style), but apparently a (near) zero I2C cable length worked for him. Unfortunately this solution won’t work for me as Wall-E2 uses three different I2C-based sensors, all located well away from the microcontroller.
It’s also possible that the motors and drivers could be isolated from the rest of the robot by placing them in some sort of metal box that would shield the rest of the robot from the switching transients caused by the drivers. That seems a bit impractical, as it would require metal fabricating unavailable to me. OTOH, I might be able to print a plastic enclosure, and then cover it with metal foil of some sort. If I go this route, I might want to consider the use of optical isolators on the motor control lines, in order to break any conduction path back to the microcontroller, and capacitive feed-throughs for the power lines.

27 July 19 Update:

I received a new batch of GY-521 MPU6050 breakout boards, so I decided to try a few more experiments. With one of the GY-521 modules, I soldered the SCL/SDA header pins to the ‘bottom’ (non-label side) and the PWR/GND pins to the ‘top’. With this setup I was able to plug the module directly into the Mega’s SCL/SDA pins, thereby reducing the I2C cable length to zero. The idea was that if the I2C cable length was contributing significantly to RFI susceptibility, then a zero length cable should reduce this to the minimum possible, as shown below:

MPU6050 directly on Mega pins, normal length power wiring

In the photo above, the Mega with the MPU6050 connected is sitting atop the Mega that is running the motors. The GND and +5V leads are normal 15cm jumper wires. As shown in the plots below, this configuration did reduce the RFI susceptibility some, but not enough to allow normal operation when lying atop the robot’s Mega.

GY-521 MPU6050 module mounted directly onto Mega, normal length power leads

I was at least a little encouraged by this plot, as it showed that the MPU6050 (and/or the Mega) was recovering from the RFI ‘flooding’ more readily than before. In previous experiments, once the MPU6050/Mega lost sync, it never recovered.

Next I tried looping the power wiring around an ‘RF choke’ magnetic core to see if raising the effective impedance of the power wiring to high-frequency transients had any effect, as shown in the following photo.

GND & +5V leads looped through an RF Choke.

Unfortunately, as far as I could tell this had very little positive effect on RFI susceptibility.

Next I tried shortening the GND & +5V leads as much as possible. After looking at the Mega pinout diagram, I realized there was GND & +5V very close to the SCL/SDA pins, so I fabricated the shortest possible twisted-pair cable and installed it, as shown in the following photo.

MPU6050 directly on Mega pins, shortest possible length power wiring

With this configuration, I was actually able to get consistent readings from the MPU6050, whether or not the motors were running – yay!!

In the plot above, the vertical scale is only from -17 deg to -17.8 deg, so all the variation is due to the MPU6050, and there is no apparent deleterious effects due to motor RFI – yay!

So, at this point it’s pretty clear that a significant culprit in the MPU6050’s RFI susceptibility is the GND/+5V and I2C cabling acting as antennae and conducting the RFI into the MPU6050 module. Reducing the effective length of the antennas was effective in reducing the amount of RFI present on the module.

With the above in mind, I also tried adding a 0.01uF ‘chip’ capacitor directly at the power input leads, thinking this might be just as effective (if not more so) than shortening the power cabling. Unfortunately, this experiment was inconclusive. The normal length power cabling with the capacitor seemed to cause just as much trouble as the setup without the cap, as shown in the following plot.

Having determined that the best configuration so far was the zero-length I2C cable and the shortest possible GND/+5V cable, I decided to try moving the MPU6U6050 module from the separate test Mega to the robot’s Mega. This required moving the motor drive lines to different pins, but this was easily accomplished. Unfortunately, when I got everything together, it was apparent that the steps taken so far were not yet effective enough to prevent RFI problems due the switch-mode motor drivers

The good news, such as it is, is that the MPU6050/Mega seems to recover fairly quickly after each ‘bad data’ excursion, so maybe we are most of the way there!

As a next step, I plan to replace the current DRV8871 switch-mode motor drivers with a single L298N dual-motor linear driver, to see if my theory about the RFI problem being mostly due to the high-frequency transients generated by the drivers and not the motors themselves. If my theory holds water, replacing the drivers should eliminate (or at least significantly suppress) the RFI problems.

28 July 2019 Update:

So today I got the L298N driver version of the robot running, and I was happy (but not too surprised) to see that the MPU6050 can operate properly with the motors ON or OFF when mounted on the robot’s Mega controller, as shown in the following photo and Excel plots

2-motor robot with L298N motor driver installed.

However, there does still seem to be one ‘fly in the ointment’ left to consider. When I re-installed the wireless link to allow me to reprogram the 2-motor robot remotely and to receive wireless telemetry, I found that the MPU6050 exhibited an abnormally high yaw drift rate unless I allowed it to stabilize for about 10 sec after applying power and before the motors started running, as shown in the following plots.

2-motor robot with HC-05 wireless link re-installed.

I have no idea what is causing this behavior.

31 July 2019 Update

So, I found a couple of posts that refer to some sort of auto-calibration process that takes on the order of 10 seconds or so, and that sounds like what is happening with my project. I constructed the following routine that waited for the IMU yaw output values to settle

int numdots = 0;
while (!bMPU6050Settled && numvals < MAX_MPU6050_SETTLING_VALUES)
{
	uint8_t IntStat = mpu.getIntStatus();
	if (IntStat & 0x10)
	{
		mpu.resetFIFO();
	}
	else if (IntStat == 0x02)
	{
		mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);
		GetIMUHeadingDeg();

		//if necessary, initialize the yaw running average array
		if (numvals < HDG_RUNNING_AVG_ARRAY_SIZE)
		{
			aHeading[numvals] = global_yawval;
			//mySerial.printf("%lu: numvals = %d, yawval = %3.3f\n", millis(), numvals, global_yawval);
			mySerial.printf(".");
			numvals++;
		}
		else
		{
			UpdateHeadingArray();
			bMPU6050Settled = IsMPU6050Settled();
			//mySerial.printf("%2.2f\t%2.3f\t%2.3f\t%2.3f\n", millis() / 1000.f, global_yawval, delta_yaw, deg_per_sec);
			mySerial.printf(".");
			numdots++;
			if (numdots >= 3*numvals)
			{
				mySerial.printf("\n");
				numdots = 0;
			}
		}

	}
}

int numdots = 0;

while (!bMPU6050Settled && numvals < MAX_MPU6050_SETTLING_VALUES)

{

uint8_t IntStat = mpu.getIntStatus();

if (IntStat & 0x10)

{

mpu.resetFIFO();

}

else if (IntStat == 0x02)

{

mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);

GetIMUHeadingDeg();

//if necessary, initialize the yaw running average array

if (numvals < HDG_RUNNING_AVG_ARRAY_SIZE)

{

aHeading[numvals] = global_yawval;

//mySerial.printf("%lu: numvals = %d, yawval = %3.3f\n", millis(), numvals, global_yawval);

mySerial.printf(".");

numvals++;

}

else

{

UpdateHeadingArray();

bMPU6050Settled = IsMPU6050Settled();

//mySerial.printf("%2.2f\t%2.3f\t%2.3f\t%2.3f\n", millis() / 1000.f, global_yawval, delta_yaw, deg_per_sec);

mySerial.printf(".");

numdots++;

if (numdots >= 3*numvals)

{

mySerial.printf("\n");

numdots = 0;

}

This was very effective in determining when the MPU6050 output had settled, but it turned out to be unneeded for my application. I’m using the IMU output for relative yaw values only, and over a very short time frame (5-10 sec), so even high yaw drift rates aren’t deleterious. In addition, this condition only lasts for a 10-15 sec from startup, so not a big deal in any case.

At this point, the MPU6050 IMU on my little two-motor robot seems to be stable and robust, with the following adjustments (in no particular order of significance)

Changed out the motor drivers from 2ea switched-mode DRV8871 motor drivers to a single dual-channel L298N linear mode motor driver. This is probably the most significant change, without which none of the other changes would have been effective. This is a shame, as the voltage drop across the L298N is significantly higher than with the switch-mode types.
Shortened the I2C cable to zero length by plugging the GY-521 breakout board directly into the I2C pins on the Mega. This isn’t an issue on my 2-motor test bed, but will be on the bigger 4-motor robot
Shortened the IMU power cable from 12-15cm to about 3cm, and installed a 10V 1uF capacitor right at the PWR & GND pins on the IMU breakout board. Again, this was practical on my test robot, but might not be on my 4-motor robot.
Changed from an interrupt driven architecture to a polling architecture. This allowed me to remove the wire from the module to the Mega’s interrupt pin, thereby eliminating that possible RF path. In addition, I revised the code to be much stricter about using only valid packets from the IMU. Now the code first clears the FIFO, and then waits for a data ready signal from the IMU (available every 50 mSec at the rate I have it configured for). Once this signal is received, the code immediately reads a packet from the FIFO if and only if it contains exactly one packet (42 bytes in this configuration). The code shown below is the function that does all of this.

bool GetIMUHeadingDeg()
{
	//Purpose: Retrieve a heading value from the IMU
	//Inputs: None
	//Outputs: 
	//	returns true if successful, false otherwise
	//	if successful, updates global_yawval
	//Plan:
	//	Step1: reset the FIFO to clean out any garbage
	//	Step2: get valid DMP packet from FIFO if it contains exactly DMPpacketSize bytes
	//	Step3: compute yaw value from FIFO contents
	//Notes:
	//	07/31/19 rewritten to incorporate FIFO wait feature

//Step1: reset the FIFO to clean out any garbage
	mpu.resetFIFO(); 

//Step2: get valid DMP packet from FIFO if it contains exactly DMPpacketSize bytes
	int FIFOWaitLoops = 0; //loop termination counter
	bool bGotDmpPacket = false;
	while (!bGotDmpPacket && FIFOWaitLoops < MAX_FIFO_WAIT_LOOP_COUNT)
	{
		uint8_t IntStat = mpu.getIntStatus();
		if (IntStat & 0x10) //FIFO overflow
		{
			mpu.resetFIFO();
			//mySerial.printf("xxx %d\n",fifoCount);
		}
		else if (IntStat == 0x02) //data ready
		{
			fifoCount = mpu.getFIFOCount();
			if (fifoCount == DMPpacketSize)
			{
				//success... I hope!
				mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);
				bGotDmpPacket = true; //terminate the loop
			}
			else //try again
			{
				mpu.resetFIFO();
			}
		}

		FIFOWaitLoops++; //backup termination counter
	}

	//check for error exit
	if (FIFOWaitLoops >= MAX_FIFO_WAIT_LOOP_COUNT)
	{
		mySerial.printf("Init Hdg Loop timed out! %lu\t%d\t%3.2f\t%d\n",
			millis(), fifoCount, global_yawval, FIFOWaitLoops);
		return false;  //report the error back to calling pgm
	}

//Step3: compute yaw value from FIFO contents
	mpu.dmpGetQuaternion(&q, fifoBuffer);
	mpu.dmpGetGravity(&gravity, &q);
	mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
	global_yprzero = ypr[0];
	global_yawval = ypr[0] * 180 / M_PI;
	return true; //success!
}

bool GetIMUHeadingDeg()

{

//Purpose: Retrieve a heading value from the IMU

//Inputs: None

//Outputs:

// returns true if successful, false otherwise

// if successful, updates global_yawval

//Plan:

// Step1: reset the FIFO to clean out any garbage

// Step2: get valid DMP packet from FIFO if it contains exactly DMPpacketSize bytes

// Step3: compute yaw value from FIFO contents

//Notes:

// 07/31/19 rewritten to incorporate FIFO wait feature

//Step1: reset the FIFO to clean out any garbage

mpu.resetFIFO();

//Step2: get valid DMP packet from FIFO if it contains exactly DMPpacketSize bytes

int FIFOWaitLoops = 0; //loop termination counter

bool bGotDmpPacket = false;

while (!bGotDmpPacket && FIFOWaitLoops < MAX_FIFO_WAIT_LOOP_COUNT)

{

uint8_t IntStat = mpu.getIntStatus();

if (IntStat & 0x10) //FIFO overflow

{

mpu.resetFIFO();

//mySerial.printf("xxx %d\n",fifoCount);

}

else if (IntStat == 0x02) //data ready

{

fifoCount = mpu.getFIFOCount();

if (fifoCount == DMPpacketSize)

{

//success... I hope!

mpu.getFIFOBytes(fifoBuffer, DMPpacketSize);

bGotDmpPacket = true; //terminate the loop

}

else //try again

{

mpu.resetFIFO();

}

FIFOWaitLoops++; //backup termination counter

}

//check for error exit

if (FIFOWaitLoops >= MAX_FIFO_WAIT_LOOP_COUNT)

{

mySerial.printf("Init Hdg Loop timed out! %lu\t%d\t%3.2f\t%d\n",

millis(), fifoCount, global_yawval, FIFOWaitLoops);

return false; //report the error back to calling pgm

}

//Step3: compute yaw value from FIFO contents

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

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

global_yprzero = ypr[0];

global_yawval = ypr[0] * 180 / M_PI;

return true; //success!

}

Here’s a short video of the robot making some planned turns using the MPU6050 for turn management. In the video, the robot executes the following set of maneuvers:

Straight for 2 seconds
CW for 20 deg, starting an offset maneuver to the right
CCW for 20 deg, finishing the maneuver
CCW for 20 deg, starting an offset maneuver to the left
CW for 20 deg, finishing the maneuver
180 deg turn CW
Straight for 3 sec
20 deg turn CCW, finishing at the original start point

So, I think it’s pretty safe to say at this point that although both the DFRobots and GY-521 MPU6050 modules have some serious RFI/EMI problems, they can be made to be reasonably robust and reliable, at least with the L298N linear mode motor drivers. Maybe now that I have killed off this particular ‘alligator’, I can go back to ‘draining the swamp’ – i.e. using relative heading information to make better decisions during wall-following operations.

Stay tuned!

Frank

Arduino Remote Programming Using A HC-05 Bluetooth Module

7 Replies

Posted 10 June 2019

As part of my recent Wall-E2 Motor Controller Study, I reincarnated my old 2-motor robot as a test platform for Pololu’s ’20D’ metal gear motors. When I got the robot put together and started testing the motors, I realized I needed a way to remotely program the Arduino controller and remotely receive telemetry, just as I currently do with my 4-wheel Wall-E2 robot.

On my Wall-E2 robot, remote programming/telemetry is accomplished using the very nice Pololu Wixel Shield. However, I have been playing around with the cheap and small HC-05 Bluetooth module, and decided to see if there was maybe a way to use this module as a replacement for the Wixel.

As I usually do, I started with LOTS of web research. I found some posts claiming to have succeeded in remotely programming an Arduino using a HC-05 module, but the information was sketchy and incomplete, so I decided I would try and pull all the various sources together into a (hopefully) more complete tutorial for folks like me who want to use a HC-05 module for this purpose.

Overall Approach:

In order to remotely program an Arduino using a HC-05, the following basic parts are required:

A wireless link (obviously) between the PC and the HC-05.
A serial link between the PC and the Arduino and between the Arduino and the HC-05. This part is also well established, and the Arduino-to-HC-05 link can be done with either a hardware port (as with the Mega 2560) or a SoftwareSerial port using the SoftwareSerial library. My tutorial uses the Mega 2560, so I use Tx/Rx1 (pins 18/19) for the Arduino-to-HC-05 link
A way of resetting the Arduino to put it back into programming mode, so the new firmware can be uploaded.
A serial connection between the HC-05 and Tx/Rx0 on the microcontroller – more about this later.

The Wireless Link

The HC-05 is a generic Bluetooth device, and as such is compatible with just about everybody’s Bluetooth setup – phones and PC’s. I plan to use this with my Dell XPS15 9570 laptop, and I can pair with the HC-05 no problem. Here’s a link to a tutorial on pairing with the HC-05, and here’s another. As another poster mentioned, the pairing mechanism creates multiple ‘outgoing’ and ‘incoming’ COM ports, and it’s hard for me to figure out which to use. In this last iteration, I found that I could remove the two ‘incoming’ COM ports and use just the ‘outgoing’ one. Don’t know if that is the right thing, but….

A serial link between the PC, the Arduino and the HC-05

This part is discussed and demoed in many tutorials, but the piece that is almost always missing is why you need to have this link in the first place. The reason is that several AT commands must be used in order to configure the HC-05 correctly for wireless Arduino program upload, and (as I understand it anyway), AT commands can only be communicated to the HC-05 via it’s hardware serial lines, and only when the HC-05 is in ‘Command’ or ‘AT’ mode. The configuration step is a one-time deal; once the HC-05 is configured, it does not need to be done again unless the application requirements change.

A way of resetting the Arduino to accept firmware uploads

This is the tricky part. As ‘gabinix’ said in this post:

Quote from: gabenix on Feb 15, 2014, 02:18 pm

Hi Paul… To be honest I couldn’t find any tutorials to explain how to program/upload sketches with the HC-05. In fact, the conclusion you came up with is in-line with all the information out there. But it’s actually an extremely simple solution.
The only thing that keeps the HC-05 from uploading a program to arduino is that it doesn’t have a DTR (Data Terminal Ready) pin which tells the arduino to reset and accept a new sketch.
The solution is to re-purpose the “state” pin (PI09) on the breakout board. It’s purpose is to attach to an LED and indicate the connection status. It’s default setting is to send the pin HIGH when a connection is made, but you can simply enter into command mode of the HC-05 and use an AT COMMAND to tell it to send the pin LOW when a connection is made.
Voila! In about 1 minute of time you have successfully re-purposed the LED pin to a DTR pin which will reset your arduino to accept a new sketch when you hit the upload button.
A couple things to note… This will work for a pro-mini without additional hardware by connecting to the DTR pin. If you’re using an UNO or similar, you will need a capacitor in between our custom “state” pin and the reset pin on the uno. The reason is that the HC-05 will drive our custom pin LOW for the entire connection which would essentially be the same as holding the reset button the entire time. Having the cap in between solves that problem.
It a quick easy fix, takes about a minute to do. It’s just a lot harder to explain the steps to do it in a couple sentences.
Here’s a link to the AT COMMAND set —> http://robopoly.epfl.ch/files/content/sites/robopoly/files/Tutoriels/bluetooth/hc-05-at_command_set.pdf
and here’s a link to a tutorial, video, and sketch on how to enter the AT COMMANDS. —> ~~http://www.techbitar.com/modify-the-hc-05-bluetooth-module-defaults-using-at-commands.html~~ <<< no longer available 🙁

So, the trick is to re-purpose the STATE output (PI09, AKA Pin 32, AKA LED2, see this link) via the AT+POLAR(X,0) command to go LOW when the connection to upload the program is first started. This signal is then connected to the Arduino’s RESET pin via the capacitor noted above (to make this signal momentary). The ‘Instructables’ tutorial on this subject at this link actually gets most of this right, except it doesn’t explain why the AT commands are being entered or what they do – so I found it a bit mysterious. In addition, it recommends soldering a wire directly to pin 32 rather than re-purposing the STATE output pin (re-purposing the STATE pin allows a no-solder setup). Eventually I ran across this link which contains a very good explanation of the AT commands used by the HC-05. The required AT commands are:

AT+ORGL  // resets the HC-05 to its original default state. AT mode must be re-established after this command

AT+ROLE=0 // sets it to 'slave' mode (prob not necessary as this is the default, but...

AT+POLAR=1,0 // sets the LED2/Pin32/STATE 'Connection' output to active LOW.

AT+UART=115200,0,0 // changes HC-05 'Data' (Bluetooth) Mode baud rate from the default 9600 to 115,200/0/0. Does not affect wired 'Command' (AT) Mode

AT+INIT // Initialize SPP Profile.

AT+ORGL // resets the HC-05 to its original default state. AT mode must be re-established after this command

AT+ROLE=0 // sets it to 'slave' mode (prob not necessary as this is the default, but...

AT+POLAR=1,0 // sets the LED2/Pin32/STATE 'Connection' output to active LOW.

AT+UART=115200,0,0 // changes HC-05 'Data' (Bluetooth) Mode baud rate from the default 9600 to 115,200/0/0. Does not affect wired 'Command' (AT) Mode

AT+INIT // Initialize SPP Profile.

My module is the variety with a small pushbutton already installed on the ‘EN’ pin, so entering ‘Command’ mode is accomplished by holding the pushbutton depressed while cycling the power, and then releasing the button once power has been applied.

When this is done, the LED will change from fast-blink to a very slow (like 2 sec ON, 2 sec OFF) blink mode, as shown in the following short video:

This indicates the HC-05 is in ‘Command’ mode and will accept AT commands. If you have the style without the pushbutton, you’ll have to figure out a way to short across the pads where the pushbutton should be, while cycling the power.

The screenshot below shows the result of executing these commands using the wired USB connection to the Arduino and the hard-wired serial connection between the Mega’s Tx1/Rx1 port and the HC-05 running in ‘Command’ mode.

HC-05 configuration using the wired serial port connection to the HC-05

NOTE: The various posts and tutorials on the HC-05 describe separate AT ‘mini’ and ‘full’ command modes; the ‘mini’ mode only recognizes a small subset of all AT commands, while ‘full’ recognizes them all. ‘Mini’ mode is entered by momentarily applying VCC to pin 34, and ‘full’ mode is entered by holding pin 34 at VCC for the entire session. One poster described this as a flaw in the HC-05 version 2 firmware which might be corrected in later versions. It appears this may have been the case, as the HC-05 module I used responded with VERSION:3.0-20170601 and recognized all the commands I gave it (not a comprehensive test, but enough to make me think this problem has gone away).

Wiring Layout for HC-05 Configuration via AT commands

I decided that this post was my chance to learn how to make ‘pictoral’ wiring diagrams using the Fritzing app. I had seen other posts with this kind of layout, and initially thought it was kinda childish. However, when I started working with Fritzing (in English, ‘Fritzing’ sounds like an adverb, not a proper noun – so a bit strange to my ears…), I realized it has a LOT of power, so now I’m a convert ;-).

HC-05 wired for initial configuration using AT commands

In the diagram above, I’m using the Rx1/Tx1 (pins 19/18) hardware serial port available on the Mega. If you are using a Uno, you’ll need to use SoftwareSerial to configure a second port for connection to the HC-05. A 2.2K/1.0K voltage divider is used to drop Arduino Tx output voltages to HC-05 Rx input levels, but no conversion is required in the other direction. The HC-05 can be powered directly from Arduino +5V, as the HC-05 has an onboard regulator.

Initial AT Configuration Arduino Sketch

void setup()
{
	Serial.begin(115200);
	Serial1.begin(38400);
}

void loop()
{
	if (Serial1.available())
	{
		Serial.write(Serial1.read());
	}
	if (Serial.available())
	{
		Serial1.write(Serial.read());
	}
}

void setup()

{

Serial.begin(115200);

Serial1.begin(38400);

}

void loop()

{

if (Serial1.available())

{

Serial.write(Serial1.read());

}

if (Serial.available())

{

Serial1.write(Serial.read());

}

All the code above does is transfer keystrokes from the Arduino to the HC-05, and vice versa. This is all that is required to configure the HC-05 using AT commands.

Serial Connection between the HC-05 and Tx/Rx0 for Program Uploads

Most Arduino microcontrollers are shipped with a small program called a ‘bootloader’ already installed. This small program is only active for a few seconds after a board reset occurs, and it’s job is to detect when a new program is being uploaded. If the bootloader sees activity on whatever serial port it is watching, it writes the incoming data into program memory and then transfers control to the user program. The stock Arduino bootloader only monitors Tx/Rx0 for this; activity on other ports (specifically Rx1 in my case) will be ignored and program uploads will fail. After the HC-05 has been initially configured via AT commands over the PC-to-Arduino-to-HC-05 serial links, the connection from the HC-05 to the Arduino must be changed so that PC-to-HC-05 data transferred over the Bluetooth link arrives at the Arduino’s Rx0 port so the stock bootloader will see it and write it to the Arduino’s program memory. This minor point wasn’t at all clear (at least not to me) in the various tutorials, so I wasted a LOT of time trying to figure out why I couldn’t get the last part of the puzzle to fit – ugh!

Shown below is my Fritzing diagram for the final configuration of my test setup, showing the Tx/Rx lines changed from Tx/Rx1 (pins 18/19) to Tx/Rx0 (pins 1/0). The HC-05 STATE output is connected to Arduino reset via a 0.22uF capacitor, with resistors to form a simple one-shot circuit. The STATE line goes LOW (after reconfiguration via the AT+POLAR=1,0 command) which causes a momentary LOW on the Arduino reset line. This is the magic required to upload programs to the Arduino wirelessly. When the Bluetooth connection is terminated, the STATE line goes HIGH again and the Arduino end of the now-charged capacitor jumps to well above 5V. The diode shown on the diagram clamps this signal to within a volt or so above +5V to avoid damage to the Arduino Reset line when this happens. This diode isn’t shown on any of the other tutorials I found, so it is possible the Arduino Reset line is clamped internally (good). It’s also possible it isn’t protected, in which case not having this diode will eventually kill the Arduino (bad).

HC-05 wired for remote program upload. Note that the Tx & Rx lines have been moved from Tx/Rx1 to Tx/Rx0

Testing

The first thing I did after configuring the HC-05 (using the above AT commands) was to see if I could still connect to and communicate with it over Bluetooth from my laptop. I used RealTerm, although any terminal program (including the Arduino IDE serial monitor) should do. The very first thing that happened is I had to re-pair the laptop with the HC-05, and the name given by the HC-05 was markedly different, as shown in the captured pairing dialog.

Pairing dialog on my Dell XPS15 9570 laptop

The next thing was to see if I could get characters from my BT serial connection through to my Arduino serial port. After fiddling around with the baud rates for a while, I realized that now I had to change the BT serial terminal baud rate from 9600 to 115200, and the Arduino-to-HC-05 baud rate from 38400 (the default ‘Command’ mode rate) to 115200. Once I did this, I could transmit characters back and forth between RealTerm (connected to the HC-05 via Bluetooth) and my Visual Studio/Visual Micro setup (connected to Arduino via the wired USB cable) – yay!

For the next step in the testing, I need to remove the hard-wired USB connection and power the Arduino from an external power source. When I did this by first removing the USB connector (thereby removing power from the HC-05) and then plugged in external power, I noticed that the HC-05 was no longer connected to my laptop (the HC-05 status LED was showing the ‘fast blink’ status, and my connection indicator LED was OFF). I checked in my BT settings panel, and the HC-05 (now announcing itself as ‘H-C-2010-06-01’) was still paired with my laptop, but just transmitting some characters from my RealTerm BT serial monitor did not re-establish the connection. However, when I changed the port number away from and then back to the BT COM port, this did re-establish the connection; the HC-05 status LED changed to the 2-blinks-pause-2-blinks cycle, and my connection LED illuminated.

So, now I connected the output of my STATUS line one-shot circuit to the Arduino reset line and changed my VS2017/VM programming port from the wired USB port to the BT port (interestingly it was still shown as ‘HC-05’ in Visual Studio/Visual Micro). After some initial problems, I got the ‘Connected’ status light, but the upload failed with the error message “avrdude: stk500v2_getsync(): timeout communicating with programmer” and the communication status changed back to ‘not connected’.

At this point I realized I was missing something critical, and yelled (more like ‘pleaded’) for help on the Arduino forum. On the forum I got a lot of detailed feedback from very knowledgeable users, most notably ‘dmjlambert’. Unfortunately dmjlambert was ultimately unsuccessful in solving the problem, but he was able to validate that the steps I had taken so far were correct as far as they went, and ‘it should just work’. To paraphrase the Edison approach to innovation, “we didn’t know what worked, but we eliminated most potential failure modes”. See this forum post for the details.

After this conversation (over several days), I decided to put the problem down for a few days and do other things, hoping that a fresh look at things with a clear head might provide some insight. A few days later when I came back to the project, I ran some tests suggested by dmjlambert to verify that the connection to the Arduino RESET pin via the 0.22uF capacitor did indeed reset the Arduino when the STATE line transitioned from HIGH to LOW. To do this I created a modified ‘Blink’ program that blinked 10 times rapidly and then transitioned to a steady slow blink. Using this program I could see that that the Arduino did indeed reset each time a Bluetooth connection to the HC-05 was established.

So, the problem had to be elsewhere, and about this time I realized I was assuming (aka ‘making an ass out of you and me’) that the program upload data being received over the Bluetooth link was somehow magically making it to the bootloader program. This had been nagging at me the whole time, but I ‘assumed’ (there’s that word again) that since this problem had never been mentioned in any of the tutorials or even in the responses to my forum posts, it must not be a problem – oops!

Anyway, to make a long story short, I moved the HC-05 – to – Arduino connection from Rx/Tx1 to Rx/Tx0 and program uploads started working immediately – YAY!!

I went back through the tutorials I had been following to see if I had missed this magic step, and didn’t find any references to moving the serial connection at all. So, if you are doing this with a UNO, you’ll need to move the serial connection from whatever pins you were using (via SoftwareSerial) to Rx/Tx0 as the last step. If you are using an Arduino Mega or other uino controller that supports additional hardware serial ports as I did, you’ll have to move the connection from Rx/Tx-whatever to Rx/Tx0 as the last step.

This tutorial was put together in the hope that I could maybe help others who are interested in using the HC-05 Bluetooth module for remote program uploads to a Arduino-compatible microcontroller, and maybe save them from some of the frustration I experienced. Please feel free to comment on this post, especially if you see something that I got wrong or missed.

13 Aug 2019 Update:

Here’s a short video showcasing the ability to program an Arduino Mega 2560 wirelessly from my Windows 10 PC using the HC-05 Bluetooth module

At the start of the video, the HC-05 status light is blinking rapidly, signalling the ‘No Connection’ state. Then, at about 2 seconds, the light changes to the slow double-blink ‘Connected’ state, the yellow LED on the Mega blinks OFF & then ON again, signalling that the Mega has been reset and is now awaiting program upload, followed immediately by rapid blinking as the new program is uploaded to the Mega’s program memory. During the upload, the HC-05 status LED continues to show the slow double-blink ‘Connected’ status. Then, at about 18 seconds, the program upload terminates and the HC-05 returns to the ‘No Connection’ state.

The small white part on the green perf-board is the 220 nF capacitor. The other two modules on the perf-board are a MPU6050 IMU and a high-side current sensor.

Stay tuned!

Frank

25 October 2021 Update:

I came back to this post to refresh my memory when trying to initialize and use a new HC-05 module for my new Wall-E3 project, and failing badly. I finally got something to work, but only after screwing around a lot. I realized I didn’t have a good handle on what mode the HC-05 was in – even though the onboard LED changes behavior to indicate the mode. So, here is a short video showing the LED behavior for the ‘disconnected’ and ‘connected’ modes.

HC-05 LED indications for ‘connected’ and ‘disconnected’ modes

In the above video, the HC-05 starts out in the normal power-on ‘disconnected’ state (rapidly flashing LED). Then after a few seconds a BT connection is established, and the LED behavior changes to ‘connected’ (two short blinks and a long pause). Then after a few more seconds the connection is dropped and the LED behavior changes back to ‘disconnected’ (rapidly flashing)