phathung.c

Embedded camera control program.

// Connecting with Camera to read and calculate 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

#define alpha_x 4444.4    // he so qui chuyen phuong x (1/m)
#define alpha_y 10222.2   // he so qui chuyen phuong y (1/m)
#define a 78              // chieu cao cua so bat anh (pixel)

boolean ok_image=0;   // Bien kiem tra trang thai nhan picture
boolean cam_ready=0;  // Bien kiem tra trang thai cua camera

unsigned int i,cordinate1[4],cordinate2[4];
signed int dau_e3=0;
char s[4],ack[3];
float 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
}

//-----------------------------------------------------------------------------
void get_camera()
{
  // Set Window 1
  printf("SW 1 33 80 111\r");
  get_string(s,4);
  getc();            // Omit ":"
  delay_us(1500);

  printf("TC\r");
  get_string(s,4);
  get_packet(cordinate1);
  getc();

   //Set Window 2 (get line)
  printf("SW 1 109 80 111\r");
  get_string(s,4);
  getc();            // Omit ":"
  delay_us(1500);

  printf("TC\r");
  get_string(s,4);
  get_packet(cordinate2);
  getc();

}

//-----------------------------------------------------------------------------
void calc_error()  // tinh toan e2 e3
{
 signed int t=0;
 float temp1=0;

 t =cordinate2[2]-cordinate2[0];

//tinh e3
 temp1=(float)(cordinate1[2]-cordinate1[0])-(float)(t); //x2-x1-t
 e3=atan(temp1*2.3/a); // 2.3=alpla_y/alpha_x

 dau_e3=abs(cordinate1[2]-cordinate2[2]);   // Xet dau e3
 dau_e3=dau_e3 - abs(cordinate2[0]-cordinate1[0]); // (x2-x2')-(x1'-x1)
 if (dau_e3<0) { e3 = - e3;}

}

//-----------------------------------------------------------------------------
// 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(5);
  output_d(byte_send);   // out 8 bits
  delay_us(10);

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

  delay_us(20);
}

//-----------------------------------------------------------------------------

void main()
{
 byte* ptr2;

 setup_adc_ports( NO_ANALOGS);

 set_tris_d(0x00);
 set_tris_e(0x00);

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

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

 for(i=0;i<4;i++) { cordinate1[i]=0;cordinate2[i]=0;}

 printf("\r");
 get_string(s,4);
 getc();            //Omit ":"
 delay_ms(200);
 if (strncmp(s,ack,3)==0) cam_ready = true;

if (cam_ready)
  {
    // 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);

    //Line Mode off
    printf("LM 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(cordinate1);
    getc();
    delay_ms(150);
  }

  delay_ms(4000);


 while (1)
  {
   get_camera();
   calc_error();
   sendbyte_psp(252);
   ptr2 = (byte*) &e3;

   for (i=0;i<4;i++)
   {
    sendbyte_psp(*ptr2);
    ptr2++;
   }
   delay_ms(10);
}

}