Posted 07 December 2019 (Pearl Harbor Day)
Back in May of this year I posted about different motor driver possibilities for my 2-motor and 4-motor robots, and showed some results for two drivers (an Adafruit Featherwing and an Adafruit DRV8871). However, I got kind of sidetracked after this post when I discovered the RFI/EMI problem with the MPU6050 IMU. At the time, I blamed the RFI/EMI problem on the non-linear nature of the newer drivers, and went back to using the L298N linear driver.
After quite a bit of experimentation and work, it finally turned out that most (if not all) the RFI/EMI problem was the Pololu 20D metal-geared motors themselves, and properly suppressing the noise at the motor terminals with bypass capacitors solved the problem. So I have decided to repeat some of my initial motor driver testing.
Adafruit DRV8871 Single Channel Motor Driver Testing:
I removed the L298N driver I have been using up until now on my 2-motor robot, and replaced it with two DRV8871 Single Channel Motor Drivers. The hookup with the DRV8871’s is actually significantly simpler than the L298N, requiring only two control lines per channel instead of three. After the normal number of errors, I got it running and started some long-term tests with the motors running continuously (with varying speeds and directions) while polling the MPU6050 every 200 mSec for yaw data.
This test ran for over 5 hours without problems, and then the GY-521 (generic MPU6050) module stopped responding to requests for data. This is the second MPU6050 module that has behaved this way – running for an indefinite amount of time and then refusing to respond.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 |
305.48 -56.81 3 158 28 4 1 305.49 -56.87 3 157 28 4 1 305.49 -56.95 3 156 28 4 1 305.49 -56.99 3 155 28 4 1 305.50 -56.93 3 154 28 4 1 305.50 -56.93 3 153 28 3 1 305.50 -56.90 3 152 28 3 1 305.51 -56.90 3 151 28 3 1 Min Yaw State Spd fifoC IloopsOloops 305.51 -56.89 3 150 28 3 1 305.51 -56.89 3 149 28 3 1 305.52 -56.87 3 148 28 3 1 305.52 -56.86 3 147 28 3 1 305.52 -56.88 3 146 28 2 1 305.53 -56.91 3 145 28 2 1 MPU6050 and/or I2C failure - resetting Arduino! max_loops fifoC InnerCount OuterCount 100 0 100 4 |
The good news is, the DRV8871 motor drivers worked flawlessly the entire time, and are efficient enough so the T0-3 size chip container was just barely warm to the touch.
I have run several long-term tests now with the DRV8871 drivers and two different MPU6050 modules, and the longest error-free run has been about 5 hours as shown above. However, I have also had the test terminate in less than 5 minutes, so this is clearly not reliable enough for prime time. Also, adding my 2-stage power supply filter back into the system did not seem to effectively suppress errors, so no I have no clue what is going on.
11 December 2019 Update:
I ran the system overnight with the same test program, except I commented out the motor run commands so the motors themselves did not run. The test ran for over 11 hours and was still running fine when I terminated it. So, the motors definitely have to be running for the problem to occur.
15 December 2019 Update:
Hoping to maybe eliminate a variable, I changed from the UNO controller to a Teensy 3.2. As usual, this ‘small change’ took a LOT more time than I thought it would.
There aren’t any good example programs for interfacing a Teensy 3.x with the MPU6050, especially using the I2CDEV/MPU6050/DMP libraries. I had a huge problem trying to track down compile issues with the I2CDev libraries, but in the end it came down to figuring out a way to get I2CDev to use i2c_t3.h instead of Wire.h. I solved this by copying I2CDev.h/cpp from my Libraries folder to my local project/solution folder, and editing the code to define the I2C implementation as I2CDEV_ARDUINO_WIRE and then replacing “#include <Wire.h>” with “#include <i2c_t3.h> in the appropriate section as shown below. I’m sure there are more elegant ways of doing this (maybe adding a ‘I2CDEV_TEENSY_3.X’ section at the top?)
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 |
// ----------------------------------------------------------------------------- // 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 // comment this out if you are using a non-optimal IDE/implementation setting // but want the compiler to shut up about it #define I2CDEV_IMPLEMENTATION_WARNINGS // ----------------------------------------------------------------------------- // I2C interface implementation options // ----------------------------------------------------------------------------- #define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project // ^^^ NBWire implementation is still buggy w/some interrupts! #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library #define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire // ----------------------------------------------------------------------------- // Arduino-style "Serial.print" debug constant (uncomment to enable) // ----------------------------------------------------------------------------- //#define I2CDEV_SERIAL_DEBUG #ifdef ARDUINO #if ARDUINO < 100 #include "WProgram.h" #else #include "Arduino.h" #endif #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE //#include <Wire.h> #include <i2c_t3.h> #endif #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY #include <I2C.h> #endif #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE #include "SBWire.h" #endif #endif |
After making the above change, the project started compiling OK, and I was able to connect to the MPU6050 and pull off yaw values using the DMP.
The next problem occurred when I tried to run a test program with the Adafruit DRV8871 drivers. The two control lines alternate between being used as digital outputs (for direction control) and analog/PWM outputs (for speed control). Unfortunately, once a Teensy 3.2 line has been written to with ‘analogWrite()’, it can no longer be used as a digital output without first running ‘pinMode([pin],OUTPUT)’ on it. This particular little ‘gotcha’ appears to have been there since around 2016, but got lost in the shuffle as it is still there in the latest TeensyDuino libraries.
After fixing that problem, I was successful in both getting yaw values from the MPU6050 using Jeff’s I2CDev libraries, and in driving my Pololu D20 metal-geared motors via the Adafruit DRV8871 motor driver modules.
After getting everything working, I took the time to fork Jeff Rowberg’s I2CDev library, edit I2CDev.h/cpp appropriately and create a pull request for Jeff. The changes were merged into the Github master distro pretty quickly, so in theory at least, all a new Teensy 3.2 user has to do is grab Jeff’s I2CDevLib stuff and go.
Here’s my Teensy 3.2 program for testing the MPU6050/DMP and the Adafruit DRV8871 motor drivers:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 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 |
/* Name: Teensy_MPU6050_DMP6_V2.ino Created: 12/13/2019 7:18:20 PM Author: FRANKNEWXPS15\Frank */ #include "MPU6050_6Axis_MotionApps_V6_12.h" elapsedMillis sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec MPU6050 mpu; //MPU6050 mpu(0x69); //using Adafruit DRV8871 motor drivers #pragma region MOTOR_PARAMS #define MOTOR1_IN1 3 #define MOTOR1_IN2 4 #define MOTOR2_IN1 5 #define MOTOR2_IN2 6 //const int INTER_STEP_DELAY_MS = 100; const int INTER_STEP_DELAY_MS = 10; enum MotorStates { MOTOR_FWD_INCREASING, MOTOR_FWD_DECREASING, MOTOR_REV_INCREASING, MOTOR_REV_DECREASING }; enum MotorStates motState = MOTOR_FWD_INCREASING; const int MOTOR_SPEED_MAX = 255; const int MOTOR_SPEED_MIN = 0; int motor_speed = 0; int pgmloops = 0; #pragma endregion Motor Parameters #pragma region MPU6050_SUPPORT uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine 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 int GetPacketLoopCount = 0; int OuterGetPacketLoopCount = 0; //RTC/FRAM/MPU6050 status flags bool bMPU6050Ready = true; bool dmpReady = false; // set true if DMP init was successful volatile float global_yawval = 0; //updated by GetIMUHeadingDeg() const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket() uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param bool bFirstTime = true; #pragma endregion MPU6050 Support const int NAV_UPDATE_INTERVAL_MSEC = 200; void setup() { Serial.begin(115200); delay(3000); Serial.println("In Setup"); Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000); Wire.setDefaultTimeout(200000); // 200ms Serial.println("After Wire.begin()"); // 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(); // supply your own gyro offsets here, scaled for min sensitivity //12/03/19 settings from calibration routines mpu.setXGyroOffset(58); mpu.setYGyroOffset(20); mpu.setZGyroOffset(12); mpu.setXAccelOffset(1618); mpu.setYAccelOffset(3046); mpu.setZAccelOffset(4554); // make sure it worked (returns 0 if successful) if (devStatus == 0) { //12/06/19 no longer needed; cal vals already acquired //// Calibration Time: generate offsets and calibrate our MPU6050 //mpu.CalibrateAccel(6); //mpu.CalibrateGyro(6); //Serial.println(); //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 MPU6050 drift rate to settle...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); bMPU6050Ready = true; Serial.printf("\nMPU6050 Ready at %2.2f Sec\n", millis() / 1000.f); } 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; } Serial.println("DRV8871 test"); pinMode(MOTOR1_IN1, OUTPUT); pinMode(MOTOR1_IN2, OUTPUT); pinMode(MOTOR2_IN1, OUTPUT); pinMode(MOTOR2_IN2, OUTPUT); //ina219.begin(); //mySerial.printf("Speed\tVolts\tMAmps\tMWatts\n"); //column headers //TCCR2B = (TCCR2B & B11111000) | B00000011; // for PWM frequency of 980.39 Hz on pins 3,11 sinceLastNavUpdateMsec = 0; } void loop() { if (sinceLastNavUpdateMsec >= NAV_UPDATE_INTERVAL_MSEC) { sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC; bool getYawResult = GetIMUHeadingDeg(); //updates global_yawval if successful Serial.printf("%3.2f\t%3.2f\t%d\t%d\t%d\t%d\n", millis() / 60000.0, global_yawval, getYawResult, fifoCount, GetPacketLoopCount, OuterGetPacketLoopCount); //Motor speed/dir management switch (motState) { case MOTOR_FWD_INCREASING: // ramp up forward if (motor_speed == 0) { //Serial.println("give it a whack to start!"); analogWrite(MOTOR1_IN1, 128); analogWrite(MOTOR2_IN1, 128); } else { analogWrite(MOTOR1_IN1, motor_speed); analogWrite(MOTOR2_IN1, motor_speed); } //have to change modes back for digital outputs pinMode(MOTOR1_IN2, OUTPUT); pinMode(MOTOR2_IN2, OUTPUT); digitalWrite(MOTOR1_IN2, LOW); digitalWrite(MOTOR2_IN2, LOW); motor_speed++; if (motor_speed >= MOTOR_SPEED_MAX) { motState = MOTOR_FWD_DECREASING; } break; case MOTOR_FWD_DECREASING: // ramp down forward analogWrite(MOTOR1_IN1, motor_speed); analogWrite(MOTOR2_IN1, motor_speed); //have to change modes back for digital outputs pinMode(MOTOR1_IN2, OUTPUT); pinMode(MOTOR2_IN2, OUTPUT); digitalWrite(MOTOR1_IN2, LOW); digitalWrite(MOTOR2_IN2, LOW); motor_speed--; if (motor_speed <= MOTOR_SPEED_MIN) { motState = MOTOR_REV_INCREASING; } break; case MOTOR_REV_INCREASING: // ramp up reverse if (motor_speed == 0) { //Serial.println("give it a whack to start!"); analogWrite(MOTOR1_IN2, 128); analogWrite(MOTOR2_IN2, 128); } else { analogWrite(MOTOR1_IN2, motor_speed); analogWrite(MOTOR2_IN2, motor_speed); } //have to change modes back for digital outputs pinMode(MOTOR1_IN1, OUTPUT); pinMode(MOTOR2_IN1, OUTPUT); digitalWrite(MOTOR1_IN1, LOW); digitalWrite(MOTOR2_IN1, LOW); motor_speed++; if (motor_speed >= MOTOR_SPEED_MAX) { motState = MOTOR_REV_DECREASING; } break; case MOTOR_REV_DECREASING: // ramp down reverse analogWrite(MOTOR1_IN2, motor_speed); analogWrite(MOTOR2_IN2, motor_speed); //have to change modes back for digital outputs pinMode(MOTOR1_IN1, OUTPUT); pinMode(MOTOR2_IN1, OUTPUT); digitalWrite(MOTOR1_IN1, LOW); digitalWrite(MOTOR2_IN1, LOW); motor_speed--; if (motor_speed <= MOTOR_SPEED_MIN) { motState = MOTOR_FWD_INCREASING; //start over } break; default: break; } pgmloops++; if (pgmloops == 1000) { pgmloops = 0; } } } 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 // 10/21/19 completely rewritten to use Homer's algorithm bool retval = false; int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow { // 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; retval = true; } return retval; } uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops) { max_loops = max(10, max_loops); //added 12/04/19 to ensure we loop at least 10 times. This really IS a hard-coded number! fifoCount = mpu.getFIFOCount(); GetPacketLoopCount = 0; //11/10/19 I have seen this fail with fifoC > 28 on occasion, so now I loop max three times OuterGetPacketLoopCount = 0; while (fifoCount != packetSize && OuterGetPacketLoopCount <= 3) { mpu.resetFIFO(); delay(1); fifoCount = mpu.getFIFOCount(); GetPacketLoopCount = 0; while (fifoCount < packetSize && GetPacketLoopCount < max_loops) { GetPacketLoopCount++; fifoCount = mpu.getFIFOCount(); delay(2); } //if we get to here, there should be exactly one packet in the FIFO OuterGetPacketLoopCount++; } if (OuterGetPacketLoopCount < 3) { mpu.getFIFOBytes(data, packetSize); return 1; } return 0; //failed to get a good packet } |
Pingback: IMU Motor Noise Troubleshooting, Part III | Paynter's Palace
Pingback: Replacing Wall-E2’s L298N Motor Drivers with Adafruit DRV8871 | Paynter's Palace
Pingback: Teensy, MPU6050 and Rowberg’s I2CDev Library, Take Two | Paynter's Palace