Lead the snake out of the hole

1. Instructions for leading the snake out of the hole

The function of leading the snake out of the hole uses the HSV color recognition function. The path where the HSV color calibration file is saved is ~/jetcobot_ws/src/jetcobot_snake_follow/scripts/HSV_config.txt. If the color recognition is not accurate enough, please open the ~/jetcobot_ws/src/jetcobot_snake_follow/scripts/HSV_calibration.ipynb file to calibrate the HSV value of the building block color. After the calibration operation is completed, it will be automatically saved to the HSV_config file. Rerun the program without additional code modification.

The main function of leading the snake out of the hole is to identify the selected color block and move forward and backward with the building block. When it reaches the maximum distance range of the arm length, it will make a clamping action to grab the building block.

2. Important code explanation

Code path: ~/jetcobot_ws/src/jetcobot_snake_follow/scripts/snake_follow.ipynb

Control the forward and backward movement of the robot arm.

xxxxxxxxxx
    def snake_control(self, name, msg):
        for key, area in msg.items():
            if key == name:
                # x = round(self.pid.calculate(math.sqrt(area)), 2)
                x = round(self.pid.calculate(area), 2)
                coords = [230-x+80, -60, 300, -95, -44, -85]
                # coords = [230-x+80, -60, 300, -90, 50, -90]
                print("x=", 230-x+80)
                if not self.snake_clamp:
                    self.snake_check = self.snake_check + 1
                    if 230-x+80 >= 220:
                        if self.snake_check > 5:
                            self.snake_grip_block(name)
                            self.snake_check = 0
                    else:
                        self.mc.send_coords(coords, 100, 1)
                        self.snake_check = 0

The camera processes the image function, passes the image captured by the camera to the function of luring the snake out of the hole for calculation, and drives the robot arm to move.

xxxxxxxxxx
def camera():
    # 打开摄像头 Open camera
    capture = cv.VideoCapture(0)
    capture.set(cv.CAP_PROP_FRAME_WIDTH, 640)
    capture.set(cv.CAP_PROP_FRAME_HEIGHT, 480)
    last_time = 0
    # Be executed in loop when the camera is opened normally 
    # 当摄像头正常打开的情况下循环执行
    while capture.isOpened():
        try:
            if model == 'Exit':
                break
            _, img = capture.read()
            # img = cv.resize(img, (640, 480))
            # # 获得运动信息 Get motion information
            img, snake_msg = snake_target.target_run(img, color_hsv, choose_color.value)
            if len(snake_msg) == 1 and time.time() - last_time > 0.7:
                snake_target.snake_control(choose_color.value, snake_msg)
                print("area:", snake_msg.get(choose_color.value))
                last_time = time.time()
            cv.putText(img, choose_color.value, (int(img.shape[0] / 2), 50), cv.FONT_HERSHEY_SIMPLEX, 2, color[random.randint(0, 254)], 2)
            imgbox.value = cv.imencode('.jpg', img)[1].tobytes()
            
        except KeyboardInterrupt:
            pass
        except Exception as e:
            print("except program:", e)
    capture.release()
    print("stop program")

3. Start the program

Start the program

Open the jupyterlab webpage and find the corresponding .ipynb program file.

Then click Run All Commands.

4. Experimental operation and effect

After the program is running, the jupyterlab webpage will display the control, with the camera screen on the top and the functions of the related buttons on the bottom. The default is to follow the blue object. If you need to follow other color objects, please click the button below to switch.

The robotic arm will control the gripper to release the clamp at intervals. At this time, put the corresponding color building block into the camera screen, the robotic arm will recognize the corresponding color, and move the building block back and forth, which can control the robotic arm to move forward and backward.

If the robot arm is led to the farthest distance, the robot arm will first rotate the gripper to trigger the function of gripping the building block, then open the gripper, put the building block into the position where the gripper can grip, and the robot arm will grip the building block to the selected color position.

If you need to exit the program, please click the [Exit] button.