diff --git a/DouglasPeucker.py b/DouglasPeucker.py new file mode 100644 index 0000000..47076a7 --- /dev/null +++ b/DouglasPeucker.py @@ -0,0 +1,48 @@ +import numpy as np +import matplotlib.pyplot as plt + +def douglas_peucker(points, epsilon): + if len(points) <= 2: + return points + + # Find the point with the maximum distance + d_max = 0 + index = 0 + end = len(points) - 1 + + for i in range(1, end): + d = perpendicular_distance(points[i], points[0], points[end]) + if d > d_max: + index = i + d_max = d + + # If the maximum distance is greater than epsilon, recursively simplify + result = [] + if d_max > epsilon: + result += douglas_peucker(points[:index + 1], epsilon) + result += douglas_peucker(points[index:], epsilon) + else: + result = [points[0], points[end]] + + return result + +def perpendicular_distance(point, line_start, line_end): + # Calculate the perpendicular distance of a point to a line + return np.abs(np.cross(line_end - line_start, line_start - point)) / np.linalg.norm(line_end - line_start) + +# Example usage +# Create a set of points representing a line +original_points = np.array([[0, 0], [1, 1], [2, 3], [3, 1], [4, 0], [5, 1], [6, -1], [7, -3]]) +epsilon = 0.1 +simplified_points = np.array(douglas_peucker(original_points, epsilon)) + + +# Plot the original and simplified lines +plt.plot(original_points[:, 0], original_points[:, 1], label='Original Line') +simplified_points_array = np.array(simplified_points) +plt.plot(simplified_points_array[:, 0], simplified_points_array[:, 1], label='Simplified Line', linestyle='dashed') +plt.scatter(simplified_points[:, 0], simplified_points[:, 1], c='red', label='Simplified Points') + +plt.legend() +plt.title('Douglas-Peucker Line Simplification') +plt.show() diff --git a/coordinates_change.py b/coordinates_change.py index 24ea5f4..3d67c45 100644 --- a/coordinates_change.py +++ b/coordinates_change.py @@ -1,33 +1,49 @@ import numpy as np +import matplotlib.pyplot as plt -def coord(contours, x_offset, y_offset, x_scale, y_scale): - new_contours = [] - for i in range(len(contours)): - for j in range(len(contours[i])): - x = contours[i][j][0][0]*x_scale + x_offset - y = contours[i][j][0][1]*y_scale + y_offset - new_contours.append([x, y]) - return new_contours + +def new_coord(contours, x_scale, y_scale, angle): + '''''' + + ''' SCALE THE IMAGE HERE ''' + scale_contours = [] + for i in contours: + x = i[0] * x_scale + y = i[1] * y_scale + z = i[2] + scale_contours.append([x, y, z]) + + ''' ROTATE THE IMAGE HERE ''' + angle = angle * np.pi / 180 + rotate_contours = [] + for i in scale_contours: + x_old = i[0] + y_old = i[1] + x = x_old * np.cos(angle) - y_old * np.sin(angle) + y = x_old * np.sin(angle) + y_old * np.cos(angle) + z = i[2] + rotate_contours.append([x, y, z]) + + return rotate_contours def scale_calculator(contours, size): - x_max = contours[0][0][0][0] - x_min = contours[0][0][0][0] - y_max = contours[0][0][0][1] - y_min = contours[0][0][0][1] + x_max = contours[0][0] + x_min = contours[0][0] + y_max = contours[0][1] + y_min = contours[0][1] - for i in range(len(contours)): - for j in range(len(contours[i])): - x = contours[i][j][0][0] - y = contours[i][j][0][1] - if x_min > x: - x_min = x - if x > x_max: - x_max = x - if y_min > y: - y_min = y - if y > y_max: - y_max = y - # print([[x_min, x_max], [y_min, y_max]]) #display max and min in x and y + for i in contours: + x = i[0] + y = i[1] + if x_min > x: + x_min = x + if x > x_max: + x_max = x + if y_min > y: + y_min = y + if y > y_max: + y_max = y + #print([[x_min, x_max], [y_min, y_max]]) #display max and min in x and y x_scale = 1.0 y_scale = 1.0 @@ -64,4 +80,71 @@ def get_size(coords): if y > y_max: y_max = y - return [[x_min, x_max], [y_min, y_max]] #display max and min in x and y \ No newline at end of file + return [[x_min, x_max], [y_min, y_max]] #display max and min in x and y + +def symetry(coords, x_max, y_max, symetry): + new_coords = [] + + for i in coords: + x = i[0] + y = i[1] + z = i[2] + if symetry == 'x': + x = x_max - x + if symetry == 'y': + y = y_max - y + new_coords.append([x, y, z]) + + return new_coords + +def set_origin(coords, origin): + [[x_min, x_max], [y_min, y_max]] = get_size(coords) + new_coords = [] + for i in coords: + x = i[0] - x_min + origin[0] + y = i[1] - y_min + origin[1] + z = i[2] + origin[2] + new_coords.append([x, y, z]) + + return new_coords + +def plot_function(coords): + x_coords = [] + y_coords = [] + z_coords = [] + + for i in coords: + x_coords.append(i[0]) + y_coords.append(i[1]) + z_coords.append(i[2]) + + plt.scatter(y_coords, x_coords, label='Points', color='blue', s=5) + plt.gca().set_aspect('equal', adjustable='box') + plt.xlabel('X-axis') + plt.ylabel('Y-axis') + plt.title('Scatter Plot of Points') + plt.legend() + plt.show() + +def draw_function(coords): + temp_set_x = [] + temp_set_y = [] + final_set_x = [] + final_set_y = [] + for i in coords: + temp_set_x.append(i[0]) + temp_set_y.append(i[1]) + if i[2] != 0: + final_set_x.append(temp_set_x) + final_set_y.append(temp_set_y) + temp_set_x = [] + temp_set_y = [] + #print(final_set_x) + #print(final_set_y) + for j in range(len(final_set_x)): + plt.plot(final_set_x[j], final_set_y[j]) + plt.gca().set_aspect('equal', adjustable='box') + plt.xlabel('X-axis') + plt.ylabel('Y-axis') + plt.title('Scatter Plot of Points') + plt.show() diff --git a/coords_creation.py b/coords_creation.py new file mode 100644 index 0000000..3aa44b7 --- /dev/null +++ b/coords_creation.py @@ -0,0 +1,55 @@ +import cv2 + +import show_img_conditions +import remove_bg +import get_head +import get_egdes +import coordinates_change +import time + +import numpy as np + +def coordinates(origin, new_size, angle, sym, live): + if live == 0: + print("Webcam take picture") + time.sleep(2) + #show_img_conditions.show() + image_path = "goutput.png" + image = cv2.imread(image_path) + #cv2.imshow('test', image) + + background_path = remove_bg.rmbg("didier.jpg") + print(background_path) + + if live == 1: + print("Picture from files") + time.sleep(2) + background_path = "output.png" + + background = cv2.imread(background_path) + #cv2.imshow('Image without background', background) + + gray_image = cv2.cvtColor(background, cv2.COLOR_BGR2GRAY) + contours = get_egdes.edges(gray_image) + #cv2.imshow("contours", edges) + #cv2.waitKey(0) + + + [x_min, y_min, x_scale, y_scale] = coordinates_change.scale_calculator(contours, new_size) + + new_coords = coordinates_change.new_coord(contours, x_scale, y_scale, angle) + + size = coordinates_change.get_size(new_coords) + + x_max = size[0][1] + y_max = size[1][1] + new_coords = coordinates_change.symetry(new_coords, x_max, y_max, sym) + + new_coords = coordinates_change.set_origin(new_coords, origin) + + print("origin : x = ", origin[0], " , y = ", origin[1], " , z = ", origin[2]) + print("size : ", int(size[0][1] - size[0][0]), "x", int(size[1][1] - size[1][0]), "mm") + print("nb points : ", len(new_coords)) + coordinates_change.plot_function(new_coords) + #coordinates_change.draw_function(new_coords) + return new_coords \ No newline at end of file diff --git a/didier.jpg b/didier.jpg new file mode 100644 index 0000000..37e7347 Binary files /dev/null and b/didier.jpg differ diff --git a/dobot.py b/dobot.py new file mode 100644 index 0000000..4a381c5 --- /dev/null +++ b/dobot.py @@ -0,0 +1,2539 @@ +import serial +from serial import SerialException +from serial.serialutil import SerialTimeoutException +import struct + +# Initialisation + +# Calcul de la checksum + +def checksum(trame): + sum = 0x00 + length = int(trame[2]) + + for i in range(3, length+3): + sum = sum + trame[i] + + sum = (0xFF + 0x01 - sum) % 0x100 + return sum + +# Permet de détecter sur quel port COM se trouve le Dobot +def findPortCOM(): + print("Recherche du port COM...") + for i in range(3, 21): + try: + test = serial.Serial() + test.baudrate = 115200 + test.port = 'COM'+str(i) + test.timeout = 1 + test.open() + trame = bytearray() #getDeviceName + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x00) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + test.write(trame) + response = test.read(1) + if response: + return i + test.close() + print(i) + except (SerialException): + 1 + +# Connexion au Dobot + +dobot = serial.Serial() +dobot.baudrate = 115200 +print(str(findPortCOM())) +dobot.port = "COM"+str(findPortCOM()) +dobot.open() + + +def setPortCOM(port): + com = 'COM' + str(port) + dobot.port = com + print("Dobot is now communicating on " + com) + + +def printByte(reponse): + print("b'{}'".format(''.join('\\x{:02x}'.format(b) for b in reponse))) + + +def getResponseLength(): + call = dobot.read(3) + return call[2] + 1 + + +# Liste des fonctions + +def getDeviceSN(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x00) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + longueur = getResponseLength() + reponse = dobot.read(longueur) + + serialNumber = reponse[2:longueur-1].decode() + printByte(reponse) + return serialNumber + + +def setDeviceName(name, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 2+len(name)) + trame.insert(3, 0x01) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + nameByte = str.encode(name) + + for i in range(0, len(name)): + trame.insert(5+i, nameByte[i]) + + trame.insert(5+len(name), checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getDeviceName(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x01) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + longueur = getResponseLength() + reponse = dobot.read(longueur) + + name = reponse[2:longueur-1].decode() + return name + + +def getDeviceVersion(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x02) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + longueur = getResponseLength() + reponse = dobot.read(longueur) + + printByte(reponse) # révision + + v1 = int(reponse[2]) + v2 = int(reponse[3]) + v3 = int(reponse[4]) + + return [v1, v2, v3] + + +def getDeviceTime(): #retourne le temps du dobot en millisecondes + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x04) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + time = reponse[2]+reponse[3]*256+reponse[4]*256*256+reponse[5]*256*256*256 + + return time + + +def getDeviceID(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x05) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + printByte(reponse) # révision + + t1 = struct.unpack("i", reponse[2:6])[0] + t2 = struct.unpack("i", reponse[6:10])[0] + t3 = struct.unpack("i", reponse[10:14])[0] + + return [t1, t2, t3] + + +def getPose(): #renvoie un tableau des positions x, y, z et des angles j1, j2, j3, j4 (aussi nommé r) + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x0A) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + x = float(struct.unpack("f", reponse[2:6])[0]) + y = float(struct.unpack("f", reponse[6:10])[0]) + z = float(struct.unpack("f", reponse[10:14])[0]) + r = float(struct.unpack("f", reponse[14:18])[0]) + j1 = float(struct.unpack("f", reponse[18:22])[0]) + j2 = float(struct.unpack("f", reponse[22:26])[0]) + j3 = float(struct.unpack("f", reponse[26:30])[0]) + j4 = float(struct.unpack("f", reponse[30:34])[0]) + + return([x, y, z, r, j1, j2, j3, j4]) + + +def resetPose(mode, rearArmAngle = 0, frontArmAngle = 0, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x0B) + trame.insert(3, 0x0B) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, mode) + + rAngle = bytearray(struct.pack("f", rearArmAngle)) + + trame.insert(6, rAngle[0]) + trame.insert(7, rAngle[1]) + trame.insert(8, rAngle[2]) + trame.insert(9, rAngle[3]) + + fAngle = bytearray(struct.pack("f", frontArmAngle)) + + trame.insert(10, fAngle[0]) + trame.insert(11, fAngle[1]) + trame.insert(12, fAngle[2]) + trame.insert(13, fAngle[3]) + + trame.insert(14, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getAlarmsState(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x14) + trame.insert(4, 0x01) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def clearAllAlarmsState(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x15) + trame.insert(4, 0x01) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setHomeParams(x, y, z, r, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x12) + trame.insert(3, 0x1E) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + xParam = bytearray(struct.pack("f", x)) + + trame.insert(5, xParam[0]) + trame.insert(6, xParam[1]) + trame.insert(7, xParam[2]) + trame.insert(8, xParam[3]) + + yParam = bytearray(struct.pack("f", y)) + + trame.insert(9, yParam[0]) + trame.insert(10, yParam[1]) + trame.insert(11, yParam[2]) + trame.insert(12, yParam[3]) + + zParam = bytearray(struct.pack("f", z)) + + trame.insert(13, zParam[0]) + trame.insert(14, zParam[1]) + trame.insert(15, zParam[2]) + trame.insert(16, zParam[3]) + + rParam = bytearray(struct.pack("f", r)) + + trame.insert(17, rParam[0]) + trame.insert(18, rParam[1]) + trame.insert(19, rParam[2]) + trame.insert(20, rParam[3]) + + trame.insert(21, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getHomeParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x1E) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + x = float(struct.unpack("f", reponse[2:6])[0]) + y = float(struct.unpack("f", reponse[6:10])[0]) + z = float(struct.unpack("f", reponse[10:14])[0]) + r = float(struct.unpack("f", reponse[14:18])[0]) + + return [x, y, z, r] + + +def setHomeCmd(isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x03) + trame.insert(3, 0x1F) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, 0x00) + trame.insert(6, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setAutoLeveling(isAutoLeveling, accuracy, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 7) + trame.insert(3, 32) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, isAutoLeveling) + + accuracyParam = bytearray(struct.pack("f", accuracy)) + + trame.insert(6, accuracyParam[0]) + trame.insert(7, accuracyParam[1]) + trame.insert(8, accuracyParam[2]) + trame.insert(9, accuracyParam[3]) + + trame.insert(10, checksum(trame)) + + dobot.write(trame) + + return dobot.read(getResponseLength()) + + +def getAutoLeveling(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 32) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + result = float(struct.unpack("f", reponse[2:6])[0]) + + return result + + +def setHHTTrigMode(mode, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 0x28) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, mode) + trame.insert(6, mode) + # mode = 1 --> Prise de position en continu + # mode = 0 --> Prise de position au relachement du bouton + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getHHTTrigMode(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x28) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + mode = int(reponse[2]) + return mode + + +def setHHTTrigOutputEnabled(isOutputEnabled, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x03) + trame.insert(3, 0x29) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, isOutputEnabled) + + trame.insert(6, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getHHTTrigOutputEnabled(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x29) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + isOutputEnabled = int(reponse[2]) + return isOutputEnabled + + +def getHHTTrigOutput(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x2A) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + output = int(reponse[2]) + return output + + +def setEndEffectorParams(xOffset, yOffset, zOffset, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x0E) + trame.insert(3, 0x3C) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + x = bytearray(struct.pack("f", xOffset)) + + trame.insert(5, x[0]) + trame.insert(6, x[1]) + trame.insert(7, x[2]) + trame.insert(8, x[3]) + + y = bytearray(struct.pack("f", yOffset)) + + trame.insert(9, y[0]) + trame.insert(10, y[1]) + trame.insert(11, y[2]) + trame.insert(12, y[3]) + + z = bytearray(struct.pack("f", zOffset)) + + trame.insert(13, z[0]) + trame.insert(14, z[1]) + trame.insert(15, z[2]) + trame.insert(16, z[3]) + + trame.insert(17, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getEndEffectorParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x3C) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + xOffset = float(struct.unpack("f", reponse[2:6])[0]) + yOffset = float(struct.unpack("f", reponse[6:10])[0]) + zOffset = float(struct.unpack("f", reponse[10:14])[0]) + + return [xOffset, yOffset, zOffset] + + +def setEndEffectorSuctionCup(onOff, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 0x3E) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, 0x01) + + trame.insert(6, onOff) + # onOff = 1 --> SuctionCup On + # onOff = 0 --> SuctionCup Off + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getEndEffectorSuctionCup(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x3E) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + isSucked = reponse[3] + return isSucked + + +def setEndEffectorGripper(onOff, isGripped, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 63) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, onOff) + # onOff = 1 --> Gripper On + # onOff = 0 --> Gripper Off + + trame.insert(6, isGripped) + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getEndEffectorGripper(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 63) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + onOff = reponse[2] + isGripped = reponse[3] + + return [onOff, isGripped] + + +def setJOGjointParams(j1Velocity, j2Velocity, j3Velocity, j4Velocity, j1Accel, j2Accel, j3Accel, j4Accel, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x22) + trame.insert(3, 70) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + j1VelocityParam = bytearray(struct.pack("f", j1Velocity)) + + trame.insert(5, j1VelocityParam[0]) + trame.insert(6, j1VelocityParam[1]) + trame.insert(7, j1VelocityParam[2]) + trame.insert(8, j1VelocityParam[3]) + + j2VelocityParam = bytearray(struct.pack("f", j2Velocity)) + + trame.insert(9, j2VelocityParam[0]) + trame.insert(10, j2VelocityParam[1]) + trame.insert(11, j2VelocityParam[2]) + trame.insert(12, j2VelocityParam[3]) + + j3VelocityParam = bytearray(struct.pack("f", j3Velocity)) + + trame.insert(13, j3VelocityParam[0]) + trame.insert(14, j3VelocityParam[1]) + trame.insert(15, j3VelocityParam[2]) + trame.insert(16, j3VelocityParam[3]) + + j4VelocityParam = bytearray(struct.pack("f", j4Velocity)) + + trame.insert(17, j4VelocityParam[0]) + trame.insert(18, j4VelocityParam[1]) + trame.insert(19, j4VelocityParam[2]) + trame.insert(20, j4VelocityParam[3]) + + j1AccelParam = bytearray(struct.pack("f", j1Accel)) + + trame.insert(21, j1AccelParam[0]) + trame.insert(22, j1AccelParam[1]) + trame.insert(23, j1AccelParam[2]) + trame.insert(24, j1AccelParam[3]) + + j2AccelParam = bytearray(struct.pack("f", j2Accel)) + + trame.insert(25, j2AccelParam[0]) + trame.insert(26, j2AccelParam[1]) + trame.insert(27, j2AccelParam[2]) + trame.insert(28, j2AccelParam[3]) + + j3AccelParam = bytearray(struct.pack("f", j3Accel)) + + trame.insert(29, j3AccelParam[0]) + trame.insert(30, j3AccelParam[1]) + trame.insert(31, j3AccelParam[2]) + trame.insert(32, j3AccelParam[3]) + + j4AccelParam = bytearray(struct.pack("f", j4Accel)) + + trame.insert(33, j4AccelParam[0]) + trame.insert(34, j4AccelParam[1]) + trame.insert(35, j4AccelParam[2]) + trame.insert(36, j4AccelParam[3]) + + trame.insert(37, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getJOGjointParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 70) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + j1Velocity = float(struct.unpack("f", reponse[2:6])[0]) + j2Velocity = float(struct.unpack("f", reponse[6:10])[0]) + j3Velocity = float(struct.unpack("f", reponse[10:14])[0]) + j4Velocity = float(struct.unpack("f", reponse[14:18])[0]) + + j1Accel = float(struct.unpack("f", reponse[18:22])[0]) + j2Accel = float(struct.unpack("f", reponse[22:26])[0]) + j3Accel = float(struct.unpack("f", reponse[26:30])[0]) + j4Accel = float(struct.unpack("f", reponse[30:34])[0]) + + return [j1Velocity, j2Velocity, j3Velocity, j4Velocity, j1Accel, j2Accel, j3Accel, j4Accel] + + +def setJOGCoordinateParams(xAxisVelocity, yAxisVelocity, zAxisVelocity, rVelocity, xAxisAccel, yAxisAccel, zAxisAccel, rAccel, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x22) + trame.insert(3, 71) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + xAxisVelocityParam = bytearray(struct.pack("f", xAxisVelocity)) + + trame.insert(5, xAxisVelocityParam[0]) + trame.insert(6, xAxisVelocityParam[1]) + trame.insert(7, xAxisVelocityParam[2]) + trame.insert(8, xAxisVelocityParam[3]) + + yAxisVelocityParam = bytearray(struct.pack("f", yAxisVelocity)) + + trame.insert(9, yAxisVelocityParam[0]) + trame.insert(10, yAxisVelocityParam[1]) + trame.insert(11, yAxisVelocityParam[2]) + trame.insert(12, yAxisVelocityParam[3]) + + zAxisVelocityParam = bytearray(struct.pack("f", zAxisVelocity)) + + trame.insert(13, zAxisVelocityParam[0]) + trame.insert(14, zAxisVelocityParam[1]) + trame.insert(15, zAxisVelocityParam[2]) + trame.insert(16, zAxisVelocityParam[3]) + + rVelocityParam = bytearray(struct.pack("f", rVelocity)) + + trame.insert(17, rVelocityParam[0]) + trame.insert(18, rVelocityParam[1]) + trame.insert(19, rVelocityParam[2]) + trame.insert(20, rVelocityParam[3]) + + xAxisAccelParam = bytearray(struct.pack("f", xAxisAccel)) + + trame.insert(21, xAxisAccelParam[0]) + trame.insert(22, xAxisAccelParam[1]) + trame.insert(23, xAxisAccelParam[2]) + trame.insert(24, xAxisAccelParam[3]) + + yAxisAccelParam = bytearray(struct.pack("f", yAxisAccel)) + + trame.insert(25, yAxisAccelParam[0]) + trame.insert(26, yAxisAccelParam[1]) + trame.insert(27, yAxisAccelParam[2]) + trame.insert(28, yAxisAccelParam[3]) + + zAxisAccelParam = bytearray(struct.pack("f", zAxisAccel)) + + trame.insert(29, zAxisAccelParam[0]) + trame.insert(30, zAxisAccelParam[1]) + trame.insert(31, zAxisAccelParam[2]) + trame.insert(32, zAxisAccelParam[3]) + + rAccelParam = bytearray(struct.pack("f", rAccel)) + + trame.insert(33, rAccelParam[0]) + trame.insert(34, rAccelParam[1]) + trame.insert(35, rAccelParam[2]) + trame.insert(36, rAccelParam[3]) + + trame.insert(37, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getJOGCoordinateParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 71) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + xAxisVelocity = float(struct.unpack("f", reponse[2:6])[0]) + yAxisVelocity = float(struct.unpack("f", reponse[6:10])[0]) + zAxisVelocity = float(struct.unpack("f", reponse[10:14])[0]) + rVelocity = float(struct.unpack("f", reponse[14:18])[0]) + xAxisVAccel = float(struct.unpack("f", reponse[18:22])[0]) + yAxisAccel = float(struct.unpack("f", reponse[22:26])[0]) + zAxisAccel = float(struct.unpack("f", reponse[26:30])[0]) + rAccel = float(struct.unpack("f", reponse[30:34])[0]) + + return [xAxisVelocity, yAxisVelocity, zAxisVelocity, rVelocity, xAxisVAccel, yAxisAccel, zAxisAccel, rAccel] + + +def setJOGCommonParams(velocityRatio, accelerationRatio, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x0A) + trame.insert(3, 72) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + velocityRatioParam = bytearray(struct.pack("f", velocityRatio)) + + trame.insert(5, velocityRatioParam[0]) + trame.insert(6, velocityRatioParam[1]) + trame.insert(7, velocityRatioParam[2]) + trame.insert(8, velocityRatioParam[3]) + + accelerationRatioParam = bytearray(struct.pack("f", accelerationRatio)) + + trame.insert(9, accelerationRatioParam[0]) + trame.insert(10, accelerationRatioParam[1]) + trame.insert(11, accelerationRatioParam[2]) + trame.insert(12, accelerationRatioParam[3]) + + trame.insert(13, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getJOGCommonParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 72) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + velocityRatio = struct.unpack("f", reponse[2:6])[0] + accelerationRatio = struct.unpack("f", reponse[6:10])[0] + + return [velocityRatio, accelerationRatio] + + +def setJOGCmd(jogMode, jogCommand, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 73) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, jogMode) + + # Values of jogMode : + + # jogMode = 0 --> cartesian coordinate system + # jogMode = 1 --> joint system + + trame.insert(6, jogCommand) + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setPTPjointParams(j1Velocity, j2Velocity, j3Velocity, j4Velocity, j1Accel, j2Accel, j3Accel, j4Accel, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x22) + trame.insert(3, 0x50) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + j1VelocityParam = bytearray(struct.pack("f", j1Velocity)) + + trame.insert(5, j1VelocityParam[0]) + trame.insert(6, j1VelocityParam[1]) + trame.insert(7, j1VelocityParam[2]) + trame.insert(8, j1VelocityParam[3]) + + j2VelocityParam = bytearray(struct.pack("f", j2Velocity)) + + trame.insert(9, j2VelocityParam[0]) + trame.insert(10, j2VelocityParam[1]) + trame.insert(11, j2VelocityParam[2]) + trame.insert(12, j2VelocityParam[3]) + + j3VelocityParam = bytearray(struct.pack("f", j3Velocity)) + + trame.insert(13, j3VelocityParam[0]) + trame.insert(14, j3VelocityParam[1]) + trame.insert(15, j3VelocityParam[2]) + trame.insert(16, j3VelocityParam[3]) + + j4VelocityParam = bytearray(struct.pack("f", j4Velocity)) + + trame.insert(17, j4VelocityParam[0]) + trame.insert(18, j4VelocityParam[1]) + trame.insert(19, j4VelocityParam[2]) + trame.insert(20, j4VelocityParam[3]) + + j1AccelParam = bytearray(struct.pack("f", j1Accel)) + + trame.insert(21, j1AccelParam[0]) + trame.insert(22, j1AccelParam[1]) + trame.insert(23, j1AccelParam[2]) + trame.insert(24, j1AccelParam[3]) + + j2AccelParam = bytearray(struct.pack("f", j2Accel)) + + trame.insert(25, j2AccelParam[0]) + trame.insert(26, j2AccelParam[1]) + trame.insert(27, j2AccelParam[2]) + trame.insert(28, j2AccelParam[3]) + + j3AccelParam = bytearray(struct.pack("f", j3Accel)) + + trame.insert(29, j3AccelParam[0]) + trame.insert(30, j3AccelParam[1]) + trame.insert(31, j3AccelParam[2]) + trame.insert(32, j3AccelParam[3]) + + j4AccelParam = bytearray(struct.pack("f", j4Accel)) + + trame.insert(33, j4AccelParam[0]) + trame.insert(34, j4AccelParam[1]) + trame.insert(35, j4AccelParam[2]) + trame.insert(36, j4AccelParam[3]) + + trame.insert(37, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getPTPjointParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x50) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + j1Velocity = float(struct.unpack("f", reponse[2:6])[0]) + j2Velocity = float(struct.unpack("f", reponse[6:10])[0]) + j3Velocity = float(struct.unpack("f", reponse[10:14])[0]) + j4Velocity = float(struct.unpack("f", reponse[14:18])[0]) + + j1Accel = float(struct.unpack("f", reponse[18:22])[0]) + j2Accel = float(struct.unpack("f", reponse[22:26])[0]) + j3Accel = float(struct.unpack("f", reponse[26:30])[0]) + j4Accel = float(struct.unpack("f", reponse[30:34])[0]) + + return [j1Velocity, j2Velocity, j3Velocity, j4Velocity, j1Accel, j2Accel, j3Accel, j4Accel] + + +def setPTPCoordinateParams(xyzVelocity, rVelocity, xyzAccel, rAccel, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x12) + trame.insert(3, 0x51) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + xyzVelocityParam = bytearray(struct.pack("f", xyzVelocity)) + + trame.insert(5, xyzVelocityParam[0]) + trame.insert(6, xyzVelocityParam[1]) + trame.insert(7, xyzVelocityParam[2]) + trame.insert(8, xyzVelocityParam[3]) + + rVelocityParam = bytearray(struct.pack("f", rVelocity)) + + trame.insert(9, rVelocityParam[0]) + trame.insert(10, rVelocityParam[1]) + trame.insert(11, rVelocityParam[2]) + trame.insert(12, rVelocityParam[3]) + + xyzAccelParam = bytearray(struct.pack("f", xyzAccel)) + + trame.insert(13, xyzAccelParam[0]) + trame.insert(14, xyzAccelParam[1]) + trame.insert(15, xyzAccelParam[2]) + trame.insert(16, xyzAccelParam[3]) + + rAccelParam = bytearray(struct.pack("f", rAccel)) + + trame.insert(17, rAccelParam[0]) + trame.insert(18, rAccelParam[1]) + trame.insert(19, rAccelParam[2]) + trame.insert(20, rAccelParam[3]) + + trame.insert(21, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getPTPCoordinateParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x51) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + xyzVelocity = float(struct.unpack("f", reponse[2:6])[0]) + rVelocity = float(struct.unpack("f", reponse[6:10])[0]) + xyzAccel = float(struct.unpack("f", reponse[10:14])[0]) + rAccel = float(struct.unpack("f", reponse[14:18])[0]) + + return([xyzVelocity, rVelocity, xyzAccel, rAccel]) + + +def setPTPJumpParams(jumpHeight, zLimit, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x0A) + trame.insert(3, 0x52) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + jumpHeightParam = bytearray(struct.pack("f", jumpHeight)) + + trame.insert(5, jumpHeightParam[0]) + trame.insert(6, jumpHeightParam[1]) + trame.insert(7, jumpHeightParam[2]) + trame.insert(8, jumpHeightParam[3]) + + zLimitParam = bytearray(struct.pack("f", zLimit)) + + trame.insert(9, zLimitParam[0]) + trame.insert(10, zLimitParam[1]) + trame.insert(11, zLimitParam[2]) + trame.insert(12, zLimitParam[3]) + + trame.insert(13, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getPTPJumpParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x52) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + jumpHeight = struct.unpack("f", reponse[2:6])[0] + zLimit = struct.unpack("f", reponse[6:10])[0] + + return [jumpHeight, zLimit] + + +def setPTPCommonParams(velocityRatio, accelerationRatio, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x0A) + trame.insert(3, 0x53) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + velocityRatioParam = bytearray(struct.pack("f", velocityRatio)) + + trame.insert(5, velocityRatioParam[0]) + trame.insert(6, velocityRatioParam[1]) + trame.insert(7, velocityRatioParam[2]) + trame.insert(8, velocityRatioParam[3]) + + accelerationRatioParam = bytearray(struct.pack("f", accelerationRatio)) + + trame.insert(9, accelerationRatioParam[0]) + trame.insert(10, accelerationRatioParam[1]) + trame.insert(11, accelerationRatioParam[2]) + trame.insert(12, accelerationRatioParam[3]) + + trame.insert(13, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getPTPCommonParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x53) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + velocityRatio = struct.unpack("f", reponse[2:6])[0] + accelerationRatio = struct.unpack("f", reponse[6:10])[0] + + return [velocityRatio, accelerationRatio] + + +def setPTPCmd(ptpMode, x, y, z, r, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x13) + trame.insert(3, 0x54) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, ptpMode) + + # Values of ptpMode : + + # ptpMode = 0 --> jUMP_XYZ | joint-jump mode in cartesian coordinate system by using absolute position + # ptpMode = 1 --> MOVj_XYZ | joint mode in cartesian coordinate system by using absolute position + # ptpMode = 2 --> MOVL_XYZ | Linear mode in cartesian coordinate system by using absolute position + + # ptpMode = 3 --> jUMP_ANGLE | jump mode in joint coordinate system by using absolute position + # ptpMode = 4 --> MOVj_ANGLE | joint mode in joint coordinate system by using absolute position + # ptpMode = 5 --> MOVL_ANGLE | Linear mode in joint coordinate system by using absolute position + + # ptpMode = 6 --> MOVj_INC | joint mode in joint coordinate system by using relative position + # ptpMode = 7 --> MOVL_INC | Linear mode in cartesian coordinate system by using relative position + # ptpMode = 8 --> MOVj_XYZ_INC | joint mode in cartesian coordinate system by using relative position + + # ptpMode = 9 --> jUMP_MOVL_XYZ | Linear-jump mode in cartesian coordinate system by using absolute position + + xParam = bytearray(struct.pack("f", x)) + + trame.insert(6, xParam[0]) + trame.insert(7, xParam[1]) + trame.insert(8, xParam[2]) + trame.insert(9, xParam[3]) + + yParam = bytearray(struct.pack("f", y)) + + trame.insert(10, yParam[0]) + trame.insert(11, yParam[1]) + trame.insert(12, yParam[2]) + trame.insert(13, yParam[3]) + + zParam = bytearray(struct.pack("f", z)) + + trame.insert(14, zParam[0]) + trame.insert(15, zParam[1]) + trame.insert(16, zParam[2]) + trame.insert(17, zParam[3]) + + rParam = bytearray(struct.pack("f", r)) + + trame.insert(18, rParam[0]) + trame.insert(19, rParam[1]) + trame.insert(20, rParam[2]) + trame.insert(21, rParam[3]) + + trame.insert(22, checksum(trame)) + + dobot.write(trame) + return dobot.read(getResponseLength()) + + +def setPTPJump2Params(startHeight, endHeight, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x0A) + trame.insert(3, 87) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + startHeightParam = bytearray(struct.pack("f", startHeight)) + + trame.insert(5, startHeightParam[0]) + trame.insert(6, startHeightParam[1]) + trame.insert(7, startHeightParam[2]) + trame.insert(8, startHeightParam[3]) + + endHeightParam = bytearray(struct.pack("f", endHeight)) + + trame.insert(9, endHeightParam[0]) + trame.insert(10, endHeightParam[1]) + trame.insert(11, endHeightParam[2]) + trame.insert(12, endHeightParam[3]) + + trame.insert(13, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getPTPJump2Params(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x57) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + startHeight = struct.unpack("f", reponse[2:6])[0] + endHeight = struct.unpack("f", reponse[6:10])[0] + zLimit = struct.unpack("f", reponse[10:14])[0] + + return [startHeight, endHeight, zLimit] + + +def setPTPPOCmd(ptpMode, x, y, z, r, ratio, address, level, isQueued = 0): + # Values of ptpMode : + # ptpMode = 0 --> jUMP_XYZ | joint-jump mode in cartesian coordinate system by using absolute position + # ptpMode = 1 --> MOVj_XYZ | joint mode in cartesian coordinate system by using absolute position + # ptpMode = 2 --> MOVL_XYZ | Linear mode in cartesian coordinate system by using absolute position + # ptpMode = 3 --> jUMP_ANGLE | jump mode in joint coordinate system by using absolute position + # ptpMode = 4 --> MOVj_ANGLE | joint mode in joint coordinate system by using absolute position + # ptpMode = 5 --> MOVL_ANGLE | Linear mode in joint coordinate system by using absolute position + # ptpMode = 6 --> MOVj_INC | joint mode in joint coordinate system by using relative position + # ptpMode = 7 --> MOVL_INC | Linear mode in cartesian coordinate system by using relative position + # ptpMode = 8 --> MOVj_XYZ_INC | joint mode in cartesian coordinate system by using relative position + # ptpMode = 9 --> jUMP_MOVL_XYZ | Linear-jump mode in cartesian coordinate system by using absolute position + + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + + # Possibilité de commander plusieurs I/0 donc longueur variable + length = 19 + 4 * len(address) + trame.insert(2, length) + + trame.insert(3, 88) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, ptpMode) + + xParam = bytearray(struct.pack("f", x)) + trame.insert(6, xParam[0]) + trame.insert(7, xParam[1]) + trame.insert(8, xParam[2]) + trame.insert(9, xParam[3]) + + yParam = bytearray(struct.pack("f", y)) + trame.insert(10, yParam[0]) + trame.insert(11, yParam[1]) + trame.insert(12, yParam[2]) + trame.insert(13, yParam[3]) + + zParam = bytearray(struct.pack("f", z)) + trame.insert(14, zParam[0]) + trame.insert(15, zParam[1]) + trame.insert(16, zParam[2]) + trame.insert(17, zParam[3]) + + rParam = bytearray(struct.pack("f", r)) + trame.insert(18, rParam[0]) + trame.insert(19, rParam[1]) + trame.insert(20, rParam[2]) + trame.insert(21, rParam[3]) + + for i in range(len(address)): + trame.insert(22 + 4 * i, ratio[i]) + # addresse encodée sur 2 octets curieusement + trame.insert(23 + 4 * i, 0) + trame.insert(24 + 4 * i, address[i]) + trame.insert(25 + 4 * i, level[i]) + + trame.insert(26, checksum(trame)) + + printByte(trame) # a supprimer + dobot.write(trame) + + printByte(dobot.read(getResponseLength())) + + +def setCPParams(acceleration, velocity, accelLimit, accelMode, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 15) + trame.insert(3, 90) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + accelerationParam = bytearray(struct.pack("f", acceleration)) + + trame.insert(5, accelerationParam[0]) + trame.insert(6, accelerationParam[1]) + trame.insert(7, accelerationParam[2]) + trame.insert(8, accelerationParam[3]) + + velocityParam = bytearray(struct.pack("f", velocity)) + + trame.insert(9, velocityParam[0]) + trame.insert(10, velocityParam[1]) + trame.insert(11, velocityParam[2]) + trame.insert(12, velocityParam[3]) + + accelLimitParam = bytearray(struct.pack("f", accelLimit)) + + trame.insert(13, accelLimitParam[0]) + trame.insert(14, accelLimitParam[1]) + trame.insert(15, accelLimitParam[2]) + trame.insert(16, accelLimitParam[3]) + + trame.insert(17, accelMode) + + # Values of accelMode : + + # accelMode = 0 --> mode mouvement avec rampe d'accélération + # accelMode = 1 --> mode mouvement sans rampe d'accélération (mettre periode à 1000 minimum et accélération max 2000) + + trame.insert(18, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getCPParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 90) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + acceleration = struct.unpack("f", reponse[2:6])[0] + velocity = struct.unpack("f", reponse[6:10])[0] + accelLimit = struct.unpack("f", reponse[10:14])[0] + accelMode = int(reponse[14]) + + return [acceleration, velocity, accelLimit, accelMode] + + +def setCPCmd(cpMode, x, y, z, velocity, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 19) + trame.insert(3, 91) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, cpMode) + + # Values of cpMode : + + # cpMode = 0 --> moving using relative position + # cpMode = 1 --> moving using absolute position + + xParam = bytearray(struct.pack("f", x)) + + trame.insert(6, xParam[0]) + trame.insert(7, xParam[1]) + trame.insert(8, xParam[2]) + trame.insert(9, xParam[3]) + + yParam = bytearray(struct.pack("f", y)) + + trame.insert(10, yParam[0]) + trame.insert(11, yParam[1]) + trame.insert(12, yParam[2]) + trame.insert(13, yParam[3]) + + zParam = bytearray(struct.pack("f", z)) + + trame.insert(14, zParam[0]) + trame.insert(15, zParam[1]) + trame.insert(16, zParam[2]) + trame.insert(17, zParam[3]) + + velocityParam = bytearray(struct.pack("f", velocity)) + + trame.insert(18, velocityParam[0]) + trame.insert(19, velocityParam[1]) + trame.insert(20, velocityParam[2]) + trame.insert(21, velocityParam[3]) + + trame.insert(22, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setARCParams(xyzVelocity, rVelocity, xyzAccel, rAccel, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 18) + trame.insert(3, 100) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + xyzVelocityParam = bytearray(struct.pack("f", xyzVelocity)) + + trame.insert(5, xyzVelocityParam[0]) + trame.insert(6, xyzVelocityParam[1]) + trame.insert(7, xyzVelocityParam[2]) + trame.insert(8, xyzVelocityParam[3]) + + rVelocityParam = bytearray(struct.pack("f", rVelocity)) + + trame.insert(9, rVelocityParam[0]) + trame.insert(10, rVelocityParam[1]) + trame.insert(11, rVelocityParam[2]) + trame.insert(12, rVelocityParam[3]) + + xyzAccelParam = bytearray(struct.pack("f", xyzAccel)) + + trame.insert(13, xyzAccelParam[0]) + trame.insert(14, xyzAccelParam[1]) + trame.insert(15, xyzAccelParam[2]) + trame.insert(16, xyzAccelParam[3]) + + rAccelParam = bytearray(struct.pack("f", rAccel)) + + trame.insert(17, rAccelParam[0]) + trame.insert(18, rAccelParam[1]) + trame.insert(19, rAccelParam[2]) + trame.insert(20, rAccelParam[3]) + + trame.insert(21, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getARCParams(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 100) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + xyzVelocity = float(struct.unpack("f", reponse[2:6])[0]) + rVelocity = float(struct.unpack("f", reponse[6:10])[0]) + xyzAccel = float(struct.unpack("f", reponse[10:14])[0]) + rAccel = float(struct.unpack("f", reponse[14:18])[0]) + + return([xyzVelocity, rVelocity, xyzAccel, rAccel]) + + +def setARCCmd(xStart, yStart, zStart, rStart, xEnd, yEnd, zEnd, rEnd, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 34) + trame.insert(3, 101) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + xStartParam = bytearray(struct.pack("f", xStart)) + + trame.insert(5, xStartParam[0]) + trame.insert(6, xStartParam[1]) + trame.insert(7, xStartParam[2]) + trame.insert(8, xStartParam[3]) + + yStartParam = bytearray(struct.pack("f", yStart)) + + trame.insert(9, yStartParam[0]) + trame.insert(10, yStartParam[1]) + trame.insert(11, yStartParam[2]) + trame.insert(12, yStartParam[3]) + + zStartParam = bytearray(struct.pack("f", zStart)) + + trame.insert(13, zStartParam[0]) + trame.insert(14, zStartParam[1]) + trame.insert(15, zStartParam[2]) + trame.insert(16, zStartParam[3]) + + rStartParam = bytearray(struct.pack("f", rStart)) + + trame.insert(17, rStartParam[0]) + trame.insert(18, rStartParam[1]) + trame.insert(19, rStartParam[2]) + trame.insert(20, rStartParam[3]) + + xEndParam = bytearray(struct.pack("f", xEnd)) + + trame.insert(21, xEndParam[0]) + trame.insert(22, xEndParam[1]) + trame.insert(23, xEndParam[2]) + trame.insert(24, xEndParam[3]) + + yEndParam = bytearray(struct.pack("f", yEnd)) + + trame.insert(25, yEndParam[0]) + trame.insert(26, yEndParam[1]) + trame.insert(27, yEndParam[2]) + trame.insert(28, yEndParam[3]) + + zEndParam = bytearray(struct.pack("f", zEnd)) + + trame.insert(29, zEndParam[0]) + trame.insert(30, zEndParam[1]) + trame.insert(31, zEndParam[2]) + trame.insert(32, zEndParam[3]) + + rEndParam = bytearray(struct.pack("f", rEnd)) + + trame.insert(33, rEndParam[0]) + trame.insert(34, rEndParam[1]) + trame.insert(35, rEndParam[2]) + trame.insert(36, rEndParam[3]) + + trame.insert(37, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setWaitCmd(milliseconds, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x06) + trame.insert(3, 0x6E) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + millisecondsParam = bytearray(struct.pack("l", milliseconds)) + + trame.insert(5, millisecondsParam[0]) + trame.insert(6, millisecondsParam[1]) + trame.insert(7, millisecondsParam[2]) + trame.insert(8, millisecondsParam[3]) + + trame.insert(9, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setTRIGCmd(adress, mode, condition, valeur, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x07) + trame.insert(3, 0x78) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + addressParam = bytearray(struct.pack("i", adress)) + # EIO address 1 ~ 20 (Voir Dobot Magician Interface Description) + + trame.insert(5, addressParam[0]) + + trame.insert(6, mode) + # Triggering mode : + # mode = 0 --> Level (Tout ou rien) + # mode = 1 --> ADC + + trame.insert(7, condition) + # Triggering condition : + # condition = 0 --> < (inférieur) + # condition = 1 --> <= (inférieur ou égal) + # condition = 2 --> >= (supérieur ou égal) + # condition = 3 --> > (supérieur) + + valeurParam = bytearray(struct.pack("h", valeur)) + + trame.insert(8, valeurParam[0]) + trame.insert(9, valeurParam[1]) + + trame.insert(10, checksum(trame)) + + dobot.write(trame) + + return dobot.read(getResponseLength()) + + +def setIOMultiplexing(address, function, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 0x82) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, address) # EIO addressing（Value range 1~20) + trame.insert(6, function) + # EIO function : + # IOFunctionDummy; //Invalid = 0 + # IOFunctionDO; // I/O output = 1 + # IOFunctionPWM; // PWM output = 2 + # IOFunctionDI; //I/O input = 3 + # IOFunctionADC; //A/D input = 4 + # IOFunctionDIPU; //Pull-up input = 5 + # IOFunctionDIPD //Pull-down input = 6 + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getIOMultiplexing(address): + # La documentation n'est pas correcte, il faut rentrer une adresse en paramètre) + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 0x82) + trame.insert(4, 0x00) + trame.insert(5, address) + trame.insert(6, 0x00) # octet nécessaire à la place du mode du pin + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + response = dobot.read(getResponseLength()) + + function = int(response[3]) + return function + + +def setIODO(address, level, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 0x83) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, address) + trame.insert(6, level) # level = 0 ou 1 + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getIODO(address): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 0x83) + trame.insert(4, 0x00) + + trame.insert(5, address) + trame.insert(6, 0x00) + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + response = dobot.read(getResponseLength()) + + level = int(response[3]) + return level + + +def setIOPWM(address, frequency, dutyRatio, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x0B) + trame.insert(3, 0x84) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, address) + + frequencyParam = bytearray(struct.pack("f", frequency)) + # 10 Hz ~ 1 MHz /!\ float + trame.insert(6, frequencyParam[0]) + trame.insert(7, frequencyParam[1]) + trame.insert(8, frequencyParam[2]) + trame.insert(9, frequencyParam[3]) + + # Pin EIO6 inversé pour une raison inconnue /!\ + if address == 6: + dutyRatio = 100 - dutyRatio + + dutyRatioParam = bytearray(struct.pack("f", dutyRatio)) + # 0 ~ 100 /!\ float + trame.insert(10, dutyRatioParam[0]) + trame.insert(11, dutyRatioParam[1]) + trame.insert(12, dutyRatioParam[2]) + trame.insert(13, dutyRatioParam[3]) + + trame.insert(14, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getIOPWM(address): + # Documentation incorrecte + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x0B) + trame.insert(3, 0x84) + trame.insert(4, 0x00) + trame.insert(5, address) + + for i in range(6, 14): + trame.insert(i, 0x00) + + trame.insert(14, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + frequency = round(struct.unpack("f", reponse[3:7])[0], 2) + dutyCycle = round(struct.unpack("f", reponse[7:11])[0], 2) + + # Pin EIO6 inversé pour une raison inconnue /!\ + if address == 6: + dutyCycle = 100 - dutyCycle + + return [frequency, dutyCycle] + + +def getIODI(address): + # Documentation incorrecte + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 0x85) + trame.insert(4, 0x00) + trame.insert(5, address) + trame.insert(6, 0x00) + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + response = dobot.read(getResponseLength()) + + level = response[3] + return level + + +def getIOADC(address): + # Documentation incorrecte + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x05) + trame.insert(3, 0x86) + trame.insert(4, 0x00) + trame.insert(5, address) + trame.insert(6, 0x00) + trame.insert(7, 0x00) + + trame.insert(8, checksum(trame)) + + dobot.write(trame) + response = dobot.read(getResponseLength()) + + valeur = struct.unpack("h", response[3:5])[0] + return valeur + + +def setEMotor(stepper, onOff, speed, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x08) + trame.insert(3, 0x87) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, stepper) + # Numéro du port où est branché le moteur : + # stepper = 0 --> stepper 1 + # stepper = 1 --> stepper 2 + + trame.insert(6, onOff) + # onOff = 1 --> Motor On + # onOff = 0 --> Motor Off + + speedParam = bytearray(struct.pack("l", speed)) + # Vitesse en pulse/s + # 10 000 pulse/s = 71 mm/s + # Il est déconseillé de dépasser les 20 000 pulse/s + + trame.insert(7, speedParam[0]) + trame.insert(8, speedParam[1]) + trame.insert(9, speedParam[2]) + trame.insert(10, speedParam[3]) + + trame.insert(11, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setColorSensor(onOff, port, version, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x05) + trame.insert(3, 0x89) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, onOff) + # onOff = 1 --> Sensor On + # onOff = 0 --> Sensor Off + + trame.insert(6, port) + # Numéro du port où est branché le moteur : + # port = 0 --> GP1 + # port = 1 --> GP2 + # port = 2 --> GP4 + # port = 3 --> GP5 + + trame.insert(7, version) + # version du Color Sensor + # version = 0 --> Verion V1.0 + # version = 1 --> version v2.0 + + trame.insert(8, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getColorSensor(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0x89) + trame.insert(4, 0x00) + + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + r = reponse[2] + g = reponse[3] + b = reponse[4] + + return [r, g, b] + + +def setIRSwitch(onOff, port, version, isQueued = 0): + # Documentation incorrecte + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x05) + trame.insert(3, 0x8A) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, onOff) + #Off = 0 + #On = 1 + + trame.insert(6, port) + # Numéro du port où est branché le capteur : + # port = 0 --> GP1 + # port = 1 --> GP2 + # port = 2 --> GP4 + # port = 3 --> GP5 + + trame.insert(7, version) + # version du IR Sensor + # version = 0 --> version V1.0 + # version = 1 --> version v2.0 + + trame.insert(8, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getIRSwitch(port): + # Documentation incorrecte + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x04) + trame.insert(3, 0x8A) + trame.insert(4, 0x00) + trame.insert(5, port) + trame.insert(6, 0x00) + + trame.insert(7, checksum(trame)) + + dobot.write(trame) + response = dobot.read(getResponseLength()) + + output = int(response[2]) + return output + + +def setAngleSensorStaticError(rearArmAngleError, frontArmAngleError, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 10) + trame.insert(3, 140) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + rearArmAngleErrorParam = bytearray(struct.pack("f", rearArmAngleError)) + + trame.insert(5, rearArmAngleErrorParam[0]) + trame.insert(6, rearArmAngleErrorParam[1]) + trame.insert(7, rearArmAngleErrorParam[2]) + trame.insert(8, rearArmAngleErrorParam[3]) + + frontArmAngleErrorParam = bytearray(struct.pack("f", frontArmAngleError)) + + trame.insert(9, frontArmAngleErrorParam[0]) + trame.insert(10, frontArmAngleErrorParam[1]) + trame.insert(11, frontArmAngleErrorParam[2]) + trame.insert(12, frontArmAngleErrorParam[3]) + + trame.insert(13, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getAngleSensorStaticError(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 140) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + rearArmAngleError = float(struct.unpack("f", reponse[2:6])[0]) + frontArmAngleError = float(struct.unpack("f", reponse[6:10])[0]) + + return [rearArmAngleError, frontArmAngleError] + + +def setWIFIConfigMode(isEnabled, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 3) + trame.insert(3, 150) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, isEnabled) + + trame.insert(6, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getWIFIConfigMode(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 150) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + isEnabled = int(reponse[2]) + return isEnabled + + +def setWIFISSID(ssid, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 2+len(ssid)) + trame.insert(3, 151) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + ssidByte = str.encode(ssid) + + for i in range(0, len(ssid)): + trame.insert(5+i, ssidByte[i]) + + trame.insert(5+len(ssid), checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getWIFISSID(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 151) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + longueur = getResponseLength() + reponse = dobot.read(longueur) + + ssid = reponse[2:longueur-1].decode() + return ssid + + +def setWIFIPassword(password, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 2+len(password)) + trame.insert(3, 152) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + passwordByte = str.encode(password) + + for i in range(0, len(password)): + trame.insert(5+i, passwordByte[i]) + + trame.insert(5+len(password), checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getWIFIPassword(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 152) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + longueur = getResponseLength() + reponse = dobot.read(longueur) + + password = reponse[2:longueur-1].decode() + return password + + +def setWIFIIPAdress(dhcp, ipAdressByte1, ipAdressByte2, ipAdressByte3, ipAdressByte4, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 7) + trame.insert(3, 153) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, dhcp) + trame.insert(6, ipAdressByte1) + trame.insert(7, ipAdressByte2) + trame.insert(8, ipAdressByte3) + trame.insert(9, ipAdressByte4) + + trame.insert(10, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getWIFIIPAdress(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 153) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + dhcp = reponse[2] + ipAdressByte1 = reponse[3] + ipAdressByte2 = reponse[4] + ipAdressByte3 = reponse[5] + ipAdressByte4 = reponse[6] + + return [dhcp, ipAdressByte1, ipAdressByte2, ipAdressByte3, ipAdressByte4] + + +def setWIFINetmask(netmaskByte1, netmaskByte2, netmaskByte3, netmaskByte4, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 6) + trame.insert(3, 154) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, netmaskByte1) + trame.insert(6, netmaskByte2) + trame.insert(7, netmaskByte3) + trame.insert(8, netmaskByte4) + + trame.insert(9, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getWIFINetmask(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 154) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + netmask = [reponse[2], reponse[3], reponse[4], reponse[5]] + return netmask + + +def setWIFIGateway(gatewayByte1, gatewayByte2, gatewayByte3, gatewayByte4, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 6) + trame.insert(3, 155) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, gatewayByte1) + trame.insert(6, gatewayByte2) + trame.insert(7, gatewayByte3) + trame.insert(8, gatewayByte4) + + trame.insert(9, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getWIFIGateway(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 155) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + gateway = [reponse[2], reponse[3], reponse[4], reponse[5]] + return gateway + + +def setWIFIDNS(dnsByte1, dnsByte2, dnsByte3, dnsByte4, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 6) + trame.insert(3, 156) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, dnsByte1) + trame.insert(6, dnsByte2) + trame.insert(7, dnsByte3) + trame.insert(8, dnsByte4) + + trame.insert(9, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getWIFIDNS(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 156) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + dns = [reponse[2], reponse[3], reponse[4], reponse[5]] + return dns + + +def getWIFIConnectStatus(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 157) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + longueur = getResponseLength() + reponse = dobot.read(longueur) + + printByte(reponse) + + isEnabled = int(reponse[2]) + return isEnabled + + +def setLostStepParams(losingStep, isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 170) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + losingStepParam = bytearray(struct.pack("f", losingStep)) + + trame.insert(5, losingStepParam[0]) + trame.insert(6, losingStepParam[1]) + trame.insert(7, losingStepParam[2]) + trame.insert(8, losingStepParam[3]) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setLostStepCmd(isQueued = 0): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 171) + + ctrlByte = 1 + 2 * isQueued + trame.insert(4, ctrlByte) + + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setQueuedCmdStartExec(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 240) + trame.insert(4, 0x01) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setQueuedCmdStopExec(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 241) + trame.insert(4, 0x01) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setQueuedCmdForceStopExec(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 242) + trame.insert(4, 0x01) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setQueuedCmdStartDownload(totalLoop, linePerLoop): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 243) + trame.insert(4, 0x01) + + totalLoopParam = bytearray(struct.pack("f", totalLoop)) + + trame.insert(5, totalLoopParam[0]) + trame.insert(6, totalLoopParam[1]) + trame.insert(7, totalLoopParam[2]) + trame.insert(8, totalLoopParam[3]) + + linePerLoopParam = bytearray(struct.pack("f", linePerLoop)) + + trame.insert(9, linePerLoopParam[0]) + trame.insert(10, linePerLoopParam[1]) + trame.insert(11, linePerLoopParam[2]) + trame.insert(12, linePerLoopParam[3]) + + trame.insert(13, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setQueuedCmdStopDownload(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 244) + trame.insert(4, 0x01) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def setQueuedCmdClear(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 0xF5) + trame.insert(4, 0x01) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + dobot.read(getResponseLength()) + + +def getQueuedCmdCurrentIndex(): + trame = bytearray() + + trame.insert(0, 0xAA) + trame.insert(1, 0xAA) + trame.insert(2, 0x02) + trame.insert(3, 246) + trame.insert(4, 0x00) + trame.insert(5, checksum(trame)) + + dobot.write(trame) + reponse = dobot.read(getResponseLength()) + + index = int(struct.unpack("i", reponse[2:6])[0]) + increment = float(struct.unpack("i", reponse[6:10])[0]) + + return [index, increment] + + +# permet de fermer le port +def close(): + dobot.close() + + + + + diff --git a/get_egdes.py b/get_egdes.py new file mode 100644 index 0000000..f353649 --- /dev/null +++ b/get_egdes.py @@ -0,0 +1,75 @@ +import cv2 +import numpy as np +from rdp import rdp + +def edges(gray_image): + threshold_1 = 0 + threshold_diff = 40 + threshold_2 = threshold_1 + threshold_diff + edges_image = cv2.Canny(gray_image, threshold_1, threshold_2) + contours, _ = cv2.findContours(edges_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + new_contours = [] + + # here we create the contours in XYZ + + for i in range(len(contours)): + if (len(contours[i]) >= 10): + new_contours.append([contours[i][0][0][0], contours[i][0][0][1], 25]) # create a point at Z = 25 before + for j in range(len(contours[i])): + x = contours[i][j][0][0] + y = contours[i][j][0][1] + z = 0 + new_contours.append([x, y, z]) + new_contours.append([contours[i][len(contours[i]) - 1][0][0], contours[i][len(contours[i]) - 1][0][1], 25]) + iteration = 0 + + while len(new_contours) >= 6000: + threshold_1 = 40 + iteration*20 + threshold_2 = threshold_1 + threshold_diff + edges_image = cv2.Canny(gray_image, threshold_1, threshold_2) + contours, _ = cv2.findContours(edges_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + new_contours = [] + + for i in range(len(contours)): + if (len(contours[i]) >= 10): + new_contours.append([contours[i][0][0][0], contours[i][0][0][1], 25]) # create a point at Z = 25 before + for j in range(len(contours[i])): + x = contours[i][j][0][0] + y = contours[i][j][0][1] + z = 0 + new_contours.append([x, y, z]) + new_contours.append([contours[i][len(contours[i]) - 1][0][0], contours[i][len(contours[i]) - 1][0][1], 25]) + + name = "points : " + str(len(new_contours)) + ", th1 : " + str(threshold_1) + ", th2 : " + str(threshold_2) + #print(name, iteration) + + iteration += 1 + #cv2.imshow(name, edges_image) + + #cv2.waitKey(0) + coords = simplified_coords(contours) + print("nb points before optimisation : ", len(new_contours)) + print("optimisation rate : ", round((len(new_contours) - len(coords))/len(new_contours), 2)) + return coords + +def simplified_coords(contours): + old_points = [] + coords = [] + + for i in range(len(contours)): + for j in range(len(contours[i])): + x = contours[i][j][0][0] + y = contours[i][j][0][1] + if (j+1 == len(contours[i])) or j == 0: + z = 15 + else: + z = 0 + old_points.append([x, y, z]) + new_points = rdp(old_points, 1.0) + #print(new_points) + #print(len(new_points)/len(old_points)) + for k in new_points: + coords.append(k) + old_points = [] + print("nb points after optimisation : ", len(coords)) + return coords diff --git a/image.png b/image.png index 995b713..eb2758b 100644 Binary files a/image.png and b/image.png differ diff --git a/main.py b/main.py index b1675ba..3418732 100644 --- a/main.py +++ b/main.py @@ -1,42 +1,41 @@ -import cv2 +import coords_creation +import robot_control +import dobot +import serial +import time +import math -import show_img_conditions -import remove_bg -import get_head -import coordinates_change -import numpy as np -#show_img_conditions.show() +y = [] +x = [] -image_path = get_head.get_image() -image = cv2.imread(image_path) -#cv2.imshow('Image', image) -image_path = 'image.png' +coords = [] -background_path = remove_bg.rmbg(image_path) -background = cv2.imread(background_path) -#cv2.imshow('Image without background', background) +origin = [200, 0, -63] # origin of the drawing +new_size = [60, 0] # new size of the picture in mm, write 0 if you want to be proportional +angle = 90 # rotation of 180° +sym = 'y' # symetry on the x-axis +live = 1 # 0 is live, 1 is for saved image -gray_image = cv2.cvtColor(background, cv2.COLOR_BGR2GRAY) -edges = cv2.Canny(gray_image, 20, 80) -cv2.imshow("contours", edges) -cv2.waitKey(0) +coords = coords_creation.coordinates(origin, new_size, angle, sym, live) -contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) +# xmin = 128, max = 267, ymin 180 -180 -x_offset = 0 # offset in the x_axis -y_offset = 0 # offset in the y_axis -new_size = [150, 0] # new size of the picture in mm, write 0 if you want to be proportional +dobot.setQueuedCmdClear() +dobot.setQueuedCmdStartExec() +dobot.setHomeCmd() +dobot.setCPParams(175, 0, 10, 1, 1) -[x_min, y_min, x_scale, y_scale] = coordinates_change.scale_calculator(contours, new_size) +delay = 0.3 +count = 0 +for i in coords: + x = i[0] + y = i[1] + z = i[2] + count += 1 + r = math.atan2(x, y) + dobot.setCPCmd(1, x, y, z, r, 1) + time.sleep(delay) + print(round(count/len(coords)*100, 4), "%") -x_offset = x_offset - x_min -y_offset = y_offset - y_min - -new_coords = coordinates_change.coord(contours, x_offset, y_offset, x_scale, y_scale) - -size = coordinates_change.get_size(new_coords) -print("x offset : ", int(x_offset+x_min), "mm") -print("y offset : ", int(y_offset+y_min), "mm") -print("size : ", int(size[0][1] - size[0][0]), "x", int(size[1][1] - size[1][0]), "mm") diff --git a/output.png b/output.png index 5a0c5fe..7575736 100644 Binary files a/output.png and b/output.png differ diff --git a/remi.jpg b/remi.jpg new file mode 100644 index 0000000..bf2701d Binary files /dev/null and b/remi.jpg differ diff --git a/robot_control.py b/robot_control.py new file mode 100644 index 0000000..8a64cc6 --- /dev/null +++ b/robot_control.py @@ -0,0 +1,182 @@ +import matplotlib.pyplot as plt +import sys +sys.path.insert(0, './dobot/librairies') +import dobot +import time +import math +import cv2 +import numpy as np +from operator import itemgetter + +imageName = "test.jpg" +ztrace = -70 +zleve = 20 +xmin,xmax,ymin,ymax = 200,305,-74,74 +xAdder = 200 +yAdder = -74 +contoursShort = 40 + +def calibration(): + dobot.setHomeCmd() + time.sleep(20) + +def displacement(x, y, z): + dobot.setPTPjointParams(200,200,200,200,200,200,200,200,1) + r=math.atan2(y, x) + dobot.setPTPCmd(1, x, y, z, r, 1) + +def _map(x, in_min, in_max, out_min, out_max): + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + +# def takePicture(imageName): +# # Setup camera +# cap = cv2.VideoCapture(0) +# # Set a smaller resolution +# cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) +# cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) +# while True: +# # Capture frame-by-frame +# result, frame = cap.read() +# frame = cv2.flip(frame, 1) +# cv2.imshow("Webcam", frame) +# if cv2.waitKey(1) == ord('q'): +# cv2.imwrite(imageName, frame) +# break +# # When everything done, release the capture +# cap.release() +# cv2.destroyAllWindows() + +# def findROICoords(imageName): +# image = cv2.imread(imageName) +# gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) +# face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + "haarcascade_frontalface_default.xml") +# # Detect faces +# faces = face_cascade.detectMultiScale(gray, 1.1, 4) +# # Draw rectangle around the faces +# scalex = 0.9 +# scaley = 0.8 +# number = 1 +# answer = "ERROR" +# if len(faces)>0: +# for (x, y, w, h) in faces: +# x,y,w,h = int(x*scalex),int(y*scaley),int((x+w)/scalex),int((y+h)/scaley) +# test = image +# cv2.rectangle(test, (x, y), (w, h), (0, 255, 0), 2) +# cv2.imshow("Faces {}".format(number), test) +# cv2.waitKey(0) +# cv2.destroyWindow("Faces {}".format(number)) +# number += 1 +# print(x,y,w,h) +# choice = 0 +# while (choice<=0 or choice>len(faces)): +# choice = int(input("Quel visage ?")) +# print(faces) +# answer = faces[choice-1] +# x,y,w,h = answer[0], answer[1], answer[2], answer[3] +# x,y,w,h = int(x*scalex),int(y*scaley),int((x+w)/scalex),int((y+h)/scaley) +# answer = [x,y,w,h] +# return(answer) + +# def getFinalROIPicture(): +# img = cv2.imread(imageName,0) +# img = cv2.medianBlur(img,5) +# ret,th1 = cv2.threshold(img,127,255,cv2.THRESH_BINARY) +# th2 = cv2.adaptiveThreshold(img,255,cv2.ADAPTIVE_THRESH_MEAN_C,\ +# cv2.THRESH_BINARY,11,2) +# th3 = cv2.adaptiveThreshold(img,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\ +# cv2.THRESH_BINARY,13,3) +# titles = ['Original Image', 'Global Thresholding (v = 127)', +# 'Adaptive Mean Thresholding', 'Adaptive Gaussian Thresholding'] +# images = [img, th1, th2, th3] +# for i in range(4): +# plt.subplot(2,2,i+1),plt.imshow(images[i],'gray') +# plt.title(titles[i]) +# plt.xticks([]),plt.yticks([]) +# plt.show() + +# choice = 3 +# while choice not in [1,2,3]: +# choice = int(input("Quel threshold ?")) +# return(images[choice]) + +# def filteredContours(edged, contoursShort): +# # Finding Contours +# contours, hierarchy = cv2.findContours(edged, +# cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) +# print("Number of Contours found = " + str(len(contours))) + +# #Filter contours too short (points) +# newcontours = [] +# for i in range(len(contours)): +# if len(contours[i]) >= contoursShort: +# newcontours.append(contours[i]) +# tuple(newcontours) +# print(len(newcontours)) + +# cv2.drawContours(image, newcontours, -1, (0, 255, 0), 3) +# cv2.imshow('NewContours', image) +# cv2.waitKey(0) +# cv2.imwrite("Contoured.png", image) +# cv2.destroyAllWindows() +# return(newcontours) + +# def scaleFinder(image,xmin,xmax,ymin,ymax): +# rangeImageX=[0,len(image[0])] +# rangeImageY=[0,len(image)] +# xScale = rangeImageX[1]/(ymax-ymin) +# yScale = rangeImageY[1]/(xmax-xmin) + +# if xScale > yScale: +# scale = xScale +# else: +# scale=yScale +# if scale <1: +# scale = 1/scale +# return(scale) + +# dobot.setQueuedCmdClear() +# dobot.setQueuedCmdStartExec() +# dobot.setWaitCmd(1000) + +# calibration() + +# roiAnswer = "Error" +# while isinstance(roiAnswer, str): +# picture = 1 +# while picture not in [1,2]: +# picture = int(input("Prendre une photo ?")) +# if picture == 1: +# takePicture(imageName) +# roiAnswer = findROICoords(imageName) +# roi = getFinalROIPicture() +# roi = roi[roiAnswer[1]:roiAnswer[3], roiAnswer[0]:roiAnswer[2]] + +# cv2.imwrite("roi.png", roi) +# cv2.imshow('ROI',roi) +# cv2.waitKey(0) + +# image = cv2.imread('roi.png') +# scale = scaleFinder(image,xmin,xmax,ymin,ymax) +# gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) #Grayscale +# edged = cv2.Canny(gray, 80, 140) #Canny +#newcontours = filteredContours(edged,contoursShort) #Find and filter contours +#for line in range(len(newcontours)): #Rescale each point +# for point in range(len(newcontours[line])): +# for coord in range(len(newcontours[line][point])): +# newcontours[line][point][coord][0] = newcontours[line][point][coord][0]/scale + xAdder +# newcontours[line][point][coord][1] = newcontours[line][point][coord][1]/scale + yAdder +''' +leve = 0 +for ligne in newcontours: + for m in range(len(ligne)): + a = ligne[m] + ztrace = _map(ligne[m][0][0], 200, 305, -64.5, -66.5) + if m == 0: + displacement(ligne[m][0][0],ligne[m][0][1],zleve) + leve += 1 + displacement(ligne[m][0][0],ligne[m][0][1],ztrace) + if m == len(ligne)-1: + displacement(ligne[m][0][0], ligne[m][0][1],zleve) + time.sleep(2) + print(round(float(leve)/float(len(newcontours)),3)*100,"%") +''' \ No newline at end of file diff --git a/test.py b/test.py index d5a516f..e3d092b 100644 --- a/test.py +++ b/test.py @@ -1,57 +1,63 @@ -import cv2 -import numpy as np +import matplotlib.pyplot as plt +import sys +sys.path.insert(0, './dobot/librairies') +import dobot +import time +import math +dobot.setQueuedCmdClear() +dobot.setQueuedCmdStartExec() -cap = cv2.VideoCapture(0) -count = 0 -condition = False -while condition == False: - ret, frame = cap.read() - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) - blurred = cv2.GaussianBlur(gray, (3, 3), 0) - height, width = blurred.shape - face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml') - faces = face_cascade.detectMultiScale(blurred, scaleFactor=1.3, minNeighbors=5, minSize=(30, 30)) +def moyenneMobile(list, length): + mm = [] + for i in range(0, len(list) - length, 1): + sum = 0 + for k in range(0, length, 1): + sum += list[i+k] + mm.append(sum / length) + return(mm) - for (x, y, w, h) in faces: - c_x = int(x + w/2) - c_y = int(y + h/2) - center = (c_x, c_y) - top = (c_x, int(c_y-h/2*1.4)) - bottom = (c_x, int(c_y+h/2*1.1)) - left = (int(c_x - w / 2 * 0.8), c_y) - right = (int(c_x + w / 2 * 0.8), c_y) +y = [] +x = [] - c_x2 = int(( int(c_x - w / 2 * 0.8) + int(c_x + w / 2 * 0.8) )/2) - c_y2 = int(( int(c_y-h/2*1.4) + int(c_y+h/2*1.1) )/2) - ''' - cv2.circle(frame, center, 10, (255, 255, 255), 5) #center - cv2.circle(frame, top, 10, (255, 255, 255), 5) # top - cv2.circle(frame, bottom, 10, (255, 255, 255), 5) # bottom - cv2.circle(frame, left, 10, (255, 255, 255), 5) # left - cv2.circle(frame, right, 10, (255, 255, 255), 5) # right - - cv2.circle(frame, (x, y), 10, (255, 255, 255), 5) # top left - cv2.circle(frame, (x+w, y), 10, (255, 255, 255), 5) # top right - cv2.circle(frame, (x, y+h), 10, (255, 255, 255), 5) # bottom left - cv2.circle(frame, (x+h, y+h), 10, (255, 255, 255), 5) # bottom right - ''' - center_ellipse = (c_x2, c_y2) - axes = (int(w/2* 0.9), int(h/2*1.4)) # Major axis, Minor axis - angle = 0 # Rotation angle - color = (0, 255, 0) # Color in BGR format - thickness = 2 +dobot.setCPParams(120,100,10,1,1) +dobot.setHomeCmd() +time.sleep(20) +delay = 2 - # Draw the ellipse on the image - #cv2.ellipse(frame, center_ellipse, axes, angle, 0, 360, color, thickness) - mask = np.zeros((height, width), dtype=np.uint8) - cv2.ellipse(mask, center_ellipse, axes, angle, 0, 360, 255, -1) - result = cv2.bitwise_and(frame, frame, mask=mask) +dobot.setHomeParams(200, 50, 0, 0, 1) +time.sleep(delay) +dobot.setCPCmd(1, 200, 100, 0, 0, 1) +time.sleep(delay) +dobot.setCPCmd(1, 100, 100, 0, 0, 1) +time.sleep(delay) +dobot.setCPCmd(1, 200, -100, 0, 0, 1) +time.sleep(delay) +dobot.setCPCmd(1, 200, 100, 0, 0, 1) +time.sleep(delay) - cv2.imshow('Head Detection', result) - if cv2.waitKey(1) & 0xFF == ord('q'): - break - -cap.release() -cv2.destroyAllWindows() +#dobot.setCPCmd(1, 250, -100, 0, 0, 1) +#dobot.setCPCmd(1, 250, 0, 0, 0, 1) +#dobot.setCPCmd(1, 170, 0, 0, 0, 1) +# Parameters: +# x - X coordinate for home position. +# y - Y coordinate for home position. +# z - Z coordinate for home position. +# r - Peripheral rotation at home position. +# Function: +# dobot.setCPCmd(api, x, y, z, r, isQueued) +''' +delay = 0.03 +for i in range(0, 150): + valeurs = dobot.getPose() + y.append(-valeurs[1]) + x.append(valeurs[0]) + time.sleep(delay) +# On trace le graphique +plt.xlabel("y") +plt.ylabel("x") +plt.plot(y, x, '.r') +#plt.axis([-90, 120, 150, 250]) +plt.axis([-300, 300, -300, 300]) +plt.show()''' \ No newline at end of file diff --git a/test2.py b/test2.py index 6f7e7a2..2919615 100644 --- a/test2.py +++ b/test2.py @@ -1,35 +1,23 @@ -import cv2 +import matplotlib.pyplot as plt +import sys +sys.path.insert(0, './dobot/librairies') +import dobot +import time +import math -# Open a connection to the webcam (default camera index is 0) -cap = cv2.VideoCapture(0) +# xmin = 128, max = 267, ymin 180 -180 +dobot.setQueuedCmdClear() +dobot.setQueuedCmdStartExec() +dobot.setHomeCmd() -# Check if the camera is opened successfully -if not cap.isOpened(): - print("Error: Could not open the camera.") - exit() -# Create a window to display the webcam feed -window_name = 'Webcam Feed' -cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) +dobot.setCPParams(175,0,10,1,1) -while True: - # Read a frame from the webcam - ret, frame = cap.read() +for i in range(3): + dobot.setCPCmd(1, 200, 0, 50, 0, 1) + dobot.setCPCmd(1, 250, 0, -50, 0, 1) + dobot.setCPCmd(1, 180, 0, -50, 0, 1) + print(dobot.getPose()) + time.sleep(2) - # Check if the frame is read successfully - if not ret: - print("Error: Could not read frame.") - break - # Display the frame in the window - cv2.imshow(window_name, frame) - - # Break the loop if 'q' key is pressed - if cv2.waitKey(1) & 0xFF == ord('q'): - break - -# Release the camera capture object -cap.release() - -# Destroy the OpenCV window -cv2.destroyAllWindows() diff --git a/test_dobot.py b/test_dobot.py new file mode 100644 index 0000000..db0718e --- /dev/null +++ b/test_dobot.py @@ -0,0 +1,22 @@ +import robot_control +import dobot +import serial +import time +import math + +dobot.setQueuedCmdClear() +dobot.setQueuedCmdStartExec() +dobot.setHomeCmd() +dobot.setCPParams(175, 0, 10, 1, 1) + +delay = 1 +count = 0 +for i in range(4): + x = 10*i + 100 + y = 10*i + 50 + z = 10*i + 50 + count += 1 + r = math.atan2(x, y) + dobot.setCPCmd(1, x, y, z, r, 1) + time.sleep(delay) +