nhanpspv1.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=0;
int set_value2=0;
int16 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[3];
int ip=0;
boolean Rev_OK=0;

//----------------------
#INT_PSP
void psp_isr()
{
  int rev_byte=0;
 while(!psp_input_full()) ;   // Wait data
 if ( !psp_overflow() )
 {
  rev_byte = psp_data;
  if (ip==0) {Rev_OK=0;}
  frame1[ip] = rev_byte;
  ip++;
  if (ip==2)
  {
    ip=0;
    Rev_OK = 1;
    set_value1=frame1[1];
    set_value2=frame1[2];    
  }
 }
}

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

void main()
{
 char s[10];
 int j=0;

 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<3;j++) {frame1[j]=0;}
 set_value1=0;
 set_value2=0; 
 enable_interrupts(INT_PSP);
 enable_interrupts(GLOBAL);

 delay_ms(10);

 while (1)
 {
   while (!Rev_ok) ;
   Rev_ok = False;

    delay_ms(10);
   pulse_1 = get_timer1();
   pulse_2 = get_timer0();
   printf("P1=%lu  ",pulse_1);
   printf("P2=%lu  ",pulse_2);

  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);
 }
    }