CCS C İLE ENCODERLİ DC MOTOR KONTROLÜ

[ilker58]

Merhaba arkadaşlar,

benim problemim encoderli dc  motorla konum kontrolü yapmak 60 derece ye kadar motorun dönmesi gerekiyor.(5 10 15 20 25 30 35
40 45 50 55 60 kademeli) ama motor adc den gelen değere göre 5 derece dönmesi gerekiyorsa 0 konumuna geri gelmelidir. Problem, encoderden gelen değeri pice nasıl sokucam ve pid kontrolü nasıl gerçekleştireceğim? Bu konu ile ilgili elinizde döküman varsa paylaşabilir misiniz?

saygılarımla

[ercanaslan]

74hc74 ve tlp521 kullanılarak yapılmış bir örnek görmüştüm.

[ilker58]

eğer örnek elinizde mevcutsa paylaşabilir misiniz?

[necati]

Kontrol etmek istediginiz motor nedir
Redüktorlü dc motor + pot
Redüktorlü dc motor + encoder mi
Bir resim veya motor un tipi ni bilmek gerek

Açıyı potla mı değiştireceksi **********
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
http://www.4shared.com/rar/c5DiMRbi/RC_PID_SERVO_POSITION_CONTROLL.html/////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "18f252.h"
#DEVICE ADC=10
#fuses H4,NOPROTECT,NOWDT,NOBROWNOUT,NOLVP
#use Delay(Clock=40000000)
#use RS232(Baud=115200,Xmit=PIN_C6,Rcv=PIN_C7,brgh1ok)
#include "math.h"
#include <stdlib.h>

#PRIORITY rb, ext

#define GAIN 5.5455  //Determined by (R1+Rf)/R1 on Board
   //eg. (10K+2.2K)/2.2K = 5.545454...
#define RESISTOR_VALUE  .1

signed int32 enc;    //
int state, PBval;
long  t1val;
signed int32  pulsewB, pulsewB_old;
float set_point, plant, plant_old;
int   flag[2];
int   calflag;
long  oncount, offcount;
float pulsetime;
float pulsetimeold;
int   timeout_error;
float pulsecent;
float pulsemax;
float pulsemin;
float deadzone;   //AN0 volts * .00002, max = .1ms
float proportional_gain, integral_gain, derivative_gain;
float proportional, integral, derivative;
float error, int_lockout, accumulator;
float spd;
char sentence[20];
signed int32 enc_buf[2];
int32 enc_cpr;
int motion_flag;
long sample;
float span;
unsigned long   spcnt;   //same pulse count
unsigned long dpcnt;    //different pulse count
char dir_str[1];
signed int32 eeprom_temp_val32;
int eeprom_temp_val8[4];
long a_position;
float current;

//-----   Function Prototypes   -------------------
void isr_PB(void);
void isr_bint0(void);
void Blink_LED(int blinks);
void mot_speed(float mot_speed);
void MF(void);
void MR(void);
void MS(void);
void MB(void);
void Calibrate_sticks(void);
void store_calibration_values(void);
void read_calibration_values(void);

#INT_RB
void isr_PB(void)
{
   PBval = input_B() & 0xc0;     //Get PORTB state and mask lower bits

   switch(state)
   {
      case 0:           //was 00
         if(PBval == 0x40)  // if now 01, increment
         {
            enc++;
            state=1;
         }
         if(PBval == 0x80)  // if now 10, decrement
         {
            enc--;
            state=3;
         }
         break;

      case 1:           //was 01
         if(PBval == 0xc0)  //if now 11, increment
         {
            enc++;
            state=2;
         }
         if(PBval == 0x00)  //if now 00, decrement
         {
            enc--;
            state=0;
         }
         break;

      case 2:           //was 11
         if(PBval == 0x80)  //if now 10, increment
         {
            enc++;
            state=3;
         }
         if(PBval == 0x40)  //if now 01, decrement
         {
            enc--;
            state=1;
         }
         break;

      case 3:           //was 10
         if(PBval == 0x00)  //if now 00, increment
         {
            enc++;
            state=0;
         }
         if(PBval == 0xc0)  //if now 11, decrement
         {
            enc--;
            state=2;
         }
         break;

      default:
         state = 0;
         break;
   }
}

