search

KoshiroRobot / Ball-Balancing-Robot

Ball Balance Robot Parts File and Program
124 stars 29 forks source link

From 60 to 120 FPS is possible (Rpi 4 and PiCamera V3 wide) #3

Open AndreaFavero71 opened 5 months ago

AndreaFavero71 commented 5 months ago

According to @KoshiroRobot (YT video), the 60 fps of the camera seems to be the limiting factor for a ball bouncing controller.

I like this project and I'm thinking to make something similar, very likely with stepper motors and silent stepper drivers (to avoid getting kicked out of the family while working on this). Before buying the material I looked into the code, to answer the question "is it preferable a Raspberry Pi 5 + V1 camera or a Raspberry Pi 4b is enough ... perhaps in combination with a more recent camera version?" I made some tests with components borrowed from other projects: Raspberry Pi 4b 2Gb with different PiCamera versions, namely V3 wide, V2 and a V1 clone. While trying to increase the camera fps, I monitored the FOV (horizontal and especially vertical) to be as per this project setup.

Result with the original code: Rpi4 + V1 --> _imgfps 62 (camera fps as per original project)

With some little code's changes: Rpi4 + V1 --> _imgfps 62 (I could not speed up the camera) Rpi4 + V2 --> _imgfps 83 (30% faster camera) Rpi4 + V3 wide --> _imgfps 120 (90% faster camera)

Conclusion: For this project, I'm going to buy a Raspberry Pi 4b 2Gb and a V3 wide camera. The total cost will be rougly the same :-( . Below some notes: 1) V3 wide camera at 120fps has quite some crop, yet the reduced FOV overlaps the one of V1 camera at 62fps. 2) By increasing the camera fps the robot controller increases the load to the cpu, yet the Rpi 4b keeps up with the camera's fps. I believes it's beneficial a synchronization between the threads, but this is a fully different topic. 3) I have no clues whether the higher fps will be sufficient to be able to bounce the ball, perhpas I'll know it soon...

If anyone is interested in these changes to the code, please let me know by adding a comment to this post and I will summarize them here; Otherwise I will proceed with the project and see how far I get.



EDIT 23/06/2024: Changes at the camera_Class file (new, changed or integrated parts have '#AF#' comment)

from picamera2 import Picamera2
from libcamera import controls
import cv2
import numpy as np
import threading
lock = threading.Lock()

import os                                    #AF# os libray
os.environ["LIBCAMERA_LOG_LEVELS"] = "2"     #AF# with "2" libcamera prints WARN, ERROR, FATAL feedback ("1" adds INFO)
ret = os.system(f"v4l2-ctl --set-ctrl wide_dynamic_range=0 -d /dev/v4l-subdev0") #AF# sets the camera HDR to off

