[TOC]

# 馬達控制

## 微處理器與控制實現


- 使用微處理器實現一些控制或機器人系統，首先應該了解微處理器在一個經典的回授控制方塊圖中扮演的角色。把微處理器也視為一個系統，本身也有輸入和輸出 :
- input 做為控制系統輸入，可以固定的寫死在程式中，也可以由更上層的控制器角色給予
- feedback 做為控制系統回授訊號，使微處理器可以結合input計算err，再依據控制算法生成output輸出
- 圖中不論是進入或從微處理器輸出的訊號，都可以是單一GPIO的類比或數位訊號，或是藉由通訊方式對訊號的"資訊"做傳輸
- 使用微處理器實現控制系統意味著控制方塊圖中的訊號變為數位訊號，在處理器中的濾波器、控制器等設計皆使用離散方式做數位訊號處理的程式設計，然而現實的訊號是連續的，使用數位訊號必然會提及<font color="red">取樣時間(Sampling Time)</font>的概念
- 取樣時間影響系統穩定度、控制精度、頻寬
- 不固定或未知的取樣時間使得信號重建、狀態預估等等無法實現，
- 使用微處理器timer的功能使得控制系統更predictable，一切都會在你的掌控之下運行
- 因為微處理器的output是基於input和feedback計算而得的，因此在實現上可以這麼做
1. 在timer ISR中放input和feedback的接收，在while中做計算與output輸出控制
2. 將所有東西都放在timer ISR
- 不論何種寫法，都應確保計算與輸出部分能在取樣時間(timer 中斷週期)內完成
- timer ISR內應該盡量保持最簡單的操作。個人推薦在ISR中放狀態、判斷flag的改變，再從while去做接收信號處理、控制計算、控制輸出是最安全的作法

## 控制器(Controller)
- 從控制方塊圖來看控制器主要的功能就是把error轉換成給入plant的訊號。控制器在控制理論中用數學方式表達，具體實現只要能把輸入依照數學式生成輸出都可稱得上控制器，比如電路、微處理器、甚至機構。
- 以微處理器實現控制器的關鍵在於如何把數學式轉換成程式，尤其是連續時間的轉移函數變成程式的離散寫法更是要特別注意Sampling Time的影響
- 本次新生訓練使用PID控制器控制馬達即可，PID的背景相關知識可[上網](https://hackmd.io/@metal35x/BkAXIC37L)查詢
- [嵌入式系統實現PID](https://blog.csdn.net/as480133937/article/details/89508034)
- PID的參數決定系統不同特性，可見[維基](https://zh.wikipedia.org/zh-tw/PID%E6%8E%A7%E5%88%B6%E5%99%A8#%E5%8F%83%E6%95%B8%E8%AA%BF%E8%A9%A6)。對參數會造成的影響有點概念後，依據響應結果調整參數也較容易
- P : 直接放大誤差，相當於系統對 error 的敏感度，P大太敏感，容易振，P小布敏感，暫態追得慢
- I : 可以改善穩態誤差
- D : 減少OverShoot、抖動

# 新訓項目
- [demo影片](https://youtube.com/playlist?list=PLMZMEiIur5WfY-CRXoh9wYM5b3S5ZztQx&si=AkLECBhe-t5LVCb8)

## 流程
1. 試著仿照一般回授系統<font color="red">畫出控制方塊圖</font>，把微處理器的輸入輸出映射到方塊圖上會更好想像微處理器要做的工作
2. 程式規劃，可以想像的到微處理應該需要做下列功能
- 使用 timer 中斷固定取樣時間
- 使用timer的encoder mode讀取編碼器計數
- 使用PID算法調整PWM輸出與DIR切換
- 注意計數器的溢出處理，當QEP計數(上/下)溢出時，能判斷馬達轉向
- 盡量使用全域變數好觀測數據與指定輸入，從debug mode的watch window 更改
3. 接線
- 規格 : 12V馬達、96-pulse 編碼器
- 馬達腳位
- 完整線路圖
- 驅動器的DIR可以控制有刷馬達轉動方向
- PWM控制馬達電壓
- HAL為電流sensor值，使用ADC讀取
- 注意馬達轉向受到馬達正負接線影響，確保你想讓馬達往編碼器正角度方向旋轉時，DIR訊號給出對的電位，否則控制器也無法作用
4. 燒錄時，電供一律關閉。在run之前，腳位皆是浮空狀態，因此也不能直接開電供。建議在run之後開電供，並且有一顆User Button作為控制總開關，避免一些控制算法因為執行到上電這段期間太久而輸出太大
5. debug mode主要用於程式出問題時debug方便，除了看變數外還能指定程式執行到哪邊。但如果是要調適參數的話請使用STMStudio。

## 低通濾波
http://blog.csdn.net/qq_37662088/article/details/125075600
## 位置控制
- 輸入一個角度，馬達會自動轉到該位置(以開機位置為0度)
- 回授為編碼器計數值轉換成角度
- 需利用DIR切換與PWM佔空比控制馬達位置
- 輸入與error單位請使用角度，調適參數盡量使error趨近於0，並讓暫態不那麼抖
- P控制器足已。
## 速度控制
- 輸入轉速，馬達會逐漸達到該轉速
- 回授為從編碼器計數值轉換成角度，再依據取樣時間轉換成角速度
- DIR固定一種即可
- 輸入與error單位請使用rpm、rms、rad/s
- P控制器足以。
- 讀取的角速度要加上一階低通濾波器避免雜訊被放大，若有作PD控制，D也要加上低通濾波器。PD控制器本身有無法抑制高頻雜訊的缺點(自動控制的觀念)。
## 電流控制
- **電流感測器(INA240A1PW)使用注意事項**
- datasheet 見 FTP 或上網查
- 理論的電壓與電流關係可以直接從 datasheet 中算得。見Table
- 實驗室使用R_sense為 15 m歐，Gain為20V/V
- 依據給驅動板的VCC值即決定V_out，可算得V_DIFF以及I_MAX
- 可讀取正反向電流，無電流時 V_out = Vcc/2
- 以 VCC 給 3V 為例，V_DIFF=150 mV，I_MAX=150m/15m=10A，可量測電流區間為0~10A對應1.5V~3V，-10A~0A對應0~1.5V，相當於
- 若微處理器讀取的ADC解析度為12bits，ADC範圍4096相當於電流範圍20A，ADC值每一格相當於 20/4096=5 mA

- **感測器校正**
- 使用電流控制前，需要知道驅動板電流感測器吐出的類比電壓值與實際電流的關係曲線
- 接線方式同上，電供給予12V。將驅動板LOAD_A、B改成接在水泥電阻的兩端
- 水泥電阻選擇高功率，低電阻可以讓我們量測到較大的電流值
- 以電表1量測水泥電阻兩端電壓，以回推實際的電流值。**法二，將驅動板之PWM腳位接上高電位，DIR腳位先接上高電位用三用電表作為電流計，改變電供電壓和水泥電阻阻值，得到不同的電流值**
- 以電表2量測HAL輸出電壓
- 給定不同PWM，並記錄每次電表1、2的數值，給的點數越多實驗結果越準
- 紀錄HAL電壓與實際電流關係曲線，並與理論值比較確認
- 將DIR腳位先上低電位，重複以上步驟。
- 註:DIR=3V時，HAL電壓>=VCC/2，且電流計數值為正。DIR=0V時，HAL電壓<=VCC/2，且電流計數值為負。

- **控制步驟**
1. 決定PWM頻率與ADC取樣頻率，為了固定ADC取樣頻率，使用timer trigger 觸發ADC轉換。
- PWM 1400 Hz
- timer trigger for ADC : 1400*100 Hz (每個PWM週期抓100筆)
- 建議參考**注意的第六項**PWM頻率越快越好，超過ADC頻率也沒關係。
2. 先用STMstudio繪製讀到的電流ADC值曲線
3. 觀察波型，應該是類似PWM佔空比的週期波
4. 添加平均濾波器，把現在接收到的資料和過去100筆(或更多)的資料作平均處理。使用array儲存過去的資料，資料筆數較多，可以使用Circular Buffer做實現

    #define BUFFER_SIZE 180
	uint32_t buffer[BUFFER_SIZE];
	int index = 0;
	float sum = 0.0;
	
	void addSample(float newSample) {
	    sum -= buffer[index];       // 移除舊的樣本
	    buffer[index] = newSample;  // 添加新的樣本
	    sum += newSample;           // 更新總和
	    index = (index + 1) % BUFFER_SIZE;  // 更新指標
	}
	
	float getAverage() {
	    return sum / BUFFER_SIZE;   // 計算平均值
	}

5. 以得到的平均值或低通濾波算法得到值**轉換成電流值(mA)作電流回授**，與給定電流目標得到誤差後進入控制器生成PWM訊號，電流目標輸入與error單位請使用mA。**建議使用PI控制器。**
6. 觀察輸入(平均電流)與輸出(平均電流)響應圖，同時繪製實際電流曲線在同一張圖上(STMStudio 取得資料後用MATLAB繪製)
- 注意
- 由於原本使用的小馬達在ADC的變化上不夠明顯、穩定，建議改使用大馬達做ADC控制，同時增加平均濾波器所收集的樣本，大馬達的ADC值落在2105~2140之間是可以控制的。
- 理論上我們可以從馬達模型推算出輸入電流會對應多少輸出扭矩，因此控制時需要將讀取到的數位電流值換算成實際電流值，這部分需要由實驗畫出對應曲線，這邊我們直接控制ADC值也可以看出電流控制效果，只是不知道實際電流值而已。**老師要求要測出實際電流的轉換公式，I=f(ADC value)。**
- 輸入電流命令(類比訊號值)使電流輸出固定，馬達會以一固定扭矩運行
- dir固定一個方向即可，以固定扭矩運行的馬達代表他在轉動時你能捏住它或將它反轉，或掛一個固定重物，逐漸增加電流命令，如果馬達從轉不動到轉起來，那麼也是有控起來
- 因為使用PWM控制馬達，因此**電流控制的雜訊會較大**，只要有看出響應趨勢以及改變輸入命令，觀察響應是否有追上即可(用STM studio 收集資料後，用MATLAB或Excel分析)。(有心的話)可參考以下影片之介紹，https://www.youtube.com/watch?v=i81TipZNoCc
- **PWM頻率越快越好**，超過ADC頻率也沒關係。CubeMX中設定ADC轉換Cycles越高雜訊越低，但要注意採樣頻率還是要小於轉換所需的頻率。
- **ADC轉換採樣頻率盡可能高**，讓低通濾波的算法中a越小，濾波算法效果越好。