#INT_EXT
void isr_bint0(void)
{
   t1val = get_timer1();         //Get Timer1 value
   //Test for change on PB0
   if(!flag[0]) //flag0 is 0
   {
      oncount = t1val;
      flag[0] = 1;
      #asm
      bcf   0x0FF1, 6   //clear INTEDG0 in INTCON2 (int on falling edge)
      #endasm
   }
   else
   {
      offcount = t1val;
      pulsewB = offcount - oncount;
      flag[0] = 0;               //reset flag
      #asm
      bsf   0x0FF1, 6   //set INTEDG0 in INTCON2 (int on rising edge)
      #endasm
   }
}

void Blink_LED(int blinks)
{
   int i;
   for(i=0;i<blinks;i++)
   {
      output_low(PIN_C0);  //Turn ON LED
      delay_ms(300);
      output_high(PIN_C0);  //Turn OFF LED
      delay_ms(300);
   }
}

void mot_speed(float mot_speed)
{
   float mot_speed_10b;

   mot_speed_10b = (mot_speed / .09775171);  //convert to 10 bit
//     mot_speed_10b *= 10.24;
   if(mot_speed_10b > 1023.0)
      mot_speed_10b = 1023.0;
   if(mot_speed_10b < 0.0)
      mot_speed_10b = 0.0;

   set_pwm1_duty((unsigned long)mot_speed_10b);
}

//MOTOR 1 DIRECTION CONTROL FUNCTIONS
void MF(void)
{
   output_low(PIN_C1);
   output_high(PIN_C3);
}
void MR(void)
{
   output_high(PIN_C1);
   output_low(PIN_C3);
}
void MS(void)
{
   output_low(PIN_C1);
   output_low(PIN_C3);
}
void MB(void)
{
   output_high(PIN_C1);
   output_high(PIN_C3);
}

void Calibrate_sticks(void)
{
   char tempbuf[10];
   int i;

   spd = 0.0;
   mot_speed(spd);
   MS();
   dir_str[0] = 's';

   set_adc_channel(3);
   delay_us(20);
   span = (float)Read_ADC() * .02;

   set_adc_channel(4);
   delay_us(20);
   a_position = Read_ADC();

   printf("\r\n\r\nCenter Sticks and press Enter('\\r').  \r\n");
   delay_ms(200);
   Blink_LED(2);     //Entering Programming Mode
   delay_ms(200);

   while(input(PIN_B3))
   {
      //pulsecent = (float)pulsewB/5000;
      pulsecent = (float)pulsewB;
   }
   pulsemin = pulsecent;
   pulsemax = pulsecent;
   printf("Move sticks to minimum and maximum values and press button.\r\n");
   delay_ms(200);
   Blink_LED(2);     //Entering Programming Mode
   delay_ms(200);
   while(input(PIN_B3))
   {
      //pulsetime = (float)pulsewB/5000;
      pulsetime = (float)pulsewB;

      if(pulsetime <= pulsemin)
         pulsemin = pulsetime;
      if(pulsetime >= pulsemax)
         pulsemax = pulsetime;
   }

//  pulsemin -= pulsecent;
//  pulsemin *= span;
//  pulsemax -= pulsecent;
  // pulsemax *= span;

//  set_point = pulsecent;
//  pulsewB_old = pulsecent;
//  plant = set_point;
//  enc = (signed int16)plant;
   enc = 0;
   printf(" New Settings\r\n");
   printf("Min  %5.3f \r\n", pulsemin);
   printf("Cent %5.3f \r\n", pulsecent);
   printf("Max  %5.3f  \r\n", pulsemax);
   printf("press enter when finished.\r\n");
   delay_ms(200);
   Blink_LED(2);
   delay_ms(200);
   while(input(PIN_B3));
   delay_ms(200);
   Blink_LED(3);

   store_calibration_values();
}

