自由研究 コップ運びロボット
目次
ドリンクサーバーの紹介
今年の自由研究は「コップ運びロボット」を作りました。
コップ運びロボットというのはドリンクサーバーの追加の機能です。
ドリンクサーバーのことを知らない人は下の動画を見てください。
これまでの改良などの動画は下のリンクにあります。
https://www.youtube.com/watch?v=UJqAMmcaHjM&list=PLaChPUIX_gwwlCS_xhRd8alEuN9CmGqdu
コップ運びロボットの紹介
自由研究で作ったコップ運びロボットなのですが、このブログで1回やったことがあります。それをもとにして、作りました。
前のブログに書いたraspberrypiで作りたいはまだ実現できていないのですがレゴで作ったコップ運びロボットを進化させました。
進化したコップ運びロボット
では、進化したコップ運びロボットを見ていきましょう。
まず、追加された機能の一覧を見ていきましょう。
主に次の4つの機能を追加しました。
- コップを機械学習で見つける機能
- ドリンクサーバーから制御できる機能
- ロボットを90度以上回転させる機能
- コップを片付ける機能
3つ目のロボットを90度以上回転させる機能というのは、ロボットがプログラムの問題で、90度以上回転しなかったので、付け加えた機能です。
そして、改良された機能として、ドリンクサーバーの目印にARマーカーを使えるようにしました。
ARマーカーとは下の写真のようなもので、本来は現実世界にキャラクターを表示させたりするための目印です。下の写真のようなものだけでなくいろいろながらのものがあります。
詳しい説明は後でするので、コップ運びロボットの本体の説明をします。
下の写真がコップ運びロボットの本体です。黒い紙がついている以外はあまり変わりません。
よくみるとセンサーが一つ追加されています。
この黒い紙は機械学習での誤認識を減らすためのものです。
では早速動いている様子を見ていきましょう。
動画の右上や左下に出ているのがパソコン上の画面です。
これを見て、わかるように少しずつ角度を調節しています。
途中でパソコンの画面が止まるのは、ロボットの動きが終わるまで待っているからです。
実行中のパソコンの画面はこんな感じです。
左上がコップ運びロボットの画面、左下がドリンクサーバーの画面、
右側に映っている画面が上から見た映像です。
コップ運びロボットにはいろいろと命令が来ています。
ドリンクサーバーのほうは、音声認識の稼働中の画面です。
プログラムのことについて
ここから先はプログラムのことについて、詳しく書こうと思います。
まず初めに、下の図がコップ運びロボットのフローチャートです。
見えにくかったら、クリックして拡大してください。
簡単に言うと、下の3つを繰り返しています。
- コップ、色のカード、ARマーカーの座標を取得
- ロボットとコップの角度を調べる
- ロボットに命令を送る
この3つにいろいろな機能を付け加えています。
追加された機能と改良された機能を一つずつ見ていきましょう
追加・改良された機能
コップを機械学習で認識する機能
コップを機械学習で見つける機能というのはOpencvというライブラリーを使って学習させました。
学習させたデータは正解データが550枚、不正解データーが430枚で学習させました。
精度を考えると正解データが7000枚、不正解データが5000枚必要だそうです。
下の画像が、学習中の画像と、学習させた正解データの画像です。
この機械学習（Haar-Like）では、周りとの明るさの差から、コップを認識しています。
一つずつ、コップの映っている座標を指定して、学習させます。
不正解データはコップが入らないようにランダムに切り抜いて作っています。
この作業が一番大変でした。
ドリンクサーバーから制御できる機能
ドリンクサーバーでは、精度を高めるために、特定の単語だけを認識するようになっています。
この、特定の単語に「コップ持ってきて」と「コップ片付けて」の言葉を追加して認識するようにしています。
言葉が認識されるとドリンクサーバーの関数が呼び出されます。
その関数を一つ追加して、その関数でパソコンと通信しています。
このコップ運びロボットはドリンクサーバーとロボットだけで動いているのではなく、ドリンクサーバーからパソコンに指示が出されて、パソコンが処理を実行して、ドリンクサーバーに指示を出しています。
通信にはsocket通信という通信方法を使っていて、wifi経由で指示を送っています。
socket通信は音声認識から、メインプログラムへの通信にも使われていて、このような場合にはwifiを経由せずにパソコン内だけで通信しています。
ロボットを90度以上回転させる機能
この機能は、角度を計算する機能にコップがロボットの前後のどちらにあるかを計算する機能を追加することで、できました。
前の、角度を計算する機能は、コップの位置がロボットの前後のどちらにあるかがわからなかったので、少しずつ、曲がりながら、調節していました。
コップがロボットの前後どちらにあるかを計算する機能を追加したことで、一回で180回れるようになりました。
角度を求める式をいろいろと変えて、cosをsinに変えることで実現できました。この部分は、仕組みや意味は分からないので知りたい人は調べてみてください。
コップを片付ける機能
この機能は、ただドリンクサーバーに行くところの行き先を別のARマーカーに置き換えただけです。
ARマーカーにはIDを振ることができて、そのIDでドリンクサーバーか、片付ける場所かを判断できるようになっています。
この機能はドリンクサーバーに向かうプログラムのコピーを作ってそのプログラムの行き先を変えています。
なので、ドリンクサーバーにコップを持っていくプログラムと片づけるプログラムは全く別のプログラムとして、実行されます。
目印にARマーカーを使えるようにする機能
これは改良された機能ですが、改良した理由として、前に使っていたカタツムリのカードは認識の精度が悪く、少し傾くと認識できなくなって今います。そのため、ARマーカーを使っています。
ARマーカーは少し隠れると、認識できなくなるので、ロボットを手前で止めて、そこからは、机の色がARマーカーの下に貼ってある紙の色のピンク色になったら、コップを置くようにしています。そのためにセンサーが一つ追加されています。
プログラムの全文
この下にはプログラムの全文を貼っておきます。
主に下の3つのプログラムで作られています。
コップを持ってくるプログラムと片づけるプログラムはほぼ同じです。
コメント（「#」から始める行）を読むと大体何をしているかがわかると思います。
プログラムに興味がない人はまとめまで飛ばしてください
コップをドリンクサーバーに持っていくプログラム
#モジュールのインポート import cv2 import numpy as np import time import socket import math #それぞれ 青、緑、ピンク のHSVの色の上限、下限を指定。 LOW_COLOR_BLUE = np.array([100, 75, 75]) HIGH_COLOR_BLUE = np.array([140, 255, 255]) LOW_COLOR_GREEN = np.array([30, 64, 0]) HIGH_COLOR_GREEN = np.array([90,255,255]) AREA_RATIO_THRESHOLD = 0.005 #大きさの閾値 def find_specific_color(frame,AREA_RATIO_THRESHOLD,LOW_COLOR,HIGH_COLOR): #色を検出する関数 # 高さ，幅，チャンネル数 h,w,c = frame.shape # hsv色空間に変換 hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV) # 色を抽出する ex_img = cv2.inRange(hsv,LOW_COLOR,HIGH_COLOR) # 輪郭抽出 contours,hierarchy = cv2.findContours(ex_img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) # 面積を計算 areas = np.array(list(map(cv2.contourArea,contours))) if len(areas) == 0 or np.max(areas) / (h*w) < AREA_RATIO_THRESHOLD: # 見つからなかったらFalseを返す return False else: # 面積が最大の塊の重心を計算し返す max_idx = np.argmax(areas) max_area = areas[max_idx] result = cv2.moments(contours[max_idx]) x = int(result["m10"]/result["m00"]) y = int(result["m01"]/result["m00"]) return (x,y) #ここら辺はよく理解できない #目的地とロボットとの角度を出す関数 def calcAngle(V1,V2): V1size = np.sqrt(V1[0] ** 2 + V1[1] ** 2) V2size = np.sqrt(V2[0] ** 2 + V2[1] ** 2) if V1size == 0 or V2size == 0: #0ベクトルエラー return False #内積を求める inner = (V1[0] * V2[0]) + (V1[1] * V2[1]) cos = inner / (V1size * V2size) rad = math.acos(cos) return rad #目的地がロボットの前にあるか後ろにあるかを判断するための関数 def calc_sin(V1,V2): V1size = np.sqrt(V1[0] ** 2 + V1[1] ** 2) V2size = np.sqrt(V2[0] ** 2 + V2[1] ** 2) if V1size == 0 or V2size == 0: return False inner = np.cross(V1,V2) sin = inner / (V1size * V2size) rad = math.asin(sin) return rad #ロボットにメッセージを送るための関数 def communication(data): with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: # ロボットのIPアドレスを指定 s.connect(('192.168.1.111', 50007)) #ロボットにメッセージを送信 s.sendall(data.encode()) #ARマーカーを読み込むための準備 aruco = cv2.aruco dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) def AR_marker(frame): #ARマーカーを認識する関数 corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, dictionary) #マーカを検出 pos_img = False if len(corners) > 0: for i in range(len(corners)): if ids[i] == 0: x = int(corners[i][0][0][0] + (corners[i][0][2][0] - corners[i][0][0][0]) / 2) y = int(corners[i][0][0][1] + (corners[i][0][2][1] - corners[i][0][0][1]) / 2 pos_img = (x,y) return pos_img f_c = 0 cascade = cv2.CascadeClassifier('AI/cascade.xml') x_c = 0 y_c = 0 def cup_pos(frame): global f_c, x_c, y_c cup_pos = False gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) cup = cascade.detectMultiScale(gray, scaleFactor=1.1, minNeighbors=1, minSize=(50, 50), maxSize=(60,60)) if len(cup) != 0: if (x_c - cup[0][0]) >= -20 and (x_c - cup[0][0]) <= 20 and (y_c - cup[0][1]) >= -20 and (y_c - cup[0][1]) <= 20: f_c += 1 else: f_c = 0 x_c = cup[0][0] y_c = cup[0][1] else: f_c = 0 if f_c > 10: for (x_c,y_c,w_c,h_c) in cup: cup_pos = (int(x_c+(w_c/2)), int(y_c+(h_c/2))) return cup_pos def main(): # webカメラを扱うオブジェクトを取得 cap = cv2.VideoCapture(0) #変数の初期値を指定 c = 1 t = 0 purpose = 0 while True: ret,frame = cap.read() if ret is False: #カメラに接続できなかった時の処理 print("cannot read image") continue pos_cup = cup_pos(frame) # 色のカードの位置を取得 pos_blue = find_specific_color( frame, AREA_RATIO_THRESHOLD, LOW_COLOR_BLUE, HIGH_COLOR_BLUE ) pos_green = find_specific_color( frame, AREA_RATIO_THRESHOLD, LOW_COLOR_GREEN, HIGH_COLOR_GREEN ) if pos_cup != False: cv2.circle(frame,pos_cup,10,(0,0,255),-1) pos_img = AR_marker(frame)#ARマーカーの位置を取得 #コップをとりに行っている場合 if purpose == 0: if pos_blue and pos_green and pos_cup is not False: #青、緑の色とコップがすべて認識できた場合 c = 1 # 画面上に表示するために、円や線を表示 cv2.circle(frame,pos_blue,10,(0,0,255),-1) cv2.circle(frame,pos_green,10,(0,0,255),-1) x = int(((pos_green[0] - pos_blue[0]) / 2) + pos_blue[0]) y = int(((pos_green[1] - pos_blue[1]) / 2) + pos_blue[1]) cv2.circle(frame,(x,y),10,(0,0,255),-1) cv2.line(frame, pos_blue, pos_green, (0, 255, 0)) cv2.line(frame, pos_cup, (x,y), (0, 255, 0)) #ロボットとコップの位置を計算 O = [x,y] P = list(pos_blue) Q = list(pos_cup) #ベクトル V1 = [P[0]-O[0],P[1]-O[1]] V2 = [Q[0]-O[0],Q[1]-O[1]] rad = calcAngle(V1,V2)#角度の計算 sin = calc_sin(V1,V2)#コップがロボットの前にあるか後ろにあるかを数値で出す #ラジアンを角度に変換 degree = math.degrees(rad) if sin > 0: #後ろにある場合は360どの角度を出す。 degree = 360 - degree degree = degree - 90 #コップをロボットが垂直になる角度を計算。 if degree < 0: #角度が0度未満の場合は右に曲がる r = "r " degree = degree * -1 #角度がマイナスにならないようにする else: #0以上の場合は左に曲がる。 r = "l " if (time.time() - t) >= 4: #4秒に1回実行 communication("4")#止まる communication(r + str(int(degree))) #角度を送信 t = time.time()#時間をリセット time.sleep(2) #2秒待っているのでじっさいには4-2で2秒に1回角度を変える communication("0") #前に進む #コップまでの距離を計算 a = (x - pos_cup[0]) ** 2 b = (y - pos_cup[1]) ** 2 if (a + b) < 30000 and (a + b) > 10000: #近づいたら超音波センサーを使ってコップを拾う communication("4") #止まる communication("8") #超音波センサーで近づく time.sleep(3) #近づくまで待つ communication("5") #持ち上げる time.sleep(6) #持ち上がるまで待つ purpose = 1 #ARマーカーに向かう処理に変更 else: #ロボットやコップを見失った場合 if c == 1: communication("4") #ロボットを止める c = 0 #ARマーカーに向かう処理 if purpose == 1: if pos_blue and pos_green and pos_img is not False: #青、緑の色とARマーカーがすべて認識できた場合 c = 1 # 画面上に表示するために、円や線を表示 cv2.circle(frame,pos_blue,10,(0,0,255),-1) cv2.circle(frame,pos_green,10,(0,0,255),-1) x = int(((pos_green[0] - pos_blue[0]) / 2) + pos_blue[0]) y = int(((pos_green[1] - pos_blue[1]) / 2) + pos_blue[1]) cv2.circle(frame,(x,y),10,(0,0,255),-1) cv2.line(frame, pos_blue, pos_green, (0, 255, 0)) cv2.circle(frame,pos_img,10,(0,0,255),-1) cv2.line(frame, pos_img, (x,y), (0, 255, 0)) #ロボットとARマーカーを計算 O = [x,y] P = list(pos_blue) Q = list(pos_img) #ベクトル V1 = [P[0]-O[0],P[1]-O[1]] V2 = [Q[0]-O[0],Q[1]-O[1]] #角度の計算 rad = calcAngle(V1,V2)#角度の計算 sin = calc_sin(V1,V2)#ARマーカーがロボットの前にあるか後ろにあるかを数値で出す #ラジアンを角度に変換 degree = math.degrees(rad) if sin > 0: #後ろにある場合は360どの角度を出す。 degree = 360 - degree degree = 90 - degree #ARマーカーをロボットが垂直になる角度を計算。 if degree < 0: #角度が0度超過の場合はひだりに曲がる r = "l " degree = degree * -1 #角度がマイナスにならないようにする else:#0以下の場合は右に曲がる。 r = "r " if (time.time() - t) >= 5:#5秒に1回実行 communication("4") #止まる communication(r + str(int(degree))) #角度を送信 t = time.time() #時間をリセット time.sleep(3)#3秒待っているのでじっさいには5-3で2秒に1回角度を変える communication("0")#前に進む #ARマーカーまでの距離を計算 a = (x - pos_img[0]) ** 2 b = (y - pos_img[1]) ** 2 if (a + b) < 50000: #近づいたらコップを置く communication("9") time.sleep(3) communication("1") time.sleep(2) communication("4") communication("6") #コップを置く time.sleep(4) #置くのを待つ #1秒後ろに下がる communication("1") time.sleep(1) communication("4") break #終わる else: #ロボットやARマーカーを見失った場合 if c == 1: communication("4") #止める c = 0 # 画面に表示する cv2.imshow('frame',frame) # キーボード入力待ち key = cv2.waitKey(1) & 0xFF # qが押された場合は終了する if key == ord('q'): break cv2.destroyAllWindows() #ウィンドウを閉じる if __name__ == '__main__': main()
コップを片付けるプログラム
#モジュールのインポート import cv2 import numpy as np import time import socket import math #それぞれ 青、緑、ピンク のHSVの色の上限、下限を指定。 LOW_COLOR_BLUE = np.array([100, 75, 75]) HIGH_COLOR_BLUE = np.array([140, 255, 255]) LOW_COLOR_GREEN = np.array([30, 64, 0]) HIGH_COLOR_GREEN = np.array([90,255,255]) LOW_COLOR_PINK = np.array([150, 100, 100]) HIGH_COLOR_PINK = np.array([188,255,255]) AREA_RATIO_THRESHOLD = 0.005 #大きさの閾値 def find_specific_color(frame,AREA_RATIO_THRESHOLD,LOW_COLOR,HIGH_COLOR): #色を検出する関数 # 高さ，幅，チャンネル数 h,w,c = frame.shape # hsv色空間に変換 hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV) # 色を抽出する ex_img = cv2.inRange(hsv,LOW_COLOR,HIGH_COLOR) # 輪郭抽出 contours,hierarchy = cv2.findContours(ex_img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) # 面積を計算 areas = np.array(list(map(cv2.contourArea,contours))) if len(areas) == 0 or np.max(areas) / (h*w) < AREA_RATIO_THRESHOLD: # 見つからなかったらFalseを返す return False else: # 面積が最大の塊の重心を計算し返す max_idx = np.argmax(areas) max_area = areas[max_idx] result = cv2.moments(contours[max_idx]) x = int(result["m10"]/result["m00"]) y = int(result["m01"]/result["m00"]) return (x,y) #ここら辺はよく理解できない #目的地とロボットとの角度を出す関数 def calcAngle(V1,V2): V1size = np.sqrt(V1[0] ** 2 + V1[1] ** 2) V2size = np.sqrt(V2[0] ** 2 + V2[1] ** 2) if V1size == 0 or V2size == 0: #0ベクトルエラー return False #内積を求める inner = (V1[0] * V2[0]) + (V1[1] * V2[1]) cos = inner / (V1size * V2size) rad = math.acos(cos) return rad #目的地がロボットの前にあるか後ろにあるかを判断するための関数 def calc_sin(V1,V2): V1size = np.sqrt(V1[0] ** 2 + V1[1] ** 2) V2size = np.sqrt(V2[0] ** 2 + V2[1] ** 2) if V1size == 0 or V2size == 0: return False inner = np.cross(V1,V2) sin = inner / (V1size * V2size) rad = math.asin(sin) return rad #ロボットにメッセージを送るための関数 def communication(data): with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: # ロボットのIPアドレスを指定 s.connect(('192.168.1.111', 50007)) #ロボットにメッセージを送信 s.sendall(data.encode()) #ARマーカーを読み込むための準備 aruco = cv2.aruco dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) def AR_marker(frame): #ARマーカーを認識する関数 corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, dictionary) #マーカを検出 pos_img = False if len(corners) > 0: for i in range(len(corners)): if ids[i] == 1: x = int(corners[i][0][0][0] + (corners[i][0][2][0] - corners[i][0][0][0]) / 2) y = int(corners[i][0][0][1] + (corners[i][0][2][1] - corners[i][0][0][1]) / 2) pos_img = (x,y) return pos_img f_c = 0 cascade = cv2.CascadeClassifier('AI/cascade.xml') x_c = 0 y_c = 0 def cup_pos(frame): global f_c, x_c, y_c cup_pos = False gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) cup = cascade.detectMultiScale(gray, scaleFactor=1.1, minNeighbors=4, minSize=(50, 50), maxSize=(60,60)) if len(cup) != 0: if (x_c - cup[0][0]) >= -20 and (x_c - cup[0][0]) <= 20 and (y_c - cup[0][1]) >= -20 and (y_c - cup[0][1]) <= 20: f_c += 1 else: f_c = 0 x_c = cup[0][0] y_c = cup[0][1] else: f_c = 0 if f_c > 2: for (x_c,y_c,w_c,h_c) in cup: cup_pos = (int(x_c+(w_c/2)), int(y_c+(h_c/2))) return cup_pos def main(): # webカメラを扱うオブジェクトを取得 cap = cv2.VideoCapture(0) #変数の初期値を指定 c = 1 t = 0 purpose = 0 while True: ret,frame = cap.read() if ret is False: #カメラに接続できなかった時の処理 print("cannot read image") continue # 色のカードの位置を取得 pos_blue = find_specific_color( frame, AREA_RATIO_THRESHOLD, LOW_COLOR_BLUE, HIGH_COLOR_BLUE ) pos_green = find_specific_color( frame, AREA_RATIO_THRESHOLD, LOW_COLOR_GREEN, HIGH_COLOR_GREEN ) pos_cup = cup_pos(frame) pos_img = AR_marker(frame)#ARマーカーの位置を取得 #コップをとりに行っている場合 if purpose == 0: if pos_blue and pos_green and pos_cup is not False: #青、緑、ピンクの色がすべて認識できた場合 c = 1 # 画面上に表示するために、円や線を表示 cv2.circle(frame,pos_blue,10,(0,0,255),-1) cv2.circle(frame,pos_green,10,(0,0,255),-1) x = int(((pos_green[0] - pos_blue[0]) / 2) + pos_blue[0]) y = int(((pos_green[1] - pos_blue[1]) / 2) + pos_blue[1]) cv2.circle(frame,(x,y),10,(0,0,255),-1) cv2.line(frame, pos_blue, pos_green, (0, 255, 0)) cv2.circle(frame,pos_cup,10,(0,0,255),-1) cv2.line(frame, pos_cup, (x,y), (0, 255, 0)) #ロボットとコップの位置を計算 O = [x,y] P = list(pos_blue) Q = list(pos_cup) #ベクトル V1 = [P[0]-O[0],P[1]-O[1]] V2 = [Q[0]-O[0],Q[1]-O[1]] rad = calcAngle(V1,V2)#角度の計算 sin = calc_sin(V1,V2)#コップがロボットの前にあるか後ろにあるかを数値で出す #ラジアンを角度に変換 degree = math.degrees(rad) if sin > 0: #後ろにある場合は360どの角度を出す。 degree = 360 - degree degree = degree - 90 #コップをロボットが垂直になる角度を計算。 if degree < 0: #角度が0度未満の場合は右に曲がる r = "r " degree = degree * -1 #角度がマイナスにならないようにする else: #0以上の場合は左に曲がる。 r = "l " if (time.time() - t) >= 4: #4秒に1回実行 communication("4")#止まる communication(r + str(int(degree))) #角度を送信 t = time.time()#時間をリセット time.sleep(2) #2秒待っているのでじっさいには4-2で2秒に1回角度を変える communication("0") #前に進む #コップまでの距離を計算 a = (x - pos_cup[0]) ** 2 b = (y - pos_cup[1]) ** 2 if (a + b) < 30000 and (a + b) > 10000: #近づいたら超音波センサーを使ってコップを拾う communication("4") #止まる communication("8") #超音波センサーで近づく time.sleep(3) #近づくまで待つ communication("5") #持ち上げる time.sleep(6) #持ち上がるまで待つ purpose = 1 #ARマーカーに向かう処理に変更 else: #ロボットやコップを見失った場合 if c == 1: communication("4") #ロボットを止める c = 0 #ARマーカーに向かう処理 if purpose == 1: if pos_blue and pos_green and pos_img is not False: #青、緑の色とARマーカーがすべて認識できた場合 c = 1 # 画面上に表示するために、円や線を表示 cv2.circle(frame,pos_blue,10,(0,0,255),-1) cv2.circle(frame,pos_green,10,(0,0,255),-1) x = int(((pos_green[0] - pos_blue[0]) / 2) + pos_blue[0]) y = int(((pos_green[1] - pos_blue[1]) / 2) + pos_blue[1]) cv2.circle(frame,(x,y),10,(0,0,255),-1) cv2.line(frame, pos_blue, pos_green, (0, 255, 0)) cv2.circle(frame,pos_img,10,(0,0,255),-1) cv2.line(frame, pos_img, (x,y), (0, 255, 0)) #ロボットとARマーカーを計算 O = [x,y] P = list(pos_blue) Q = list(pos_img) #ベクトル V1 = [P[0]-O[0],P[1]-O[1]] V2 = [Q[0]-O[0],Q[1]-O[1]] #角度の計算 rad = calcAngle(V1,V2)#角度の計算 sin = calc_sin(V1,V2)#ARマーカーがロボットの前にあるか後ろにあるかを数値で出す #ラジアンを角度に変換 degree = math.degrees(rad) if sin > 0: #後ろにある場合は360どの角度を出す。 degree = 360 - degree degree = 90 - degree #ARマーカーをロボットが垂直になる角度を計算。 if degree < 0: #角度が0度超過の場合はひだりに曲がる r = "l " degree = degree * -1 #角度がマイナスにならないようにする else:#0以下の場合は右に曲がる。 r = "r " if (time.time() - t) >= 5:#5秒に1回実行 communication("4") #止まる communication(r + str(int(degree))) #角度を送信 t = time.time() #時間をリセット time.sleep(3)#3秒待っているのでじっさいには5-3で2秒に1回角度を変える communication("0")#前に進む #ARマーカーまでの距離を計算 a = (x - pos_img[0]) ** 2 b = (y - pos_img[1]) ** 2 if (a + b) < 50000: #近づいたらコップを置く communication("9") time.sleep(3) communication("1") time.sleep(2) communication("4") communication("6") #コップを置く time.sleep(4) #置くのを待つ #1秒後ろに下がる communication("1") time.sleep(1) communication("4") break #終わる else: #ロボットやARマーカーを見失った場合 if c == 1: communication("4") #止める c = 0 # 画面に表示する cv2.imshow('frame',frame) # キーボード入力待ち key = cv2.waitKey(1) & 0xFF # qが押された場合は終了する if key == ord('q'): break cv2.destroyAllWindows() #ウィンドウを閉じる if __name__ == "__main__": main()
ドリンクサーバーと通信をするプログラム
import take import clearup import socket # AF = IPv4 という意味 # TCP/IP の場合は、SOCK_STREAM を使う with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: # IPアドレスとポートを指定 s.bind(('ドリンクサーバーのIPアドレス', 50008)) # 1 接続 s.listen(1) # connection するまで待つ while True: # 誰かがアクセスしてきたら、コネクションとアドレスを入れる conn, addr = s.accept() with conn: while True: # データを受け取る data = conn.recv(1024).decode() if not data: break print(data) if data == "0": print("コップを持ってくる") take.main() elif data == "1": print("コップを片付ける") clearup.main()
まとめ
最初に思っていたよりも、精度が高いものができました。
しかし、動画でもわかる通り、絶対に自分で取りに行ったほうが速いです。
これは、ロボットの精度を高めるために遅くしています。
ぜひ、次作るときはRaspberrypiで作りたいです。
参考
https://tony-mooori.blogspot.com/2015/10/python_27.html http://ww38.kumajournal.com/coding/python/vec-degree/ ←無くなっていました https://qiita.com/__init__/items/5c89fa5b37b8c5ed32a4 https://qiita.com/sugman_a/items/1af4664521decebace8b https://premium.aidemy.net/magazine/entry/2018/05/21/210100 https://note.nkmk.me/python-opencv-draw-function/ https://www.pc-koubou.jp/magazine/19743
動画
上に書いたことと同じことを動画でも紹介しています。
