実験5 モーションセンシングによるパン・チルト制御実験

実験の目的：

IIC の基本的な動作原理を理解し、ジャイロスコープのデータを取得します。
ジャイロスコープを用いて、2軸パン・チルトの姿勢をリアルタイムに制御します。
STM32 の GPIO 周辺機能ピンの設定方法を習得します。
STM32CubeMX で新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようにします。
keil IDE でユーザーコードを追加し、プロジェクトのテストを行えるようにします。

応用シーン

医療分野における、患者の手のリハビリテーション運動制御。

機能要件：

ジャイロスコープを用いて、2軸ジンバルの姿勢をリアルタイムに制御します。例えば、ジャイロスコープを z 軸に沿って回転させると、ジンバル上のサーボも同様に動きます。

注：加速度センサーとジャイロスコープのデータを読み取り、ロール角とピッチ角を計算します。地磁気センサーのデータがないため、ヨー角はジャイロスコープのデータで近似しますが、ジャイロスコープは積分ドリフトの影響を受けるため、この方法は短時間の近似計算にしか適さない点に注意してください。そのため、本実験ではロール角（roll）とピッチ角（pitch）のみを使用します。

	pitch は x 軸回りの回転で、ピッチ角とも呼ばれます。yaw は y 軸回りの回転で、ヨー角とも呼ばれます。roll は z 軸回りの回転で、ロール角とも呼ばれます。
3次元空間における右手直交座標系	航空

ハードウェアの準備：

STM32 メインボード V1.3 * 1、ST-Link ダウンロードケーブル * 1、3S バッテリーボックス * 1、リチウム電池 * 3、ジャイロスコープ * 1、2自由度パン・チルト * 1

試作構成：

回路接続：

ハードウェアモジュール

ヒント：ハードウェアおよびソフトウェア環境の設定にあたっては、開発ボード、環境設定、電源モジュールの基本的な使い方に習熟しておくことを強くお勧めします。電子モジュールについては必要に応じて参照してください。

UNI-STM32 開発ボード【V1.3】の紹介

UNI-STM32 開発環境の設定【V2.0】

バッテリーおよび電源モジュール使用上の注意（必読）

電子モジュールの紹介

ピンの紹介

まずインターフェースの位置を全体的に把握し、その後ピン表に従って配線を完了します。まず IIC インターフェースとサーボインターフェースのおおよその位置を確認してください。

ジャイロスコープピン	stm32 ピン	パン・チルト電子ハードウェア	stm32 ピン
5V	VCC_5V	ジンバル下段サーボ	PA2
SDA	SCL(PB8)	ジンバル上段サーボ	PA3
SCL	SDA(PB7)
GND	GND

実際の配線

配線のコツ：配線時は、ジャイロスコープの 5V ピンを stm32 開発ボードの 5V（図中の黄色の線）に接続し、その後順番に配線してください。

ソフトウェア設計：

プロジェクトのダウンロード

ソースリンク：（HumansX サポートより提供）

プログラムコード

