# OpenCV 循跡車演算法

傳統做法使用紅外線感測器，利用不同材質反射的紅外線多寡判斷材質。但是這樣有以下缺點：

1. 雜訊影響大：在實際的測試中，需要距離地面約 1 cm 距離，才能有較精準的感應。若車子在運動過程中震動，該因素將難以消除。
2. 資訊有限：僅能取得數個資料點，必須針對少數幾個點建立控制計畫，局限性大。
3. 演算法設計難度高：在僅能知道數個點的資訊之下，實作演算法

另外，Arduino 雖然是能夠迅速上手的單片機，但是仍然缺乏一個現代作業系統有的各種優良特色：排程、多執行緒、平行運算等等。因此僅能做出非常有限的控制。

現在是二十一世紀，我們已經有非常發達的作業系統，摩爾定律也讓我們有重量與效能兼具的處理器。我們應該問問：在處理器上裝個作業系統，能不能為我們帶來什麼好處？

影像辨識：

1. 一般性的解決方案：不用調感測器敏感度調到升天，只需要關注在該用什麼樣的演算法從影像中抽取需要的資訊。

2. 功能擴展性佳：若需要擴充功能，僅需在演算法上面修改，配上多執行緒的程式技巧，使不同的執行緒負責不同功能，硬體方面並不需要做太多更動。舉例來說，若希望增加避障功能，僅需增加「分辨前景、後景」的演算即，即可偵測障礙物。這是紅外線感測器難以匹敵的。

3. 彈性高：若紅外線感應器需要更高解析度，則需要安裝更多硬體。然而，使用影像辨識的方式，僅需改變程式中的一個數字，便可做到兩倍紅外線感測器才能做到的事，且解析度更高，辨識更準確。

4. 價格低廉：以本次實驗中的攝影機為例，僅新台幣 70 元，價格接近 一顆 TCRT5000 紅外線模組。

本次氣動車計畫中，計劃使用 Raspberry Pi 搭配 Respbian 作業系統，作為影像處理的核心，並且使用 OpenCV 作為處理影像的函式庫，搭配 Arduino 作為 I/O slave

## 架構


## 影像處理


程式處理之流程如下：

1. 將圖片沿 y 方向，分割成 N 等分。
2. 對於每一張分割的圖片，尋找顏色最深的部分，並且找出包住該深色部分的輪廓，並尋找該輪廓的質心。
3. 對於每一張圖，找到該質心後，檢測所有線段質心圍成的凸包(convex hull)面積。若面積太大，表示該資料受到其他陰影干擾，予以捨棄。
4. 使用以下的方式計算伺服馬達轉動角度：
$$\phi = \left(\left(1 - \frac {|x|}{W/2}\right)\theta + \left(\frac {|x|}{W/2}\right)\frac {x}{W/2} \cdot \pi\right)$$
6. 將該角度存入 PID 控制器
7. 由 PID 控制器計算出轉動角度
8. 轉成伺服馬達對應的 PWM，並且輸出。

## 辨識黑線

攝影機取到的影像資料為 BGR 表示，若需要偵測黑線，則必須同時對藍、綠、紅等像素的數值大小進行限制，程式上較麻煩。因此，這裡把影像的表示，從 BGR 三色，轉換成「彩度（Hue） - 飽和度(Saturation)- 亮度(Value)」表示法，即 HSV 影像空間。各種顏色對應之 HSV 可以繪製出如下的柱狀圖：


