nhanhung.c

Embedded camera control program.

#device  PIC16F877 *=16 ADC=10
#include <16f877.h>
#include <math.h>
#include <stdlib.h>

#fuses hs,nowdt,noprotect,put,nolvp,brownout
#use delay(clock=20000000) 	// 20 MhZ
#use rs232(baud=38400,parity=N,xmit=PIN_C6, rcv=PIN_C7)

#use fast_io(D)
#use fast_io(E)
#define kp 1.87
#define ki 0.3

int set_value1=10;
int set_value2=10;
int i=0;
int16 pulse_1,pulse_2;
int error_1,error_2;
float error_1p,error_2p;
float error_1pi,error_2pi;
signed int16 duty_1=0;
signed int16 duty_2=0;
int error_b1=0;
int error_b2=0;
int frame1[5];
int ip=0;
float k;
char s[5];
byte* ptr;
float e3=0;
boolean Rev_OK=0;
int t;
//----------------------
#INT_PSP
void psp_isr()
{
  //set_value1=15;
  //set_value2=15;
 while(!psp_input_full()) ;   // Wait data
 if ( !psp_overflow() )
 {

   //delay_us(1);
   frame1[ip] = psp_data;
   if (frame1[0]==252) ip++;

   if (ip==5)
   {
     ip=0;
     Rev_OK = 1;
     ptr = (byte*) &e3; // Khoi phuc e3
     for (i=1;i<=4;i++) {*ptr = frame1[i]; ptr++;}
   // printf("%f   ",e3);
   }
 }

}

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

void main()
{
 int j=0;
   gets(s);
  K=atof(s);
  printf("%f ",K);
  //delay_ms(5000);
   delay_ms(5000);
 setup_adc_ports( NO_ANALOGS);
 setup_psp(PSP_ENABLED);

 set_tris_e(0xff); // Port E Input
 set_tris_d(0xff);

   setup_TIMER_0(RTCC_EXT_H_TO_L);
   #asm
   BSF    0x81,3 //Prescaler assignment to WDT, nen de y chu 0x o truoc chu 81, co vay compiler moi tim dung thanh ghi can tim
   #endasm
   setup_TIMER_1(T1_EXTERNAL);
   setup_CCP1(CCP_PWM);
   setup_CCP2(CCP_PWM);
   setup_TIMER_2(T2_DIV_BY_1, 255, 1);

   output_high(PIN_A2);
   output_high(PIN_A5);
   output_high(PIN_B2);
   output_high(PIN_B5);
      set_timer0(0);
   set_timer1(0);



 for (j=0;j<5;j++) {frame1[j]=0;}

 enable_interrupts(INT_PSP);
 enable_interrupts(GLOBAL);

 delay_ms(10);

 while (1)
 {
     delay_ms(10);
   pulse_1 = get_timer1();//banh trai
   pulse_2 = get_timer0();//phai

   //printf("T=%lu  ",pulse_1);
  // printf("P=%lu  \r",pulse_2);


   if (Rev_Ok)
   {// printf("X1=%u\r"   ,frame1[1]);
     //printf("X2=%u\r"    ,frame1[2]);
     //printf("Y1=%u\r"    ,frame1[3]);
     //printf("Y2=%u\r"    ,frame1[4]);
      e3*=180;
      e3/=3.14;
     printf("e3=%f\r",e3);
     set_value1=10;
     set_value2=10;

      t=(int)(e3*k);
      set_value2-=t;
      set_value1+=t;
     Rev_Ok = False;

   printf("S1=%u\r  ",set_value1);
   printf("S2=%u\r  ",set_value2);
   }

  if (set_value1 > pulse_1)
  {
   error_1 = set_value1 - pulse_1;
   error_1p  = error_1*kp;
   error_1pi = error_1p + error_b1*ki;
   duty_1+= (int16)(error_1pi);
   duty_1 = (duty_1>1023?1023:duty_1);
  }
else
{
   error_1 = pulse_1-set_value1;
   error_1p  = error_1*kp;
   error_1pi = error_1p + error_b1*ki;
   duty_1 -= (int16)(error_1pi);
   duty_1 = (duty_1<60?60:duty_1);
}
   error_b1 = error_1;
   set_PWM1_duty(duty_1);

  If (set_value2 > pulse_2)
  {
   error_2 = set_value2 - pulse_2;
   error_2p  = error_2*kp;
   error_2pi = error_2p + error_b2*ki;
   duty_2+= (int16)(error_2pi);
   duty_2 = (duty_2>1023?1023:duty_2);
  }
else
{
   error_2 = pulse_2-set_value2;
   error_2p  = error_2*kp;
   error_2pi = error_2p + error_b2*ki;
   duty_2 -= (int16)(error_2pi);
   duty_2 = (duty_2<60?60:duty_2);
}
   error_b2 = error_2;
   set_PWM2_duty(duty_2);

//   printf("E1=%u   ",error_1);

    // printf("E2=%u   ",error_2);

set_timer0(0);
set_timer1(0);




 }

}