Salve a tutti!
Ho da poco iniziato ad approfondire le mie conoscenze riguardanti Arduino e sto cercando di portare a termine un progetto che lega ARDUINO UNO e la IMU 6DOF-MPU-6000 from Invensense (accelerometer + gyroscope) comprata sul sito della Drotek(Fr) a circa 20 euro. Il progetto è implementare questo sistema su un robottino LEGO NXT due ruote in modo che possa auto-bilanciarsi da solo grazie appunto all'utilizzo di giroscopio e accelerometro. Non ho tanto tempo da dedicarci però non vedo l'ora di vedere questo robottino stare da solo sulle sue gambe.
Vi spiego il mio problema: premesso che intendo usare il protocollo SPI e non l'I2C per le maggiori prestazioni del primo (non vorrei dire fesserie) e spulciando il web ho trovato un semplice codice che dovrebbe restituirmi i valori sugli assi x,y,z dell'accelerazione. Ho letto una miriade di codici, ma quello che ho trovato su questo forum sembra essere il più chiaro. Non capisco per quale motivo mi restituisca tutti zero sulle tre colonne accx, accy, accz più il tempo di ciclo (sempre uguale circa 4300). Ho adattato il codice che prevedeva l'utilizzo di 3 schede, perchè necessito soltanto di una di esse.
Posto il codice:
#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 = 10;
//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(GyroZ(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);
// TEMP_DIS @ PWR_MGMT_1, wake device and select GyroZ clock
SPIwrite(0x6B,0x03,ChipSelPin1);
delay(150);
// I2C_IF_DIS @ USER_CTRL, disable I2C interface
SPIwrite(0x6A,0x10,ChipSelPin1);
delay(150);
// SMPRT_DIV @ SMPRT_DIV, sample rate at 1000Hz
SPIwrite(0x19,0x00,ChipSelPin1);
delay(150);
// DLPF_CFG @ CONFIG, digital low pass filter at 42Hz
SPIwrite(0x1A,0x03,ChipSelPin1);
delay(150);
// FS_SEL @ GYRO_CONFIG, gyro scale at 250dps
SPIwrite(0x1B,0x00,ChipSelPin1);
delay(150);
// AFS_SEL @ ACCEL_CONFIG, accel scale at 2g (1g=8192)
SPIwrite(0x1C,0x00,ChipSelPin1);
delay(150);
}
Se qualcuno ha un consiglio da darmi ringrazio anticipatamente.
Grazie dell'attenzione.
D.M.