(來源：https://zh.wikipedia.org/wiki/HSL和HSV色彩空间)

由該圖可以發現：若需要辨識深色，僅需尋找 Value 低的像素，而不需要針對單一像素的 3 個變數都進行限制。如此一來，便可將 BGR 需要設立的 3 個條件，轉為 HSV 空間中的單一條件：

for i in range(0, slice_num) :
    ...
    sliced_img = img[X_DIV*i + 1:X_DIV*(i+1), int(0):int(IMG_WIDTH)]
    imgHSV = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(imgHSV, LB, UB)
    ...

## 雜訊過濾

### Erosion & Dilation

Erosion 可以想像成用一個黑色的筆刷，沿著所有黑線邊緣刷過一遍。


白色部分看起像是被黑色部分「腐蝕」。

Dilation 則是相反的運算：


若進行 erosion，接著進行 dilation，則對於大面積的白色圖樣，其大小大致維持不變; 但對於較細小雜訊，該雜訊將會在 erision 的過程中被消除。將 Erosion 與 Dilation 接連使用的動作，稱作 Open 與 Close。

* Open : 先進行 Erision，再進行 Dilation


* Close : 先進行 Dilation，再進行 Erosion


在本次應用中，預計兩者都使用，以達到消除雜訊的功能。雖然在使用 HSV 過濾時，已經有夠好的效果。但考量日後可能在不同場地進行測驗，因此仍然加入該程式。

* Open


* Closed


* Processed


相關程式如下：

maskOpen = cv2.morphologyEx(mask,cv2.MORPH_OPEN,kernelOpen)
maskClose = cv2.morphologyEx(maskOpen,cv2.MORPH_CLOSE,kernelClose)
maskFinal = maskClose
_,conts,_ = cv2.findContours(maskFinal.copy(),\
            cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_NONE)

### 循跡線

能夠辨識出一個圖形中的深色部分之後，接著更進一步找出循跡線。我們看總和以上各個部分，設計一個尋找接近黑線形狀的 將螢幕沿 y 方向分割成數個部分，分別找出該分割中循跡線線段的質心，最後將所有點連接起來，可以大致得到線段中各個點。

def extract_polygon(img, slice_num=16, LB=np.array([0,0,0]), UB=np.array([180,255,75])):

    IMG_HEIGHT, IMG_WIDTH,_ = img.shape
    X_DIV = int(IMG_HEIGHT/float(slice_num))
    kernelOpen = np.ones((5,5))
    kernelClose = np.ones((20,20))
    poly_points = [None] * slice_num
    detected_contours = [None] * slice_num
    
    for i in range(0, slice_num) :
        sliced_img = img[X_DIV*i + 1:X_DIV*(i+1), int(0):int(IMG_WIDTH)]
        blur = cv2.GaussianBlur(sliced_img,(5,5),0)
        imgHSV = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(imgHSV, LB, UB)
        maskOpen = cv2.morphologyEx(mask,cv2.MORPH_OPEN,kernelOpen)
        maskClose=cv2.morphologyEx(maskOpen,cv2.MORPH_CLOSE,kernelClose)
        maskFinal = maskClose
        _,conts,_ = cv2.findContours(maskFinal.copy(),\
                    cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_NONE)

        if(conts):
            c = max(conts, key = cv2.contourArea)
            
            M = cv2.moments(c)
            poly_points[i] = (int(M['m10']/M['m00']), int(M['m01']/M['m00']) + X_DIV * i)
            detected_contours[i] = c

    contours = [i for i in detected_contours if i is not None]
    points = [i for i in poly_points if i is not None]

    return points, contours

以 `slice_num = 8` 為例，該函式得到結果示意如下：

* 原圖：


* 結果：


取得各點質心位置之後，可以進一步得到各種資訊：割線角度、切線角度，甚至可以由資料擬合出曲線。在本次應用中，我們僅使用曲線的質心位置，以及曲線的割線角度。

### 凸包

成功讀取曲線之後，接下來發現另外一個問題：因為是依照顏色深淺，因此
若影像邊緣有其他陰影，則可能誤判循跡線。舉例如下面的狀況：


在影像的邊緣出現其他東西（實際上是電腦椅的輪子）。`extract_polygon` 函式讀取直線的結果如下：


雖然在循跡時，未必會發生如圖片中極端的狀況，但在一些狀況中，如場地靠近窗戶時，窗櫺的陰影有可能會造成類似的影響：


雖然影響不若上述例子大，但仍會對循跡形成偏誤。因此在辨識過程當中，必須排除該狀況。針對這點，採取的作法如下：

1. 計算所有線段點形成之「凸包(convex hull)」。
2. 計算凸包圍出的多邊形面積。
3. 若該面積太大（這裡以「影像總面積的 1/50」作為基準），則捨棄該資料，沿用上一次循跡的參數。

在計算幾何中，一群點 $P$ 的凸包 $Q$ ，若且唯若滿足：

1. $Q$ 為 $P$ 的子集。
2. $Q$ 中的所有點，在平面上形成一凸多邊形 $CH(Q)$。
3. 對於任意 $p_i \in P$，$p_i$ 均在 $CH(Q)$ 內部，或是 $CH(Q)$ 的邊界。

示意圖如下：


在上圖中，$Q = {p_{10}, p_2, p_1, p_0, p_{12}}$ 形成之凸多邊形 $CH(Q)$ 為 $P$ 的凸包。

(來源：Introduction to Algorithms
Third Edition, THOMAS H. CORMEN, CHARLES E. LEISERSON)

判定凸包的用意在於：如果在攝影機視野範圍，內有黑線以外的深色部分（如：陰影）被偵測到，以致於產生誤判時，各點形成之凸包面積將會遠大於正確觀測到循跡線時，各點形成之凸包面積。如下圖：

* 無陰影干擾時：



* 有陰影干擾時：



兩相比較可以發現兩者面積差異極大，因此選擇以該作法作為抵抗干擾的方法。相關程式碼如下：

path = extract_polygon(img, resolution)
hull = cv2.convexHull(np.array(path))
area = cv2.contourArea(hull)
...
if IMG_HEIGHT * IMG_HEIGHT / 50.0 < area:
    ctrl = ctrl_last
...

## 誤差定義

在控制過程中，往往定義一個誤差(error)，目標為使誤差變成 0。在循跡的過程中，該誤差要如何定義？考慮以下幾種狀況：

1. 定義循跡線質心距離中央的 x 座標為誤差：該方法類似於紅外線感應器陣列。然而，在某些狀況下，該方法並不合適。考慮以下狀況：


黑線在影像中呈現對角線時，該線質心位置與原點距離並不太遠，因此並不會做出角度太大的轉向。但實際上，當黑線在影像中呈現該狀況時，車子正準備經歷一個大角度之轉彎。這是僅使用質心位置時，無法察覺的狀況。因此需要修正。

2. 定義黑線距垂直線的角度為誤差：既然能夠辨識角度，直接令伺服馬達依照黑線的角度轉，亦為一種直覺的策略。然而該作法缺點為：最多僅能令車子依照循跡線平行之軌跡運動，因此在轉彎處容易有「切西瓜」的狀況。另外，考慮以下狀況：


影像中，循跡線角度往左，因此若僅使用角度作為判斷依據，判斷結果為往左方轉彎。然而在該狀況下，往左轉將反而會使車身與循跡線偏移越來越多，最後完全看不見循跡線。因此該作法亦非最佳方法。

由上述討論可知：僅使用循跡線角度或質心位置規劃循跡演算法，都有其侷限性。

那麼，該如何定義誤差？試思考人的駕車習慣：

1. 若偏離道路中央太多，那麼會調整方向盤，使車子在道路中央。
2. 若車子在道路中央，但道路正在轉彎時，那麼會隨著道路方向進行轉向。
3. 如果在越遠的地方才開始出現彎道，那麼當下轉彎的修正量越少。

結合以上，我們可以知道：

1. 當循跡線在影像越邊緣時，「質心位置」的資訊越重要，角度相對較不重要。
2. 當循跡線質心大致在畫面中央時，「質心位置」資訊並不重要，但「角度」重要性大大提高。
3. 當質心 y 座標越大，轉彎的角度較少。

總和以上，我們認為：若直線的角度 $\theta$ 與直線的質心位置 $CM = [x, y]$，則誤差 $\phi = \phi(x, y, \theta)$，定義：

$$\begin{cases}
\phi_x = \frac {x}{w} \cdot \pi\\\\
\phi_\theta = \theta
\end{cases}$$

則：

$$\frac {\phi}{\pi} = e^{-ky}\left(W_\theta \phi_\theta + W_{x}\phi_x\right)$$

其中：

$$\begin{cases}W_\theta + W_x = 1\\ \\
W_x \to 0\ ,W_{\theta} \to 1\ if\ x \to 0\\ \\
W_x \to 1\ ,W_{\theta} \to 0\ if\ |x| \to \frac {W}{2}
\end{cases}$$

取：

$$
\begin{cases}
W_x = \frac {|x|}{W/2}\\ \\
W_\theta = 1 - \frac {|x|}{W/2}
\end{cases}
$$

則：

$$\phi = e^{-\frac {ky}{H/2}}\left(\left(1 - \frac {|x|}{W/2}\right)\theta + \left(\frac {|x|}{W/2}\right)\frac {x}{W/2} \cdot \pi\right)$$

其中$H$ 是影像長度，$W$ 是影像寬度，$k$ 是某一非負實數。該函數在 $x \to 0$ 時，$\phi \to \theta$，即在線靠近中央時，需要轉向的角度接近直線之角度; 而當 $|x| \to \frac {W}{2}$ 時，則越偏向以偏移量決定轉彎角度。該函數為程式中的 `evaluate_function()`：

def evaluate_function(angle_part, translate_part, x, y):
    x = abs(x)
    return math.exp(-1*y/IMG_HEIGHT/20.)*(
        (1 - 1.*x/IMG_WIDTH/2.0)*angle_part +
         1.*x/IMG_WIDTH/2 * translate_part)

以該函數作為 error 的指標，控制目標為使得 $\phi = 0$，並進行 PID 控制。

## PID 控制

1. P : evaluation function 的值
2. I : 使用 Queue 進行實作，取 2000 個資料點之加總值
3. D : 紀錄上一次迴圈的 $\phi$ 值，與這次的 $\phi$ 值相減

本次使用的 PID 控制器物件實作如下：

class PID_controller():
    def __init__(self, KPID, QUEUE_SIZE = 2000):
        self.PID = list([0., 0., 0.])
        self.KPID = list([1.*KPID[0], 1.*KPID[1]/QUEUE_SIZE, 1.*KPID[2]])
        self.integral_ghb = Queue.Queue(QUEUE_SIZE)
        self.last_tmp = 0
        self.ctrl = 0
        self.queue_size = QUEUE_SIZE
    ...

其中 `PID` 為 PID 各值的大小，P 值對應 `PID[0]`，I 值對應 `PID[1]`，D 值對應 `PID[2]`。、`KPID` 為 $K_P$, $K_I$, $K_D$ 的大小，其對應方式同 `PID` 振率。`integral_ghb` 為一個先進先出(first-in first-out) 的佇列(queue)，來方便數值積分計算。

該物件中 `PID_controller.step()` 成員函數為主要的計數方式：

class PID_controller():
    ...
    def step(self, cur_data):
        self.PID[0] = cur_data
        self.PID[2] = cur_data - self.last_tmp
        self.last_tmp = cur_data
        if not self.integral_ghb.full():
            self.integral_ghb.put(cur_data)
            self.PID[1] += cur_data
        else:
            self.PID[1] = self.PID[1] - self.integral_ghb.get() + cur_data
            self.integral_ghb.put(cur_data)
        self.ctrl =  sum(self.PID[i] * self.KPID[i] for i in range(3))/sum(self.KPID)
    ...

在 `integral_ghb` 未滿之前，資料會持續填入該佇列當中，並且`PID[1]` 不斷加上新紀錄的值。直到 `integral_ghb` 滿了後，每次有新資料進入時，PID[1] 將減去 `integral_ghb` 內部最老的那筆數據，並且加上最新的數據，藉此可以再 $O(1)$ 的時間內得到新的積分數據，而不用每次都將 `integral_ghb` 內部的數據從頭加總。

# 電路


由 Raspberry Pi 經由 74HCT244 緩衝閘，及輸出 PWM 訊號控制馬達。

註：實際使用的電池為 3 顆 18650 串連

## 電源供給

串連 3 顆 18650 來達成 11.1 V (實際上範圍約 10.6 ~ 12.5V)的電壓，並且並聯一個變壓模組，輸出 5.5V 電壓，供給各 IC 與伺服馬達電力。

最初考量鋰聚電池(Li-Po)，如同過去多數學長姐的作法。但該電池較昂貴，而且需要另外添購充電裝置，使用上較不方便。經過思考過後，決定使用 18650 鋰離子電池串連。除了電池更換方便之外，市面上有許多電芯採用 18650 電池的行動電源，因此僅需將既有的行動電源稍微改裝，便可得到一個現成的 18650 充電器。亦可將既有的行動電源插入 18650 電池後，作為一個 5V 供電，供給 Raspberry Pi 。此外，在專題結束後，暫時不用的 18650 電池，也可以直接改裝成手機的行動電源。可以說是一個經濟的選擇。


## 邏輯準位轉換

Raspberry Pi 和 Arduino 類似，有眾多輸入、輸出腳位。然而，為了降低能耗，其 GPIO (General Purpose Input/Output) 之電路，以 3.3V 作為標準，而非 Arduino 使用的 5V 作為。這意味著：若使用專為 Arduino 設計的馬達與模組，除了單純連接電路之外，還必須處理邏輯準位的不同。該問題有以下幾種解法：

1. Logic Level Shifter：將 GPIO 輸出之電壓，施加在 MOSFET 的閘極，而源極與汲極連接至 5V。當 GPIO 輸出產生高低變化時，將控制 MOSFET 的源極與汲極導通與切斷，進而形成 5V 的 PWM。

由於直接將 GPIO 腳位連接未經保護的電路，容易因使用不當使板子毀壞，因此我們並未使用該方法。

2. 使用緩衝閘(Buffer Gate)：緩衝閘(Buffer Gate) 為邏輯電路中的一種邏輯閘，其為兩個 NOT 閘串接而成，因此並不會對原邏輯電路中的真值進行任何運算，其目的為「將輸入訊號復述一遍」，以增強在邏輯閘之間傳遞的訊號強度。


假定有一個以 5V 為邏輯電路準位的緩衝閘，在輸入 Raspberry Pi 的 3.3V PWM 時，會自動將其高電位的 3.3V duty cycle 部分視為 HIGH，並且以 5V 邏輯的高電位輸出，即可得到 5V 的 PWM。

3. 主從式架構（Master-Slave Archetecture）：該作法使用 Raspberry Pi 另外控制一片處理器(如 Arduino)，由該處理器全權負責與周邊裝置的 I/O，兩者之間藉由 USB 或 UART 等機制溝通（在本次應用中使用 USB）。

第 3 種方法聽起來雖然疊床架屋，但是最能保護 Raspbery Pi 板子的方法，因為 I/O 當中的意外（如短路）均由負責 I/O 的處理器承擔，相較於直接燒毀 GPIO 腳位保險得多。另外，在軟體的協作方面亦有好處：較擅長 C++ 及 Arduino 程式的人，可以在 Arduino 上開發程式，最後再撰寫交換兩者資訊的程式即可。

然而，該方法面臨透過 USB 傳輸時的同步問題，即當 Arduino 端傳輸資料時，Raspberry Pi 端未必處在等待接收資料的狀態; 反之，當 Raspberry Pi 傳送控制伺服馬達的程式給 Arduino 時，Arduino 也未必接收到。因此，對於需要較快反應的伺服馬達，該作法並不適合。因此在期末測試時，決定刪除該去除 Arduino，改將所有訊號經由 74HCT244 進行 Interfacing 後，進行輸出。

另外，相較於僅需輸輸出 PWM 的伺服馬達，聲納模組同時需要同時進行輸入與輸出。因此聲納有一個獨立的雙向邏輯準位轉換 IC 。

# 物件化的程式設計

期中測試時，給乎所有程式都放在同一個檔案中。而期末測試時進一步利用物件導向的功能，將車子、PID 控制器等功能，利用物件導向的程式哲學，進一步封裝成各個物件。除了使程式維護方便外，在撰寫遙控程式時，直接使用封裝好的程式物件進行相應的動作即可。

(`ARSCHLOCH.py`)

import time
import pigpio 
import sonar_ranger
import numpy as np

class ARSCHLOCH:
    def __init__(self):
        self.STEER = 19
        self.FAN_ANG = 13
        self.ESC = 6
        self.SONAR_ECHO = 17
        self.SONAR_TRIG = 27
        self.S_MID = 16
        self.S_AMP = 11
        self.MAX_DUTY = self.S_MID + self.S_AMP
        self.MIN_DUTY = self.S_MID - self.S_AMP
        self.ESC_MINDUTY = 10
        self.ESC_MAXDUTY = 40
        self.SERVO_PIN = [self.STEER, self.FAN_ANG]
        self.ESC_PIN = [self.ESC]
        self.SENSOR_PIN_OUT = []
        self.SENSOR_PIN_IN = []
        self.PWM_TEST_SEQ = [self.S_MID, self.S_MID + self.S_AMP, self.S_MID, self.S_MID - self.S_AMP]
        self.pi = pigpio.pi()
        self.ranger = sonar_ranger.ranger(self.pi, self.SONAR_TRIG, self.SONAR_ECHO)

    def turn_on(self):
        for i in self.SERVO_PIN + self.SENSOR_PIN_OUT:
            self.pi.set_mode(i, pigpio.OUTPUT)
            self.pi.set_PWM_frequency(i, 50)
            self.pi.set_PWM_dutycycle(i, self.S_MID)
        for i in self.ESC_PIN:
            self.pi.set_mode(i, pigpio.OUTPUT)
            self.pi.set_PWM_frequency(i, 50)
            self.pi.set_PWM_dutycycle(i, 0)
        for i in self.SENSOR_PIN_IN:
            self.pi.set_mode(i, pigpio.INPUT)
            self.pi.set_PWM_frequency(i, 50)

    def turn_off(self):
        print("exit routing start...")
        print("Halt BLDC...")
        for i in self.ESC_PIN:
            self.pi.set_PWM_dutycycle(i, 0)
        print("Halt Servos...")
        for i in self.SERVO_PIN:
            self.pi.set_PWM_dutycycle(i, self.S_MID)
        time.sleep(1)
        for i in self.SERVO_PIN:
            self.pi.set_PWM_dutycycle(i, 0)
        print("Stop sonar ranger ...")
        self.ranger.cancel()
        print("Clean Sensor pins...")
        for i in self.SENSOR_PIN_IN:
            self.pi.set_PWM_dutycycle(i, 0)
        self.pi.stop()


    def set_fan(self, theta):
        if theta < 0:
            theta = 0
        elif theta > 180:
            theta = 180
        duty = np.interp(theta, [0, 180], [self.MAX_DUTY, self.MIN_DUTY]) 
        self.pi.set_PWM_dutycycle(self.FAN_ANG, int(duty))



    def accelerate(self, speed = 1):
        if speed < 0:
            duty = self.ESC_MINDUTY
        elif speed > self.ESC_MAXDUTY - self.ESC_MINDUTY:
            duty = self.ESC_MAXDUTY
        else:
            duty = speed + self.ESC_MINDUTY
        self.pi.set_PWM_dutycycle(self.ESC, duty)

    def steer(self, theta):
        if theta < 0:
            theta = 0
        elif theta > 180:
            theta = 180
        theta = (np.interp(theta, [0, 180], [35, 145]))
        duty = np.interp(theta, [0, 180], [self.MAX_DUTY, self.MIN_DUTY])
        self.pi.set_PWM_dutycycle(self.STEER, int(duty))

    def get_distance(self):
        return self.ranger.read()

    def test_servo(self):
        for i in self.PWM_TEST_SEQ:
            for j in self.SERVO_PIN:
                self.pi.set_PWM_dutycycle(j, i)
            time.sleep(1)

    def callibrate_ESC(self):
        print("calibration high ...")
        self.pi.set_PWM_dutycycle(self.ESC, self.ESC_MAXDUTY)
        time.sleep(4)
        print("calibration low ...")
        self.pi.set_PWM_dutycycle(self.ESC, self.ESC_MINDUTY)
        time.sleep(2)
        self.pi.set_PWM_dutycycle(self.ESC, self.ESC_MINDUTY)
        print("Finish calibration !")

# 例外處理

如果使用 Arduino 控制，常常需要手動按下微處理器的重設扭，替程式進行重設來重新開始。一但程式行為不合預期，就必需徒手接住氣動車並強行拔掉電源使其停止。該作法雖然堪用，但過程中危險性較高。因此，若希望開發過程安全，則需要有結束程序的方法。另外，程序結束後，仍需要做各類硬體資源的清理（如：替伺服馬達轉中心位置、命令無刷馬達停止運轉），否則將有程式已經結束，但訊號仍在持續輸出的狀況。

因為使用更接近現代作業系統，因此可以善用作業系統的特徵，利用 `atexit()` 函數，註冊程序結束時的清理步驟：

    ...
    from ARSCHLOCH import *
    arschloch = ARSCHLOCH()
    arschloch.turn_on()
    arschloch.set_fan(170)
    arschloch.callibrate_ESC()
    atexit.register(arschloch.turn_off)
    ...

有了這個設置，加上使用遠端連線，僅需要在電腦端中止程，就能將車子狀態回復至開始前，不需要在車子仍在運動的狀態下追車子。

# 附錄

## 主程式 (`color_block.py`)

mport cv2
import numpy as np
import math
import time
import atexit
from pid import *
from geometry import *
from equalization import *

ON_RPI = 1
CAMERA_NO = -1
IMG_WIDTH = 640
IMG_HEIGHT = 480
NORMAL_SPEED = 3
THRUST_SPEED = 7
cam = cv2.VideoCapture(CAMERA_NO)
controller = PID_controller([1000, 16, 200])
ctrl_last = 0
ctrl = 0
no_flip = 0 
dist = 100000

if ON_RPI:
    from ARSCHLOCH import *
    arschloch = ARSCHLOCH()
    arschloch.turn_on()
    arschloch.set_fan(170)
    arschloch.callibrate_ESC()
    atexit.register(arschloch.turn_off)
    arschloch.accelerate(THRUST_SPEED)
    time.sleep(0.7)
    arschloch.accelerate(NORMAL_SPEED)

def evaluate_function(angle_part, translate_part, x, y):
    x = abs(x)
    return math.exp(-0*y/IMG_HEIGHT/20.)*(
        (1 - 1.*x/IMG_WIDTH/2.0)*angle_part + 
         1.*x/IMG_WIDTH/2.0 * translate_part)


while True:
    resolution = 16
    #dist = arschloch.get_distance()
    _, img = cam.read()
    img = cv2.resize(img,(IMG_WIDTH,IMG_HEIGHT))
    img = img[int(0.7*IMG_HEIGHT):IMG_HEIGHT, int(0 * IMG_WIDTH):int(1 * IMG_WIDTH)]
    path, poly = extract_polygon(img, resolution)

    if not len(path) == 0:
        n = len(path)
        hull = cv2.convexHull(np.array(path))
        area = cv2.contourArea(hull)
        AREA = IMG_HEIGHT * IMG_WIDTH
        print(area/1./AREA)
        if AREA / 80 < area:
            ctrl = ctrl_last
            if __debug__:
                print("Noise Detected ! ! !")
                cv2.drawContours(img,[hull],0,(0,0,255),-1)            
        else:
            curve_cm = [0, 0]
            for i in range(0, len(path)):
                curve_cm[0] += 1.*path[i][0]/len(path)
                curve_cm[1] += 1.*path[i][1]/len(path)

            vec_sec = [path[0][0]-path[n-1][0], path[0][1]-path[n-1][1]]
            ang_sec = 90 - np.angle(vec_sec[0] - vec_sec[1] * 1J, deg=True)

            translate_part = 90 - np.interp(curve_cm[0], [0, IMG_WIDTH], [180, 0])
            angle_part = ang_sec

            ctrl_estimate = evaluate_function(angle_part, translate_part, \
                            curve_cm[0] - IMG_WIDTH/2.0, IMG_HEIGHT - curve_cm[1])

            controller.step(ctrl_estimate)
            ctrl = controller.get_ctrl()

            if abs(ctrl -  ctrl_last) < 2:
                no_flip  += 1
            else:
                no_flip = 0
            accelerate = no_flip / 30

            ctrl_last = ctrl

            if __debug__:
                print("Translate Part : " + str(translate_part))
                print("Angle Part : " + str(angle_part))
                print("flip no : " + str(no_flip))
                print("ctrl : " + str(ctrl))
                #print("dist : " + str(dist))
                cv2.drawContours(img,[hull],0,(255,0,0),-1)

        if __debug__:
            for i in range(len(path) - 1):
                cv2.line(img, path[i] ,path[i + 1],(0, 255, 0),5)
            for i in range(len(poly) - 1):
                cv2.drawContours(img,poly[i],0,(255,0,0),1)            
            cv2.imshow("cam",img)
            cv2.waitKey(10)

        if ON_RPI:
            arschloch.steer(90 - ctrl)

## 影像處理程式 (`geometry.py`)

import cv2
import math
import numpy as np

def extract_polygon(img, slice_num=16, LB=np.array([0,0,0]), UB=np.array([180,255,75])):

    IMG_HEIGHT, IMG_WIDTH,_ = img.shape
    X_DIV = int(IMG_HEIGHT/float(slice_num))
    kernelOpen = np.ones((5,5))
    kernelClose = np.ones((20,20))
    poly_points = [None] * slice_num
    detected_contours = [None] * slice_num
    
    for i in range(0, slice_num) :
        sliced_img = img[X_DIV*i + 1:X_DIV*(i+1), int(0):int(IMG_WIDTH)]
        blur = cv2.GaussianBlur(sliced_img,(5,5),0)
        imgHSV = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(imgHSV, LB, UB)
        maskOpen = cv2.morphologyEx(mask,cv2.MORPH_OPEN,kernelOpen)
        maskClose=cv2.morphologyEx(maskOpen,cv2.MORPH_CLOSE,kernelClose)
        maskFinal = maskClose
        _,conts,_ = cv2.findContours(maskFinal.copy(),\
                    cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_NONE)

        if(conts):
            c = max(conts, key = cv2.contourArea)
            
            M = cv2.moments(c)
            poly_points[i] = (int(M['m10']/M['m00']), int(M['m01']/M['m00']) + X_DIV * i)
            detected_contours[i] = c

    contours = [i for i in detected_contours if i is not None]
    points = [i for i in poly_points if i is not None]

    return points, contours

## 循跡車物件 (`ARSCHLOCH.py`)

import time
import pigpio 
import sonar_ranger
import numpy as np

class ARSCHLOCH:
    def __init__(self):
        self.STEER = 19
        self.FAN_ANG = 13
        self.ESC = 6
        self.SONAR_ECHO = 17
        self.SONAR_TRIG = 27
        self.S_MID = 16
        self.S_AMP = 11
        self.MAX_DUTY = self.S_MID + self.S_AMP
        self.MIN_DUTY = self.S_MID - self.S_AMP
        self.ESC_MINDUTY = 10
        self.ESC_MAXDUTY = 40
        self.SERVO_PIN = [self.STEER, self.FAN_ANG]
        self.ESC_PIN = [self.ESC]
        self.SENSOR_PIN_OUT = []
        self.SENSOR_PIN_IN = []
        self.PWM_TEST_SEQ = [self.S_MID, self.S_MID + self.S_AMP, self.S_MID, self.S_MID - self.S_AMP]
        self.pi = pigpio.pi()
        self.ranger = sonar_ranger.ranger(self.pi, self.SONAR_TRIG, self.SONAR_ECHO)

    def turn_on(self):
        for i in self.SERVO_PIN + self.SENSOR_PIN_OUT:
            self.pi.set_mode(i, pigpio.OUTPUT)
            self.pi.set_PWM_frequency(i, 50)
            self.pi.set_PWM_dutycycle(i, self.S_MID)
        for i in self.ESC_PIN:
            self.pi.set_mode(i, pigpio.OUTPUT)
            self.pi.set_PWM_frequency(i, 50)
            self.pi.set_PWM_dutycycle(i, 0)
        for i in self.SENSOR_PIN_IN:
            self.pi.set_mode(i, pigpio.INPUT)
            self.pi.set_PWM_frequency(i, 50)

    def turn_off(self):
        print("exit routing start...")
        print("Halt BLDC...")
        for i in self.ESC_PIN:
            self.pi.set_PWM_dutycycle(i, 0)
        print("Halt Servos...")
        for i in self.SERVO_PIN:
            self.pi.set_PWM_dutycycle(i, self.S_MID)
        time.sleep(1)
        for i in self.SERVO_PIN:
            self.pi.set_PWM_dutycycle(i, 0)
        print("Stop sonar ranger ...")
        self.ranger.cancel()
        print("Clean Sensor pins...")
        for i in self.SENSOR_PIN_IN:
            self.pi.set_PWM_dutycycle(i, 0)
        self.pi.stop()


    def set_fan(self, theta):
        if theta < 0:
            theta = 0
        elif theta > 180:
            theta = 180
        duty = np.interp(theta, [0, 180], [self.MAX_DUTY, self.MIN_DUTY]) 
        self.pi.set_PWM_dutycycle(self.FAN_ANG, int(duty))



    def accelerate(self, speed = 1):
        if speed < 0:
            duty = self.ESC_MINDUTY
        elif speed > self.ESC_MAXDUTY - self.ESC_MINDUTY:
            duty = self.ESC_MAXDUTY
        else:
            duty = speed + self.ESC_MINDUTY
        self.pi.set_PWM_dutycycle(self.ESC, duty)

    def steer(self, theta):
        if theta < 0:
            theta = 0
        elif theta > 180:
            theta = 180
        theta = (np.interp(theta, [0, 180], [35, 145]))
        duty = np.interp(theta, [0, 180], [self.MAX_DUTY, self.MIN_DUTY])
        self.pi.set_PWM_dutycycle(self.STEER, int(duty))

    def get_distance(self):
        return self.ranger.read()

    def test_servo(self):
        for i in self.PWM_TEST_SEQ:
            for j in self.SERVO_PIN:
                self.pi.set_PWM_dutycycle(j, i)
            time.sleep(1)

    def callibrate_ESC(self):
        print("calibration high ...")
        self.pi.set_PWM_dutycycle(self.ESC, self.ESC_MAXDUTY)
        time.sleep(4)
        print("calibration low ...")
        self.pi.set_PWM_dutycycle(self.ESC, self.ESC_MINDUTY)
        time.sleep(2)
        self.pi.set_PWM_dutycycle(self.ESC, self.ESC_MINDUTY)
        print("Finish calibration !")

if __name__ == "__main__":
    import atexit
    arschloch = ARSCHLOCH()
    arschloch.turn_on()
    atexit.register(arschloch.turn_off)
    # test program goes here.

##