GY-521(MPU6050) Nema 17 signal noise


#1

Hello all,

I created a balancing robot using arduino last year based on Joop’s YABR (http://www.brokking.net/yabr_main.html). My version used Arduino Nano, GY-521 (MPU6050), Nema 17 stepper motors. Now, I want to move to Photon, and I have written the following program for testing the components:

// 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.h>
#include <math.h>
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) && !defined (PARTICLE)
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
int16_t gx, gy, gz;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
float angle_pitch, angle_roll;
long loop_timer;
float angle_roll_acc, angle_pitch_acc;
long acc_total_vector;
float pid_setpoint = 0.0;

// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
// not so easy to parse, and slow(er) over UART.
#define OUTPUT_READABLE_ACCELGYRO

// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
// binary, one right after the other. This is very fast (as fast as possible
// without compression or data loss), and easy to parse, but impossible to read
// for a human.
//#define OUTPUT_BINARY_ACCELGYRO

#if defined (PARTICLE)
#define LED_PIN D7 // (Particle is D7)
#else
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
#endif

#define LEFT_STEP_PIN A2
#define LEFT_DIR_PIN  A3
#define RIGHT_STEP_PIN A4
#define RIGHT_DIR_PIN  A5

bool blinkState = false;
bool stepState  = false;

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // initialize serial communication
    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
    // it's really up to you depending on your project)
    Serial.begin(9600);

    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
    
    for(int cal_int = 0; cal_int < 2000 ; cal_int ++)
    {
        accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        gyro_x_cal += gx;                                              //Add the gyro x-axis offset to the gyro_x_cal variable
        gyro_y_cal += gy;                                              //Add the gyro y-axis offset to the gyro_y_cal variable
        gyro_z_cal += gz;                                              //Add the gyro z-axis offset to the gyro_z_cal variable
        delay(3);            
    }
    
    gyro_x_cal /= 2000;                                                  //Divide the gyro_x_cal variable by 2000 to get the avarage offset
    gyro_y_cal /= 2000;                                                  //Divide the gyro_y_cal variable by 2000 to get the avarage offset
    gyro_z_cal /= 2000;                                                  //Divide the gyro_z_cal variable by 2000 to get the avarage offset
  
    // configure Arduino LED for
    pinMode(LED_PIN, OUTPUT);
    pinMode(LEFT_STEP_PIN, OUTPUT);
    pinMode(LEFT_DIR_PIN, OUTPUT);
    pinMode(RIGHT_STEP_PIN, OUTPUT);
    pinMode(RIGHT_DIR_PIN, OUTPUT);
    
    digitalWrite(LEFT_DIR_PIN, TRUE);
    digitalWrite(LEFT_STEP_PIN, FALSE);
    digitalWrite(RIGHT_DIR_PIN, TRUE);
    digitalWrite(RIGHT_STEP_PIN, FALSE);
    
    loop_timer = micros();                                               //Reset the loop timer
}

void loop() {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    //Accelerometer angle calculations
    acc_total_vector = sqrt((ax*ax)+(ay*ay)+(az*az));  //Calculate the total accelerometer vector
    //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
    angle_pitch_acc = asin((float)ay/acc_total_vector)* 57.296;       //Calculate the pitch angle
    angle_roll_acc = asin((float)ax/acc_total_vector)* -57.296;       //Calculate the roll angle
    
    gx -= gyro_x_cal;                                                //Subtract the offset calibration value from the raw gyro_x value
    gy -= gyro_y_cal;                                                //Subtract the offset calibration value from the raw gyro_y value
    gz -= gyro_z_cal;                                                //Subtract the offset calibration value from the raw gyro_z value
  
    angle_pitch += gx * 0.0000611;                                  //Calculate the traveled pitch angle and add this to the angle_pitch variable
    angle_roll += gy * 0.0000611;                                   //Calculate the traveled roll angle and add this to the angle_roll variable
    
    angle_roll = angle_roll * 0.9 + angle_roll_acc * 0.1;
    
    // these methods (and a few others) are also available
    //accelgyro.getAcceleration(&ax, &ay, &az);
    //accelgyro.getRotation(&gx, &gy, &gz);
    /*
    if(angle_roll_acc > pid_setpoint)
    {
        digitalWrite(LEFT_DIR_PIN, FALSE);
        digitalWrite(RIGHT_DIR_PIN, TRUE);
    }
    else
    {
        digitalWrite(LEFT_DIR_PIN, TRUE);
        digitalWrite(RIGHT_DIR_PIN, FALSE);
    }
    */
    // Serial.print(angle_pitch_acc); Serial.print("\t");
    Serial.println(angle_roll);

    // blink LED to indicate activity
    digitalWrite(LED_PIN, blinkState);
    blinkState = !blinkState;
    
    //Drive Steppers
    digitalWrite(LEFT_STEP_PIN, stepState);
    digitalWrite(RIGHT_STEP_PIN, stepState);
    stepState = !stepState;
    
    //delay(10);
    
    
    while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
    loop_timer = micros();                                               //Reset the loop timer
    
}