class Camera:
    def __init__(self):
        #カメラの初期化と設定とスタート

        self.picam2 = Picamera2()
        self.height = 480
        self.width = 480
        self.scale = 0.5                       #AF# scaling down factor
        self.interp_method = cv2.INTER_AREA    #AF# interpolation method resamples using pixel area relation
        focus_dist_m = 0.3                     #AF# focus distance in meters
        focus_dist = 1/focus_dist_m if focus_dist_m > 0 else 3  #AF preventing zero division

        #AF# settings for V3 wide camera @120fps
        self.main = {"format": 'RGB888', "size": (self.height, self.width)}  #AF# RGB instead of XRGB
        lores = {"size": (self.width, self.height), "format": "YUV420"}      #AF# low res (with Rpi5 it could be RGB instead!)
        self.config = self.picam2.create_video_configuration(
                main = self.main,
                lores = lores,                          #AF# added low resolution
                display = "lores",                      #AF# added display stream (low resolution)
                controls={
                "FrameDurationLimits": (8333,8333),
                "ExposureTime": 8000,
                "FrameRate": 120,                       #AF# added FrameRate control
                "AfMode": controls.AfModeEnum.Manual,   #AF# added Manual Focus control
                "LensPosition": focus_dist,             #AF# added Focus distance control
                "NoiseReductionMode": controls.draft.NoiseReductionModeEnum.Fast  #AF# added fast noise reduction
                }
        )

        self.picam2.align_configuration(self.config)    #AF# added align_configuration, ensuring higher performances
        self.picam2.configure(self.config)

        # 蛍光ピンクのHSV範囲を定義
        self.lower_pink = np.array([140, 150, 50])  # H: 約140度から
        self.upper_pink = np.array([180, 255, 255])  # H: 約170度まで
        #カメラをスタート
        self.picam2.start()

        image = self.take_pic(resize=False)        #AF# one image from the camera is taken     
        h, w = image.shape[:2]                     #AF# image height and width
        self.ww = int(w * self.scale)              #AF# scaled image width (reused at main.py for consistency)
        self.hh = int(h * self.scale)              #AF# scaled image height (reused at main.py for consistency)
        self.min_area = (self.ww * self.hh)//200   #AF# minimum ball area, based on image area
        self.max_area = (self.ww * self.hh)//6     #AF# maximum ball area, based on image area 

    def take_pic(self, resize = True):             #AF# added resize argument
        image = self.picam2.capture_array('main')  #AF# with a Raspberry Pi 5 this could use 'lores' instead
        if resize:                                 #AF# case resize is set True
            #AF# image is scaled to increase fps (lower resolution for ball position, i assume still enough)
            image = cv2.resize(image, (self.ww, self.hh), interpolation = self.interp_method)
        return image

    def show_video(self, image, wait=1):
        cv2.imshow("Live", image)
        cv2.waitKey(wait)

    def find_ball(self, image):
        ball_presence = False          #AF# added presumption of no ball is found
        # HSV色空間に変換
        image_hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        # 色範囲に基づくマスクを作成
        mask = cv2.inRange(image_hsv, self.lower_pink, self.upper_pink)
        # 輪郭を見つける
        contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

        if contours:
            # 最大の輪郭を見つける
            largest_contour = max(contours, key=cv2.contourArea)
            # 最小外接円を取得
            (x, y), radius = cv2.minEnclosingCircle(largest_contour)
            area = cv2.contourArea(largest_contour)  # 面積を計算
            if self.max_area > area > self.min_area:  # ノイズを無視するための閾値  #AF# ball area thresholds based on image area
                ball_presence = True                     #AF# ball is found
                # 円を画像に描画
                cv2.circle(image, (int(x), int(y)), int(radius), (0, 255, 0), 2)
                self.show_video(image)
                d = radius*2
                h = 10000/d
                # 中心を修正
#                 x -= self.height / 2                   #AF# commented out
#                 y -= self.width / 2                    #AF# commented out
                x -= self.hh / 2                         #AF# Ball CM coordinate x
                y -= self.ww / 2                         #AF# Ball CM coordinate y
                x, y = -y, x
                return int(x), int(y), int(area)  # 画像と座標、面積を返す

        if not ball_presence:                            #AF# added case no ball is detected
            self.show_video(image, wait=20)              #AF# image is shown, at 50Hz
            return -1, -1, 0  # ボールが検出されなかった場合  #AF return when no ball is detected

    def clean_up_cam(self):
        self.picam2.stop()
        self.picam2.close()
        cv2.destroyAllWindows()



And those at main.py (again, changes have the '#AF#' comments):

import class_BBRobot
import class_Camera
import class_PID
import time
import threading
import numpy as np

# 画像サイズ（高さ、幅）とチャンネル数（ここでは3チャンネル = RGB）
camera = class_Camera.Camera()
# height = 480      #AF# commented out (duplicate of data at camera Class)
# width = 480       #AF# commented out (duplicate of data at camera Class)
height = camera.hh  #AF# height is retrieved from the first image, at camera init
width = camera.ww   #AF# width is retrieved from the first image, at camera init
channels = 3

# 空の画像を作成（全てのピクセルが0）
image = np.zeros((height, width, channels), dtype=np.uint8)

#ロボットに使うサーボ番号
ids = [1, 2, 3]

