実験12 相対位置制御による四輪メカナムホイール全方向シャーシの矩形軌跡描画

実験の目的：

PIDアルゴリズムを理解し、PIDでシャーシを正確に制御して矩形軌跡を描きます。
四輪駆動差動シャーシの基本的な運動原理を理解し、プログラムで制御します。
STM32 の GPIO 周辺機能ピン の設定方法を習得します。
STM32CubeMX で新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようにします。
Keil IDE でユーザーコードを追加し、プロジェクトのテストを行えるようにします。

運動の基本原理：

四輪駆動の全方向メカナムホイールシャーシは全方向への平行移動が可能で、そのためには各ホイールの回転方向を制御する必要があります。シャーシの移動方向と各ホイールの回転方向の関係は次のとおりです。


前進	後退

右平行移動	左平行移動

その場で左旋回	その場で右旋回

PIDの基本原理：

PIDコントローラは、産業用制御システムで広く使われているフィードバック制御器です。その名称は、比例（P）・積分（I）・微分（D）という3つの基本的な制御動作に由来します。PID制御の基本原理は次のとおりです。

PIDコントローラの構成

比例制御（P）：現在の誤差（目標値と実際値の差）に基づいて制御します。比例制御は誤差を小さくする働きがありますが、定常偏差が残る場合があります。

数式：

積分制御（I）：過去の誤差の累積に基づいて制御します。積分制御は定常偏差をなくす働きがありますが、システムの応答が遅くなる場合があります。

数式：

微分制御（D）：誤差の変化率に基づいて制御します。微分制御は将来の誤差の変化を予測し、システムの応答速度を高める働きがあります。

数式：

PIDコントローラの出力

PIDコントローラの総出力は、この3つの項の和です。

機能要件：

T字型速度プロファイル（内側ループは速度ループPID）の相対位置モードで、シャーシに矩形軌跡を描かせます。

準備するハードウェア：

STM32メインボード V1.3 *1、ST-Link書き込みケーブル *1、3S電池ボックス *1、リチウム電池 *3、エンコーダ付きモーター30 *4、エンコーダモーターケーブル *4

試作機の構成：

実験7 サスペンション付き矩形フレームおよび四輪駆動全方向メカナムホイールシャーシの構造設計

回路接続：

ハードウェアモジュール

ヒント：開発ボード、環境構築、電源モジュールの基本的な使い方に先に慣れておくことを強くお勧めします。電子モジュールについては必要に応じて参照してください。

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

UNI-STM32開発環境の構築【V2.0】

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

電子モジュールの紹介

ピンの説明

ハードウェア	STM32インターフェース
左前輪モーター	M1
右前輪モーター	M2
左後輪モーター	M3
右後輪モーター	M4

まず モーターインターフェース の位置を全体的に把握してから、ピン表に従って配線してください。

実機の配線

配線のコツ：エンコーダモーター付属のケーブルは逆挿し防止構造になっています。コネクタの向きを確認してから接続してください。

ソフトウェア設計：

設計フロー

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

ソースファイル：FourWheelMacCar_AbsPos.rar

プログラムコード

コアコード：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 "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "motor.h"
#include "DWT_time.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 */