コアコード：main.c。プロジェクトの完全なコードは（ソースリンク）をクリックしてダウンロードしてください。

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "math.h"
#include "servo.h"
#include "mpu6050.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
#define DT      0.01f   // 10ms
#define ALPHA   0.98f
#define M_PI    3.141592653f 
static float roll = 0.0f, pitch = 0.0f;
void UpdateIMU(); //update mpu6050 data
void Gimbal_Status_Update(); // gimbal status update
float map_float(float value, float in_min, float in_max, float out_min, float out_max);
float servo_1_angle = 0;
float servo_2_angle = 0;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_TIM2_Init();
  MX_USART1_UART_Init();
  MX_I2C1_Init();
  /* USER CODE BEGIN 2 */
  Servo_Init();    //servo init
  printf("Mpu6050 test\r\n");
  HAL_Delay(1000);
  while(MPU6050_Init(&hi2c1, accel_2g, gyro_250) != HAL_OK){
      printf("No Mpu6050 Module, Please check the device connection!\r\n");
      HAL_Delay(1000);
  }
  printf("mpu6050 init success\r\n");
  HAL_Delay(1000);
  float ax, ay, az;  // acc
  float gx, gy, gz;  // gyro
  float temperature; // tem
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
    UpdateIMU();            //update mpu6050 data
    HAL_Delay(20);          //wait 20 ms
    Gimbal_Status_Update(); //gimbal status update
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 84;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */
// update mpu6050 data
void UpdateIMU() {
    float ax, ay, az, gx, gy, gz;
    MPU_Get_RAW_Accelerometer(&ax, &ay, &az);
    MPU_Get_RAW_Gyroscope(&gx, &gy, &gz);  // 假设已转为 °/s

    // 加速度计角度（静态）
    float acc_roll  = atan2f(ay, az) * 180.0f / M_PI;
    float acc_pitch = atan2f(-ax, sqrtf(ay*ay + az*az)) * 180.0f / M_PI;

    // 互补滤波
    roll  = ALPHA * (roll + gx * DT) + (1 - ALPHA) * acc_roll;
    pitch = ALPHA * (pitch + gy * DT) + (1 - ALPHA) * acc_pitch;

    //printf("Roll: %.2f, Pitch: %.2f\r\n", roll, pitch);
}

// gimbal status update
void Gimbal_Status_Update(){
    //calculate servo 1 angle
    if(roll > 16){
        servo_1_angle = map_float(roll, 16.0f, 90.0f, 135.0f, 0.0f);
    }
    else if( (roll <=16) && (roll >= -24) ){
        servo_1_angle = 135;
    }
    else{
        servo_1_angle = map_float(roll, -90.0f, -24.0f, 270.0f, 135.0f);
    }

    //calculate servo 2 angle
    if(pitch > 14){
        servo_2_angle = map_float(pitch, 14.0f, 50.0f, 135.0f, 45.0f);
    }
    else if( (pitch <=14) && (pitch >= -26) ){
        servo_2_angle = 135;
    }
    else{
        servo_2_angle = map_float(pitch, -50.0f, -26.0f, 225.0f, 135.0f);
    }


    if(servo_1_angle <=0 ) { servo_1_angle = 0; }
    if(servo_1_angle >=270) { servo_1_angle = 270; }
    if(servo_2_angle <=45 ) { servo_2_angle = 45; }
    if(servo_2_angle >=225) { servo_2_angle = 225; }

    //printf("ang1: %.2f, ang2: %.2f\r\n", servo_1_angle, servo_2_angle);
    servo_write270(1, servo_1_angle);  //servo1 move to 270 degree
    servo_write270(2, servo_2_angle);  //servo2 move to 225 degree
}

