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