July | 2018 | Paynter's Palace

Posted 12 July 2018

In the last installment of this particular saga, I described another chapter in my ongoing effort to add heading knowledge to Wall-E2’s (my autonomous wall-following robot) super powers. In that post, I described my attempt to utilize the Inversense IMU 6050 6DOF breakout board from DFRobots. I posted some results that showed problems with vibration screwing up the results, and then getting error-free results using an ‘air pillow’ (a piece of air-filled packing material). At the time, this led me to believe that the cause of the bad data was motor vibration. However, when I tried adding some foam vibration dampening material, it didn’t seem to help – I was still getting intermittent stretches of bad data, with or without the motors running. Clearly I still didn’t understand what was happening.

Once again I ran away from the whole thing, to regroup and let my mind work on the problem for a while; back to basketball, bridge, and general goofing off.

After some more web research and just thinking about what I knew and didn’t know, I started to suspect that what I was seeing was an artifact of the way the sensor(s) communicated with the main controller via the I2C serial interface. When yaw measurements went bad, they went really bad, rapidly cycling from positive to negative values, and this didn’t make a lot of sense. Maybe the main controller wasn’t keeping up with the sensor data stream, and the software was trying to form heading values using bits from two different measurements; this would explain why the heading sign changed from measurement to measurement. Also, I wasn’t utilizing the INT pin on the IMU6050 module, just pulling data out of the FIFO as rapidly as possible; could that be part of the problem too?

So, I decided to start all over again with the IMU6050 sensor on an ASP plugboard, with a spare Arduino Mega 2560 controller identical to the one being used to run Wall-E2, as shown in the following photo. I also hooked up the INT pin, and used Jeff Rowberg’s I2CDev materials and MPU6050 example programs as the starting point.

DFRobots Inversense IMU6050 breakout board (board with blue LED, between FRAM and RTC) on an ASP plugboard, controlled by an Arduino Mega 2560

After getting everything going, I ran some long-term tests to see I could produce ‘bad’ yaw readings independent of the robot platform. And, of course, I couldn’t get the darned thing to fail, no matter how long I ran it. Shown below is a 20-minute long plot

20-minute run with no observed problems

Next, I tried inserting some delays into the ‘do other stuff’ part of the main loop, to try and simulate the normal robot processing delays. This had no effect up until the delay reached 40mSec or so, and then I started to see problems very similar to what I had seen before with both the MPU9250 and 6050 sensor setups.

On robot test displaying yaw value and bytes remaining in MPU6050 FIFO

Then I modified the code again to check for FIFO byte lengths that weren’t an integral multiple of the normal packet length (42 in this case), and to reset the FIFO if the condition was detected. This seemed to eliminate the ‘bad data’ effect, regardless of the amount of delay introduced in the processing portion of loop().

About 6 minutes with the motors running. Program modified to reset the FIFO whenever a non-modulo ‘bytes remaining’ condition was detected

Detail view of the last 100 seconds of the previous plot

Summary:

The Invensense MPU6050/DMP/FIFO combination is sensitive to delays in the main processing loop, even when using the INT line with an Interrupt Service Routine (ISR). When the main loop processing delays get beyond about 40mSec, the ‘mpu.getFIFOBytes(fifoBuffer, packetSize);’ call will occasionally not remove the correct number of bytes from the FIFO, leaving a non-modulo (packetsize) number of bytes remaining in the FIFO. When this happens, the next read will get (and process) bytes from two different packets, resulting in wildly varying yaw value outputs. This condition is (now, after knowing what’s going on) fairly easy to recognize, as the error condition generally causes adjacent yaw values to have different signs, resulting in a classic sawtooth output.

The way to eliminate this artifact is to check for non-modulo (packetsize) FIFO bytes remaining value each time, and reset the FIFO when this happens. Whatever good data is still in the FIFO will be lost, but the data that you do get will be valid.

I have included below my test program, with the FIFO modulo check and FIFO reset mechanism. Note that this program also includes my motor control code, which obviously will not work with your setup.

