---
tags: 東京威力 TEL
---

# V. STM
## <font color = "orange">02. PID Control</font>

### <font color = "pink">2-5. 用 Matlab 算 PID </font>
> Matlab辅助PID调参 https://sourcelizi.github.io/202001/matlab-sys-identfiy-pid/

<font color="yellow"> **Step 1. 下載 Matlab** </font>
>[Matlab下載網站](https://www.mathworks.com/downloads/)

記得用**學校帳號**登入Matlab，進入後會出現如下畫面。


其他設定原則上都不用更改，看個人喜好。
而有一步是選擇要安裝的工具：


如果考慮電腦空間容量有限，至少**要選擇**以下幾項：
* MATLAB
* Control System Toolbox
* System Identification Toolbox
* Symbolic Math Toolbox
* Optimization Toolbox
* Simulink
* Simulink Control Design
* Simulink Design Optimization

<font color="yellow"> **Step 2. 確認 STM 中斷函式的頻率**</font>
時鐘樹（Clock tree）是 STM32 中很重要的一部分，可以全面瞭解所有時鐘源配置，因為各個功能都有自己的運作頻率，在使用 Timer 前最好有一定的認識。

上圖是STM32H723ZGT6的時鐘樹
可以看到APB1連接的是TIM2、TIM3、TIM4、TIM5、TIM6、TIM7、TIM23、TIM24、TIM12、TIM13、TIM14
APB2連接的是TIM17、TIM16、TIM15、TIM1、TIM8

所以從STM32CubeIDE中的Clock Configuration看APB1、APB2的實際時脈頻率
以上圖為例APB1 Timer Clocks = 64MHz；APB2 Timer Clocks = 64MHz

:::info
假設我要的定時器中斷週期為1ms，使用timer 6：
我的 APB1 timer clock = 64 MHz
那麼PSC跟Counter Period就會是
預分頻器Prescaler(PSC) = 64-1
計數器週期Counter Period = 1000-1
:::

$$
Timer\ Clock = {APB\ Clock \over {Prescaler + 1}} = {64,000,000 \over {63 + 1}} = 1\ MHz
$$
$$
Counter\ Frequency = {Timer\ Clock \over {Counter\ Period + 1}} = {1,000,000 \over {999 + 1}} = 1\ kHz
$$
中斷週期= 1/1kHz = 1ms

<font color="yellow"> **Step 3. 設定PWM輸出時鐘 rps**</font>
預分頻器Prescaler(PSC) = 0
計數器週期Counter Period = 6400-1

<font color="yellow"> **Step 4. 在 STM 開一個長度 1000 的陣列抓 rps**</font>
我們會透過陣列來記錄車輪轉速，之後會在 Matlab 把資料模擬成曲線。
其中紀錄的是**輪速從靜止至給滿 PWM (6400) 的過程**。

這邊提供我的版本。
要用 Matlab 調 PID 時直接把這個函式放進中斷函式就可以了。

// 參數 abcd 決定要紀錄哪個輪子
void motor_matlab(bool a, bool b, bool c, bool d){
	static int array_count=0;

    /*enc 1*/
	fr.CountNow = __HAL_TIM_GetCounter(&htim2);
	__HAL_TIM_SetCounter(&htim2,0);

	fr_array[array_count]=fr.PIDControl_manual();
	if(array_count>4000) array_count=4000;

	HAL_GPIO_WritePin(MOTORPLUS_PORT_fr, MOTORPLUS_PIN_fr, GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTORMINUS_PORT_fr, MOTORMINUS_PIN_fr, GPIO_PIN_RESET);

	if(a) __HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_2, 6400);
	else __HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_2, 0);


    /*enc 2*/
	fl.CountNow = __HAL_TIM_GetCounter(&htim5);
	__HAL_TIM_SetCounter(&htim5,0);

	fl_array[array_count]=fl.PIDControl_manual();
	if(array_count>4000) array_count=4000;

	HAL_GPIO_WritePin(MOTORPLUS_PORT_fl, MOTORPLUS_PIN_fl, GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTORMINUS_PORT_fl, MOTORMINUS_PIN_fl, GPIO_PIN_RESET);

	if(b) __HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_1, 6400);
	else __HAL_TIM_SET_COMPARE(&htim12, TIM_CHANNEL_1, 0);


    /*enc 3*/
	br.CountNow = __HAL_TIM_GetCounter(&htim3);
	__HAL_TIM_SetCounter(&htim3,0);

	br_array[array_count]=br.PIDControl_manual();
	if(array_count>4000) array_count=4000;

	HAL_GPIO_WritePin(MOTORPLUS_PORT_br, MOTORPLUS_PIN_br, GPIO_PIN_SET);
	HAL_GPIO_WritePin(MOTORMINUS_PORT_br, MOTORMINUS_PIN_br, GPIO_PIN_RESET);

	if(c) __HAL_TIM_SET_COMPARE(&htim15, TIM_CHANNEL_1, 6400);
	else __HAL_TIM_SET_COMPARE(&htim15, TIM_CHANNEL_1, 0);


    /*enc 4*/
	bl.CountNow = __HAL_TIM_GetCounter(&htim4);
	__HAL_TIM_SetCounter(&htim4,0);

	bl_array[array_count]=bl.PIDControl_manual();
	if(array_count>4000) array_count=4000;

	HAL_GPIO_WritePin(MOTORPLUS_PORT_bl, MOTORPLUS_PIN_bl, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(MOTORMINUS_PORT_bl, MOTORMINUS_PIN_bl, GPIO_PIN_SET);

	if(d) __HAL_TIM_SET_COMPARE(&htim15, TIM_CHANNEL_2, 6400);
	else __HAL_TIM_SET_COMPARE(&htim15, TIM_CHANNEL_2, 0);

	array_count++;
}

