モータ制御のためのTIM/ADC設定(STM32・コード記述編)

前回はCubeMXでの設定をしました。

今回はこちらの設定をCubeMXで吐いたコードを編集します。

①CubeIDEプロジェクトの作成

こちらの手順にしたがい、CubeIDEで開発できる環境を整えてください。
またC++を利用して記述するため、C++に対応できるようにMakefileを編集してください。

編集箇所が多いので、抜けていたり、環境がイタズラしたりと、うまく行かないことがあると思います。
そのようなときのために、編集済み・確認済みのgitを用意しておきましたので、こちらを利用してください。

https://github.com/YutakaNakamura/G431_MC_Proj/tree/LED_UART_Check

②Main.cppを記述
以下のように、int main()を適当に編集します。

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_LPUART1_UART_Init();
  MX_ADC1_Init();
  MX_TIM1_Init();
  MX_ADC2_Init();
  /* USER CODE BEGIN 2 */
  if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1) != HAL_OK)
  {
   Error_Handler();
  }
  if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2) != HAL_OK)
  {
   Error_Handler();
  }
  if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3) != HAL_OK)
  {
   Error_Handler();
  }

  //disableにすれば出力されない
  if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4) != HAL_OK)
  {
   Error_Handler();
  }

  TIM1 -> PSC = 17000;
  TIM1 -> ARR = 10000;
  TIM1 -> CCR1 = 7500;
  TIM1 -> CCR2 = 5000;
  TIM1 -> CCR3 = 2500;
  TIM1 -> CCR4 = 9990;

  HAL_ADCEx_InjectedStart_IT(&hadc1);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  mySqrt<float> msqrt(0.1f);

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

    /* USER CODE BEGIN 3 */
	  char buf[] = "USART TEST\r\n";
	  HAL_UART_Transmit(&hlpuart1, (uint8_t*)buf, sizeof(buf), 1000);

	  float sqrt5 = msqrt.Calc(5.0f);
	  int delay = 100 * sqrt5;
	  HAL_Delay(delay);
	  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET);
	  HAL_Delay(delay);
	  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET);
  }
  /* USER CODE END 3 */
}

MX_TIM1_Init();
でCubeMXで指定したタイマの設定がされます。
MX_ADC1_Init();
でCubeMXで指定したADCの設定がされます。

これらは内部でレジスタを設定しています。
Ctrl+関数をクリック してジャンプしていくと、いずれはレジスタを叩いている場所に飛べるはずです。

HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1)
でTIM1のCH1のPWMを出力します。

TIM1 -> PSC = 17000;
TIM1 -> ARR = 10000;
TIM1 -> CCR1 = 7500;
TIM1 -> CCR2 = 5000;
TIM1 -> CCR3 = 2500;
TIM1 -> CCR4 = 9990;
ではレジスタを直叩きします。それぞれの意味は以下の通りです。
プリスケーラを17000 (170MHzのクロックを、10KHzまで分周します)
TIM1のカウンターを10000に設定。(10000でリセットがかかる)
CH1の閾値を7500に設定。
CH2の閾値を5000に設定。
CH3の閾値を2500に設定。
CH4の閾値を9990に設定。

ちなみにこのPWMのDutyを編集するのに、レジスタ直叩きの他に、
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 7500);
WRITE_REG(TIM1->CCR1, 7500);
などがありますが、全部同値です。マクロ仲介してわかりにくくなってるだけなので、直叩きしています。
また、HALの構造体を再度宣言して、HAL_TIM_PWM_ConfigChannel()関数を叩く手法もありますが、こちらは処理時間的に勿体ないので使いません。

これをコピペをするだけだと、
mySqrt<float> msqrt(0.1f);の定義で詰まって動かないと思いますが、
class mySqrtの宣言・定義は前回のcppに対応する記事を見てください