/*
    Name:       Arduino_IMU6050_Test3.ino
    Created:	6/22/2018 9:25:22 PM
    Author:     FPWIN7LAPTOP\Frank
*/
#include<elapsedMillis/elapsedMillis.h>
#include <PrintEx.h>
StreamEx mySerial = Serial;

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

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

// 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); //06/23/18 chg to AD0 high addr

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

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

//07/05/18 copied from FourWD_WallE2.ino
//02/29/16 hardware defines
#define NO_MOTORS

#pragma region MOTORCONSTS
//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 = 40; //chg to 40 at 5:55pm
const int LEFT_SPEED_COMP_VAL_FWD = 15; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_FWD = -LEFT_SPEED_COMP_VAL_FWD; //right speed compensation value
const int LEFT_SPEED_COMP_VAL_REV = 5; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_REV = -LEFT_SPEED_COMP_VAL_REV; //right speed compensation value

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

//drive wheel rotation constants - used for calculating time required to rotate X degrees
const double DRIVE_WHEEL_FULL_SPEED_RPM = 200.0; //at MOTOR_SPEED_FULL - rev 05/20/19
const double DRIVE_WHEEL_DIAM_MM = 66.0;
const double DriveWheelSpdMMPerSec = PI * DRIVE_WHEEL_DIAM_MM * DRIVE_WHEEL_FULL_SPEED_RPM / 60;
const double DriveWheelDegPerSec = 360 * DRIVE_WHEEL_FULL_SPEED_RPM / 60.0;
const int NUM_DEG_FOR_90_DEG_TURN = 90; //replace 'magic numbers' with program const
const int MOTOR_RAMPUP_MSEC = 500; //added 05/20/17 - used in RotateCW()

								   //Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;
#pragma endregion Motor Parameters


//vvvvvvvvvvvvvvvvvvv  PIN ASSIGNMENTS vvvvvvvvvvvvvvvvvvv//

#pragma region MOTOR_PINS
//Right Motors
int enA_R = 46;
int enB_R = 44;
int in1_R = 42;
int in2_R = 40;

int in3_R = 38;
int in4_R = 36;

// left motors
int enA_L = 8;
int in1_L = 9;
int in2_L = 10;

int in3_L = 11;
int in4_L = 12;
int enB_L = 13;
#pragma endregion Motor Pin Assignments

#pragma region LOOP_VARS
int leftspeednum = MOTOR_SPEED_HALF;
int rightspeednum = MOTOR_SPEED_HALF;

//02/13/16 added for 'pause' debug
int m_FinalLeftSpeed = 0;
int m_FinalRightSpeed = 0;
#pragma endregion Loop Variables

elapsedMillis mSecSinceLastReset;
elapsedMillis mSecSinceLastHeartbeat;
const int _6050_RESET_INTERVAL_MSEC = 1000;
const int HEARTBEAT_INTERVAL_MSEC = 250;
const int MAX_AD_VALUE = 1023;
const float ADC_REF_VOLTS = 5.0; //03/27/18 now using analogReference(EXTERNAL) with Teensy 3.3V connected to AREF 

const int HEARTBEAT_LED = 35;

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

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

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

void setup() 
{
	// join I2C bus (I2Cdev library doesn't do this automatically)
	Wire.begin();

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

	//07/06/18 copied from FourWD_WallE2.ino

	//12/06/15 set up right motor pins as outputs
	pinMode(enA_R, OUTPUT);
	pinMode(enB_R, OUTPUT);
	pinMode(in1_R, OUTPUT);
	pinMode(in2_R, OUTPUT);

	//12/06/15 set up left motor pins as outputs
	pinMode(enA_L, OUTPUT);
	pinMode(enB_L, OUTPUT);
	pinMode(in1_L, OUTPUT);
	pinMode(in2_L, OUTPUT);

	//heartbeat LED
	pinMode(HEARTBEAT_LED, OUTPUT);
	pinMode(HEARTBEAT_LED-2, OUTPUT);


	// initialize device
	Serial.println(F("Initializing I2C devices..."));
	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);

		// enable Arduino interrupt detection
		Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
		attachInterrupt(0, 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(")"));
	}


	//07/06/18 copied from FourWD_WallE2.ino
	//mySerial.printf("initializing motors\n");

	//10/08/17 make sure motors control lines are all in STOP state
	StopLeftMotors();
	StopRightMotors();

	//01/26/15 start the robot going straight
	//MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);
	MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_HALF);
	mySerial.printf("Setup Accomplished - entering loop()\n");

	mSecSinceLastReset = 0; //should be last statement in Setup()
}

