camera5.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,j=0,idata=0;
int data_binary[3][10];
//-----------------------------------------------------------------------------
// 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(10);
  output_d(byte_send);   // out 8 bits
  delay_us(10);

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

  delay_us(10);
}
//-----------------------------------------------------------------------------
void get_camera()
{
 int data=0;
int col_i=0,row_i=0;
while (data!=0xaa) data=getc();
 data=getc();
if (data==0xaa)
{while (data!=0xaa) data=getc();}
 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 main()
{
 boolean cam_ready=0;
 int t1=0,t2=0,ta1=0,ta2=0;
 char s1[15];
 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 (j=0;j<10;j++)
    {
       data_binary[i][j]=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)

 //Init camera
 //reset cam
 //printf("rs \r");
 //get_string(s,4);
 //getc();
 //gets(s1);
 //getc();
 //delay_ms(50);

 // 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();
   delay_ms(200);
   //if ((!input(pin_b5)) && (ok_image))
   if (ok_image)
   {
    t1=0;t2=0;
    ta1=0;ta2=0;
    idata=0;
    for (j=0;j<10;j++)
    {
      if (data_binary[0][j]!=0)
      {
       if (idata==0)
       {
        t1=data_binary[0][j];
        ta1=j+1;
       }
      }
      else
      {
       if (idata!=0)
       {
        t2=data_binary[0][j-1];
        ta2=j;
       }
      }
      idata=data_binary[0][j];
    }
    sendbyte_psp(252);//init for data
    sendbyte_psp(t1);
    sendbyte_psp(ta1);
    sendbyte_psp(t2);
    sendbyte_psp(ta2);
    //sendbyte_psp(cordinate[0]);
    //.sendbyte_psp(cordinate[2]);
   }
 }
}