②stm32g4xx_it.cを記述
こちらにADCの終了割り込み時の動作を記述します。
別に他の所に関数を書いてもいいのですが、今回はお試しなのでここに全部書いちゃいます。
void ADC1_2_IRQHandler(void)内に書いていきます。
内部でUSARTでの出力処理を行うため、#include “usart.h”が必要になります。

/******************************************************************************/
/* STM32G4xx Peripheral Interrupt Handlers                                    */
/* Add here the Interrupt Handlers for the used peripherals.                  */
/* For the available peripheral interrupt handler names,                      */
/* please refer to the startup file (startup_stm32g4xx.s).                    */
/******************************************************************************/

#include "usart.h"
/**
  * @brief This function handles ADC1 and ADC2 global interrupt.
  */
void ADC1_2_IRQHandler(void)
{
  /* USER CODE BEGIN ADC1_2_IRQn 0 */

	  char buf[] = "■ADC interrupt\r\n";
	  HAL_UART_Transmit(&hlpuart1, (uint8_t*)buf, sizeof(buf), 1000);

	  volatile int adc1 = ADC1 -> JDR1;
	  volatile int adc2 = ADC1 -> JDR2;
	  volatile int adc3 = ADC1 -> JDR3;

	  char str[100] = {0};

	  sprintf(str,"adc1:%d, adc2:%d, adc3:%d\r\n",adc1,adc2,adc3);
	  HAL_UART_Transmit(&hlpuart1, (uint8_t*)str, sizeof(str), 1000);

  /* USER CODE END ADC1_2_IRQn 0 */
  HAL_ADC_IRQHandler(&hadc1);
  HAL_ADC_IRQHandler(&hadc2);
  /* USER CODE BEGIN ADC1_2_IRQn 1 */

  /* USER CODE END ADC1_2_IRQn 1 */
}

ADCの動作が終了したタイミングで、こちらが呼ばれて、USARTでADCの入力値を返します。

volatile int adc1 = ADC1 -> JDR1;
volatile int adc2 = ADC1 -> JDR2;
volatile int adc3 = ADC1 -> JDR3;

 

こちらがADCの値が格納されているレジスタです。
レジスタを叩くときには、最適化を防止するためvolatileをつけることが肝心です。

修正は以上です。
マイコンに書き込んでみると次のようになります。

また、UARTの出力を見てみると、次のようになります。

これらから、目的のタイミングで動作していることがわかりました。
以上で全説明は終了です。

今回作成したものは以下から取得できます。

https://github.com/YutakaNakamura/G431_MC_Proj/tree/LED_UART_Check

モータ制御のためのTIM/ADC設定(STM32・CubeMX編)

①概要

モータ制御をするためには、PWMを出力するだけではなく、ADCで適切なタイミングで状態を読み取る必要があります。
(適当な電圧出力だけでも回りはしますが、”それはモータ制御なのか?…”と問われると違うと思います。)

一例として、以下の画像のようなタイミングで動作させることが、今回の目標です。

画像を説明すると、TIMを利用して、75%,50%,25% DutyのPWMと、100%タイミングで、
ADC用のトリガ信号を出力します。また、このトリガ信号を利用して、ADCを動かしてみます。
“なぜこのタイミングでADCを動作させるか”というと実際の(3シャント方式電流検出の)モータドライバの回路は以下のようになっています。

モータから流れる電流が、橙の丸で囲ったシャント抵抗に流れる電流を測定するには、ローサイド側のMOSFETが下がっている時しか(流れないので)検出できないのです。
そのため、確実にFETがOFFになるタイマ100%時にADCを動作させます。

以下が今回執筆時環境です。

IDE
STM32CubeIDE
Version: 1.2.0
Build: 5034_20200108_0926 (UTC)

CubeMX
Version: 5.4.0

Board
NUCLEO-G431RB

②PWM設定

次のようにPWMを設定します。

TIM1 Mode and Configuration – Mode
Channel1 PWM Generation CH1
Channel2 PWM Generation CH2
Channel3 PWM Generation CH3
Channel4 PWM Generation CH4
と設定していきます。

