加速度センサーとジャイロセンサーをArduinoで使って学び、Telloの慣性ユニットに触れる(^_-)-☆
加速度センサーとジャイロセンサーをArduinoで使って学び、Telloの慣性ユニットに触れる(^_-)-☆

加速度センサーとジャイロセンサーをArduinoで使って学び、Telloの慣性ユニットに触れる(^_-)-☆

  • 例:

adlx345
sensor

#include <Wire.h> //Wire.h: I2C通信に必要なライブラリ。
#include <Adafruit_Sensor.h>   //ADXL345用のAdafruitライブラリ
#include <Adafruit_ADXL345_U.h>  //ADXL345用のAdafruitライブラリ

/* Assign a unique ID to this sensor at the same time *///センサーオブジェクトの初期化
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);   //accelという名前でセンサーオブジェクトを作成//=12345はセンサーのユニークID

void displaySensorDetails(void)  //センサーの基本情報をシリアルモニターに出力します
{
  sensor_t sensor;//sensor_t構造体を使用して、以下の情報を表示します。
  accel.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);//センサー名
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);//ドライバのバージョン
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);//ユニークID
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" m/s^2");//加速度の最大値(単位は m/s²)
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" m/s^2");//加速度の最小値(単位は m/s²)
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" m/s^2");  //解像度(単位は m/s²)
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

void displayDataRate(void)  //センサーのデータ取得レート(サンプリングレート)を表示
{
  Serial.print  ("Data Rate:    "); 
  
  switch(accel.getDataRate())//現在のデータレートを取得//switch文で該当する値を文字列として表示
  {
    case ADXL345_DATARATE_3200_HZ:
      Serial.print  ("3200 "); 
      break;
    case ADXL345_DATARATE_1600_HZ:
      Serial.print  ("1600 "); 
      break;
    case ADXL345_DATARATE_800_HZ:
      Serial.print  ("800 "); 
      break;
    case ADXL345_DATARATE_400_HZ:
      Serial.print  ("400 "); 
      break;
    case ADXL345_DATARATE_200_HZ:
      Serial.print  ("200 "); 
      break;
    case ADXL345_DATARATE_100_HZ:
      Serial.print  ("100 "); 
      break;
    case ADXL345_DATARATE_50_HZ:
      Serial.print  ("50 "); 
      break;
    case ADXL345_DATARATE_25_HZ:
      Serial.print  ("25 "); 
      break;
    case ADXL345_DATARATE_12_5_HZ:
      Serial.print  ("12.5 "); 
      break;
    case ADXL345_DATARATE_6_25HZ:
      Serial.print  ("6.25 "); 
      break;
    case ADXL345_DATARATE_3_13_HZ:
      Serial.print  ("3.13 "); 
      break;
    case ADXL345_DATARATE_1_56_HZ:
      Serial.print  ("1.56 "); 
      break;
    case ADXL345_DATARATE_0_78_HZ:
      Serial.print  ("0.78 "); 
      break;
    case ADXL345_DATARATE_0_39_HZ:
      Serial.print  ("0.39 "); 
      break;
    case ADXL345_DATARATE_0_20_HZ:
      Serial.print  ("0.20 "); 
      break;
    case ADXL345_DATARATE_0_10_HZ:
      Serial.print  ("0.10 "); 
      break;
    default:
      Serial.print  ("???? "); 
      break;
  }  
  Serial.println(" Hz");  
}

