export from camera.c

Embedded camera control program.

#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)
#use fast_io(B)

#define Rmin 25
#define Rmax 93
#define Gmin 15
#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
//-----------------------------------------------
unsigned int cordinate1[4],cordinate2[4];
int i;
signed int e3;
unsigned dau_e3;
char s[4],ack[3];
boolean cam_ready;
//-----------------------------------------------
void get_string(char * c ,int length)
{
  int len=0;
  char s;

  do
   {
   s=getc();
   If ((s>=' ')&&(s<='~')&&(s!=13))
   If (len<length)
    {c[len++]=s;}
   }
   while (s!=13);
   c[len]=0;
   return;
}
//-----------------------------------------------
 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()
{
  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 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(20);

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

  delay_us(20);
}
//-----------------------------------------------
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;}

}
//-----------------------------------------------


void main()
{
 byte * ptr1;
 int i;
 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;
 }

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

If (cam_ready = true)
{ // 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);
  ptr1 = (byte*) &e3;
  for (i=0;i<4;i++)
  {
   sendbyte_psp(*ptr1);
   ptr1++;
  }
  }
}