Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

yac servo ctrl position update #30

Merged
merged 2 commits into from
Aug 26, 2023
Merged

yac servo ctrl position update #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4da9e58
yac servo ctrl position update
ggh-png Aug 26, 2023
fd96dc5
Merge branch 'main' of https://github.com/dawan0111/yacyac into featu…
ggh-png Aug 26, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion vision_opencv
Open in desktop
Submodule vision_opencv deleted from 066793
Binary file added vision_opencv.zip
View file
Open in desktop
Binary file not shown.
6 changes: 3 additions & 3 deletions yacyac_core/bt_xml/bt_yac_supply.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@

<BehaviorTree ID="YacSupply">
<Sequence>
<SetBlackboard output_key="Goal_a" value="6;0;0;0;0;0;0;0" />
<SetBlackboard output_key="Goal_a" value="6;0;0;0;0;0;3;0" />
<SetBlackboard output_key="Goal_b" value="0;0;0;0;6;0;0;0" />
<SetBlackboard output_key="Goal_c" value="0;0;0;0;0;0;6;0" />

<Sequence name="SetGoal">
<YacSupplyCilent goal="{Goal_a}" />
<YacSupplyCilent goal="{Goal_b}" />
<YacSupplyCilent goal="{Goal_c}" />
<!-- <YacSupplyCilent goal="{Goal_b}" /> -->
<!-- <YacSupplyCilent goal="{Goal_c}" /> -->
</Sequence>
</Sequence>
</BehaviorTree>
Expand Down
212 changes: 139 additions & 73 deletions yacyac_servo/yacyac_servo/action_servo_ctrl.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
from yacyac_interface.action import Supply as SupplyAction
from dynamixel_sdk_custom_interfaces.msg import SetPosition
from dynamixel_sdk_custom_interfaces.srv import GetPosition
from std_msgs.msg import Int32

# 목적 포지션을 받으면 그에 맞게 서보를 움직임
# 목적 포지션에 도달하면 도착 메시지를 보냄
Expand All @@ -26,14 +27,29 @@ def __init__(self):
# 현재 포지션을 서비스 클라이언트로 받아옴
self.cli = self.create_client(GetPosition, "/get_position")


# 현재 알약 배출개수 펍
self.pub_yac = self.create_publisher(Int32, "/yacyac/current_yac", 10)

self.cmd_pose_id = Servo()
self.cnt = 0

self.current_yac = 0


# 큰약
# 목적 포지션 list
# 1023 to 300 degree
# 시계방향으로 돌아가는 포지션 리스트
self.big_position_set = [0, 164, 328, 492, 656, 820]

# 작은약
# 목적 포지션 list
# 1023 to 360 degree
# 1023 to 300 degree
# 시계방향으로 돌아가는 포지션 리스트
self.cw_position_set = [0, 164, 328, 492, 656, 820]
self.small_position_set = [60, 224, 388, 552, 716, 880]
# 반 시계방향으로 돌아가는 포지션 리스트
self.ccw_position_set = [915,751, 587,423, 259]

# 방향 flag
self.position_flag = [0, 0, 0, 0, 0, 0, 0, 0, 0]
# 현재 포지션 index
Expand All @@ -45,11 +61,11 @@ def __init__(self):
self.yac_cnt = 0

print ("init positioning...")
# for i in range(8):
# # 근처 포지션으로 이동합니다.
# self.init_position(i)
# # 원점 포지션으로 이동합니다.
# # self.reset_position(i)
for i in range(8):
# 근처 포지션으로 이동합니다.
self.init_position(i)
# 원점 포지션으로 이동합니다.
# self.reset_position(i)
print ("init positioning done!!!")


Expand All @@ -64,7 +80,7 @@ def execute_callback(self, goal_handle):
for idx in range(len(supply_list)):
if supply_list[idx] != 0:
self.control_position(idx, supply_list[idx], goal_handle)

self.current_yac = 0
goal_handle.succeed()

result = SupplyAction.Result()
Expand All @@ -77,7 +93,6 @@ def execute_callback(self, goal_handle):




def init_position(self, id):
req = GetPosition.Request()
req.id = id
Expand All @@ -97,45 +112,54 @@ def init_position(self, id):

cmd_pose = SetPosition()
cmd_pose.id = id
flag = 0
for i, val in enumerate(self.cw_position_set):
diff = abs(response.position - val)
if diff < min_diff:
min_diff = diff
closest_idx = i
cmd_pose.position = self.cw_position_set[closest_idx]
flag = 1
for i, val in enumerate(self.ccw_position_set):
diff = abs(response.position - val)
if diff < min_diff:
min_diff = diff
closest_idx = i
cmd_pose.position = self.ccw_position_set[closest_idx]
flag = 0
if flag == 0:
self.position_flag[id] = 1
# print("cw")
else:
self.position_flag[id] = 0
if id < 2 :
for i, val in enumerate(self.big_position_set):
diff = abs(response.position - val)
if diff < min_diff:
min_diff = diff
closest_idx = i
cmd_pose.position = self.big_position_set[closest_idx]

