実験1-2 リモートコントロール：四輪駆動メカナム四軸複合ロボットによる把持

先に四輪駆動メカナム全方向四軸複合ロボットの組み立てを完了してください。詳細は以下のリンクをご覧ください。

実験1-1 四輪駆動メカナム全方向四軸複合ロボットの構造組み立て

本節では、PS2 コントローラーを使って四輪駆動メカナム全方向四軸複合ロボットをリモート操作し、物体を把持することに重点を置きます。

実験の目的：

SPI 通信の仕組みを理解し、SPI インターフェースを使ってコントローラーのボタンデータを取得できるようになること。
四輪駆動メカナムシャーシの運動原理に基づき、四輪駆動メカナム四軸複合ロボットをリモート操作して把持できるようになること。
STM32 の GPIO 周辺機能ピンを設定する方法を習得すること。
STM32CubeMX で新規プロジェクトを作成し、MDK_ARM プロジェクトファイルを生成できるようになること。
Keil IDE でユーザーコードを追加し、プロジェクトのテストを行えるようになること。

四輪駆動メカナムシャーシの基本運動原理：

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


前進	後退

右平行移動	左平行移動

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

四軸直列ロボットアームの運動原理：

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

機能要件：

ロボットをリモート操作して把持を行います。

回路接続：

ハードウェアモジュール

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

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

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

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

電子モジュールの紹介（リンクをクリックすると、モーター、サーボ、PS2 コントローラーの基本的な紹介と使い方を確認できます）

ピンの説明

まず モーター、サーボ、PS2 コントローラー（SPI インターフェース） の位置を全体的に把握し、ピン表に従って配線してください。

モーター、サーボ、PS2 コントローラー（SPI インターフェース）に対応するピンを確認します。

ロボットアーム各部の紹介

以下のピン表に従って配線してください。

ハードウェア	STM32 インターフェース	ロボットアーム	STM32 開発ボード	PS2 コントローラー	STM32 メインボード
左前輪モーター	M1	旋回台サーボ	PA2	GND	GND
右前輪モーター	M2	上腕サーボ	PA3	VCC	VCC_3V3
左後輪モーター	M3	前腕サーボ	PH9	PS2_DAT	PC11–MISO
右後輪モーター	M4	手首サーボ	PH6	PS2_CMD	PC12–MOSI
		グリッパーサーボ	PF6	PS2_CS	PA15–CS
				PS2_CLK	PC10–CLK

実機の配線

配線のコツ：エンコーダーモーター付属のケーブルは逆挿し防止構造になっているため、差し込み口をよく確認してから接続してください。サーボを配線する際は、GND が開発ボード上の GND と対応するように注意してください。

ソフトウェア設計：

設計フロー

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

ソースコードファイル：19.Remote_Control_Robot.zip

プログラムコード

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

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "ps2.h"
#include "motor.h"
#include "servo.h"
#include "DWT_time.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 */
uint16_t catch_angle = Arm_Claw_Open;      //set catch init angle
float global_speed_subdivision = 1;        //Set global speed subdivision

float arm_target_x = 50.0;                 //set arm x init position
float arm_target_y = 0.0;                  //set arm y init position
float arm_target_z = 0.0;                  //set arm z init position
float set_arm_x_min = 30.0;                //set arm x min value
float set_arm_x_max = 180.0;               //set arm x max value
float set_arm_y_min = -100.0;              //set arm y min value
float set_arm_y_max = 100.0;               //set arm y max value
float set_arm_z_min = -120.0;               //set arm z min value
float set_arm_z_max = 120.0;               //set arm z max value
void arm_position_limit();                 //set arm limit

float car_move_vx = 0.0; //The velocity of the car in the forward and backward directions, denoted as vx
float car_move_vy = 0.0; //The velocity in the left and right translation directions of the car is denoted by vy
float car_move_vw = 0.0; //The velocity in the turnleft and turnright translation directions of the car is denoted by vw
float car_move_vx_min = -1.0;  //Set the minimum velocity of the car vx
float car_move_vx_max =  1.0;  //Set the maximum velocity of the car vx
float car_move_vy_min = -1.0;  //Set the minimum velocity of the car vy
float car_move_vy_max =  1.0;  //Set the maximum velocity of the car vy
  float car_move_vw_min = -1.0;  //Set the minimum velocity of the car vw
  float car_move_vw_max =  1.0;  //Set the maximum velocity of the car vw
  bool  car_flag1 = false;
  bool  car_flag2 = false;
  bool  car_flag3 = false;
  float set_car_velocity = 0.1;
  uint8_t car_state = 0;   //1:forward  2:back  3:turnleft   4:turnright  5:transleft  6:transright 7:stop
  void car_move_limit();   //Speed limit for cars
  void car_move_state(uint8_t mode);
