EV3 直接命令 - 第 5 課 從 EV3 的感測器讀取資料
摘要：
我們從 EV3 裝置的一些自反映開始並詢問它：
埠 16 上連線了什麼型別的裝置？
埠 16 上的感測器的模式？請給你的 EV3 傳送如下的直接命令：----------------------------------------...
我們從 EV3 裝置的一些自反映開始並詢問它：
- 埠 16 上連線了什麼型別的裝置？
-
埠 16 上的感測器的模式？請給你的 EV3 傳送如下的直接命令：
---------------------------------------- \len \cnt \ty\hd\op\cd\la\no\ty\mo\ ---------------------------------------- 0x|0B:00|2A:00|00|02:00|99|05|00|10|60|61| ---------------------------------------- \11\42\re\0,2 \I \G \0 \16\0 \1 \ \\\ \\n \E \ \ \ \ \ \\\ \\p \T \ \ \ \ \ \\\ \\u \_ \ \ \ \ \ \\\ \\t \T \ \ \ \ \ \\\ \\_ \Y \ \ \ \ \ \\\ \\D \P \ \ \ \ \ \\\ \\e \E \ \ \ \ \ \\\ \\v \M \ \ \ \ \ \\\ \\i \O \ \ \ \ \ \\\ \\c \D \ \ \ \ \ \\\ \\e \E \ \ \ \ \ ----------------------------------------
一些說明：
-
我們以 CMD
GET_TYPEMODE使用操作opInput_device - 如果被用作感測器埠，電機埠 A 的編號為 16。
- 我們將獲得兩個數作為應答，型別和模式。
- 型別需要一個位元組且將佔據位元組 0 的位置，模式也需要一個位元組且被放在位元組 1 的位置。
我獲得瞭如下的回答：
---------------------- \len \cnt \rs\ty\mo\ ---------------------- 0x|05:00|2A:00|02|07|00| ---------------------- \5\42\ok\7 \0 \ ----------------------
它是說：電機埠 A 處的感測器具有型別 7 且實際處於模式 0 中。如果你看一下文件EV3 Firmware Developer Kit 的第 5 章，其標題為Device type list ，你發現型別 7 和模式 0 代表EV3-Large-Motor-Degree 。ofollow,noindex">### EV3 Firmware Developer Kit 。
讀取電機的實際位置
我們來到一個非常有趣的問題：電機埠 A 的電機實際位置是多少？我傳送了這個命令：
---------------------------------------------- \len \cnt \ty\hd\op\cd\la\no\ty\mo\va\v1\ ---------------------------------------------- 0x|0D:00|2A:00|00|04:00|99|1C|00|10|07|00|01|60| ---------------------------------------------- \13\42\re\0,4 \I \R \0 \16\E \D \1 \0 \ \\\ \\n \E \ \ \V \e \ \ \ \\\ \\p \A \ \ \3 \g \ \ \ \\\ \\u \D \ \ \- \r \ \ \ \\\ \\t \Y \ \ \L \e \ \ \ \\\ \\_ \_ \ \ \a \e \ \ \ \\\ \\D \R \ \ \r \ \ \ \ \\\ \\e \A \ \ \g \ \ \ \ \\\ \\v \W \ \ \e \ \ \ \ \\\ \\i \ \ \ \- \ \ \ \ \\\ \\c \ \ \ \M \ \ \ \ \\\ \\e \ \ \ \o \ \ \ \ \\\ \\ \ \ \ \t \ \ \ \ \\\ \\ \ \ \ \o \ \ \ \ \\\ \\ \ \ \ \r \ \ \ \ ----------------------------------------------
我得到了答覆：
---------------------------- \len \cnt \rs\degrees\ ---------------------------- 0x|07:00|2A:00|02|00:00:00:00| ---------------------------- \7\42\ok\0\ ----------------------------
然後我用手移動電機並再次傳送相同的直接命令。這次回覆是：
---------------------------- \len \cnt \rs\degrees\ ---------------------------- 0x|07:00|2A:00|02|50:07:00:00| ---------------------------- \7\42\ok\1872\ ----------------------------
那是說：電機移動了 1, 872 度（5.2 周）。這似乎是對的！
技術細節
是時候看一下幕後的東西了！你需要理解：
- 埠編號的系統，
- 我們使用的操作的引數，和
- 如何定位並解包全域性記憶體。
埠編號的系統
感測器有四個埠，電機有四個埠。感測器埠的編號是 1 到 4：
- 埠 1: PORT = 0x|00| 或 LCX(0)
- 埠 2: PORT = 0x|01| 或 LCX(1)
- 埠 3: PORT = 0x|02| 或 LCX(2)
- 埠 4: PORT = 0x|03| 或 LCX(3)
這似乎有點滑稽，但計算機通常從數字 0 開始計數，人類從數字 1 開始計數。我們剛剛瞭解到，電機也是感測器，我們可以從中讀取電機的實際位置。電機埠標為字母 A 到 D，但通過如下方式定位：
- 埠 A: PORT = 0x|10| 或 LCX(16)
- 埠 B: PORT = 0x|11| 或 LCX(17)
- 埠 C: PORT = 0x|12| 或 LCX(18)
- 埠 D: PORT = 0x|13| 或 LCX(19)
我在我的模組 ev3.py 中添加了一個小函式：
def port_motor_input(port_output: int)-> bytes:
"""
get corresponding input motor port (from output motor port)
"""
if port_output == PORT_A:
return LCX(16)
elif port_output == PORT_B:
return LCX(17)
elif port_output == PORT_C:
return LCX(18)
elif port_output == PORT_D:
return LCX(19)
else:
raise ValueError("port_output needs to be one of the port numbers [1, 2, 4, 8]")
從電機輸出埠轉換到輸入埠。
操作 opInput_Device
opInput_Device 的兩個變體的簡短描述，我們已經使用了：
-
opInput_Device = 0x|99| 的 CMD GET_TYPEMODE = 0x|05|：
引數
- (Data8) LAYER：鏈 layer 號
- (Data8) NO：埠編號
返回值
- (Data8) TYPE：裝置型別 - (Data8) MODE：裝置模式
-
opInput_Device = 0x|99| 的 CMD READY_RAW = 0x|1C|：
引數
- (Data8) LAYER：鏈 layer 號
- (Data8) NO：埠編號
- (Data8) TYPE：裝置型別
- (Data8) MODE：裝置模式
-
(Data8) VALUES：返回值的個數
返回值 - (Data32) VALUE1：以特定模式從感測器接收的第一個值
這裡 Data32 是說這是一個 32 位有符號整數。 返回的資料是值，但請記住，返回引數如 VALUE1 是引用。引用是區域性或全域性記憶體的地址。閱讀下一部分了解詳情。
定址全域性記憶體
在第 2 課中，我們介紹了常量引數和區域性變數。你將記得，我們已經看到了 LCS，LC0，LC1，LC2，LC4，LV0，LV1，LV2 和 LV4，並寫了三個函式 LCX(value:int)，LVX(value:int) 和 LCS(value:str)：
FUNCTIONS LCS(value:str) -> bytes pack a string into a LCS LCX(value:int) -> bytes create a LC0, LC1, LC2, LC4, dependent from the value LVX(value:int) -> bytes create a LV0, LV1, LV2, LV4, dependent from the value
我們討論了標識位元組，它定義了變數的型別和長度：
現在我們編寫另一個函式 GVX，它返回全域性記憶體的地址。如你已經知道的那樣，標識位元組的位 0 代表短格式或長格式：
0b 0... .... 0b 1... ....
如果位 1 和 2 是0b .11. ....
，它們代表全域性變數，它們是全域性記憶體的地址。
位 6 和 7 代表後續的值的長度
0b .... ..00 0b .... ..01 0b .... ..10 0b .... ..11
現在我們寫 4 個全域性變數作為二進位制掩碼，我們不需要符號，因為地址總是正數。 V 代表地址（值）的一位。
-
GV0：0b 011V VVVV，5 位地址，範圍：0 - 31，長度：1 位元組，由前導位 011 標識。 -
GV1：0b 1110 0001 VVVV VVVV，8 位地址，範圍：0 - 255，長度：2 位元組，由前導位元組0x|E1|標識。 -
GV2：0b 1110 0010 VVVV VVVV VVVV VVVV，16 位地址，範圍：0 – 65.536，長度：3 位元組，由前導位元組0x|E2|標識。 -
GV4：0b 1110 0011 VVVV VVVV VVVV VVVV VVVV VVVV VVVV VVVV，32 位地址，範圍：0 – 4,294,967,296，長度：5 位元組，由前導位元組0x|E3|標識。
一些說明：
struct.unpack
一個新模組函式：GVX
請給你的 ev3 模組新增一個函式GVX(value)
，依賴於值，它返回 GV0，GV1，GV2或 GV4 中最短的型別。我已經完成了，現在我的模組 ev3 的文件如下：
FUNCTIONS GVX(value:int) -> bytes create a GV0, GV1, GV2, GV4, dependent from the value LCS(value:str) -> bytes pack a string into a LCS LCX(value:int) -> bytes create a LC0, LC1, LC2, LC4, dependent from the value LVX(value:int) -> bytes create a LV0, LV1, LV2, LV4, dependent from the value port_motor_input(port_output:int) -> bytes get corresponding input motor port (from output motor port)
解包全域性記憶體
我已經提到，已經有了解包全域性記憶體的好工具了。在 Python 3 中，這個工具是struct — Interpret bytes as packed binary data 。
一位元組無符號整數
我的從電機埠 A 讀取模式和型別的程式：
#!/usr/bin/env python3
import ev3, struct
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
my_ev3.verbosity = 1
ops = b''.join([
ev3.opInput_Device,
ev3.GET_TYPEMODE,
ev3.LCX(0),# LAYER
ev3.port_motor_input(PORT_A), # NO
ev3.GVX(0),# TYPE
ev3.GVX(1)# MODE
])
reply = my_ev3.send_direct_cmd(ops, global_mem=2)
(type, mode) = struct.unpack('BB', reply[5:])
print("type: {}, mode: {}".format(type, mode))
模式 ‘BB’ 把全域性記憶體分為兩個 1 位元組的無符號整數值。這個程式的輸出是：
08:08:13.477998 Sent 0x|0B:00|2A:00|00|02:00|99:05:00:10:60:61| 08:08:13.558793 Recv 0x|05:00|2A:00|02|07:00| type: 7, mode: 0
四個位元組的浮點數和四個位元組的有符號整數
我的讀取埠 A 和埠 D 上的電機的電機位置的程式：
#!/usr/bin/env python3
import ev3, struct
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
my_ev3.verbosity = 1
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0),# LAYER
ev3.port_motor_input(PORT_A), # NO
ev3.LCX(7),# TYPE
ev3.LCX(0),# MODE
ev3.LCX(1),# VALUES
ev3.GVX(0),# VALUE1
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0),# LAYER
ev3.port_motor_input(PORT_D), # NO
ev3.LCX(7),# TYPE
ev3.LCX(0),# MODE
ev3.LCX(1),# VALUES
ev3.GVX(4)# VALUE1
])
reply = my_ev3.send_direct_cmd(ops, global_mem=8)
(pos_a, pos_d) = struct.unpack('<fi', reply[5:])
print("positions in degrees (ports A and D): {} and {}".format(pos_a, pos_d))
格式 ‘<fi’ 將全域性記憶體分為一個 4 位元組的浮點數和一個 4 位元組的有符號整數，都是小尾端的。輸出是：
08:32:32.865522 Sent 0x|15:00|2A:00|00|08:00|99:1D:00:10:07:00:01:60:99:1C:00:13:07:00:01:64| 08:32:32.949266 Recv 0x|0B:00|2A:00|02|00:80:6C:C4:54:04:00:00| positions in degrees (ports A and D): -946.0 and 1108
字串
我們讀取 EV3 裝置的名字：
#!/usr/bin/env python3
import ev3, struct
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
my_ev3.verbosity = 1
ops = b''.join([
ev3.opCom_Get,
ev3.GET_BRICKNAME,
ev3.LCX(16),# LENGTH
ev3.GVX(0)# NAME
])
reply = my_ev3.send_direct_cmd(ops, global_mem=16)
(brickname,) = struct.unpack('16s', reply[5:])
brickname = brickname.split(b'\x00')[0]
brickname = brickname.decode("ascii")
print("Brickname:", brickname)
說明：
- 格式 ‘16s’ 描述了一個 16 位元組的字串。
- brickname = brickname.split(b’\x00’)[0] 佔據了以 0 結尾的字串的第一部分。你需要那樣做是因為 EV3 裝置不清除全域性記憶體。在字串的右端部分也許有一些垃圾。等一會兒，然後我將演示這個問題。
- brickname = brickname.decode(“ascii”) 從位元組型別建立一個字串型別。
這個程式的輸出是：
08:55:00.098825 Sent 0x|0A:00|2B:00|00|10:00|D3:0D:81:20:60| 08:55:00.138258 Recv 0x|13:00|2B:00|02|6D:79:45:56:33:00:00:00:00:00:00:00:00:00:00:00| Brickname: myEV3
帶有垃圾的字串
我們傳送兩個直接命令，第二個讀取一個字串：
#!/usr/bin/env python3
import ev3, struct
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
my_ev3.verbosity = 1
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0),# LAYER
ev3.port_motor_input(PORT_A), # NO
ev3.LCX(7),# TYPE
ev3.LCX(0),# MODE
ev3.LCX(1),# VALUES
ev3.GVX(0),# VALUE1
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0),# LAYER
ev3.port_motor_input(PORT_D), # NO
ev3.LCX(7),# TYPE
ev3.LCX(0),# MODE
ev3.LCX(1),# VALUES
ev3.GVX(4)# VALUE1
])
reply = my_ev3.send_direct_cmd(ops, global_mem=8)
(pos_a, pos_d) = struct.unpack('<fi', reply[5:])
print("positions in degrees (ports A and D): {} and {}".format(pos_a, pos_d))
ops = b''.join([
ev3.opCom_Get,
ev3.GET_BRICKNAME,
ev3.LCX(16),# LENGTH
ev3.GVX(0)# NAME
])
reply = my_ev3.send_direct_cmd(ops, global_mem=16)
這個程式的輸出是：
09:13:30.379771 Sent 0x|15:00|2A:00|00|08:00|99:1D:00:10:07:00:01:60:99:1C:00:13:07:00:01:64| 09:13:30.433495 Recv 0x|0B:00|2A:00|02|00:08:90:C5:FE:F0:FF:FF| positions in degrees (ports A and D): -4609.0 and -3842 09:13:30.433932 Sent 0x|0A:00|2B:00|00|10:00|D3:0D:81:20:60| 09:13:30.502499 Recv 0x|13:00|2B:00|02|6D:79:45:56:33:00:FF:FF:00:00:00:00:00:00:00:00|
以 0 結尾的字串 ‘myEV3’ (0x|6D:79:45:56:33:00|) 的長度為 6 個位元組。接下來的兩個位元組 (0x|FF:FF|) 是來自於第一個直接命令的垃圾。
最快的拇指
觸屏感測器的型別編號為 16，且有兩個模式，0: EV3-Touch 和 1: EV3-Bump。第一個測試，如果感測器實際被觸摸了，第二個從上次清除感測器開始計算觸控。我們通過一個小程式演示這些模式。它計數，摸感測器在五秒鐘內撞擊的頻率（請在埠 2 插入你的觸控感測器）：
#!/usr/bin/env python3
import ev3, struct, time
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
def change_color(color)-> bytes:
return b''.join([
ev3.opUI_Write,
ev3.LED,
color
])
def play_sound(vol: int, freq: int, dur:int)-> bytes:
return b''.join([
ev3.opSound,
ev3.TONE,
ev3.LCX(vol),
ev3.LCX(freq),
ev3.LCX(dur)
])
def ready()->None:
ops = change_color(ev3.LED_RED)
my_ev3.send_direct_cmd(ops)
time.sleep(3)
def steady()->None:
ops_color = change_color(ev3.LED_ORANGE)
ops_sound = play_sound(1, 200, 60)
my_ev3.send_direct_cmd(ops_color + ops_sound)
time.sleep(0.25)
for i in range(3):
my_ev3.send_direct_cmd(ops_sound)
time.sleep(0.25)
def go()->None:
ops_clear = b''.join([
ev3.opInput_Device,
ev3.CLR_CHANGES,
ev3.LCX(0),# LAYER
ev3.LCX(1)# NO
])
ops_color = change_color(ev3.LED_GREEN_FLASH)
ops_sound = play_sound(10, 200, 100)
my_ev3.send_direct_cmd(ops_clear + ops_color + ops_sound)
time.sleep(5)
def stop()->None:
ops_read = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0),# LAYER
ev3.LCX(1),# NO
ev3.LCX(16),# TYPE - EV3-Touch
ev3.LCX(0),# MODE - Touch
ev3.LCX(1),# VALUES
ev3.GVX(0),# VALUE1
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0),# LAYER
ev3.LCX(1),# NO
ev3.LCX(16),# TYPE - EV3-Touch
ev3.LCX(1),# MODE - Bump
ev3.LCX(1),# VALUES
ev3.GVX(4)# VALUE1
])
ops_sound = play_sound(10, 200, 100)
reply = my_ev3.send_direct_cmd(ops_sound + ops_read, global_mem=8)
(touched, bumps) = struct.unpack('<ff', reply[5:])
if touched == 1:
bumps += 0.5
print(bumps, "bumps")
for i in range(3):
ready()
steady()
go()
stop()
ops_color = change_color(ev3.LED_GREEN)
my_ev3.send_direct_cmd(ops_color)
print("**** Game over ****")
我們使用了一個新操作：opInput_Device = 0x|99| 的 CMD CLR_CHANGES = 0x|1A|，有這些引數：
- (Data8) LAYER：鏈 layer 號
- (Data8) NO：埠編號
它清除感測器，所有它的內部資料被設定為初始值。
鬱悶的長頸鹿
讓我們編寫一個程式，它使用類TwoWheelVehicle
和紅外感測器。紅外感測器的型別編號為 33，它的模式 0 讀取感測器前方的自由距離。我們使用它來探測小車前方的障礙和坑洞。轉換你的小車並放置紅外感測器，使其看向前方，但從上到下（向下約30 - 60°）。感測器讀取小車前方的區域並在遇到意外狀況時停止運動：
#!/usr/bin/env python3
import ev3, ev3_vehicle, struct, random
vehicle = ev3_vehicle.TwoWheelVehicle(
0.02128,# radius_wheel
0.1175,# tread
protocol=ev3.BLUETOOTH,
host='00:16:53:42:2B:99'
)
def distance()-> float:
ops = b''.join([
ev3.opInput_Device,
ev3.READY_SI,
ev3.LCX(0),# LAYER
ev3.LCX(0),# NO
ev3.LCX(33),# TYPE - EV3-IR
ev3.LCX(0),# MODE - Proximity
ev3.LCX(1),# VALUES
ev3.GVX(0)# VALUE1
])
reply = vehicle.send_direct_cmd(ops, global_mem=4)
return struct.unpack('<f', reply[5:])[0]
speed = 25
vehicle.move(speed, 0)
for i in range(10):
while True:
dist = distance()
if dist < 15 or dist > 20:
break
vehicle.stop()
vehicle.sync_mode = ev3.SYNC
angle = 135 + 45 * random.random()
if random.random() > 0.5:
vehicle.drive_turn(speed, 0, angle)
else:
vehicle.drive_turn(speed, 0, angle, right_turn=True)
vehicle.sync_mode = ev3.STD
speed -= 2
vehicle.move(speed, 0)
vehicle.stop()
一些註釋：
-
如果你從ev3-python3
下載了模組 ev3_vehicle.py，請消除屬性
sync_mode的設定(vehicle.sync_mode = ev3.SYNC 或 vehicle.sync_mode = ev3.STD) -
演算法的核心部分是：
while True: dist = distance() if dist < 15 or dist > 20: break vehicle.stop()
這個程式碼在自由距離小於 15 cm 或大於 20 cm 時（具體值依賴於物件的構造）停止運動。這是說：如果小車到了桌子的邊緣（距離變大），它將停止，以及如果它到了一個障礙物處（小距離），它也將停止。
-
停止後，車輛以隨機方向及隨機角度開啟（範圍在 135 到 180°）。sync_mode 設定為 SYNC，我們想要程式等待直到轉彎完成：
vehicle.sync_mode = ev3.SYNC angle = 135 + 45 * random.random() if random.random() > 0.5: vehicle.drive_turn(speed, 0, angle) else: vehicle.drive_turn(speed, 0, angle, right_turn=True) vehicle.sync_mode = ev3.STD
-
然後速度減小，小車向前移動，迴圈再次開始：
speed -= 2 vehicle.move(speed, 0)
-
迴圈數限制為10。
- 我的感測器放在一個裝配長頸鹿頸部的結構上。這個以及越來越慢的運動就成了這個名字。
- 一個缺點是感測器直接向前聚焦。如果車輛以小角度移動到桌子的邊緣或靠著障礙物，它將會識別它太晚。
技術上會發生什麼？
drive_turn
現在是時候適配你的程式來滿足你的需要和你的小車的構造了。我發現將小車放在桌面上，其中桌面的一部分被屏障隔開，是最令人印象深刻的。
導引頭
紅外感測器有另一種有趣的模式：seeker
。這個模式讀取 EV3 紅外信標的方向和距離。信標允許在四個訊號通道中選一個。請在埠 2 插入 IR 感測器，開啟信標，選擇一個通道，把它放在紅外感測器的前方，然後執行這個程式：
#!/usr/bin/env python3
import ev3, struct
my_ev3 = ev3.EV3(protocol=ev3.BLUETOOTH, host='00:16:53:42:2B:99')
ops_read = b''.join([
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0),# LAYER
ev3.LCX(1),# NO
ev3.LCX(33),# TYPE - IR
ev3.LCX(1),# MODE - Seeker
ev3.LCX(8),# VALUES
ev3.GVX(0),# VALUE1 - heading channel 1
ev3.GVX(4),# VALUE2 - proximity channel 1
ev3.GVX(8),# VALUE3 - heading channel 2
ev3.GVX(12),# VALUE4 - proximity channel 2
ev3.GVX(16),# VALUE5 - heading channel 3
ev3.GVX(20),# VALUE6 - proximity channel 3
ev3.GVX(24),# VALUE5 - heading channel 4
ev3.GVX(28)# VALUE6 - proximity channel 4
])
reply = my_ev3.send_direct_cmd(ops_read, global_mem=32)
(
h1, p1,
h2, p2,
h3, p3,
h4, p4,
) = struct.unpack('8i', reply[5:])
print("heading1: {}, proximity1: {}".format(h1, p1))
print("heading2: {}, proximity2: {}".format(h2, p2))
print("heading3: {}, proximity3: {}".format(h3, p3))
print("heading4: {}, proximity4: {}".format(h4, p4))
朝向的範圍為 [-25 - 25]，負值代表左，0 代表直行，正的代表右邊。接近性的範圍為 [0 - 100]，且以 cm 計。這個操作讀取所有 4 個通道，每個通道兩個值。這個程式的輸出是(seeker 通道是 2)：
heading1: 0, proximity1: -2147483648 heading2: -21, proximity2: 27 heading3: 0, proximity3: -2147483648 heading4: 0, proximity4: -2147483648
信標放在紅外感測器的左前方，距離為 27 cm。通道 1，3，和 4 返回一個距離值 -2147483648，它是 0x|00:00:00:80|（小尾端，最高位為 1，所有其它的為 0），表示沒訊號 。
PID 控制器
PID 控制器
持續計算錯誤值，作為所需設定值和測量過程變數之間的差值。控制器嘗試通過控制變數的調整隨時間最小化誤差。這是一個偉大的演算法，它修改一個過程的引數直到過程達到它的目的狀態。最好的是，你不需要知道你的引數的精確依賴以及過程的狀態。一個典型的例子是加熱房間的暖氣片。過程變數是房間的溫度，控制器改變暖氣片閥的位置直到房間溫度穩定在設定的點。我們將使用 PID 控制器調整小車移動的引數speed
和turn
。我們給模組ev3
新增一個類 PID：
class PID(): """ object to implement a PID controller """ def __init__(self, setpoint: float, gain_prop: float, gain_der: float=None, gain_int: float=None, half_life: float=None ): self._setpoint = setpoint self._gain_prop = gain_prop self._gain_int = gain_int self._gain_der = gain_der self._half_life = half_life self._error = None self._time = None self._int = None self._value = None def control_signal(self, actual_value: float) -> float: if self._value is None: self._value = actual_value self._time = time.time() self._int = 0 self._error = self._setpoint - actual_value return self._gain_prop * self._error else: time_act = time.time() delta_time = time_act - self._time self._time = time_act if self._half_life is None: self._value = actual_value else: fact1 = math.log(2) / self._half_life fact2 = math.exp(-fact1 * delta_time) self._value = fact2 * self._value + actual_value * (1 - fact2) error = self._setpoint - self._value if self._gain_int is None: signal_int = 0 else: self._int += error * delta_time signal_int = self._gain_int * self._int if self._gain_der is None: signal_der = 0 else: signal_der = self._gain_der * (error - self._error) / delta_time self._error = error return self._gain_prop * error + signal_int + signal_der
這實現了一個PID控制器，只有一個修改：half_life
。實際值可能有噪聲或通過離散步驟改變，我們對它們進行平滑，因為當實際值隨機或離散變化時，導數部分將顯示峰值。half_life
的維度 [s] 為時間，並且是阻尼的半衰期。但請記住：平滑控制器使其變得遲緩！
它的文件為：
class PID(builtins.object) |object to implement a PID controller | |Methods defined here: | |__init__(self, setpoint:float, gain_prop:float, gain_der:float=None, gain_int:float=None, half_life:float=None) |Parametrizes a new PID controller | |Arguments: |setpoint: ideal value of the process variable |gain_prop: proportional gain, |high values result in fast adaption, but too high values produce oscillations or instabilities | |Keyword Arguments: |gain_der: gain of the derivative part [s], decreases overshooting and settling time |gain_int: gain of the integrative part [1/s], eliminates steady-state error, slower and smoother response |half_life: used for discrete or noisy systems, smooths actual values [s] | |control_signal(self, actual_value:float) -> float |calculates the control signal from the actual value | |Arguments: |actual_value: actual measured process variable (will be compared to setpoint) | |Returns: |control signal, which will be sent to the process
保持專注
請將紅外感測器放在車輛上，水平放在前面。將其插入埠 2，選擇信標通道 1，啟用信標，然後啟動這個程式：
#!/usr/bin/env python3
import ev3, ev3_vehicle, struct
vehicle = ev3_vehicle.TwoWheelVehicle(
0.02128,# radius_wheel
0.1175,# tread
protocol=ev3.BLUETOOTH,
host='00:16:53:42:2B:99'
)
ops_read = b''.join([
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0),# LAYER
ev3.LCX(1),# NO
ev3.LCX(33),# TYPE - IR
ev3.LCX(1),# MODE - Seeker
ev3.LCX(2),# VALUES
ev3.GVX(0),# VALUE1 - heading channel 1
ev3.GVX(4)# VALUE2 - proximity channel 1
])
speed_ctrl = ev3.PID(0, 2, half_life=0.1, gain_der=0.2)
while True:
reply = vehicle.send_direct_cmd(ops_read, global_mem=8)
(heading, proximity) = struct.unpack('2i', reply[5:])
if proximity == -2147483648:
print("**** lost connection ****")
break
turn = 200
speed = round(speed_ctrl.control_signal(heading))
speed = max(-100, min(100, speed))
vehicle.move(speed, turn)
vehicle.stop()
說明：
- 我們選擇了通道 1，這隻允許讀取該通道的值。
- 控制器不是一個 PID，它的 PD 帶有平滑的值。
- 如果你移動信標，你的小車將改變它的方向並保持信標在它的眼鏡的焦點上。
- 這個程式在關閉信標時停止。
- 前進方向是過程變數，其設定值為 0（直行）。通過將車輛轉動到位來完成調整。
- 請改變 PD 控制器的引數以瞭解控制機制。
- 沒有穩定狀態錯誤，因為 control_signal == 0 將程序保持在穩定狀態並且是唯一的穩定狀態。
跟我來
我們稍微改變了程式的程式碼，但從根本上改變了它的含義：
#!/usr/bin/env python3
import ev3, ev3_vehicle, struct
vehicle = ev3_vehicle.TwoWheelVehicle(
0.02128,# radius_wheel
0.1175,# tread
protocol=ev3.BLUETOOTH,
host='00:16:53:42:2B:99'
)
ops_read = b''.join([
ev3.opInput_Device,
ev3.READY_RAW,
ev3.LCX(0),# LAYER
ev3.LCX(1),# NO
ev3.LCX(33),# TYPE - IR
ev3.LCX(1),# MODE - Seeker
ev3.LCX(2),# VALUES
ev3.GVX(0),# VALUE1 - heading channel 1
ev3.GVX(4)# VALUE2 - proximity channel 1
])
speed_ctrl =ev3.PID(10, 4, half_life=0.1, gain_der=0.2)
turn_ctrl =ev3.PID(0, 8, half_life=0.1, gain_der=0.3)
while True:
reply = vehicle.send_direct_cmd(ops_read, global_mem=8)
(heading, proximity) = struct.unpack('2i', reply[5:])
if proximity == -2147483648:
print("**** lost connection ****")
break
turn = round(turn_ctrl.control_signal(heading))
turn = max(-200, min(200, turn))
speed = round(-speed_ctrl.control_signal(proximity))
speed = max(-100, min(100, speed))
vehicle.move(speed, turn)
vehicle.stop()
這個程式使用 heading 來控制移動引數turn
和proximity
繼而控制它的speed
。speed_ctrl
的設定點是一個距離 (10 cm)。如果距離增長，控制器增加小車的速度。你可以減少 10 釐米以下的距離，然後車輛向後移動。控制器總是試圖保持或達到信標和紅外感測器距離為 10 釐米的狀態。請改變兩個感測器的引數。
如果信標穩定向前移動並且車輛跟隨信標會發生什麼？這就像駕駛車隊一樣，可以研究穩態誤差。則 speed = gain_properror，即 speed = gain_prop (proximity - setpoint)。這是說：proximity = speed / gain_prop + setpoint。信標和感測器之間的穩定距離隨著速度從 10 cm (speed == 0) 到 35 cm (speed == 100) 的增加而增加。如果我們模擬車輛車隊，這正是我們想要的。
我們可以設定gain_int
為一個正值。甚至非常小的值將消除穩態誤差。巡航將保持在 10 cm 的距離，甚至在高速的情況下。
結論
這一課是關於感測器值的。我們已經看到，電機也是感測器，它允許我們讀取實際的電機位置。我們寫了一些小程式，使用紅外感測器來控制帶有兩個驅動輪的車輛的運動。這是我們第一個真正的機器人程式。讀取感測器值的機器，可以對環境作出反應。
我們獲得了一些 PID 控制器方面的經驗，PID 控制器是受控過程的行業標準。調整它們的引數取代了複雜演算法的編碼。我們的程式，使用PID控制器是驚人的緊湊和驚人的統一。PID控制器似乎功能強大且通用。
下一課將改進我們的類TwoWheelVehicle
，併為多工做好準備。我期待著再次見到你。