if closest_idx > 3:
self.position_flag[id] = 1
else:
self.position_flag[id] = 0

# print("ccw")
# 리스트의 요소값과 같은 값이 있으면 그 인덱스를 반환
if flag:
for i, val in enumerate(self.cw_position_set):
# 리스트의 요소값과 같은 값이 있으면 그 인덱스를 반환

for i, val in enumerate(self.big_position_set):
if val == cmd_pose.position:
self.position_cnt[id] = i
self.position_cnt[id] = i
break
else:
for i, val in enumerate(self.ccw_position_set):

else :
for i, val in enumerate(self.small_position_set):
diff = abs(response.position - val)
if diff < min_diff:
min_diff = diff
closest_idx = i
cmd_pose.position = self.small_position_set[closest_idx]

if closest_idx > 3:
self.position_flag[id] = 1
else:
self.position_flag[id] = 0

# print("ccw")
# 리스트의 요소값과 같은 값이 있으면 그 인덱스를 반환

for i, val in enumerate(self.small_position_set):
if val == cmd_pose.position:
self.position_cnt[id] = i
break
self.position_cnt[id] = i
break

self.pub.publish(cmd_pose)
time.sleep(0.1)
# time.sleep(0.1)
else:
self.get_logger().info(f"Service call for ID {id} failed")



def control_position(self, id, servo, goal_handle):
# 제조할 약의 개수
yac_num = servo
Expand All @@ -146,53 +170,96 @@ def control_position(self, id, servo, goal_handle):
# 역방향 진행
yac_num -= 1
yac_cnt += 1
self.current_yac += 1
msg = Int32()
msg.data = self.current_yac
self.pub_yac.publish(msg)
# yac_cnt
print(id, "번 약 ", servo, "개중 ", yac_cnt, "개 배출중...")
feedback_msg = SupplyAction.Feedback()
self.yac_cnt += 1
feedback_msg.partial_sequence.append(self.yac_cnt)
goal_handle.publish_feedback(feedback_msg)
# self.get_logger().info('Feedback: {0}'.format(feedback_msg.partial_sequence))
if self.position_flag[id] == 1:
cmd_pose = SetPosition()
self.position_cnt[id] += 1
if self.position_cnt[id] >= len(self.ccw_position_set):
self.position_cnt[id] = 0
self.position_flag[id] = 0
cmd_pose.position = self.cw_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
else:
cmd_pose.position = self.ccw_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
else :
cmd_pose = SetPosition()
self.position_cnt[id] += 1
if self.position_cnt[id] >= len(self.cw_position_set):
self.position_cnt[id] = 0
self.position_flag[id] = 1
cmd_pose.position = self.ccw_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
else:
cmd_pose.position = self.cw_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)

# print(id, "번 약 ", servo, "개중 ", yac_cnt, "개 배출중...")

if id < 2:
print("큰약")
if self.position_flag[id] == 1:
cmd_pose = SetPosition()
self.position_cnt[id] += 1
if self.position_cnt[id] == len(self.big_position_set):
self.position_cnt[id] = 4
self.position_flag[id] = 0
cmd_pose.position = self.big_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
else:
cmd_pose.position = self.big_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
# print(self.position_cnt)
else :
cmd_pose = SetPosition()
self.position_cnt[id] -= 1
if self.position_cnt[id] < 0:
self.position_cnt[id] = 1
self.position_flag[id] = 1
cmd_pose.position = self.big_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
else:
cmd_pose.position = self.big_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
# print(self.position_cnt)



else:
print("작은약")
if self.position_flag[id] == 1:
cmd_pose = SetPosition()
self.position_cnt[id] += 1
if self.position_cnt[id] == len(self.small_position_set):
self.position_cnt[id] = 4
self.position_flag[id] = 0
cmd_pose.position = self.small_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
else:
cmd_pose.position = self.small_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
# print(self.position_cnt)
else :
cmd_pose = SetPosition()
self.position_cnt[id] -= 1
if self.position_cnt[id] < 0:
self.position_cnt[id] = 1
self.position_flag[id] = 1
cmd_pose.position = self.small_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
else:
cmd_pose.position = self.small_position_set[self.position_cnt[id]]
cmd_pose.id = id
self.pub.publish(cmd_pose)
# print(self.position_cnt)


time.sleep(1)

time.sleep(0.5)
print(id, "번 약 배출이 완료되었습니다.")


def reset_position(self, id):
cmd_pose = SetPosition()
cmd_pose.id = id
cmd_pose.position = self.cw_position_set[0]
if id < 2:
cmd_pose.position = self.big_position_set[0]
else:
cmd_pose.position = self.small_position_set[0]
self.pub.publish(cmd_pose)
time.sleep(0.1)




Expand All @@ -205,7 +272,6 @@ def main(args=None):
servo_node.destroy_node()
rp.shutdown()


if __name__ == "__main__":
print("start servo_node")
main()
Loading