/* 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();
  MX_SPI3_Init();
  MX_TIM2_Init();
  MX_TIM10_Init();
  MX_TIM12_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

  servo_init();                           //servo init
  arm_reset();                            //arm reset
  HAL_Delay(1000);
  arm_home();                             //arm go home
  HAL_Delay(1000);

  //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

  PS2_Init();                             //ps2 modele init
  PS2_Report_t rep;                       //create ps2 modele
  /* USER CODE END 2 */

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

    /* USER CODE BEGIN 3 */

    uint16_t get_ps2 = PS2_Read(&rep);
    //printf("LX=%d LY=%d RX=%d RY=%d\r\n", rep.lx, rep.ly, rep.rx, rep.ry);
    switch(get_ps2){
        //control catch section
        case 1://press L1
            catch_angle = catch_angle + 2;
            break;
        case 7://Press R1
            catch_angle = catch_angle - 2;
            break;

        //control global speed
        case 2://Press L2
            global_speed_subdivision = global_speed_subdivision - 0.1;
            break;

        case 8://Press R2
            global_speed_subdivision = global_speed_subdivision + 0.1;
            break;

        //control arm position
        case 3://press Up
            arm_target_x = arm_target_x + 3.0;
            break;
        case 4://Press Left
            arm_target_y = arm_target_y - 3.0;
            break;
        case 5://Press Right
            arm_target_y = arm_target_y + 3.0;
            break;
        case 6://Press Down
            arm_target_x = arm_target_x - 3.0;
            break;

        case 9://Press Triangle
            arm_target_z = arm_target_z + 3.0;
            break;
        case 12://Press X
            arm_target_z = arm_target_z - 3.0;
            break;

        //case 10: printf("  Press Rectangle \r\n"); break; //Press Rectangle
        //case 11: printf("  Press Circle \r\n");    break; //Press Circle
        //case 13: printf("  Press SELECT \r\n");    break; //Press SELECT
        //case 14: printf("  Press START \r\n");     break; //Press START
        //default: break;
    }
    //set catch angle
    catch_angle = catch_limit(catch_angle);
    //printf("catch_angle: %d\r\n", catch_angle);
    claw(catch_angle);

    //set global speed subdivision
    global_speed_subdivision = speed_limit(global_speed_subdivision);

    //calculate arm limit
    arm_position_limit();
    linearInterpolation(arm_target_x, arm_target_y, arm_target_z, 2, 2);

    //Calculate the left turn and right turn of the car
    if( (rep.lx >= 120) && (rep.lx <= 134) ){
        car_flag1 = false;
    }
    else{
        if( rep.lx < 120 ){
            car_state = 3;
        }
        if( rep.lx > 134 ){
            car_state = 4;
        }
        car_flag1 = true;
    }

    //Calculate the forward and backward movement of the car
    if( (rep.ly >= 120) && (rep.ly <= 134) ){
        car_flag2 = false;
    }
    else{
        if( rep.ly < 120 ){
            car_state = 1;
        }
        if( rep.ly > 134 ){
            car_state = 2;
        }
        car_flag2 = true;
    }

    //Calculate the left and right horizontal translation of the car
    if( (rep.rx >= 120) && (rep.rx <= 134) ){
        car_flag3 = false;
    }
    else{
        if( rep.rx < 120 ){
            car_state = 5;
        }
        else if( rep.rx > 134 ){
            car_state = 6;
        }
        car_flag3 = true;
    }

    //calculate car speed limit
    car_move_limit();
    if( car_flag1 || car_flag2 || car_flag3 ){
        car_move_state(car_state);
    }
    else{
        car_move_state(7);
    }

    HAL_Delay(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);
  }
}

//set arm limit
void arm_position_limit(){
    arm_target_x = (arm_target_x < set_arm_x_min) ? set_arm_x_min : ((arm_target_x > set_arm_x_max) ? set_arm_x_max : arm_target_x);
    arm_target_y = (arm_target_y < set_arm_y_min) ? set_arm_y_min : ((arm_target_y > set_arm_y_max) ? set_arm_y_max : arm_target_y);
    arm_target_z = (arm_target_z < set_arm_z_min) ? set_arm_z_min : ((arm_target_z > set_arm_z_max) ? set_arm_z_max : arm_target_z);
}

