ラズパイ4 秒針解析・モーター位置決め並行処理

概要

 先々回の秒針解析処理と 先回のL6470利用 ステップモーター駆動の内容をラズパイ4で並行処理する検討をしました。次の動画(8倍速再生)の通り、左側時計秒針とステップモーターの矢印は、同じ速さで動いています。
 実際にはステップモーター移動は、秒針解析結果を反映しているのではなく、ラズパイ側時計の経過時間を絶対位置に変換して、位置決めを行っています。
 位置決めは、秒針解析の変化(例:11 →12秒)をトリガーにしています。つまり、何らかの理由でステップモーターが “BUSY” 状態にある場合を除き、1秒に1回の頻度で経過時間に相当する絶対位置の計算と位置決め動作を繰り返しています。

ラズパイ4 秒針解析とステップモーター位置決めの並行処理(8倍速再生)


 接続は先回のL6470利用 ステップモーター駆動の接続にWEBカメラ(USB接続)を追加した状態です。

プログラム

  先々回の秒針解析処理のプログラムをベースに、先回のL6470利用 ステップモーター駆動処理の内容を部分的に追加しています。

import cv2
import math
import numpy as np
import time

# L6470 利用対応
import wiringpi as wp                   
import RPi.GPIO as GPIO
import struct
import sys

# 秒針検出変数初期化 
x3 = 0              			# 秒針中心座標(x)
y3 = 0              			# 秒針中心座標(y)
sec_trs=0           			# 経過時間
ang_pin=0           			# 秒針角度
ang_hsi=0           			# 補正角度
pic_cnt=0           			# 画像保存枚数

# L6470 SPIチャンネル設定・GPIO設定
SPI_CH = 0                              # SPI チャンネル
SPI_HZ = 1000000                        # SPI 通信速度
GPIO_Nbr = 25                           # GPIO-NO
GPIO.setmode(GPIO.BCM)                  # GPIO-NO 指定
GPIO.setup(GPIO_Nbr,GPIO.IN)            # GPIO INPUT 指定

# L6470初期設定    
def INIT_L6470():
    spi_send([0x00,0x00,0x00,0xc0])     # Reset Device
    spi_send([0x05,0x00,0x0e])          # Acceleration (12)
    spi_send([0x06,0x00,0x0e])          # Deceleration (12)
    spi_send([0x07,0x00,0x0e])          # Maximum speed (10)
    spi_send([0x08,0x00,0x01])          # Minimum speed (13)
    spi_send([0x15,0x03,0xFF])          # Full-step speed (10)
    spi_send([0x16,0x03])               # Micro Step (8)
    spi_send([0x09,0x50])               # Holding Kval (8)
    spi_send([0x0A,0x50])               # Constant Speed Kval (8)
    spi_send([0x0B,0x50])               # Acceleration starting Kval (8)
    spi_send([0x0C,0x50])               # Deceleration starting Kbal (8)

# SPI データ送信
def spi_send(spi_dat_ary):
    for itm in spi_dat_ary:
        tmp=struct.pack("B",itm)
        wp.wiringPiSPIDataRW(SPI_CH, tmp)
        
# JOG (SPEED指定 : 0---30000)
def L6470_run(run_spd):
    # 方向検出
    if (run_spd > 0):
        dir = 0x50
        spd = run_spd
    else:
        dir = 0x51
        spd = -1 * run_spd
        
    L6470_SEND_MOVE_CMD( dir , spd )

# 移動量指定移動
def L6470_POSITIONING(MV_DIST):
    # 方向検出
    if (MV_DIST > 0):
        dir = 0x40
    else:
        dir = 0x41
        MV_DIST = -1 * MV_DIST
        
    L6470_SEND_MOVE_CMD( dir , MV_DIST )

# 絶対位置指定移動
def L6470_MOVE_ABS(MV_DIST):
    dir = 0x60
    #dir = 0x69
    if (MV_DIST < 0):
        MV_DIST = -1 * MV_DIST
        
    L6470_SEND_MOVE_CMD( dir , MV_DIST )
        
