I am a high school student who is fairly new to coding and is working on a robot. I need to retrieve the position of two motors from two quadrature rotary encoders. I also need to retrieve yaw from the MPU6050, which I have done so far utilizing the DMP inside of it. I am currently using an Arduino uno and I've been able to get the data I need from each sensor individually using separate programs.
What I need help with is combining these into one program that can read all 3 sensors at the same time, and do it efficentlly enough that I can add code do adjust the motor output based on the sensor data. The reason I'm having trouble is that I think I need 5 interrupt pins (2 for each encoder and 1 for the MPU); however, the Arduino Uno only has 2 interrupt pins. I've looked into the pinchangeinterupt library, but I'm not sure how it works. Could some please tell me what the best way to do this is? Or if there's a way to do this without interrupts?
Thanks.
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// MPU6050 object
MPU6050 mpu;
// DMP (Digital Motion Processing) configuration variables
uint8_t mpuIntStatus; // Holds actual interrupt status byte from MPU
uint8_t devStatus; // 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 buffer
uint8_t fifoBuffer[64]; // FIFO storage buffer
// Orientation/motion variables
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // Gravity vector
float ypr[3]; // [yaw, pitch, roll] in radians
volatile bool mpuInterrupt = false; // Flag for MPU interrupt
// Interrupt detection routine
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
// Initialize serial communication
Serial.begin(115200);
while (!Serial); // Wait for Serial Monitor
// Initialize I2C
Wire.begin();
Wire.setClock(400000); // Set I2C clock to 400kHz
// Initialize MPU6050
Serial.println("Initializing MPU6050...");
mpu.initialize();
// Verify connection
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// Load and configure the DMP
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
// Check if DMP initialization was successful
if (devStatus == 0) {
// Set the offsets (adjust these values as needed for your sensor)
mpu.setXAccelOffset(-3020);
mpu.setYAccelOffset(-354);
mpu.setZAccelOffset(726);
mpu.setXGyroOffset(125);
mpu.setYGyroOffset(-55);
mpu.setZGyroOffset(29);
// Enable DMP
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
// Attach interrupt for DMP
Serial.println("Enabling interrupt detection...");
attachInterrupt(digitalPinToInterrupt(2), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// Set DMP packet size
packetSize = mpu.dmpGetFIFOPacketSize();
Serial.println("DMP ready! Waiting for data...");
} else {
// If DMP initialization failed
Serial.print("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}
}
void loop() {
// Wait for MPU interrupt or FIFO overflow
if (!mpuInterrupt && fifoCount < packetSize) {
//other code added here
return;
}
// Reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// Get current FIFO count
fifoCount = mpu.getFIFOCount();
// Handle FIFO overflow
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// Reset FIFO
mpu.resetFIFO();
Serial.println("FIFO overflow!");
return;
}
// Check for valid DMP data
if (mpuIntStatus & 0x02) {
// Read a packet from FIFO
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
// Get quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
// Get gravity vector
mpu.dmpGetGravity(&gravity, &q);
// Calculate Yaw, Pitch, and Roll
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// Convert radians to degrees
float yaw = ypr[0] * 180 / M_PI;
float pitch = ypr[1] * 180 / M_PI;
float roll = ypr[2] * 180 / M_PI;
// Print Yaw, Pitch, Roll
Serial.print("Yaw: ");
Serial.print(yaw);
Serial.print("\tPitch: ");
Serial.print(pitch);
Serial.print("\tRoll: ");
Serial.println(roll);
}
}
Code For Encoder:
#include <Encoder.h>
// Change these two numbers to the pins connected to your encoder.
// Best Performance: both pins have interrupt capability
// Good Performance: only the first pin has interrupt capability
// Low Performance: neither pin has interrupt capability
Encoder myEnc(2, 3);
// avoid using pins with LEDs attached
void setup() {
Serial.begin(9600);
Serial.println("Basic Encoder Test:");
pinMode(encoderPinA, INPUT_PULLUP);
pinMode(encoderPinB, INPUT_PULLUP);
}
long oldPosition = -999;
void loop() {
long newPosition = myEnc.read();
if (newPosition != oldPosition) {
oldPosition = newPosition;
Serial.println(newPosition);
}
}
I mean that you interrupt is merely setting a flag that loop() periodically checks and acts upon. Instead of checking the flag inside the loop function, you could check the condition that the interrupt is testing for.
The Arduino folks wrote an example of doing this:
...and in your code, you'd move the MPU6050 interrupt output to a different pin, disable the interrupt activation, and then check the rising of the new pin with something like this completely untested snippet:
void loop() {
// check for a riding edge on the pin
const int MpuPin = 12; // or any other pin for the MPU6050 INT
static int mpuPinStatusLast = LOW; // persistent memory for state-change-detection
int mpuPinStatus = digitalRead(MpuPin);
if( mpuPinStatus != mpuPinStatusLast){ // changed?
mpuPinStatusLast = mpuPinStatus; // save new state
if(mpuPinStatus == HIGH) { // rising edge?
mpuInterrupt = true; // flag the rising edge
}
}
// Wait for MPU interrupt or FIFO overflow
if (!mpuInterrupt && fifoCount < packetSize) {
//other code added here
return;
}
...
Then you aren't using a interrupt pin for watching for the pin to change.
I think I understand what you mean now; I tried implementing it with the MPU6050 and just one encoder. The MPU6050 works fine as long as I don't spin the encoder. When I do spin the encoder I get ticks from the encoder for a half second, but then it stops working, and if I continue spinning the encoder, I eventually stop getting data from MPU6050. Do you have any idea why this is? (code bellow)
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <Encoder.h>
#define encoderPinA 3
#define encoderPinB 5
// MPU6050 object
MPU6050 mpu;
Encoder myEnc(encoderPinA, encoderPinB);
// DMP (Digital Motion Processing) configuration variables
uint8_t mpuIntStatus; // Holds actual interrupt status byte from MPU
uint8_t devStatus; // 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 buffer
uint8_t fifoBuffer[64]; // FIFO storage buffer
// Orientation/motion variables
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // Gravity vector
float ypr[3]; // [yaw, pitch, roll] in radians
volatile bool mpuInterrupt = false; // Flag for MPU interrupt
void setup() {
// Initialize serial communication
Serial.begin(115200);
pinMode(encoderPinA, INPUT_PULLUP);
pinMode(encoderPinB, INPUT_PULLUP);
while (!Serial); // Wait for Serial Monitor
// Initialize I2C
Wire.begin();
Wire.setClock(400000); // Set I2C clock to 400kHz
// Initialize MPU6050
Serial.println("Initializing MPU6050...");
mpu.initialize();
// Verify connection
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// Load and configure the DMP
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
// Check if DMP initialization was successful
if (devStatus == 0) {
// Set the offsets (adjust these values as needed for your sensor)
mpu.setXAccelOffset(-3020);
mpu.setYAccelOffset(-354);
mpu.setZAccelOffset(726);
mpu.setXGyroOffset(125);
mpu.setYGyroOffset(-55);
mpu.setZGyroOffset(29);
// Enable DMP
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
// Attach interrupt for DMP
Serial.println("Enabling interrupt detection...");
mpuIntStatus = mpu.getIntStatus();
// Set DMP packet size
packetSize = mpu.dmpGetFIFOPacketSize();
Serial.println("DMP ready! Waiting for data...");
} else {
// If DMP initialization failed
Serial.print("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}
}
long oldPosition = -999;
void loop() {
// check for a riding edge on the pin
const int MpuPin = 4; // or any other pin for the MPU6050 INT
static int mpuPinStatusLast = LOW; // persistent memory for state-change-detection
int mpuPinStatus = digitalRead(MpuPin);
if( mpuPinStatus != mpuPinStatusLast){ // changed?
mpuPinStatusLast = mpuPinStatus; // save new state
if(mpuPinStatus == HIGH) { // rising edge?
mpuInterrupt = true; // flag the rising edge
}
}
// Wait for MPU interrupt or FIFO overflow
if (!mpuInterrupt && fifoCount < packetSize) {
//other code added here
long newPosition = myEnc.read();
if (newPosition != oldPosition) {
oldPosition = newPosition;
Serial.println(newPosition);
}
return;
}
// Reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// Get current FIFO count
fifoCount = mpu.getFIFOCount();
// Handle FIFO overflow
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// Reset FIFO
mpu.resetFIFO();
Serial.println("FIFO overflow!");
return;
}
// Check for valid DMP data
if (mpuIntStatus & 0x02) {
// Read a packet from FIFO
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
// Get quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
// Get gravity vector
mpu.dmpGetGravity(&gravity, &q);
// Calculate Yaw, Pitch, and Roll
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// Convert radians to degrees
float yaw = ypr[0] * 180 / M_PI;
// Print Yaw, Pitch, Roll
Serial.print("Yaw: ");
Serial.println(yaw);
}
}
I couldn't figure out the problem, thanks for the idea though, I'm sure it's a more efficient method and a more experienced coder could have made it work, but I was able to come up with a different solution that seems to be working for now using the pinchangeinterrupt library. Thanks for everyone's help though. This is what I came up with:
#include <PinChangeInterrupt.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
// DMP (Digital Motion Processing) configuration variables
uint8_t mpuIntStatus; // Holds actual interrupt status byte from MPU
uint8_t devStatus; // 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 buffer
uint8_t fifoBuffer[64]; // FIFO storage buffer
// Orientation/motion variables
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // Gravity vector
float ypr[3]; // [yaw, pitch, roll] in radians
// Pins for the quadrature encoder
const int encoderPinA1 = 4; // Connected to digital pin 2 (Interrupt 0)
const int encoderPinB1 = 5; // Connected to digital pin 3 (Interrupt 1)
const int encoderPinA2 = 6; // Connected to digital pin 2 (Interrupt 0)
const int encoderPinB2 = 7; // Connected to digital pin 3 (Interrupt 1)
volatile bool mpuInterrupt = false; // Flag for MPU interrupt
// Variables to store encoder counts and direction
volatile long encoderTicks1 = 0;
volatile int lastEncoded1 = 0;
volatile long encoderTicks2 = 0;
volatile int lastEncoded2 = 0;
// Interrupt detection routine
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
// Initialize the encoder pins as inputs
pinMode(encoderPinA1, INPUT_PULLUP); // Enable internal pull-up resistor
pinMode(encoderPinB1, INPUT_PULLUP); // Enable internal pull-up resistor
pinMode(encoderPinA2, INPUT_PULLUP); // Enable internal pull-up resistor
pinMode(encoderPinB2, INPUT_PULLUP); // Enable internal pull-up resistor
// Attach interrupts to handle encoder signals
attachPCINT(digitalPinToPCINT(encoderPinA1), handleEncoder1, CHANGE);
attachPCINT(digitalPinToPCINT(encoderPinB1), handleEncoder1, CHANGE);
attachPCINT(digitalPinToPCINT(encoderPinA2), handleEncoder2, CHANGE);
attachPCINT(digitalPinToPCINT(encoderPinB2), handleEncoder2, CHANGE);
// Start the serial monitor
Serial.begin(115200);
while (!Serial); // Wait for Serial Monitor
// Initialize I2C
Wire.begin();
Wire.setClock(400000); // Set I2C clock to 400kHz
// Verify connection
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// Load and configure the DMP
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
// Check if DMP initialization was successful
if (devStatus == 0) {
// Set the offsets (adjust these values as needed for your sensor)
mpu.setXAccelOffset(-3020);
mpu.setYAccelOffset(-354);
mpu.setZAccelOffset(726);
mpu.setXGyroOffset(125);
mpu.setYGyroOffset(-55);
mpu.setZGyroOffset(29);
// Enable DMP
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
// Attach interrupt for DMP
Serial.println("Enabling interrupt detection...");
attachInterrupt(digitalPinToInterrupt(2), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// Set DMP packet size
packetSize = mpu.dmpGetFIFOPacketSize();
Serial.println("DMP ready! Waiting for data...");
} else {
// If DMP initialization failed
Serial.print("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println("");
}
}
void loop() {
// Wait for MPU interrupt or FIFO overflow
if (!mpuInterrupt && fifoCount < packetSize) {
// Print the encoder ticks to the Serial Monitor
noInterrupts(); // Temporarily disable interrupts to read shared variable safely
long ticks1 = -encoderTicks1;
long ticks2 = encoderTicks2;
interrupts(); // Re-enable interrupts
Serial.print("Encoder Ticks1: ");
Serial.println(ticks1);
Serial.print("Encoder Ticks2: ");
Serial.println(ticks2);
// Adjust delay as needed
return;
}
// Reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// Get current FIFO count
fifoCount = mpu.getFIFOCount();
// Handle FIFO overflow
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// Reset FIFO
mpu.resetFIFO();
Serial.println("FIFO overflow!");
return;
}
// Check for valid DMP data
if (mpuIntStatus & 0x02) {
// Read a packet from FIFO
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
// Get quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
// Get gravity vector
mpu.dmpGetGravity(&gravity, &q);
// Calculate Yaw, Pitch, and Roll
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// Convert radians to degrees
float yaw = ypr[0] * 180 / M_PI;
float pitch = ypr[1] * 180 / M_PI;
float roll = ypr[2] * 180 / M_PI;
// Print Yaw, Pitch, Roll
Serial.print("Yaw: ");
Serial.print(yaw);
Serial.print("\tPitch: ");
Serial.print(pitch);
Serial.print("\tRoll: ");
Serial.println(roll);
}
}
// Interrupt Service Routines
void handleEncoder1() {
int MSB = digitalRead(encoderPinA1); // Most Significant Bit
int LSB = digitalRead(encoderPinB1); // Least Significant Bit
int encoded = (MSB << 1) | LSB; // Combine the A and B signals into a 2-bit number
int sum = (lastEncoded1 << 2) | encoded; // Get the transition state (4-bit number)
// Determine the direction based on the state transition table
if (sum == 0b0001 || sum == 0b0111 || sum == 0b1110 || sum == 0b1000) {
encoderTicks1++;
} else if (sum == 0b0010 || sum == 0b0100 || sum == 0b1101 || sum == 0b1011) {
encoderTicks1--;
}
lastEncoded1 = encoded; // Store the last encoded value
}
void handleEncoder2() {
int MSB2 = digitalRead(encoderPinA2); // Most Significant Bit
int LSB2 = digitalRead(encoderPinB2); // Least Significant Bit
int encoded2 = (MSB2 << 1) | LSB2; // Combine the A and B signals into a 2-bit number
int sum2 = (lastEncoded2 << 2) | encoded2; // Get the transition state (4-bit number)
// Determine the direction based on the state transition table
if (sum2 == 0b0001 || sum2 == 0b0111 || sum2 == 0b1110 || sum2 == 0b1000) {
encoderTicks2++;
} else if (sum2 == 0b0010 || sum2 == 0b0100 || sum2 == 0b1101 || sum2 == 0b1011) {
encoderTicks2--;
}
lastEncoded2 = encoded2; // Store the last encoded value
}