Currently, I am powering Photon from my PC using USB cable and the LiPo is only used to drive the steppers. I have checked the ground connections and all the connections are fine. Also, I have not yet used timer interrupt to drive the steppers, which I plan to do it as the next step.

Here’s the problem:

  • The angle (printed using Arduino serial monitor) is fine as long as the stepper is turned off. As soon as the stepper power is turned on, the displayed angle goes erratic and deviates from actual angle. There are pull-up resistors in the GY-521 board itself, yet I tried pull-up, pull-down configuratios using two 2.2 K Ohm resistors - but nothing seems to solve the problem.

When I swap Photon with Arduino nano, the problem disappears and the angle reading is fine. I don’t know what I am doing wrong with Photon. Please can anyone help me to solve this issue?

Kind regards,
Laksh.


#2

One thing you need to consider also is that the rate at which loop() is called on the Photon in AUTOMATIC mode is much slower than on a nano due to the cloud stuff that’s done in between iterations of loop().
To get that possible impact out of your preliminary tests, you may want to use SYSTEM_MODE(MANUAL) to keep the cloud connection off.
If you want to flash new firmware OTA you then need to put the device in Safe Mode to reenable the cloud connection temporarily for the update.

Change that

    while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
    loop_timer = micros();                                               //Reset the loop timer

from the end of loop() to

  if (micros - loop_timer < 4000) return;
  loop_timer = micros();

and put it at the top of loop().
As you have it now (in AUTOMATIC mode) after waiting 4000µs the cloud housekeeping adds another ~1000µs. If you use the alternative approach, you can keep AUTOMATIC on and have the cloud housekeeping “hide” in your 4000µs delay.

Do you mean just powered but not stepping or actually stepping?
Could it be that the noise of the setppers impacts the common ground level? If so, try adding some filter caps.

It doesn’t matter for your actual problem but loop_timer should be unsigned long.


#3

Hello ScruffR,

Thank you for your prompt response.

SYSTEM_MODE(MANUAL) / Safe Mode to reenable:

How do I do this? Sorry, I am still new to Photon but I will search the online docs, and in the meantime could you also give some pointers?

Do you mean just powered but not stepping or actually stepping?

I meant the steppers are powered and actually stepping (quite slowly). I considered the noise and already have connected two 100uF capacitors.

Kind regards,
Laksh.


#4

Which part of that?
SYSTEM_MODE(MANUAL)
or
Safe Mode

Can you scope the Vin and SCL/SDA signals?


#5

Hello ScruffR,

I was reading the online docs and stumbled across the links you gave. Now, I think I know how to do both. I will add the line SYSTEM_MODE(MANUAL); as per the example and then use the method given in the page https://docs.particle.io/tutorials/device-os/led/photon/#safe-mode to get breathing magenta (safe mode). Am I right? (Eager to return home and check whether this works in the evening !!)

Sorry, I don’t have an oscilloscope to check Vin, SCL/SDA signals. By the way, I have connected the 100 uF capacitors to Vin and ground of Stepper Motor driver A4988 ( as per https://howtomechatronics.com/tutorials/arduino/how-to-control-stepper-motor-with-a4988-driver-and-arduino/). Is that what you meant with filter capacitor? Or Should I connect them elsewhere?

Kind regards,
Laksh.


#6

@AGLakshmanan, the capacitor, ideally larger than 100uF, needs to be connected as close as possible to the Vin pin and nearby GND. You may also want to put a parallel 0.1uF cap to filter high-frequency noise. Are you powering via a high-quality USB power supply?


#7

Hello Peekay123,

Thanks for your inputs. How much large do you recommend ? I will order the caps.

I didn’t understand where I should place the 0.1uF caps. Did you mean parallel to the SCL, SDA connections to Photon?

Kind regards,
Laksh.


#8

@AGLakshmanan, you can use a 470uF to 1000uF cap and put the 0.1uF cap IN PARALLEL with it near the Vin pin. As for the SDA/SCL line, you don’t typically need caps.

How are you powering everything?


#9

Hello peekay123,

I am powering the stepper motors using 12V LiPo (stepped down to 8V). I am powering the photon using usb cable from my laptop. I have tied all the grounds together.

Kind regards,
Laksh.


#10

Sorry, I had not mentioned it earlier. I don’t have high quality USB power supply. Would this be the problem as well?


#11

For now try separating the stepper power GND from the rest (prividing your A4988 boards do have distinct GND for VMOT and Vcc) to see if that would help isolate the issue.


#12

Hello ScruffR, peekay123,

I am glad to let you know that both of your solutions work:

Although I didn’t need the 0.1 uF cap, increasing the value of 100uF to 470uF reduced the noise drastically.

Thanks to both of you !! :star_struck::+1::+1:.

I have also implemented the timer ISR task from SparkIntervalTimer library for Stepper control. Now, I need to fine tune the PID parameters for balancing the bot and enable wifi remote control.

Sorry for straying off topic, but at some point I wish to implement this in FreeRTOS (which I think is supported by Photon), to gain some RTOS experience (which I have none). Do you guys have any tips/suggestions for FreeRTOS implementation (things I could obviously avoid - like the ‘if return’ solution instead of ‘while’?

Kind regards,
Laksh.