Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@etoolsLab369

PIC32MX＋OV7670　カラートラッキング with ステッピングモータ

Last updated at Posted at 2024-11-11

OV7670　カラートラッキング

随時更新予定。
24.11.14 DMAモジュールの停止と復帰を改変。DCH1CONbits.CHENビットの使用に変更。

ステッピングモータ２個で、水平、垂直方向にカメラを移動させ、対象物を追尾します。

IMG_5098.JPG

カラートラッキング概略

トラッキング概略図.jpg

画面を９分割し、エリア名を１から９にします。５が画面の中心になります。
対象物(赤い紙)の左端と右端の座標を、逐次検索します。その座標をもとに、対象物がいるエリアを特定します。
1,2,3にあるならカメラを上昇、7,8,9にあるならカメラを下降させます。同様に、
1,4,7にある場合は、カメラを左に、3,6,9にある場合は、カメラを右に振って、中心のエリア５に移動させます。
移動は、ステッピングモータ28BYJ-48を2個、使用しました。
ファイル名 ファイル名

PIC32ピン割り振り表

ピン配カラートラッキング.png

トラッキングの方法

モータ駆動方向.jpg
対象物の左端と右端の座標から、対象エリアを特定し、あらかじめ決めておいた方向に、ステッピングモータを駆動します。左端と、右端の両方が中心のエリア５に入ったら、ステッピングモータを停止します。
青い矢印は、画面上の移動方向で、ステッピングモータの駆動方向は１８０度逆になります。

ダミー行の追加で、トラッキング制御の時間を作る。

ダミーライン.jpg

カメラのダミー行設定レジスタ
DM_LNL(0x92):Dummy Row low 8bits
DM_LNH(0x93):Dummy Row high 8bits
DM＿LNLに0x30、DM_LNHに0x01を設定し、３０４行分のダミー行を設定しました。

ブランクエリア設定　SCCBで送信。
//blunk area
     I2C2_b2Write(OV7670deviceAdd,0x92, 0x30, true);
     I2C2_b2Write(OV7670deviceAdd,0x93, 0x01, true);

