2535 lines
62 KiB
Python
2535 lines
62 KiB
Python
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()
|
||
except (SerialException):
|
||
1
|
||
|
||
# Connexion au Dobot
|
||
|
||
dobot = serial.Serial()
|
||
dobot.baudrate = 115200
|
||
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()
|