Pages: 1 [2]   Go Down
Author Topic: PROCESSING + ARDUINO  (Read 1938 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 13
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

 
Code:

 

import processing.serial.*;

Serial myPort; 
 float [] hq = null;
float [] kalman = new float [3]; // psi, theta, phi
 



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;
final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;
 


void setup()
{
  size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
  textureMode(NORMAL);
  fill(255);
  stroke(color(44,48,32));
 
 
 
println(Serial.list()); // Use this to print connected serial devices
  myPort = new Serial(this, Serial.list()[1], 115200);
  myPort.bufferUntil('\n'); // Buffer until line fee d
 
  // The font must be located in the sketch's "data" directory to load successfully
  font = loadFont("CourierNew36.vlw");
 
 
  // Loading the textures to the cube
  // The png files alow to put the board holes so can increase  realism
 
 
 
 
 
  delay(100);
  myPort.clear();
  myPort.write("1");
}


 float decodeFloat(String inString) {
  byte [] inData = new byte[3];
 
  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));
   
  }
     
  int intbits = (inData[2] << 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 >= 4) {
        kalman[0] = decodeFloat(inputStringArr[0]);
        kalman[1] = decodeFloat(inputStringArr[1]);
        kalman[2] = decodeFloat(inputStringArr[2]);
         
      }
    }
  }
}
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(-kalman[2]);
    rotateX(-kalman[1]);
    rotateY(-kalman[0]);
   
    buildBoxShape();
   
  popMatrix();
}


