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

# OV7670　カラートラッキング
:::note
随時更新予定。
24.11.14 DMAモジュールの停止と復帰を改変。DCH1CONbits.CHENビットの使用に変更。
:::

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

<iframe width="560" height="315" src="https://www.youtube.com/embed/rS-ryuoq7-s?si=dVTT4FoDqAzVhcPl" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>

![IMG_5098.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/2dc6fbf9-3cf6-9a69-6147-92bce3834e3a.jpeg)

:::note
# カラートラッキング概略
![トラッキング概略図.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/12f6969d-09a0-2abc-6834-f07346958f8a.jpeg)

画面を９分割し、エリア名を１から９にします。５が画面の中心になります。
対象物(赤い紙)の左端と右端の座標を、逐次検索します。その座標をもとに、対象物がいるエリアを特定します。
1,2,3にあるならカメラを上昇、7,8,9にあるならカメラを下降させます。同様に、
1,4,7にある場合は、カメラを左に、3,6,9にある場合は、カメラを右に振って、中心のエリア５に移動させます。
移動は、ステッピングモータ28BYJ-48を2個、使用しました。
<img width="280" alt="ファイル名" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/f796c500-22d9-18ca-83aa-a6ad996ff707.jpeg">    <img width="280" alt="ファイル名" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/619bfbef-6456-11fc-2a53-2360c163d266.jpeg">

:::



# PIC32ピン割り振り表
![ピン配カラートラッキング.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/7296a85e-4f5c-f540-d643-4f9a5a9b138e.png)


:::note
# トラッキングの方法

![モータ駆動方向.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/71b9d381-02ac-cf9b-39a5-d735113d13c6.jpeg)
対象物の左端と右端の座標から、対象エリアを特定し、あらかじめ決めておいた方向に、ステッピングモータを駆動します。左端と、右端の両方が中心のエリア５に入ったら、ステッピングモータを停止します。
**<font color="DeepPink">青い矢印は、画面上の移動方向で、ステッピングモータの駆動方向は１８０度逆になります。</font>**

:::

:::note 
# ダミー行の追加で、トラッキング制御の時間を作る。
![ダミーライン.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3693934/efa40fb1-a303-b1a1-a092-2a7eaaf0c7c0.jpeg)

カメラのダミー行設定レジスタ
DM_LNL(0x92):Dummy Row low 8bits
DM_LNH(0x93):Dummy Row high 8bits
DM＿LNLに0x30、DM_LNHに0x01を設定し、３０４行分のダミー行を設定しました。
```C:ブランクエリア設定　SCCBで送信。
//blunk area
     I2C2_b2Write(OV7670deviceAdd,0x92, 0x30, true);
     I2C2_b2Write(OV7670deviceAdd,0x93, 0x01, true);
```

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

:::


# トラッキングのコード（抜粋）
:::note
DMA2とSPIをつかって、QQVGA(160×120)の画面データをRAMから液晶に送信させています。<font color="DeepPink">このとき、DMAモジュールを一旦OFFにしています。</font>
そして、VSYNC割込みで、DMAモジュールを再度、ONにしています。この操作で、１画面を転送することができるようになりました。

24.11.14訂正：<font color="deepPink">`DCH1CONbits.CHEN`</font>で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;
    }
}
```

```C:識別トラッキング
//トラッキング対象物の左端と右端の座標を取得。
//座標がある画面エリア番号を取得。
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;
}
```