control_motor2.c

Embedded camera control program.

#device  PIC16F877 *=16 ADC=10
#include <16F877.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#fuses hs, nowdt, noprotect, put, nolvp, brownout
#use delay(clock=20000000)
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7)
#use I2C(slave,sda=PIN_C4,scl=PIN_C3,address=0xe0,NOFORCE_SW)
#define kp 1.87
#define ki 0.3
//float ki,kp;
//char s[10];
int set_value1=15;
int set_value2=15;
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;
void main()
{
  // gets(s);
  // Kp=atof(s);
  // printf("%f ",Kp);
  // gets(s);
   //Ki=atof(s);
  // printf("%f ",Ki);

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


 while(1)
 {
  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);
 }

}