Arduinoで加速度センサーと地磁気センサーを使いたい

フォーラム(掲示板)ルール
フォーラム(掲示板)ルールはこちら　　※コードを貼り付ける場合は [code] と [/code] で囲って下さい。詳しくはこちら
返信
t_n_y_09
記事: 8
登録日時: 8年前

Arduinoで加速度センサーと地磁気センサーを使いたい

#1

投稿記事 by t_n_y_09 » 7年前

Arduinoで加速度センサーと地磁気センサーを使いたいと考えています。

各々のプログラムはネットから広い動作確認できたのですが、プログラムをまとめられませんでした。

どこが違うか教えてください。

加速度センサーのみのプログラム

コード: 

// MPU-6050 Accelerometer + Gyro
  #include <Wire.h>
 
  #define MPU6050_ACCEL_XOUT_H       0x3B   // R
  #define MPU6050_WHO_AM_I           0x75   // R
  #define MPU6050_PWR_MGMT_1         0x6B   // R/W
  #define MPU6050_I2C_ADDRESS 0x68
 
typedef union accel_t_gyro_union{
  struct{
    uint8_t x_accel_h;
    uint8_t x_accel_l;
    uint8_t y_accel_h;
    uint8_t y_accel_l;
    uint8_t z_accel_h;
    uint8_t z_accel_l;
    uint8_t t_h;
    uint8_t t_l;
    uint8_t x_gyro_h;
    uint8_t x_gyro_l;
    uint8_t y_gyro_h;
    uint8_t y_gyro_l;
    uint8_t z_gyro_h;
    uint8_t z_gyro_l;
  }
  reg;
  struct{
    int16_t x_accel;
    int16_t y_accel;
    int16_t z_accel;
    int16_t temperature;
    int16_t x_gyro;
    int16_t y_gyro;
    int16_t z_gyro;
  }
  value;
};
 
void setup(){
  Wire.begin();
  int error;
  uint8_t c;
  Serial.begin(9600);
  Serial.print("InvenSense MPU-6050");
  Serial.print("June 2012");
  error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
  Serial.print("WHO_AM_I : ");
  Serial.print(c,HEX);
  Serial.print(", error = ");
  Serial.println(error,DEC);
  error = MPU6050_read (MPU6050_PWR_MGMT_1, &c, 1);
  Serial.print("PWR_MGMT_1 : ");
  Serial.print(c,HEX);
  Serial.print(", error = ");
  Serial.println(error,DEC);
  MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
}
 
  void loop(){
  int error;
  float dT;
  accel_t_gyro_union accel_t_gyro;
  error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
  Serial.print(error,DEC);
  Serial.print("\t");
 
    uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
  SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);
 
  dT = ( (float) accel_t_gyro.value.temperature + 12412.0) / 340.0;
  Serial.print(dT, 1);
  Serial.print("\t");
 
  float acc_x = accel_t_gyro.value.x_accel / 16384.0; //FS_SEL_0 16,384 LSB / g
  float acc_y = accel_t_gyro.value.y_accel / 16384.0;
  float acc_z = accel_t_gyro.value.z_accel / 16384.0;

  Serial.print(acc_x, 2);
  Serial.print("x\t");
  Serial.print(acc_y, 2);
  Serial.print("y\t");
  Serial.print(acc_z, 2);
  Serial.print("z\t");
 
  float acc_angle_x = atan2(acc_x, acc_z) * 360 / 2.0 / PI;
  float acc_angle_y = atan2(acc_y, acc_z) * 360 / 2.0 / PI;
  float acc_angle_z = atan2(acc_x, acc_y) * 360 / 2.0 / PI;
 
  Serial.print(acc_angle_x, 2);
  Serial.print("x\t");
  Serial.print(acc_angle_y, 2);
  Serial.print("y\t");
  Serial.print(acc_angle_z, 2);
  Serial.print("z\t");
 
  float gyro_x = accel_t_gyro.value.x_gyro / 131.0;  //FS_SEL_0 131 LSB / (°/s)
  float gyro_y = accel_t_gyro.value.y_gyro / 131.0;
  float gyro_z = accel_t_gyro.value.z_gyro / 131.0;

  Serial.print(gyro_x, 2);
  Serial.print("x\t");
  Serial.print(gyro_y, 2);
  Serial.print("y\t");
  Serial.print(gyro_z, 2);
  Serial.println("z\t");
}
 
