Go Down

### Topic: Reading and then applying serial data from 9DOF Razor IMU AHRS (Read 5047 times)previous topic - next topic

#### john also

##### Feb 04, 2011, 08:32 amLast Edit: Feb 04, 2011, 08:34 am by john also Reason: 1
Hi,

I've got a 9DOF Razor IMU AHRS hooked up to an Arduino (ATmega 328), and I'm able to read it's serial data in the serial monitor. It looks like this -

Code: [Select]
`!ANG:-11.35,-8.51,14.78!ANG:-13.21,-9.75,14.29!ANG:-14.73,-11.15,13.33!ANG:-15.83,-12.74,11.84!ANG:-16.71,-14.07,10.29!ANG:-17.37,-14.93,8.96!ANG:-18.29,-15.52,8.11!ANG:-19.28,-16.12,7.46!ANG:-19.40,-15.98,6.56!ANG:-18.80,-15.27,5.64`

I'm using the AHRS code from here - http://code.google.com/p/sf9domahrs/

More specifically, this -
Code: [Select]
`#include <Wire.h>// ADXL345 Sensitivity(from datasheet) => 4mg/LSB   1G => 1000mg/4mg = 256 steps// Tested value : 248#define GRAVITY 248  //this equivalent to 1G in the raw data coming from the accelerometer #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square#define ToRad(x) (x*0.01745329252)  // *pi/180#define ToDeg(x) (x*57.2957795131)  // *180/pi// LPR530 & LY530 Sensitivity (from datasheet) => (3.3mv at 3v)at 3.3v: 3mV/º/s, 3.22mV/ADC step => 0.93// Tested values : 0.92#define Gyro_Gain_X 0.92 //X axis Gyro gain#define Gyro_Gain_Y 0.92 //Y axis Gyro gain#define Gyro_Gain_Z 0.92 //Z axis Gyro gain#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second#define Kp_ROLLPITCH 0.02#define Ki_ROLLPITCH 0.00002#define Kp_YAW 1.2#define Ki_YAW 0.00002/*For debugging purposes*///OUTPUTMODE=1 will print the corrected data, //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)#define OUTPUTMODE 1//#define PRINT_DCM 0     //Will print the whole direction cosine matrix#define PRINT_ANALOGS 0 //Will print the analog raw data#define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw#define ADC_WARM_CYCLES 50#define STATUS_LED 13 int8_t sensors[3] = {1,2,0};  // Map the ADC channels gyro_x, gyro_y, gyro_zint SENSOR_SIGN[9] = {-1,1,-1,1,1,1,-1,-1,-1};  //Correct directions x,y,z - gyros, accels, magnetormeterfloat G_Dt=0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possiblelong timer=0;   //general purpuse timerlong timer_old;long timer24=0; //Second timer used to print values int AN[6]; //array that store the 3 ADC filtered data (gyros)int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensorsint ACC[3];          //array that store the accelerometers dataint accel_x;int accel_y;int accel_z;int magnetom_x;int magnetom_y;int magnetom_z;float MAG_Heading;float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vectorfloat Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vectorfloat Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector datafloat Omega_P[3]= {0,0,0};//Omega Proportional correctionfloat Omega_I[3]= {0,0,0};//Omega Integratorfloat Omega[3]= {0,0,0};// Euler anglesfloat roll;float pitch;float yaw;float errorRollPitch[3]= {0,0,0}; float errorYaw[3]= {0,0,0};unsigned int counter=0;byte gyro_sat=0;float DCM_Matrix[3][3]= {  {    1,0,0  }  ,{    0,1,0  }  ,{    0,0,1  }}; float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros herefloat Temporary_Matrix[3][3]={  {    0,0,0  }  ,{    0,0,0  }  ,{    0,0,0  }}; //ADC variablesvolatile uint8_t MuxSel=0;volatile uint8_t analog_reference;volatile uint16_t analog_buffer[8];volatile uint8_t analog_count[8];void setup(){  Serial.begin(57600);  pinMode (STATUS_LED,OUTPUT);  // Status LED    Analog_Reference(DEFAULT);  Analog_Init();  I2C_Init();  Accel_Init();  Read_Accel();  Serial.println("Sparkfun 9DOF Razor AHRS");  digitalWrite(STATUS_LED,LOW);  delay(1500);  // Magnetometer initialization  Compass_Init();    // Initialze ADC readings and buffers  Read_adc_raw();  delay(20);    for(int i=0;i<32;i++)    // We take some readings...    {    Read_adc_raw();    Read_Accel();    for(int y=0; y<6; y++)   // Cumulate values      AN_OFFSET[y] += AN[y];    delay(20);    }      for(int y=0; y<6; y++)    AN_OFFSET[y] = AN_OFFSET[y]/32;      AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];    //Serial.println("Offset:");  for(int y=0; y<6; y++)    Serial.println(AN_OFFSET[y]);    delay(2000);  digitalWrite(STATUS_LED,HIGH);      Read_adc_raw();     // ADC initialization  timer=millis();  delay(20);  counter=0;}void loop() //Main Loop{  if((millis()-timer)>=20)  // Main loop runs at 50Hz  {    counter++;    timer_old = timer;    timer=millis();    if (timer>timer_old)      G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)    else      G_Dt = 0;        // *** DCM algorithm    // Data adquisition    Read_adc_raw();   // This read gyro data    Read_Accel();     // Read I2C accelerometer        if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)      {      counter=0;      Read_Compass();    // Read I2C magnetometer      Compass_Heading(); // Calculate magnetic heading        }        // Calculations...    Matrix_update();    Normalize();    Drift_correction();    Euler_angles();    // ***      printdata();        //Turn off the LED when you saturate any of the gyros.    if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300)))      {      if (gyro_sat<50)        gyro_sat+=10;      }    else      {      if (gyro_sat>0)        gyro_sat--;      }      if (gyro_sat>0)      digitalWrite(STATUS_LED,LOW);      else      digitalWrite(STATUS_LED,HIGH);      }   }`