# データ加工・送信(共通)
def L6470_SEND_MOVE_CMD( cmd , DAT ):
    tmp=[]
    tmp.append(cmd)
    tmp.append((0x0F0000 & DAT) >> 16)
    tmp.append((0x00FF00 & DAT) >> 8)
    tmp.append((0x00FF & DAT))
    spi_send(tmp)
        

# 原点設定
def L6470_SET_ORIGIN():
    time.sleep(0.1)
    spi_send([0xD8])                    # Reset Position
    time.sleep(0.3)

# 原点移動
def L6470_MOVE_ORIGIN():
    spi_send([0x70])                    # Go Home
    time.sleep(0.5)

# ドライバーBUSY解除待ち
def wait_until_not_busy():
    while True:
        time.sleep(0.2)
        mtr_sts = GPIO.input(GPIO_Nbr)
        #print(mtr_sts)
        
        if GPIO.input(GPIO_Nbr) == GPIO.HIGH :
            print("L6470 NOT BUSY")
            break    
    time.sleep(0.2)



##############################
capture = cv2.VideoCapture(0)
if capture.isOpened() is False:
  raise IOError

wp.wiringPiSPISetup(SPI_CH,SPI_HZ)  	# SPI 接続
INIT_L6470()                        	# L6470初期設定
L6470_SET_ORIGIN()                  	# 原点位置設定(0)
wait_until_not_busy()               	# ドライバーBUSY解除待ち

start_time = time.time()
passed_time_bef = 0
loop_cn = 0