// MPU6050_read
int MPU6050_read(int start, uint8_t *buffer, int size){
  int i, n, error;
  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);
  if (n != 1)
    return (-10);
  n = Wire.endTransmission(false);   // hold the I2C-bus
  if (n != 0)
    return (n);
  // Third parameter is true: relase I2C-bus after data is read.
  Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
  i = 0;
  while(Wire.available() && i<size){
    buffer[i++]=Wire.read();
  }
  if ( i != size)
    return (-11);
  return (0);  // return : no error
}
 
// MPU6050_write
int MPU6050_write(int start, const uint8_t *pData, int size){
  int n, error;
  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);        // write the start address
  if (n != 1)
    return (-20);
  n = Wire.write(pData, size);  // write data bytes
  if (n != size)
    return (-21);
  error = Wire.endTransmission(true); // release the I2C-bus
  if (error != 0)
    return (error);
  return (0);         // return : no error
}
 
// MPU6050_write_reg
int MPU6050_write_reg(int reg, uint8_t data){
  int error;
  error = MPU6050_write(reg, &data, 1);
  return (error);
}
地磁気センサーのみのプログラム

コード: 


#include <Wire.h>
#include <DFRobot_QMC5883.h>

DFRobot_QMC5883 compass;

void setup()
{
  Serial.begin(115200);
  while (!compass.begin())
  {
    Serial.println("Could not find a valid QMC5883 sensor, check wiring!");
    delay(500);
  }

    if(compass.isHMC()){
        Serial.println("Initialize HMC5883");
        compass.setRange(HMC5883L_RANGE_1_3GA);
        compass.setMeasurementMode(HMC5883L_CONTINOUS);
        compass.setDataRate(HMC5883L_DATARATE_15HZ);
        compass.setSamples(HMC5883L_SAMPLES_8);
    }
   else if(compass.isQMC()){
        Serial.println("Initialize QMC5883");
        compass.setRange(QMC5883_RANGE_2GA);
        compass.setMeasurementMode(QMC5883_CONTINOUS); 
        compass.setDataRate(QMC5883_DATARATE_50HZ);
        compass.setSamples(QMC5883_SAMPLES_8);
   }
  }
void loop()
{
  Vector norm = compass.readNormalize();

  // Calculate heading
  float heading = atan2(norm.YAxis, norm.XAxis);

  // Set declination angle on your location and fix heading
  // You can find your declination on: http://magnetic-declination.com/
  // (+) Positive or (-) for negative
  // For Bytom / Poland declination angle is 4'26E (positive)
  // Formula: (deg + (min / 60.0)) / (180 / M_PI);
  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / PI);
  heading += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0){
    heading += 2 * PI;
  }

  if (heading > 2 * PI){
    heading -= 2 * PI;
  }

  // Convert to degrees
  float headingDegrees = heading * 180/M_PI; 

  // Output
  Serial.print(" Heading = ");
  Serial.print(heading);
  Serial.print(" Degress = ");
  Serial.print(headingDegrees);
  Serial.println();

  delay(100);
}
2つ合わせたプログラム

コード: 


#include <Wire.h>
#include <DFRobot_QMC5883.h>

#define MPU6050_ACCEL_XOUT_H       0x3B   // R
#define MPU6050_WHO_AM_I           0x75   // R
#define MPU6050_PWR_MGMT_1         0x6B   // R/W
#define MPU6050_I2C_ADDRESS 0x68

DFRobot_QMC5883 compass;

typedef union accel_t_gyro_union{
  struct{
    uint8_t x_accel_h;
    uint8_t x_accel_l;
    uint8_t y_accel_h;
    uint8_t y_accel_l;
    uint8_t z_accel_h;
    uint8_t z_accel_l;
    uint8_t t_h;
    uint8_t t_l;
    uint8_t x_gyro_h;
    uint8_t x_gyro_l;
    uint8_t y_gyro_h;
    uint8_t y_gyro_l;
    uint8_t z_gyro_h;
    uint8_t z_gyro_l;
  }
  reg;
  struct{
    int16_t x_accel;
    int16_t y_accel;
    int16_t z_accel;
    int16_t temperature;
    int16_t x_gyro;
    int16_t y_gyro;
    int16_t z_gyro;
  }
  value;
};

