I'm am currently working with the Processing sketch https://github.com/ptrbrtz/razor-9dof-ahrs/blob/master/Processing/Razor_AHRS_test/Razor_AHRS_test.pde and line 181 is never reached because the variable synched is never TRUE. So only "Connecting to Razor..." appears and nothing else. I have printed the frameCount and it keeps increasing.
What makes the synched flag become true? Is there some physical action I need to take?
I'm not familiar with the Razor_AHRS, but first can you check if you're using the right serial port ? The code uses the 1st serial port, but depending on your configuration/OS this might need to be updated. (On OSX for example, this may point to a Bluetooth port)
Try using Serial Monitor in Arduino or CoolTerm to connect to the Razor serial port with 57600 baud rate and try to send the same info your sketch is sending:
#ob
#o1
#oe0
#s00
After you've sent the last packet you should receive this data in Serial Monitor/Cool Term:
#SYNCH00\r\n
At least that's what I gather form the source code you've linked to.
If the above works, it means there's something wrong with how you're sending/receiving serial data in Processing. Otherwise, it's not a Processing issue at all and need to step back and check other resources you have on this device and double check the circuit is all fine.
In the processing sketch I can see a few things that potentially could go awry:
if (frameCount == 2)if (serial.available() < token.length())The 1st condition in theory should work because of the delay call, but I would recommend using a millis() based approach to delay() and use a separate flag, not the frameCount itself, just in case.
Again, the 2nd condition should also work, but in case something gets delayed in serial communication for some reason and you miss a packet with the exact number of bytes you expect, you would miss your cue here for the synched flag. I suggest buffering one byte a time until you've reached the last byte. Luckily in you're case, you know what that is(\n) and there's a function which does this for you: Serial's bufferUntil().
This need to be cancelled though after you've read the SYNC string.
Alternatively you can manually append characters to a String until reaching \n while looking for synching.
This is at the top of my head and I can't test properly without the device, but I'm hinting at something along these lines:
/******************************************************************************************
* Test Sketch for Razor AHRS v1.4.2
* 9 Degree of Measurement Attitude and Heading Reference System
* for Sparkfun "9DOF Razor IMU" and "9DOF Sensor Stick"
*
* Released under GNU GPL (General Public License) v3.0
* Copyright (C) 2013 Peter Bartz [http://ptrbrtz.net]
* Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
* Written by Peter Bartz (peter-bartz@gmx.de)
*
* Infos, updates, bug reports, contributions and feedback:
* https://github.com/ptrbrtz/razor-9dof-ahrs
******************************************************************************************/
/*
NOTE: There seems to be a bug with the serial library in Processing versions 1.5
and 1.5.1: "WARNING: RXTX Version mismatch ...".
Processing 2.0.x seems to work just fine. Later versions may too.
Alternatively, the older version 1.2.1 also works and is still available on the web.
*/
import processing.opengl.*;
import processing.serial.*;
// IF THE SKETCH CRASHES OR HANGS ON STARTUP, MAKE SURE YOU ARE USING THE RIGHT SERIAL PORT:
// 1. Have a look at the Processing console output of this sketch.
// 2. Look for the serial port list and find the port you need (it's the same as in Arduino).
// 3. Set your port number here:
final static int SERIAL_PORT_NUM = 0;
// 4. Try again.
final static int SERIAL_PORT_BAUD_RATE = 57600;
float yaw = 0.0f;
float pitch = 0.0f;
float roll = 0.0f;
float yawOffset = 0.0f;
PFont font;
Serial serial;
boolean synched = false;
boolean isSetup = false;
int now,wait = 3000;//wait 3s for Razor to bootup
String serialString = "";
int serialDataLen= 12;
int[] serialBytes = new int[serialDataLen];//4 bytes pair float value * 3 values
int byteCount = 0;
// Global setup
void setup() {
// Setup graphics
size(640, 480, OPENGL);
smooth();
noStroke();
frameRate(50);
// Load font
// font = loadFont("Univers-66.vlw");
// textFont(font);
// Setup serial port I/O
println("AVAILABLE SERIAL PORTS:");
println(Serial.list());
String portName = Serial.list()[SERIAL_PORT_NUM];
println();
println("HAVE A LOOK AT THE LIST ABOVE AND SET THE RIGHT SERIAL PORT NUMBER IN THE CODE!");
println(" -> Using port " + SERIAL_PORT_NUM + ": " + portName);
try{
serial = new Serial(this, portName, SERIAL_PORT_BAUD_RATE);
now = millis();
}catch(Exception e){
System.err.println("Error opening serial port " + portName+"!\nPlease double check your port");
e.printStackTrace();
exit();
}
}
void setupRazor() {
println("Trying to setup and synch Razor...");
if(millis() - now >= wait){
// On Mac OSX and Linux (Windows too?) the board will do a reset when we connect, which is really bad.
// See "Automatic (Software) Reset" on http://www.arduino.cc/en/Main/ArduinoBoardProMini
// So we have to wait until the bootloader is finished and the Razor firmware can receive commands.
// To prevent this, disconnect/cut/unplug the DTR line going to the board. This also has the advantage,
// that the angles you receive are stable right from the beginning.
// delay(3000); // 3 seconds should be enough
// Set Razor output parameters
serial.write("#ob"); // Turn on binary output
serial.write("#o1"); // Turn on continuous streaming output
serial.write("#oe0"); // Disable error message output
// Synch with Razor
serial.clear(); // Clear input buffer up to here
serial.write("#s00"); // Request synch token
isSetup = true;
}
}
float readFloat(int b0,int b1, int b2, int b3) {
// Convert from little endian (Razor) to big endian (Java) and interpret as float
return Float.intBitsToFloat(b0 + (b1 << 8) + (b2 << 16) + (b3 << 24));
}
void draw() {
// Reset scene
background(0);
lights();
// Sync with Razor
if (!synched) {
textAlign(CENTER);
fill(255);
text("Connecting to Razor...", width/2, height/2, -200);
if (!isSetup) setupRazor(); // Set ouput params and request synch token
return;
}
// Draw board
pushMatrix();
translate(width/2, height/2, -350);
drawBoard();
popMatrix();
textFont(font, 20);
fill(255);
textAlign(LEFT);
// Output info text
text("Point FTDI connector towards screen and press 'a' to align", 10, 25);
// Output angles
pushMatrix();
translate(10, height - 10);
textAlign(LEFT);
text("Yaw: " + ((int) yaw), 0, 0);
text("Pitch: " + ((int) pitch), 150, 0);
text("Roll: " + ((int) roll), 300, 0);
popMatrix();
}
void serialEvent(Serial p) {
if(p.available() > 0){
if(!synched){
//wait for \n
char c = p.readChar();
serialString += c;
if(c == '\n' && serialString.endsWith("#SYNCH00\r\n")) synched = true;
}else{
//synched, wait for 12 or more bytes
int b = p.read();
serialBytes[byteCount++] = b;
if(byteCount >= serialDataLen){//we've got enough data, parse it
yaw = readFloat(serialBytes[0],serialBytes[1],serialBytes[2],serialBytes[3]);
pitch = readFloat(serialBytes[4],serialBytes[5],serialBytes[6],serialBytes[7]);
roll = readFloat(serialBytes[8],serialBytes[9],serialBytes[10],serialBytes[11]);
byteCount = 0;
}
}
}
}
void keyPressed() {
switch (key) {
case '0': // Turn Razor's continuous output stream off
serial.write("#o0");
break;
case '1': // Turn Razor's continuous output stream on
serial.write("#o1");
break;
case 'f': // Request one single yaw/pitch/roll frame from Razor (use when continuous streaming is off)
serial.write("#f");
break;
case 'a': // Align screen with Razor
yawOffset = yaw;
}
}
void drawArrow(float headWidthFactor, float headLengthFactor) {
float headWidth = headWidthFactor * 200.0f;
float headLength = headLengthFactor * 200.0f;
pushMatrix();
// Draw base
translate(0, 0, -100);
box(100, 100, 200);
// Draw pointer
translate(-headWidth/2, -50, -100);
beginShape(QUAD_STRIP);
vertex(0, 0 ,0);
vertex(0, 100, 0);
vertex(headWidth, 0 ,0);
vertex(headWidth, 100, 0);
vertex(headWidth/2, 0, -headLength);
vertex(headWidth/2, 100, -headLength);
vertex(0, 0 ,0);
vertex(0, 100, 0);
endShape();
beginShape(TRIANGLES);
vertex(0, 0, 0);
vertex(headWidth, 0, 0);
vertex(headWidth/2, 0, -headLength);
vertex(0, 100, 0);
vertex(headWidth, 100, 0);
vertex(headWidth/2, 100, -headLength);
endShape();
popMatrix();
}
void drawBoard() {
pushMatrix();
rotateY(-radians(yaw - yawOffset));
rotateX(-radians(pitch));
rotateZ(radians(roll));
// Board body
fill(255, 0, 0);
box(250, 20, 400);
// Forward-arrow
pushMatrix();
translate(0, 0, -200);
scale(0.5f, 0.2f, 0.25f);
fill(0, 255, 0);
drawArrow(1.0f, 2.0f);
popMatrix();
popMatrix();
}