void draw() {
  background(#000000);
  fill(#ffffff);
 
  readQ();
 
  if(hq != null) { // use home quaternion
   
    text(" ", 20, VIEW_SIZE_Y - 30);
  }
  else {
     
    text(" ", 20, VIEW_SIZE_Y - 30);
  }
 
  textFont(font, 20);
  textAlign(LEFT, TOP);
 
  text("kalman Angles:\nYaw (psi)  : " + degrees(kalman[0]) + "\nPitch (theta): " + degrees(kalman[1]) + "\nRoll (phi)  : " + degrees(kalman[2]), 200, 20);
 
 
  drawCube();
}




 
 
I modify code on internet and try apply for my project but when i move sensor > >> my processing doésn't change.


* y.JPG (81.2 KB, 1197x639 - viewed 18 times.)
« Last Edit: April 16, 2014, 02:59:16 am by ariesking » Logged

Manchester (England England)
Offline Offline
Brattain Member
*****
Karma: 627
Posts: 34222
Solder is electric glue
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

See that commented out print statement in the readQ function. Un comment it an see if anything is being recieved from your arduino.
You. Eed to post your arduino code as well.
Logged

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

See that commented out print statement in the readQ function. Un comment it an see if anything is being recieved from your arduino.
You. Eed to post your arduino code as well.
here is  arduino code
Code:
#include <Wire.h>
#include "Kalman.h"  
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

Kalman kalmanX, kalmanY, kalmanZ; // Create the Kalman instances

const uint8_t MPU6050 = 0x68; // If AD0 is logic low on the PCB the address is 0x68, otherwise set this to 0x69
const uint8_t HMC5883L = 0x1E; // Address of magnetometer

/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
double magX, magY, magZ;
int16_t tempRaw;

double roll, pitch, yaw; // Roll and pitch are calculated using the accelerometer while yaw is calculated using the magnetometer

double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only
double compAngleX, compAngleY, compAngleZ; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

// TODO: Lav zero rutine til accelerometer, magnetomer og gyroscope
#define MAG0MAX 603
#define MAG0MIN -578

#define MAG1MAX 542
#define MAG1MIN -701

#define MAG2MAX 547
#define MAG2MIN -556

float magOffset[3] = { (MAG0MAX + MAG0MIN) / 2, (MAG1MAX + MAG1MIN) / 2, (MAG2MAX + MAG2MIN) / 2 };
double magGain[3];

void setup() {
  delay(100); // Wait for sensors to get ready

  Serial.begin(115200);
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(MPU6050, 0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(MPU6050, 0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(MPU6050, 0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  while (i2cWrite(HMC5883L, 0x02, 0x00, true)); // Configure device for continuous mode
  // TODO: Sæt sample rate
 

  delay(100); // Wait for sensors to stabilize

  /* Set Kalman and gyro starting angle */
  updateMPU6050();
  updateHMC5883L();
  updatePitchRoll();
  updateYaw();

  kalmanX.setAngle(roll); // First set roll starting angle
  gyroXangle = roll;
  compAngleX = roll;

  kalmanY.setAngle(pitch); // Then pitch
  gyroYangle = pitch;
  compAngleY = pitch;

  kalmanZ.setAngle(yaw); // And finally yaw
  gyroZangle = yaw;
  compAngleZ = yaw;

  timer = micros(); // Initialize the timer
}

void loop() {
  /* Update all the IMU values */
  updateMPU6050();
  updateHMC5883L();

  double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();


  /* Roll and pitch estimation */
  updatePitchRoll();
  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s

#ifdef RESTRICT_PITCH
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    compAngleX = roll;
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
    kalmanY.setAngle(pitch);
    compAngleY = pitch;
    kalAngleY = pitch;
    gyroYangle = pitch;
  } else
    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter

  if (abs(kalAngleY) > 90)
    gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading
  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif


  /* Yaw estimation */
  updateYaw();
  double gyroZrate = gyroZ / 131.0; // Convert to deg/s
  // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
  if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {
    kalmanZ.setAngle(yaw);
    compAngleZ = yaw;
    kalAngleZ = yaw;
    gyroZangle = yaw;
  } else
    kalAngleZ = kalmanZ.getAngle(yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter


  /* Estimate angles using gyro only */
  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * dt;
  gyroZangle += gyroZrate * dt;
  //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
  //gyroYangle += kalmanY.getRate() * dt;
  //gyroZangle += kalmanZ.getRate() * dt;

  /* Estimate angles using complimentary filter */
  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
  compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;

  // Reset the gyro angles when they has drifted too much
  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;
  if (gyroZangle < -180 || gyroZangle > 180)
    gyroZangle = kalAngleZ;



Serial.print(kalAngleX); Serial.print("\t");
   Serial.print(kalAngleY); Serial.print("\t");
  Serial.print(kalAngleZ); Serial.print("\t");
  

  delay(10);
}

void updateMPU6050() {
  while (i2cRead(MPU6050, 0x3B, i2cData, 14)); // Get accelerometer and gyroscope values
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = -((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (i2cData[6] << 8) | i2cData[7];
  gyroX = -(i2cData[8] << 8) | i2cData[9];
  gyroY = (i2cData[10] << 8) | i2cData[11];
  gyroZ = -(i2cData[12] << 8) | i2cData[13];
}

void updateHMC5883L() {
  while (i2cRead(HMC5883L, 0x03, i2cData, 6)); // Get magnetometer values
  magX = ((i2cData[0] << 8) | i2cData[1]);
  magZ = ((i2cData[2] << 8) | i2cData[3]);
  magY = ((i2cData[4] << 8) | i2cData[5]);
}

void updatePitchRoll() {
  
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  roll = atan2(accY, accZ) * RAD_TO_DEG;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
}

void updateYaw() { /
  magX *= -1; // Invert axis - this it done here, as it should be done after the calibration
  magZ *= -1;

  magX *= magGain[0];
  magY *= magGain[1];
  magZ *= magGain[2];

  magX -= magOffset[0];
  magY -= magOffset[1];
  magZ -= magOffset[2];

  double rollAngle = kalAngleX * DEG_TO_RAD;
  double pitchAngle = kalAngleY * DEG_TO_RAD;

  double Bfy = magZ * sin(rollAngle) - magY * cos(rollAngle);
  double Bfx = magX * cos(pitchAngle) + magY * sin(pitchAngle) * sin(rollAngle) + magZ * sin(pitchAngle) * cos(rollAngle);
  yaw = atan2(-Bfy, Bfx) * RAD_TO_DEG;

  yaw *= -1;
}
 
Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 615
Posts: 49388
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

On the Arduino end:
Code:
Serial.print(kalAngleX); Serial.print("\t");
   Serial.print(kalAngleY); Serial.print("\t");
  Serial.print(kalAngleZ); Serial.print("\t");

On the Processing end:
Code:
  myPort.bufferUntil('\n'); // Buffer until line fee d
Since you never bother to send a carriage return or line feed, hell is going to freeze over before the serialEvent() method gets called.
Logged

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

can you guide me repair it?
Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 615
Posts: 49388
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
can you guide me repair it?
You need to have the Arduino ACTUALLY send a line feed. You can see what you need to send in the Processing program.

The tab (\t) after the last value is useless. It should be a line feed (\n).
Logged

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

Quote
can you guide me repair it?
You need to have the Arduino ACTUALLY send a line feed. You can see what you need to send in the Processing program.

The tab (\t) after the last value is useless. It should be a line feed (\n).
Thank for your reply.
i repair it into :
Code:
Serial.print(kalAngleX); Serial.print("\t");
Serial.print(kalAngleY); Serial.print("\t");
Serial.print(kalAngleZ); Serial.print("\n");
and the result as my attach below:

But when run processing code: 3value I print on arduino not respond with on processing >> so, Object can't move.


* tr.JPG (20.2 KB, 229x370 - viewed 14 times.)

* rhe.JPG (32.34 KB, 802x634 - viewed 20 times.)
Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 615
Posts: 49388
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

You are not trying to run Processing and the Serial Monitor at the same time, are you?

When you use the bufferUntil() method, Processing triggers the serialEvent() method when the appropriate character arrives for the specified port. Your readQ() method is useless
Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 615
Posts: 49388
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Even if your readQ() method actually read anything, what it reads is strings. Your decodeFloat() function expects that the String passed in consists of 3 bytes of binary data. It does not, so that code is crap, too. Processing knows how to split a tab delimited String into an array of floats. Why don't you just let it? Your Processing code is far more complex (and wrong) than it needs to be.
Logged

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

 
Code:

 
 

import processing.serial.*;

Serial myPort; 
String stringKalmanX, stringKalmanY,stringKalmanZ;
 



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;
final int VIEW_SIZE_X = 800, VIEW_SIZE_Y = 600;
 


void setup()
{
  size(VIEW_SIZE_X, VIEW_SIZE_Y, P3D);
  textureMode(NORMAL);
  fill(255);
  stroke(color(44,48,32));
 
 
 
println(Serial.list()); // Use this to print connected serial devices
  myPort = new Serial(this, Serial.list()[1], 115200);
  myPort.bufferUntil('\n'); // Buffer until line feed
 
  // The font must be located in the sketch's "data" directory to load successfully
  font = loadFont("CourierNew36.vlw");
  // Loading the textures to the cube
  // The png files alow to put the board holes so can increase  realism
 
  delay(100);
  myPort.clear();
  myPort.write("1");
}


void serialEvent (Serial myPort)
{
   stringKalmanX = myPort.readStringUntil('\t');
   stringKalmanY = myPort.readStringUntil('\t');
    stringKalmanZ = myPort.readStringUntil('\n');

}
 
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(-stringKalmanX);
    rotateX(-stringKalmanY);
    rotateY(-stringKalmanZ);
   
    buildBoxShape();
   
  popMatrix();
}
final int minAngle = 0;
final int maxAngle = 180;

void convert() {

 if (stringKalmanX != null) {
    // Trim off any whitespace:
    stringKalmanX = trim(stringKalmanX);
    // Convert to an float and map to the screen height, then save in buffer:   
    kalmanX[kalmanX.length-1] = map(float(stringKalmanX), minAngle, maxAngle, 0);
  }
 
  /* Convert the kalman filter y-axis */
  if (stringKalmanY != null) {
    // Trim off any whitespace:
    stringKalmanY = trim(stringKalmanY);
    // Convert to an float and map to the screen height, then save in buffer:   
    kalmanY[kalmanY.length-1] = map(float(stringKalmanY), minAngle, maxAngle, 0);
  }
 
 
   /* Convert the kalman filter y-axis */
  if (stringKalmanZ != null) {
    // Trim off any whitespace:
    stringKalmanZ = trim(stringKalmanZ);
    // Convert to an float and map to the screen height, then save in buffer:   
    kalmanZ[kalmanZ.length-1] = map(float(stringKalmanZ), minAngle, maxAngle, 0);
  }
 
  }
void printAxis() {
 
  print(stringKalmanX);
  print('\t');
  print(stringKalmanY);
  print('\t');
  print(stringKalmanZ);
  println();
}

 
 

when i change as this. >>>>HAVE an error as my attach below.


* 1.JPG (58.82 KB, 494x593 - viewed 14 times.)
« Last Edit: April 16, 2014, 08:16:39 am by ariesking » Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 615
Posts: 49388
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Why are you trying to negate a string?

Read the whole string in serialEvent, up to the \n. Then, use split to split the String, and float [] to make an array of floats of the pieces that the String gets split into.
Logged

Pages: 1 [2]   Go Up
Jump to: