実験6 ビジョンによる追従 — 4輪メカナムホイールシャーシによる移動物体の追跡

実験の目的：

DMA の基本的な動作原理を理解し、DMA を用いて AI ビジョンモジュールが取得したデータを扱えるようにします。
4輪駆動メカナムホイールシャーシの運動原理に基づき、AI ビジョンモジュールと組み合わせて追従を実現します。
STM32 の GPIO 周辺機能ピンの設定方法を習得します。
STM32CubeMX で新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようにします。
keil IDE でユーザーコードを追加し、プロジェクトのテストを行えるようにします。

DMA の基本原理：

DMA：Direct Memory Access（ダイレクトメモリアクセス）の略です。DMA 転送は、あるアドレス空間から別のアドレス空間へデータをコピーするもので、周辺機器とメモリ間、またはメモリ間で高速なデータ転送を実現します。

DMA の転送モード：DMA の役割は直接的なデータ転送を実現することで、従来のデータ転送で必要だった CPU レジスタの関与を省きます。主に次の4種類のデータ転送があります。

周辺機器からメモリへ
メモリから周辺機器へ
メモリからメモリへ
周辺機器から周辺機器へ

周辺機器とメモリ間で通信する際のデータの流れは次のとおりです。

4輪駆動メカナムホイールシャーシの基本運動原理：

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


前進	後退

右平行移動	左平行移動

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

応用シーン

自動化された生産ラインでは、産業用ロボットが物体追跡技術を用いてワークの位置と動きをリアルタイムに追跡し、正確な把持・組立・搬送などの作業を実現して、生産効率と製品品質を向上させることができます。

機能要件：

4輪駆動メカナムホイールシャーシで移動物体（例えば青い物体）を追跡します。

ハードウェアの準備：

STM32 メインボード V1.3 * 1、ST-Link ダウンロードケーブル * 1、3S バッテリーボックス * 1、リチウム電池 * 3、AI ビジョンモジュール * 1、Type-C データケーブル * 1

試作構成：

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

回路接続：

ハードウェアモジュール

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

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

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

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

電子モジュールの紹介（クリックするとモーターと AI ビジョンモジュールの基本的な紹介を確認できます）

ピンの紹介

まず モーター M1～M4、AI ビジョンモジュール（ K210 ）のインターフェース の位置を全体的に把握し、その後ピン表に従って配線を完了します。

シャーシモーター	STM32 インターフェース	AI モジュール（K210）	STM32 開発ボード
左前輪モーター	M1	5V	5V
右前輪モーター	M2	GND	GND
左後輪モーター	M3	RX	USART3_TX
右後輪モーター	M4	TX	USART3_RX

このうち K210 インターフェース のピン位置は下図のとおりです。ピン表に従って配線を完了してください。

実際の配線

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

ソフトウェア設計：

設計フロー

STM32 は K210 ビジョンモジュールから目標の座標とサイズを受信し、領域分割＋領域フィードバック によって車両が自動的に目標を追跡します。

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

ソースファイル：17 Car_Vision_Tracking.zip

プログラムコード

プロジェクトの完全なコードは（ソースファイル）をクリックしてダウンロードしてください。

AI ビジョンモジュール（K210）のプログラム

1. 物体の Lab 色域値を取得する

'''
功能：实现检测特定物体，并将该物体的lab色域值范围打印出来
日期：2026-04-09
'''

import sensor
import image
import time
import lcd

#载入按键控制模块
from hiwonder import hw_key, hw_led , fill_light

#初始化LCD
lcd.init()
#以下是初始化传感器
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 100)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)

#创建LED控制对象
led = hw_led()
#创建补光灯控制对象
f_led = fill_light()

#LED,补光灯开
#led.led_onoff(1)    #LED开
#led.led_onoff(0)    #LED关
#f_led.fill_onoff(0) #补光灯开
#time.sleep(0.5)  #延时0.5s

