import matplotlib.pyplot as plt
import sys
sys.path.insert(0, './dobot/librairies')
import dobot
import time
import math
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 = []
x = []

dobot.setCPParams(120,100,10,1,1)
dobot.setHomeCmd()
time.sleep(20)
delay = 2


dobot.setHomeParams(200, 50, 0, 0, 1)
time.sleep(delay)
dobot.setCPCmd(1, 200, 100, 0, 0, 1)
time.sleep(delay)
dobot.setCPCmd(1, 100, 100, 0, 0, 1)
time.sleep(delay)
dobot.setCPCmd(1, 200, -100, 0, 0, 1)
time.sleep(delay)
dobot.setCPCmd(1, 200, 100, 0, 0, 1)
time.sleep(delay)

#dobot.setCPCmd(1, 250, -100, 0, 0, 1)
#dobot.setCPCmd(1, 250, 0, 0, 0, 1)
#dobot.setCPCmd(1, 170, 0, 0, 0, 1)
# Parameters:
# x - X coordinate for home position.
# y - Y coordinate for home position.
# z - Z coordinate for home position.
# r - Peripheral rotation at home position.
# Function:
# dobot.setCPCmd(api, x, y, z, r, isQueued)
'''
delay = 0.03
for i in range(0, 150):
    valeurs = dobot.getPose()
    y.append(-valeurs[1])
    x.append(valeurs[0])
    time.sleep(delay)

# On trace le graphique
plt.xlabel("y")
plt.ylabel("x")
plt.plot(y, x, '.r')
#plt.axis([-90, 120, 150, 250])
plt.axis([-300, 300, -300, 300])
plt.show()'''