import matplotlib.pyplot as plt
import sys
#sys.path.insert(0, './dobot/librairies')
import dobot
import time

dobot.setQueuedCmdClear()
dobot.setQueuedCmdStartExec()

def moyenneMobile(list, length):
    mm = []
    for i in range(0, len(list) - length, 1):
        sum = 0
        for k in range(0, length, 1):
            sum += list[i+k]

        mm.append(sum / length)
    return(mm)

y = []
z = []

dobot.setPTPJumpParams(60,130,1)
dobot.setPTPCmd(0, 259.6, 111, 9.8, 0, 1)
dobot.setPTPCmd(0, 206, -130, 52.8, 0, 1)

print(dobot.getPTPJumpParams())


delay = 0.02

for i in range(0, 300):
    valeurs = dobot.getPose()
    y.append(-valeurs[1])
    z.append(valeurs[2])

    time.sleep(delay)


# On trace le graphique

plt.xlabel("y")
plt.ylabel("z")
plt.plot(y, z, 'r')
plt.axis([-130, 150, 0, 150])

plt.show()