diff --git a/Mybuddy/mybuddy/scripts/follow_display.py b/Mybuddy/mybuddy/scripts/follow_display.py index 202bfbf..872e7d5 100755 --- a/Mybuddy/mybuddy/scripts/follow_display.py +++ b/Mybuddy/mybuddy/scripts/follow_display.py @@ -117,7 +117,7 @@ def talker(): # waist_radian = [0] # 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======= @@ -145,9 +145,10 @@ def talker(): # 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] - 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 # print("all_coords:%s" % coords) @@ -170,6 +171,10 @@ def talker(): marker_.pose.position.y = right_coords[0] / 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.r = 0.0 marker_.color.g = 1.0 diff --git a/Mybuddy/mybuddy/scripts/slider_control.py b/Mybuddy/mybuddy/scripts/slider_control.py index 2c491a8..3ca5c64 100755 --- a/Mybuddy/mybuddy/scripts/slider_control.py +++ b/Mybuddy/mybuddy/scripts/slider_control.py @@ -43,7 +43,7 @@ def callback(data): time.sleep(0.02) # 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.set_encoder(3,1,data_list3[0]*4096/(2*math.pi)+2048,1) diff --git a/Mybuddy/mybuddy/scripts/test_control.py b/Mybuddy/mybuddy/scripts/test_control.py index b0fd257..11d2e61 100644 --- a/Mybuddy/mybuddy/scripts/test_control.py +++ b/Mybuddy/mybuddy/scripts/test_control.py @@ -12,7 +12,7 @@ mb = MyBuddy(port, baud) # mb.set_free_mode(2) # mb.power_on() # mb.release_all_servos() -# mb.set_servo_calibration(1,4) +# mb.set_servo_calibration(3,1) # i = 1 # while i < 7: @@ -23,7 +23,7 @@ mb = MyBuddy(port, baud) ''' ------------get info-------------- ''' -# print(mb.get_radians(2)) +# print(mb.get_radians(1)) # print(mb.get_encoders(3)) # print(mb.get_encoder(1,1)) # print(mb.get_angles(1)) @@ -40,5 +40,5 @@ mb = MyBuddy(port, baud) # print(mb.get_angles(2)) # mb.send_angles(1,[0,0,0,0,0,0],30,0) # mb.send_angle(1,3,30,30) -# mb.set_encoder() +# mb.set_encoder(3,1,2048,1) time.sleep(1) \ No newline at end of file