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()