psp_wrt_transmitter.c

Embedded camera control program.

// Connecting with camera for reading and calculating the errors
// Giai thuat doc camera 2
#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 Rmin 30
#define Rmax 80
#define Gmin 35
#define Gmax 85
#define Bmin 75
#define Bmax 140

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

#define psp_cs PIN_E2
#define psp_wr PIN_E1
#define psp_rd PIN_E0

#use fast_io(E)


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,cordinate[4];
char s[4],ack[3],s1[25];
unsigned int data_binary[10];  // Doc 1 hang cua anh binary
float e2=0,e3=0;
int xx1=0,xx2=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 redundant numbers
}

//-----------------------------------------------------------------------------
void get_camera()
{
 int data=0,index=0;

 data=getc();
 if (data==0xaa)
 {
  data=getc();
  while (data!=0xaa)
  {
   data=getc();
   if (index<10) data_binary[index++]=data; // Only get the first row of picture
  }
 }

 data=getc();//get off 0xaa
 if (data==0xaa) {ok_image=1;}
 get_packet(cordinate);
}
//-----------------------------------------------------------------------------
void calc_error()  // tinh toan e2 e3
{
 int j,t=0,mask;//xx1=0,xx2=0,
 float temp1=0;

// Tim be ngang t cua duong thang trong goi data_binary de tinh e3
 i=0;j=7; mask=0x80;
 while ( (data_binary[i]==0) && (i<10) ) i++;
 if (i==0) t=0;
 else    // if (i<10)
 {
  while ( !(data_binary[i] & mask) && (j>=0) ) {j--; mask >>= 1;}
  xx1 = i*8 + 8 - j;// byte[0]=0... byte[i-1]=0 byte[i]!=0,j la bit khac 0 dau tien tinh tu [7..0] trong byte[i]

  i=9;j=7;mask=0x80;
  while ( (data_binary[i]==0) && (i>=0) ) i--;
  while ( (data_binary[i] & mask) && (j>=0) ) {j--; mask >>= 1;}
  xx2 = i*8 + 7 - j;// byte[10]=0... byte[i+1]=0 byte[i]!=0,j la bit bang 0 dau tien tinh tu [7..0] trong byte[i]
  t = xx2 - xx1;
 }

//tinh e3
 temp1=(float)(cordinate[2]-cordinate[0])-(float)(t); //x2-x1-t
 e3=atan(temp1/a);

 j=abs(cordinate[2]-xx2);   // Xet dau e3
 j=j-abs(xx1-cordinate[0]); // (x2-xx2)-(xx1-x1)
 if (j<0) {e3=-e3;}

//tinh e2
 temp1=(float)((cordinate[0]+cordinate[2])/2);
 temp1=(40-temp1)/alpha_x;
 //out_line=temp1;
 //temp1=temp1/1000; // Convert from mm to m
 e2=temp1*cos(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(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 main()
{
 int timer1,timer2;
 float temp1=0;

 setup_adc_ports( NO_ANALOGS);
 setup_timer_1(T1_INTERNAL | T1_DIV_BY_8);
 //set_timer1(0);

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

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

 for(i=0;i<4;i++) cordinate[i]=0;
 for (i=0;i<10;i++) data_binary[i]=0;

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

 while (!cam_ready)
 {
  printf("\r");
  get_string(s,4);
  getc();            //Omit ":"
  delay_ms(100);
  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(300);

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

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

   //set window
    printf("SW 1 33 80 111\r");
    get_string(s,4);
    getc();            // Omit ":"
    delay_ms(1000);

    //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);
    //get_string(s1,25);
    getc();
    delay_ms(200);

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

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

  }

  delay_ms(3000);
  printf("TC \r");
  get_string(s,4); // Get ACK

 while (1)
 {

  set_timer1(0);
  get_camera();
  temp1= (float)(0.0016*get_timer1());
  timer1 = (int)(temp1);

  set_timer1(0);
  calc_error();
  temp1= (float)(0.0016*get_timer1());
  timer2 = (int)(temp1);

  //if (ok_image)
  //{
    sendbyte_psp(252); //init for data
    sendbyte_psp(timer1);
    sendbyte_psp(timer2);
    sendbyte_psp(xx1);
    sendbyte_psp(xx2);
    sendbyte_psp(cordinate[0]); //x1
    sendbyte_psp(cordinate[2]); //x2
    sendbyte_psp(cordinate[1]); //y1
    sendbyte_psp(cordinate[3]); //y2

    for(i=0;i<4;i++) cordinate[i]=0;
    //ok_image=0;
  //}
 }


}