実験7 ビジョン認識によるロボットアームの仕分け・搬送プロジェクト

実験の目的：

AI ビジョンモジュールで取得した画像データから物体を認識します。
4自由度直列型ロボットアームの運動学アルゴリズムに基づき、AI ビジョンモジュールと組み合わせて仕分け・搬送を実現します。
STM32 の GPIO 周辺機能ピンの設定方法を習得します。
STM32CubeMX で新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようにします。
keil IDE でユーザーコードを追加し、プロジェクトのテストを行えるようにします。

基本原理：

3軸ロボットアームの先端にスイングモジュールを追加することで、エンドエフェクタであるグリッパーの向きを制御できます。
取り付け位置は、回転軸が鉛直下向き、X 軸方向、Y 軸方向など、さまざまな方向が可能です。ただし一般的に第4軸は、3軸構成では先端を平行に保てないという問題を補うために使われるため、直列4軸の第4関節は通常、第2・第3関節と同じ回転方向になります。
運動学アルゴリズムは必要に応じてダウンロードしてください：直列4自由度ロボットアームの運動学計算.pdf

機能要件：

目標の色（青、赤など）の物体を認識し、ロボットアームでそれぞれ異なる保管エリアへ搬送します。

ハードウェアの準備：

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

試作構成：

STP ファイル：4自由度ロボットアームSTP.rar

回路接続：

ハードウェアモジュール

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

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

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

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

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

ピンの紹介

配線の際は、まずロボットアーム全体を把握します。

本実験で使用する サーボと AI ビジョンモジュール のピンを確認します。

下のピン表に従って配線を完了します。

ロボットアーム	STM32 開発ボード	AI モジュール（K210）	STM32 開発ボード
ターンテーブルサーボ	PA2	5V	5V
上腕サーボ	PA3	GND	GND
前腕サーボ	PH9	RX	USART3_TX
手首サーボ	PH6	TX	USART3_RX
グリッパーサーボ	PF6

実際の配線

配線のコツ：サーボを配線する際は、GND が開発ボードの GND と対応するように注意してください。

ソフトウェア設計：

設計フロー

K210 のビジョン認識とシリアル通信のフローチャート

STM32 によるビジョン認識データの受信

STM32 は K210 ビジョンモジュールから色認識データを受信し、4自由度ロボットアームを制御して色別の仕分け・把持を行います。

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

ソースファイル：18.Arm_Vision_Handing.rar

プログラムコード

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 のメインプログラム

コアコード：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 "servo.h"
#include "config.h"
#include "four_dof_arm.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_TIM2_Init();
  MX_USART1_UART_Init();
  MX_TIM12_Init();
  MX_TIM10_Init();
  MX_USART3_UART_Init();
  /* USER CODE BEGIN 2 */

  servo_init();//servo init

  arm_reset(); //arm reset
  HAL_Delay(1500);

  uint32_t action_delay = 300;
  HAL_UART_Receive_IT(&huart3, &rx_data, 1);

  uint16_t detect_color_num = 0;
  /* USER CODE END 2 */

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

    /* USER CODE BEGIN 3 */

    //如果STM32接收到视觉模块发送到的信息（此时recv_flag值为true）
    if(recv_flag == true){
        //printf("------------------------\r\n");
        //printf("  Func: 0x%02X\r\n", received_data.func_num); //function code
        //printf("  Position: X=%d\r\n", received_data.x);      //X-coordinate of the center
        //printf("  Position: Y=%d\r\n", received_data.y);      //Center y-coordinate
        //printf("  Size: W=%d\r\n", received_data.w);          //Rectangle frame width
        //printf("  Size: H=%d\r\n", received_data.h);          //Rectangle frame height
        //printf("  Color: %s\r\n", received_data.color_str);   //Recognized color string

        //If the detected color is blue
        if( strcmp("Blue", received_data.color_str) == 0 ){
            detect_color_num ++;
            if(detect_color_num >= 10){
                detect_color_num = 0;
                //Place the object on the left side
                arm_catch_sth_and_put_left();
            }
            //printf("i find blue color\r\n");
        }

        //If the detected color is red
        if( strcmp("Red", received_data.color_str) == 0){
            detect_color_num ++;
            if(detect_color_num >= 10){
                detect_color_num = 0;
                //Place the object on the right side
                arm_catch_sth_and_put_right();
            }
            //printf("i find red color\r\n");
        }
        clear();                                              //清空本次识别到的信息
    }
    HAL_Delay(50);
  }
  /* 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 */