:::info
陣列長度不一定要 5000，只要不會有 overflow 問題就好。
:::

<font color="yellow"> **Step 5. 紀錄轉速。**</font>
這裡的陣列必須放在 Expressions。
程式執行時，變數會在現場表達式 ( live exporession )變動，但因為我們要先結束程式再存取變數值，所以要放在 Expressions，程式結束後才會有值。

展開前 500 個元素，預設值應該都會是0。


接下來要注意操作的順序：
1.$\;$STM 程式是讓某個輪子一直輸出最高轉速。
2.$\;$燒錄完後，**先上直流馬達的電**（12 V）。
3.$\;$執行程式，輪子立刻全速旋轉，大約一秒就「暫停」程式。
4.$\;$紀錄完數據後馬達下電（12 V）。

<font color="yellow"> **Step 6. 把陣列前 500 筆資料複製到 Excel。**</font>




:::info
要取幾筆沒有一定，只要確定資料中的輪速有到最高速，並且在最高速有來回震盪即可。
:::

<font color="yellow"> **Step 5. 把 RPS 資料丟到 Matlab 上。**</font>
打開 Matlab，點上方「Variable」->「New Variable」。


複製 RPS 資料到 Matlab。


:::warning
注意 Matlab 的資料如果有 NaN 的話必須刪掉
:::

<font color="yellow"> **Step 7. Rename 和新建另一個 Variable。**</font>
把剛讀取到的轉速名稱改成「output」，改名稱的地方在最右方的欄位中。


再開一個新的「Variable」，命名為「input」。
根據「output」的資料數量，在第一欄寫入對應數量的「1」，如下圖。


<font color="yellow"> **Step 8. 開啟 systemIdentification。**</font>

上方列表找到「APPS」，再開啟「System Identification」

<font color="yellow"> **Step 9. 設定 Time Domain Data。**</font>



:::info
Import Data 的參數設定：
* Input 與 Output 填的是先前修改的檔案名 input 與 output。
* Start Time 設 0。
* Sample Time 是樣本的時間差(秒)，也就是陣列元素取樣頻率，因為剛剛上面設定的 Timer 頻率是 1 kHz，所以時間差是 0.001 秒。
:::

<font color="yellow"> **Step 10. 設定 Transfer Function，並生成 Model。**</font>

利用剛剛導入的數據生成 Transfer Function

設定 TF 要得極點和零點個數，極點 2，零點 1 (要比極點少)
下方選擇Discrete-time，因為我們輸入的資料不是連續的，encoder讀到的數據是一階一階的
最後點 Estimate


可以觀察模擬出的 TF 的擬合程度，像我的有94.08%，最後按 Close即可

<font color="yellow"> **Step 11. 雙擊 tf1，輸出數據。**</font>


會有的是模擬出的 Transfer Function
按 Export 輸出數據，會出現在workspace

<font color="yellow"> **Step 12. 開啟 「PID tuner」，輸入模型**</font>

Import 剛剛的 tf 模型

把Type P 改成 PI，就可以開始正式調參數啦!

<font color="yellow"> **Step 13. 設定 Block。**</font>
建立 「PID Controller」 區塊。


一樣的方式建立「Step」、「Transfer Fcn」、「Sum」區塊。


 

<font color="yellow"> **Step 14. 修改 Block 流程如下。**</font>


<font color="yellow"> **Step 15. 點擊 Sum 區塊並修改參數。**</font>


<font color="yellow"> **Step 16. 點擊 Step 區塊並修改參數。**</font>


<font color="yellow"> **Step 17. 點擊 Transfer Fcn 區塊並修改參數。**</font>
把剛剛點擊 tf1 後保留的視窗內的參數照著填過來。


改完之後如果出現如下圖示表示有修改，但圖框太小顯示不完全。


將圖框拉大後如下。


<font color="yellow"> **Step 18. 點擊 PID Controller 區塊並修改參數。**</font>


<font color="yellow"> **Step 19. 調 PID 的視窗。**</font>



:::info
雖然越早穩定越好，但可能 0.1~0.25 秒也許是極限了，要知道這只是理論值、模擬環境。
如果調得太極端，可能會讓馬達實際上過衝太多，大家可以自己去試試看最適合的界線在哪裡。
:::

<font color="yellow"> **Step 20. 將得到的 PID 參數丟回 STM 測試就可以了！**</font>
PID 參數會在 「PID Controller」區塊中。


:::success
調出來是小 P 大 I，也許很反直覺，但這是因為我們以 time domain 的角度去思考。
還記得前面有設定 step function 、transfer function 和一些分數的值嗎？

$$ \mathscr{L} \{u(t)\} = 1/s $$
unit step function 在 laplace tranform 後變數在分母，就姑且先這樣理解為什麼 PI 大小相反。
transfer function 將原本的 T-domain 轉移到 S-domain，其中 $s$ 代表 frequency。
laplace tranform 大二上就會學到了，很期待吧 !
:::