- プログラムの内容
1. つぎの各通信のためのハードウェア定義・初期設定：
I2C
PWM (タイマ)
DIR (GPIO)
ADC
2. PI 制御ループ
そこで main() を，つぎの構成にする：
main()
{
// ハードウェア定義・初期設定
INIT()
// 着信監視を設定
HAL_I2C_EnableListen_IT(
&hi2c1
);
// motor_store_buffer を
// 着信データを受けるバッファに設定
HAL_I2C_Slave_Receive_IT(
&hi2c1,
motor_store_buffer,
24
);
// PI 制御ループ
PI_Loop();
}
プログラムは，main() から PI_Loop() に入って，無限ループする。
INIT() は後に回して，PI_Loop() から見ていく。
- PI_Loop()
シャント抵抗電圧を ADC で受けて，電圧値を取得
これから I_meas を計算
I_cmd と I_meas に対し，
PI 式でモータ電流値を計算
対応する電圧値の duty比 で，て，
PWM をモータドライバ IC へ
電流方向が変更されたときは，，
DIR をモータドライバ IC へ
以上を繰り返す
よって，PI_Loop() はつぎの構成になる：つぎの
PI_Loop()
{
while(1)
{
// ADC 処理
I_MEAS_Get();
// PI 処理
PI_Calc();
// PWM 処理
PWM_Send();
// DIR 処理
DIR_Send();
}
}
- HAL_I2C_SlaveRxCpltCallback()
歩行脳NNストアから送られてくる目標電流値の着信の検知と，目標電流値の更新は，つぎの関数の割り込みで実現する：
I2C_HandleTypeDef hi2c1;
uint8_t motor_store_buffer[24];
volatile int16_t i_cmd[12];
void HAL_I2C_SlaveRxCpltCallback(
I2C_HandleTypeDef *hi2c)
{
int i;
// 届いたデータは byte 列
// byte 列から電流値を復元
for(i = 0 ; i < 12 ; i++)
{
//符号なし8bit整数型
uint8_t high;
uint8_t low;
high = motor_store_buffer[i * 2];
low = motor_store_buffer[i * 2 + 1];
i_cmd[i] = (int16_t)( (high << 8) | low );
}
// HAL_I2C_Slave_Receive_IT() は
// １回受信したら終了
// →再登録が必要
HAL_I2C_Slave_Receive_IT(
&hi2c1,
motor_store_buffer,
24
);
}
HAL_I2C_SlaveRxCpltCallback() は，つぎのように使う関数：
void HAL_I2C_SlaveRxCpltCallback(
I2C_HandleTypeDef *hi2c)
{
(着信に対する処理）
}
PI_Loop() を構成する関数
I_MEAS_Get()
PI_Calc()
PWM_Send();
DIR_Send();
を，順番に見ていく。
- I_MEANS_Get()
// シャント抵抗電圧を取得
uint16_t adc_raw;
adc_raw
= HAL_ADC_GetValue(&hadc1);
// シャント抵抗電圧 → 電流換算
current = adc_raw * CURRENT_SCALE;
for(int i = 0; i < MOTOR_NUM; i++)
motor[i].I_meas = current;
}
- PI_Calc()
for(int i = 0; i < MOTOR_NUM; i++)
{
// 誤差
float error =
motor[i].I_cmd - motor[i].I_meas;
// 積分
motor[i].integral += error * KI;
// PI 計算値
motor[i].pi = KP * error + motor[i].integral;
}
- DIR_PWM_Send()
for(int i = 0; i < MOTOR_NUM; i++)
{
// PI 値 (-12V 〜 +12V 相当) を
// duty比 (0.0 〜 1.0) に変換
// motor[i].pi → motor[i].duty
motor[i].duty
= fabs(motor[i].pi) / MAX_OUTPUT;
// duty に対応する PWM を
// モータドライバ IC に送信
duty =
(duty < 0) ? 0 :
(duty > 1) ? 1 :
motor[i].duty;
__HAL_TIM_SET_COMPARE(
&htim1,
TIM_CHANNEL_1,
duty * htim1.Init.Period
);
}
- DIR_Send()
for(int i = 0; i < MOTOR_NUM; i++)
{
if(motor[i].I_cmd > 0.0f)
motor[i].dir = 1;
else if(motor[i].I_cmd <0.0f)
motor[i].dir = -1;
// モータドライバ IC に指示
// 正転
if (dir > 0) {
HAL_GPIO_WritePin(
AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(
AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
}
// 逆転
else if (dir < 0) {
HAL_GPIO_WritePin(
AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(
AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
}
// ブレーキ
else {
HAL_GPIO_WritePin(
AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
HAL_GPIO_WritePin(
AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
}
}
以上のコーディングで，使うことになった定数・変数等は：，
- ライブラリ・定数・グローバル変数
以上の構成は，ライブラリ・定数・グローバル変数をつぎのように措くことになる：
/////////////////////////////////////////////////
// ライブラリ
/////////////////////////////////////////////////
#include "main.h"
#include
/////////////////////////////////////////////////
// 定数
/////////////////////////////////////////////////
// モータの数
#define MOTOR_NUM 12
// PI 式の係数値
#define KP 0.5f
#define KI 0.1f
/////////////////////////////////////////////////
// グローバル変数
/////////////////////////////////////////////////
// I2C1
I2C_HandleTypeDef hi2c1;
// 受信バッファ
// ( [24] : 2byte電流値が 12モータ分)
uint8_t motor_store_buffer[24];
// 目標電流値 (mA)
volatile int16_t i_cmd[12];
// モータの配列
typedef struct
{
float I_cmd; // 目標電流
float I_meas; // 実電流
float integral; // 積分項
float pi; // PI 計算値
int dir; // 電流の方向
} PI_Control;
PI_Control motor[MOTOR_NUM];
// 初期設定
for(int i = 0; i < MOTOR_NUM; i++)
{
motor[i].I_cmd = 0.0f;
motor[i].I_meas = 0.0f;
motor[i].integral = 0.0f;
motor[i].pi = 0.0f;
motor[i].dir = 1;
duty[i].dir = 1;
}
後回しにしてきた INIT() は，つぎのようになる：
INIT()
{
// I2C
INIT_I2C();
// PWM
INIT_PWM();
// DIR
INIT_DIR();
// STBY 処理
INIT_STBY();
// ADC
INIT_ADC();
}
これを構成している関数
INIT_I2C()
INIT_PWM()
INIT_DIR()
INIT_STBY()
INIT_ADC()
を，順に見ていく。
- INIT_I2C()
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_I2C1_Init();
// 使用ピンの設定
// GPIOをI2Cモードにする
// STM32でよくある構成は
// PB6 → SCL
// PB7 → SDA
// SCL
// SDA
GPIO_InitStruct.Mode
= GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull
= GPIO_PULLUP;
GPIO_InitStruct.Speed
= GPIO_SPEED_FREQ_HIGH;
- INIT_PWM()
// PWMタイマ起動
HAL_TIM_PWM_Start(
&htim1,
TIM_CHANNEL_1
);
// 変換：duty → PWM
__HAL_TIM_SET_COMPARE(
&htim1,
TIM_CHANNEL_1,
duty * htim1.Init.Period
);
- INIT_DIR()
// AIN1, AIN2 を出力ピンにする
// GPIO_MODE_OUTPUT_PP 設定
// GPIO: AIN1
HAL_GPIO_WritePin(
AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
// GPIO: AIN2,
HAL_GPIO_WritePin(
AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
- INIT_STBY()
// STBYピンを出力に設定
// STBY を有効化
HAL_GPIO_WritePin(
STBY_GPIO_Port,
STBY_Pin,
GPIO_PIN_SET
);
- INIT_ADC()
// モータ電流取得用ADC開始
// ADCが変換できる状態にする
HAL_ADC_Start(&hadc1);
|