void dmpDataReady()
{
	mpuInterrupt = true;
}


// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
long whilecount = 0;
int resetcount = 0;

void loop() 
{
	//visible heartbeat added 07/15/18 (can't use pin 13 - its a motor control pin)
	if (mSecSinceLastHeartbeat >= HEARTBEAT_INTERVAL_MSEC)
	{
		mSecSinceLastHeartbeat -= HEARTBEAT_INTERVAL_MSEC;
		bool ledState = digitalRead(HEARTBEAT_LED);
		digitalWrite(HEARTBEAT_LED, !ledState);
	}

	// if programming failed, don't try to do anything
	if (!dmpReady) return;


	// wait for MPU interrupt or extra packet(s) available
	while (!mpuInterrupt && fifoCount < packetSize) 
	{
		whilecount++;

		bool readval = digitalRead(HEARTBEAT_LED - 2);
		digitalWrite(HEARTBEAT_LED - 2, !readval);
		// other program behavior stuff here
		// .
		// .
		// .
		// if you are really paranoid you can frequently test in between other
		// stuff to see if mpuInterrupt is true, and if so, "break;" from the
		// while() loop to immediately process the MPU data
		// .
		// .
		// .

		//10/08/17 make sure motors control lines are all in STOP state
		//StopLeftMotors();
		//StopRightMotors();
		//if (mpuInterrupt)
		//{
		//	mySerial.printf("break 1\n");
		//	break;
		//}

		//fwd 1 sec
		//SetLeftMotorDir(FWD_DIR);
		//SetRightMotorDir(FWD_DIR);

		//if (mpuInterrupt)
		//{
		//	mySerial.printf("break 2\n");
		//	break;
		//}

		//delay(50);
		//delay(50);
		//delay(50);
		//delay(100);
		//delay(10);
		//delay(10);
		//delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		delay(10);
		//01/26/15 start the robot going straight
		//MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

		if (mpuInterrupt)
		{
			fifoCount = mpu.getFIFOCount();
			//mySerial.printf("%ld\t%d\n", whilecount, fifoCount);
			whilecount = 0;
			break;
		}



	}

	// reset interrupt flag and get INT_STATUS byte
	mpuIntStatus = mpu.getIntStatus();

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

	// check for overflow (this should never happen unless our code is too inefficient)
	if ((mpuIntStatus & 0x10) || 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 & 0x02)
	{
		// wait for correct available data length, should be a VERY short wait
		while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

		//// read a packet from FIFO
		//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;


		//07/08/18 added to watch for non-modulo FIFO counts
		if (fifoCount == 0 || fifoCount%packetSize != 0)
		{
			mpu.resetFIFO();
			resetcount++;
		}
		else
		{
			//07/07/18 modified to read all outstanding packets
			// read a packet from FIFO
			while (fifoCount >= packetSize)
			{
				mpu.getFIFOBytes(fifoBuffer, packetSize);
				fifoCount -= packetSize;
			}

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

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

	if (mpuInterrupt)
	{
		mpuInterrupt = false; //reset for next time
		long msec = millis();
		float hours = (float)msec / 3600000.;
		float volts = GetVolts();
		//mySerial.printf("%ld\t%3.2f V\t%3.2f\t%d\t%d\n", msec, volts, yawval, fifoCount, resetcount);
		mySerial.printf("%2.2f\t%3.2f V\t%3.2f\t%d\t%d\n", hours, volts, yawval, fifoCount, resetcount);
	}
}

float GetVolts()
{
	int voltreading = analogRead(A1);
	float v = ((float)voltreading / MAX_AD_VALUE)*ADC_REF_VOLTS;
	return v;
}

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

void StopLeftMotors()
{
	//10/08/17 added analogWrite calls to ensure speed set to zero
	analogWrite(enA_L, MOTOR_SPEED_OFF);
	analogWrite(enB_L, MOTOR_SPEED_OFF);

	digitalWrite(in1_L, LOW);
	digitalWrite(in2_L, LOW);
	digitalWrite(in3_L, LOW);
	digitalWrite(in4_L, LOW);
}

void StopRightMotors()
{
	//10/08/17 added analogWrite calls to ensure speed set to zero
	analogWrite(enA_R, MOTOR_SPEED_OFF);
	analogWrite(enB_R, MOTOR_SPEED_OFF);

	digitalWrite(in1_R, LOW);
	digitalWrite(in2_R, LOW);
	digitalWrite(in3_R, LOW);
	digitalWrite(in4_R, 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
	//01/24/15 - removed timing - now continuous run
	RunBothMotors(leftspeednum, rightspeednum);
}



//
////12/26/14 added for independent motor speed
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:
	//    Step 1: Apply drive to both wheels
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//    01/24/15 - revised for continuous run - no timing
	//  01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()
	//	12/07/15 - chg args from &int to int
	//Step 1: Apply drive to both wheels

	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);
}
//11/25/15 added to utilize new private member variable bLeftMotorDirIsFwd
void SetLeftMotorSpeed(int speed)
{
	//if (bLeftMotorDirIsFwd)
	//if (CurrentOpMode != MODE_IRHOMING) //10/13/17 rev to bypass comp in IR homing mode
	{
		if (bLeftMotorDirIsFwd)
		{
			speed = speed + LEFT_SPEED_COMP_VAL_FWD;
		}
		else //must be REV
		{
			speed = speed + LEFT_SPEED_COMP_VAL_REV;
		}
	}

	//12/12/15 added here to make sure speed vals always within valid ranges
	//12/20/15 chg from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (255)
	speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
	speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

#ifndef NO_MOTORS
	analogWrite(enA_L, speed);
	analogWrite(enB_L, speed);
#endif

	//02/13/16 added for 'pause' debug
	m_FinalLeftSpeed = speed;
	//Serial.print(", Left Speed = "); Serial.println(speed);
}

//11/25/15 added to utilize new private member variable bRightMotorDirIsFwd
void SetRightMotorSpeed(int speed)
{
	//Serial.print("SetRightMotorSpeed: "); Serial.print("Input = "); Serial.print(speed);

	//if (bRightMotorDirIsFwd)
	//if (CurrentOpMode != MODE_IRHOMING) //10/13/17 rev to bypass comp in IR homing mode
	{
		if (bRightMotorDirIsFwd)
		{
			speed = speed + RIGHT_SPEED_COMP_VAL_FWD;
		}
		else //must be REV
		{
			speed = speed + RIGHT_SPEED_COMP_VAL_REV;
		}
	}

	//12/20/15 chg from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (2550
	speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
	speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

#ifndef NO_MOTORS
	analogWrite(enA_R, speed);
	analogWrite(enB_R, speed);
#endif

	//02/13/16 added for 'pause' debug
	m_FinalRightSpeed = speed;
	//Serial.print(", Right Speed = "); Serial.println(speed);
}


void SetLeftMotorDir(boolean bIsFwdDir)
{
	if (bIsFwdDir)
	{
		digitalWrite(in1_L, HIGH);
		digitalWrite(in2_L, LOW);
		digitalWrite(in3_L, HIGH);
		digitalWrite(in4_L, LOW);

	}
	else //must be REV
	{
		digitalWrite(in1_L, LOW);
		digitalWrite(in2_L, HIGH);
		digitalWrite(in3_L, LOW);
		digitalWrite(in4_L, HIGH);
	}

	//added 11/25/15
	bLeftMotorDirIsFwd = bIsFwdDir;
}

void SetRightMotorDir(boolean bIsFwdDir)
{
	if (bIsFwdDir)
	{
		digitalWrite(in1_R, HIGH);
		digitalWrite(in2_R, LOW);
		digitalWrite(in3_R, HIGH);
		digitalWrite(in4_R, LOW);

	}
	else //must be REV
	{
		digitalWrite(in1_R, LOW);
		digitalWrite(in2_R, HIGH);
		digitalWrite(in3_R, LOW);
		digitalWrite(in4_R, HIGH);
	}
http://gfpbridge.com/wp-admin/post.php?post=3212&action=edit#
	//added 11/25/15
	bRightMotorDirIsFwd = bIsFwdDir;
}

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

Name: Arduino_IMU6050_Test3.ino

Created: 6/22/2018 9:25:22 PM

Author: FPWIN7LAPTOP\Frank

#include<elapsedMillis/elapsedMillis.h>

#include <PrintEx.h>

StreamEx mySerial = Serial;

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)

// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>

// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation

// is used in I2Cdev.h

#include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files

// for both classes must be in the include path of your project

#include "I2Cdev.h"

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

// 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); //06/23/18 chg to AD0 high addr

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

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.

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

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

//07/05/18 copied from FourWD_WallE2.ino

//02/29/16 hardware defines

#define NO_MOTORS

#pragma region MOTORCONSTS

//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 = 40; //chg to 40 at 5:55pm

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

const int RIGHT_SPEED_COMP_VAL_FWD = -LEFT_SPEED_COMP_VAL_FWD; //right speed compensation value

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

const int RIGHT_SPEED_COMP_VAL_REV = -LEFT_SPEED_COMP_VAL_REV; //right speed compensation value

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

//drive wheel rotation constants - used for calculating time required to rotate X degrees

const double DRIVE_WHEEL_FULL_SPEED_RPM = 200.0; //at MOTOR_SPEED_FULL - rev 05/20/19

const double DRIVE_WHEEL_DIAM_MM = 66.0;

const double DriveWheelSpdMMPerSec = PI * DRIVE_WHEEL_DIAM_MM * DRIVE_WHEEL_FULL_SPEED_RPM / 60;

const double DriveWheelDegPerSec = 360 * DRIVE_WHEEL_FULL_SPEED_RPM / 60.0;