void setup()
{
  Serial.begin(115200);
  
  Wire.begin();
  int error;
  uint8_t c;
  
  while (!compass.begin())
  {
    Serial.println("Could not find a valid QMC5883 sensor, check wiring!");
    delay(500);
  }

    if(compass.isHMC()){
        Serial.println("Initialize HMC5883");
        compass.setRange(HMC5883L_RANGE_1_3GA);
        compass.setMeasurementMode(HMC5883L_CONTINOUS);
        compass.setDataRate(HMC5883L_DATARATE_15HZ);
        compass.setSamples(HMC5883L_SAMPLES_8);
    }
   else if(compass.isQMC()){
        Serial.println("Initialize QMC5883");
        compass.setRange(QMC5883_RANGE_2GA);
        compass.setMeasurementMode(QMC5883_CONTINOUS); 
        compass.setDataRate(QMC5883_DATARATE_50HZ);
        compass.setSamples(QMC5883_SAMPLES_8);
   }
  }


  Serial.print("InvenSense MPU-6050");
  Serial.print("June 2012");
  error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
  Serial.print("WHO_AM_I : ");
  Serial.print(c,HEX);
  Serial.print(", error = ");
  Serial.println(error,DEC);
  error = MPU6050_read (MPU6050_PWR_MGMT_1, &c, 1);
  Serial.print("PWR_MGMT_1 : ");
  Serial.print(c,HEX);
  Serial.print(", error = ");
 Serial.println(error,DEC);
  MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);

}

void loop()
{
  Vector norm = compass.readNormalize();

  // Calculate heading
  float heading = atan2(norm.YAxis, norm.XAxis);

  // Set declination angle on your location and fix heading
  // You can find your declination on: http://magnetic-declination.com/
  // (+) Positive or (-) for negative
  // For Bytom / Poland declination angle is 4'26E (positive)
  // Formula: (deg + (min / 60.0)) / (180 / M_PI);
  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / PI);
  heading += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  if (heading < 0){
    heading += 2 * PI;
  }

  if (heading > 2 * PI){
    heading -= 2 * PI;
  }

  // Convert to degrees
  float headingDegrees = heading * 180/M_PI; 

  // Output
  Serial.print(" Heading = ");
  Serial.print(heading);
  Serial.print(" Degress = ");
  Serial.print(headingDegrees);
  Serial.println();

  delay(100);
}


  int error;
  float dT;
  accel_t_gyro_union accel_t_gyro;
  error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));
  Serial.print(error,DEC);
  Serial.print("\t");
 
    uint8_t swap;
#define SWAP(x,y) swap = x; x = y; y = swap
  SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l);
  SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l);
  SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l);
  SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l);
  SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l);
  SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l);
  SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l);
 
  dT = ( (float) accel_t_gyro.value.temperature + 12412.0) / 340.0;
  Serial.print(dT, 1);
  Serial.print("\t");
 
  float acc_x = accel_t_gyro.value.x_accel / 16384.0; //FS_SEL_0 16,384 LSB / g
  float acc_y = accel_t_gyro.value.y_accel / 16384.0;
  float acc_z = accel_t_gyro.value.z_accel / 16384.0;

  Serial.print(acc_x, 2);
  Serial.print("x\t");
  Serial.print(acc_y, 2);
  Serial.print("y\t");
  Serial.print(acc_z, 2);
  Serial.print("z\t");
 
  float acc_angle_x = atan2(acc_x, acc_z) * 360 / 2.0 / PI;
  float acc_angle_y = atan2(acc_y, acc_z) * 360 / 2.0 / PI;
  float acc_angle_z = atan2(acc_x, acc_y) * 360 / 2.0 / PI;
 
  Serial.print(acc_angle_x, 2);
  Serial.print("x\t");
  Serial.print(acc_angle_y, 2);
  Serial.print("y\t");
  Serial.print(acc_angle_z, 2);
  Serial.print("z\t");
 
  float gyro_x = accel_t_gyro.value.x_gyro / 131.0;  //FS_SEL_0 131 LSB / (°/s)
  float gyro_y = accel_t_gyro.value.y_gyro / 131.0;
  float gyro_z = accel_t_gyro.value.z_gyro / 131.0;

  Serial.print(gyro_x, 2);
  Serial.print("x\t");
  Serial.print(gyro_y, 2);
  Serial.print("y\t");
  Serial.print(gyro_z, 2);
  Serial.println("z\t");
}
 
