optimize the code

Mybuddy/mybuddy/scripts/follow_display.py

@@ -76,13 +76,13 @@ def talker():
         joint_state_send.header.stamp = rospy.Time.now()
 
         # =======left_radians=======
-        lencoder = mb.get_encoders(1)
+        # lencoder = mb.get_encoders(1)
-        left_radians = []
+        # left_radians = []
-        for index, value in enumerate(lencoder):
+        # for index, value in enumerate(lencoder):
-            value = (value-2048)*2*math.pi/4096
+        #     value = (value-2048)*2*math.pi/4096
-            left_radians.append(value)
+        #     left_radians.append(value)
 
-        # left_radians = mb.get_radians(1)
+        left_radians = mb.get_radians(1)
         # print("left_radians: %s" % left_radians)
         # if not left_radians:
         #     left_radians = [-0.008, 0.073, -0.008, 0.162, -0.479, 0.767]
@@ -91,13 +91,13 @@ def talker():
         #     print("left_radians: %s" % left_radians)
         
         # =======right_radians=======
-        rencoder = mb.get_encoders(2)
+        # rencoder = mb.get_encoders(2)
-        right_radians = []
+        # right_radians = []
-        for index, value in enumerate(rencoder):
+        # for index, value in enumerate(rencoder):
-            value = (value-2048)*2*math.pi/4096
+        #     value = (value-2048)*2*math.pi/4096
-            right_radians.append(value)
+        #     right_radians.append(value)
 
-        # right_radians = mb.get_radians(2)
+        right_radians = mb.get_radians(2)
         # # print("right_radians: %s" % right_radians)
         # if not right_radians:
         #     right_radians = [-0.008, -0.073, -0.008, 0.162, -0.479, 0.767]
@@ -107,18 +107,22 @@ def talker():
 
         # =======waist_radian=======
 
-        wencoder = mb.get_encoder(3,1)
+        # wencoder = mb.get_encoder(3,1)
-        waist_radian = []
+        wangles = mb.get_angles(3)[0]*(math.pi/180)
+        print("wangles : %s" % wangles)
+        # waist_radian =wangles[0]*(math.pi/180)
+        waist_radian =[]
+        waist_radian.append(wangles)
+        print("waist_radian : %s" % waist_radian)
+
         # for index, value in enumerate(wencoder):
         #     value = (value-2048)*2*math.pi/4096
-        waist_radian.append((wencoder-2048)*2*math.pi/4096)
+        # waist_radian.append((wencoder-2048)*2*math.pi/4096)
         # print("waist_radian: %s "%waist_radian)
         # if not waist_radian:
         #     waist_radian = [0]
         #     print("set waist_radian:%s" % waist_radian)
             
-        # mb.set_encoder(3,1,waist_radian[0]*4096/(2*math.pi)+2048,1)
-
 
         # =======all_radians=======
         all_radians = left_radians + right_radians + waist_radian
@@ -132,23 +136,24 @@ def talker():
 
         # =======left_coords=======
         left_coords = mb.get_coords(1)
-        print("left_coords: %s" % left_coords)
+        # print("left_coords: %s" % left_coords)
-        if not left_coords:
+        # if not left_coords:
-            # rospy.loginfo("error [101]: can not get left_coord values")
+        #     # rospy.loginfo("error [101]: can not get left_coord values")
-            left_coords = [-50.4, 63.4, 411.6, -91.23, -0.08, -90.08]
+        #     left_coords = [-50.4, 63.4, 411.6, -91.23, -0.08, -90.08]
             # print("set lc:",left_coords)
 
         # =======right_coords=======
         right_coords = mb.get_coords(2)
-        print("right_coords: %s " % right_coords)
+        # print("right_coords: %s " % right_coords)
-        if not right_coords:
+        # if not right_coords:
-            # rospy.loginfo("error [101]: can not get right_coords values")
+        #     # rospy.loginfo("error [101]: can not get right_coords values")
-            right_coords = [50.4, -63.4, 411.6, -91.23, -0.08, -90.08]
+        #     right_coords = [50.4, -63.4, 411.6, -91.23, -0.08, -90.08]
         #     # print("set rc:",right_coords)
         # right_coords = [50.4, -63.4, 411.6, -91.23, -0.08, -90.08]
 
-        waist_coords = mb.get_coords(3)
+        # waist_coords = mb.get_coords(3)
-        print("waist_coords: %s " % waist_coords)
+        waist_coords = mb.get_angles(3)
+        print("waist_coords: %s " % waist_coords[0])
         # coords = left_coords + right_coords
         # print("all_coords:%s" % coords) 
 

Mybuddy/mybuddy/scripts/slider_control.py

@@ -36,17 +36,16 @@ def callback(data):
     print("right_arm: %s" % data_list2)
     print("waist: %s" % data_list3)
 
-    print("\n")
     mb.send_radians(1,data_list1, 50)
-    time.sleep(0.05)
+    time.sleep(0.02)
     mb.send_radians(2,data_list2, 50)
     time.sleep(0.02)
 
-    # print(data_list3[0])
+    print(data_list3[0]* (180 / math.pi))
-    # mb.send_angle(3,1,data_list3[0]* (180 / math.pi),350)
+    print("\n")
-    # mb.send_radians(3,data_list3, 50)
 
-    mb.set_encoder(3,1,data_list3[0]*4096/(2*math.pi)+2048,1)
+    mb.send_angle(3, 1, data_list3[0]* (180 / math.pi), 10)
+    # mb.send_radians(3,data_list3, 50)
     time.sleep(0.02)
 
 def listener():

Mybuddy/mybuddy/scripts/test_control.py

@@ -37,7 +37,7 @@ mb = MyBuddy(port, baud)
 # mb.send_radians(2,[-0.136, -0.462, -0.62, 0.813, 0.218, -0.071],30)
 # mb.send_angles(2,[6.32, -94.39, 13.62, -15.38, -94.21, 62.84],30,1)
 # time.sleep(2)
-# print(mb.get_angles(2))
+print(mb.get_angles(3))
 # mb.send_angles(1,[0,0,0,0,0,0],30,0)
 # mb.send_angle(1,3,30,30)
 # mb.set_encoder(3,1,2048,1)

Mybuddy/mybuddy_moveit/scripts/sync_plan.py

@@ -33,7 +33,6 @@ def callback(data):
     mb.set_encoder(3,1,data_list3[0]*4096/(2*math.pi)+2048,1)
     time.sleep(0.02)
 
-
 def listener():
     global mb
     rospy.init_node("mybuddy_reciver", anonymous=True)
@@ -47,6 +46,5 @@ def listener():
     # spin() simply keeps python from exiting until this node is stopped
     rospy.spin()
 
-
 if __name__ == "__main__":
     listener()