import pypot.dynamixel
import pypot.robot

ports = pypot.dynamixel.get_available_ports()
print 'Found ports', ports
port = ports[0]
print 'Using', port

io = pypot.dynamixel.DxlIO(port)

ids = io.scan(range(40))
print 'Found ids', ids

motors = [pypot.dynamixel.motor.DxlMXMotor(id) for id in ids]
motors

c = pypot.dynamixel.controller.BaseDxlController(io, motors)

robot = pypot.robot.Robot([c])

robot.start_sync()

for m in robot.motors:
    m.compliant = False
    m.goal_position = 0

from pypot.primitive.utils import Sinus

s1 = Sinus(robot, 50, robot.motors, amp=30)
s2 = Sinus(robot, 50, robot.motors, amp=5, freq=2)

s1.start()

%pylab inline

import time

pos = []

start = time.time()
while time.time() - start < 5:
    pos.append(robot.motors[0].present_position)
    time.sleep(0.02)
    
plot(linspace(0, 5, len(pos)), pos)

s2.start()

import time

pos = []

start = time.time()
while time.time() - start < 5:
    pos.append(robot.motors[0].present_position)
    time.sleep(0.02)
    
plot(linspace(0, 5, len(pos)), pos)

s1.stop()

import time

pos = []

start = time.time()
while time.time() - start < 5:
    pos.append(robot.motors[0].present_position)
    time.sleep(0.02)
    
plot(linspace(0, 5, len(pos)), pos)

s2.stop()

import time

pos = []

s1.start()
s2.start()
start = time.time()
while time.time() - start < 10:
    pos.append(robot.motors[0].present_position)
    time.sleep(0.02)
    
    if (3 < time.time() - start < 9):
        s2.pause()
    else:
        s2.resume()
    
plot(linspace(0, 10, len(pos)), pos)

s1.stop()
s2.stop()