// MPU6050_read
int MPU6050_read(int start, uint8_t *buffer, int size){
  int i, n, error;
  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);
  if (n != 1)
    return (-10);
  n = Wire.endTransmission(false);   // hold the I2C-bus
  if (n != 0)
    return (n);
  // Third parameter is true: relase I2C-bus after data is read.
  Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true);
  i = 0;
  while(Wire.available() && i<size){
    buffer[i++]=Wire.read();
  }
  if ( i != size)
    return (-11);
  return (0);  // return : no error
}
 
// MPU6050_write
int MPU6050_write(int start, const uint8_t *pData, int size){
  int n, error;
  Wire.beginTransmission(MPU6050_I2C_ADDRESS);
  n = Wire.write(start);        // write the start address
  if (n != 1)
    return (-20);
  n = Wire.write(pData, size);  // write data bytes
  if (n != size)
    return (-21);
  error = Wire.endTransmission(true); // release the I2C-bus
  if (error != 0)
    return (error);
  return (0);         // return : no error
}
 
// MPU6050_write_reg
int MPU6050_write_reg(int reg, uint8_t data){
  int error;
  error = MPU6050_write(reg, &data, 1);
  return (error);
}

}

以下のようなエラーが出ました。
///////////////////////////////////////////////////////////////////////////////
Arduino：1.6.6 (Windows 7), マイコンボード："Arduino/Genuino Uno"

C:\Users\Pilot_2\Documents\Arduino\all.0411\all.0411.ino:40:1: warning: 'typedef' was ignored in this declaration [enabled by default]

};

^

all.0411:73: error: 'Serial' does not name a type

Serial.print("InvenSense MPU-6050");

^

all.0411:74: error: 'Serial' does not name a type

Serial.print("June 2012");

^

all.0411:75: error: 'error' does not name a type

error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);

^

all.0411:76: error: 'Serial' does not name a type

Serial.print("WHO_AM_I : ");

^

all.0411:77: error: 'Serial' does not name a type

Serial.print(c,HEX);

^

all.0411:78: error: 'Serial' does not name a type

Serial.print(", error = ");

^

all.0411:79: error: 'Serial' does not name a type

Serial.println(error,DEC);

^

all.0411:80: error: 'error' does not name a type

error = MPU6050_read (MPU6050_PWR_MGMT_1, &c, 1);

^

all.0411:81: error: 'Serial' does not name a type

Serial.print("PWR_MGMT_1 : ");

^

all.0411:82: error: 'Serial' does not name a type

Serial.print(c,HEX);

^

all.0411:83: error: 'Serial' does not name a type

Serial.print(", error = ");

^

all.0411:84: error: 'Serial' does not name a type

Serial.println(error,DEC);

^

all.0411:85: error: expected constructor, destructor, or type conversion before '(' token

MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);

^

all.0411:87: error: expected declaration before '}' token

}

^

exit status 1
'Serial' does not name a type

This report would have more information with
"コンパイル中の詳細な出力を表示する"
enabled in File > Preferences.
///////////////////////////////////////////////////////////////////////////////
ページトップ
アバター
みけCAT
記事: 6734
登録日時: 14年前
住所: 千葉県
連絡を取る:
みけCAT に連絡する

Re: Arduinoで加速度センサーと地磁気センサーを使いたい

#2

投稿記事 by みけCAT » 7年前

40行目の警告は、typedefで定義する型が書かれていないために出ているので、
型名のaccel_t_gyro_unionをunionの直後ではなく、unionの定義の{}の後に書くといいでしょう。
73行目以降のエラーは、関数の外とみなされる場所に関数内に書くべきコードがあるために起きています。
きちんと読むと違う問題かもしれませんが、ざっと見た限りでは
・70行目に余計な}がある
・126行目にあるべき関数の定義の最初の部分が無い
・226行目に余計な}がある
ようです。
複雑な問題？マシンの性能を上げてOpenMPで殴ればいい！(死亡フラグ)
ページトップ
t_n_y_09
記事: 8
登録日時: 8年前

Re: Arduinoで加速度センサーと地磁気センサーを使いたい

#3

投稿記事 by t_n_y_09 » 7年前

解決できました、ありがとうございます!

もう一つお聞きしたいのですが、UART通信のGPSも同じ一つのarduinoで出力させたいのですが、通信方法が違っても同じマイコンで出力は可能でしょうか…?
ページトップ
返信

“C言語何でも質問掲示板” へ戻る