Pages: [1]   Go Down
Author Topic: MPU6050 + Arduino + Processing  (Read 5435 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 14
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi
I bought the MPU6050 because i thought i could use the 3 axis gyro in a simple way. But trying to get it to work have proven to be a major problem.

The MPU6050 is on sparkfuns breakout board
http://www.sparkfun.com/products/11028

I conneced the MPU6050 to a arduino mini 3.3v
vcc to vcc
GND to GND
VIO to vcc
SDA to A4
SCL to A5
INT to 2

The arduino is program with firmata.

I have tried this code by Noah Zerkin i think.
https://github.com/bzerk/MPU6050_DMP_6_axis_demo_/blob/master/MPU6050_DMP_6_axis_demo_.pde

But can't get it to work.
I have but the Wire.h file under: processing/libraries/Wire/
and the file interrupt.h under:    processing/libraries/avr/interrupt.h

but still get and error:
Code:
processing.app.SketchException: unexpected char: 'i'
at processing.mode.java.JavaBuild.preprocess(JavaBuild.java:368)
at processing.mode.java.JavaBuild.preprocess(JavaBuild.java:197)
at processing.mode.java.JavaBuild.build(JavaBuild.java:156)
at processing.mode.java.JavaBuild.build(JavaBuild.java:135)
at processing.mode.java.JavaMode.handleRun(JavaMode.java:177)
at processing.mode.java.JavaEditor$20.run(JavaEditor.java:481)
at java.lang.Thread.run(Thread.java:680)

Thankful for eny help I can get !
« Last Edit: May 17, 2012, 02:21:17 pm by micke85 » Logged

Offline Offline
Edison Member
*
Karma: 9
Posts: 1010
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

This is his video of the MPU-6050 :


I loaded the program and tried to compile it with Arduino 1.0

Wire.send should be Wire.write
Wire.receive should be Wire.read

Comment these out:
// #include <avr/interrupt.h>
// #include <avr/pgmspace.h>

This code: "Wire.write(0x00)" causes an error. Replace all of them with "Wire.write((uint8_t)0x00)"

That compiles into 9788 bytes. I have ordered an MPU-6050, but I haven't tested the code. The file that compiles is attached.

* MPU_6050.ino (28.86 KB - downloaded 109 times.)
« Last Edit: May 17, 2012, 08:10:01 pm by Krodal » Logged

Gosport, UK
Offline Offline
Faraday Member
**
Karma: 19
Posts: 3114
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
have but the Wire.h file under: processing/libraries/Wire/
and the file interrupt.h under:    processing/libraries/avr/interrupt.h
Why are you trying to use Arduino libraries in Processing?
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 14
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

I have finally got the sensor to work in arduino by using the mpu6050_raw library, but how do i get the information to processing ???
I have tried to install mpu6050_DMP6 on the arduino and then use the code by Fabio Varesano, Freelmu_cube but can't get it to work.

Code:
/**
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/cu.usbmodem641"; // 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 get a error while running the script in processing, witch I don't now what it means?
Code:
local java[86668] <Error>: CGContextGetCTM: invalid context 0x0
« Last Edit: May 30, 2012, 09:34:03 am by micke85 » Logged

Pages: [1]   Go Up
Jump to: