Go Down

Topic: issues with Uno R3 using FreeIMU v0.4, MPU6050, and Processing (Read 1 time) previous topic - next topic

rperez88

Hello all. I recently bought a MPU-6050 to use with my Uno R3.

Heres the specific board I'm using: https://www.sparkfun.com/products/11028
and i have it connected like so:

VDD - Arduino 3.3v
GND - Arduino GND
INT - Arduino digital pin 2
FSYNC - leave unconnected
SCL - Arduino SCL (dedicated pin or Analog 5)
SDA - Arduino SDA (dedicated pin or Analog 4)
VIO - Arduino 3.3v
CLK - leave unconnected
ASCL - leave unconnected
ASDA - leave unconnected

I followed this beginner guide: http://playground.arduino.cc/Main/MPU-6050got it working with little effort.
After that I tried using this guide which includes using the IDE "Processing" to get a visual representation in 3d space: http://www.varesano.net/blog/fabio/initial-tests-freeimu-v04-and-mpu6050

This is where I'm running into issues. In his video as he turns the arduino with the mpu6050, the rectangle on screen moves accordingly.
When I'm using my arduino and the MPU6050, the "processing" rectangle on screen is just free floating, me moving the actual chip with around has no effect on it.

Heres the code I'm using on my arduino, its just the FreeIMU_quaternion example sketch:
Code: [Select]
#include <ADXL345.h>
#include <bma180.h>
#include <HMC58X3.h>
#include <ITG3200.h>
#include <MS561101BA.h>
#include <I2Cdev.h>
#include <MPU60X0.h>
#include <EEPROM.h>

//#define DEBUG
#include "DebugUtils.h"
#include "CommunicationUtils.h"
#include "FreeIMU.h"
#include <Wire.h>
#include <SPI.h>


float q[4];

// Set the FreeIMU object
FreeIMU my3IMU = FreeIMU();

void setup() {
 Serial.begin(115200);
 Wire.begin();
 
 delay(5);
 my3IMU.init();
 delay(5);
}


void loop() {
 my3IMU.getQ(q);
 serialPrintFloatArr(q, 4);
 Serial.println("");
 delay(20);
}


And here's what I'm using in processing:
Code: [Select]
/**
Visualize a cube which will assumes the orientation described
in a quaternion coming from the serial port.

INSTRUCTIONS:
This program has to be run when you have the FreeIMU_quaternion
program running on your Arduino and the Arduino connected to your PC.
Remember to set the serialPort variable below to point to the name the
Arduino serial port has in your system. You can get the port using the
Arduino IDE from Tools->Serial Port: the selected entry is what you have
to use as serialPort variable.


Copyright (C) 2011 Fabio Varesano - http://www.varesano.net/

This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

import processing.serial.*;

Serial myPort;  // Create object from Serial class

final String serialPort = "/dev/tty.usbmodem1411"; // replace this with your serial port. On windows you will need something like "COM1".

float [] q = new float [4];
float [] hq = null;
float [] Euler = new float [3]; // psi, theta, phi

int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)

PFont font;
final int VIEW_SIZE_X = 1024, VIEW_SIZE_Y = 768;


void setup()
{
 size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
 myPort = new Serial(this, serialPort, 115200);  
 
 // The font must be located in the sketch's "data" directory to load successfully
 font = loadFont("CourierNew36.vlw");
 
 /*
 float [] axis = new float[3];
 axis[0] = 0.0;
 axis[1] = 0.0;
 axis[2] = 1.0;
 float angle = PI/2.0;
 
 hq = quatAxisAngle(axis, angle);
 
 hq = new float[4];
 hq[0] = 0.0;
 hq[1] = 0.0;
 hq[2] = 0.0;
 hq[3] = 1.0;
 */
 
 delay(100);
 myPort.clear();
 myPort.write("1");
}


float decodeFloat(String inString) {
 byte [] inData = new byte[4];
 
 if(inString.length() == 8) {
   inData[0] = (byte) unhex(inString.substring(0, 2));
   inData[1] = (byte) unhex(inString.substring(2, 4));
   inData[2] = (byte) unhex(inString.substring(4, 6));
   inData[3] = (byte) unhex(inString.substring(6, 8));
 }
     
 int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
 return Float.intBitsToFloat(intbits);
}


void readQ() {
 if(myPort.available() >= 18) {
   String inputString = myPort.readStringUntil('\n');
   //print(inputString);
   if (inputString != null && inputString.length() > 0) {
     String [] inputStringArr = split(inputString, ",");
     if(inputStringArr.length >= 5) { // q1,q2,q3,q4,\r\n so we have 5 elements
       q[0] = decodeFloat(inputStringArr[0]);
       q[1] = decodeFloat(inputStringArr[1]);
       q[2] = decodeFloat(inputStringArr[2]);
       q[3] = decodeFloat(inputStringArr[3]);
     }
   }
 }
}



