camera 5rd.c

Embedded camera control program.

// Connecting with camera for reading and calculating the errors
// Chu y khong duoc dung ngat
// Cach doc camera 1, wait to the end of each packet
// Doc hai hang duoi

#device PIC18F458 *=16 ADC=10
#include "18F458.h"
#use delay(clock=20000000)
#fuses NOWDT,WDT128,HS, NOPROTECT, NOOSCSEN, BROWNOUT, BORV20, PUT, NOCPD, STVREN, NODEBUG, NOLVP, NOWRT, NOWRTD, NOWRTB, NOCPB, NOWRTC, NOEBTR, NOEBTRB
#use rs232(baud=115200,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8,DISABLE_INTS)
#zero_ram

#include <math.h>
//#include <string.h>
#include <stdlib.h>

#use fast_io(D)
#use fast_io(E)
#use fast_io(A)

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

#define Rmin 35 //30//30
#define Rmax 80 //65//81
#define Gmin 35 //20//35
#define Gmax 90 //65//85
#define Bmin 70 //20//73
#define Bmax 120 //65//110

#define alpha_x 3633.33    // =23/6 he so qui chuyen phuong x (1/m)
#define alpha_y 9333.33    // =56/6 he so qui chuyen phuong y (1/m)

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];
unsigned int data_binary[3][10];  // Doc 1 hang cua anh binary
float e2=0,e3=0;

// debug vars
float temp1;
int h_win;

// Calc e23 vars
signed int j,dau_e3=0;
unsigned int mask;
int t1,t8,t24;
int x1_1=0,x1_2=0,x8_1=0,x8_2=0,x24_1=0,x24_2=0;
float delta_t=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()
{
 char data=0;
 int col_i=0,row_i=0;

 data=getc();
 while (data!=13) data=getc();  // Cho den khi nhan ki tu xuong dong
 data=getc();  //0xaa
 //if (data==0xaa) output_b(0x08);
 data=getc();
 while(data!=0xaa)
 {
   if (col_i<10)
   {
     if (row_i==1)  data_binary[0][col_i]=data;  // row 1
     if (row_i==8)  data_binary[1][col_i] =data; // row 8
     if (row_i==24) data_binary[2][col_i]=data;  // row 24
   }
   else
   {
     row_i++;
     col_i=0;
     if (row_i==1)  data_binary[0][0]=data;  // row 1
     if (row_i==8)  data_binary[1][0] =data; // row 8
     if (row_i==24) data_binary[2][0]=data;  // row 24
   }

   data=getc();
   col_i++;
 }

 data=getc();
 if (data=0xaa) { output_b(0x0A);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 1
 i=0;j=7; mask=0x80;
 while ( (data_binary[0][i]==0) && (i<10) ) i++;
 if (i==10) t1=0;
 else    // if (i<10)
 {
  while ( (!(data_binary[0][i] & mask)) && (j>=0) ) {j--; mask >>= 1;}
  x1_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;}
  x1_2 = i*8 + 8 - j;

  t1 = x1_2 - x1_1;
 }

// Xu ly hang 8
 i=0;j=7; mask=0x80;
 while ( (data_binary[1][i]==0) && (i<10) ) i++;
 if (i==10) t8=0;
 else    // if (i<10)
 {
  while ( (!(data_binary[1][i] & mask)) && (j>=0) ) {j--; mask >>= 1;}
  x8_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;}
  x8_2 = i*8 + 8 - j;

  t8 = x8_2 - x8_1;
 }

// Xu ly hang 24
 i=0;j=7; mask=0x80;
 while ( (data_binary[2][i]==0) && (i<10) ) i++;
 if (i==10) t24=0;
 else    // if (i<10)
 {
  while ( (!(data_binary[2][i] & mask)) && (j>=0) ) {j--; mask >>= 1;}
  x24_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;}
  x24_2 = i*8 + 8 - j;

  t24 = x24_2 - x24_1;
 }

 if (t8!=0) delta_t = t8 + 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-x8_2)-(x8_1-1))<0 ) dau_e3=0; // e3<0
   }
   else // Case: line is vertical
   {
     if (cordinate[0]<3)
     {
       delta_t=0;
       if (t24<t1)  dau_e3=0;/*e3<0 */
     }
     else
     {
       if (cordinate[2]>78) { delta_t=0; if (t24>t1)  dau_e3=0;/*e3<0 */ }
       else  // Case: Normal
       {
         if ( ((cordinate[2]-x1_2)-(x1_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.9*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);
}

//-----------------------------------------------------------------------------
// 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(15);

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

  delay_us(20);
}
//-----------------------------------------------------------------------------
void main()
{
 byte* ptr1,ptr2;
 int timer1=0;

 set_tris_d(0x00);
 set_tris_e(0x00);
 set_tris_a(0xFF);

 output_b(0x00);    // 2 led sang 11

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

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

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

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

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

 output_b(0x02);   // 01

 printf("\r");
 get_string(s,4);
 getc();           //Omit ":"

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


  // White balance on
 printf("CR 18 44 \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);

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

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

 output_b(0x04);   // 10

 cam_ready = 0;

 printf("TC %u %u %u %u %u %u\r",Rmin,Rmax,Gmin,Gmax,Bmin,Bmax);
 get_string(s,4); // Get ACK
 if (strncmp(s,ack,3)==0) cam_ready = true;

 delay_ms(1);

 while (!cam_ready)
 {
   printf("TC %u %u %u %u %u %u\r",Rmin,Rmax,Gmin,Gmax,Bmin,Bmax);
   get_string(s,4); // Get ACK
   if (strncmp(s,ack,3)==0) cam_ready = true;
 }

 delay_ms(120000); // Wait a moment
 output_b(0x00);  // Ok, All is ready 11

 while ((cordinate[0]==0) && (cordinate[1]==0) && (cordinate[2]==0) && (cordinate[3]==0))
 {
   get_camera(); // Omit some null packets
 }

 while (1)
 {
   set_timer1(0);
   output_b(0x02); // tat het 01

   while (INPUT(PIN_A1)) ;   // Khi bang 1 thi cho o day

   get_camera();
   output_b(0x04); // Sang het

   if (ok_image)
   {
     ok_image=0;
     calc_error();
     sendbyte_psp(252);  // Kiem tra data nhan co dung khong
     ptr1 = (byte*) &e2;  // Gui e2
     for (i=0;i<4;i++)
     {
       sendbyte_psp(*ptr1);
       ptr1++;
     }
     delay_us(100);
     ptr2 = (byte*) &e3;  // Gui e3
     for (i=0;i<4;i++)
     {
       sendbyte_psp(*ptr2);
       ptr2++;
     }

     // Nhan data de ve do thi
     sendbyte_psp(x8_1); //x1'
     sendbyte_psp(x8_2); //x2'
     sendbyte_psp(cordinate[0]); //x1
     sendbyte_psp(cordinate[1]); //y1
     sendbyte_psp(cordinate[2]); //x2
     sendbyte_psp(cordinate[3]); //y2
     sendbyte_psp(timer1); //tong thoi gian
   }
   delay_us(10);
   temp1= (float)(0.0016*get_timer1());
   timer1 = (int)(temp1);
 }

}