import super_servo_functions as super
if __name__ == "__main__":
super.init()
super.enableAll()
while True:
x = float(raw_input("Enter Target x:"))
y = float(raw_input("Enter Target y:"))
z = float(raw_input("Enter Target z:"))
a0, a1, a2, a3, a4, isPossible = super.processIK(x, y, z)
super.setSpeedAll(120)
if isPossible:
print("Output Angles are:", a0, a1, a2, a3, a4)
a = raw_input("Enter Any Key to Move to Angles ")
super.move2angle(a0, a1, a2, a3, a4)
else:
pass
elif mode == 2:
super.move2standard()
elif mode == 3:
super.move2standard_norm()
elif mode == 11:
super.disableAll()
msg = bot()
msg.nav = True
pub.publish(msg)
def listener():
rospy.init_node("Dynamixel_listener",anonymous=True)
rospy.Subscriber("Servo_Angles",Angles,callbackAngle)
rospy.Subscriber("Servo_Position",Position,callbackPos)
rospy.Subscriber("Dyx",Dyx2,callbackD)
while not rospy.is_shutdown():
rospy.spin()
if __name__ == "__main__":
super.init() #Enable the AX-12 Communication Line
super.enableAll() #Enable Torque Control on All Servo Motors
listener() #Run the Listener Node
