kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-05 19:47:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

optimize the code

Browse source
This commit is contained in:
2929ss 2022-07-18 10:29:50 +08:00
parent 765e3f1c67
commit 3c4c088eca
4 changed files with 38 additions and 36 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

59
Mybuddy/mybuddy/scripts/follow_display.py
View file

@ -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)

11
Mybuddy/mybuddy/scripts/slider_control.py
View file

@ -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():

2
Mybuddy/mybuddy/scripts/test_control.py
View file

@ -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)

2
Mybuddy/mybuddy_moveit/scripts/sync_plan.py
View file

@ -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()