カメラやりだした。

日付11/28

カメラやりだした。

ざるそば君が消えてから僕はカメラを触り始めました。

今日やったことを大雑把に説明するとカメラでゴールを捉えてその幅を利用したキーパープログラムです。

ゴールの幅？と思いますよね。

この全方位カメラ、前やっていたキーパープログラムのように動かそうとするならどうしてもゴールの幅が必要です。図を入れました。

一応これがゴールの端です。これを見ればわかると思いますが自ゴールのセンターだけを使ってゴールの端を求めることができませんね。それでゴールの幅を使うのです。ゴールのセンター±(ゴールの幅 / 2をすれば)ゴールの両端の座標を求めることができます。これでゴールからはみ出すことなくできますね。で、これを利用してゴール上のどこにいるのかを数直線で表せないかというのを考えてみることにしました。これ考えるのに1時間かかりました。で、やってみたんですけれど全く成功しませんでした。数値は出ます。しかしゴールの中心がどうしても±20くらいでずれてしまいました。ずれの原因はロボットのゴールに対する角度です。ですから角度補正1000%でやれば使えますね(信頼性皆無)とりあえずプログラムを後ろに乗せておきます！

あと今回映像補正をしてみました。先ほどの画像を見て気付きませんでしたか？

そうです。カメラで見ている映像が反転しています。


img = sensor.snapshot().rotation_corr(y_rotation = 180)

これでできました。Y軸で180度回転させています。ちなみに左の写真が反転させる前の画像です。右の写真はZ軸を45度傾けた画像になります。今日調べてて色々わかったので良かったです。プログラむをcolor_tracking_v3という名前であげておこうと思います。


import sensor, image, time , math , pyb, micropython
from pyb import ExtInt
from pyb import UART
from pyb import LED

#床からopenMVの基盤まで11cm

#閾値 Thresholds　= [(L Min, L Max, A Min, A Max, B Min, B Max)]



Ops_threshold  = [(11, 42, -13, 8, -33, -9)]    #blue
Own_threshold= [(0, 100, -27, 29, 22, 59)]      #yellow


cen_x = 166 #カメラの中心ｘ座標
cen_y = 116 #カメラの中心ｙ座標
OwnX = 0
OwnY = 0
OpsX = 0
OpsY = 0

OwnW = 0    #自ゴールの幅
OpsW = 0    #相手ゴールの幅
OwnGoalRightEnd = 0 #自ゴールの右端
OwnGoalLeftEnd = 0  #自ゴールの左端
CenterPerOwnGoal = 0 #自ゴールに置ける自分の位置


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 1000)
sensor.set_contrast(0)
sensor.set_brightness(0)
sensor.set_saturation(0)#-3~3
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
micropython.alloc_emergency_exception_buf(100)
clock = time.clock()
usb = pyb.USB_VCP()
led = pyb.LED(1)
uart = UART(1, 230400)
uart.init(230400, bits = 8, parity = None, stop = 1)

#関数定義

def sear (thr):
    big_Area = 0
    big_x = 0
    big_y = 0
    big_w = 0
    for goal in img.find_blobs(thr, pixels_threshold = 100, area_threshold = 100,marge = True):
        img.draw_rectangle(goal.rect())
        img.draw_cross(goal.cx(), goal.cy())
        Area = goal.h() * goal.w()
        if(Area > big_Area):
            big_Area = Area
            big_x = -(goal.cx() - cen_x)
            big_y = -(cen_y - goal.cy())
            big_w = goal.w()
    if(big_x == 0 and big_y == 0):
        big_x = -1
        big_y = -1
    return big_x, big_y ,big_w

def callback(line):
    print(OwnX, OwnY ,OpsX, OpsY)

    uart.write('A')
    uart.writechar(OwnX)
    uart.writechar(OwnY)
    uart.writechar(OpsX)
    uart.writechar(OpsY)

#uart.writechar( n & 0b0000000011111111)
#uart.writechar((n & 0b1111111100000000)>>8)

#割り込み
exint = pyb.ExtInt("P4", ExtInt.IRQ_RISING, pyb.Pin.PULL_DOWN, callback)
pyb.enable_irq(True)


while(True):
    clock.tick()
    img = sensor.snapshot().rotation_corr(y_rotation = 180)
    OwnX, OwnY ,OwnW = sear(Own_threshold)
    OpsX, OpsY ,OpsW = sear(Ops_threshold)


    OwnGoalRightEnd = OwnX + (OwnW/2)
    OwnGoalLeftEnd  = OwnX - (OwnW/2)
    if(int(OwnX) != 0):
        CenterPerOwnGoal = (OwnGoalRightEnd + OwnGoalLeftEnd)/2
    print(OwnX, OwnY,OwnW,int(OwnGoalRightEnd),int(OwnGoalLeftEnd),int(CenterPerOwnGoal))