What I need to do is insert an 'if' statement into this code (I want to be able to trigger a Speakjet speech synth based on the movements of a dancer wearing the accelerometer - more or less random phenomes. A human machine cyborg hybrid generating a hybrid language). For example, at the very basic level, I'm starting with this -

Code: [Select]
` {  if (accel_x < 40) // accel x data     {pinMode(txPin, OUTPUT);    SpeakJetSerial.begin(9600); //setup software serial port to connect to speakjet    char SayIt[] = {189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006}; // makes speak jet say "Shit! Shit! Shit!  SpeakJetSerial.println(SayIT); // send to speakjet  }   }`

But the problem is, whenever I alter the 9DOF code, I end up with errors like these -

"Speak___9DOF:144: error: 'Analog_Reference' was not declared in this scope
Speak___9DOF:145: error: 'Analog_Init' was not declared in this scope
Speak___9DOF:146: error: 'I2C_Init' was not declared in this scope"

For these parts of the original code (which worked fine before)-

Analog_Reference(DEFAULT);
Analog_Init();
I2C_Init();

I'm still very new to writing code, and I don't quite understand what a lot of the AHRS code is for, or how to interpret it. How come the code works fine before I add anything, and then when I add something without any flaws, parts of the original code suddenly have error messages?

And, does anyone have general suggestions for harvesting the data coming in from the 9DOF and using it to trigger functions in the Speakjet (for example)?

Thanks for any suggestions. I hope this post is clear enough.

#1
##### Feb 04, 2011, 08:51 am
Post your entire modified code - perhaps its something simple like you just have things put in the wrong section,or are missing a ) or } somewhere. Can't tell from you snippet.
Designing & building electrical circuits for over 25 years.  Screw Shield for Mega/Due/Uno,  Bobuino with ATMega1284P, & other '328P & '1284P creations & offerings at  my website.

#### john also

#2
##### Feb 04, 2011, 05:16 pm
Here it is - just trying to start real simple.

