# -*- coding: utf-8 -*-
from protocol import Protocol
import time
proto = Protocol('com4')
proto.move_servo(0, 1.)
alfa2 = math.acos( (b*b + k*k - n*n) / ( 2 * b * k) )
angle2 = alfa1 + alfa2
return (angle1, angle2 - (18.19 / 180. * math.pi), angle3)
count = 0.0
proto = Protocol('com4')
while 1:
count = count + 0.1
#pos = (((math.cos(count) + 1.0) / 2.0) * (-120.0) - 100.0, 0.0)
pos = [-140., 0.0]
res = calc_angles(a, c, k, angle1, pos)
proto.move_servo(0, 2, res[0]/math.pi)
proto.move_servo(0, 1, res[1]/math.pi)
proto.move_servo(0, 0, res[2]/math.pi)
#for id, value in enumerate(res):
# proto.move_servo(0, id, value/math.pi)
time.sleep(0.01)
print "pos:{0} angle1:{1} angle2:{2} angle3:{3}".format(pos, res[0]/math.pi * 180., res[1]/math.pi * 180., res[2]/math.pi * 180.)
if self.cur_time >= a1[0] and self.cur_time < a2[0]:
p1 = a1[1]
p2 = a2[1]
k = (self.cur_time - a1[0]) / (a2[0] - a1[0])
return [p1[0] + (p2[0] - p1[0]) * k, p1[1] + (p2[1] - p1[1]) * k]
anim = Animation()
get_angles(anim.get_pos())
proto = Protocol('com4')
r = 80.
a = 0.0
d_a = math.pi / 180.
y = 60
while 1:
#1. тест echo
pos = [math.cos(a) * r + 140, math.sin(a) * r - y]
#angles = get_angles(anim.get_pos())
angles = get_angles(pos)
proto.move_servo(2, angles[0])
proto.move_servo(3, angles[1])
anim.inc_time()
#time.sleep(anim.dt)
a = a + d_a
r = (math.cos(a / 2.) + 1.) / 2. * 40. +20
y = (math.cos(a / 2.) + 1.) / 2. * 60
# -*- coding: utf-8 -*-
from protocol import Protocol
import time
proto = Protocol('com4')
while 1:
#1. тест echo
for i in range(2):
proto.echo()
time.sleep(1)
for i in range(180):
proto.move_servo(3, i)
time.sleep(0.1)