#PIDの係数とphiの大きさを決める係数
K_PID = [0.015, 0.0001, 0.0051] #0.015, 0.0001, 0.0051
k = 1
a = 1
#ロボットとカメラとPID規則をインスタンス化
Robot = class_BBRobot.BBrobot(ids)
pid = class_PID.PID(K_PID, k, a)

#ロボットを準備して初期位置へ
Robot.set_up()
Robot.Initialize_posture()
pz_ini = Robot.ini_pos[2]

# frame_count = 0              #AF# moved into the functions, removing a global variable
# start_time = time.time()     #AF# moved into the functions, removing a global variable
fps = 0  # 初期値として0を設定
img_fps = 0
rob_fps = 0
fps_check = 120                #AF# variable for fpf check and printout

#ボールの座標
x = -1
y = -1
area = -1
goal = [100, 0]
xx = -1                        #AF# initial ball coordinate x, that will be corrected by image scaling
yy = -1                        #AF# initial ball coordinate y, that will be corrected by image scaling

def get_img():
    global image, img_fps #, img_start_time    #AF# removed one global
    img_start_time = time.time()               #AF# img_start_time from global to local variable
    img_frame_count = 0
    while(1):
        image = camera.take_pic()
        #img_fpsの計算
        img_frame_count += 1
        if img_frame_count >= fps_check:
            img_end_time = time.time()
            img_elapsed_time = img_end_time - img_start_time
            img_fps = int(round(fps_check / img_elapsed_time,0))
            img_start_time = img_end_time
            img_frame_count = 0

def cont_rob():
    global x, y, area, rob_fps #, rob_start_time    #AF# removed one global
    rob_start_time = time.time()                    #AF# rob_start_time from global to local variable
    rob_frame_count = 0
    while(1):
        x, y, area = camera.find_ball(image)
        #rob_fpsの計算
        rob_frame_count += 1
        if rob_frame_count >= fps_check:
            rob_end_time = time.time()
            rob_elapsed_time = rob_end_time - rob_start_time
            rob_fps = int(round(fps_check / rob_elapsed_time,0))
            rob_start_time = rob_end_time
            rob_frame_count = 0

try:
    camera_thread = threading.Thread(target=get_img)
    rob_thread = threading.Thread(target=cont_rob)
    camera_thread.start()
    rob_thread.start()
    i = 0                                         #AF# index to limit fps print-out
    while(1):
        xx = int(x / camera.scale)                #AF# coordinate x is corrected due to image scaling
        yy = int(y / camera.scale)                #AF# coordinate y is corrected due to image scaling
#         Current_value = [x, y, area]            #AF# commented out
        Current_value = [xx, yy, area]            #AF# corrected coordinates are used (for consistency with the PID constants)
        if x != -1:
            theta, phi = pid.compute(goal, Current_value)
            pos = [theta, phi, pz_ini]
            Robot.control_t_posture(pos, 0.01)
            i += 1                                #AF# index for fps printout is incremented
            if i >= fps_check:                    #AF# case the index equals the threshold for fps printout
                print(f"img_fps: {img_fps},  rob_fps: {rob_fps},  xx: {xx},  yy:{yy}")  #AF# fps printout (only when ball detection)
                i = 0                             #AF# index to limit fps printout is set back to zero
        else:                                     #AF# added else case (no ball detection)
            time.sleep(0.01)                      #AF# little sleep gives time to update image on screen

finally:
    Robot.clean_up()
    camera.clean_up_cam()
DaHouzKat commented 5 months ago

Silent stepper motors and drivers, i really like the "sound" of that ! I hope you'll succeed make it bounce a ball !! I will definitely follow up on your progress !!!

Neon22 commented 5 months ago

Very interested to follow. You might also consider the openMV module. Runs python too. has the high refresh rate circle transform built in. very impressive frame rates. (Uses Hough transform)

Adding the docs for find_circle (and everything else) here:

Neon22 commented 5 months ago

You might need the global shutter camera option to get frame rates up to 400fps.