{"components":[{"id":"_stickscreen","createTime":1790922055206,"name":"screen","x":0,"y":0,"width":120,"height":240,"backgroundColor":"#111111","backgroundImage":"","size":0,"screenType":"default","type":"screen"}],"type":"stick-C","versions":"Beta","units":[],"hats":[],"stamps":[],"chains":[],"blockly":"readydatareplyserver_ipserver_portfrom m5stack import * from m5ui import * from uiflow import * # I2C 和 Pin 用來直接和伺服馬達 Hat 溝通（原因寫在「伺服馬達」那一段）。 from machine import I2C, Pin # math 提供 sin（正弦）和 pi（圓周率），用來算出腳擺動的波形。 import math # ---------------- wiring 接線 ---------------- # 六隻腳各接在 Hat 的哪一個通道。清單裡的順序就是腳的順序。 LEFT_CHANNELS = [6, 7, 8] # 左腳 L1、L2、L3 接的通道 RIGHT_CHANNELS = [2, 3, 4] # 右腳 R1、R2、R3 接的通道 # 把兩個清單接起來，得到六隻腳的順序 L1 L2 L3 R1 R2 R3（和 flybrain/config.py 相同）。 # 之後程式裡說「第 leg 隻腳」，leg = 0～2 是左腳、3～5 是右腳。 CHANNELS = LEFT_CHANNELS + RIGHT_CHANNELS # 每隻腳的轉動方向：+1 表示角度變大時腳往前擺，-1 表示角度變大時腳往後擺。 # 左右兩邊的馬達是鏡像裝的：左邊馬達 0 度是往前、180 度是往後；右邊剛好相反。 LEG_DIRECTION = [-1, -1, -1, 1, 1, 1] # 每隻腳的微調角度（單位：度）。組裝時馬達裝歪了一點，就在這裡補回來，平常是 0。 LEG_TRIM = [0, 0, 0, 0, 0, 0] NEUTRAL = 90 # 站好時每顆馬達的角度：90 度是正中間 ANGLE_MIN = 10 # 馬達最小只轉到 10 度，避免撞到機構的極限 ANGLE_MAX = 170 # 馬達最大只轉到 170 度 # ---------------- gait 步態 ---------------- FREQ_HZ = 1.0 # 每秒走幾個步態週期；電池 3.7 V 時 SG-90 跟得上這個速度 AMPLITUDE_DEG = 30 # 腳從正中間往前、往後各擺幾度 # 波形的「歪斜程度」：讓腳往前收得快、往後推地慢，像真的昆蟲一樣。 HARMONIC_RATIO = 0.25 # 每隻腳的起始相位（單位：弧度）。0 和 pi 剛好差半個週期， # 所以 L1、L3、R2（相位 0）和 L2、R1、R3（相位 pi）兩組三腳架會輪流擺動。 PHASE_OFFSETS = [0.0, math.pi, 0.0, math.pi, 0.0, math.pi] STEP_MS = 20 # 每 20 毫秒更新一次馬達角度（一秒 50 次） MOVE_CYCLES = 3 # 按一次前進或後退，走 3 個步態週期 # 轉彎要走幾個週期，是在實體機器人上量出來的：3 個週期大約轉 75 度，6 個週期大約轉 135 度。 # 想重新校正時：新的週期數 = 舊的週期數 × 想要的角度 ÷ 量到的角度。 TURN_CYCLES = 1.8 # 左轉、右轉：3 × 45 ÷ 75 -> 轉 45 度 UTURN_CYCLES = 8 # 迴轉：6 × 180 ÷ 135 -> 轉 180 度 # 動作表：每個按鈕名稱對應一組（左邊幅度, 右邊幅度, 走幾個週期, 螢幕顯示的文字）。 # 幅度 1.0 表示那一邊正常往前走；幅度是負的，那一邊的步伐就倒過來播放（變成往後走）。 # 這是因為波形是「奇函數」：-wave(p) 剛好等於 wave(-p)。 # 轉彎時讓兩邊互相反著走（一邊往前、一邊往後），身體就原地旋轉， # 比只讓一邊走、另一邊停住轉得快。 MOTIONS = { \"forward\": (1.0, 1.0, MOVE_CYCLES, \"FORWARD\"), \"backward\": (-1.0, -1.0, MOVE_CYCLES, \"BACK\"), \"left\": (-1.0, 1.0, TURN_CYCLES, \"LEFT\"), \"right\": (1.0, -1.0, TURN_CYCLES, \"RIGHT\"), \"uturn\": (1.0, -1.0, UTURN_CYCLES, \"U-TURN\"), } # 找出波形 sin(p) + HARMONIC_RATIO × sin(2p) 在一個週期裡的最大高度。 # 輸入：沒有。輸出：最大高度（一個正的小數）。 # 為什麼：之後把波形除以這個數，波形的最高點就剛好是 1，擺動角度才會剛好是 AMPLITUDE_DEG。 def _wave_peak(): largest = 0.0 # 把一整圈（0 到 2π）切成 720 小格，一格一格算，記下看過的最大值。 for step in range(720): phase_in_radians = step * math.pi / 360.0 height = math.sin(phase_in_radians) + HARMONIC_RATIO * math.sin(2.0 * phase_in_radians) # 波形有正有負，我們要的是離 0 最遠的距離，所以取絕對值。 if abs(height) > largest: largest = abs(height) return largest # 程式一開始就先算好最大高度存起來，之後每次畫波形直接拿來用，不用重算。 WAVE_PEAK = _wave_peak() # 步態波形：給一個相位（弧度），回傳腳該擺到哪裡，範圍是 -1 到 1。 # 輸入：phase 相位。輸出：-1（腳在最後面）到 1（腳在最前面）之間的數。 # 這一行公式和 FlyBrain 的 flybrain/cpg.py 完全一樣，所以機器人走路的樣子和模擬的果蠅相同。 def wave(phase): height = math.sin(phase) + HARMONIC_RATIO * math.sin(2.0 * phase) return height / WAVE_PEAK # ---------------- screen font 螢幕字型 ---------------- # 把螢幕換成小字型。 # 輸入、輸出：都沒有。 # 為什麼：StickC 的螢幕只有 80 像素寬，大字放不下動作名稱。 # 不同版本的 UIFlow 小字型的名字不一樣，所以兩個名字都試，有哪個就用哪個。 def use_small_font(): for font_name in (\"FONT_DefaultSmall\", \"FONT_Small\"): # hasattr 檢查 lcd 有沒有這個名字的字型；getattr 用名字把字型拿出來。 if hasattr(lcd, font_name): lcd.font(getattr(lcd, font_name)) return # ---------------- servos 伺服馬達 ---------------- # 這支程式不用 UIFlow 內建的 hat.get(hat.SERVOS)，因為實際測試時， # Hat v1.1 明明插好了它還是報錯「Servos hat maybe not connect」。 # 所以改成自己用 I2C 直接寫 Hat 的暫存器，順便能在螢幕上顯示匯流排上真正找到了什麼。 HAT_SDA = 0 # StickC 上方排針的 G0 腳：I2C 資料線 SDA HAT_SCL = 26 # StickC 上方排針的 G26 腳：I2C 時脈線 SCL HAT_V11_ADDR = 0x36 # Hat 8Servos v1.1 的 I2C 位址 HAT_V10_ADDR = 0x38 # 舊版 Hat 8Servos v1.0 的位址：角度暫存器一樣，但沒有電源開關 REG_POWER = 0x30 # 只有 v1.1 有的電源暫存器：寫 1 才會打開馬達電源 # 把 I2C 掃到的位址清單轉成好讀的文字，例如 [54, 104] 變成 \"['0x36', '0x68']\"。 # 輸入：addresses 位址清單（整數）。輸出：一段文字，用來顯示在螢幕和錯誤訊息上。 def addresses_as_text(addresses): hex_addresses = [] for address in addresses: hex_addresses.append(hex(address)) return str(hex_addresses) # Servos8Hat 代表插在 StickC 上的那塊伺服馬達 Hat。 # 建立它的時候會找出 Hat 是哪一版，之後用 write_servo_angle() 叫某一個通道的馬達轉到某個角度。 class Servos8Hat: # 建立時執行一次：打開 I2C、掃描匯流排、認出 Hat 的版本。 # 找不到 Hat 就在螢幕寫 NO HAT 並停下來，不會假裝在走。 def __init__(self): self.i2c = I2C(1, sda=Pin(HAT_SDA), scl=Pin(HAT_SCL), freq=100000) # scan() 問一遍匯流排上所有裝置，回傳有回答的位址清單。 found = self.i2c.scan() if HAT_V11_ADDR in found: self.addr = HAT_V11_ADDR # v1.1 開機時馬達電源是關的，一定要寫 1 到電源暫存器，馬達才會動。 self.i2c.writeto_mem(self.addr, REG_POWER, bytes([1])) elif HAT_V10_ADDR in found: self.addr = HAT_V10_ADDR else: use_small_font() lcd.clear() lcd.print(\"NO HAT\", 4, 6, 0xff4444) lcd.print(\"I2C: \" + addresses_as_text(found), 4, 24, 0xffffff) raise OSError(\"8Servos hat not on I2C, found \" + addresses_as_text(found)) # 叫一顆馬達轉到指定角度。 # 輸入：channel 是 Hat 上印的通道 1～8；angle 是 0～180 的整數角度。 # Hat 的第 n 個通道的角度暫存器位址是 n - 1（CH1 在 0x00、CH2 在 0x01……）。 def write_servo_angle(self, channel, angle): self.i2c.writeto_mem(self.addr, channel - 1, bytes([angle])) servos = Servos8Hat() # 每隻腳上一次真正送給馬達的角度。一開始還沒送過，所以是 None（沒有值）。 last_angle = [None] * 6 # 每隻腳上一次「想要」的角度（還沒套上方向和微調之前），滑回站姿時要從這裡出發。 last_pose = [NEUTRAL] * 6 # 讓第 leg 隻腳轉到 angle 度。 # 輸入：leg 是 0～5 的腳編號；angle 是「想要」的角度（90 = 正中間）。 # 這裡會先套上這隻腳的方向和微調，再把角度限制在安全範圍裡，最後才送給馬達。 def write_leg(leg, angle): # 套方向：離正中間多少度，乘上 +1 或 -1，左右兩邊的鏡像就對上了；再加上微調。 angle = NEUTRAL + LEG_DIRECTION[leg] * (angle - NEUTRAL) + LEG_TRIM[leg] # 馬達只接受整數角度，先四捨五入。 angle = int(round(angle)) # 限制範圍：太小就用最小值，太大就用最大值。 if angle < ANGLE_MIN: angle = ANGLE_MIN if angle > ANGLE_MAX: angle = ANGLE_MAX # 角度和上次一樣就不送，讓 I2C 匯流排安靜一點。 if angle != last_angle[leg]: servos.write_servo_angle(CHANNELS[leg], angle) last_angle[leg] = angle # 六隻腳全部回到正中間（站姿）。 def stand(): for leg in range(6): last_pose[leg] = NEUTRAL write_leg(leg, NEUTRAL) # 依照步態波形，把六隻腳擺到這個相位該在的位置。 # 輸入：phase 目前的相位；amp_left、amp_right 是左右兩邊的幅度（來自動作表 MOTIONS）。 def pose(phase, amp_left, amp_right): for leg in range(6): # leg 0～2 是左腳，用左邊的幅度；3～5 是右腳，用右邊的幅度。 if leg < 3: amplitude = AMPLITUDE_DEG * amp_left else: amplitude = AMPLITUDE_DEG * amp_right # 每隻腳加上自己的相位差，兩組三腳架才會輪流擺。 last_pose[leg] = NEUTRAL + amplitude * wave(phase + PHASE_OFFSETS[leg]) write_leg(leg, last_pose[leg]) # ---------------- screen 螢幕 ---------------- # 清掉螢幕，上面一行寫 FlyBrain，下面一行寫 text（例如目前的動作名稱）。 def show(text): lcd.clear() lcd.print(\"FlyBrain\", 4, 6, 0x2dd4bf) lcd.print(text, 4, 24, 0xffffff) # ---------------- motion state 動作狀態 ---------------- # 轉彎走 1.8 個週期，停下來時腳還在步伐中間，每隻腳偏了大約 30 度。 # 如果六顆馬達同一瞬間一起跳回 90 度，電流會突然變很大，讓 Hat 斷電。 # 所以動作結束（或被別的按鈕打斷）時，先用 SETTLE_STEPS 個小步慢慢滑回站姿。 # 每個動作也都從站姿、相位 0 開始，所以同一個按鈕每次轉的角度都一樣。 SETTLE_STEPS = 10 # 滑回站姿分 10 小步，共 200 毫秒，每步大約 3 度 motion = None # 正在做的動作名稱；None 表示沒有在走 pending = None # 腳還沒收回來時又按的按鈕，等站好再做 steps_left = 0 # 這個動作還剩幾個 20 毫秒小步 settle_left = 0 # 滑回站姿還剩幾小步 settle_from = [NEUTRAL] * 6 # 開始滑回時每隻腳的位置 phase = 0.0 # 目前的步態相位（弧度） # 開始做一個動作：從相位 0 開始，算出這個動作要走幾個 20 毫秒小步。 # 輸入：name 動作名稱，例如 \"forward\"。 # 例如前進：3 個週期 × 1000 毫秒 ÷ (每秒 1 個週期 × 每步 20 毫秒) = 150 小步。 def begin(name): # global 表示要改的是函式外面那個變數，不是在函式裡另外建一個新的。 global motion, pending, steps_left, phase motion = name pending = None phase = 0.0 cycles = MOTIONS[name][2] steps_left = int(round(cycles * 1000 / (FREQ_HZ * STEP_MS))) # 停止步態，從腳現在的位置開始慢慢滑回站姿。 def settle(): global motion, settle_left motion = None for leg in range(6): settle_from[leg] = last_pose[leg] settle_left = SETTLE_STEPS # Remote+ 按鈕會呼叫這個函式。 # 輸入：name 動作名稱。 # 如果腳正在走或還沒收回來，就先記下這個按鈕，等腳回到站姿再開始，不會突然換動作。 def start(name): global pending show(MOTIONS[name][3]) if motion is not None: # 正在走：記下新動作，先滑回站姿。 pending = name settle() elif settle_left > 0: # 正在滑回站姿：記下新動作，滑完就開始。 pending = name else: # 已經站好：直接開始。 begin(name) # A 鍵的緊急停止：不等待、不滑行，直接站好，並清掉所有排隊中的動作。 def stop(): global motion, pending, steps_left, settle_left motion = None pending = None steps_left = 0 settle_left = 0 stand() show(\"READY\") # 一個 20 毫秒的節拍：如果正在走，就讓步態往前一小步；如果正在滑回站姿，就滑一小步。 # 主迴圈（積木版是 Loop 積木）每次呼叫這個函式，然後等 STEP_MS 毫秒。 def tick(): global phase, steps_left, settle_left if motion is not None: amp_left = MOTIONS[motion][0] amp_right = MOTIONS[motion][1] # 相位每 20 毫秒前進一點：一秒走 FREQ_HZ 圈，一圈是 2π。 phase += 2.0 * math.pi * FREQ_HZ * STEP_MS / 1000.0 pose(phase, amp_left, amp_right) steps_left -= 1 if steps_left <= 0: settle() return if settle_left > 0: settle_left -= 1 # share 從 0.9 慢慢變到 0：腳離正中間的距離每一步縮小一點。 share = settle_left / SETTLE_STEPS for leg in range(6): last_pose[leg] = NEUTRAL + (settle_from[leg] - NEUTRAL) * share write_leg(leg, last_pose[leg]) if settle_left == 0: # 滑完了：有排隊的動作就開始做，沒有就顯示 READY。 if pending is not None: begin(pending) else: show(\"READY\") use_small_font() stop()# 積木：執行程式碼（貼這一段 Setup 用的小工具） import network import uselect UDP_PORT = 4210 # 昆蟲在這個 port 等電腦的訊息；電腦端的 bug_server.py 也用 4210 server_ip = \"\" # 電腦的 IP，收到 HELLO 之後才知道 server_port = 0 # 電腦接收回覆的 port reply = \"\" # 這一次要回給電腦的文字；空字串表示不用回 # 用 WiFi 晶片的 MAC 位址最後 4 個十六進位數字，替這隻昆蟲取一個不會和別隻重複的名字。 # 輸出：例如 \"bug-3f2a\"。 def bug_name(): mac = network.WLAN(network.STA_IF).config(\"mac\") return \"bug-\" + \"%02x%02x\" % (mac[-2], mac[-1]) # 這隻昆蟲在 WiFi 網路上的 IP 位址，例如 \"192.168.1.23\"。 def my_ip(): return network.WLAN(network.STA_IF).ifconfig()[0] # 處理電腦傳來的一則訊息，決定要做什麼、要回什麼。 # 輸入：data 收到的 bytes。 # 結果：需要回覆時，把回覆文字放進全域變數 reply（Loop 裡的「傳送」積木會把它送出去）。 def handle_udp(data): global server_ip, server_port, reply reply = \"\" text = data.decode().strip() parts = text.split(\",\") if parts[0] == \"HELLO\" and len(parts) == 3: # 記住電腦的地址，之後的回覆都送到這裡。 server_ip = parts[1] server_port = int(parts[2]) reply = \"HERE,\" + bug_name() + \",\" + my_ip() elif parts[0] == \"GO\" and len(parts) == 2: if parts[1] in MOTIONS: start(parts[1]) reply = \"OK,\" + parts[1] else: reply = \"ERR,unknown motion \" + parts[1] elif parts[0] == \"STOP\": stop() reply = \"OK,STOP\" elif parts[0] == \"PING\": # 狀態：正在做的動作，或是 ready（站著等指令）。 if motion is not None: reply = \"PONG,\" + motion else: reply = \"PONG,ready\" else: reply = \"ERR,unknown message\"0.0.0.04210udp_watch = uselect.poll() udp_watch.register(udpsocket, uselect.POLLIN)lcd.print(bug_name(), 4, 42, 0xffffff) lcd.print(my_ip(), 4, 58, 0xffffff)ready = udp_watch.poll(0)readydata64handle_udp(data)ANDNEQreplyNEQserver_port0reply0.0.0.0server_ip4210server_porttick()20AwasPressedstop()","Blockly.Remotes":[],"Blockly.RemotePlus":[{"id":"__title","blockId":"","createTime":1790922055150,"name":"M5RemoteTitle","dragAndDrop":false,"resizable":false,"options":{"minWidth":1,"minHeight":1,"maxWidth":6,"maxHeight":10,"defaultWidth":2,"defaultHeight":1},"w":2,"h":1,"bgColor":"#0080FF","color":"#fff","fontsize":"M","label":"M5Remote","interval":3000,"code":"","event":"","dataSource":"none","ezdataToken":"HRwQw8nngzoaou2gVcSx8ppd5Mwapdlt","topic":"","needShadow":false,"type":"title","x":null,"y":null}],"modules":["remotePlus"],"cbIdList_":[],"eventCBIdList_":[],"apikey":"DA5E4F5F","uuid":"fd8f14a7-4fe2-4afa-8ebb-ae30af85cc9b"}