# -*- 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)