#帧率时钟
clock = time.clock()

#方框参数
r = [(320//2)-(50//2), (240//2)-(50//2), 50, 50]

#创建按键控制对象
key = hw_key()

state = key.key_scan(1)

#显示图像，等待按键按下
while (state == 0):
    state = key.key_scan(1)
    img = sensor.snapshot()
    img.draw_rectangle(r)
    lcd.display(img)

print("Start learning ...")
threshold = [50, 50, 0, 0, 0, 0] # Middle L, A, B values.

#第一次赋值
img = sensor.snapshot()
hist = img.get_histogram(roi=r)
lo = hist.get_percentile(0.01) #获得直方图在1%范围内的CDF(根据需要调整)!
hi = hist.get_percentile(0.99) #获得直方图在99%范围内的CDF(根据需要调整)!
threshold[0] = (threshold[0] + lo.l_value()) // 2
threshold[1] = (threshold[1] + hi.l_value()) // 2
threshold[2] = (threshold[2] + lo.a_value())
threshold[3] = (threshold[3] + hi.a_value())
threshold[4] = (threshold[4] + lo.b_value())
threshold[5] = (threshold[5] + hi.b_value())

#连续捕捉了50张图像学习阶段
for i in range(50):
    img = sensor.snapshot()
    hist = img.get_histogram(roi=r)
    lo = hist.get_percentile(0.01) #获得直方图在1%范围内的CDF(根据需要调整)!
    hi = hist.get_percentile(0.99) #获得直方图在99%范围内的CDF(根据需要调整)!
    #hi = hist.get_percentile(0.70) #获得直方图在99%范围内的CDF(根据需要调整)!
    #以百分位数表示的平均值
    threshold[0] = (threshold[0] + lo.l_value()) // 2
    threshold[1] = (threshold[1] + hi.l_value()) // 2
    threshold[2] = (threshold[2] + lo.a_value()) // 2
    threshold[3] = (threshold[3] + hi.a_value()) // 2
    threshold[4] = (threshold[4] + lo.b_value()) // 2
    threshold[5] = (threshold[5] + hi.b_value()) // 2
    #将识别区域用方框圈出来
    for blob in img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
        img.draw_rectangle(r, color=(0,255,0))
    lcd.display(img)
print("threshold:", threshold)

#loop
while(True):
    #用于计算帧率的函数，这里表示开始计时
    clock.tick()
    #从传感器捕获一张图像
    img = sensor.snapshot()
    #遍历图像中找到的颜色区块
    for blob in img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10):
        #绘制矩形和十字标记
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
    #显示在LCD上
    lcd.display(img)
    #打印帧率
    #print(clock.fps())

2. Lab 色域値をシリアルポートプログラムに書き込む

'''
功能: 将检测得到的红色/蓝色物体数据通过封装的串口通讯协议发送
日期：2026-04-09
'''

import binascii
import sensor
import image
import time
import lcd
import _thread
from hiwonder import hw_key, hw_led , fill_light, hw_uart

# 创建串口对象
serial = hw_uart()

"""
视觉颜色检测封装类
"""
class VisionModule:
    def __init__(self):
        # 初始化LCD
        lcd.init()
        # 初始化传感器
        sensor.reset()
        sensor.set_pixformat(sensor.RGB565)
        sensor.set_framesize(sensor.QVGA)
        sensor.skip_frames(time = 100)
        sensor.set_auto_gain(False)
        sensor.set_auto_whitebal(False)

        # 帧率时钟
        self.clock = time.clock()

        # 创建LED控制对象
        led = hw_led()
        # 创建补光灯控制对象
        f_led = fill_light()

        # 打开补光灯（根据需要修改）
        #led.led_onoff(1)    # LED开
        #led.led_onoff(0)    # LED关（若需关闭则取消注释）
        #f_led.fill_onoff(1) # 补光灯开
        #time.sleep(0.5)     # 延时0.5s

        #LED,补光灯开
        led.led_onoff(1)    #LED开
        led.led_onoff(0)    #LED关
        f_led.fill_onoff(0) #补光灯开
        time.sleep(0.5)  #延时0.5s

        # 储存颜色的LAB阈值（示例仅红、蓝，可自行添加更多）
        self.color_thresholds = [
            #(42, 80, 21, 54, 22, 42),       #Red1
            #(37, 76, 21, 45, 37, 51),       #Red2
            #(33, 81, 28, 55, -4, 19),       #Red3
            #(44, 76, 24, 52, 41, 53),       #Red4
            #(29, 57, 37, 52, 21, 37),       #Red5
            (29, 81, 21, 55, -4, 53),        #Red 红色色域值数据融合结果

            #(28, 71, -33, -14, -11, 4),     #blue1
            #(24, 71, -4, 12, -41, -21),     #blue2
            #(46, 78, -6, 15, -53, -33),     #blue3
            #(31, 71, -20, -4, -27, -6),     #blue4
            #(21, 53, -18, -2, -25, -4),     #blue5
            #(45, 74, -10, 7, -45, -23),     #blue6
            #(29, 76, -41, -22, -7, 14),     #blue7
            #(38, 79, -27, -12, -21, -3),    #blue8
            #(21, 79, -41, 15, -53, 14),     #Blue 蓝色色域值数据融合结果
            #(43, 57, -6, 12, -46, -24),       #Blue 蓝色色域值数据融合结果
            (10, 98, -2, 27, -46, -16),

            # (23, 52, -29, -9, -9, 7),     # Green
            # (65, 78, -10, -5, 38, 50),    # Yellow
            # (20, 50, 17, 37, -34, -14),   # purple
        ]

        self.color_strings = ['Red', 'Blue', 'Green', 'Yellow', 'purple', 'Other1', "Other2", "Other3", "Other4", "Other5"]

class FuncTag:
    """功能号定义"""
    START = 0
    FIND_MULTI_COLOR = 0x10

def str_2_int(data_str):
    """字符串转int"""
    bb = binascii.hexlify(data_str)
    bb = str(bb)[2:-1]
    int_h = int(bb[0]) * 16
    int_l = int(bb[1], 16)
    return int_h + int_l

def int_to_hex_str(byte_val):
    """将整数转换为两位十六进制字符串"""
    hex_str = hex(byte_val)
    # 去掉 '0x' 前缀，并转换为大写
    hex_str = hex_str[2:].upper()
    # 确保是两位
    if len(hex_str) == 1:
        hex_str = '0' + hex_str
    return hex_str

def send_data(img, x, y, w, h, msg):
    '''
    发送数据（协议头 0xAA 0x55 + 功能号 + 数据长度 + 数据 + 回车符）
    将原来的CRC校验位替换为\n（回车）

    Args:
        x: 检测框中心x坐标
        y: 检测框中心y坐标
        w: 检测框宽度
        h: 检测框高度
        msg: 消息内容
    '''
    START_1 = 0xAA
    START_2 = 0x55
    FUNC_num = FuncTag.FIND_MULTI_COLOR  # 功能编号
    Length = 0  # 数据长度
    data = []  # 数据组

    # 参数都为0
    if x == 0 and y == 0 and w == 0 and h == 0:
        pass
    else:
        # x(小端模式)
        low = x & 0xFF  # 低位
        high = x >> 8 & 0xFF  # 高位
        data.append(low)
        data.append(high)

        # y(小端模式)
        low = y & 0xFF  # 低位
        high = y >> 8 & 0xFF  # 高位
        data.append(low)
        data.append(high)

        # w(小端模式)
        low = w & 0xFF  # 低位
        high = w >> 8 & 0xFF  # 高位
        data.append(low)
        data.append(high)

        # h(小端模式)
        low = h & 0xFF  # 低位
        high = h >> 8 & 0xFF  # 高位
        data.append(low)
        data.append(high)

    # msg
    color_r = 0
    color_g = 0
    color_b = 0
    position_x = 0
    position_y = 0
    if msg is not None:
        if msg == "Red":
            color_r = 255
            color_g = 0
            color_b = 0
            position_x = 46
            position_y = 0
            img.draw_string(0, 0, "SenRed:", color=(color_r, color_g, color_b), scale=1)
        elif msg == "Blue":
            color_r = 0
            color_g = 0
            color_b = 255
            position_x = 52
            position_y = 20
            img.draw_string(0, 20, "SendBlue:", color=(color_r, color_g, color_b), scale=1)
        else:
            color_r = 255
            color_g = 255
            color_b = 255
            position_x = 56
            position_y = 30
            img.draw_string(0, 30, "SendOther:", color=(color_r, color_g, color_b), scale=1)
        for i in range(len(msg)):
            msg_int = str_2_int(msg[i])
            data.append(msg_int)

    Length += len(data)
    send_buf = [FUNC_num, Length]
    for i in range(len(data)):
        send_buf.append(data[i])

    send_buf.insert(0, START_1)  # 插入协议头1
    send_buf.insert(1, START_2)  # 插入协议头2
    send_buf.append(0x0A)  # 加入回车符 \n (0x0A)，替换原来的CRC校验位

    serial.send_bytearray(send_buf)  # 发送数据

    # 将字节数组转换为十六进制字符串显示
    hex_str_list = [int_to_hex_str(b) for b in send_buf]
    hex_str = ' '.join(hex_str_list)
    img.draw_string(position_x, position_y, hex_str, color=(color_r, color_g, color_b), scale=1)

def main():
    # 创建视觉检测管理实例
    vision = VisionModule()
    while True:
        #用于计算帧率的函数，这里表示开始计时
        #从传感器捕获一张图像
        img = sensor.snapshot()
        #遍历多种颜色
        for color_idx, threshold in enumerate(vision.color_thresholds):
            #遍历图像中找到的颜色区块
            blobs = img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10)
            #是否找到
            if blobs:
                area_max = 0
                blob_max = blobs[0]
                #寻找最大的颜色块
                for blob in blobs:
                    area = blob.w()*blob.h()
                    if(area_max < area):
                        blob_max = blob
                        area_max = area
                if area_max < 2000:
                    continue
                #画方框
                color_set = (255, 255, 255)
                if color_idx == 0:
                    color_set = (255, 0, 0)      # 红色框
                elif color_idx == 1:
                    color_set = (0, 0, 255)      # 蓝色框
                elif color_idx == 2:
                    color_set = (0, 255, 0)      # 绿色框
                else:
                    color_new = (255, 255, 255)
                img.draw_rectangle(blob_max.rect(), color=color_set, thickness=5)
                img.draw_cross(blob_max.cx(), blob_max.cy(), color=(255, 255, 0), thickness=5)
                img.draw_string(blob_max.cx() + 10, blob_max.cy() - 10, vision.color_strings[color_idx], color=(color_set))
                #将方框数据与消息赋值
                send_x = blob_max.cx()
                send_y = blob_max.cy()
                send_w = blob_max.w()
                send_h = blob_max.h()
                #发送数据
                send_data(img, send_x,send_y,send_w,send_h, vision.color_strings[color_idx])

        #显示在LCD上
        lcd.display(img)
        #打印帧率
        #print(clock.fps())