void store_calibration_values(void)
{
   printf("Storing calibration values. \r\n");

   eeprom_temp_val32 = (signed int32)pulsemin;
   eeprom_temp_val8[0] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[1] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[2] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[3] = 0x000000ff & eeprom_temp_val32;
   write_eeprom(0, eeprom_temp_val8[0]);
   write_eeprom(1, eeprom_temp_val8[1]);
   write_eeprom(2, eeprom_temp_val8[2]);
   write_eeprom(3, eeprom_temp_val8[3]);

   printf(" Address 0 = 0x%2X  \r\n ", eeprom_temp_val8[0]);
   printf(" Address 1 = 0x%2X  \r\n ", eeprom_temp_val8[1]);
   printf(" Address 2 = 0x%2X  \r\n ", eeprom_temp_val8[2]);
   printf(" Address 3 = 0x%2X  \r\n ", eeprom_temp_val8[3]);
   printf(" pulsemin = %6.2f \r\n ", pulsemin);

   eeprom_temp_val32 = (signed int32)pulsecent;
   eeprom_temp_val8[0] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[1] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[2] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[3] = 0x000000ff & eeprom_temp_val32;
   write_eeprom(4, eeprom_temp_val8[0]);
   write_eeprom(5, eeprom_temp_val8[1]);
   write_eeprom(6, eeprom_temp_val8[2]);
   write_eeprom(7, eeprom_temp_val8[3]);

   printf(" Address 4 = 0x%2X  \r\n ", eeprom_temp_val8[0]);
   printf(" Address 5 = 0x%2X  \r\n ", eeprom_temp_val8[1]);
   printf(" Address 6 = 0x%2X  \r\n ", eeprom_temp_val8[2]);
   printf(" Address 7 = 0x%2X  \r\n ", eeprom_temp_val8[3]);
   printf(" pulsecent = %6.2f \r\n ", pulsecent);

   eeprom_temp_val32 = (signed int32)pulsemax;
   eeprom_temp_val8[0] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[1] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[2] = 0x000000ff & eeprom_temp_val32;
   eeprom_temp_val32 >>= 8;
   eeprom_temp_val8[3] = 0x000000ff & eeprom_temp_val32;
   write_eeprom(8, eeprom_temp_val8[0]);
   write_eeprom(9, eeprom_temp_val8[1]);
   write_eeprom(10, eeprom_temp_val8[2]);
   write_eeprom(11, eeprom_temp_val8[3]);

   printf(" Address 8 = 0x%2X  \r\n ", eeprom_temp_val8[0]);
   printf(" Address 9 = 0x%2X  \r\n ", eeprom_temp_val8[1]);
   printf(" Address 10 = 0x%2X  \r\n ", eeprom_temp_val8[2]);
   printf(" Address 11 = 0x%2X  \r\n ", eeprom_temp_val8[3]);
   printf(" pulsemax = %6.2f \r\n ", pulsemax);
}

