# WRO 2022 早鳥高中組創意賽 ## 作品報告書 [投影片](http://13.208.247.127/public/upload/1352dc06c9.pdf) ## 主題 & 故事劇情 ### 主題概要 實作一個完整的 ADHD 家庭照護系統 包含智慧機器人、 AI 智能關節律動辨識技術、節奏樂器控制模組、遠端控 制APP 等 可適時引導 ADHD 孩子透過舞蹈表演、律動、 音樂等方法促進身心健康,舞出注意力 ### 劇情 設計一個機器人系統,能在家自動巡邏,也能辨識 ADHD 孩子的臉部情緒 若是發現心情不美麗,則會邀 請他來跳個舞律動一下 並且用 AI 影像辨識技術來觀測 他的律動狀況 而遠在上班的爸媽也可以透過手機 APP 隨時掌握孩子的狀況 ![](https://i.imgur.com/XMnoMYU.jpg) ### 參考資料 [急性律動活動對ADHD兒童注意力之影響](https://hdl.handle.net/11296/dvq362) ## 成果影片 ## 使用設備 * 平板電腦 & Android 手機(手機應用程式) * Macbook(作為Broker & 執行程式 & Demo) * Matrix 套件(機器人主體) * EV3 主機 & 馬達(進行打鼓 & 拍手) * Jetson Nano AI 開發版(MQTT & 骨骼分析 & 囧臉辨識) * USB Cam 攝影鏡頭 ## Motor Contoller 馬達控制模組 ![](https://i.imgur.com/iwk02Zr.jpg =300x300) ### Matrix Motor 全向輪的移動 ```cpp=36 int motor_one, motor_two, motor_three; void Speed_Moto_Control(float vx,float vy,float vz){ motor_one = (-VX_VALUE*vx + VY_VALUE*vy + L_value*vz); motor_two = (-VX_VALUE*vx - VY_VALUE*vy + L_value*vz); motor_three = (vx + L_value*vz); } ``` ### 參考資料 [三輪的全向輪運動學正解](https://www.codeprj.com/zh/blog/59976e1.html) ## Matrix Mini 感測器控制模組 * Matrix Ultrasonic Sensor : 放在車身前中右來偵測與牆的位置 ![](https://i.imgur.com/q6sOvzf.jpg =300x250) * MATRIX 類比光感測器 : 使用類比光感測器確認機器人所在區域 ![](https://i.imgur.com/pNv2aWo.jpg =300x300) ## Matrix 家庭巡邏 ### 巡邏程式碼(沿著左牆走) :::spoiler PutsYourHandsUp 車的巡邏程式 ```cpp=151 //D1 前 D2 左 D3 右(Ultrasonic Sensor) int d1 = Mini.D1.US.get(); int d2 = Mini.D2.US.get(); int d3 = Mini.D3.US.get(); int d4 = Mini.D4.get(); // 離牆太近往右 if(d2 < 11){ turn_right(7); counts = 0; } // 離牆太遠往左 else if(d2 > 20 && d2 <= 50){ move_left(Front_sp+1); counts = 0; } // 離牆超遠(要回家) else if(d2 > 50){ counts++; if(counts>8){ front(Front_sp); delay(700); turn_left(LR_sp); delay(300); front(Front_sp); delay(700); counts = 0; } } // 沿著左牆走的基本程式 else{ counts = 0; // OO 左前 -> 右轉 if(LEFT(d2) && FRONT(d1) && !RIGHT(d3)){ turn_right(LR_sp); } // OX 左 -> 直走 else if(LEFT(d2) && !FRONT(d1) && !RIGHT(d3)) { front(Front_sp); } // XO 前 -> 右轉 else if(!LEFT(d2) && FRONT(d1) && !RIGHT(d3)) { turn_right(LR_sp); } // XX 無 -> 直走 else if(!LEFT(d2) && !FRONT(d1) && !RIGHT(d3)) { front(Front_sp); } } } ``` ::: :::spoiler Cindy 車的巡邏程式 ```cpp=90 else if(((fp-t<1 && t<=fp) || (fp==1 && t==7)) && fp!=0){ //防撞(fp是前車位置,t是我的位置) Mini.I2C1.MXctrl.motorSet(1,0);//po是power,是根據電量調整動力用的 Mini.I2C1.MXctrl.motorSet(2,0); Mini.I2C1.MXctrl.motorSet(3,0); delay(10); } else if(m<=20 || r<=20 || l<=10){//靠牆太近,原地右轉 Mini.I2C1.MXctrl.motorSet(1,-7-po); Mini.I2C1.MXctrl.motorSet(2,-7-po); Mini.I2C1.MXctrl.motorSet(3,-7-po); } else if(l>=30){//離牆太遠,向左偏轉 Mini.I2C1.MXctrl.motorSet(1,14+po); Mini.I2C1.MXctrl.motorSet(2,-14-po); Mini.I2C1.MXctrl.motorSet(3,11+po); } else if(l<=10){//離牆稍近,向右偏轉 Mini.I2C1.MXctrl.motorSet(1,14+po); Mini.I2C1.MXctrl.motorSet(2,-14-po); Mini.I2C1.MXctrl.motorSet(3,-11-po); } else{//距離完美,直走 Mini.I2C1.MXctrl.motorSet(1,+15+po); Mini.I2C1.MXctrl.motorSet(2,-15-po); Mini.I2C1.MXctrl.motorSet(3,0); } } ``` ::: ## EV3樂器打擊控制模組 ### 律動節拍程式碼(打鼓 & 拍手) :::spoiler 打鼓&拍手程式 ```python= #!/usr/bin/env pybricks-micropython from pybricks.hubs import EV3Brick from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor) from pybricks.parameters import Port, Stop, Direction, Button, Color from pybricks.tools import wait, StopWatch, DataLog from pybricks.robotics import DriveBase from pybricks.media.ev3dev import SoundFile, ImageFile from umqtt.robust import MQTTClient import time import os c=0 tp=0 MQTT_Topic_Beat = 'Beat' MQTT_Topic_Mother = 'Mother' MQTT_ClientID = 'alvin_ev3' MQTT_Broker = '192.168.100.114' # This program requires LEGO EV3 MicroPython v2.0 or higher. # Click "Open user guide" on the EV3 extension tab for more information. def getmessages(topic, msg): global c,tp if(topic==MQTT_Topic_Beat.encode()): if(c==0): print("A") print(time.time()-tp) tp=time.time() a() c=1 else: print("B") print(time.time()-tp) tp=time.time() b() c=0 else: print("hello") ho() # Create your objects here. ev3 = EV3Brick() # Write your program here. ev3.speaker.beep() l_m = Motor(Port.A) m_m = Motor(Port.B) r_m = Motor(Port.C) l_m.reset_angle(0) m_m.reset_angle(0) r_m.reset_angle(0) def a(): wait(850) l_m.reset_angle(0) l_m.run_target(7000, -610,Stop.HOLD, True) def b(): pass #r_m.reset_angle(0) #r_m.run_target(7000, -610 ,Stop.HOLD, True) def ho(): wait(5000) l_m.reset_angle(0) l_m.run_target(7000, -610,Stop.HOLD, True) wait(608) r_m.reset_angle(0) r_m.run_target(7000, -610,Stop.HOLD, True) wait(608) l_m.reset_angle(0) l_m.run_target(7000, -610,Stop.HOLD, True) wait(608) r_m.reset_angle(0) r_m.run_target(7000, -610,Stop.HOLD, True) wait(608) client = MQTTClient(MQTT_ClientID, MQTT_Broker) client.connect() client.set_callback(getmessages) client.subscribe(MQTT_Topic_Beat) client.subscribe(MQTT_Topic_Mother) while True: client.check_msg() time.sleep(0.1) ``` ::: ## Jetson Nano AI影像辨識模組 ### 囧臉辨識&骨骼分析 #### 使用技術 * Teachable Machine 影像分類訓練 #### 訓練的過程 1. 搜集訓練資料(拍很多照片) 2. 標註各分類(Class)名字 3. 開始訓練(Training) 4. 驗證信心分數(避免誤判) 5. 下載Modle模型 #### 程式碼 :::spoiler 囧臉辨識程式 ```python= import tensorflow as tf import cv2 import numpy as np import time import paho.mqtt.client as mqtt import paho.mqtt.publish as publish import _thread import serial model = tf.keras.models.load_model('keras_model.h5', compile=False)#model data = np.ndarray(shape=(1, 224, 224, 3), dtype=np.float32) cap = cv2.VideoCapture(0) if not cap.isOpened(): print("Cannot open camera") exit() while True: ret, frame = cap.read() if not ret: print("Cannot receive frame") break img = cv2.resize(frame , (398, 224) ) img = img[0:224, 80:304] image_array = np.asarray(img) normalized_image_array = (image_array.astype(np.float32) / 127.0) - 1 data[0] = normalized_image_array prediction = model.predict(data) a = prediction[0][0] print(a) if a>=0.80: test = "Face Found Confidence:" test2 = str(round(a*100,2)) test = test + test2 publish.single( topic="Jiong", payload="1", hostname="192.168.100.114", port=1883 ) print('Embarssing') elif a<0.80: test = "No Face Found" print('Normal') cv2.putText(frame,test,(10,30),cv2.FONT_HERSHEY_COMPLEX,1,(255,0,0),2,cv2.LINE_AA) cv2.imshow('Face', frame) if cv2.waitKey(500) == ord('q'): break # 建立 MQTT Client 物件 client = mqtt.Client() # 設定建立連線回呼函數 client.on_connect = on_connect # 設定接收訊息回呼函數 client.on_message = on_message # 設定登入帳號密碼(若無則可省略) # client.username_pw_set("myuser","mypassword") # 連線至 MQTT 伺服器(伺服器位址,連接埠) client.connect("192.168.100.114", 1883) # 進入無窮處理迴圈 cap.release() cv2.destroyAllWindows() ``` ::: :::spoiler 骨骼分析程式 ```python= import cv2 import numpy as np import mediapipe as mp import time import json from decimal import Decimal import paho.mqtt.publish as publish import time import paho.mqtt.client as mqtt import _thread tim=-1 avg=0 avgn=0 sced=0 def on_connect(client,userdata,flags,rc): client.subscribe("Music") def on_message(client,userdata,msg): global tim print("m") tim=time.time() client=mqtt.Client() client.on_connect=on_connect client.on_message=on_message client.connect("192.168.100.114",1883) sted=0 def mqt(whatever): client.loop_forever() _thread.start_new_thread(mqt,(0,)) badtime=0 frame_count=0 tim=-1.0 mp_drawing = mp.solutions.drawing_utils # mediapipe 繪圖方法 mp_drawing_styles = mp.solutions.drawing_styles # mediapipe 繪圖樣式 mp_holistic = mp.solutions.holistic # mediapipe 全身偵測方法 cap = cv2.VideoCapture(0) poin=0 poinp=0 poinpp=0 # mediapipe 啟用偵測全身 with mp_holistic.Holistic( min_detection_confidence=0.5, min_tracking_confidence=0.5) as holistic: if not cap.isOpened(): print("Cannot open camera") exit() frame_rate=10 prev =0 while True: time_elapsed = time.time()-prev ret, img = cap.read() #frame_count+=1 #print(frame_count) if time_elapsed >1./frame_rate: prev=time.time() if not ret: print("Cannot receive frame") break img = cv2.resize(img,(520,300)) #img = cv2.resize(img,(300,200)) img2 = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) # 將 BGR 轉換成 RGB results = holistic.process(img2) # 開始偵測全身 # 面部偵測,繪製臉部網格 mp_drawing.draw_landmarks( img, results.face_landmarks, mp_holistic.FACEMESH_CONTOURS, landmark_drawing_spec=None, connection_drawing_spec=mp_drawing_styles .get_default_face_mesh_contours_style()) kk=str(results.pose_landmarks).replace('landmark','') i=0 j=0 xs='' ys='' xn=0.0 yn=0.0 x=[0.0]*100 y=[0.0]*100 poinpp=poinp poinp=poin poin=0.0 if(kk != 'NONE'): while i<len(kk): if(kk[i]=='x'): i+=2 while True: if kk[i]=='\n': break else: xs+=kk[i] i+=1 xn=float(xs) xs='' if(kk[i]=='y' and kk[i-1]!='t'): i+=2 while True: if kk[i]=='\n': break else: ys+=kk[i] i+=1 yn=float(ys) ys='' if(j>=10): poin+=abs((x[j]-xn)*(x[j]-xn)+((y[j]-yn)*(y[j]-yn)*2))*(3-yn) x[j]=xn y[j]=yn j+=1 i+=1 print(poin) print(tim) print(time.time()) if(poin+poinp+poinpp<=270 and poin+poinp+poinpp!=0): print("Great") elif(poin+poinp+poinpp<=315): print("Good") else: print("Bad") rl=x[12]+x[11] if(rl<1.2 and rl!=0 and tim!=-1): publish.single( topic="Alvincarst", payload=1, hostname="192.168.100.114", port=1883) #print("sp"+str(rl)) print("st") if(tim!=-1 and time.time()-tim>=65 and sted==0 ): publish.single( topic="Dance", payload=1, hostname="192.168.100.114", port=1883) sted=1 # 身體偵測,繪製身體骨架 if(sted==1 and sced==0): avg+=poin avgn+=1 if(sted==1 and time.time()-tim>=96 and sced==0): re=time.strftime("%Y-%m-%d %I:%M:%S %p",time.localtime()) re+=" 舞蹈分數: " re+=str(round(180-(avg/avgn)),2) if(avg/avgn>=120): re+=" Bad" elif(avg/avgn<=80): re+=" Great" else: re+=" Good" publish.single( topic="Activity", payload=re, hostname="192.168.100.114", port=1883) sced=1 mp_drawing.draw_landmarks( img, results.pose_landmarks, mp_holistic.POSE_CONNECTIONS, landmark_drawing_spec=mp_drawing_styles .get_default_pose_landmarks_style()) cv2.imshow('oxxostudio', img) if cv2.waitKey(5) == ord('q'): break # 按下 q 鍵停止 cap.release() cv2.destroyAllWindows() ``` ::: ### 參考資料 [安裝 TensorFlow GPU](https://hackmd.io/@0p3Xnj8xQ66lEl0EHA_2RQ/Skoa_phv) [骨骼分析範例程式](https://colab.research.google.com/drive/19txHpN8exWhstO6WVkfmYYVC6uug_oVR) [MediaPipe Pose](https://google.github.io/mediapipe/solutions/pose.html) ## 遠端控制、室內定位、事件列表 ### 使用 MQTT 通訊協定系統 #### MQTT原理 ![](https://i.imgur.com/QANlExT.png) #### 概念圖總覽 ![](https://i.imgur.com/DY7M5RZ.jpg) #### Jeston Nano AI 板(PutYourHandsUp車)MQTT程式 :::spoiler 程式碼 ```python= import time import paho.mqtt.client as mqtt import paho.mqtt.publish as publish import _thread import serial StopMove = True print("loading") ##while True: ##localtime = time.localtime() ##result = time.strftime("%Y-%m-%d %I:%M:%S %p", localtime) def on_connect(client, userdata, flags, rc): print("Connected with result code " + str(rc)) # 每次連線之後,重新設定訂閱主題 client.subscribe([("Go", 0),("Jiong", 0),("Music", 0)]) def on_message(client, userdata, msg): global StopMove IP = "192.168.100.114" messin = int(msg.payload) if (msg.topic == "Go"): localtime = time.localtime() result = time.strftime("%Y-%m-%d %I:%M:%S %p", localtime) now = " 開始巡邏" result = result + now w_data='g' bytes = serial_port.write(w_data.encode()) print('g') publish.single( topic = "Activity", payload = result, hostname = IP, port = 1883, ) check = True #(check = true -> no find) (check = false -> find embarrasing face) elif (msg.topic == "Jiong"): if (StopMove == True ) : w_data="s" bytes = serial_port.write(w_data.encode()) localtime = time.localtime() result = time.strftime("%Y-%m-%d %I:%M:%S %p", localtime) now = " 找到囧臉" result = result + now print(result) #Stop publish.single( topic = "Activity", payload = result, hostname = IP, port = 1883, ) publish.single( topic = "AskDance", payload = "1", hostname = IP, port = 1883, ) publish.single( topic = "Eye", payload = "1", hostname = IP, port = 1883, ) StopMove = False #continue elif(msg.topic == "Music"): w_data="m" bytes = serial_port.write(w_data.encode()) print('m') # 建立 MQTT Client 物件 client = mqtt.Client() # 設定建立連線回呼函數 client.on_connect = on_connect # 設定接收訊息回呼函數 client.on_message = on_message # 設定登入帳號密碼(若無則可省略) # client.username_pw_set("myuser","mypassword") # 連線至 MQTT 伺服器(伺服器位址,連接埠) client.connect("192.168.100.114", 1883) # 進入無窮處理迴圈 ``` ::: #### 兩台車之間的通訊 :::spoiler 程式碼 ```python= while True: try: r_data = '1' w_data = '1' while True: # Write data to serial time.sleep(1) while True: # Read data from serial r_data = serial_port.read()//讀取來自matrix的訊息 if(str(r_data)!='b\'\\n\'' and str(r_data)!='b\'\\r\''): print("msg:"+str(r_data)) time.sleep(0.1) if(str(r_data)>='b\'1\'' and str(r_data)<='b\'7\'')://排除除了位子以外的data mq=int(r_data) print("mqtted"+str(r_data)) publish.single(//使用mqtt上傳 topic="CindyPlace", payload=mq,hostname="192.168.100.114",port=1883 ) time.sleep(1) ``` ::: ### 室內定位 #### 定位程式 ###### 使用類比光感測器確認區域 :::spoiler 程式碼 ```cpp=100 int d4 = Mini.D4.get(); Serial.print(lines); Serial.print("\n"); // 算走過黑線數量 if(d4 == 1) { if(last_line == 0) lines++; } last_line = d4; ``` ::: ###### 使用Uart(通用非同步收發傳輸器)程式碼 :::spoiler python 介面 ```python=83 def mqt(ka): client.loop_forever() _thread.start_new_thread(mqt,(0,)) serial_port = serial.Serial( port="/dev/ttyUSB0", baudrate=9600, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, ) while True: try: r_data = '1' w_data = '1' while True: # Write data to serial bytes = serial_port.write(w_data.encode()) time.sleep(1) while True: # Read data from serial r_data = serial_port.read() print("msg:"+str(r_data)) time.sleep(1) if(str(r_data)>='b\'1\'' and str(r_data)<='b\'7\''): mq=int(r_data) print("mqtted"+str(r_data)) publish.single( topic="HandsUpPlace", payload=mq,hostname="192.168.100.114",port=1883 ) print("Hiiiiiii") #except KeyboardInterrupt: #print("Exiting Program") #except Exception as exception_error: #print("Error occurred. Exiting Program") #print("Error: " + str(exception_error)) finally: serial_port.close() pass ``` ::: :::spoiler Arduino IDE 介面 & 巡邏定點 ```cpp=126 r_data = Serial.read(); // read() return integer, and read one byte from serial buffer and save to receivedData // st = start, sta = stop in anothor round if(r_data=='g'){ st=1; sta=0; Serial.println('g'); lines=1; } else if(r_data == 's'){ Serial.println('s'); st =0; } else if(r_data=='m'){ Serial.println('m'); st=1; sta=1; } r_data='n'; ``` ::: ### 手機應用程式&介面 #### 機器人眼睛介面&程式 ![](https://i.imgur.com/eTfJFU8.jpg) ![](https://i.imgur.com/mslypaE.png) #### 巡邏定位介面&巡邏定位程式 ![](https://i.imgur.com/ZuZ6JSL.png =400x500) ![](https://i.imgur.com/nXc1x5L.png) #### 活動紀錄介面&程式 ![](https://i.imgur.com/Xd3MXHb.png =400x500) ![](https://i.imgur.com/MV8Bxl0.png) ### 音樂&音訊播放 #### 程式碼 :::spoiler 音樂程式 ```python= import pygame import time import paho.mqtt.client as mqtt import paho.mqtt.publish as publish pygame.init() print("loading") def on_connect(client, userdata, flags, rc): print("Connected with result code " + str(rc)) # 每次連線之後,重新設定訂閱主題 client.subscribe("Music") def on_message(client, userdata, msg): messin = int(msg.payload) IP = "192.168.100.114" if messin == 1: localtime = time.localtime() result = time.strftime("%Y-%m-%d %I:%M:%S %p", localtime) now = " 開始跳舞" result = result + now publish.single( topic="Activity", payload = result, hostname = IP, port=1883, ) print("YES") past = 0 times = 2 pygame.mixer.music.load("./LoveYou.mp3") pygame.mixer.music.play(loops=0, start = 12.3) while True: #print(pygame.mixer.music.get_pos()) if(pygame.mixer.music.get_pos() == 80000): pygame.mixer.music.stop() if(pygame.mixer.music.get_pos() - past == 550): #print(pygame.mixer.music.get_pos()) if(times == 1): publish.single( topic="Beat", payload="1", hostname = IP, port=1883, ) if(times == 4): times = 0 times += 1 past = pygame.mixer.music.get_pos() else : print("No") while pygame.mixer.music.get_busy == True: #or interupt: continue # 建立 MQTT Client 物件 client = mqtt.Client() # 設定建立連線回呼函數 client.on_connect = on_connect # 設定接收訊息回呼函數 client.on_message = on_message # 設定登入帳號密碼(若無則可省略) # client.username_pw_set("myuser","mypassword") # 連線至 MQTT 伺服器(伺服器位址,連接埠) client.connect("192.168.100.114", 1883) # 進入無窮處理迴圈 client.loop_forever() ``` ::: :::spoiler 音訊播放 ```python= import pygame import time import paho.mqtt.client as mqtt import paho.mqtt.publish as publish pygame.init() print("loading") def on_connect(client, userdata, flags, rc): print("Connected with result code " + str(rc)) # 每次連線之後,重新設定訂閱主題 client.subscribe("AskDance") def on_message(client, userdata, msg): messin = int(msg.payload) IP = "192.168.100.114" if messin == 1: print("YES") past = 0 times = 2 pygame.mixer.music.load("./Ask.mp3") pygame.mixer.music.play(loops=0, start = 0.0) while True: past = pygame.mixer.music.get_pos() else : print("No") while pygame.mixer.music.get_busy == True: #or interupt: continue # 建立 MQTT Client 物件 client = mqtt.Client() # 設定建立連線回呼函數 client.on_connect = on_connect # 設定接收訊息回呼函數 client.on_message = on_message # 設定登入帳號密碼(若無則可省略) # client.username_pw_set("myuser","mypassword") # 連線至 MQTT 伺服器(伺服器位址,連接埠) client.connect("192.168.100.114", 1883) # 進入無窮處理迴圈 client.loop_forever() ``` ::: ### 參考資料 [安裝 Mosquitto MQTT Broker](https://oranwind.org/post-post-2/) [物聯網基礎傳輸協議 - MQTT](https://ithelp.ithome.com.tw/articles/10224407) [Paho Python MQTT Client Subscribe With Examples](http://www.steves-internet-guide.com/subscribing-topics-mqtt-client/) [新增或刪除清單顯示器的項目](https://www.omdte.com/app-inventor%E5%AD%B8%E7%BF%92%E8%A8%98%E9%8C%8462%EF%BC%8C%E6%96%B0%E5%A2%9E%E6%88%96%E5%88%AA%E9%99%A4%E6%B8%85%E5%96%AE%E9%A1%AF%E7%A4%BA%E5%99%A8%E7%9A%84%E9%A0%85%E7%9B%AE%EF%BC%8C%E5%81%9A/) [pygame.mixer.music](https://www.pygame.org/docs/ref/music.html) ## 其他參考來源 & 除錯技術 [NVIDIA Jetson Nano — 環境安裝](https://d246810g2000.medium.com/nvidia-jetson-nano-for-jetpack-4-4-01-%E7%92%B0%E5%A2%83%E5%AE%89%E8%A3%9D-fd48d5658a13) [日常筆記(除錯/安裝/終端機指令/查 Jetson Nano 電量)](https://www.evernote.com/shard/s485/sh/10072c6a-03dc-670c-eb30-42848fdb2004/cc708cd05b300bf8bd415f917ac54fa8) ## 應用網站 [Teachable Machine 訓練網站](https://teachablemachine.withgoogle.com/) [App inventor2](http://appinventor.mit.edu/) **本文件 QR Code** ![](https://i.imgur.com/TlB6SN1.png =300x300) **日常筆記 QR Code** ![](https://i.imgur.com/chqK4My.png =300x300)