```python
import cv2
import numpy as np
import imutils
import serial
import time
from Uarm import Uarm
#機械手臂連接設定
arm = Uarm('/dev/ttyACM0', debug=True)
arm.speed = 7000
ser = serial.Serial("/dev/ttyACM0",115200,timeout=1.0)
time.sleep(2)
#機械手臂初始預設位置
```python
```ser.write("G0 X150 Y-100 Z50 F2000000\n".encode())
def
```python
``` displayIMG(frame, windowName):
cv2.namedWindow( windowName, cv2.WINDOW_NORMAL )
cv2.resizeWindow(windowName, 600, 600)
cv2.imshow(windowName, frame)
cap = cv2.VideoCapture(0)
while(True):
ret,frame = cap.read()
#灰階
gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
#高斯模糊
blurred = cv2.GaussianBlur(gray,(5, 5), 0)
# 邊緣偵測
canny = cv2.Canny(blurred, 30, 150)
#二直化
ret1,thresh = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY)
#輪廓檢測
thresh1, contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
#輪廓面積
# area = cv2.contourArea(cnts)
#輪廓週長
#perimeter = cv2.arcLength(cnts,True)
#顯示幾個物件
#物件中心
# cnt = cv2.findContours(frame.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
_,cnt, hierarchy= cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE)
print("有 {} 個物件".format(len(cnt)))
if(len(cnt)>0):
print("false")
else:
print("true")
coins = frame.copy()
cv2.drawContours(coins, cnt, -1, (0, 255, 0), 2)
clone = frame.copy()
cv2.drawContours(clone, cnt, -1, (0, 255, 0), 2)
imgall = blurred.copy()
for cnt in cnt:
M = cv2.moments(cnt)
# imgnew = cv2.drawContours(frame, contours, -1, (0,255,0), 3)
if M["m00"] != 0:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
else:
# set values as what you need in the situation
cX, cY = 0, 0
cv2.circle(frame,(cX,cY),3,(0,255,0),3)
frame = cv2.drawContours(frame, contours, -1, (0, 255, 0), 2)
cv2.circle(thresh,(cX,cY),3,(0,255,0),3)
thresh = cv2.drawContours(thresh, contours, -1, (0, 255, 0), 2)
cv2.circle(canny,(cX,cY),3,(255,255,255),3)
canny = cv2.drawContours(canny, contours, -1, (0, 255, 0), 2)
area = cv2.contourArea(cnt)
perimeter = cv2.arcLength(cnt,False)
#epsilon = 0.02*perimeter
#approx = cv2.approxPolyDP(cnt,epsilon,True)
#imgnew1 = cv2.drawContours(frame, approx, -1 (0,0,255), 3)
cv2.imshow("frame",canny)
# cv2.imshow("frame1",thresh)
cv2.imshow("frame2",imgnew)
cv2.imshow('contours',frame)
#if cv2.waitKey(1) & 0xFF == ord('q'):
# break
#機械手臂動作
ser.write("M2231 V1\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2201 R129\n".encode())
time.sleep(2)
ser.write("G2205 S60\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("M2231 V0\n".encode())
time.sleep(2)
ser.write("G2201 H37\n".encode())
time.sleep(2)
ser.write("G0 X150 Y-100 Z50 F2000000\n".encode()) #TWO
time.sleep(2)
ser.write("M2231 V1\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2201 R129\n".encode())
time.sleep(2)
ser.write("G2201 H60\n".encode())
time.sleep(2)
ser.write("G2205 S60\n".encode())
time.sleep(2)
ser.write("M2231 V0\n".encode())
time.sleep(2)
ser.write("G2201 H67\n".encode())
time.sleep(2)
ser.write("G0 X150 Y-100 Z50 F2000000\n".encode()) #THREE
time.sleep(2)
ser.write("M2231 V1\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2201 R125\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2205 S31\n".encode())
time.sleep(2)
ser.write("M2231 V0\n".encode())
time.sleep(2)
ser.write("G2201 H37\n".encode())
time.sleep(2)
ser.write("G0 X150 Y-100 Z50 F2000000\n".encode()) #FOUR
time.sleep(2)
ser.write("M2231 V1\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2201 H65\n".encode())
time.sleep(2)
ser.write("G2201 R125\n".encode())
time.sleep(2)
ser.write("G2205 S32\n".encode())
time.sleep(2)
ser.write("G2201 H62\n".encode())
time.sleep(2)
ser.write("M2231 V0\n".encode())
time.sleep(2)
ser.write("G2201 H67\n".encode())
time.sleep(2)
ser.write("G0 X150 Y-100 Z50 F2000000\n".encode()) #FIVE
time.sleep(2)
ser.write("M2231 V1\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2201 R121\n".encode())
time.sleep(2)
ser.write("G2201 H88\n".encode())
time.sleep(2)
ser.write("G2205 S85\n".encode())
time.sleep(2)
ser.write("G2202 N3 V84\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("M2231 V0\n".encode())
time.sleep(2)
ser.write("G2201 H37\n".encode())
time.sleep(2)
ser.write("G2202 N3 V90\n".encode())
time.sleep(2)
ser.write("G0 X150 Y-100 Z50 F2000000\n".encode()) #SIX
time.sleep(2)
ser.write("M2231 V1\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2201 R121\n".encode())
time.sleep(2)
ser.write("G2201 H90\n".encode())
time.sleep(2)
ser.write("G2205 S85\n".encode())
time.sleep(2)
ser.write("G2202 N3 V84\n".encode())
time.sleep(2)
ser.write("G2201 H61\n".encode())
time.sleep(2)
ser.write("M2231 V0\n".encode())
time.sleep(2)
ser.write("G2201 H85\n".encode())
time.sleep(2)
ser.write("G2202 N3 V90\n".encode())
time.sleep(2)
ser.write("G0 X150 Y-100 Z50 F2000000\n".encode()) #Seven
time.sleep(2)
ser.write("M2231 V1\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2201 R116\n".encode())
time.sleep(2)
ser.write("G2201 H90\n".encode())
time.sleep(2)
ser.write("G2205 S57\n".encode())
time.sleep(2)
ser.write("G2202 N3 V84\n".encode())
time.sleep(2)
ser.write("G2201 H32\n".encode())
time.sleep(2)
ser.write("M2231 V0\n".encode())
time.sleep(2)
ser.write("G2201 H37\n".encode())
time.sleep(2)
ser.write("G2202 N3 V90\n".encode())
time.sleep(2)
ser.write("G0 X150 Y-100 Z50 F2000000\n".encode()) #eight
time.sleep(2)
ser.write("M2231 V1\n".encode())
time.sleep(2)
ser.write("G2201 H30\n".encode())
time.sleep(2)
ser.write("G2201 H90\n".encode())
time.sleep(2)
ser.write("G2201 R116\n".encode())
time.sleep(2)
ser.write("G2205 S57\n".encode())
time.sleep(2)
ser.write("G2202 N3 V84\n".encode())
time.sleep(2)
ser.write("G2201 H61\n".encode())
time.sleep(2)
ser.write("M2231 V0\n".encode())
time.sleep(2)
ser.write("G2201 H67\n".encode())
time.sleep(2)
ser.write("G2202 N3 V90\n".encode())
time.sleep(2)
ser.write("G0 X150 Y-100 Z50 F2000000\n".encode())
time.sleep(2)
cap.release()
cv2.waitkey(1)
cv2.destroyAllWindows()
以上程序码是python3 opencv 的部份
下面这部份的程序码是用于机械手臂转角度的程序码
try:
import serial # pyserial
import time
import _thread
except ImportError:
print("Some libraries are needed. Run \"pip3 install -r requirements.txt\" to install them.")
quit()
class Uarm:
""" Library for Uarm Swift Pro"""
MAX_SPEED = 7000 # TODO findout max speed
DEF_SPEED = 5000 # Default speed
debug = False
switch = False # Flag for endeffector switch
connected = False
ack = False
response = b'ok \r\n'
def __init__(self, port, baudrate=115200, debug=False):
self.debug = debug
self.__index = 0
self.__speed = self.DEF_SPEED
try:
self.ser = serial.Serial(port, baudrate, timeout=1)
except:
print("ERROR - Couldn't open {} port".format(port))
quit()
try:
_thread.start_new_thread(self._serialcheckthread, (0.05,))
except:
print("Error: unable to start thread")
quit()
while not self.connected:
time.sleep(0.1)
time.sleep(1) # Sometimes it reports more initial stuff
self.switch = self.ack = False
# self.waitfor('@5') # Report event of power connection
# time.sleep(1)
# while self.ser.inWaiting() > 0:
# line = self.ser.readline()
# if (self.debug): print("DEBUG - ", line)
# if line.startswith('@5'.encode('utf-8')):
# print("@5")
# time.sleep(0.1)
# time.sleep(1)
print("uArm connected to {}".format(port))
self.sendraw("M2213 V0") # default buttons false
# TODO check types and ranges
@property
def speed(self):
return self.__speed
@speed.setter
def speed(self, value):
_value = int(value)
if _value <= 0:
# raise ValueError("Only positive non zero speed values")
print("WARNING - Only positive non zero speed values")
elif _value > self.MAX_SPEED:
print("WARNING - value exceeds max speed ", self.MAX_SPEED)
else:
self.__speed = _value
@property
def index(self):
""" Get the actual index """
return self.__index
# Public Methods ==================================================================
# receive thread
def _serialcheckthread(self, delay):
while True:
if self.ser.inWaiting() > 0:
self.response = self.ser.readline()
if self.debug: print("DEBUG - response: ", self.response)
if self.response.startswith('ok'.encode('utf-8')):
self.ack = True
elif self.response.startswith('@6'.encode('utf-8')):
self.switch = True
elif self.response.startswith('E22'.encode('utf-8')):
print("ERROR - Point not reachable")
self.ack = True
elif self.response.startswith('@5'.encode('utf-8')):
self.connected = True
else:
time.sleep(delay)
def sendraw(self, string):
""" Send a raw GCode string """
self.ack = False
if type(string) != str:
print("ERROR - argument {} must be a str.".format(type(string)))
return
string += '\n'
self.ser.write(string.encode('utf-8'))
if self.debug: print("DEBUG - gcode sent: " + string, end='')
while not self.ack:
time.sleep(0.05)
return True
def move(self, x, y, z, speed=None):
""" Move the arm to an absolute x, y, z position """
if speed is None:
speed = self.speed
self.sendraw("G0 X{} Y{} Z{} F{}".format(x, y, z, speed)) # Relative displacement
return True
def moverel(self, x, y, z, speed=None):
""" Move the arm to a relative x, y z position """
if speed is None:
speed = self.speed
self.sendraw("G2204 X{} Y{} Z{} F{}".format(x, y, z, speed)) # Relative displacement
return True
def pause(self, seconds):
# milisec = int(seconds) * 1000
# self.sendraw("G2004 P{}".format(milisec)) # Pause
if self.debug: print("DEBUG - pause {} seconds".format(seconds))
time.sleep(seconds)
return True
def gripper(self, active):
if active:
var = 1
else:
var = 0
self.sendraw("M2232 V{}".format(var)) # Gripper open/close
return True
def wrist(self, angle):
""" Set wist servo to a specific angle"""
self.sendraw("G2202 N3 V{}".format(angle))
return True
def attach_all(self, truefalse):
"""Energize / Deenergize all the motors"""
if truefalse:
self.sendraw("M17")
else:
self.sendraw("M2019")
def report_pos(self, seconds):
""" Reports arm position every x seconds
format of response: @3 X154.71 Y194.91 Z10.21"""
if seconds==False or seconds<0:
seconds = 0
self.sendraw("M2120 V{}".format(seconds))
# def waitfor(self, string):
# delay = 0.05
# for i in range(int(60/delay)): # wait for 10 seconds
# if self.response.startswith(string.encode('utf-8')):
# #if self.response[0:2] == b'ok':
# break
# time.sleep(delay)
# else:
# print ("ERROR - No aknoledge receive from arm")
# self.close()
# quit()
# self.response=b'' # clear response
def close(self):
""" Close serial connexion and release the arm"""
self.ser.close() # close port
# Private Methods ==================================================================
def check_reachable(self, x, y, z):
# TODO check if reachable
return True
"我们最终的目标是使用摄像头侦测物件的位置然后机械手臂去直接吸取的动作从左边抓到右边,在进行堆叠成2x2的体积"
这是我们机械手臂的程序码,目前的问题是执行的时候 摄像头的视窗跑出来是当机的像下面的图案,然後關不掉。
希望有大大可以帮我们解决问