TIM1 Mode and Configuration – Configuration – Parameter Setting
Counter Setting
  Counter Mode Center Aligned mode 1
Trigger Output(TRGO) Parameters
  Trigger Event Selection TRGO Update Event
  Trigger Event Selection TRGO2 Output Compare(OC4REF)
と設定していきます。

今回は設定しませんでしたが、Prescalerに値を設定するとカウントを分周します。
≒カウント動作が遅くなります。

Counter Period がカウント周期です。
本来はここに10000などの値を設定しますが肝心な箇所なので、
こんなコーダーに任せるよりも自分でソース内に記述したほうがいいよね???
の精神で、今回は後で触ります。

Center Aligned mode1を選ぶことによって、は山のようなカウンタの動作をします。 
(本記事の一番上の画像・Tim Counter参照)

TRGOの設定は、TRGO2にCH4のカウンタ値との比較の出力を出します。
このTRGO2を利用して、今回はADCを動かしていきます。

Break And Dead Time management – BRK Configuration
  設定無し
Break And Dead Time management – BRK2 Configuration
  設定無し
Break And Dead Time management – Output Configuration
  設定無し
Clear Input
  設定無し
Pulse On Compare ( Common for Channel 3 and 4)
  設定無し

PWM Generation Channel 1
  Mode PWM mode 1
PWM Generation Channel 2
  Mode PWM mode 1
PWM Generation Channel 3
  Mode PWM mode 1
PWM Generation Channel 4
  Mode PWM mode 2

PWM Generation ChannelのPulseには、数値を入れることで、TIMのカウントと比較して、カウントよりもPulseの値が大きいときは出力をONにします。
PWM mode2にすることで負論理になります。

こちらも、Counter Periodと同様、ソース内で変更します。覚えておいてください。

TIM1 Mode and Configuration – Configuration – NVIC Setting
すべてにチェックをいれます。

割り込みの設定です。とりあえず使わなくても特に損しないのでチェックを入れておきます。

以上がPWM設定です。次にADCの設定に移ります。

③ADC設定

次のように設定します。

ADC1 Mode and Configuration – Mode
IN1 IN1 Single-ended
IN7 IN7 Single-ended
IN6 IN6 Single-ended

と設定していきます。
図中では、IN2,IN12にもSingle-endedで使っていますが、モータの電流測定は上記3種の予定で、実際には以下のものを観測することを想定しております。
IN1 – モータU相電流
IN7 – モータV相電流
IN6 – モータW相電流
IN2 – VBUS電圧
IN12 – ポテンショメータ出力
(ADC2) IN8 – IC温度出力

なので、今回は駆動用の電流検出のみに目をむけて、ADC1のIN1,7,6のみ説明致します。

ADC1 Mode and Configuration – Configuration – Parameter Settings
ADCs_Common_Settings ~ ADC_Regular_ConversionMode
  設定無し
ADC_Injected_ConversionMode
Enable Injected Conversions Enable
Number Of Conversions 3
External Trigger Source Timer 1 Trigger Out event 2
External Trigger Conversion Edge Trigger detection on the rising edge
Rank 1 Channel Channel 1
Rank 2 Channel Channel 7
Rank 3 Channel Channel 6

とりあえず設定項目が非常に多いので、大切な事だけ書きました。
Injected_ConversionModeは割り込み要求用のADC設定です。
Timer1のTRGO2から、立ち上がりエッジで割り込みがかかり、Ch1,Ch7,Ch6の順に変換します。

この3チャンネルだけはこのようにタイマに連動した割り込みの設定が必要ですが、他のチャンネルはRegular_ConversionModeなどに入れて、適当に使えばよいです。

ADC1 Mode and Configuration – Configuration – NVIC Settings
チェックをいれます。

以上が、ADCの設定の説明でした。
あとは自分が使いたいものを増やしたりして、コードを生成すれば良いです。

コード記述編へ続く。

鏡映変換Q(θ)について

ここでは鏡映変換