if __name__ == '__main__':
    main()

下位機 stm32 のメインプログラム

以下に示す stm32 のメインプログラムは 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 "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "motor.h"
#include "config.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_DMA_Init();
    MX_TIM1_Init();
    MX_USART1_UART_Init();
    MX_TIM3_Init();
    MX_TIM8_Init();
    MX_TIM5_Init();
    MX_TIM6_Init();
    MX_USART3_UART_Init();
    /* USER CODE BEGIN 2 */
    HAL_Delay(20);
    Motor_Init();                           //motor init
    dwt_init();                             //DWT_start
    enable(&m1);                            //M1 motor enable
    enable(&m2);                            //M2 motor enable
    enable(&m3);                            //M3 motor enable
    enable(&m4);                            //M4 motor enable

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

  //If omitted here, the acceleration defaults to 10.0, the uniform velocity to 6.28, and the deceleration to 10.0
  set_t_speed(&m1, 6.0, 3.14, 6.0);       //set m1 T speed (include: acc,uniform,dec)
  set_t_speed(&m2, 6.0, 3.14, 6.0);       //set m2 T speed (include: acc,uniform,dec)
  set_t_speed(&m3, 6.0, 3.14, 6.0);       //set m3 T speed (include: acc,uniform,dec)
  set_t_speed(&m4, 6.0, 3.14, 6.0);       //set m4 T speed (include: acc,uniform,dec)
  HAL_TIM_Base_Start_IT(&htim6);          //Base TIM6 start

  HAL_UART_Receive_IT(&huart3, &rx_data, 1);

  float vel_y = 0.0f;
  float vel_x = 0.0f;
  uint16_t area = 0;
  uint16_t max_area = 4000;
  uint16_t min_area = 3200;
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */

  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */

    //Check whether new visual data has been received from the host computer
    if(recv_flag == true){

        //step 1：Convert pixel coordinates to region indices (0~63)
        int8_t cur_zone = received_data.x / ZONE_WIDTH;

        //Boundary protection: Ensure that cur_zone remains within the valid range [0, TOTAL_ZONES-1]
        if(cur_zone < 0) cur_zone = 0;
        if(cur_zone >= TOTAL_ZONES) cur_zone = TOTAL_ZONES - 1;

        //step 2：Initialize the lateral velocity variable
        vel_y = 0.0f;
        vel_x = 0.0f;
        area = 0;
        area = received_data.w * received_data.h;

        //step 3：Calculate speed based on the target area
        //The target is in the left area (area 0~29)
        //The closer the target is to the left, the faster the car should move left to track it
        if(cur_zone < DEAD_ZONE_START){

            //Calculate the "distance" from the boundary of the dead zone
            //When offset = 0, cur_zone = 29 (nearest to the center, with the slowest speed)
            //When offset = 29, cur_zone = 0 (furthest from the center, fastest speed)
            int16_t offset = DEAD_ZONE_START - 1 - cur_zone;

            // Linear interpolation formula for speed:
            // When offset=0: vel_y = -VEL_MIN (the slowest leftward movement speed)
            // When offset=29: vel_y = -VEL_MAX (the fastest leftward moving speed)
            // Formula: vel = base value + (range) * (progress ratio)
            vel_y = -VEL_MIN - (VEL_MAX - VEL_MIN) * (float)offset / (DEAD_ZONE_START - 1);
        }

        //The target is in the right area (area 35~63)
        //The closer the target is to the right, the faster the car should move to the right to track it
        else if(cur_zone > DEAD_ZONE_END){

            int16_t offset = cur_zone - (DEAD_ZONE_END + 1);
            vel_y = VEL_MIN + (VEL_MAX - VEL_MIN) * (float)offset / (TOTAL_ZONES - DEAD_ZONE_END - 1);
        }

        //30~34
        else{
            vel_y = 0.0f;
            if(area > max_area){
                vel_x = -0.06f;
            }
            else if(area < min_area){
                vel_x = 0.06f;
            }
            else{
                vel_x = 0.0f;
            }
        }

        //step4： Execute motion command
        car_move(vel_x, -vel_y, 0.0f);
        //printf("area:%d\r\n", area);

        //Clear the received flag and reset received_data for the next iteration
        clear();
    }

    //No valid visual data has been received
    else{
        car_move(0.0f, 0.0f, 0.0f);
    }
    HAL_Delay(100);

  }
  /* 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 に設定します。

設定が完了すると、対応するピン配置（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】（リンク先の「7. よくある設定 Q&A」の Q3 に解決方法があります）

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

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

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

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

3. GPIO 設定

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

モーター	ピン	パラメータ	User Label
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：PWM Generation CH3
Channel4（チャネル4）：PWM Generation CH4

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

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

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

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

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

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

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

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

モーター	ピン	パラメータ	User Label
M2	PB0	TIM3_CH3
	PB1	TIM3_CH4
	PH14	GPIO_Input	M2B
	PH15	GPIO_EXTI15	M2A

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

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

モーター	ピン	パラメータ	User Label
M3	PC8	TIM8_CH3
	PC9	TIM8_CH4
	PE9	GPIO_EXTI9	M3A
	PE11	GPIO_Input	M3B

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

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

モーター	ピン	パラメータ	User Label
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 にチェックを入れます。

AI ビジョンモジュールの設定

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

(3) 割り込みの設定。NVIC Settings をクリックし、USART3 の割り込みを Enabled に設定します（チェックボックスにチェック）。

(4) DMA の設定。DMA Settings をクリックし、Add ボタンをクリックします。

表示された Select 列で Select をクリックし、USART3_TX を選択します。

選択後、表示されるタブはデフォルトのままで構いません。パラメータは下図のように設定します。

続けて Add をクリックし、表示された Select 列で Select をクリックして USART3_RX を選択します。

設定結果は下図のとおりです。これで DMA の設定は完了です。

プロジェクトファイルの再保存

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

4. コードの生成

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

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

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

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

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

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

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

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

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

プロジェクトをダウンロードします。ここではデモを用意しています（本ドキュメントの「ソフトウェア設計 — プロジェクトのダウンロード」から直接ダウンロードできます）。
本ドキュメントの 回路接続 に従って実際の配線を完了します。
STM32 プログラムを書き込みます。Keil で拡張子 .uvprojx のプロジェクトファイルを開き、Load します。

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

注：STM32 開発ボードへのプログラムのダウンロード後、ST-Link を PC から取り外してください。

AI ビジョンモジュール（K210）にプログラムをダウンロードします。オフラインで動作させる必要があります（ヒント： オフラインモードでダウンロードすると、その後は K210 ビジョンモジュールに電源を入れるだけで、PC に接続していなくても MicroPython ファイルが実行され、オフライン動作が実現されます）。

(1) Type-C データケーブルの一端を AI モジュールに、もう一端を PC の USB ポートに接続します。

(2) CanMV IDE で serial_send_msg.py を開きます。

(3) 接続ボタンをクリックすると接続画面が表示されます。Advanced Settings にチェックを入れ、Mode-3 を選択して OK をクリックし、接続を開始します（注：シリアルポートは K210 モジュールが接続されているポート番号です。不明な場合は、いったん PC の USB ケーブルを抜いてから K210 モジュールを PC に接続し、新たに追加されたポート番号が該当のシリアルポート番号になります）。

接続が完了すると、下図のように接続成功のマークが表示されます。

(4) 接続に成功したら、ツールバーの「Tools」をクリックし、「Save the currently opened script as (main.py) to CanMV Cam」を選択します。

(5) 続いて「Yes」をクリックします。

(6) 書き込みに成功するとメッセージが表示されるので、「OK」をクリックします。これで MicroPython ファイルが K210 ビジョンモジュールに保存されます。

バッテリースイッチと STM32 開発ボードのスイッチをオンにすると、プログラムが動作を開始します。物体を動かして動作を確認してください。

実験結果

物体の移動に合わせて、シャーシがビジョンによる追従を行います。この例では、青い物体が移動すると、シャーシが移動して追従します。

コード解説

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

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

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

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

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

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

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

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

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

技術サポート

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