camera1.c

Embedded camera control program.

//Connecting with camera for reading and calculating the errors

#device  PIC16F877 *=16 ADC=10
#include <16f877.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>

#fuses XT,NOWDT,NOPROTECT,NOLVP,NOBROWNOUT,PUT
#use delay(clock=20000000) 	// 20 MhZ
#use rs232(baud=115200,parity=N,xmit=PIN_C6, rcv=PIN_C7)

#define psp_cs PIN_E2
#define psp_wr PIN_E1
#define psp_rd PIN_E0
#use fast_io(D)
#use fast_io(E)

#define Rmin 25
#define Rmax 93
#define Gmin 25
#define Gmax 72
#define Bmin 24
#define Bmax 66

boolean ok_image=0;
int cordinate[4];
char s[4],ack[3];
int i=0,k=0,idata=0;
unsigned int data_binary[3][10];

int set_valueT=7,set_valueP=7;
int wheel_T=1,wheel_P=1;

signed int j,dau_e3=0;
unsigned int mask;
int t2,t20,t46;
int x2_1=0,x2_2=0,x20_1=0,x20_2=0,x46_1=0,x46_2=0;
float delta_t=0;
int h_win;
float temp1,e3=0;
//-----------------------------------------------------------------------------
// Get a string from camera
void get_string(char * s,int max)
{
 int len;
 char c;
 len = 0;
 do
 { c=getc();
   if ((c>=' ') && (c<='~') && (c!=13))
   if(len<max-1) { s[len++]=c; }
 } while(c!=13);

 s[len]=0;
 return;
}

//-----------------------------------------------------------------------------
// Return 2 cordinates (x1,y1),(x2,y2),4 values->variable cordinate[3] correlatively
void get_packet(int* cordin)
{
 char c,space;

 s[0] = 0;
 i = 0;
 space = 0;
 getc();      // Omit Packet's Name Character, eg. M,S,C ...
 getc();      // Omit first space

 do
 {
  c = getc();
  if ((c!=32) && (c!=13))   // Begin a new cordinate
  {
   if ((c>=' ') && (c<='~'))
   {
    s[i] = c;
    i++;
   }
  }
  else
  {
   s[i] = 0;
   if (s[0]!=0) cordin[space] = atoi(s);
   s[0]=0;
   space++;
   i=0;
  }
 } while ((space<4) && (c!=13));

 while (getc()!=13) {}           // Omit remains numbers
}

//-----------------------------------------------------------------------------
// Send a byte to PSP
void sendbyte_psp(int byte_send)
{
  output_low(psp_cs);    // low
  output_high(psp_rd);   // high
  output_low(psp_wr);    // low

  delay_us(15);
  output_d(byte_send);   // out 8 bits
  delay_us(15);

  output_high(psp_wr);
  output_high(psp_cs);
  output_high(psp_rd);

  delay_us(15);
}
//-----------------------------------------------------------------------------
void get_camera()
{
 int data=0;
int col_i=0,row_i=0;
printf("TC\r");
//get_string(s,4);
  set_valueT=getc();            //Omit ":"
  delay_ms(50);
/*  // data=getc();
//while (data!=13) data=getc();
   data=getc();
if (data==0xaa)
{
 data=getc();
 while (data!=0xaa)
  {
   data=getc();
   col_i++;
   if (col_i<=10)
    {
     if (row_i==2)  data_binary[0][col_i]=data;
     if (row_i==20) data_binary[1][col_i]=data;
     if (row_i==46) data_binary[2][col_i]=data;
    }
   else {row_i++;col_i=0;}
  }
}
data=getc();//get off 0xaa
if (data==0xaa) {ok_image=1;}
get_packet(cordinate);*/
}
//-----------------------------------------------------------------------------