void displayRange(void) //加速度の計測範囲を表示します。
{
  Serial.print  ("Range:         +/- "); 
  
  switch(accel.getRang())   //現在の範囲を取得し、±16g, ±8g, ±4g, ±2gのいずれかを表示
  {
    case ADXL345_RANGE_16_G:
      Serial.print  ("16 "); 
      break;
    case ADXL345_RANGE_8_G:
      Serial.print  ("8 "); 
      break;
    case ADXL345_RANGE_4_G:
      Serial.print  ("4 "); 
      break;
    case ADXL345_RANGE_2_G:
      Serial.print  ("2 "); 
      break;
    default:
      Serial.print  ("?? "); 
      break;
  }  
  Serial.println(" g");  
}
void setup(void) 
{
#ifndef ESP8266
  while (!Serial); // for Leonardo/Micro/Zero
#endif
  Serial.begin(9600);//シリアル通信の初期化 
  Serial.println("Accelerometer Test"); Serial.println("");
  
  /* Initialise the sensor */
  if(!accel.begin())
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    Serial.println("Ooops, no ADXL345 detected ... Check your wiring!");
    while(1);
  }

  /* Set the range to whatever is appropriate for your project */
  accel.setRange(ADXL345_RANGE_16_G);//測定範囲を±16gに設定//コメントアウトを変更すれば、別の範囲も選択可能
  // accel.setRange(ADXL345_RANGE_8_G);
  // accel.setRange(ADXL345_RANGE_4_G);
  // accel.setRange(ADXL345_RANGE_2_G);
  
  /* Display some basic information on this sensor */
  displaySensorDetails();
  
  /* Display additional settings (outside the scope of sensor_t) */
  displayDataRate();
  displayRange();
  Serial.println("");
}

void loop(void) 
{
  /* Get a new sensor event */ 
  sensors_event_t event; 
  accel.getEvent(&event);
 
  /* Display the results (acceleration is measured in m/s^2) */
  Serial.print("X: "); Serial.print(event.acceleration.x); Serial.print("  ");  //X軸: 横方向の加速度。
  Serial.print("Y: "); Serial.print(event.acceleration.y); Serial.print("  ");//Y軸: 縦方向の加速度。
  Serial.print("Z: "); Serial.print(event.acceleration.z); Serial.print("  ");Serial.println("m/s^2 "); //Z軸: 高さ方向の加速度。
  delay(500);
               
              
}
X: 2.28  Y: 2.31  Z: 9.85  m/s^2
X: 0.63  Y: 0.16  Z: 9.96  m/s^2
X: 1.61  Y: 2.35  Z: 10.12  m/s^2
X: -2.20  Y: 4.90  Z: 7.18  m/s^2

LED
#include <Wire.h>
#include <Adafruit_ADXL345_U.h>

Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);

void setup() {
    Serial.begin(9600);
    if (!accel.begin()) {
        Serial.println("ADXL345 not detected!");
        while (1);
    }
    accel.setRange(ADXL345_RANGE_2_G);
}

void loop() {
    sensors_event_t event;
    accel.getEvent(&event);

    // X軸の加速度 (-10 ~ 10 m/s^2) を 0~255 のPWMに変換
    int pwmValue = map(event.acceleration.x, -10, 10, 0, 255);
    analogWrite(9, pwmValue);  // PWM出力 (D9ピン)
    Serial.print("pwmValue: ");
    Serial.println(pwmValue);
    delay(200);
    
    Serial.print("acceleration: ");
    Serial.println(event.acceleration.x);
    delay(200);
}
LED
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL345_U.h>

// ADXL345センサーを初期化
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);

// LEDのピン番号
const int led1 = 2;  // 前
const int led2 = 3;  // 後ろ
const int led3 = 4;  // 右
const int led4 = 5;  // 左

void setup() {
  Serial.begin(9600);

  // LEDピンを出力に設定
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);
  pinMode(led4, OUTPUT);

  // ADXL345の初期化
  if (!accel.begin()) {
    Serial.println("ADXL345を検出できません。接続を確認してください。");
    while (1);
  }

  accel.setRange(ADXL345_RANGE_16_G);  // 測定範囲を±16Gに設定
}

void loop() {
  sensors_event_t event;
  accel.getEvent(&event);  // ADXL345から加速度データを取得
  
  float x = event.acceleration.x;
  float y = event.acceleration.y;

  Serial.print("X軸: "); Serial.print(x); Serial.print(" m/s^2 ");
  Serial.print("Y軸: "); Serial.print(y); Serial.println(" m/s^2 ");

  // 傾きの閾値(m/s^2)
  float threshold = 5.0;    //threshold を5.0 m/s²に設定しています。この値を超えるかどうかで傾きを判定します。

  // LEDの点灯制御
  digitalWrite(led1, y > threshold);    // 前に傾けたらLED1//傾きの方向に応じたLEDを点灯させます
  digitalWrite(led2, y < -threshold);   // 後ろに傾けたらLED2
  digitalWrite(led3, x > threshold);    // 右に傾けたらLED3
  digitalWrite(led4, x < -threshold);   // 左に傾けたらLED4

  delay(100);  // 100msの遅延//100ミリ秒ごとにデータを更新し、LEDの点灯を切り替えます
}

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

