lines = []
updatexy(t)
p[2] = y + 40
p[1] = x
test.update_destination_point(p, s)
lines = to_lines(test.joint_points)
lines2 = to_lines(test.foward_model(test.joint_angle_only))
for _line in lines2:
lines.append(_line)
g.redraw(lines)
# sys.stdout.write("\r" + test.return_model())
print test.return_model_for_low_level()
test.send_serial()
sys.stdout.flush()
test = RoverArm([50, 40, 15])
test.update_destination_point([40, 0, 40], [1, 0, 0])
test.ros_begin()
test.send_serial()
lines = to_lines(test.joint_points)
# test.establish_serial_connection()
# test.serial_write()
g = Grapher(lines)
g.redraw(lines)
while not test.my_rospy.is_shutdown():
g.show(anim)