# ループ処理
while(True):
  try:
    ret, frame = capture.read()
    if ret is False:
      raise IOError

    frame = cv2.rotate(frame,cv2.ROTATE_180)
    frm_h , frm_w , frm_ch = frame.shape[:3]
    frm_tgt = frame[ 0:frm_h , 0:math.floor(frm_w/2) ]
    
    img_gray = cv2.cvtColor(frm_tgt,cv2.COLOR_BGR2GRAY) 	# グレイ変換
    edges = cv2.Canny(img_gray,10,220,apertureSize=3)       	# エッジ検出

    # ハフ変換で時計部円検出 (半径等条件設定により、時計部のみ検出する様にしている)
    circles=cv2.HoughCircles(img_gray,cv2.HOUGH_GRADIENT,1,30,param1=70,param2=30,minRadius=80,maxRadius=110)
    cnt_cntr=0
    if circles is not None:
      for cx,cy,r in circles.squeeze(axis=0).astype(int):
        cv2.circle(frame,(cx,cy),r,(0,255,0),2)
        cv2.circle(frame,(cx,cy),1,(0,0,255),3)
        x3_tmp = cx
        y3_tmp = cy
        cnt_cntr = cnt_cntr + 1

    if cnt_cntr==1:
      x3 = x3_tmp
      y3 = y3_tmp

    if x3 > 0 and y3 > 0 :
      # ハフ変換で直線抽出   
      lines = cv2.HoughLinesP(edges, rho=1, theta=np.pi/180, threshold=90,minLineLength=20,maxLineGap=5)
      ang_hsi =0
      
      if lines is not None:  
        for line in lines:
          x1 , y1 , x2 , y2 = line[0]                       	# 検出直線始点・終点座標取得

          # 円中心と検出した直線の距離計算
          u = np.array([ x2-x1 , y2-y1 ])
          v = np.array([ x3-x1 , y3-y1 ])
          D_CTR = math.floor(abs(np.cross(u,v) / np.linalg.norm(u))) 
          
          D_P1 = math.sqrt((x1-x3)**2+(y1-y3)**2)           	# 円中心と直線始点の距離計算
          D_P2 = math.sqrt((x2-x3)**2+(y2-y3)**2)           	# 円中心と直線終点の距離計算

          # 距離が短い点を始点として角度計算(ラジアン)
          if D_P1 > D_P2 and D_CTR < 10:
            rslt_rad = math.atan2( y1-y2 , x1-x2 )
          else :
            rslt_rad = math.atan2( y2-y1 , x2-x1 ) 
            
          rslt_ang = math.degrees(rslt_rad)                 	# ラジアンから角度変換

          # 0秒=0度として、角度変換
          if rslt_ang > -90 :
            rslt_ang_360 = rslt_ang + 90
          else :
            rslt_ang_360 = rslt_ang + 450

          dsp_ang = str(math.floor(rslt_ang_360))
  
          if D_CTR < 10:
            # 円中心からの距離が10以下 → 秒針と判定
            cv2.putText(frame,dsp_ang,(x2,y2),cv2.FONT_HERSHEY_PLAIN,2,(255,0,255),1,cv2.LINE_AA)
            cv2.line(frame,(x1,y1),(x2,y2),(255,0,255),3)   	# 秒針描画(ピンク)
            ang_pin = rslt_ang_360
      
          else:
            # 秒針以外に検出した直線
            if rslt_ang < 15 and rslt_ang > -15 and D_CTR > 150 and D_CTR < 220 and y1 < y3 and y2 < y3 :
              # スクリーン上側の水平方向直線(傾き10度以内 + 他条件)
              cv2.putText(frame,str(math.floor(rslt_ang))+",D" + str(D_CTR),(x1,y1+30),cv2.FONT_HERSHEY_PLAIN,1,(255,125,0),1,cv2.LINE_AA)
              cv2.line(frame,(x1,y1),(x2,y2),(255,255,0),3) 	# スクリーン上側水平方向直線(水色)
              ang_hsi = rslt_ang                            	# 補正角度記憶
              
            else:
              # その他の検出した直線
              #cv2.putText(frame,str(math.floor(rslt_ang))+",D" + str(D_CTR),(x1,y1-10),cv2.FONT_HERSHEY_PLAIN,1,(0,0,255),1,cv2.LINE_AA)
              cv2.line(frame,(x1,y1),(x2,y2),(0,0,255),2)  	# 赤色直線
           
        
  except KeyboardInterrupt:
    cv2.putText(frame,"Error",(25,25),cv2.FONT_HERSHEY_PLAIN,2,(0,0,255),1,cv2.LINE_AA)

  # 経過時間から移動量計算
  passed_time = math.floor(time.time() - start_time)
  if loop_cn == 0 :
    passed_time_bef = passed_time
    loop_cn = loop_cn + 1

  if passed_time_bef != passed_time :
    if GPIO.input(GPIO_Nbr) == GPIO.HIGH :
      PASSED_STEP = math.floor(passed_time * 1600 / 60)		# 移動座標計算
      L6470_MOVE_ABS(PASSED_STEP)               		# 移動量指定移動
      
      #cv2.putText(frame,"TIME : "+str(passed_time-passed_time_bef)+"sec",(15,90),cv2.FONT_HERSHEY_PLAIN,3,(255,255,0),1,cv2.LINE_AA)
      passed_time_bef = passed_time
  

  # 角度から秒に変換(秒針傾きと補正角度考慮)
  sec_trs = math.floor( 60 * (ang_pin - ang_hsi) / 360 )
  if sec_trs<=0:
    sec_trs = 60+sec_trs
  if sec_trs>=60:
    sec_trs = sec_trs-60  

  # 経過時間・時計部中心座標等、表示追加
  cv2.putText(frame,"TIME : "+str(sec_trs)+"sec",(15,45),cv2.FONT_HERSHEY_PLAIN,3,(255,255,0),1,cv2.LINE_AA)
  cv2.putText(frame,"( Center Of Circle : "+str(x3)+" , "+str(y3) + " )",(350,25),cv2.FONT_HERSHEY_PLAIN,1,(0,255,255),1,cv2.LINE_AA)


  edges_rgb=cv2.cvtColor(edges,cv2.COLOR_GRAY2RGB)        	# 画像合成用に変換
  cv2.putText(edges_rgb,"Canny Edge Detection",(15,45),cv2.FONT_HERSHEY_PLAIN,3,(0,0,255),1,cv2.LINE_AA)

  cv2.imshow('frame',frame)

  time.sleep(0.09)
  
  # ESCで処理中断
  k = cv2.waitKey(1)
  if k==27:
    break

capture.release()
cv2.destroyAllWindows()

まとめ

 画像処理しながらステップモーター制御できるか、少し疑問に感じたので、過去2回の内容を利用し試してみました。処理の遅延等なく、ラズパイでもこの様なことが出来ることを確認しました。

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です