void setup() {
  Serial.begin(115200);
  Wire.begin();  // Arduino AVRではピン番号を指定しない

  // MPU6050の初期化
  mpu.initialize();
  if (mpu.testConnection()) {
    Serial.println("MPU6050の接続に成功しました");
  } else {
    Serial.println("MPU6050の接続に失敗しました");
    while (1);  // エラー時は停止
  }
}

void loop() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;

  // 加速度とジャイロのデータを取得
  mpu.getAcceleration(&ax, &ay, &az);
  mpu.getRotation(&gx, &gy, &gz);

  // データを表示
  Serial.print("加速度 (X, Y, Z): ");
  Serial.print(ax); Serial.print(", ");
  Serial.print(ay); Serial.print(", ");
  Serial.println(az);

  Serial.print("ジャイロ (X, Y, Z): ");
  Serial.print(gx); Serial.print(", ");
  Serial.print(gy); Serial.print(", ");
  Serial.println(gz);

  delay(1000);  // 0.5秒ごとにデータを取得
}
ジャイロ (X, Y, Z): 1235, 290, -170
加速度 (X, Y, Z): 15076, 1992, -6080
ジャイロ (X, Y, Z): -950, 5698, -4739
加速度 (X, Y, Z): 6788, 9320, -1992
ジャイロ (X, Y, Z): 4236, -11815, -7154

import socket
import time
import matplotlib.pyplot as plt

# TelloのIPアドレスとポート設定
TELLO_IP = '192.168.10.1'
TELLO_PORT = 8889
LOCAL_PORT = 9000

# ソケットを作成してバインド
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('', LOCAL_PORT))
sock.settimeout(3)

def send_command(command):
    """Telloにコマンドを送信"""
    sock.sendto(command.encode('utf-8'), (TELLO_IP, TELLO_PORT))

def receive_response():
    """Telloからの応答を受信"""
    try:
        response, _ = sock.recvfrom(1024)
        return response.decode('utf-8')
    except socket.timeout:
        return 'timeout'

# グラフ初期設定
plt.ion()
fig, ax = plt.subplots()
pitch_values, roll_values, yaw_values, time_stamps = [], [], [], []
start_time = time.time()

# Telloを起動
send_command('command')
print(receive_response())

try:
    while True:
        send_command('attitude?')
        response = receive_response()
        print(f"Attitude: {response}")
        
        if response != 'timeout':
            # attitude?の応答をパース: pitch:1;roll:2;yaw:3)
            data = response.split(';')
            pitch = int(data[0].split(':')[1])
            roll = int(data[1].split(':')[1])
            yaw = int(data[2].split(':')[1])
            
            # データをリストに追加
            elapsed_time = time.time() - start_time
            time_stamps.append(elapsed_time)
            pitch_values.append(pitch)
            roll_values.append(roll)
            yaw_values.append(yaw)

            # グラフの更新
            ax.clear()
            ax.plot(time_stamps, pitch_values, label='Pitch')
            ax.plot(time_stamps, roll_values, label='Roll')
            ax.plot(time_stamps, yaw_values, label='Yaw')
            ax.set_xlabel('Time (s)')
            ax.set_ylabel('Angle (degrees)')
            ax.set_title('Tello Attitude Real-time Plot')
            ax.legend()
            plt.pause(0.1)  # 100msごとにグラフを更新
            
        time.sleep(0.4)  # 400msごとにデータ取得

except KeyboardInterrupt:
    print("\n終了します。")
finally:
    sock.close()
    plt.ioff()
    plt.show()

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <WiFi.h>

// Wi-Fi設定
const char* ssid = "3*****CC****-2G";
const char* password = "2**********:";

WiFiServer server(12345);
Adafruit_MPU6050 mpu;