Code: [Select]
`// Sparkfveun 9DOF Razor IMU AHRS// 9 Degree of Measurement Attitude and Heading Reference System// Firmware v1.0//// Released under Creative Commons License // Code by Doug Weibel and Jose Julio// Based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel// Axis definition:    // X axis pointing forward (to the FTDI connector)   // Y axis pointing to the right    // and Z axis pointing down.// Positive pitch : nose up// Positive roll : right wing down// Positive yaw : clockwise/* Hardware version - v13 ATMega328@3.3V w/ external 8MHz resonator High Fuse DA        Low Fuse FF ADXL345: Accelerometer HMC5843: Magnetometer LY530: Yaw Gyro LPR530: Pitch and Roll Gyro        Programmer : 3.3v FTDI        Arduino IDE : Select board  "Arduino Duemilanove w/ATmega328"*/// This code works also on ATMega168 Hardware#include <Wire.h>// ADXL345 Sensitivity(from datasheet) => 4mg/LSB   1G => 1000mg/4mg = 256 steps// Tested value : 248#define GRAVITY 248  //this equivalent to 1G in the raw data coming from the accelerometer #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square#define ToRad(x) (x*0.01745329252)  // *pi/180#define ToDeg(x) (x*57.2957795131)  // *180/pi// LPR530 & LY530 Sensitivity (from datasheet) => (3.3mv at 3v)at 3.3v: 3mV/º/s, 3.22mV/ADC step => 0.93// Tested values : 0.92#define Gyro_Gain_X 0.92 //X axis Gyro gain#define Gyro_Gain_Y 0.92 //Y axis Gyro gain#define Gyro_Gain_Z 0.92 //Z axis Gyro gain#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second#define Kp_ROLLPITCH 0.02#define Ki_ROLLPITCH 0.00002#define Kp_YAW 1.2#define Ki_YAW 0.00002/*For debugging purposes*///OUTPUTMODE=1 will print the corrected data, //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)#define OUTPUTMODE 1//#define PRINT_DCM 0     //Will print the whole direction cosine matrix#define PRINT_ANALOGS 0 //Will print the analog raw data#define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw#define ADC_WARM_CYCLES 50#define STATUS_LED 13 #define txPin 2#define rxPin 3#include <SoftwareSerial.h>SoftwareSerial SpeakJetSerial =  SoftwareSerial(rxPin, txPin);int8_t sensors[3] = {1,2,0};  // Map the ADC channels gyro_x, gyro_y, gyro_zint SENSOR_SIGN[9] = {-1,1,-1,1,1,1,-1,-1,-1};  //Correct directions x,y,z - gyros, accels, magnetormeterfloat G_Dt=0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possiblelong timer=0;   //general purpuse timerlong timer_old;long timer24=0; //Second timer used to print values int AN[6]; //array that store the 3 ADC filtered data (gyros)int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensorsint ACC[3];          //array that store the accelerometers dataint accel_x;int accel_y;int accel_z;int magnetom_x;int magnetom_y;int magnetom_z;float MAG_Heading;float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vectorfloat Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vectorfloat Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector datafloat Omega_P[3]= {0,0,0};//Omega Proportional correctionfloat Omega_I[3]= {0,0,0};//Omega Integratorfloat Omega[3]= {0,0,0};// Euler anglesfloat roll;float pitch;float yaw;float errorRollPitch[3]= {0,0,0}; float errorYaw[3]= {0,0,0};unsigned int counter=0;byte gyro_sat=0;float DCM_Matrix[3][3]= {  {    1,0,0  }  ,{    0,1,0  }  ,{    0,0,1  }}; float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros herefloat Temporary_Matrix[3][3]={  {    0,0,0  }  ,{    0,0,0  }  ,{    0,0,0  }}; //ADC variablesvolatile uint8_t MuxSel=0;volatile uint8_t analog_reference;volatile uint16_t analog_buffer[8];volatile uint8_t analog_count[8];void setup(){   Serial.begin(57600);  pinMode (STATUS_LED,OUTPUT);  // Status LED    Analog_Reference(DEFAULT);   Analog_Init();  I2C_Init();  Accel_Init();  Read_Accel();  Serial.println("Sparkfun 9DOF Razor AHRS");  digitalWrite(STATUS_LED,LOW);  delay(1500);   // Magnetometer initialization  Compass_Init();    // Initialze ADC readings and buffers  Read_adc_raw();  delay(20);    for(int i=0;i<32;i++)    // We take some readings...    {    Read_adc_raw();    Read_Accel();    for(int y=0; y<6; y++)   // Cumulate values      AN_OFFSET[y] += AN[y];    delay(20);    }      for(int y=0; y<6; y++)    AN_OFFSET[y] = AN_OFFSET[y]/32;      AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];    //Serial.println("Offset:");  for(int y=0; y<6; y++)    Serial.println(AN_OFFSET[y]);    delay(2000);  digitalWrite(STATUS_LED,HIGH);      Read_adc_raw();     // ADC initialization  timer=millis();  delay(20);  counter=0;}void loop() //Main Loop{  if((millis()-timer)>=20)  // Main loop runs at 50Hz  {    counter++;    timer_old = timer;    timer=millis();    if (timer>timer_old)      G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)    else      G_Dt = 0;        // *** DCM algorithm    // Data adquisition    Read_adc_raw();   // This read gyro data    Read_Accel();     // Read I2C accelerometer        if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)      {      counter=0;      Read_Compass();    // Read I2C magnetometer      Compass_Heading(); // Calculate magnetic heading        }        // Calculations...    Matrix_update();     Normalize();    Drift_correction();    Euler_angles();    // ***       printdata();        //Turn off the LED when you saturate any of the gyros.    if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300)))      {      if (gyro_sat<50)        gyro_sat+=10;      }    else      {      if (gyro_sat>0)        gyro_sat--;      }      if (gyro_sat>0)      digitalWrite(STATUS_LED,LOW);      else      digitalWrite(STATUS_LED,HIGH);      }    {  if (accel_x < 40) // accel x data      {pinMode(txPin, OUTPUT);    SpeakJetSerial.begin(9600); //setup software serial port to connect to speakjet    char Shit[] = {189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006};   SpeakJetSerial.println(Shit);  }   }`

