I'm trying to simulate or mimick the movements of my IMU, mpu-6050 to a cube in processing. I have used Jeff Rowberg's code to get the Euler angles from the digital motion processor of the IMU, and i was able to successfully send them to the processing IDE through my arduino duemilanove. But the cube violently jitters all over the screen, although i have not given any code for changing the coordinates. Any idea why? I want it to be stationary at a coordinate and roatate in the x,y,z directions depending upon the values from my IMU. I've basically used the rotateX,rotateY, rotateZ functions to rotate the cube. Here are my arduino and processing sketchs.
- #include "Wire.h"
- #include "I2Cdev.h"
- #include "MPU6050_6Axis_MotionApps20.h"
- MPU6050 mpu;
- #define OUTPUT_READABLE_EULER
- bool dmpReady =false;
- uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
- 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
- float euler[3];
- Quaternion q;
- volatile bool mpuInterrupt = false;
- void dmpDataReady()
- {
- mpuInterrupt=true;
- }
- void setup()
- {
- Wire.begin();
- Serial.begin(9600);
- mpu.initialize();
- while(Serial.available() && Serial.read());
- devStatus= mpu.dmpInitialize();
- if(devStatus == 0)
- {
- mpu.setDMPEnabled(true);
- attachInterrupt(0,dmpDataReady,RISING);
- mpuIntStatus= mpu.getIntStatus();
- dmpReady=true;
- packetSize=mpu.dmpGetFIFOPacketSize();
- }
- else
- {
- Serial.print(F("dmp initialization failed"));
- }
- }
- void loop()
- {
- if(!dmpReady)
- return;
- mpuInterrupt = false;
- mpuIntStatus = mpu.getIntStatus();
- fifoCount = mpu.getFIFOCount();
- if ((mpuIntStatus & 0x10) || fifoCount == 1024)
- {
- mpu.resetFIFO();
- Serial.println(F("FIFO overflow!"));
- }
- else if (mpuIntStatus & 0x02)
- {
- while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
- mpu.getFIFOBytes(fifoBuffer, packetSize);
- // track FIFO count here in case there is > 1 packet available
- // (this lets us immediately read more without waiting for an interrupt)
- fifoCount -= packetSize;
- // #ifdef OUTPUT_READABLE_EULER
- // display Euler angles in degrees
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetEuler(euler, &q);
- /* Serial.print("euler\t");
- Serial.print(euler[0] * 180/M_PI);
- Serial.print("\t");
- Serial.print(euler[1] * 180/M_PI);
- Serial.print("\t");
- Serial.println(euler[2] * 180/M_PI);
- #endif*/
- for(int i=0;i<4;i++)
- {
- Serial.println(euler[i],4);
- delay(10);
- }
- }
- }
- import processing.serial.*;
- import processing.opengl.*;
- Serial myPort;
- float[] value=new float[4];
- String[] msg=new String[5];
- float x, y,z;
- void setup()
- {
- size(600,600,OPENGL);
- noStroke();
- String portName="COM4";
- myPort = new Serial(this, portName, 9600);
- }
- void draw()
- {
- serialEvent();
- rotateX(value[0]);
- rotateY(value[1]);
- rotateZ(value[2]);
- background(255);
- fill(246, 225, 65);
- lights();
- translate(200,200);
- box(50);
- }
- void serialEvent()
- {
- // get msg till line break (ASCII > 13)
- for(int i=0;i<4;i++)
- {
- msg[i] = myPort.readStringUntil(13);
- if(msg[i] != null)
- {
- value[i] = float(msg[i]);
- println(value);
- }
- }
- }
1