カメラ制御コード – Inside PIDream

残り4つということで、だいたい1年前のコード公開

PFC.PAIORL.BIT.B6 = 1; 右
PFC.PAIORL.BIT.B7 = 1; 左
PFC.PAIORL.BIT.B8 = 1; 同期
PFC.PAIORL.BIT.B9 = 1; 中央
PFC.PAIORL.BIT.B10 = 1;
PFC.PAIORL.BIT.B11 = 1;
PFC.PAIORL.BIT.B12 = 1;
PFC.PAIORL.BIT.B13 = 1;
PFC.PAIORL.BIT.B14 = 1;
PFC.PAIORL.BIT.B15 = 1;
*/

#include <machine.h>
#include <stdarg.h>
#include "iodefine.h"
#include "sci.h"
#include "7125sio.h"

#define CLK_H PB.DR.BIT.B3=1
#define CLK_L PB.DR.BIT.B3=0
#define SCLK_H PE.DRL.BIT.B14=1
#define SCLK_L PE.DRL.BIT.B14=0
#define WAIT for(i=0;i<1000;i++){} /*======================================*/ /* プロトタイプ宣言 */ /*======================================*/ void main(void); unsigned char dipsw_getL( void ); unsigned char dipsw_getH( void ); void setReg(unsigned char data ); /*======================================*/ /* グローバル変数の宣言 */ /*======================================*/ unsigned int watc=0; unsigned char reg[16]; unsigned int h_cnt; unsigned int adcnt; unsigned char data[256]; /************************************************************************/ /* メインプログラム */ /************************************************************************/ void main( void ) { unsigned int pri_v,pri_h; int i,j; char dif,min,max,minad,maxad,out; unsigned char rf,lf,lsh; unsigned char blevel,wlevel; char mr,ml; unsigned char digisensor; /* マイコン機能の初期化 */ init_all(); //初期化 sci1_init(38400,8,0,1); reg[ 0]=0x00;//Image Capture Mode 0 = Continuous Mode reg[ 1]=0x10;//NA reg[ 2]=0x20;//Interrupt setting reg[ 3]=0x30;//Data Rate 0 = 1/4 reg[ 4]=(0x40|(dipsw_getL()+3));//露光時間 3=285fps 2=303fps 1=323fps reg[ 5]=0x50;//露光時間 reg[ 6]=0x60;//露光時間 reg[ 7]=0x70;//露光時間 reg[ 8]=0x86;//Gain reg[ 9]=0x90;//MSB setting reg[10]=0xA0;//Black level output reg[11]=0xB0;//Offset subtraction reg[12]=0xC0;//STRB reg[13]=0xD2;//NA reg[14]=0xE0;//Power save mode reg[15]=0xFE;//NA lsh=30+dipsw_getH()*10;// 1=100 2=120 3=140 //black=30~40 RESET: digisensor = 0x00; WDT.WRITE.WTCNT = 0x5A00; PB.DR.BIT.B2=0;//リセット H -> L

CMT0.CMCNT = 0;
CMT.CMSTR.BIT.STR0 = 1;//cmt0 スタート
while(CMT0.CMCNT<150){CLK_H;CLK_L;}//200us待ち CMT.CMSTR.BIT.STR0 = 0;//cmt0 停止 CLK_H; CLK_L; PB.DR.BIT.B2=1;//リセット L -> H

CMT0.CMCNT = 0;
CMT.CMSTR.BIT.STR0 = 1;//cmt0 スタート
while(CMT0.CMCNT<75){CLK_H;CLK_L;}//100us待ち CMT.CMSTR.BIT.STR0 = 0;//cmt0 停止 CLK_H; CLK_L; setReg(reg[ 0]); setReg(reg[ 1]); setReg(reg[ 2]); setReg(reg[ 3]); setReg(reg[ 4]); setReg(reg[ 5]); setReg(reg[ 6]); setReg(reg[ 7]); setReg(reg[ 8]); setReg(reg[ 9]); setReg(reg[10]); setReg(reg[11]); setReg(reg[12]); setReg(reg[13]); setReg(reg[14]); setReg(reg[15]); RESTART: CLK_H; CLK_L; PB.DR.BIT.B1=1;//スタート CLK_H; PB.DR.BIT.B1=0;//スタート信号戻す 920ns リセット開始から0.5ms後 CLK_L; CLK_H; i=0; WDT.WRITE.WTCNT = 0x5A00; //2.73ms以内にクリア NEXTREAD: while(PF.DRL.BIT.B1==0){//STRBシグナル待 CLK_L; CLK_H; } AD0.ADCR.BIT.ADST = 1;//AD変換開始 if(i==0){ PA.DRL.BYTE.H = digisensor;//xxxx x001 PA.DRL.BIT.B8 = 0; //mainCPUへの同期信号クリア } CLK_L;CLK_H; CLK_L;CLK_H;//STRB -> L
CLK_L;CLK_H;

while(AD0.ADCSR.BIT.ADF == 0);//AD変換終了まで待つ
data[i] = AD0.ADDR0 >> 8; //値を保存

i++;
if(i<128){goto NEXTREAD;} //32*5= 160 setReg(0x21);//Set STOP=1 min=-10; max= 10; minad=0; maxad=0; //ライン重心計算 for(i=1;i<29;i++){ dif=data[i+95]-data[i+96]; //96 97 if(i<24 && dif5 && dif>max){max=dif;maxad=i;} //白→黒
}
if(min==-10 || max==10 ||(minad>maxad)){
out=63;
}else{
out=(minad+maxad);
}
PA.DRL.BYTE.H = out<<2; /* ラインマーカしきい値計算 */ blevel=200; for(i=65;i<=91;i++){ if(blevel>data[i]){blevel=data[i];}
if(blevel>data[i+32]){blevel=data[i+32];}
}
wlevel=blevel+lsh;

digisensor=0x01;
//左マーカ
if(data[ 99]>wlevel || data[ 67]>wlevel){digisensor= digisensor | 0x40;}//0100 0000

//右マーカ
if(data[ 89]>wlevel || data[121]>wlevel){digisensor= digisensor | 0x04;}//0000 0100

//中心ライン
if(data[109]>wlevel || data[110]>wlevel || data[111]>wlevel || data[78]>wlevel){
digisensor= digisensor | 0x10;}//中央線 0001 0000

if(data[104]>wlevel || data[ 72]>wlevel){digisensor= digisensor | 0x20;} //左中マーカ
if(data[116]>wlevel || data[ 84]>wlevel){digisensor= digisensor | 0x08;} //右中マーカ

PA.DRL.BIT.B9 = 1;//

PA.DRL.BIT.B8 = 1;//同期線L->H

goto RESTART;

}

/************************************************************************/
/* ディップスイッチ値読み込み */
/* 戻り値スイッチ値 0～15 */
/************************************************************************/
unsigned char dipsw_getL( void )
{
unsigned char sw=0;

if(PE.PRL.BIT.B2==0){sw+=2;}
if(PE.PRL.BIT.B3==0){sw+=1;}

return sw;
}

if(PE.PRL.BIT.B0==0){sw+=2;}
if(PE.PRL.BIT.B1==0){sw+=1;}

return sw;
}

/************************************************************************/
/* レジスタ書き込み */
/* */
/************************************************************************/
void setReg( unsigned char data )
{
int wt=0;
//SCLK_Lからスタート
//adress
if((data&0x80)==0x80){PE.DRL.BIT.B15=1;}else{PE.DRL.BIT.B15=0;}
SCLK_H;SCLK_L;
if((data&0x40)==0x40){PE.DRL.BIT.B15=1;}else{PE.DRL.BIT.B15=0;}
SCLK_H;SCLK_L;
if((data&0x20)==0x20){PE.DRL.BIT.B15=1;}else{PE.DRL.BIT.B15=0;}
SCLK_H;SCLK_L;
if((data&0x10)==0x10){PE.DRL.BIT.B15=1;}else{PE.DRL.BIT.B15=0;}
SCLK_H;SCLK_L;
//data
if((data&0x08)==0x08){PE.DRL.BIT.B15=1;}else{PE.DRL.BIT.B15=0;}
SCLK_H;SCLK_L;
if((data&0x04)==0x04){PE.DRL.BIT.B15=1;}else{PE.DRL.BIT.B15=0;}
SCLK_H;SCLK_L;
if((data&0x02)==0x02){PE.DRL.BIT.B15=1;}else{PE.DRL.BIT.B15=0;}
SCLK_H;SCLK_L;
if((data&0x01)==0x01){PE.DRL.BIT.B15=1;}else{PE.DRL.BIT.B15=0;}
SCLK_H;
PB.DR.BIT.B16=1;//LOAD_H
CLK_H;
PB.DR.BIT.B16=0;//LOAD_L
SCLK_L;
CLK_L;
}