Hi all,
I have been struggling with successfully reading yaw values from an MPU6050 using Jeff Rowberg's I2Cdevlib-MPU6050 "MPU6050_DMP6." I am trying to run 4 DC brushed motors using Pololu's G2 high-power motor driver (Pololu G2 High-Power Motor Driver 18v17) in locked-antiphase operation, while reading from an MPU6050 in I2C, both using a Teensy 4.1. There is a GND line, DIR line, and PWM line that control the motor driver. I send 0 or 1 to the PWM line to either deactivate or activate the motor driver, and I send analogWrite 0-128 for clockwise rotation or analogWrite 128-256 for anti-clockwise rotation in the DIR line in 100KHz. The motor is powered in 12V from the battery, the Teensy is powered in 5V from the battery through a voltage regulator, and MPU6050 is powered in 3.3V from the Teensy.
The problem is that whenever I turn on the motors using a switch, my MPU6050's values become either erratic or freeze immediately. The faster I run the motors, the MPU6050 freezes faster. It is quite obvious that the problem is electromagnetic interference from my motors since my MPU6050 works absolutely fine without my motors running.
I have tried connecting all the components to an Arduino Mega, but the MPU6050 was not fixed. However, when I disconnect the battery source to the Teensy (via the voltage regulator), AND when I disconnect the motor DIR, GND, and PWM lines and connect those three lines to an Arduino Mega, no matter how close the mainboard, Teensy, and MPU6050 are to the 4 motors, the MPU6050 works absolutely fine; my hardware connection is fine.
When I connect the three motor driver lines to my Teensy but not the battery source, there is still some noise; the MPU6050 freezes in 15-30 seconds when motors are MAX speed. When I connect the motor driver lines to an Arduino Mega and the battery source to the Teensy, my MPU6050 immediately freezes.
I have deduced that the noise is mainly from the battery source and motor driver lines.
Can anybody provide some advice on what I could do to eliminate this problem?
Thanks in advance, and I apologize for the lengthy description.
Here is my code (for the Teensy):
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
/* define motor driver pins */
#define motorPWM1 2
#define motorPWM2 3
#define motorPWM3 4
#define motorPWM4 5
#define motorDIR1 6
#define motorDIR2 7
#define motorDIR3 8
#define motorDIR4 9
/* MPU6050 program*/
MPU6050 mpu1(0x68);
#define INTERRUPT_PIN1 25
bool dmpReady1 = false; // set true if DMP init was successful
bool dmpReady2 = false;
uint8_t mpu1IntStatus; // holds actual interrupt status byte from MPU
uint8_t mpu2IntStatus;
uint8_t devStatus1; // return status after each device operation (0 = success, !0 = error)
uint8_t devStatus2;
uint16_t packetSize1; // expected DMP packet size (default is 42 bytes)
uint16_t packetSize2;
uint8_t fifoBuffer[64]; // FIFO storage buffer
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 ypr1[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float ypr2[3];
double yaw;
volatile bool mpu1Interrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady1() {
mpu1Interrupt = true;
}
void setup() {
/* setup for MPU6050 */
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
mpu1.initialize();
pinMode(INTERRUPT_PIN1, INPUT);
devStatus1 = mpu1.dmpInitialize();
mpu1.setXGyroOffset(-136);
mpu1.setYGyroOffset(142);
mpu1.setZGyroOffset(-14);
mpu1.setZAccelOffset(1213);
if (devStatus1 == 0 && devStatus2 == 0) {
mpu1.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN1), dmpDataReady1, RISING);
mpu1IntStatus = mpu1.getIntStatus();
dmpReady1 = true;
dmpReady2 = true;
packetSize1 = mpu1.dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
}
/* setup for motor drivers (LAP) */
pinMode(motorPWM1, OUTPUT);
pinMode(motorPWM2, OUTPUT);
pinMode(motorPWM3, OUTPUT);
pinMode(motorPWM4, OUTPUT);
pinMode(motorDIR1, OUTPUT);
pinMode(motorDIR2, OUTPUT);
pinMode(motorDIR3, OUTPUT);
pinMode(motorDIR4, OUTPUT);
analogWriteFrequency(motorDIR1, 100000); //PWM is 100KHz
analogWriteFrequency(motorDIR2, 100000);
analogWriteFrequency(motorDIR3, 100000);
analogWriteFrequency(motorDIR4, 100000);
}
void loop() {
/* MPU6050 */
if (!dmpReady1) return;
if (mpu1.dmpGetCurrentFIFOPacket(fifoBuffer)) {
mpu1.dmpGetQuaternion(&q, fifoBuffer);
mpu1.dmpGetGravity(&gravity, &q);
mpu1.dmpGetYawPitchRoll(ypr1, &q, &gravity);
}
yaw = (ypr1[0] * 180 / M_PI); //get yaw value from MPU6050
Serial.print("yaw; ");
Serial.println(yaw);
/* motor 1 */
digitalWrite(motorPWM1, HIGH);
analogWrite(motorDIR1, 0);
/* motor 2 */
digitalWrite(motorPWM2, HIGH);
analogWrite(motorDIR2, 0);
/* motor 3 */
digitalWrite(motorPWM3, HIGH);
analogWrite(motorDIR3, 0);
/* motor 4 */
digitalWrite(motorPWM4, HIGH);
analogWrite(motorDIR4, 0);
}
