diff --git a/Mybuddy/mybuddy/scripts/follow_display.py b/Mybuddy/mybuddy/scripts/follow_display.py index 872e7d5..14e496d 100755 --- a/Mybuddy/mybuddy/scripts/follow_display.py +++ b/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) - left_radians = [] - for index, value in enumerate(lencoder): - value = (value-2048)*2*math.pi/4096 - left_radians.append(value) + # lencoder = mb.get_encoders(1) + # left_radians = [] + # for index, value in enumerate(lencoder): + # value = (value-2048)*2*math.pi/4096 + # 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) - right_radians = [] - for index, value in enumerate(rencoder): - value = (value-2048)*2*math.pi/4096 - right_radians.append(value) + # rencoder = mb.get_encoders(2) + # right_radians = [] + # for index, value in enumerate(rencoder): + # value = (value-2048)*2*math.pi/4096 + # 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) - waist_radian = [] + # wencoder = mb.get_encoder(3,1) + 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) - if not left_coords: - # rospy.loginfo("error [101]: can not get left_coord values") - left_coords = [-50.4, 63.4, 411.6, -91.23, -0.08, -90.08] + # print("left_coords: %s" % left_coords) + # if not left_coords: + # # rospy.loginfo("error [101]: can not get left_coord values") + # 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) - if not right_coords: - # rospy.loginfo("error [101]: can not get right_coords values") - right_coords = [50.4, -63.4, 411.6, -91.23, -0.08, -90.08] + # print("right_coords: %s " % right_coords) + # if not right_coords: + # # rospy.loginfo("error [101]: can not get right_coords values") + # 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) - print("waist_coords: %s " % waist_coords) + # waist_coords = mb.get_coords(3) + waist_coords = mb.get_angles(3) + print("waist_coords: %s " % waist_coords[0]) # coords = left_coords + right_coords # print("all_coords:%s" % coords) diff --git a/Mybuddy/mybuddy/scripts/slider_control.py b/Mybuddy/mybuddy/scripts/slider_control.py index 3ca5c64..9b84f1e 100755 --- a/Mybuddy/mybuddy/scripts/slider_control.py +++ b/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]) - # mb.send_angle(3,1,data_list3[0]* (180 / math.pi),350) - # mb.send_radians(3,data_list3, 50) + print(data_list3[0]* (180 / math.pi)) + print("\n") - 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(): diff --git a/Mybuddy/mybuddy/scripts/test_control.py b/Mybuddy/mybuddy/scripts/test_control.py index 11d2e61..c1f0bd7 100644 --- a/Mybuddy/mybuddy/scripts/test_control.py +++ b/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) diff --git a/Mybuddy/mybuddy_moveit/scripts/sync_plan.py b/Mybuddy/mybuddy_moveit/scripts/sync_plan.py index a348260..5748108 100755 --- a/Mybuddy/mybuddy_moveit/scripts/sync_plan.py +++ b/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()