void calc_error()  // tinh toan e2 e3
{
// Tim be ngang t cua duong thang trong goi data_binary de tinh e3

// Xu ly hang 2
 i=0;j=7; mask=0x80;
 while ( (data_binary[0][i]==0) && (i<10) ) i++;
 if (i==10) t2=0;
 else    // if (i<10)
 {
  while ( (!(data_binary[0][i] & mask)) && (j>=0) ) {j--; mask >>= 1;}
  x2_1 = i*8 + 8 - j;

  i=9;j=0;mask=0x01;
  while ( (data_binary[0][i]==0) && (i>=0) ) i--;
  while ( !(data_binary[0][i] & mask) && (j<8) ) {j++; mask <<= 1;}
  x2_2 = i*8 + 8 - j;

  t2 = x2_2 - x2_1;
 }

// Xu ly hang 20
 i=0;j=7; mask=0x80;
 while ( (data_binary[1][i]==0) && (i<10) ) i++;
 if (i==10) t20=0;
 else    // if (i<10)
 {
  while ( (!(data_binary[1][i] & mask)) && (j>=0) ) {j--; mask >>= 1;}
  x20_1 = i*8 + 8 - j;

  i=9;j=0;mask=0x01;
  while ( (data_binary[1][i]==0) && (i>=0) ) i--;
  while ( !(data_binary[1][i] & mask) && (j<8) ) {j++; mask <<= 1;}
  x20_2 = i*8 + 8 - j;

  t20 = x20_2 - x20_1;
 }

// Xu ly hang 46
 i=0;j=7; mask=0x80;
 while ( (data_binary[2][i]==0) && (i<10) ) i++;
 if (i==10) t46=0;
 else    // if (i<10)
 {
  while ( (!(data_binary[2][i] & mask)) && (j>=0) ) {j--; mask >>= 1;}
  x46_1 = i*8 + 8 - j;

  i=9;j=0;mask=0x01;
  while ( (data_binary[2][i]==0) && (i>=0) ) i--;
  while ( !(data_binary[2][i] & mask) && (j<8) ) {j++; mask <<= 1;}
  x46_2 = i*8 + 8 - j;

  t46 = x46_2 - x46_1;
 }

 if (t20!=0) delta_t = t20 + 0.2 ;  // Calc delta_t (plus error approximately 0.4 )
 else delta_t=0;

//tinh e3 suy xet cac truong hop
 h_win=cordinate[3]-cordinate[1];  //y2-y1
 dau_e3=1;    // first mount e3>0

 if (h_win>0)
 {
   if ( (cordinate[0]<3) && (cordinate[2]>140) )  // Case: line is horizontal
   {
     delta_t=-delta_t;
     if ( ((80-x20_2)-(x20_1-1))<0 ) dau_e3=0; // e3<0
   }
   else // Case: line is vertical
   {
     if (cordinate[0]<3)
     {
       delta_t=0;
       if (t46<t2)  dau_e3=0;/*e3<0 */
     }
     else
     {
       if (cordinate[2]>78) { delta_t=0; if (t46>t2)  dau_e3=0;/*e3<0 */ }
       else  // Case: Normal
       {
         if ( ((cordinate[2]-x2_2)-(x2_1-cordinate[0]))<0 ) dau_e3=0; //e3<0
       }
     }
   }
 }
 else e3=0;

 temp1=(float)(cordinate[2]-cordinate[0])-(float)(delta_t); //x2-x1-delta_t
 e3=atan(2.3*temp1/h_win);

 if (dau_e3==0) { if (e3>0)e3=-e3;}

//tinh e2
// temp1=(float)((cordinate[0]+cordinate[2])/2);
 //temp1=(40-temp1)/alpha_x;
// e2=temp1*cos(e3);
}
//-------------------------------------------------------------
void calc_velo()
{
  if (e3>0)
  {set_valueT=5;
   wheel_T=0;
   set_valueP=5;
   wheel_P=1;}
  if (e3<0)
  {set_valueT=5;
   wheel_T=1;
   set_valueP=5;
   wheel_P=0;}
  set_valueT=70;
  set_valueP=20;
   wheel_T=0;
   wheel_P=1;
}
//---------------------------------------------------

void main()
{
 boolean cam_ready=0;
 setup_adc_ports( NO_ANALOGS);
 set_tris_d(0x00);
 set_tris_e(0x00);
 set_tris_b (0x20);

 s[0]=0;
 strcpy(ack,"ACK");

 for (i=0;i<3;i++)
  {
   for (k=0;k<10;k++)
    {
       data_binary[i][k]=0;
    }
  }
 for(i=0;i<4;i++) {cordinate[i]=0;}

 delay_ms(1000); // Thoi gian delay de camera on dinh, thuong ban dau phai 1-2(s)

 // Check camera status
 printf("\r");
 get_string(s,4);
 getc();            //Omit ":"
 delay_ms(50);
 if (strncmp(s,ack,3)==0) cam_ready = true;
 while (!cam_ready)
 {
  printf("\r");
  get_string(s,4);
  getc();            //Omit ":"
  delay_ms(50);
  if (strncmp(s,ack,3)==0) cam_ready = true;
 }
 if (cam_ready)
  {
    //set window
    printf("SW 1 1 80 143\r");
    get_string(s,4);
    getc();            // Omit ":"
    delay_ms(150);

    // White balance on
    printf("CR 18 44\r");
    get_string(s,4);
    getc();            // Omit ":"
    delay_ms(150);

    // Poll Mode on
    printf("PM 1\r");
    get_string(s,4);
    getc();            // Omit ":"
    delay_ms(150);

    // Middle Mass mode off
    printf("MM 0\r");
    get_string(s,4);
    getc();            // Omit ":"
    delay_ms(150);

    //set RGB range
    printf("TC %u %u %u %u %u %u\r", Rmin,Rmax,Gmin,Gmax,Bmin,Bmax);
    get_string(s,4);
    get_packet(cordinate);
    getc();
    delay_ms(150);

    //Should have
    printf("\r");
    get_string(s,4);
    getc();            //Omit ":"
    delay_ms(150);

    //Line Mode on
    printf("LM 1\r");
    get_string(s,4);
    getc();            // Omit ":"
    delay_ms(150);

    // Poll Mode off
  //  printf("PM 0\r");
   // get_string(s,4);
   // getc();            // Omit ":"
   // delay_ms(150);

    delay_ms(12000);     // Delay after Initial to stable 3-5(s)
  }
   //Delay time transmit a charactor
   printf("DM 0\r");
   get_string(s,4);
   getc();            // Omit ":"
   delay_ms(150);

   //Track color
   printf("TC \r");
   get_string(s,4);

 while (1)
 {
     get_camera();
                  //if ((!input(pin_b5)) && (ok_image))
    //calc_error();
     delay_us(5);
     //calc_velo();
     sendbyte_psp(252);  // Kiem tra data nhan co dung khong
     sendbyte_psp(set_valueT);//chuong trinh nay truyen nhan ok
     sendbyte_psp(set_valueP);
     sendbyte_psp(wheel_T);
     sendbyte_psp(wheel_p);

     delay_us(10);

    }

}