/* 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 設定

3.1 ターンテーブルと上腕サーボのパラメータ設定

ピン	タイマー	ユーザーラベル
PA2	TIM2_CH3	SERVO1

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

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

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

サーボ S2 のパラメータ

ピン	タイマー
PA3	TIM2_CH4

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

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

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

プリスケーラ、自動リロード値、デフォルトのパルス値を設定します。TIM2 をクリックし、Parameter Settings パネルを選択します。設定結果は下図のとおりです。

パラメータ	設定値	意味
Prescaler (PSC)	`84-1`	プリスケーラ。84MHz のシステムクロックを 1MHz に分周します
Counter Period (ARR)	`19999`	自動リロード値。PWM 周期を決定します
auto-reload preload	Enable	自動リロードのプリロードを有効化
サーボのデフォルトパルス Pulse	500	500us=0.5ms。デフォルトのパルス幅を 0.5ms に設定し、サーボの初期角度は 0 度になります。

3.2 前腕と手首サーボのパラメータ設定

ピン	タイマー
PH9	TIM12_CH2
PH6	TIM12_CH1

3.1 のサーボパラメータの設定方法を参考に、前腕と手首のパラメータを設定します。設定後の GPIO Settings のパラメータは、下図の赤枠部分のとおりです。

Parameter Settings をクリックし、プリスケーラ、自動リロード値、デフォルトのパルス値を確認します。

3.3 グリッパーサーボのパラメータ設定

ピン	タイマー
PF6	TIM10_CH1

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

Timers パネルをクリックして TIM10 を選択し、Activated にチェックを入れ、PWM Generation CH1 を選択します。設定結果は次のとおりです。

ピンの色が緑色に変わったことを確認します。

プリスケーラ、自動リロード値、デフォルトのパルス値を設定します。TIM10 の Parameter Settings パネルをクリックします。デフォルトは次のとおりです。

次のようにパラメータを変更する必要があります。

パラメータ	設定値	意味
Prescaler	`84-1`	プリスケーラ。84MHz のシステムクロックを 1MHz に分周します
Counter Period (ARR)	`19999`	自動リロード値。PWM 周期を決定します
auto-reload preload	Enable	自動リロードのプリロードを有効化
サーボのデフォルトパルス Pulse	500	500us=0.5ms。デフォルトのパルス幅を 0.5ms に設定し、サーボの初期角度は 0 度になります。

設定結果は下図のとおりです。

3.4 AI ビジョンモジュールのパラメータ設定

ヒント：USART1 は stm32 メインボードと PC の通信インターフェース、USART3 は AI モジュール（K210）と STM32 の通信インターフェースです。

ピン	機能
PB10	USART3_TX
PB11	USART3_RX

ピンの設定。

(1) GPIO をクリックし、Pinout view 画面下部で PB10 を検索すると、対応するピンが点滅します。点滅しているピンを左クリックして USART3_TX を選択します。続いて Pinout view 画面下部で PB11 を検索すると、対応するピンが点滅します。点滅しているピンを左クリックして USART3_RX を選択します（この時点では USART3 が未設定のため、ピンに黄色の警告が表示されます）。

(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. コードのダウンロードとテスト

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

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

(2) バッテリースイッチと STM32 開発ボードのスイッチをオンにし、動作を確認します。

7. ロボットアームの操作・調整方法

自分でロボットアームを組み立てた場合、どのパラメータを設定する必要があるでしょうか。また、どのように調整すれば、ロボットアームを実験動画のとおりに動作させられるでしょうか。デモプログラムと合わせて一緒に見ていきましょう。

ロボットアームを初期化角度に調整します。初期状態は下図のとおりです。

上腕と前腕の長さを測定して記録します。例えばテスト時の実測値は、上腕の長さが 109mm、前腕の長さが 126mm でした。

プログラムを開き、赤枠部分のコードを次のように変更します。

ロボットアームの初期化状態におけるサーボの角度を記録します。

(1) ロボットアームが初期状態にあるときのサーボ値を記録し、下表に記入します。

ハードウェア	ロボットアーム初期状態のサーボ値
ターンテーブルサーボ
上腕サーボ
前腕サーボ
手首サーボ

状態	対応するサーボ値
グリッパー開
グリッパー閉
物体把持時の角度

以下はテスト時のデータ値です。

ハードウェア	ロボットアーム初期状態のサーボ値
ターンテーブルサーボ	135
上腕サーボ	123
前腕サーボ	138
手首サーボ	38

状態	対応するサーボ値
グリッパー開	65
グリッパー閉	142
物体把持時の角度	126

初期化角度を four_dof_arm.h ファイルに書き込みます。プログラムを開き、パラメータ値 135、123、138、134、65、142、126 を対応する位置にコピーします（図の赤枠部分を参照）。

これにより、逆運動学関数 void get_coordinate(float x, float y, float z) を呼び出す際に、4つの定数値（Arm_Alpha_Angle、Arm_Theta_Angle、Arm_Beta_Angle、Arm_Gamma_Angle）が使用されます。

逆運動学によってロボットアームの各サーボの角度を計算し、補間を行うことでロボットアームの動作を制御します。

ロボットアームの可動範囲から離れた位置でスイッチを入れ、プログラムを実行します。

注意：ロボットアームは動作速度が速く先端の慣性が大きいため、電源を入れる際は必ずロボットアームの球状の可動範囲から離れ、ぶつからないようにしてください。

再現時の注意点

再現する際は、各関節サーボの出力側サーボホーンの取り付け方向を、実物の例示写真およびデモ動画と一致させてください。これはロボットアームの各関節の回転方向に影響し、運動学の式における方向とも関係します。方向が正しくない場合、ある関節の回転角度や運動方向が大きくずれることがあります。
ロボットアームの初期化状態では、各関節の角度をできるだけサーボの中間値付近（80～150）にすることをお勧めします。この範囲外の場合は、構造を分解・再組立して調整してください。これはロボットアームの可動範囲に影響します。サーボの回転範囲は 0～270 度、中間値は 135 で、こうすることで各関節サーボが最大の可動範囲を得られます。中間値の範囲が適切でないと、ロボットアームの到達可能点が減少し、本来到達できるはずの点でも運動学の式で計算すると解が得られず、理論上の到達可能点に到達できなくなります。

実験結果

4自由度ロボットアームとビジョンモジュールを組み合わせ、物体の色に応じた仕分け・搬送を実現します。

(1) 赤い物体を検出した場合、ロボットアームは赤い物体を把持し、左側に置きます。

(2) 青い物体を検出した場合、ロボットアームは青い物体を把持し、右側に置きます。

コード解説

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株式会社までお問い合わせください。