実験2 四輪駆動・四輪全方向メカナムシャーシのライントレース

実験の目的：

モーターの PID 制御の応用と、四輪駆動メカナムシャーシの運動学モデリングを理解すること。
ライントレースの原理と組み合わせて、シャーシのライントレースを実現すること。
STM32 の GPIO 周辺機能ピン を設定する方法を習得すること。
STM32CubeMX で新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようになること。
Keil IDE でユーザーコードを追加し、プロジェクトのテストを行えるようになること。

ライントレースの原理：

ロボットのライントレースには、対応するライントレースセンサーを使用する必要があります。一般的なライントレースでは、黒線の両側のエッジを検出します。理解しやすいように、ロボットに黒線を検出するセンサーが左右に 1 個ずつ、計 2 個搭載されていると仮定すると、センサーの反応と車体の動きの関係は以下のとおりです。


両側のセンサーがどちらも反応していない場合、車体はずれていないことを示し、車体は直進します。	左側のセンサーが反応した場合、車体が右にずれていることを示し、車体を左に旋回させて位置を修正する必要があります。	右側のセンサーが反応した場合、車体が左にずれていることを示し、車体を右に旋回させて位置を修正する必要があります。

機能要件：

シャーシロボットが特定のラインに沿って走行します。たとえば、床に黒線を敷いてルートとし、ロボットを指定したルートに沿って走行させます。

ハードウェアの準備：

STM32 メインボード V1.3 × 1、ST-Link ダウンロードケーブル × 1、3S バッテリーボックス × 1、リチウムバッテリー × 3、4 チャンネルライントレースモジュール × 1、エンコーダーモーター 30 × 4、エンコーダーモーターケーブル 2.54 × 4

プロトタイプ構成：

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

回路接続：

ハードウェアモジュール

ヒント：ハードウェアおよびソフトウェア環境の設定

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

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

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

電子モジュールの紹介

ピンの説明

シャーシのハードウェア	STM32 インターフェース	4 チャンネルライントレースモジュールのピン	STM32 のピン
エンコーダー付き DC モーター	M3	3.3V	3.3V
エンコーダー付き DC モーター	M4	GND	GND
エンコーダー付き DC モーター	M5	S1	PC2
エンコーダー付き DC モーター	M6	S2	PC3
		S3	PC4
		S4	PC5

実機の配線

ソフトウェア設計：

設計フロー

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

ソースコードのリンク：（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 "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "motor.h"
#include "string.h"
#include "DWT_time.h"
#include "line_sensor.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 */
float vel = 0.2;             //set car speed
float vel_pwm = 150;
float vx_vel_pwm = 80;
float vw_vel_pwm = 200;
uint8_t sum_line_sensor = 0; //Record the sum of the displacement
                             //of the four line patrol sensors