//Speed limit for cars
void car_move_limit(){
    car_move_vx = (car_move_vx < car_move_vx_min) ? car_move_vx_min : ((car_move_vx > car_move_vx_max) ? car_move_vx_max : car_move_vx);
    car_move_vy = (car_move_vy < car_move_vy_min) ? car_move_vy_min : ((car_move_vy > car_move_vy_max) ? car_move_vy_max : car_move_vy);
    car_move_vw = (car_move_vw < car_move_vw_min) ? car_move_vw_min : ((car_move_vw > car_move_vw_max) ? car_move_vw_max : car_move_vw);
}

//car move state
void car_move_state(uint8_t mode){
    switch(mode){
        //car forward
        case 1:
            car_move_vx = set_car_velocity * global_speed_subdivision;
            car_move_vy = 0.0;
            car_move_vw = 0.0;
            break;
        //car back
        case 2:
            car_move_vx = -set_car_velocity * global_speed_subdivision;
            car_move_vy = 0.0;
            car_move_vw = 0.0;
            break;
        //car turn left
        case 3:
            car_move_vx = 0.0;
            car_move_vy = 0.0;
            car_move_vw = set_car_velocity * global_speed_subdivision * 2;
            break;
        //car turn right
        case 4:
            car_move_vx = 0.0;
            car_move_vy = 0.0;
            car_move_vw = -set_car_velocity * global_speed_subdivision * 2;
            break;
        //car trans left
        case 5:
            //printf("5\r\n");
            car_move_vx = 0.0;
            car_move_vy =  set_car_velocity * global_speed_subdivision;
            car_move_vw = 0.0;
            break;
        //car trans right
        case 6:
            //printf("6\r\n");
            car_move_vx = 0.0;
            car_move_vy = -set_car_velocity * global_speed_subdivision;
            car_move_vw = 0.0;
            break;
        //car stop
        case 7:
            car_move_vx = 0.0;
            car_move_vy = 0.0;
            car_move_vw = 0.0;
            break;
        default: break;
    }
    //printf("vx: %f, vy: %f, vw: %f\r\n", car_move_vx, car_move_vy, car_move_vw);
    car_move(car_move_vx, car_move_vy, car_move_vw);
}
/* 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 を左クリックして機能を 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 にチェックを入れます。

旋回台および上腕サーボのパラメータ設定

ピン	タイマー	ユーザーラベル
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 度になります。

前腕および手首サーボのパラメータ設定

ピン	タイマー
PH9	TIM12_CH2
PH6	TIM12_CH1

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

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

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

ピン	タイマー
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 度になります。

設定後の状態は下図のとおりです。

PS2 コントローラーのパラメータ設定

PS2 のピンパラメータ表：

ピン	パラメータ
PC12	SPI3_MOSI
PC11	SPI3_MISO
PA15	GPIO_Output
PC10	SPI3_SCK

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

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

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

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

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

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

4. コードの生成

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

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

コードの生成に成功しました。

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

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

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

ヒント 2：独自の .c ファイルを作成しても構いません。

ヒント 3：ユーザーコードは自分で書いても構いませんが、（コード解説）に掲載している例を参考にして学ぶこともできます。

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

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

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

(1) バッテリーのスイッチと STM32 開発ボードのスイッチをオンにし、コントローラーで複合ロボットを操作します。

(2) コントローラーのスイッチをオンにします。

（3）インジケーターの点滅が止まれば、接続に成功しています。

（4）コントローラーのボタンを押して、ロボットの把持動作を操作します。各ボタンの機能は以下のとおりです。

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

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

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

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

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

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

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

ハードウェア	ロボットアーム初期状態のサーボ値
旋回台サーボ
上腕サーボ
前腕サーボ
手首サーボ

状態	サーボ値
グリッパー開
グリッパー閉
グリッパーで物体を把持する角度

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

ハードウェア	ロボットアーム初期状態のサーボ値
旋回台サーボ	135
上腕サーボ	123
前腕サーボ	138
手首サーボ	130

状態	サーボ値
グリッパー開	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 です。こうすることで各関節サーボが最大の可動範囲を得られます。中央値の範囲が適切でないと、ロボットアームの到達可能な点が減少し、本来到達できる点でも運動学の式で計算すると解が得られず、理論上到達可能な点にロボットアームが到達できなくなります。

実験結果

コントローラーで四輪駆動メカナム四軸複合ロボットを遠隔操作し、移動と把持動作を模擬します。

コード解説

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/ で始まり終わる部分の間のコードを確認してください。以下の赤枠部分が記入したユーザーコードです。

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

技術サポート

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