#include #include #include #include #include #include #include // configuration PICDEM2+quartz #pragma config OSC = HS // dépend de l'oscillateur utilisé #pragma config WDT = OFF // pas de wd pour debug #pragma config LVP = OFF // pas de lvp pour l'icd #pragma config PBADEN = OFF // RB4:RB0 digitales au reset #define ADR_CAM_WRITE 0xB0 // Adresse de la camera en ecriture #define ADR_CAM_READ 0xB1 // Adresse de la camera en lecture #define led PORTBbits.RB2 // Test PIC int X1,X2,X3,X4; int Y1,Y2,Y3,Y4; int i,s,p=0,j,k,indice0,indice1,indice2,indice3,indice4; int l,n_tab=0,n_mes=0; unsigned int distance; unsigned int dmin; unsigned char data_buf[16]; //Ecrire un octet sur le bus (vers un slave receiver) // Structure blob typedef struct blob blob; struct blob{ int X; int Y; }; void init_cam (void); void main (void) { blob tab[4],tab_mes[4]; // Initialisation structure Blob Delay10TCYx(5); // 50ms init_cam(); for(;;) { led=!led; // LED de test StartI2C(); IdleI2C(); // Le maitre (PIC) génère un bit de START WriteI2C(ADR_CAM_WRITE); IdleI2C(); // Connexion à l'adresse d'écriture 0xB0 WriteI2C(0x36); IdleI2C(); // Adresse de stockage des données StopI2C();IdleI2C();// Le maitre génère un bit de STOP StartI2C(); IdleI2C();// Le maitre (PIC) génère un bit de START WriteI2C(ADR_CAM_READ); IdleI2C();// Connexion à l'adresse de lecture 0xB1 for(i=0;i<16;i++) { data_buf[i]=ReadI2C(); IdleI2C(); // Reception de 16 octets if(i<15) {AckI2C(); IdleI2C();} // Le maitre accuse la bonne reception des octets else {NotAckI2C();IdleI2C();} // Le maitre émet un NotAck pour le dernier octet reçu } StopI2C();IdleI2C(); // Le maitre génère un bit de STOP X1 = data_buf[1]; //coordonnées du blob 1 Y1 = data_buf[2]; s = data_buf[3]; X1 += (s & 0x30) <<4; Y1 += (s & 0xC0) <<2; X2 = data_buf[4]; //coordonnées du blob 2 Y2 = data_buf[5]; s = data_buf[6]; X2 += (s & 0x30) <<4; Y2 += (s & 0xC0) <<2; X3 = data_buf[7]; //coordonnées du blob 3 Y3 = data_buf[8]; s = data_buf[9]; X3 += (s & 0x30) <<4; Y3 += (s & 0xC0) <<2; X4 = data_buf[10]; //coordonnées du blob 4 Y4 = data_buf[11]; s = data_buf[12]; X4 += (s & 0x30) <<4; Y4 += (s & 0xC0) <<2; //Tracking BLOB if(p==1) { tab_mes[0].X = X1; tab_mes[0].Y = Y1; // Arrivé de nouvelles coordonnées tab_mes[1].X = X2; tab_mes[1].Y = Y2; tab_mes[2].X = X3; tab_mes[2].Y = Y3; tab_mes[3].X = X4; tab_mes[3].Y = Y4; n_mes=0; for(l=0;l<4;l++) { if((tab_mes[l].X != 1023) && (tab_mes[l].Y != 1023)) // On compte le nombre de points mesurés { n_mes++;} } if( n_tab>n_mes) // Disparition dun point de l'écran { for(j=0;j<4;j++) { // On compare 1 point mesuré ... if(j==0) { dmin=65536; for(k=0;k<4;k++) // aux 4 points précédent { distance= fabs((tab[k].X -tab_mes[j].X) -(tab[k].Y -tab_mes[j].Y)); if(distance<=dmin) // On recherche la plus petite distance {dmin=distance;indice0=k;}// Indice du blob le plus proche } tab[indice0].X=tab_mes[j].X;// On assigne le blob correspondant tab[indice0].Y=tab_mes[j].Y; } if(j==1) { dmin=65536; for(k=0;k<4;k++) { if(k!=indice0)// aux 3 points précédent restant { distance= fabs((tab[k].X -tab_mes[j].X) -(tab[k].Y -tab_mes[j].Y)); if(distance<=dmin) {dmin=distance;indice1=k;} } } tab[indice1].X=tab_mes[j].X; tab[indice1].Y=tab_mes[j].Y; } if(j==2) { dmin=65536; for(k=0;k<4;k++) { if(k!=indice0 && k!= indice1) // aux 2 points précédent restant { distance= fabs((tab[k].X -tab_mes[j].X) -(tab[k].Y -tab_mes[j].Y)); if(distance<=dmin) {dmin=distance;indice2=k;} } } tab[indice2].X=tab_mes[j].X; tab[indice2].Y=tab_mes[j].Y; } if(j==3) { for(k=0;k<4;k++) { if(k!=indice0 && k!= indice1 && k!= indice2) // au point restant { indice3=k; } } tab[indice3].X=tab_mes[j].X; tab[indice3].Y=tab_mes[j].Y; } } } else // Apparition d'un point sur l'écran ( ou nombre de points inchangés ) { for(j=0;j<4;j++) { // On compare 1 ancien point ... if(j==0){ dmin=65536; for(k=0;k<4;k++) // aux 4 nouveaux points { distance= fabs((tab[j].X -tab_mes[k].X) -(tab[j].Y -tab_mes[k].Y)); if(distance<=dmin) // On recherche la plus petite distance {dmin=distance;indice0=k;} // Indice du blob le plus proche } tab[j].X =tab_mes[indice0].X; // On assigne le blob correspondant tab[j].Y =tab_mes[indice0].Y; } if(j==1){ dmin=65536; for(k=0;k<4;k++) { if(k!=indice0) // aux 3 nouveaux points restant { distance= fabs((tab[j].X -tab_mes[k].X) -(tab[j].Y -tab_mes[k].Y)); if(distance<=dmin) {dmin=distance;indice1=k;} } } tab[j].X =tab_mes[indice1].X; tab[j].Y =tab_mes[indice1].Y; } if(j==2){ dmin=65536; for(k=0;k<4;k++) { if(k!=indice0 && k!=indice1) // aux 2 nouveaux points restant { distance= fabs((tab[j].X -tab_mes[k].X) -(tab[j].Y -tab_mes[k].Y)); if(distance<=dmin) {dmin=distance;indice2=k;} } } tab[j].X =tab_mes[indice2].X; tab[j].Y =tab_mes[indice2].Y; } if(j==3){ for(k=0;k<4;k++) { if(k!=indice0 && k!=indice1 && k!=indice2) // au point restant { indice4=k; } } tab[j].X =tab_mes[indice4].X; tab[j].Y =tab_mes[indice4].Y; } } } } // end p=1 else{ tab[0].X = X1; tab[0].Y = Y1; // Arrivé des premières coordonnées des points tab[1].X = X2; tab[1].Y = Y2; tab[2].X = X3; tab[2].Y = Y3; tab[3].X = X4; tab[3].Y = Y4; for(l=0;l<4;l++) { if((tab[l].X != 1023) && (tab[l].Y != 1023))// On compte le nombre de points traqués { n_tab++;} } p=1; } // end (p=o) n_tab=n_mes; printf(" 1(X;Y): %d %d 2(X;Y): %d %d 3(X;Y): %d %d 4(X;Y): %d %d \n\r ",tab[0].X,tab[0].Y,tab[1].X,tab[1].Y,tab[2].X,tab[2].Y,tab[3].X,tab[3].Y); } // end for } // end void void init_cam (void) { unsigned char x ; TRISBbits.TRISB2=0; //Liaison série OpenUSART( USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE & USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH, 25 ); OpenI2C(MASTER,SLEW_OFF); SSPADD = 9 ; // 100Khz pour Fosc=4Mhz x=SSPBUF ; // Lecture factice pour assurer un buffer vide // IR sensor initialize StartI2C();IdleI2C(); WriteI2C(ADR_CAM_WRITE); IdleI2C(); WriteI2C(0x06);IdleI2C(); // Activation de l'horloge pixel WriteI2C(0x13); IdleI2C(); StopI2C(); IdleI2C(); Delay10TCYx(10); // 100ms StartI2C(); IdleI2C(); WriteI2C(ADR_CAM_WRITE); IdleI2C(); WriteI2C(0x06); IdleI2C(); // Activation camera 2 WriteI2C(0x1A); IdleI2C(); StopI2C(); IdleI2C(); Delay10TCYx(10); StartI2C(); IdleI2C(); WriteI2C(ADR_CAM_WRITE); IdleI2C(); WriteI2C(0x30); IdleI2C(); // Registre de COMMANDE début de configuration WriteI2C(0x01); IdleI2C(); StopI2C(); IdleI2C(); Delay10TCYx(10); StartI2C(); IdleI2C(); WriteI2C(ADR_CAM_WRITE); IdleI2C(); WriteI2C(0x00); IdleI2C(); // Configuration du bloc 1 de Sensibilité (9 octets) WriteI2C(0x02); IdleI2C(); // UNKNOW WriteI2C(0x00); IdleI2C(); WriteI2C(0x00); IdleI2C(); WriteI2C(0x71); IdleI2C(); WriteI2C(0x01); IdleI2C(); WriteI2C(0x00); IdleI2C(); WriteI2C(0x64); IdleI2C(); // MAX SIZE WriteI2C(0x00); IdleI2C(); WriteI2C(0xAA); IdleI2C(); // GAIN StopI2C(); IdleI2C(); Delay10TCYx(10); StartI2C(); IdleI2C(); WriteI2C(ADR_CAM_WRITE); IdleI2C(); WriteI2C(0x1A); IdleI2C(); // Configuration du bloc 2 de Sensibilité (2 octets) WriteI2C(0x63); IdleI2C(); // GAINLIMIT WriteI2C(0x03); IdleI2C(); // MINSIZE StopI2C(); IdleI2C(); Delay10TCYx(10); StartI2C(); IdleI2C(); WriteI2C(ADR_CAM_WRITE); IdleI2C(); WriteI2C(0x33); IdleI2C(); // MODE ETENDU du format des données WriteI2C(0x03); IdleI2C(); StopI2C(); IdleI2C(); Delay10TCYx(10); StartI2C(); IdleI2C(); WriteI2C(ADR_CAM_WRITE); IdleI2C(); WriteI2C(0x30); IdleI2C(); // Registre de COMMANDE fin de configuration WriteI2C(0x08); IdleI2C(); StopI2C(); IdleI2C(); Delay10TCYx(10); }