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

add waist coord

Browse source
This commit is contained in:
2929ss 2022-06-20 11:52:10 +08:00
parent 70820e84d1
commit 765e3f1c67
3 changed files with 12 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -117,7 +117,7 @@ def talker():
# waist_radian = [0] # waist_radian = [0]
# print("set waist_radian:%s" % waist_radian) # print("set waist_radian:%s" % waist_radian)
mb.set_encoder(3,1,waist_radian[0]*4096/(2*math.pi)+2048,1) # mb.set_encoder(3,1,waist_radian[0]*4096/(2*math.pi)+2048,1)
# =======all_radians======= # =======all_radians=======
@ -145,9 +145,10 @@ def talker():
# 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) # # print("set rc:",right_coords)
# 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] waist_coords = mb.get_coords(3)
print("waist_coords: %s " % waist_coords)
# coords = left_coords + right_coords # coords = left_coords + right_coords
# print("all_coords:%s" % coords) # print("all_coords:%s" % coords)
@ -170,6 +171,10 @@ def talker():
marker_.pose.position.y = right_coords[0] / 1000 marker_.pose.position.y = right_coords[0] / 1000
marker_.pose.position.z = right_coords[2] / 1000 marker_.pose.position.z = right_coords[2] / 1000
time.sleep(0.02)
marker_.pose.position.x = waist_coords[1] / 1000 * -1
marker_.color.a = 1.0 marker_.color.a = 1.0
marker_.color.r = 0.0 marker_.color.r = 0.0
marker_.color.g = 1.0 marker_.color.g = 1.0

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

@ -43,7 +43,7 @@ def callback(data):
time.sleep(0.02) time.sleep(0.02)
# print(data_list3[0]) # print(data_list3[0])
# mb.send_angle(3,1,data_list3[0]* (180 / math.pi),50) # mb.send_angle(3,1,data_list3[0]* (180 / math.pi),350)
# mb.send_radians(3,data_list3, 50) # mb.send_radians(3,data_list3, 50)
mb.set_encoder(3,1,data_list3[0]*4096/(2*math.pi)+2048,1) mb.set_encoder(3,1,data_list3[0]*4096/(2*math.pi)+2048,1)

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

@ -12,7 +12,7 @@ mb = MyBuddy(port, baud)
# mb.set_free_mode(2) # mb.set_free_mode(2)
# mb.power_on() # mb.power_on()
# mb.release_all_servos() # mb.release_all_servos()
# mb.set_servo_calibration(1,4) # mb.set_servo_calibration(3,1)
# i = 1 # i = 1
# while i < 7: # while i < 7:
@ -23,7 +23,7 @@ mb = MyBuddy(port, baud)
''' '''
------------get info-------------- ------------get info--------------
''' '''
# print(mb.get_radians(2)) # print(mb.get_radians(1))
# print(mb.get_encoders(3)) # print(mb.get_encoders(3))
# print(mb.get_encoder(1,1)) # print(mb.get_encoder(1,1))
# print(mb.get_angles(1)) # print(mb.get_angles(1))
@ -40,5 +40,5 @@ mb = MyBuddy(port, baud)
# print(mb.get_angles(2)) # print(mb.get_angles(2))
# mb.send_angles(1,[0,0,0,0,0,0],30,0) # mb.send_angles(1,[0,0,0,0,0,0],30,0)
# mb.send_angle(1,3,30,30) # mb.send_angle(1,3,30,30)
# mb.set_encoder() # mb.set_encoder(3,1,2048,1)
time.sleep(1) time.sleep(1)