float map_float(float value, float in_min, float in_max, float out_min, float out_max)
{
    // 防止除零错误
    if (in_min == in_max) {
        return out_min; // 或者返回某个默认值
    }

    // 线性映射公式：out = out_min + (out_max - out_min) * (value - in_min) / (in_max - in_min)
    return out_min + (out_max - out_min) * (value - in_min) / (in_max - in_min);
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

操作手順：

全体の流れ：

(1) STM32CubeMX ソフトウェアでピン割り当てと周辺機能の初期化を設定し、設定コードを生成します。

(2) keil5 で MDK_ARM プロジェクトファイルを開き、コードの編集・コンパイル・デバッグを行います。

基本パラメータ：

本ドキュメントの（ハードウェアモジュール）に従って開発環境を設定した後の基本パラメータは次のとおりです。

ソフトウェアバージョン：STM32CubeMX V6.14.0

ハードウェアプラットフォーム：STM32F407IGT6

コンパイル環境：MDK_ARM V5.3.2

デバッグツール：ST-LINK/V2

1. 新規プロジェクトの作成と MCU の設定

1.1 MCU の選択

STM32CubeMX ソフトウェアを開き、File > New Project をクリックします。

2. MCU の絞り込み条件を設定して STM32F407IGT6 を選択し、バージョン欄をクリックしてから、最後に Start Project をクリックしてプロジェクトの設定を開始します。

プロジェクト設定画面は次のとおりです。

1.2 クロック設定とコア設定

Pinout & Configuration の System Core で、RCC（リセットおよびクロック制御） をクリックします。

HSE クロックソースを Crystal/Ceramic Resonator（水晶またはセラミック振動子）に設定します。

設定が完了すると、対応するピン（OSC_IN と OSC_OUT）が自動的に設定されます（下図の緑色の部分を参照）。

機能：

外部高速水晶発振器（HSE）を接続し、システムに高精度のクロック源を供給するために使用します。

**RCC_OSC_IN** 水晶入力ピン。

**RCC_OSC_OUT** 水晶出力ピン（帰還信号）

デフォルト機能：

**PH0**: **OSC_IN**（外部水晶入力）。

**PH1** : **OSC_OUT**（外部水晶出力）。

デバッグインターフェースのピンを設定します。System Core で SYS をクリックし、Debug モードを Serial Wire に設定します。

設定が完了すると、対応するピン配置（pin out）が緑色になります。

緑色の部分をクリックすると、具体的な内容を確認できます。

1.3 クロックツリーの設定

クロックパラメータを順に設定します：1. 入力周波数の値を 8 に変更、2. HSE を選択、3. PLLCLK を選択、4. HCLK パラメータを 84 に変更。その他のパラメータはデフォルトのままで構いません。

ヒント：設定時にはエラーが自動的に検出されます。周波数設定が誤っている場合は、エラーが赤色で表示されます。

2. MCU 初期化パラメータの設定

2.1 USART シリアルポートの設定

PA9 と PA10 をそれぞれ USART1_TX と USART1_RX に設定します。GPIO をクリックし、ピンを設定します（この時点では USART1 が未設定のため、ピンに黄色の警告が表示されます）。

Connectivity — USART1 をクリックし、モードを Asynchronous に設定します（USART1 を設定すると関連ピンが緑色になり、ピン設定が正常であることを示します）。

2.2 プロジェクト管理の設定

デフォルトの画面は次のとおりです。

プロジェクト名を入力します（任意に設定可能）。

プロジェクトの保存先を選択します（C ドライブには保存しないでください）。

コンパイル環境として MDK_ARM を選択します。

バージョン番号 V5.3.2 を選択します。

ファームウェアパッケージ名とバージョン。

（ヒント：デフォルトでは Use latest available version にチェックが入っていますが、ローカル PC に最新バージョンがない場合はエラーになります。その場合はパッケージを自分でダウンロードしてローカルにインストールする必要があります。インストール方法は下記リンクを参照してください。）

UNI-STM32 開発環境の設定【V2.0】（リンク先の「6. よくある設定 Q&A」の Q3 に解決方法があります）

上記の設定が完了すると、下図のようになります。

2.Project Manager > Code Generator ウィンドウを次のように設定します。

3.Project Manager > Advanced Settings ウィンドウで HAL ライブラリを選択します。

プロジェクトを保存します：File — Save Project をクリックします。

3. GPIO 設定 — ジャイロスコープとサーボのパラメータ

3.1 ジャイロスコープの設定

IIC ピンの設定。

(1) GPIO をクリックし、Pinout ビュー画面下部で PB8 を検索すると、対応するピンが点滅します。点滅しているピンを左クリックし、I2C1_SCL を選択します。

PB7 を左クリックし、I2C1_SDA を選択します。（ヒント： I2C1 が未設定のため、ピンに黄色の警告が表示されます）

(2) Connectivity — I2C1 をクリックし、モードを I2C に設定します（設定後、関連ピンが緑色になり、ピン設定が正常であることを示します）。

3.2 サーボ SERVO1 の設定

ピン	タイマー	User Label
PA2	TIM2_CH3	SERVO1

GPIO をクリックしてピン PA2 を探します。PA2 を左クリックして TIM2_CH3 を選択します（注：ピン割り当て完了後、ピンはオレンジ色になります。これはピン割り当てのみが完了し、対応する周辺機能がまだ有効化されていないことを示します）。

Timers パネルをクリックして TIM2 の設定を選択し、Channel3 のドロップダウンから PWM Generation CH3 を選択します。GPIO Settings を左クリックし、PA2 の行をダブルクリックして User Label を SERVO1 に設定します。

ピンの色が緑色に変われば、正常に設定が完了しています。

3.3 サーボ SERVO2 の設定

ピン	タイマー	User Label
PA3	TIM2_CH4	SERVO2

GPIO をクリックしてピン PA3 を探します。PA3 を左クリックして TIM2_CH4 を選択します（注：ピン割り当て完了後、ピンはオレンジ色になります。これはピン割り当てのみが完了し、対応する周辺機能がまだ有効化されていないことを示します）。

Timers パネルをクリックして TIM2 の設定を選択し、Channel4 のドロップダウンから PWM Generation CH4 を選択します。

2つのサーボのパラメータは次のように設定されていることが確認できます。

PA3 の User Label を SERVO2 に設定できます。

プロジェクトファイルを再度保存します。

ヒント：必ずプロジェクトファイルを再度保存してください（主にパラメータを保存するため）。File — Save Project をクリックします。

4. コードの生成

1. GENERATE CODE をクリックしてコードを生成します。

コードが自動的に生成されます。

コード生成が成功しました。

5. ユーザーコードの追加

上記で生成したフレームワークをもとに、keil エディタでユーザーコードを追加し、コンパイルとデバッグを行います。

ヒント 1：例えば、main.c の /* USER CODE BEGIN xx */ と /* USER CODE END xx */ の間にユーザーコードを追加します。

ヒント 2：.c ファイルを独自に作成することもできます。

ヒント 3：ユーザーコードは自分で記述できます。（コード解説）に掲載されている例を参考に、真似しながら学習することもできます。

6. コードのダウンロードとテスト

(1) プロジェクトをダウンロードします。ここではデモを使用します。デモは本ドキュメントの「ソフトウェア設計 — プロジェクト」から直接ダウンロードできます。

参考リンク（UNI-STM32 開発環境の設定【V2.0】の「5. プログラムのダウンロード」）を参照してください。

(2) メインボードのスイッチをオンにし、ジャイロスコープを動かして、2自由度ジンバルの回転を観察します。

実験結果

ジャイロスコープを動かすと、2軸ジンバルの姿勢がリアルタイムに制御されます。例えば、ジャイロスコープを z 軸に沿って回転させると、ジンバル上のサーボも同様に動きます。


Z 方向に沿って（左に回転）すると、ジンバルも左に回転します。	Z 方向に沿って（右に回転）すると、ジンバルも右に回転します。

コード解説

1. ユーザーコードの解説

下図は、プロジェクトファイルに追加したユーザーコードを示しています。

2. ユーザーコードの取り込み方法

まず、ユーザー定義ファイル（.c や .h など）を自分のプロジェクトファイルに取り込み、keil5 IDE で .c ファイルを追加してコンパイルします。

既存のユーザーファイルの取り込み：keil で既存のユーザーファイルを取り込む方法を参照してください。

次に、main.c ファイルにユーザーコードを追加します。

ユーザーコード：/* USER CODE BEGIN xx */ で始まり、/* USER CODE END xx */ で終わります。

(1) まず、参照するプロジェクトファイルのコードを開きます。main.c ファイルは主に stm32cubemx によって自動生成されたものです。このフレームワークをもとに実現したい機能を補完・改良します。特に /* USER xx で始まり終わるコードを確認してください。下図の赤い部分が記入したユーザーコードです。

ユーザーコードをコピーします：/* USER CODE BEGIN 4 */ と /* USER CODE END 4 */ の間のコード（179 行目から 243 行目までがジャイロスコープとパン・チルトの処理コードです）。

(2) 上記の赤枠のコードを自分のプロジェクトファイル main.c にコピーし、コンパイルしてダウンロードし、動作を確認します。

技術サポート

資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。