void buildBoxShape() {
 //box(60, 10, 40);
 noStroke();
 beginShape(QUADS);
 
 //Z+ (to the drawing area)
 fill(#00ff00);
 vertex(-30, -5, 20);
 vertex(30, -5, 20);
 vertex(30, 5, 20);
 vertex(-30, 5, 20);
 
 //Z-
 fill(#0000ff);
 vertex(-30, -5, -20);
 vertex(30, -5, -20);
 vertex(30, 5, -20);
 vertex(-30, 5, -20);
 
 //X-
 fill(#ff0000);
 vertex(-30, -5, -20);
 vertex(-30, -5, 20);
 vertex(-30, 5, 20);
 vertex(-30, 5, -20);
 
 //X+
 fill(#ffff00);
 vertex(30, -5, -20);
 vertex(30, -5, 20);
 vertex(30, 5, 20);
 vertex(30, 5, -20);
 
 //Y-
 fill(#ff00ff);
 vertex(-30, -5, -20);
 vertex(30, -5, -20);
 vertex(30, -5, 20);
 vertex(-30, -5, 20);
 
 //Y+
 fill(#00ffff);
 vertex(-30, 5, -20);
 vertex(30, 5, -20);
 vertex(30, 5, 20);
 vertex(-30, 5, 20);
 
 endShape();
}


void drawCube() {  
 pushMatrix();
   translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
   scale(5,5,5);
   
   // a demonstration of the following is at
   // http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube
   rotateZ(-Euler[2]);
   rotateX(-Euler[1]);
   rotateY(-Euler[0]);
   
   buildBoxShape();
   
 popMatrix();
}


void draw() {
 background(#000000);
 fill(#ffffff);
 
 readQ();
 
 if(hq != null) { // use home quaternion
   quaternionToEuler(quatProd(hq, q), Euler);
   text("Disable home position by pressing \"n\"", 20, VIEW_SIZE_Y - 30);
 }
 else {
   quaternionToEuler(q, Euler);
   text("Point FreeIMU's X axis to your monitor then press \"h\"", 20, VIEW_SIZE_Y - 30);
 }
 
 textFont(font, 20);
 textAlign(LEFT, TOP);
 text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 20);
 text("Euler Angles:\nYaw (psi)  : " + degrees(Euler[0]) + "\nPitch (theta): " + degrees(Euler[1]) + "\nRoll (phi)  : " + degrees(Euler[2]), 200, 20);
 
 drawCube();
}


void keyPressed() {
 if(key == 'h') {
   println("pressed h");
   
   // set hq the home quaternion as the quatnion conjugate coming from the sensor fusion
   hq = quatConjugate(q);
   
 }
 else if(key == 'n') {
   println("pressed n");
   hq = null;
 }
}

// See Sebastian O.H. Madwick report
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation

void quaternionToEuler(float [] q, float [] euler) {
 euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
 euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
 euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}

float [] quatProd(float [] a, float [] b) {
 float [] q = new float[4];
 
 q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
 q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
 q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
 q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
 
 return q;
}

// returns a quaternion from an axis angle representation
float [] quatAxisAngle(float [] axis, float angle) {
 float [] q = new float[4];
 
 float halfAngle = angle / 2.0;
 float sinHalfAngle = sin(halfAngle);
 q[0] = cos(halfAngle);
 q[1] = -axis[0] * sinHalfAngle;
 q[2] = -axis[1] * sinHalfAngle;
 q[3] = -axis[2] * sinHalfAngle;
 
 return q;
}

// return the quaternion conjugate of quat
float [] quatConjugate(float [] quat) {
 float [] conj = new float[4];
 
 conj[0] = quat[0];
 conj[1] = -quat[1];
 conj[2] = -quat[2];
 conj[3] = -quat[3];
 
 return conj;
}


I'm using the FreeIMU example Quaternion uploaded to my Uno, installed all libraries in the FreeIMU, and the processing sketch he provides on his blog. Everything compiles fine on my end, but when I get to the visual representation in processing my rectangle is free floating, and fabios reacts with his arduino/mpu6050's position.

Any insight would be greatly appreciated

wolfbite


did you ever get it working? I didnt see any reply's and wondered how you went

I'm just starting out

I know half my problem was testing with breadboards

now solder up and working, ide & processing working, but cant see to get the next bit working

Go Up