/* 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_TIM1_Init();
  MX_USART1_UART_Init();
  MX_TIM3_Init();
  MX_TIM8_Init();
  MX_TIM5_Init();
  MX_TIM6_Init();
  /* USER CODE BEGIN 2 */
  HAL_Delay(3500);
  Motor_Init();                           //编码电机引脚初始化
  dwt_init();                             //DWT_start
  enable(&m1);                            //M1 编码电机使能
  enable(&m2);                            //M2 编码电机使能
  enable(&m3);                            //M3 编码电机使能
  enable(&m4);                            //M4 编码电机使能

  //设置电机连接轮胎半径为0.0475米,如果没有设置轮胎半径, 则半径默认为 0.0325米
  set_radius(&m1, 0.0475);                //设置M1电机连接的轮子半径0.0475米. 单位：米
  set_radius(&m2, 0.0475);                //设置M2电机连接的轮子半径0.0475米. 单位：米
  set_radius(&m3, 0.0475);                //设置M3电机连接的轮子半径0.0475米. 单位：米
  set_radius(&m4, 0.0475);                //设置M4电机连接的轮子半径0.0475米. 单位：米

  //如果此处省略，则加速度默认为10.0，匀速为6.28，减速度为10.0
  set_t_speed(&m1, 6.0, 3.14, 6.0);       //设置M1电机加速度, 匀速, 减速度
  set_t_speed(&m2, 6.0, 3.14, 6.0);       //设置M2电机加速度, 匀速, 减速度
  set_t_speed(&m3, 6.0, 3.14, 6.0);       //设置M3电机加速度, 匀速, 减速度
  set_t_speed(&m4, 6.0, 3.14, 6.0);       //设置M4电机加速度, 匀速, 减速度
  HAL_TIM_Base_Start_IT(&htim6);          //开启基础定时器TIM6

  float motor_postion = 0.3f;
  /* USER CODE END 2 */

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

    /* USER CODE BEGIN 3 */

    car_abs_pos( motor_postion, 0.0, 0.0);    //小车使用相对位置模式前进0.3米

    car_abs_pos(0.0, -motor_postion, 0.0);    //小车使用相对位置模式右平移0.3米

    car_abs_pos(-motor_postion, 0.0, 0.0);    //小车使用相对位置模式后退0.0米

    car_abs_pos( 0.0, motor_postion, 0.0);    //小车使用相对位置模式左平移0.3米

    car_abs_pos(0.0, 0.0,  1.570796326f);     //小车左转1.570796弧度（也就是兀/2 -> 90°）

    car_abs_pos(0.0, 0.0, -3.141592653f);     //小车右转3.141592弧度（也就是兀 -> 180°）

    car_abs_pos(0.0, 0.0,  1.570796326f);     //小车左转90度

    HAL_Delay(10000);                         //程序等待10秒
  }
  /* 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 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if(htim->Instance == TIM6){
    update(&m1);
    update(&m2);
    update(&m3);
    update(&m4);
  }
}
/* 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 に設定します。

設定が完了すると、対応するピン（pinout）が緑色に変わります。

緑色の部分をクリックすると、詳細を確認できます。

1.3 クロックツリーの設定

クロックパラメータを順番に設定します：1. Input frequency の値を 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】（リンク先の「7. よくある設定Q&A」のQ3に解決方法があります）

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

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

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

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

3. GPIO設定 - モーターピン

M1モーターのパラメータ設定

モーター	ピン	パラメータ	ユーザーラベル
M1	PE13	TIM1_CH3
	PE14	TIM1_CH4
	PD10	GPIO_Input	M1B
	PG2	GPIO_EXTI2	M1A

GPIO をクリックし、PE13、PE14 ピンを探します。

ピン配置図で PE13 を左クリックし、PE13 の機能として TIM1_CH3 を選択します。同様に PE14 を左クリックし、機能として TIM1_CH4 を選択します。詳細は下図を参照してください。（注意：ピン割り当て後のピンはオレンジ色になります。これはピンの割り当てのみが完了し、対応する周辺機能がまだ有効化されていないことを示します。）

左側の Timers パネルをクリックし、TIM1 を選択してパラメータを設定します。

Channel3（チャネル3）：PWM Generation CH3
Channel4（チャネル4）：PWM Generation CH4

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

GPIO をクリックし、画面下部で PD10 を検索すると、ピンビュー上の PD10 が点滅します。

PD10 を左クリックし、GPIO_Input を選択します。

ユーザーラベルを設定します：Configuration で PD10 の行をダブルクリックし、右側の GPIO mode のドロップダウンで Input mode を選択して、User Label に M1B と入力します。

GPIO をクリックし、画面下部で PG2 を検索すると、ピンビュー上の PG2 が点滅します。

PG2 を左クリックし、GPIO_EXTI2 を選択します。

ユーザーラベルを設定します：Configuration で PG2 の行をダブルクリックし、右側の GPIO mode のドロップダウンで External Interrupt Mode with Rising/Falling edge trigger detection を選択して、User Label に M1A と入力します。

M2モーターのパラメータ設定

モーター	ピン	パラメータ	ユーザーラベル
M2	PB0	TIM3_CH3
	PB1	TIM3_CH4
	PH14	GPIO_Input	M2B
	PH15	GPIO_EXTI15	M2A

M1 の設定方法を参考にしてください。M2モーターのパラメータ設定後の状態は次のとおりです。

M3モーターのパラメータ設定

モーター	ピン	パラメータ	ユーザーラベル
M3	PC8	TIM8_CH3
	PC9	TIM8_CH4
	PE9	GPIO_EXTI9	M3A
	PE11	GPIO_Input	M3B

M1 の設定方法を参考にしてください。M3モーターのパラメータ設定後の状態は次のとおりです。

M4モーターのパラメータ設定

モーター	ピン	パラメータ	ユーザーラベル
M4	PH10	TIM5_CH1
	PH11	TIM5_CH2
	PD12	GPIO_EXTI12	M4A
	PD13	GPIO_Input	M4B

M1 の設定方法を参考にしてください。M4モーターのパラメータ設定後の状態は次のとおりです。

割り込みの設定

タイマーの役割の比較：

モーター用タイマー（Timxx）：モーターのPWM出力に使用します。

TIM6：制御アルゴリズムの実行周期用タイマー

（1）TIM6 をクリックして Activated にチェックを入れます。下の NVIC Settings をクリックし、Enabled にチェックを入れます。

（2）NVIC Settings をクリックし、モーターの外部割り込みについて Enabled にチェックを入れます。

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

ヒント：プロジェクトファイルは必ずもう一度保存してください（主にパラメータを保存するためです）。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）電池スイッチとメインボードのスイッチをオンにして、動作を確認します。

実験結果

車体が0.3m前進、0.3m右へ平行移動、0.3m後退、0.4m左へ平行移動し、左に90度旋回、右に180度旋回、再び左に90度旋回します。プログラムが10秒待機して、矩形軌跡の描画が1回完了します。その後、これを繰り返します。

コードの解説

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

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

2. ユーザーコードのインポート方法

1つ目は、ユーザー定義ファイル（.c、.h など）を自分のプロジェクトフォルダにインポートし、Keil5 IDE で .c ファイルを追加してコンパイルする方法です。

既存のユーザーファイルのインポート：Keilで既存のユーザーファイルをインポートする方法を参照してください。

2つ目は、main.c ファイルにユーザーコードを追加する方法です。

ユーザーコード：/* USER CODE BEGIN xx */ で始まり、/* USER CODE END xx */ で終わる部分が目印です。

（1）まず参考プロジェクトのコードを開きます。main.c ファイルは主に STM32CubeMX によって自動生成されたものです。このフレームワークをもとに実現したい機能を追加・補完しています。具体的には /* USER xx */ で始まり終わる部分のコードを確認してください。下図の赤枠部分が追加したユーザーコードです。

（2）上記の赤枠部分のコードを自分のプロジェクトの main.c にコピーし、コンパイル・書き込みを行うと動作を確認できます。

技術サポート

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