void read_calibration_values(void)
{
   printf("Reading calibration values. \r\n");

   eeprom_temp_val32 = 0;                    //zero the 32bit register
   eeprom_temp_val8[3] = read_eeprom(3);     //read the eeprom address
   eeprom_temp_val32 += eeprom_temp_val8[3]; //add it to the 32bit register
   eeprom_temp_val32 <<= 8;                  //shift 32bit register left 8 bits
   eeprom_temp_val8[2] = read_eeprom(2);
   eeprom_temp_val32 += eeprom_temp_val8[2];
   eeprom_temp_val32 <<= 8;
   eeprom_temp_val8[1] = read_eeprom(1);
   eeprom_temp_val32 += eeprom_temp_val8[1];
   eeprom_temp_val32 <<= 8;
   eeprom_temp_val8[0] = read_eeprom(0);
   eeprom_temp_val32 += eeprom_temp_val8[0];

   pulsemin = (float)eeprom_temp_val32;

   printf(" Address 0 = 0x%2X  \r\n ", eeprom_temp_val8[0]);
   printf(" Address 1 = 0x%2X  \r\n ", eeprom_temp_val8[1]);
   printf(" Address 2 = 0x%2X  \r\n ", eeprom_temp_val8[2]);
   printf(" Address 3 = 0x%2X  \r\n ", eeprom_temp_val8[3]);
   printf(" pulsemin = %6.2f \r\n ", pulsemin);

   eeprom_temp_val32 = 0;
   eeprom_temp_val8[3] = read_eeprom(7);
   eeprom_temp_val32 += eeprom_temp_val8[3];
   eeprom_temp_val32 <<= 8;
   eeprom_temp_val8[2] = read_eeprom(6);
   eeprom_temp_val32 += eeprom_temp_val8[2];
   eeprom_temp_val32 <<= 8;
   eeprom_temp_val8[1] = read_eeprom(5);
   eeprom_temp_val32 += eeprom_temp_val8[1];
   eeprom_temp_val32 <<= 8;
   eeprom_temp_val8[0] = read_eeprom(4);
   eeprom_temp_val32 += eeprom_temp_val8[0];

   pulsecent = (float)eeprom_temp_val32;

   printf(" Address 4 = 0x%2X  \r\n ", eeprom_temp_val8[0]);
   printf(" Address 5 = 0x%2X  \r\n ", eeprom_temp_val8[1]);
   printf(" Address 6 = 0x%2X  \r\n ", eeprom_temp_val8[2]);
   printf(" Address 7 = 0x%2X  \r\n ", eeprom_temp_val8[3]);
   printf(" pulsecent = %6.2f \r\n ", pulsecent);

   eeprom_temp_val32 = 0;                    //zero the 32bit register
   eeprom_temp_val8[3] = read_eeprom(11);     //read the eeprom address
   eeprom_temp_val32 += eeprom_temp_val8[3]; //add it to the 32bit register
   eeprom_temp_val32 <<= 8;                  //shift 32bit register left 8 bits
   eeprom_temp_val8[2] = read_eeprom(10);
   eeprom_temp_val32 += eeprom_temp_val8[2];
   eeprom_temp_val32 <<= 8;
   eeprom_temp_val8[1] = read_eeprom(9);
   eeprom_temp_val32 += eeprom_temp_val8[1];
   eeprom_temp_val32 <<= 8;
   eeprom_temp_val8[0] = read_eeprom(8);
   eeprom_temp_val32 += eeprom_temp_val8[0];

   pulsemax = (float)eeprom_temp_val32;

   printf(" Address 8 = 0x%2X  \r\n ", eeprom_temp_val8[0]);
   printf(" Address 9 = 0x%2X  \r\n ", eeprom_temp_val8[1]);
   printf(" Address 10 = 0x%2X  \r\n ", eeprom_temp_val8[2]);
   printf(" Address 11 = 0x%2X  \r\n ", eeprom_temp_val8[3]);
   printf(" pulsemax = %6.2f \r\n ", pulsemax);

}