const int NUM_DEG_FOR_90_DEG_TURN = 90; //replace 'magic numbers' with program const

const int MOTOR_RAMPUP_MSEC = 500; //added 05/20/17 - used in RotateCW()

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

#pragma endregion Motor Parameters

//vvvvvvvvvvvvvvvvvvv PIN ASSIGNMENTS vvvvvvvvvvvvvvvvvvv//

#pragma region MOTOR_PINS

//Right Motors

int enA_R = 46;

int enB_R = 44;

int in1_R = 42;

int in2_R = 40;

int in3_R = 38;

int in4_R = 36;

// left motors

int enA_L = 8;

int in1_L = 9;

int in2_L = 10;

int in3_L = 11;

int in4_L = 12;

int enB_L = 13;

#pragma endregion Motor Pin Assignments

#pragma region LOOP_VARS

int leftspeednum = MOTOR_SPEED_HALF;

int rightspeednum = MOTOR_SPEED_HALF;

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

int m_FinalLeftSpeed = 0;

int m_FinalRightSpeed = 0;

#pragma endregion Loop Variables

elapsedMillis mSecSinceLastReset;

elapsedMillis mSecSinceLastHeartbeat;

const int _6050_RESET_INTERVAL_MSEC = 1000;

const int HEARTBEAT_INTERVAL_MSEC = 250;

const int MAX_AD_VALUE = 1023;

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

const int HEARTBEAT_LED = 35;

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

// === INTERRUPT DETECTION ROUTINE ===

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

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

volatile float yawval = 0;

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

// === INITIAL SETUP ===

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

void setup()

{

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

Wire.begin();

// initialize serial communication

Serial.begin(115200);

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

//07/06/18 copied from FourWD_WallE2.ino

//12/06/15 set up right motor pins as outputs

pinMode(enA_R, OUTPUT);

pinMode(enB_R, OUTPUT);

pinMode(in1_R, OUTPUT);

pinMode(in2_R, OUTPUT);

//12/06/15 set up left motor pins as outputs

pinMode(enA_L, OUTPUT);

pinMode(enB_L, OUTPUT);

pinMode(in1_L, OUTPUT);

pinMode(in2_L, OUTPUT);

//heartbeat LED

pinMode(HEARTBEAT_LED, OUTPUT);

pinMode(HEARTBEAT_LED-2, OUTPUT);

// initialize device

Serial.println(F("Initializing I2C devices..."));

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

// enable Arduino interrupt detection

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

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

}

