everything update

README.md

@@ -97,7 +97,7 @@ alias eb='nano ~/.bashrc'
 alias sb='source ~/.bashrc'
 alias gs='git status'
 alias gp='git pull'
-
+alias cool="sudo sh -c 'echo 100 > /sys/devices/pwm-fan/target_pwm'"
 alias gala="source /opt/ros/galactic/setup.bash; echo \"ROS2 galactic\""
 alias killgazebo="killall gzserver gzclient"
 alias ros_domain="export ROS_DOMAIN_ID=13"

initenv.sh

@@ -1,9 +1,9 @@
 echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout",  SYMLINK+="yacyac/lidar"' >/etc/udev/rules.d/yacyac_lidar.rules
 
 
-echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", MODE:="0666", GROUP:="dialout",  SYMLINK+="yacyac/motor"' >/etc/udev/rules.d/yacyac_motor.rules
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", MODE:="0666", ATTRS{serial}=="FT7W96TF", GROUP:="dialout",  SYMLINK+="yacyac/motor"' >/etc/udev/rules.d/yacyac_motor.rules
 
-echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", ATTRS{serial}=="FT4TCVL4", GROUP:="dialout",  SYMLINK+="yacyac/servo"' >/etc/udev/rules.d/yacyac_servo.rules
+echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014", MODE:="0666", ATTRS{serial}=="FT4TCVL4", GROUP:="dialout",  SYMLINK+="yacyac/servo"' >/etc/udev/rules.d/yacyac_servo.rules
 
 service udev reload
 sleep 2

md/src/com.cpp