void main()
{
   spcnt=0;
   pulsewB = 0;
   accumulator = 0;          //clear the accumulated error

   setup_adc_ports(ALL_ANALOG);
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_spi(FALSE);
   setup_wdt(WDT_OFF);
   setup_spi(FALSE);
   setup_wdt(WDT_OFF);
   setup_ccp1(CCP_PWM);
   set_pwm1_duty(0);
   setup_timer_0(RTCC_DIV_4);
   setup_timer_1(T1_INTERNAL|T1_DIV_BY_2);   // setup interrupts
   setup_timer_2(T2_DIV_BY_1, 255, 1);
   setup_timer_3(T3_DISABLED|T3_DIV_BY_1);
   PORT_B_PULLUPS(TRUE);
   #asm
   bsf   0x0FF1, 6   //set INTEDG0 in INTCON2 (int on rising edge)
   #endasm

   set_tris_a(0xff);    //RA0 is the potentiometer
   set_tris_b(0xff);    //PB0 = Servo Pulse Input
                        //PB1 = NC
                        //PB2 = NC
                        //PB3 = Calibrate Pushbutton
                        //PB4 = NC
                        //PB5 = NC
                        //PB6 = ENCA
                        //PB7 = ENCB

   output_c(0x00);      //Start with LED off,
                        //IN1 is LOW
                        //EN is LOW
                        //IN2 is LOW
                        //TX is LOW

   set_tris_c(0xa0);    //PC0 used for LED/Debug
                        //PC1 = PHA
                        //PC2 = EN1
                        //PC3 = EN2
                        //PC4 = Not Connected
                        //PC6 is TX, PC7 is RX

  PBval = input_B() & 0xc0;     //Get PORTB state and mask lower bits
   // Get initial encoder state
   if((PBval & 0x00)) //If PB6 is low and PB7 is low
   {
      state = 0;
   }
   if((PBval & 0x40)) //If PB6 is high and PB7 is low
   {
      state = 1;
   }
   if((PBval & 0xc0)) //If PB6 is high and PB7 is high
   {
      state = 2;
   }
   if((PBval & 0x80)) //If PB6 is low and PB7 is high
   {
      state = 3;
   }
   enable_interrupts(INT_RB);                //Enable PORTB change Interupt
   enable_interrupts(INT_EXT);               //Enable External interrupt PB0
   enable_interrupts(GLOBAL);

   delay_ms(20);
   Blink_LED(2);

   enc = 0; //setpoint
   set_point = 0.0;
   plant = 0.0;
   plant_old = 0.0;
   int_lockout = 50.0;
   motion_flag = 0;
   pulsetime = 0.0;
   pulsetimeold = 0.0;
   accumulator = 0.0;
   spd = 0.0;
   spcnt = 0;
   dpcnt = 0;

   set_timer0(0x9e58);        // 65536-((.010ms/(4/40000000))/4) = 40536
//   enable_interrupts(INT_TIMER0);

      set_adc_channel(3);
      delay_us(20);
      span = (float)Read_ADC() * .02;

   pulsecent = 7500;   //Default values
   pulsemin = 5400;
   pulsemax = 96000;

   read_calibration_values();
   set_point = ((float)pulsewB - pulsecent) * span;
   pulsewB_old = pulsewB;
   plant = set_point;
   enc = (signed int16)plant;

   while(1)
   {
   #asm
wait_TMR0F:
   btfss 0xff2, 2    //INTCON, TMR0IF
   goto  wait_TMR0F
   #endasm
   set_timer0(0x9e58);        // 65536-((.010ms/(4/40000000))/4) = 40536
   #asm
   bcf   0xff2, 2    //Clear the TMR0IF
   #endasm

   if(!INPUT(PIN_B3))
   {
      spd = 0.0;
      mot_speed(spd);
      MS();
      dir_str[0] = 's';
      read_calibration_values();
      Calibrate_sticks();
      read_calibration_values();
   }
      output_high(PIN_C0);  //Turn OFF LED
      output_low(PIN_C0);  //Turn ON LED

      set_adc_channel(0);
      delay_us(20);
      proportional_gain = (float)Read_ADC();
      proportional_gain *= .0001;

      set_adc_channel(1);
      delay_us(20);
      integral_gain = (float)Read_ADC();
      integral_gain *= .00001;

      set_adc_channel(2);
      delay_us(20);
      derivative_gain = (float)Read_ADC();
      derivative_gain *= .0001;

      set_adc_channel(3);
      delay_us(20);
      span = (float)Read_ADC();
      span *= .02;

      set_adc_channel(4);
      delay_us(20);
      current = Read_ADC();
      current *= .0048828125; //voltage
      current /= RESISTOR_VALUE;           //Resistor value
      current /= GAIN;        //OpAmp Gain

      if(pulsewB == pulsewB_old)
      {
         spcnt++;
      }
      else
      {
         spcnt = 0;
      }

      set_point = ((((float)pulsewB + (float)pulsewB_old)/2.0) - pulsecent) * span;
         // Average current and last value, subtract the center value and multiply by
         //   the span.
      plant = (float)enc;
      pulsewB_old = pulsewB;

      if(spcnt < 25)
      {
         if((set_point <= plant + (10.0 * span)) && (set_point >= plant - (10.0 * span)))
         {
            MS();
            mot_speed(0.0);
            dir_str[0] = 's';
            motion_flag = 0;
            int_lockout = 50.0;
            motion_flag = 0;
            pulsetime = 0.0;
            pulsetimeold = 0.0;
            accumulator = 0.0;
            spd = 0.0;
            integral = 0.0;
            proportional = 0.0;
            derivative = 0.0;
            spcnt = 0;
         }
         else
         {
            if(set_point > plant)
            {
               if(dir_str[0] == 'r')
               {
                  MS();
                  delay_us(100);
               }
               MF();
               dir_str[0] = 'f';
               motion_flag = 1;
            }
            if(set_point < plant)
            {
               if(dir_str[0] == 'f')
               {
                  MS();   //motor stop
                  motion_flag = 0;
                  delay_us(100);
               }
               MR();
               dir_str[0] = 'r';
               motion_flag = 1;
            }
         }

         error = set_point - plant;   //set point - plant
         error = abs(error);

            //calculate the proportional component
         proportional = error * proportional_gain;

      /*   if(proportional >= saturation)
            proportional = saturation;
         if(proportional < -saturation)
            proportional = -saturation;
      */
            //calculate the integral component
    //     if(error <= int_lockout)
    //     {
            accumulator += error * integral_gain;
            integral = accumulator;
            if(integral >= 50.0)
            {
               integral = 50.0;
               accumulator = 50.0;
            }
   //      }

            //calculate the derivative component
         derivative = plant - plant_old;   //plant - last plant
         derivative *= derivative_gain;
         plant_old = plant;

         spd = proportional + integral - derivative; // Add PID components

         if(spd < 5.0)
         {
            spd = 0.0;
            mot_speed(spd);
            MS();
            dir_str[0] = 's';
         }
         if(spd > 100.0)
            spd = 100.0;

         mot_speed(spd);

         printf("%4.1f %6.1f %6.1f %6.2f %6.2f %6.2f %5.4f %5.4f %5.4f %c %4.2f %4.2f \r\n",
            spd, set_point, plant, proportional,integral, derivative,
            proportional_gain, integral_gain, derivative_gain, dir_str[0], span, current);

      }
      else
      {
         MS();
         mot_speed(0.0);
         printf("Deadstick!\r\n");
      }
   }
   disable_interrupts(INT_TIMER0);
}

[ilker58]

motor linki http://www.pololu.com/catalog/product/1447  motor hem reduktorlü hem de encoderli ayrıca potansıyometreden gelen değerlere göre 5 10 15 20 25 30 35 40 45 50 55 60 dereceleri dönmesi gerekiyor mesela 5 derece dönecekse motor 5 derece dönüp 0 konumuna geri gelmeli.

İlginiz için teşekkürler

[ercanaslan]

bu akşam şemasını yollamaya çalışırım

[ilker58]

çok teşekkür ederim.

[ilker58]

necati, ilginiz için çok teşekkürler fakat download linki çalışmıyor...

[ercanaslan]

Biraz geç oldu kusura bakma
[IMG]http://img515.imageshack.us/img515/4395/encoderzm.png[/img]

[ilker58]

ercan bey çok teşekkürler

[z]

error = set_point - plant;   //set point - plant
         error = abs(error);

İkinci satırda hatanın mutlak değerini almak işi mahvetmiş.