#### PaulS

#3
##### Feb 04, 2011, 06:11 pm
Code: [Select]
`  {  if (accel_x < 40) // accel x data      {pinMode(txPin, OUTPUT);    SpeakJetSerial.begin(9600); //setup software serial port to connect to speakjet    char Shit[] = {189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006};   SpeakJetSerial.println(Shit);  }`
What is the first { for? The { defines a block of code, and is typically used after an if, switch, for, or do statement, not placed randomly in the code.

This is your new code. Count how many { and how many } there are.

The SpeakJetSerial.begin() statement does not belong here. Why would you want to initialize the communications channel every time you want to say something. Move that to setup() so it happens once.

#### john also

#4
##### Feb 04, 2011, 06:21 pm
Ok, thanks. That makes perfect sense. I made those changes, but I still get these errors -

Speak___9DOF:146: error: 'Analog_Reference' was not declared in this scope
Speak___9DOF:147: error: 'Analog_Init' was not declared in this scope

+ many more

from the ORIGINAL accelorometer code. Wondering why this only happens when I change some code somewhere else?

#5
##### Feb 04, 2011, 07:16 pm
If you select Tools: Autoformat,
it will tell you have too many left curly brace.
That's a good test there.
If you go thru your code and click the cursor next to ) and }, then  you can use the vertical scroll bar to see where the mating ( & { are. Make these match up and are defining what you think is being defined.
Designing & building electrical circuits for over 25 years.  Screw Shield for Mega/Due/Uno,  Bobuino with ATMega1284P, & other '328P & '1284P creations & offerings at  my website.

#### john also

#6
##### Feb 04, 2011, 07:28 pm
Ok, thanks. The error messages I'm getting are all in relation to these portions of the orginal code -

Analog_Reference(DEFAULT);
Analog_Init();
I2C_Init();
Accel_Init();

ect.

It says they are "not declared in this scope"

#7
##### Feb 04, 2011, 07:32 pm
That sounds like you've lost  the library callout or something along those lines then.
Designing & building electrical circuits for over 25 years.  Screw Shield for Mega/Due/Uno,  Bobuino with ATMega1284P, & other '328P & '1284P creations & offerings at  my website.

#### john also

#8
##### Feb 04, 2011, 09:39 pm
Yeah, turned out to be quite obvious. When I copied the code into the new window to modify it I forgot to transfer over the libraries that were tabbed behind it!

Thanks!

#### john also

#9
##### Feb 04, 2011, 09:52 pm
I would still take any suggestions on processing this data, if anyone has worked with this model before (the Sparkfun 9DOF Razor IMU AHRS).

Go Up

Please enter a valid email to subscribe