メカナム４輪サンプルコード解説
#include "main.h"
#include "Altair_library_for_CubeIDE/altair.h"
CAN_HandleTypeDef hcan1;
CAN_HandleTypeDef hcan2;
TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
TIM_HandleTypeDef htim5;
TIM_HandleTypeDef htim10;
TIM_HandleTypeDef htim11;
TIM_HandleTypeDef htim12;
TIM_HandleTypeDef htim13;
TIM_HandleTypeDef htim14;
USART_HandleTypeDef husart2;
USART_HandleTypeDef husart3;
// --- Altair ライブラリ用オブジェクト ---
// モータドライバ（4輪メカナム）
MotorDriver motorFR; // Front Right
MotorDriver motorFL; // Front Left
MotorDriver motorBR; // Back Right
MotorDriver motorBL; // Back Left
// エンコーダ
Encoder encoderFR;
Encoder encoderFL;
Encoder encoderBR;
Encoder encoderBL;
EncoderData encoderDataFR;
EncoderData encoderDataFL;
EncoderData encoderDataBR;
EncoderData encoderDataBL;
// PID コントローラ
Pid pidFR;
Pid pidFL;
Pid pidBR;
Pid pidBL;
// 運動学
Kinematics kinematics;
// 制御パラメータ
#define CONTROL_PERIOD_MS 10
#define WHEEL_DIAMETER_MM 100.0f
#define ROBOT_DIAMETER_MM 1000.0f
// エンコーダの PPR（ご使用のエンコーダに合わせて変更してください）
#define ENCODER_PPR 8192
// CAN 受信で使用するスケーリング（int16 -> 物理量）
#define CAN_VEL_SCALE_MM_S 1.0f // 1LSB = 1 mm/s
#define CAN_OMEGA_SCALE_DEG_S 100.0f // 1LSB = 10 deg/s
// 目標速度 [mm/s, deg/s]
float targetVx = 0.0f;
float targetVy = 0.0f;
float targetOmega = 0.0f;
// CAN ライブラリ受信デバッグ用（CubeMonitor で監視する）
volatile uint32_t can_rx_count = 0; // 受信回数
volatile int16_t can_last_vx_raw = 0; // 生データ vx
volatile int16_t can_last_vy_raw = 0; // 生データ vy
volatile int16_t can_last_om_raw = 0; // 生データ omega
volatile uint8_t can_last_dlc = 0; // 直近フレーム DLC
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_CAN1_Init(void);
static void MX_CAN2_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM2_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM4_Init(void);
static void MX_TIM5_Init(void);
static void MX_TIM10_Init(void);
static void MX_TIM11_Init(void);
static void MX_TIM12_Init(void);
static void MX_TIM13_Init(void);
static void MX_TIM14_Init(void);
static void MX_USART2_Init(void);
static void MX_USART3_Init(void);
int main(void){
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_CAN1_Init();
MX_CAN2_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_TIM5_Init();
MX_TIM10_Init();
MX_TIM11_Init();
MX_TIM12_Init();
MX_TIM13_Init();
MX_TIM14_Init();
MX_USART2_Init();
MX_USART3_Init();
// CAN1 を Altair ライブラリで初期化（割り込み・フィルタ設定）
Can_Init(&hcan1);
// --- ここから 4輪メカナム制御 ---
// モータ初期化（ピン割り当てに応じてタイマを接続）
// MD
// * B14(TIM12 CH1) B15(TIM12 CH2) -> Front Right
// * A8 (TIM1 CH1) A9 (TIM1 CH2) -> Front Left
// * A6 (TIM13 CH1) A7 (TIM14 CH1) -> Back Right
// * B8 (TIM10 CH1) B9 (TIM11 CH1) -> Back Left
MotorDriver_Init(&motorFR, &htim12, TIM_CHANNEL_1, &htim12, TIM_CHANNEL_2);
MotorDriver_Init(&motorFL, &htim1, TIM_CHANNEL_1, &htim1, TIM_CHANNEL_2);
MotorDriver_Init(&motorBR, &htim13, TIM_CHANNEL_1, &htim14, TIM_CHANNEL_1);
MotorDriver_Init(&motorBL, &htim10, TIM_CHANNEL_1, &htim11, TIM_CHANNEL_1);
// エンコーダ初期化
// encoder
// * A0 A1 TIMER5
// * B3 A15 TIMER2
// * B6 B7 TIMER4
// * C6 C7 TIMER3
Encoder_Init(&encoderFR, &htim5, WHEEL_DIAMETER_MM, ENCODER_PPR, CONTROL_PERIOD_MS);
Encoder_Init(&encoderFL, &htim2, WHEEL_DIAMETER_MM, ENCODER_PPR, CONTROL_PERIOD_MS);
Encoder_Init(&encoderBR, &htim4, WHEEL_DIAMETER_MM, ENCODER_PPR, CONTROL_PERIOD_MS);
Encoder_Init(&encoderBL, &htim3, WHEEL_DIAMETER_MM, ENCODER_PPR, CONTROL_PERIOD_MS);
// PID 初期化（まずはP制御のみ。ゲインは実機で要調整）
Pid_Init(&pidFR);
Pid_Init(&pidFL);
Pid_Init(&pidBR);
Pid_Init(&pidBL);
Pid_setGain(&pidFR, 10.0, 0.0, 0.0, 0.0);
Pid_setGain(&pidFL, 10.0, 0.0, 0.0, 0.0);
Pid_setGain(&pidBR, 10.0, 0.0, 0.0, 0.0);
Pid_setGain(&pidBL, 10.0, 0.0, 0.0, 0.0);
// メカナム運動学設定
kinematics.mode = MEKANUM;
kinematics.robot_diameter = ROBOT_DIAMETER_MM;
kinematics.wheel_radius = WHEEL_DIAMETER_MM / 2.0f;
uint32_t lastControlTick = HAL_GetTick();
while (1){
uint32_t now = HAL_GetTick();
if (now - lastControlTick >= CONTROL_PERIOD_MS)
{
lastControlTick += CONTROL_PERIOD_MS;
// --- CAN 受信処理 ---
if (g_can1_rx_data.new_data_flag)
{
can_rx_count++;
can_last_dlc = g_can1_rx_data.dlc;
// PC からの Vx/Vy/Omega 形式（ID 0x123）を想定
if (g_can1_rx_data.std_id == 0x123 && g_can1_rx_data.dlc >= 6)
{
int16_t vx_raw = (int16_t)((g_can1_rx_data.data[1] << 8) | g_can1_rx_data.data[0]);
int16_t vy_raw = (int16_t)((g_can1_rx_data.data[3] << 8) | g_can1_rx_data.data[2]);
int16_t om_raw = (int16_t)((g_can1_rx_data.data[5] << 8) | g_can1_rx_data.data[4]);
can_last_vx_raw = vx_raw;
can_last_vy_raw = vy_raw;
can_last_om_raw = om_raw;
targetVx = (float)vx_raw / CAN_VEL_SCALE_MM_S; // [mm/s]
targetVy = (float)vy_raw / CAN_VEL_SCALE_MM_S; // [mm/s]
targetOmega = (float)om_raw / CAN_OMEGA_SCALE_DEG_S; // [deg/s]
}
// 読み取り完了
g_can1_rx_data.new_data_flag = 0;
}
// --- エンコーダ更新 ---
Encoder_Interrupt(&encoderFR, &encoderDataFR);
Encoder_Interrupt(&encoderFL, &encoderDataFL);
Encoder_Interrupt(&encoderBR, &encoderDataBR);
Encoder_Interrupt(&encoderBL, &encoderDataBL);
// --- 運動学：目標ホイール速度 [rps] ---
float speedFR, speedFL, speedBR, speedBL;
Kinematics_GetTargetSpeeds(&kinematics,
targetVx,
targetVy,
targetOmega,
&speedFR, &speedFL, &speedBR, &speedBL);
// --- PID 制御（目標 rps vs 実測 rps）---
double uFR = Pid_control(&pidFR, speedFR, encoderDataFR.rps, CONTROL_PERIOD_MS);
double uFL = Pid_control(&pidFL, speedFL, encoderDataFL.rps, CONTROL_PERIOD_MS);
double uBR = Pid_control(&pidBR, speedBR, encoderDataBR.rps, CONTROL_PERIOD_MS);
double uBL = Pid_control(&pidBL, speedBL, encoderDataBL.rps, CONTROL_PERIOD_MS);
// 出力を -99〜99 [%] にクリップ
if (uFR > 99.0) uFR = 99.0;
if (uFR < -99.0) uFR = -99.0;
if (uFL > 99.0) uFL = 99.0;
if (uFL < -99.0) uFL = -99.0;
if (uBR > 99.0) uBR = 99.0;
if (uBR < -99.0) uBR = -99.0;
if (uBL > 99.0) uBL = 99.0;
if (uBL < -99.0) uBL = -99.0;
// モーターへ反映（機体の配線方向により符号は適宜反転してください）
MotorDriver_setSpeed(&motorFR, (int)uFR);
MotorDriver_setSpeed(&motorFL, (int)uFL);
MotorDriver_setSpeed(&motorBR, (int)uBR);
MotorDriver_setSpeed(&motorBL, (int)uBL);
}
}
}
//以下設定
このコードの全体像
このサンプルは、Altair ライブラリを使って「4 輪メカナムロボット」を速度指令で動かす最小構成のコードです。
- PC などから CAN で送られてくる \(V_x, V_y, \omega\)（前後・左右・旋回の目標速度）を受信
- メカナムの運動学から「各ホイールの目標回転数」を計算
- エンコーダで測ったホイールの実回転数と比較して PID 制御
- モータドライバに PWM 出力を流して、目標どおりの速度になるよう制御
という流れになっています。
グローバル変数とオブジェクト
-
CAN_HandleTypeDef hcan1, hcan2
→ STM32CubeMX が生成する CAN ペリフェラルのハンドル。Can_Init(&hcan1);で Altair ライブラリに渡します。 -
MotorDriver motorFR, motorFL, motorBR, motorBL
→ 4 輪分のモータドライバオブジェクト（Front/Back, Right/Left）。
MotorDriver_Init()で「どのタイマのどのチャネルがどの方向の PWM か」を教えています。 -
Encoder encoderFR ...とEncoderData encoderDataFR ...
→ 4 輪分のエンコーダを扱うための構造体。Encoder_Init()で、 - どのタイマをエンコーダモードで使うか
- 車輪直径
WHEEL_DIAMETER_MM - エンコーダの PPR
ENCODER_PPR -
制御周期
CONTROL_PERIOD_MSを設定します。Encoder_Interrupt()を周期ごとに呼ぶことで、encoderDataX.rps（回転数 [rps]）が更新されます。 -
Pid pidFR ...
→ 4 輪分の PID コントローラ。Pid_setGain()で \(K_p, K_i, K_d\) とバイアスを設定しています。ここでは \(P\) 制御のみで、\(K_p = 10\)、他は 0 です（実機で必ず調整が必要）。 -
Kinematics kinematics
→ メカナム運動学を計算するための構造体。
kinematics.mode = MEKANUM;でメカナムモードを指定し、robot_diameter（車体直径）とwheel_radius（車輪半径）を与えることで、 \(V_x, V_y, \omega\) から各ホイールの回転数を求められるようにしています。
制御パラメータと定数
-
CONTROL_PERIOD_MS
→ 制御ループの周期 [ms]。ここでは 10ms（=100Hz 制御）。 -
WHEEL_DIAMETER_MM/ROBOT_DIAMETER_MM
→ 車輪直径・機体直径 [mm]。メカナムの運動学で使用します。実機寸法に合わせてください。 -
ENCODER_PPR
→ エンコーダの 1 回転あたりパルス数（PPR）。使用するエンコーダに合わせて必ず変更します。 -
CAN_VEL_SCALE_MM_S,CAN_OMEGA_SCALE_DEG_S
→ CAN で送られてくる 16bit 整数を「物理量」に直すためのスケーリング係数です。
ここではコメントと数値が少し食い違っているので、実際に使うときは次のように整理するとよいです：- 例：
CAN_VEL_SCALE_MM_S = 1.0fなら「1 LSB = 1 mm/s」 - 例：
CAN_OMEGA_SCALE_DEG_S = 10.0fなら「1 LSB = 10 deg/s」 など、自分のプロトコルに合わせて決めてください。
- 例：
main 関数の初期化
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_CAN1_Init();
...
MX_USART3_Init();
Can_Init(&hcan1);
ここまでは STM32 のお決まり処理です。
- HAL / クロック / GPIO / タイマ / CAN / USART などを CubeMX 生成コードで初期化
Can_Init(&hcan1);で Altair ライブラリ側の CAN 設定（フィルタ・割り込み）を実施
その後で、Altair のモータドライバ・エンコーダ・PID・運動学の設定を行っています。
モータとエンコーダの初期化
MotorDriver_Init(&motorFR, &htim12, TIM_CHANNEL_1, &htim12, TIM_CHANNEL_2);
...
Encoder_Init(&encoderFR, &htim5, WHEEL_DIAMETER_MM, ENCODER_PPR, CONTROL_PERIOD_MS);
-
モータ：どのタイマのどのチャネルが「正転」「逆転」の PWM かを指定
コメントに、STM32 のピン番号とタイマ対応が書いてあります。基板の配線に合わせてここを変更します。 -
エンコーダ：どのタイマをエンコーダモードとして使うかを指定
WHEEL_DIAMETER_MMとENCODER_PPRから、「何パルスで何 mm 進んだか」を内部で計算します。
制御ループの流れ
uint32_t lastControlTick = HAL_GetTick();
while (1) {
uint32_t now = HAL_GetTick();
if (now - lastControlTick >= CONTROL_PERIOD_MS) {
lastControlTick += CONTROL_PERIOD_MS;
...
}
}
HAL_GetTick() を使って、10ms ごとに制御処理を呼び出すループになっています。中身の流れは次のとおりです。
- CAN から速度指令を受信
- エンコーダからホイール速度を更新
- 運動学で「各ホイールの目標回転数」を計算
- PID で各ホイールを制御
- モータドライバに出力
1. CAN 受信と速度指令の更新
if (g_can1_rx_data.new_data_flag) {
...
if (g_can1_rx_data.std_id == 0x123 && g_can1_rx_data.dlc >= 6) {
int16_t vx_raw = ...;
int16_t vy_raw = ...;
int16_t om_raw = ...;
targetVx = (float)vx_raw / CAN_VEL_SCALE_MM_S;
targetVy = (float)vy_raw / CAN_VEL_SCALE_MM_S;
targetOmega = (float)om_raw / CAN_OMEGA_SCALE_DEG_S;
}
g_can1_rx_data.new_data_flag = 0;
}
- ID
0x123のフレームを「\(V_x, V_y, \omega\) 指令」とみなしています。 - データは 16bit 符号付き整数（little endian）で
- Byte0-1: \(V_x\)
- Byte2-3: \(V_y\)
- Byte4-5: \(\omega\) という前提で復元しています。
- スケーリング係数で割ることで、[mm/s]・[deg/s] の実際の物理量に変換しています。
2. エンコーダ更新
Encoder_Interrupt(&encoderFR, &encoderDataFR);
...
- 各ホイールごとに
Encoder_Interrupt()を呼ぶことで、 - 回転数
rps - 進んだ距離 [mm]
などが
encoderDataXに更新されます。
3. 運動学でホイール目標速度を計算
float speedFR, speedFL, speedBR, speedBL;
Kinematics_GetTargetSpeeds(&kinematics,
targetVx,
targetVy,
targetOmega,
&speedFR, &speedFL, &speedBR, &speedBL);
- 与えられた \(V_x, V_y, \omega\) から、4 輪それぞれの「目標回転数 [rps]」を計算します。
- メカナム特有の「斜めローラ付きホイールによる自由度 3 の移動」をここで実現しています。
4. PID 制御とモータ出力
double uFR = Pid_control(&pidFR, speedFR, encoderDataFR.rps, CONTROL_PERIOD_MS);
...
// -99〜99[%] にクリップ
MotorDriver_setSpeed(&motorFR, (int)uFR);
...
- 目標回転数（
speedFRなど）とエンコーダ実測値（encoderDataFR.rps）の差を PID で補正して、 -99〜99[%] のデューティ比としてモータドライバに渡しています。 MotorDriver_setSpeed()は、符号（正転/逆転）と大きさ（デューティ）を内部で PWM に変換します。
実際の機体の配線により「前に進むはずが後ろに進む」場合は、個々のホイールごとに符号を反転させてください。
実機で調整すべきポイント
ENCODER_PPR: 使っているエンコーダの仕様に合わせるWHEEL_DIAMETER_MM,ROBOT_DIAMETER_MM: 実寸に合わせるCONTROL_PERIOD_MS: 制御周波数（100Hz〜1kHz くらいが目安）CAN_VEL_SCALE_MM_S,CAN_OMEGA_SCALE_DEG_S: 上位機器との CAN プロトコルに合わせるPid_setGain()の \(K_p, K_i, K_d\) : 実機試験でチューニング（最初は \(P\) のみで小さめから）MotorDriver_setSpeed()の前後での符号 : 機体の動きと合わせて必要なら反転