Hi all,
I am totally new to Arduino as well as this forum, so please forgive me if I've posted this incorrectly.
I'm currently doing a project titled "Inertial Sensor Network for Gait Analysis", and I used the following hardware:
- Arduino Pro Mini ATmega328 8Mhz 3.3V
- Invensense MPU-6000 6-axis accelerometer/gyroscope
- $6 Bluetooth serial
Here's the thing:
- To do a good gait analysis, a minimum sampling rate of 100Hz is required.
- I used 3 of the acce/gyro, connected to Pro Mini using SPI interface.
- With the Pro Mini, I will:
a) Read from all three sensors, use Serial.print() to push it to the Bluetooth serial com, and process the data with Processing, or....
b) Do all the calculations on the Pro Mini, use Serial.print() to push it to the Bluetooth serial com, and plot the data with Processing. - MPU-6000 gives acceleration and gyroscope reading. In order to acquire angle/orientation, calculations need to be done.
- Assume that I am setting the sensors correctly, the sensors should be giving out sampling rate of 1000Hz.
Questions:
- I used micros() to obtain the time delay with each loop, and found out that it is below 100Hz (I am expecting faster since it is a 8Mhz processor, and sensor is giving 1000Hz of readings). I need the Serial.print() cycle for every loop to be at least 100Hz. Is my method correct? Could it be that I am polling my sensors fast enough, but Serial.print() made it slow?
- How do I make it perform faster?
#include <SPI.h>
#include <math.h>
#define ToD(x) (x/131)
#define ToG(x) (x*9.80665/16384)
#define xAxis 0
#define yAxis 1
#define zAxis 2
#define Aoffset 0.8
//--- Settings /CS pins at pins 7, 8, and 9 ---//
const int ChipSelPin1 = 7;
const int ChipSelPin2 = 8;
const int ChipSelPin3 = 9;
int time=0;
int time_old=0;
float angle=0;
float angleX=0;
float angleY=0;
float angleZ=0;
//--- Setup for sensors ---//
void setup()
{
Serial.begin(115200); // start the serial communication at baud rate of 115200
//Serial.println("MPU-6000 Data Acquisition");
//--- SPI settings ---//
//Serial.print("Initializing SPI Protocol...");
SPI.begin(); // start the SPI library
SPI.setClockDivider(SPI_CLOCK_DIV2); // setting SPI at 4Mhz
//Serial.print(".");
SPI.setBitOrder(MSBFIRST); // data delivered MSB first
//Serial.print(".");
SPI.setDataMode(SPI_MODE0); // latched on rising edge, transitioned on falling edge, active low
//Serial.println(".");
//Serial.println("SPI Initialized");
delay(100);
//--- Configure the chip select pins as output ---//
pinMode(ChipSelPin1, OUTPUT);
pinMode(ChipSelPin2, OUTPUT);
pinMode(ChipSelPin3, OUTPUT);
ConfigureMPU6000(); // configure chip
}
void loop()
{
time_old=time;
Serial.println(AcceX(ChipSelPin1));
Serial.print(" ");
Serial.print(AcceY(ChipSelPin1));
Serial.print(" ");
Serial.print(AcceZ(ChipSelPin1));
Serial.print(" ");
Serial.print(AcceX(ChipSelPin2));
Serial.print(" ");
Serial.print(AcceY(ChipSelPin2));
Serial.print(" ");
Serial.print(AcceZ(ChipSelPin2));
Serial.print(" ");
Serial.print(AcceX(ChipSelPin3));
Serial.print(" ");
Serial.print(AcceY(ChipSelPin3));
Serial.print(" ");
Serial.print(AcceZ(ChipSelPin3));
Serial.print(" ");
time=micros();
int dt=time-time_old;
Serial.print(dt);
Serial.println();
}
//***********************************************************//
//-----------------------------------------------------------//
//---------- Self-made functions for easier coding ----------//
//-----------------------------------------------------------//
//***********************************************************//
//------------------------------------------//
//--- Function for SPI writing to sensor ---//
//------------------------------------------//
void SPIwrite(byte reg, byte data, int ChipSelPin)
{
uint8_t dump;
digitalWrite(ChipSelPin,LOW);
dump=SPI.transfer(reg);
dump=SPI.transfer(data);
digitalWrite(ChipSelPin,HIGH);
}
//--------------------------------------------//
//--- Function for SPI reading from sensor ---//
//--------------------------------------------//
uint8_t SPIread(byte reg,int ChipSelPin)
{
uint8_t dump;
uint8_t return_value;
uint8_t addr=reg|0x80;
digitalWrite(ChipSelPin,LOW);
dump=SPI.transfer(addr);
return_value=SPI.transfer(0x00);
digitalWrite(ChipSelPin,HIGH);
return(return_value);
}
//--- Functions for reading raw data ---//
int AcceX(int ChipSelPin)
{
uint8_t AcceX_H=SPIread(0x3B,ChipSelPin);
uint8_t AcceX_L=SPIread(0x3C,ChipSelPin);
int16_t AcceX=AcceX_H<<8|AcceX_L;
return(AcceX);
}
int AcceY(int ChipSelPin)
{
uint8_t AcceY_H=SPIread(0x3D,ChipSelPin);
uint8_t AcceY_L=SPIread(0x3E,ChipSelPin);
int16_t AcceY=AcceY_H<<8|AcceY_L;
return(AcceY);
}
int AcceZ(int ChipSelPin)
{
uint8_t AcceZ_H=SPIread(0x3F,ChipSelPin);
uint8_t AcceZ_L=SPIread(0x40,ChipSelPin);
int16_t AcceZ=AcceZ_H<<8|AcceZ_L;
return(AcceZ);
}
int GyroX(int ChipSelPin)
{
uint8_t GyroX_H=SPIread(0x43,ChipSelPin);
uint8_t GyroX_L=SPIread(0x44,ChipSelPin);
int16_t GyroX=GyroX_H<<8|GyroX_L;
return(GyroX);
}
int GyroY(int ChipSelPin)
{
uint8_t GyroY_H=SPIread(0x45,ChipSelPin);
uint8_t GyroY_L=SPIread(0x46,ChipSelPin);
int16_t GyroY=GyroY_H<<8|GyroY_L;
return(GyroY);
}
int GyroZ(int ChipSelPin)
{
uint8_t GyroZ_H=SPIread(0x47,ChipSelPin);
uint8_t GyroZ_L=SPIread(0x48,ChipSelPin);
int16_t GyroZ=GyroZ_H<<8|GyroZ_L;
return(GyroZ);
}
//--- Function to obtain angles based on accelerometer readings ---//
float AcceDeg(int ChipSelPin,int AxisSelect)
{
float Ax=ToG(AcceX(ChipSelPin));
float Ay=ToG(AcceY(ChipSelPin));
float Az=ToG(AcceZ(ChipSelPin));
float ADegX=((atan(Ax/(sqrt((Ay*Ay)+(Az*Az)))))/PI)*180;
float ADegY=((atan(Ay/(sqrt((Ax*Ax)+(Az*Az)))))/PI)*180;
float ADegZ=((atan((sqrt((Ax*Ax)+(Ay*Ay)))/Az))/PI)*180;
switch (AxisSelect)
{
case 0:
return ADegX;
break;
case 1:
return ADegY;
break;
case 2:
return ADegZ;
break;
}
}
//--- Function to obtain angles based on gyroscope readings ---//
float GyroDeg(int ChipSelPin, int AxisSelect)
{
time_old=time;
time=millis();
float dt=time-time_old;
if (dt>=1000)
{
dt=0;
}
float Gx=ToD(GyroX(ChipSelPin));
if (Gx>0 && Gx<1.4)
{
Gx=0;
}
float Gy=ToD(GyroY(ChipSelPin));
float Gz=ToD(GyroZ(ChipSelPin));
angleX+=Gx*(dt/1000);
angleY+=Gy*(dt/1000);
angleZ+=Gz*(dt/1000);
switch (AxisSelect)
{
case 0:
return angleX;
break;
case 1:
return angleY;
break;
case 2:
return angleZ;
break;
}
}
//--- Function to initialize MPU6000 chip ---//
void ConfigureMPU6000()
{
// DEVICE_RESET @ PWR_MGMT_1, reset device
SPIwrite(0x6B,0x80,ChipSelPin1);
delay(150);
SPIwrite(0x6B,0x80,ChipSelPin2);
delay(150);
SPIwrite(0x6B,0x80,ChipSelPin3);
delay(150);
// TEMP_DIS @ PWR_MGMT_1, wake device and select GyroZ clock
SPIwrite(0x6B,0x03,ChipSelPin1);
delay(150);
SPIwrite(0x6B,0x03,ChipSelPin2);
delay(150);
SPIwrite(0x6B,0x03,ChipSelPin3);
delay(150);
// I2C_IF_DIS @ USER_CTRL, disable I2C interface
SPIwrite(0x6A,0x10,ChipSelPin1);
delay(150);
SPIwrite(0x6A,0x10,ChipSelPin2);
delay(150);
SPIwrite(0x6A,0x10,ChipSelPin3);
delay(150);
// SMPRT_DIV @ SMPRT_DIV, sample rate at 1000Hz
SPIwrite(0x19,0x00,ChipSelPin1);
delay(150);
SPIwrite(0x19,0x00,ChipSelPin2);
delay(150);
SPIwrite(0x19,0x00,ChipSelPin3);
delay(150);
// DLPF_CFG @ CONFIG, digital low pass filter at 42Hz
SPIwrite(0x1A,0x03,ChipSelPin1);
delay(150);
SPIwrite(0x1A,0x03,ChipSelPin2);
delay(150);
SPIwrite(0x1A,0x03,ChipSelPin3);
delay(150);
// FS_SEL @ GYRO_CONFIG, gyro scale at 250dps
SPIwrite(0x1B,0x00,ChipSelPin1);
delay(150);
SPIwrite(0x1B,0x00,ChipSelPin2);
delay(150);
SPIwrite(0x1B,0x00,ChipSelPin3);
delay(150);
// AFS_SEL @ ACCEL_CONFIG, accel scale at 2g (1g=8192)
SPIwrite(0x1C,0x00,ChipSelPin1);
delay(150);
SPIwrite(0x1C,0x00,ChipSelPin2);
delay(150);
SPIwrite(0x1C,0x00,ChipSelPin3);
delay(150);
}