uint8_t LineSensorData[4];
/* 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_USART1_UART_Init();
  MX_TIM8_Init();
  MX_TIM5_Init();
  MX_TIM6_Init();
  /* USER CODE BEGIN 2 */
  HAL_Delay(80);
  Motor_Init();                           //motor init
  dwt_init();                             //DWT_start
  enable(&m1);                            //m3 enable
  enable(&m2);                            //m4 enable
  enable(&m3);                            //m5 enable
  enable(&m4);                            //m6 enable

  //If omitted here, the radius defaults to 0.0325
  set_radius(&m1, 0.0475);                //set m3 wheel radius. unit: m
  set_radius(&m2, 0.0475);                //set m4 wheel radius. unit: m
  set_radius(&m3, 0.0475);                //set m5 wheel radius. unit: m
  set_radius(&m4, 0.0475);                //set m6 wheel radius. unit: m

  HAL_TIM_Base_Start_IT(&htim6);          //Base TIM6 start
  /* USER CODE END 2 */

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

    /* USER CODE BEGIN 3 */

    ///*
    sum_line_sensor = read_linesensor_int();
    //printf("line_sensor_sum: %d\r\n", sum_line_sensor);
    //HAL_Delay(20);
    ///*
    switch(sum_line_sensor){
        //sensor1 trigger, sensor2,sensor3,sensor4 didn't trigger
        case 14:
            car_move_pwm( 0.0,  0.0, vw_vel_pwm);
            break;
        //sensor1,sensor2 trigger, sensor3,sensor4 didn't trigger
        case 12:
            car_move_pwm( 0.0,  0.0, vw_vel_pwm * 0.75);
            break;
        //sensor1,sensor2,sensor3 trigger, sensor4 didn't trigger
        case 8:
            car_move_pwm( 0.0,  0.0, vw_vel_pwm);
            break;

        //sensor4 trigger, sensor1,sensor2,sensor3 didn't trigger   
        case 7:
            car_move_pwm( 0.0,  0.0, -vw_vel_pwm);
            break;
        //sensor3,sensor4 trigger, sensor1,sensor2 didn't trigger   
        case 3:
            car_move_pwm( 0.0,  0.0, -vw_vel_pwm * 0.75);
            break;
        case 1:
            car_move_pwm( 0.0,  0.0, -vw_vel_pwm);
            break;
        default:
            car_move_pwm( vx_vel_pwm,  0.0,  0.0);
            break;

    }
    //*/
  }
  /* 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】（リンク先の「6. よくある設定 Q&A」の Q3 に解決方法があります）

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

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

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

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

3. GPIO の設定：モーター 4 個とライントレースモジュール

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

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

GPIO をクリックし、PC8、PC9 ピンを探します。

ピン配置図で PC8 を左クリックして機能を TIM8_CH3 に、PC9 を左クリックして機能を TIM8_CH4 に設定します（下図参照）。（注意：ピン割り当てが完了するとピンはオレンジ色になります。これはピン割り当てのみが完了し、対応する周辺機能がまだ有効化されていないことを示します。）

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

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

これでピンの色が緑色に変わり、設定が正常に完了したことを示します。

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

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

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

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

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

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

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

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

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

モーター	ピン	パラメータ	ユーザーラベル
M5	PC6	TIM8_CH1
	PC7	TIM8_CH2
	PA0	GPIO_EXTI0	M5A
	PA1	GPIO_Input	M5B

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

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

モーター	ピン	パラメータ	ユーザーラベル
M6	PH12	TIM5_CH3
	PI0	TIM5_CH4
	PA6	GPIO_EXTI6	M6A
	PA7	GPIO_Input	M6B

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

モーターの割り込み設定

タイマーの役割の比較：

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

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

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

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

ライントレースモジュールのパラメータ設定

ピン	機能
PC2	Input
PC3	Input
PC4	Input
PC5	Input

PC2 ピンの設定

（1）GPIO をクリックし、Pinout ビュー画面の下部で PC2 を検索すると、ピンビュー上の PC2 が点滅します。PC2 を左クリックし、GPIO_Input を選択します。

列 1	列 2

（2）PC2 の行をダブルクリックし、GPIO Pull-up/Pull-down の項目でドロップダウンをクリックして Pull-up を選択します。

図中の設定の意味

Pull-up：対応するピンの内部プルアップ抵抗を有効にし、デフォルトで High レベルになります。

Pull-down：内部プルダウン抵抗を有効にし、デフォルトで Low レベルになります。

No pull-up/pull-down：プルアップ・プルダウンを無効にします。安定したレベルを確保するには外部回路が必要です。

同様の方法で、PC3、PC4、PC5 ピンの設定を完了します。最終的な状態は以下のとおりです。

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

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

実験結果

シャーシロボットが特定のラインに沿って走行します。床に黒線を敷いてルートとし、ロボットが指定したルートに沿って走行します。

コード解説

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

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

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

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

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

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

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

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

 /* USER CODE BEGIN 2 */
  HAL_Delay(80);
  Motor_Init();                           //motor init
  dwt_init();                             //DWT_start
  enable(&m1);                            //m3 enable
  enable(&m2);                            //m4 enable
  enable(&m3);                            //m5 enable
  enable(&m4);                            //m6 enable

  //If omitted here, the radius defaults to 0.0325
  set_radius(&m1, 0.0475);                //set m3 wheel radius. unit: m
  set_radius(&m2, 0.0475);                //set m4 wheel radius. unit: m
  set_radius(&m3, 0.0475);                //set m5 wheel radius. unit: m
  set_radius(&m4, 0.0475);                //set m6 wheel radius. unit: m

  HAL_TIM_Base_Start_IT(&htim6);          //Base TIM6 start
  /* USER CODE END 2 */

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

    /* USER CODE BEGIN 3 */

    ///*
    sum_line_sensor = read_linesensor_int();
    //printf("line_sensor_sum: %d\r\n", sum_line_sensor);
    //HAL_Delay(20);
    ///*
    switch(sum_line_sensor){
        //sensor1 trigger, sensor2,sensor3,sensor4 didn't trigger
        case 14:
            car_move_pwm( 0.0,  0.0, vw_vel_pwm);
            break;
        //sensor1,sensor2 trigger, sensor3,sensor4 didn't trigger
        case 12:
            car_move_pwm( 0.0,  0.0, vw_vel_pwm * 0.75);
            break;
        //sensor1,sensor2,sensor3 trigger, sensor4 didn't trigger
        case 8:
            car_move_pwm( 0.0,  0.0, vw_vel_pwm);
            break;

        //sensor4 trigger, sensor1,sensor2,sensor3 didn't trigger   
        case 7:
            car_move_pwm( 0.0,  0.0, -vw_vel_pwm);
            break;
        //sensor3,sensor4 trigger, sensor1,sensor2 didn't trigger   
        case 3:
            car_move_pwm( 0.0,  0.0, -vw_vel_pwm * 0.75);
            break;
        case 1:
            car_move_pwm( 0.0,  0.0, -vw_vel_pwm);
            break;
        default:
            car_move_pwm( vx_vel_pwm,  0.0,  0.0);
            break;

    }
    //*/
  }
  /* USER CODE END 3 */

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

技術サポート

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