void setup() {
  Serial.begin(115200);

  // Wi-Fi接続
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Wi-Fi接続中...");
  }
  Serial.println("Wi-Fi接続完了");
  Serial.print("IPアドレス: ");
  Serial.println(WiFi.localIP());

  // サーバー開始
  server.begin();

  // MPU6050初期化
  if (!mpu.begin()) {
    Serial.println("MPU6050初期化失敗");
    while (1);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
}

void loop() {
  WiFiClient client = server.available();
  if (client) {
    while (client.connected()) {
      sensors_event_t a, g, temp;
      mpu.getEvent(&a, &g, &temp);

      // 加速度データ送信
      client.println(String(a.acceleration.x, 2) + "," + 
                     String(a.acceleration.y, 2) + "," + 
                     String(a.acceleration.z, 2));

      delay(100);
    }
    client.stop();
  }
}

      

import socket
import time
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

# ESP32のIPアドレスとポート
HOST = "192.168.3.**"  # 必要に応じてESP32のIPアドレスに変更Arduino IDEのシリアルモニターで確認
PORT = 12345

def connect_to_esp32():
    """
    ESP32に接続を試みる関数
   なぜかこれをしないとすぐに切れるTelloのWiFiの影響か
    最大5回試行しそれでも失敗した場合はNoneを返す
    """
    for attempt in range(5):
        try:
            print(f"ESP32への接続をトライしています... (試行 {attempt + 1}/5)")
            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            sock.settimeout(10)  # タイムアウトを10秒に設定
            sock.connect((HOST, PORT))
            print("ESP32に接続成功!(^^)/")
            return sock
        except Exception as e:
            print(f"再接続失敗: {e}")
            time.sleep(2)  # 再接続までの待機時間
    return None

# 初回接続
sock = connect_to_esp32()
if not sock:
    print("ESP32への接続ができませんでした(´;ω;`)")
    exit(1)

# データ格納用リスト
time_data = []  # 時間
x_data = []     # x軸
y_data = []     # y軸
z_data = []     # z軸

start_time = time.time()
buffer = ""  # 受信データのバッファ

# グラフ更新関数
def update(frame):
    global sock, buffer
    current_time = time.time() - start_time

    try:
        # データ受信とバッファ処理
        buffer += sock.recv(1024).decode("utf-8")
        lines = buffer.split("\n")  # 改行で分割
        buffer = lines.pop()  # 最後の未完成データをバッファに残す

        for line in lines:
            line = line.strip()
            if line:
                print(f"受信データ: {line}")
                values = list(map(float, line.split(',')))
                if len(values) == 3:
                    # データを格納
                    time_data.append(current_time)
                    x_data.append(values[0])
                    y_data.append(values[1])
                    z_data.append(values[2])

                    # 古いデータを削除必要なら
                    if len(time_data) > 100:  # 表示するデータ数を100に制限フィリーズするのを避けるため
                        time_data.pop(0)
                        x_data.pop(0)
                        y_data.pop(0)
                        z_data.pop(0)

        # グラフを更新
        ax.clear()
        ax.plot(time_data, x_data, label="X_data")
        ax.plot(time_data, y_data, label="Y_data")
        ax.plot(time_data, z_data, label="Z_data")
        ax.legend(loc="upper right")
        ax.set_title("Acceleration data")
        ax.set_xlabel("time (second)")
        ax.set_ylabel("Acceleration (m/s²)")
        ax.grid()

    except socket.timeout:
        print("データ受信がタイムアウトしました。再接続を試みます...")
        sock.close()
        sock = connect_to_esp32()
        if not sock:
            print("再接続に失敗しました。プログラムを終了します。")
            exit(1)
    except Exception as e:
        print(f"エラー: {e}")
        exit(1)

# matplotlibのセットアップ
fig, ax = plt.subplots()
ani = FuncAnimation(fig, update, interval=100, cache_frame_data=False)  # キャッシュ無効化

try:
    plt.show()
except KeyboardInterrupt:
    print("プログラムを終了します。")

# ソケットを閉じる
if sock:
    sock.close()

30
31
33
34
36
32

ホーム » プログラミング » 加速度センサーとジャイロセンサーをArduinoで使って学び、Telloの慣性ユニットに触れる(^_-)-☆

投稿





コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です