\[
Q(\theta) = \left(
\begin{array}{cc}
\cos(2\theta) & \sin(2\theta) \\
\sin(2\theta) & -\cos(2\theta)
\end{array}
\right)
\]

について考えます。

まず、モータ制御をするにあたって、次のような性質がある行列が分かると便利です。

「α,β平面にある任意の点xを、”角度θを対称とする角度かつ、原点からの距離がxと同じ”点に映す。」

何言ってるかわからないので、下の図を見てください。

はい、これを考えていきます。
考えるにあたり、変換を次のように解釈すれば容易です。

①回転行列をかけて補助線0度にする。

②0度を中心の鏡映変換を考えてあげる(ここではTとします。)

③補助線をもとの角度θに戻す。

この操作をすることで、

\[Q(\theta)=R(\theta)TR(-\theta)\]

という行列に変換できます。
(結局Tで鏡映変換しているので、面倒くさくしてるだけじゃん!)と思われると思いますが、0度の鏡映変換は、β(y座標)を反転させるだけなので、極めて簡単な行列で表現できます。

\[
T =
\begin{pmatrix}
1 & 0 \\
0 & -1
\end{pmatrix}
\]

これを使って以下のように計算できます。

\[
R(\theta)TR(-\theta)
\]

\[
= \begin{pmatrix}
\cos\theta & -\sin\theta \\
\sin\theta & \cos\theta
\end{pmatrix}
\begin{pmatrix}
1 & 0 \\
0 & -1
\end{pmatrix}
\begin{pmatrix}
\cos\theta & \sin\theta \\
-\sin\theta & \cos\theta
\end{pmatrix}
\]

\[
=\begin{pmatrix}
\cos^2 \theta – \sin^2 \theta & 2\sin\theta \cos\theta \\
2\sin\theta \cos\theta & -\cos^2 \theta + \sin^2 \theta
\end{pmatrix}
\]

\[
=\begin{pmatrix}
\cos(2\theta) & \sin(2\theta) \\
\sin(2\theta) & -\cos(2\theta)
\end{pmatrix}
\]

\[
=Q(\theta)
\]

つまり、θを対称とする鏡映変換は、このような2θの三角関数の行列で表現できる訳です。面白いですね。

モータ制御キットを作った

モータを制御するためには、
・電源
・コントローラ
・ドライバ
などとそれぞれ分かれているため、配線などが煩雑になってしまいます。


さらに、私は(現実ではなく)インターネット上で知り合った方と一緒にモータ制御をやっているため、個人個人の環境で全然違う事が起こる現象、いわゆる「おま環」が発生するのを嫌い、作ってみました。

まずはマイコン基板を作ります。

途中遊びでレース柄の基板も作ってみました。
仕事じゃないのでオシャレな柄で作りたいなーって思って作っているのですが、流石にフットプリントが全く見えないデザインは、実装がしんどいので、今度は実装まで頼もうと思います…

3相ドライバは中華製のものを購入しました。
以下が実際に作成したものを、中華製の市販品に接続してる風景です。

CADを作成して、アクリルは発注して作ってもらいました。

本棚にも収まるので、制御則など、理論を触りたい人にとってはとても収まりが良いかと思います。

一品仕上げるのに、非常に手間がかかるため(主にはんだ付けで!)
現在は市販等の予定はありません…

モータの開発環境をA基板上に収めた

モータの開発環境は非常に散らかります。
モータ・インバータ・電源と3つの要素があり、それぞれを配線が結ぶからです。
片付けるのが大変だし、何かと異常が発生していても気づきにくいので、秋月のA基板の上に収めました。

使用している環境としては、
P-NUCLEO-IHM001のキット
https://jp.rs-online.com/web/p/power-management-development-kits/9064630/
をSTM32F446
http://akizukidenshi.com/catalog/g/gM-10176/
で開発。
電源は秋月のこちらを利用。
http://akizukidenshi.com/catalog/g/gM-10663/

現在オブザーバ構成中の為、ひたすらMATLABとIDEをにらめっこ。
(すいません…これ以上の進展はありません…)