//07/06/18 copied from FourWD_WallE2.ino

//mySerial.printf("initializing motors\n");

//10/08/17 make sure motors control lines are all in STOP state

StopLeftMotors();

StopRightMotors();

//01/26/15 start the robot going straight

//MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_HALF);

mySerial.printf("Setup Accomplished - entering loop()\n");

mSecSinceLastReset = 0; //should be last statement in Setup()

}

void dmpDataReady()

{

mpuInterrupt = true;

}

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

// === MAIN PROGRAM LOOP ===

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

long whilecount = 0;

int resetcount = 0;

void loop()

{

//visible heartbeat added 07/15/18 (can't use pin 13 - its a motor control pin)

if (mSecSinceLastHeartbeat >= HEARTBEAT_INTERVAL_MSEC)

{

mSecSinceLastHeartbeat -= HEARTBEAT_INTERVAL_MSEC;

bool ledState = digitalRead(HEARTBEAT_LED);

digitalWrite(HEARTBEAT_LED, !ledState);

}

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

if (!dmpReady) return;

// wait for MPU interrupt or extra packet(s) available

while (!mpuInterrupt && fifoCount < packetSize)

{

whilecount++;

bool readval = digitalRead(HEARTBEAT_LED - 2);

digitalWrite(HEARTBEAT_LED - 2, !readval);

// other program behavior stuff here

// .

// if you are really paranoid you can frequently test in between other

// stuff to see if mpuInterrupt is true, and if so, "break;" from the

// while() loop to immediately process the MPU data

// .

//10/08/17 make sure motors control lines are all in STOP state

//StopLeftMotors();

//StopRightMotors();

//if (mpuInterrupt)

//{

// mySerial.printf("break 1\n");

// break;

//}

//fwd 1 sec

//SetLeftMotorDir(FWD_DIR);

//SetRightMotorDir(FWD_DIR);

//if (mpuInterrupt)

//{

// mySerial.printf("break 2\n");

// break;

//}

//delay(50);

//delay(100);

//delay(10);

delay(10);

//01/26/15 start the robot going straight

//MoveAhead(MOTOR_SPEED_LOW, MOTOR_SPEED_LOW);

if (mpuInterrupt)

{

fifoCount = mpu.getFIFOCount();

//mySerial.printf("%ld\t%d\n", whilecount, fifoCount);

whilecount = 0;

break;

}

// reset interrupt flag and get INT_STATUS byte

mpuIntStatus = mpu.getIntStatus();

// get current FIFO count

fifoCount = mpu.getFIFOCount();

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

if ((mpuIntStatus & 0x10) || 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 & 0x02)

{

// wait for correct available data length, should be a VERY short wait

while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

//// read a packet from FIFO

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

//07/08/18 added to watch for non-modulo FIFO counts

if (fifoCount == 0 || fifoCount%packetSize != 0)

{

mpu.resetFIFO();

resetcount++;

}

else

{

//07/07/18 modified to read all outstanding packets

// read a packet from FIFO

while (fifoCount >= packetSize)

{

mpu.getFIFOBytes(fifoBuffer, packetSize);

fifoCount -= packetSize;

}

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

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

yawval = ypr[0] * 180 / M_PI;

}

if (mpuInterrupt)

{

mpuInterrupt = false; //reset for next time

long msec = millis();

float hours = (float)msec / 3600000.;

float volts = GetVolts();

//mySerial.printf("%ld\t%3.2f V\t%3.2f\t%d\t%d\n", msec, volts, yawval, fifoCount, resetcount);

mySerial.printf("%2.2f\t%3.2f V\t%3.2f\t%d\t%d\n", hours, volts, yawval, fifoCount, resetcount);

}

float GetVolts()

{

int voltreading = analogRead(A1);

float v = ((float)voltreading / MAX_AD_VALUE)*ADC_REF_VOLTS;

return v;

}

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

void StopBothMotors()

{

StopLeftMotors();

StopRightMotors();

}

void StopLeftMotors()

{

//10/08/17 added analogWrite calls to ensure speed set to zero

analogWrite(enA_L, MOTOR_SPEED_OFF);

analogWrite(enB_L, MOTOR_SPEED_OFF);

digitalWrite(in1_L, LOW);

digitalWrite(in2_L, LOW);

digitalWrite(in3_L, LOW);

digitalWrite(in4_L, LOW);

}

void StopRightMotors()

{

//10/08/17 added analogWrite calls to ensure speed set to zero

analogWrite(enA_R, MOTOR_SPEED_OFF);

analogWrite(enB_R, MOTOR_SPEED_OFF);

digitalWrite(in1_R, LOW);

digitalWrite(in2_R, LOW);

digitalWrite(in3_R, LOW);

digitalWrite(in4_R, 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

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

RunBothMotors(leftspeednum, rightspeednum);

}

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

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:

// Step 1: Apply drive to both wheels

//Notes:

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

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

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

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

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

//Step 1: Apply drive to both wheels

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

}

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

void SetLeftMotorSpeed(int speed)

{

//if (bLeftMotorDirIsFwd)

//if (CurrentOpMode != MODE_IRHOMING) //10/13/17 rev to bypass comp in IR homing mode

{

if (bLeftMotorDirIsFwd)

{

speed = speed + LEFT_SPEED_COMP_VAL_FWD;

}

else //must be REV

{

speed = speed + LEFT_SPEED_COMP_VAL_REV;

}

//12/12/15 added here to make sure speed vals always within valid ranges

//12/20/15 chg from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (255)

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

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

#ifndef NO_MOTORS

analogWrite(enA_L, speed);

analogWrite(enB_L, speed);

#endif

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

m_FinalLeftSpeed = speed;

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

}

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

void SetRightMotorSpeed(int speed)

{

//Serial.print("SetRightMotorSpeed: "); Serial.print("Input = "); Serial.print(speed);

//if (bRightMotorDirIsFwd)

//if (CurrentOpMode != MODE_IRHOMING) //10/13/17 rev to bypass comp in IR homing mode

{

if (bRightMotorDirIsFwd)

{

speed = speed + RIGHT_SPEED_COMP_VAL_FWD;

}

else //must be REV

{

speed = speed + RIGHT_SPEED_COMP_VAL_REV;

}

//12/20/15 chg from MOTOR_SPEED_FULL (200) to MOTOR_SPEED_MAX (2550

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

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

#ifndef NO_MOTORS

analogWrite(enA_R, speed);

analogWrite(enB_R, speed);

#endif

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

m_FinalRightSpeed = speed;

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

}

void SetLeftMotorDir(boolean bIsFwdDir)

{

if (bIsFwdDir)

{

digitalWrite(in1_L, HIGH);

digitalWrite(in2_L, LOW);

digitalWrite(in3_L, HIGH);

digitalWrite(in4_L, LOW);

}

else //must be REV

{

digitalWrite(in1_L, LOW);

digitalWrite(in2_L, HIGH);

digitalWrite(in3_L, LOW);

digitalWrite(in4_L, HIGH);

}

//added 11/25/15

bLeftMotorDirIsFwd = bIsFwdDir;

}

void SetRightMotorDir(boolean bIsFwdDir)

{

if (bIsFwdDir)

{

digitalWrite(in1_R, HIGH);

digitalWrite(in2_R, LOW);

digitalWrite(in3_R, HIGH);

digitalWrite(in4_R, LOW);

}

else //must be REV

{

digitalWrite(in1_R, LOW);

digitalWrite(in2_R, HIGH);

digitalWrite(in3_R, LOW);

digitalWrite(in4_R, HIGH);

}

http://gfpbridge.com/wp-admin/post.php?post=3212&action=edit#

//added 11/25/15

bRightMotorDirIsFwd = bIsFwdDir;

}

Stay tuned,

Frank

Paynter's Palace

Monthly Archives: July 2018

Integrating Time, Memory, and Heading Capability, Part IV

I2C Hangup problem with Inversense MPU6050

Integrating Time, Memory, and Heading Capability, Part III

Summary: