---
tags: Course, Self-balanced Car, Instruction
---
# 雙輪平衡車
## 0. 目錄
1. 程式架構
2. 外部中斷 - 編碼器(Encoder)計數
3. timer中斷 & 外部中斷 - 輪速計算
4. 馬達控制函式 - 輪速命令控制
5. 藍芽相關函式 & 配對
6. 相關函式
## 1. 程式架構
在這個專題中，我們會建議Arduino Code使用下面的架構:
```cpp
#include <MsTimer2.h>
//include other libraries here
void InterruptFunc1(){
//Call the encoder counting function here.
}
void InterruptFunc2(){
//Call the encoder counting function here.
}
void TimerInterrupt(){
sei();
//Call the speed calculation function here.
}
void setup(){
//attachInterrupt(...[InterruptFunc1]...);
//attachInterrupt(...[InterruptFunc2]...);
//Iitial MsTimer2 here
}
void loop(){
//UART and Bluetooth communication function
}
```
其中兩個外部中斷的函式會更新編碼器的狀態。Timer中斷函式中讀取雙輪車的狀態資料(偏移角度、輪速等)，並根據回授的狀態計算控制輸出。
## 2. 外部中斷 - 編碼器(Encoder)計數
編碼器可以將位置資料透過某些方式(光、磁力)轉換成數位資料，紀錄馬達的轉動角度。編碼器可以藉由A, B兩個觸發源的相位差判斷轉向。
使用上，我們需要定義兩個腳位:
* **外部中斷腳位:**
* 需選擇 Arduino 板的外部中斷腳位
* **方向判斷腳位:**
* 根據回傳的狀態可以知道馬達的轉向
設一個變數count來儲存編碼器的移動位置。每當encoder觸發外部中斷，我們根據方向判斷腳位的狀態決定 +1 或 -1。接著，我們便可以藉由目前 count 的數值和編碼器的精度(一圈的觸發點個數)的比例計算出目前旋轉的角度。範例程式如下。
```cpp
#define INT_PIN 2
#define DIR_PIN 9
const int SECTION_NUMBER = 390;
int count = 0;
void EncoderInterrupt(){
if (digitalRead(DIR_PIN) == HIGH) --count;
else ++count;
}
float CalculateAngle(int count){
return 2*PI*((float)count/(float)SECTION_NUMBER)*RAD_TO_DEG;
}
void setup(){
attachInterrupt(digitalPinToInterrupt(INT_PIN),
EncoderInterrupt,
RISING);
}
void loop(){
Serial.print("Angle: ");
Serial.println( CalculateAngle(count) );
}
```
## 3. Timer中斷 & 外部中斷 - 輪速計算
使用 Polling 的方式往往會因為其他程式的干擾而以無法正確的時間間隔觸發。因此，為了確認速度的計算間隔為定值，我們會使用Timer來觸發輪速計算函式。
我們用上面的範例程式做延伸，加入以下程式碼:
```cpp
#inlcude <MsTimer2.h>
//...
float lastAngle = 0.0;
float dT = 0.01;
void TimerInterrupt(){
sei();
float speed = (currentAngle - lastAngle)/dT;
}
//...
void setup()
{
//...
MsTimer2::set(dT*1000, TimerInterrupt);
MsTimer2::start();
//...
}
```
注意當 Timer 被啟動後，由於它的優先度較高，它會禁止外部中斷和其他低層級中斷的觸發，**因此需要呼叫 sei() 來釋放中斷權限**。
上例中速度的計算是直接對角度做差分所得到，但這種方式得到的速度資訊會有很大的雜訊干擾。學過訊號與系統或DSP的同學可以嘗試設計數位濾波器處理速度資料以得到較好的結果。有興趣的同學可以參照[這理](https://www.robots.ox.ac.uk/~sjrob/Teaching/SP/l6.pdf)(p79開始)。
## 4. 馬達控制函式 - 輪速命令控制
馬達的控制需要定義以下腳位:
* **PWM 控制腳位**
* 選擇Arduino板的pwm輸出腳位，這個腳位會控制馬達的轉速。關於pwm的相關資料可參照[這理](http://thats-worth.blogspot.com/2014/04/arduino-pwm-arduino-pulse-width.html)。
* **方向控制腳位 * 2**
* 透過兩個腳位的邏輯輸出控制馬達的旋轉方向。
* **Standby 腳位**
以下是一個簡單的範例程式，透過UART傳送速度命令給控制馬達的轉速。
```cpp
/*
* You may need to modified the definition
* of pins for your hardware setting.
*/
#define PWM_PIN 5
#define LEFT_PIN 8
#define RIGHT_PIN 7
#define STANDBY_PIN 6
void setup(){
Serial.begin(9600);
pinMode(PWM_PIN, OUTPUT);
pinMode(LEFT_PIN, OUTPUT);
pinMode(RIGHT_PIN, OUTPUT);
pinMode(STANDBY_PIN, OUTPUT);
}
void motorRotate(int effort){
if(effort > 0){
digitalWrite(LEFT_PIN, LOW);
digitalWrite(RIGHT_PIN, HIGH);
}
else{
digitalWrite(LEFT_PIN, HIGH);
digitalWrite(RIGHT_PIN, LOW);
}
digitalWrite(STANDBY_PIN, HIGH);
analogWrite(PWM_PIN, effort);
}
void controller(float reference){
/*
* You may want to design your own control law here
*/
int effort = static_cast<int>(reference);
motorRotate(effort);
}
void loop(){
String reference = "";
if (Serial.available() > 0) {
reference = Serial.readString();
controller(reference.toFloat());
}
delay(100);
}
```
以上的範例並沒有實作速度控制器，只是單純將指令轉換成pwm的數值。請同學先收集馬達的響應資料，再運用上學期的學習到的控制相關內容，設計一個速度控制器。關於如何蒐集馬達資料和進行分析，我們會在下一個部分解說。
另外，以上範例是使用 polling 的方式去觸發控制器的回授控制。請同學熟悉馬達控制的架構之後，將上面的範例改成用 timer 觸發控制、用 polling 接收控制命令，以利之後專題的進行。
## 5. 用UART(或藍牙)將Arduino的輸出內容傳到電腦(Matlab)
以下提供兩種與電腦傳送資料的方式:
* **UART**
UART是一種通訊架構的總稱。以Arduino nano上的RS-232介面為例，由TX, RX兩個數位輸出組成。優點是構造簡單，但缺點是只能實現一對一的連接，且通訊速度稍慢，一般而言最高為115.2kbps。由於現今電腦已不再設有RS-232的街口，因此大多需要一條轉USB接口的線方便我們開發使用。
$~~~~~~~~~~~~~~~$
* **Bluetooth**
一種無線通訊技術標準，用來讓固定與行動裝置，在短距離間交換資料，以形成個人區域網路。它最初的設計，是希望建立一個RS-232資料線的無線通訊替代版本。它能夠連結多個裝置，克服同步的問題。(來源: [維基百科](https://zh.wikipedia.org/wiki/%E8%97%8D%E7%89%99))
### 5-1. 用UART和Matlab進行資料傳輸
以下是一個 Arduino 的資料透過 UART 將傳送到 Matlab 的範例:
**Arduino**
```cpp
void setup(){
Serial.begin(9600);
}
float f = 3; //Hz
float t = 0.0;
void loop(){
while (t < 10.0){
float data = 5*sin(2*PI*f*t);
Serial.println(data);
t += 0.01;
delay(10);
}
}
```
上面範例會讓Arduino輸出一個固定頻率的 sine 波。在這裡我們輸出資料使用println，讓我們的資料結尾會有一個換行符號，讓我們用matlab接收字串時更為方便。
**Matlab**
```matlab
clc
clear
s = serial('COM19', 'BaudRate', 9600, 'Terminator','CR');
fopen(s);
data_length = 100;
data = zeros(data_length, 1);
for i = 1:data_length
data(i) = str2double( fscanf(s) );
end
objs = instrfind;
fclose(objs);
```
上面的是一個讀取固定長度資料量的範例。首先，我們需要透過 _serial()_ 產生一個 Serial 物件，接著呼叫 _fopen()_ 讓 matlab 跟選定的 serial port 連結。需要注意的是，UART 的 port 同時間只能一對一連結，因此開啟 Arduino 端的 Seria 視窗，或是上傳檔案時，matlab 會無法連上相同的 port。反之亦然。
_fscanf()_ 是matlab讀取文字檔案的函式，我們用它來讀取從Arduino傳來的 "一行" 資料，這是因為我們在arduino端會在每筆資料的結尾加上'\n'的換行符號，可以用這個方式的讀取較為簡單方便。而由於 _fscanf()_ 回傳 char 型態的資料，我們需要用 _str2double()_ 轉換成 float 資料型態，方便後續的資料處理。
最後，記得要用 _fclose()_ 讓剛剛連上的 port 中斷連結，讓 Arduino 端可以使用。instrfind 會回傳所有可用的 port 的物件，用這個方式關閉比較能夠保證不會漏關掉，導致下次 matlab 連不到。
然而，上面的寫法有一個缺點，如果Aduino發送的資料少於100筆，由於 _fscanf()_ 會一直等待下一筆資料進來，maltab會停在 for 迴圈裡面。同學們可以想一想如何避免這個問題的發生。
將接收到的資料繪製出來，會得到下列結果。
另外，同學們可以嘗試調整不同的頻率，看看輸出訊號會怎麼改變? delay時間和輸出結果有沒有關係? 有興趣的同學可以去複習一下訊號與系統或DSP，應證一下測試的結果。
### 5-2. 使用藍芽傳送資料到matlab
**Arduino**
我們需要先 include 藍芽相關函式，並進行初始化設定，設定我們需求的藍芽baudrate, 以及藍芽裝置的 Tx、Rx 腳位。另外，該函式庫有針對HC-05以及HC-06有不同的設定，因此需要在這邊定義你所使用的是哪一種類的裝置。
```cpp
#include "bluetooth.h"
#define SERIAL_BAUD_RATE 38400
#define BT_RX A0
#define BT_TX 13
//#define BT_SPECIFICATION "HC05"
#define BT_SPECIFICATION "HC06"
```
設定完參數後，接著宣告一個 bluetooth 物件並啟動。我們會先檢查裝置是否在ATMode，並可以預先寫好想要想在 AT Mode 底下對裝置進行的設定，就不用每次都要透過 Arduino IDE 的 Serial 視窗手寫輸入。
值得注意的是，HC-05需要透過硬體輸入才會被動進入 AT Mode，而HC-06在裝置未連線前會主動進入 AT Mode。
```cpp
// create devices
Bluetooth myBT(BT_RX, BT_TX, BT_SPECIFICATION);
void setup() {
Serial.begin(SERIAL_BAUD_RATE);
// Terminal setting
// BT_SPECIFICATION: HC05 -> Both NL & CR
// BT_SPECIFICATION: HC06 -> No line ending
myBT.Begin();
Serial.println("Ready!");
// the name and the pin coed are changed in AT mode only
//Serial.write(myBT.CheckATMode());
if (myBT.CheckATMode()) {
Serial.write("In AT mode.\n");
/*
Serial.write(myBT.setPinCode("2018"));
Serial.write(myBT.setName("mars06"));
Serial.write("set over\n");
delay(1000);
// no any meaning for HC06 because there is command to response wanted message
Serial.write(myBT.getPinCode());
Serial.write(myBT.getName());
*/
Serial.write("read over");
} else {
Serial.write("Not in AT mode\n");
}
}
```
下面是一個簡單的範例，會將 Arduino 視窗的輸入字串透過藍芽傳輸出去。
```cpp
void loop() {
if (myBT.available() > 0) {
Serial.println(myBT.ReadString());
}
if (Serial.available() > 0) {
myBT.WriteString(Serial.readString().c_str());
}
delay(100);
}
```
**Matlab**
基本上的使用和UART差不多，只是我們要改成在matlab產生一個藍芽物件:
```
instrhwinfo('Bluetooth');
b = Bluetooth('[your device name]', 1);
fopen(b)
```
之後就像使用uart一樣，針對bluetooth物件進行資料存取或發送。
## 6. 相關函數庫
本次專題使用了一些第三方函式庫，有需要的同學可以透過以下連結到github下載。
* Timer相關:
* MsTimer2 [https://github.com/PaulStoffregen/MsTimer2](https://github.com/PaulStoffregen/MsTimer2)
* 三軸加速規相關:
* MPU6050 [https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050](https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050)
* I2Cdev [https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev](https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/I2Cdev)
由於MPU6050的通訊方式是透過I2C，因此會需要相關的函式庫。
* All Libraries:
* [https://github.com/evanyeh5118/DSCS_2019_spring_Lib](https://github.com/evanyeh5118/DSCS_2019_spring_Lib)
本次專題相關的函式庫都包含在裡面，包含助教群自己編寫的部份以及第三方函式庫。注意BalanbotEncoder, BalanbotMotor, BalanbotController 這三個函式庫僅有架構並未實作功能，同學們需參照講義前面的內容逐一編寫程式以完成本雙輪自行車專題。
Sign in with Wallet
Connect another wallet