※ダミーが少ないと、新しい画面の先頭データ割り込み(VSYNC割込み）が、表示タイミングとかさなり、マイコンにリセットが掛かります。

トラッキングのコード（抜粋）

DMA2とSPIをつかって、QQVGA(160×120)の画面データをRAMから液晶に送信させています。このとき、DMAモジュールを一旦OFFにしています。
そして、VSYNC割込みで、DMAモジュールを再度、ONにしています。この操作で、１画面を転送することができるようになりました。

24.11.14訂正：DCH1CONbits.CHENでON,OFFさせる方法に変えました。

interrupt.c

#include <proc/p32mx370f512h.h>

#include "interrupt.h"
#include "PIC32_Graphics.h"
#include "OV7670Camera.h"
#include "ds_ST7735S.h"
#include "stepping.h"

//Timer1 interrupt
void __ISR(_TIMER_1_VECTOR,IPL1AUTO) ISR_Timer1(void)
{
    IFS0bits.T1IF=0;
    tm1.cnt++;
    if(tm1.cnt%5==0)
    {
        tm1.fg1=true;
    }
    if(tm1.cnt==500)
    {
        tm1.cnt=0;
        tm1.fg=true;
        IEC0bits.T1IE=0;
        //LATBbits.LATB0=~LATBbits.LATB0;
    } 
}

//USART1 RX interrupt
void __ISR(_UART_1_VECTOR,IPL1AUTO) ISR_UART1(void)
{
    if( IFS1bits.U1RXIF==1)
    {
        IFS1bits.U1RXIF=0;
        if(U1STAbits.URXDA)
        {
            do{
                rxU1.ch=U1RXREG;
                rxU1.buf[rxU1.length]=rxU1.ch;
                rxU1.length++;
                if(rxU1.length>=RXUSART_BUFFER_SIZE)
                {
                    rxU1.length=0;
                }
            }while(U1STAbits.URXDA);  
            if(rxU1.ch==0x0a && rxU1.length>=3)//0x0a:'\n'
            {
                //rxUsart.buf[rxUsart.length-2]='\0';//\a\n終端文字FromPC
                rxU1.buf[rxU1.length-2]=0x00;//\a\n終端文字FromPC
                rxU1.completed=true;  
                IEC1bits.U1RXIE=0;
            } 
        }
    }
}


//VSYNC割込み INT2:RD4
void __ISR(_EXTERNAL_2_VECTOR, IPL7AUTO) ISR_INT2(void)
{
    LATBbits.LATB10 = ~LATBbits.LATB10;
    IFS0bits.INT2IF=0;
    OV7670.LineCounter=0;
    OV7670.HpixCounter=0;
    
    //DMACONbits.ON = 1;     // DMAモジュールを有効にする
    DCH1CONbits.CHEN=1;//DMA1を有効にする。
    //IEC2bits.DMA1IE = 1;
    //IEC2bits.DMA2IE = 1;
}

//HREF割込み　INT0:RF6
void __ISR(_EXTERNAL_0_VECTOR, IPL7AUTO) ISR_INT0(void)
{
    IFS0bits.INT0IF=0;
    if(INTCONbits.INT0EP==1)
    {//立上り割込み処理
        LATBbits.LATB8=1;
        INTCONbits.INT0EP=0;//立下り待ちにする。
        
        
        //printf("%d ", OV7670.HpixCounter);
        OV7670.HpixCounter=0;
    }else
    {//立下り割込み処理
        INTCONbits.INT0EP=1;//立上り待ちにする。
        OV7670.LineCounter++;
        if(OV7670.LineCounter==120)
        {
            OV7670.LineCounter=0;
        }
        LATBbits.LATB8=0;
    }
    
    
}

//PCLK割込み INT1:RD1
void __ISR(_EXTERNAL_1_VECTOR, IPL7AUTO) ISR_INT1(void)
{
    IFS0bits.INT1IF=0;
    LATBbits.LATB9=~LATBbits.LATB9;
    //OV7670.portBval = (uint8_t)PORTE;
    //sprite1[OV7670.HpixCounter++]=OV7670.portBval;
    //sprite1[OV7670.HpixCounter++]=PORTE;
}

//DMA2
//トリガー:Timer2
//from sprite1 to SPI2BUF
//sprite1 size:160*2
//SPI2BUF size:1
void __ISR(_DMA2_VECTOR, IPL7AUTO) ISR_Dma2(void)
{     
    IFS2bits.DMA2IF = 0;        // DMA1割り込みフラグのクリア
    if(DCH2INTbits.CHBCIF == 1)
    {
        SPI2_Stop();
        T2CONbits.ON=0;
        //LATBbits.LATB1=~LATBbits.LATB1;
        DCH2SSA = KVA_TO_PA(&sprite1);
        
        DCH2CONbits.CHEN = 1;   // DMA1を有効にする
        
        DCH2INTbits.CHBCIF = 0; // ブロック終了割り込みフラグのクリア
        
        DCH2ECONbits.SIRQEN = 1;
    }
    
}

//DMA1
//トリガー:外部割込み１ INT1
//from PORTE(size 1)
//to sprite1(size160*2)
void __ISR(_DMA1_VECTOR, IPL7AUTO) ISR_Dma1(void)
{
    uint8_t mxNum;
    
    IFS2bits.DMA1IF=0;
    if(DCH1INTbits.CHDDIF==1)
    {
        DCH1INTbits.CHDDIF=0;
        
        
        //DMACONbits.ON = 0; //DMAモジュールをOFFにする。
        DCH1CONbits.CHEN=0;//DMA1:disable SYNC割込みで復帰。
       
        mxNum = colorCap();

        /*ステッピングモータに指示を出す。水平方向*/
        if(existAreaNum!=0 && existAreaNum1!=0)
        {
            if(existAreaNum==1 || existAreaNum==4 || existAreaNum==7 ||
                    existAreaNum1==1 || existAreaNum1==4 || existAreaNum1==7)
                stLeftMoving(1);
            if(existAreaNum==3 || existAreaNum==6 || existAreaNum==9 ||
                    existAreaNum1==3 || existAreaNum1==6 || existAreaNum1==9)
                stRightMoving(1);
        }
        /*ステッピングモータに指示を出す。垂直方向*/
        if(existAreaNum!=0 && existAreaNum1!=0)
        {
            if(existAreaNum==1 || existAreaNum==2 || existAreaNum==3 ||
                    existAreaNum1==1 || existAreaNum1==2 || existAreaNum1==3)
                st1DownMoving(1);
            if(existAreaNum==7 || existAreaNum==8 || existAreaNum==9 ||
                    existAreaNum1==7 || existAreaNum1==8 || existAreaNum1==9)
                st1UpMoving(1);
        }
        
        
        
        
        WriteMatrix();//９分割のラインを描画
        pushSprite();//RAM上の画面データをDMA2＋SPIで送信開始。
        
        DCH1DSA = KVA_TO_PA(&sprite1);
        DCH1CONbits.CHEN=1;
        
        //IEC2bits.DMA1IE = 0;
        //IEC2bits.DMA2IE = 0;
    }
}
識別トラッキング
//トラッキング対象物の左端と右端の座標を取得。
//座標がある画面エリア番号を取得。
uint8_t colorCap(void)
{
    int col,row,pix,err;
    static uint8_t xmin, xmax, length;
    uint8_t color,red,green,blue;
    static int row1=20;
    bool exit;
    static bool exist;
    static int no_exist=0;
    int counter;
    
    //uint8_t existAreaNum;
    char ch;
    
    exist = true;
    exit = false;
    counter=0;
    for(row=row1; row<row1+30; row++)
    {
        for(pix=10; pix<300; pix++)
        {
            color = sprite1[(row*320)+pix];
            if(pix%2==0)
            {
                red = color>>3;
                green = (color&0x07)<<3;
                
                
                
            }else{
                blue = color & 0x1F;
                if(red>15 && green<10)
                //if(red>20 && green>10 && blue<10)
                {
                    col = pix/2; 
                    
                    
                    if(OV7670.cCap.x.min==0)
                    {
                        OV7670.cCap.x.min = col;
                        OV7670.cCap.y.min = row;
                        OV7670.cCap.x.max = col;
                        OV7670.cCap.y.max = row;
                    }else
                    {
                        if(col<OV7670.cCap.x.min)
                        {
                            OV7670.cCap.x.min = col;
                            OV7670.cCap.y.min = row;
                        }
                        if(col>OV7670.cCap.x.max)
                        {
                            OV7670.cCap.x.max=col;
                            OV7670.cCap.y.max = row;
                        }
                    }
                    /*exist=true;
                    counter++;
                    if(counter>5)
                    {
                        exit=true;
                        exist = true;
                    }*/
                }   
            }
            if(exit)
            {
                exit=false;
                break;
            }
        }
        //if(exit)break;
    }
    
    if((row==row1+30) && (pix==300))
        no_exist++;
    if(no_exist>=20)
    {
        no_exist=0;
        existAreaNum=0;
        exist=false;
    }
    
    row1+=30;
    if(row1>=110)
    {
        row1=20;
    }
    
    if(exist && OV7670.cCap.x.min!=0)
    {
        
        existAreaNum = getMatrix(OV7670.cCap);
        existAreaNum1 = getMatrix1(OV7670.cCap);
#define indication
#ifdef indication
        bresenham_circle(OV7670.cCap.x.min, OV7670.cCap.y.min, 6, 0x0f, 0x3F, 0x00);
        bresenham_circle(OV7670.cCap.x.min, OV7670.cCap.y.min,  5, 0x0f, 0x3F, 0x00);
        if(existAreaNum!=0)
        {
            ch=0x30+existAreaNum;
            ST7735S_Printf1216(numPoint[existAreaNum][0],
                numPoint[existAreaNum][1],&ch,1,0x1F,0x3F,0x1F);
        }
        bresenham_circle(OV7670.cCap.x.max, OV7670.cCap.y.max,  6, 0x0f, 0x00, 0x1F);
        bresenham_circle(OV7670.cCap.x.max, OV7670.cCap.y.max,  5, 0x0f, 0x00, 0x1F);
        
        if(existAreaNum1!=0)
        {
            ch=0x30+existAreaNum1;
            ST7735S_Printf1216(numPoint[existAreaNum1][0],
                numPoint[existAreaNum1][1],&ch,1,0x1F,0x3F,0x1F);
        }
#endif
        OV7670.cCap.x.min = 0;
        return existAreaNum;
    }
    OV7670.cCap.x.min = 0;
    return 0;
    
}

//エリア番号取得（左端）
uint8_t getMatrix(_captureData pt)
{
    uint8_t returnNum=0;
    
    if(0<pt.y.min && pt.y.min<=40)
    {
        if( 0<pt.x.min && pt.x.min<=53)
            returnNum=1;
        if(53<pt.x.min && pt.x.min<=106)
            returnNum=2;
        if(106<pt.x.min && pt.x.min<=159)
            returnNum=3;
    }
    
    if(40<pt.y.min && pt.y.min<=80)
    {
        if( 0<pt.x.min && pt.x.min<=53)
            returnNum=4;
        if(53<pt.x.min && pt.x.min<=106)
            returnNum=5;
        if(106<pt.x.min && pt.x.min<=159)
            returnNum=6;
    }
    
    
    if(80<pt.y.min && pt.y.min<=119)
    {
        if( 0<pt.x.min && pt.x.min<=53)
            returnNum=7;
        if(53<pt.x.min && pt.x.min<=106)
            returnNum=8;
        if(106<pt.x.min && pt.x.min<=159)
            returnNum=9;
    }
    return returnNum;
}

//画面エリア番号取得(右端）
uint8_t getMatrix1(_captureData pt)
{
    uint8_t returnNum=0;
    
    if(0<pt.y.max && pt.y.max<=40)
    {
        if( 0<pt.x.max && pt.x.max<=53)
            returnNum=1;
        if(53<pt.x.max && pt.x.max<=106)
            returnNum=2;
        if(106<pt.x.max && pt.x.max<=159)
            returnNum=3;
    }
    
    if(40<pt.y.max && pt.y.max<=80)
    {
        if( 0<pt.x.max && pt.x.max<=53)
            returnNum=4;
        if(53<pt.x.max && pt.x.max<=106)
            returnNum=5;
        if(106<pt.x.max && pt.x.max<=159)
            returnNum=6;
    }
    
    
    if(80<pt.y.max && pt.y.max<=119)
    {
        if( 0<pt.x.max && pt.x.max<=53)
            returnNum=7;
        if(53<pt.x.max && pt.x.max<=106)
            returnNum=8;
        if(106<pt.x.max && pt.x.max<=159)
            returnNum=9;
    }
    return returnNum;
}
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?