@@ -26,7 +26,7 @@ extern void PubRobotPose(void);
 int InitSerialComm(void)
 {
     try {
-        ser.setPort("/dev/ttyUSB0");
+        ser.setPort("/dev/yacyac/motor");
         ser.setBaudrate(robotParamData.nBaudrate);
         serial::Timeout to = serial::Timeout::simpleTimeout(1667); // 1667 when baud is 57600, 0.6ms
         ser.setTimeout(to);                                        // 2857 when baud is 115200, 0.35ms

yacyac_core/src/bt_ros2.cpp

@@ -20,8 +20,8 @@ int main(int argc, char** argv)
 
     auto nh = rclcpp::Node::make_shared("yacyac_core");
     // nh->declare_parameter("bt_xml", "bt_nav_mememan.xml");
-    // nh->declare_parameter("bt_xml", "bt_yac_supply.xml");
-    nh->declare_parameter("bt_xml", "bt_nav_yac_supply.xml");
+    nh->declare_parameter("bt_xml", "bt_yac_supply.xml");
+    // nh->declare_parameter("bt_xml", "bt_nav_yac_supply.xml");
     std::string bt_xml;
     nh->get_parameter("bt_xml", bt_xml);
     bt_xml = packagePath + "/bt_xml/" + bt_xml;
@@ -33,11 +33,12 @@ int main(int argc, char** argv)
 
     BT::BehaviorTreeFactory factory;
 
-    // RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str());
-    factory.registerNodeType<Nav2Client>("Nav2Client");
+    RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str());
+    // factory.registerNodeType<Nav2Client>("Nav2Client");
     factory.registerNodeType<YacSupplyCilent>("YacSupplyCilent");
-    factory.registerNodeType<QRClient>("QRClient");
-    factory.registerNodeType<Message>("Message");
+    // std::cout << "둥" << std::endl;
+    // factory.registerNodeType<QRClient>("QRClient");
+    // factory.registerNodeType<Message>("Message");
 
     // Trees are created at deployment-time (i.e. at run-time, but only once at
     // the beginning). The currently supported format is XML. IMPORTANT: when the

yacyac_servo/yacyac_servo/servo_ctrl.py

@@ -24,22 +24,32 @@ def __init__(self):
 
         self.cmd_pose_id = Servo()
         self.cnt = 0
+        # 큰약
         # 목적 포지션 list
-        # 1023 to 360 degree
+        # 1023 to 300 degree
         # 시계방향으로 돌아가는 포지션 리스트
-        self.cw_position_set =  [0, 164, 328, 492, 656, 820] 
+        self.big_cw_position_set =  [0, 164, 328, 492, 656, 820] 
         # 반 시계방향으로 돌아가는 포지션 리스트 
-        self.ccw_position_set = [915,751, 587,423, 259]
+        self.big_ccw_position_set = [915,751, 587,423, 259]
+
+        # 작은약
+        # 목적 포지션 list
+        # 1023 to 300 degree
+        # 시계방향으로 돌아가는 포지션 리스트
+        self.small_cw_position_set =  [0, 164, 328, 492, 656, 820] 
+        # 반 시계방향으로 돌아가는 포지션 리스트 
+        self.small_ccw_position_set = [915,751, 587,423, 259]
+
         # 방향 flag
         self.position_flag = [0, 0, 0, 0, 0, 0, 0, 0, 0]
         # 현재 포지션 index
         self.position_cnt =  [0, 0, 0, 0, 0, 0, 0, 0, 0]
         print ("init positioning...")
         for i in range(8):
             # 근처 포지션으로 이동합니다.
-            self.init_position(i)
+            # self.init_position(i)
             # 원점 포지션으로 이동합니다. 
-            # self.reset_position(i)
+            self.reset_position(i)
         print ("init positioning done!!!")
 
 
@@ -63,39 +73,73 @@ 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
-                # print("ccw")
-            # 리스트의 요소값과 같은 값이 있으면 그 인덱스를 반환
-            if flag: 
-                for i, val in enumerate(self.cw_position_set):
-                    if val == cmd_pose.position:
-                        self.position_cnt[id] = i
-                        break
-            else:
-                for i, val in enumerate(self.ccw_position_set):
-                    if val == cmd_pose.position:
-                        self.position_cnt[id] = i
-                        break
+            if id < 2 :
+                for i, val in enumerate(self.big_cw_position_set):
+                    diff = abs(response.position - val)
+                    if diff < min_diff:
+                        min_diff = diff
+                        closest_idx = i
+                        cmd_pose.position = self.big_cw_position_set[closest_idx]
+                        flag = 1
+                for i, val in enumerate(self.big_ccw_position_set):
+                    diff = abs(response.position - val)
+                    if diff < min_diff:
+                        min_diff = diff
+                        closest_idx = i
+                        cmd_pose.position = self.big_ccw_position_set[closest_idx]
+                        flag = 0
+                if flag == 0:
+                    self.position_flag[id] = 1
+                    # print("cw")
+                else:
+                    self.position_flag[id] = 0
+                    # print("ccw")
+                # 리스트의 요소값과 같은 값이 있으면 그 인덱스를 반환
+                if flag: 
+                    for i, val in enumerate(self.big_cw_position_set):
+                        if val == cmd_pose.position:
+                            self.position_cnt[id] = i
+                            break
+                else:
+                    for i, val in enumerate(self.big_ccw_position_set):
+                        if val == cmd_pose.position:
+                            self.position_cnt[id] = i
+                            break
+            else :
+                for i, val in enumerate(self.small_cw_position_set):
+                    diff = abs(response.position - val)
+                    if diff < min_diff:
+                        min_diff = diff
+                        closest_idx = i
+                        cmd_pose.position = self.small_cw_position_set[closest_idx]
+                        flag = 1
+                for i, val in enumerate(self.small_ccw_position_set):
+                    diff = abs(response.position - val)
+                    if diff < min_diff:
+                        min_diff = diff
+                        closest_idx = i
+                        cmd_pose.position = self.small_ccw_position_set[closest_idx]
+                        flag = 0
+                if flag == 0:
+                    self.position_flag[id] = 1
+                    # print("cw")
+                else:
+                    self.position_flag[id] = 0
+                    # print("ccw")
+                # 리스트의 요소값과 같은 값이 있으면 그 인덱스를 반환
+                if flag: 
+                    for i, val in enumerate(self.small_cw_position_set):
+                        if val == cmd_pose.position:
+                            self.position_cnt[id] = i
+                            break
+                else:
+                    for i, val in enumerate(self.small_ccw_position_set):
+                        if val == cmd_pose.position:
+                            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")
 
@@ -114,43 +158,71 @@ def control_position(self, id, servo):
             # yac_cnt 
 
             print(id, "번 약 ", servo, "개중 ", yac_cnt, "개 배출중...")
-            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)
+            if id < 2:
+                if self.position_flag[id] == 1:
+                    cmd_pose = SetPosition()
+                    self.position_cnt[id] += 1
+                    if self.position_cnt[id] >= len(self.big_ccw_position_set):
+                        self.position_cnt[id] = 0
+                        self.position_flag[id] = 0
+                        cmd_pose.position = self.big_cw_position_set[self.position_cnt[id]]
+                        cmd_pose.id = id
+                        self.pub.publish(cmd_pose)
+                    else:    
+                        cmd_pose.position = self.big_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.big_cw_position_set):
+                        self.position_cnt[id] = 0
+                        self.position_flag[id] = 1
+                        cmd_pose.position = self.big_ccw_position_set[self.position_cnt[id]]
+                        cmd_pose.id = id
+                        self.pub.publish(cmd_pose)
+                    else:    
+                        cmd_pose.position = self.big_cw_position_set[self.position_cnt[id]]
+                        cmd_pose.id = id 
+                        self.pub.publish(cmd_pose)
+            else:
+                if self.position_flag[id] == 1:
+                    cmd_pose = SetPosition()
+                    self.position_cnt[id] += 1
+                    if self.position_cnt[id] >= len(self.small_ccw_position_set):
+                        self.position_cnt[id] = 0
+                        self.position_flag[id] = 0
+                        cmd_pose.position = self.small_cw_position_set[self.position_cnt[id]]
+                        cmd_pose.id = id
+                        self.pub.publish(cmd_pose)
+                    else:    
+                        cmd_pose.position = self.small_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.small_cw_position_set):
+                        self.position_cnt[id] = 0
+                        self.position_flag[id] = 1
+                        cmd_pose.position = self.small_ccw_position_set[self.position_cnt[id]]
+                        cmd_pose.id = id
+                        self.pub.publish(cmd_pose)
+                    else:    
+                        cmd_pose.position = self.small_cw_position_set[self.position_cnt[id]]
+                        cmd_pose.id = id 
+                        self.pub.publish(cmd_pose)
 
 
-            time.sleep(1)
+            # time.sleep(1)
         print(id, "번 약 배출이 완료되었습니다.")
 
     def reset_position(self, id):
         cmd_pose = SetPosition()
         cmd_pose.id = id
-        cmd_pose.position = self.cw_position_set[0]
+        cmd_pose.position = self.big_cw_position_set[0]
         self.pub.publish(cmd_pose)
-        time.sleep(0.1)
+        # time.sleep(0.1)
 
     def callback_yac(self, msg):
         servo_list = list(msg.servo_list)