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.
