thiết kế chế tạo robot dò đường.

**doken** · 09-19-2014, 04:35 AM

Gửi bởi trantu93

bác làm rồi giải thích thuật toán cụ thể không.em có đọc pid nhưng ko biết áp dụng kiểu gì cả.thanhks bác trước

Bạn xem #10 nhé.

Gửi bởi thiet_kt

ngoài lề tí nhé:
mta_cdt bác học hệ quân sự hay dân sự?
khóa mấy ạ?

Mình học khóa 5 dân sự.

**mituot** · 09-19-2014, 04:41 AM

vay bác sinh năm 1987 à?
em học hệ quân sự(gần năm cuối.hihi)

**lavie079** · 09-20-2014, 07:55 AM

Gửi bởi mta_cdt

Bạn xem #10 nhé.

Mình học khóa 5 dân sự.

em thử lưu biến rồi việc vào cua đã mượ mà hơn nhưng khi hết đoạn cua vào đường thẳng thì xe bị cua quá đà.có cách nào khắc phục nó không ạ!nếu có thể bác giải thích hộ toán hoặc cho em xin 1 đoan code nếu đc trên codevisionavr thì cảm tạ bác niều lắm

**nguyenanhthuoc** · 09-20-2014, 12:30 PM

Gửi bởi trantu93

em thử lưu biến rồi việc vào cua đã mượ mà hơn nhưng khi hết đoạn cua vào đường thẳng thì xe bị cua quá đà.có cách nào khắc phục nó không ạ!nếu có thể bác giải thích hộ toán hoặc cho em xin 1 đoan code nếu đc trên codevisionavr thì cảm tạ bác niều lắm

Đây cũng là một kiểu PID viết hơi khác 1 chút.

Mã:

/*****************************************************
This program was produced by the
CodeWizardAVR V2.05.0 Professional
Automatic Program Generator
© Copyright 1998-2010 Pavel Haiduc, HP InfoTech s.r.l.
http://www.hpinfotech.com

Project :
Version :
Date    : 5/2/2014
Author  : NeVaDa
Company :
Comments:


Chip type               : ATmega16
Program type            : Application
AVR Core Clock frequency: 8.000000 MHz
Memory model            : Small
External RAM size       : 0
Data Stack size         : 256
*****************************************************/

#include <mega16.h>
#include <stdio.h>
#include <delay.h>
#include <alcd.h>
#define ADC_VREF_TYPE 0x00
#define chanel 8
//const char dir=0;
  int count=0, step=0, start=0;
  unsigned int adc[8];
  unsigned char status[chanel] = {1,2,4,8,16,32,64,128};
  int level[chanel] = {400,400,400,400,400,400,400,400};
  unsigned char outStatus;
  //float e, old_e=0,e_sum=0;
  float error, old_error, Kp=1.2, Kd=10, Ki=0;                           
  //int u;
  int max_e=200, Mid=900,Max;    
 
unsigned int read_adc(unsigned char adc_input)
{
    ADMUX=adc_input | (ADC_VREF_TYPE & 0xff);
    // Delay needed for the stabilization of the ADC input voltage
    delay_us(200);
    // Start the AD conversion
    ADCSRA|=0x40;
    // Wait for the AD conversion to complete
    while ((ADCSRA & 0x10)==0);
    ADCSRA|=0x10;
    return ADCW;
}
////////////////////////DOC TAT CA CAC KENH VAO CUNG NHO MIN/////////////////
void read_adc_all()
  {
    unsigned char i;
    unsigned int temp;
    outStatus = 0;
    for (i=0; i<chanel; i++)
    {
        temp = read_adc(i);
        if (temp<level[i])
        {
            adc[i]=0;
        }
        else
        {
            adc[i]=temp;
            outStatus |= status[i];          
        }
        delay_us(50);                                
    }          
    if (start ==0)
     {
        if ((adc[1]>0)&&(adc[2]>0)&&(adc[3]>0)&&(adc[4]>0)&&(adc[5]>0)&&(adc[6]>0))
        {
            start=1;
            count++;
        }
     }
    else
        if ((adc[2]==0)||(adc[3]==0)||(adc[4]==0)||(adc[5]==0))start=0;
  }
/////////////////////// TINH VI TRI XE/////////////////////////////////////
int vitrixe()
{  

    char j;
    unsigned int sum1=0, sum2=0;
    float temp; 
    read_adc_all();
    for (j=0; j<chanel;j++)
    {
        sum1=sum1+adc[j]*(j+1);
        sum2=sum2+adc[j];
    }
    if (sum2!=0)
    {
        temp = (float)sum1*100;
        temp = temp/sum2;
    }
    else temp=0;
    return temp-450;
}            

//---------------------------------------------------
int PID_control()
{
    float delta, Udk;
    int error_sum=0;
   
    error =(float)vitrixe();            // Sai so dieu khien
  
   
    if ((error<-350) && (old_error>=0)) {error=-error;} 
   
    delta = (error - old_error);
    old_error = error;                  //Luu gia tri sai so  
    error_sum = error_sum+error; // Thanh phan tich phan
                                     
    // Khong cho thanh phan tich phan vuot qua gia tri max
    if (error_sum <-max_e) error_sum = -max_e;
    if (error_sum > max_e) error_sum = max_e;
   
    Udk = Kp*error + Ki*error_sum + Kd*delta;  //Tin hieu dieu khien

    // Giam sat gia tri dieu khien ko duoc vuot qua ngwong
    if (Udk <-Max) Udk = -Max;
    if (Udk > Max) Udk = Max;
   
    return (int)Udk;
}


void main(void)
{ 
    unsigned char lcd_buffer[16];
    int duong, am;
   // float delta;
    int Udk;
  //unsigned int temp=0;
// Declare your local variables here

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=Out Func4=Out Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=0 State4=0 State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD=0x30;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 125.000 kHz
// Mode: Fast PWM top=ICR1
// OC1A output: Non-Inv.
// OC1B output: Non-Inv.
// Noise Canceler: Off
// Input Capture on Rising Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0xA2;
TCCR1B=0x5B;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x03;
ICR1L=0xFF;
OCR1AH=0x3;
OCR1AL=0xff;
OCR1BH=0x3;
OCR1BL=0xff;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

// USART initialization
// USART disabled
UCSRB=0x00;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// ADC initialization
// ADC Clock frequency: 125.000 kHz
// ADC Voltage Reference: AREF pin
// ADC Auto Trigger Source: Free Running
ADMUX=ADC_VREF_TYPE & 0xff;
ADCSRA=0xA6;
SFIOR&=0x1F;

// SPI initialization
// SPI disabled
SPCR=0x00;

// TWI initialization
// TWI disabled

// Alphanumeric LCD initialization
// Connections specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS - PORTC Bit 7
// RD - PORTC Bit 6
// EN - PORTC Bit 5
// D4 - PORTC Bit 4
// D5 - PORTC Bit 3
// D6 - PORTC Bit 2
// D7 - PORTC Bit 1
// Characters/line: 16
lcd_init(16);

TWCR=0x00;

Max = 1023-Mid;
max_e=Mid/4;

    while (1)
    {
        Udk = PID_control(); 
        if (Udk==Max)
        {
            am = Mid-Udk*10;
            if (am<0) am = 0;  
            duong = Mid+Udk;
        } else if (Udk==-Max)
        {             
            duong = Mid+Udk*10;
            if (duong<0) duong = 0;  
            am = Mid-Udk;
        }    
        else
        {  
            am = Mid - Udk;
            duong = Mid +Udk;
        }

        if((count==4)||(count==8)||(count==13)||(count>13))
        { 
        //delay_ms(100);
           OCR1A=0;
           OCR1B=0;
           count++;
           delay_ms(1000);
        }
        else
        {
            OCR1A=am;
            OCR1B=duong;
        }
         //*  sprintf(lcd_buffer,"Vi tri xe = %d",vitrixe());
             
        sprintf(lcd_buffer,"%d %d %d %d",adc[0],adc[1],adc[2],adc[3]);
        lcd_gotoxy(0,0);
        lcd_puts(lcd_buffer);
        sprintf(lcd_buffer,"%d %d %d %d",adc[4],adc[5],adc[6],adc[7]); 
        //sprintf(lcd_buffer,"Udk = %d",(int)Udk);
        lcd_gotoxy(0,1);
        lcd_puts(lcd_buffer);
      }
}

**seodn** · 09-21-2014, 07:10 PM

Gửi bởi mta_cdt

Đây cũng là một kiểu PID viết hơi khác 1 chút.

Mã:

/*****************************************************
This program was produced by the
CodeWizardAVR V2.05.0 Professional
Automatic Program Generator
© Copyright 1998-2010 Pavel Haiduc, HP InfoTech s.r.l.
http://www.hpinfotech.com

Project :
Version :
Date    : 5/2/2014
Author  : NeVaDa
Company :
Comments:


Chip type               : ATmega16
Program type            : Application
AVR Core Clock frequency: 8.000000 MHz
Memory model            : Small
External RAM size       : 0
Data Stack size         : 256
*****************************************************/

#include <mega16.h>
#include <stdio.h>
#include <delay.h>
#include <alcd.h>
#define ADC_VREF_TYPE 0x00
#define chanel 8
//const char dir=0;
  int count=0, step=0, start=0;
  unsigned int adc[8];
  unsigned char status[chanel] = {1,2,4,8,16,32,64,128};
  int level[chanel] = {400,400,400,400,400,400,400,400};
  unsigned char outStatus;
  //float e, old_e=0,e_sum=0;
  float error, old_error, Kp=1.2, Kd=10, Ki=0;                          
  //int u;
  int max_e=200, Mid=900,Max;   

unsigned int read_adc(unsigned char adc_input)
{
    ADMUX=adc_input | (ADC_VREF_TYPE & 0xff);
    // Delay needed for the stabilization of the ADC input voltage
    delay_us(200);
    // Start the AD conversion
    ADCSRA|=0x40;
    // Wait for the AD conversion to complete
    while ((ADCSRA & 0x10)==0);
    ADCSRA|=0x10;
    return ADCW;
}
////////////////////////DOC TAT CA CAC KENH VAO CUNG NHO MIN/////////////////
void read_adc_all()
  {
    unsigned char i;
    unsigned int temp;
    outStatus = 0;
    for (i=0; i<chanel; i++)
    {
        temp = read_adc(i);
        if (temp<level[i])
        {
            adc[i]=0;
        }
        else
        {
            adc[i]=temp;
            outStatus |= status[i];         
        }
        delay_us(50);                               
    }         
    if (start ==0)
     {
        if ((adc[1]>0)&&(adc[2]>0)&&(adc[3]>0)&&(adc[4]>0)&&(adc[5]>0)&&(adc[6]>0))
        {
            start=1;
            count++;
        }
     }
    else
        if ((adc[2]==0)||(adc[3]==0)||(adc[4]==0)||(adc[5]==0))start=0;
  }
/////////////////////// TINH VI TRI XE/////////////////////////////////////
int vitrixe()
{ 

    char j;
    unsigned int sum1=0, sum2=0;
    float temp;
    read_adc_all();
    for (j=0; j<chanel;j++)
    {
        sum1=sum1+adc[j]*(j+1);
        sum2=sum2+adc[j];
    }
    if (sum2!=0)
    {
        temp = (float)sum1*100;
        temp = temp/sum2;
    }
    else temp=0;
    return temp-450;
}           

//---------------------------------------------------
int PID_control()
{
    float delta, Udk;
    int error_sum=0;
  
    error =(float)vitrixe();            // Sai so dieu khien
 
  
    if ((error<-350) && (old_error>=0)) {error=-error;}
  
    delta = (error - old_error);
    old_error = error;                  //Luu gia tri sai so 
    error_sum = error_sum+error; // Thanh phan tich phan
                                    
    // Khong cho thanh phan tich phan vuot qua gia tri max
    if (error_sum <-max_e) error_sum = -max_e;
    if (error_sum > max_e) error_sum = max_e;
  
    Udk = Kp*error + Ki*error_sum + Kd*delta;  //Tin hieu dieu khien

    // Giam sat gia tri dieu khien ko duoc vuot qua ngwong
    if (Udk <-Max) Udk = -Max;
    if (Udk > Max) Udk = Max;
  
    return (int)Udk;
}


void main(void)
{
    unsigned char lcd_buffer[16];
    int duong, am;
   // float delta;
    int Udk;
  //unsigned int temp=0;
// Declare your local variables here

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=Out Func4=Out Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=0 State4=0 State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD=0x30;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 125.000 kHz
// Mode: Fast PWM top=ICR1
// OC1A output: Non-Inv.
// OC1B output: Non-Inv.
// Noise Canceler: Off
// Input Capture on Rising Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0xA2;
TCCR1B=0x5B;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x03;
ICR1L=0xFF;
OCR1AH=0x3;
OCR1AL=0xff;
OCR1BH=0x3;
OCR1BL=0xff;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

// USART initialization
// USART disabled
UCSRB=0x00;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// ADC initialization
// ADC Clock frequency: 125.000 kHz
// ADC Voltage Reference: AREF pin
// ADC Auto Trigger Source: Free Running
ADMUX=ADC_VREF_TYPE & 0xff;
ADCSRA=0xA6;
SFIOR&=0x1F;

// SPI initialization
// SPI disabled
SPCR=0x00;

// TWI initialization
// TWI disabled

// Alphanumeric LCD initialization
// Connections specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS - PORTC Bit 7
// RD - PORTC Bit 6
// EN - PORTC Bit 5
// D4 - PORTC Bit 4
// D5 - PORTC Bit 3
// D6 - PORTC Bit 2
// D7 - PORTC Bit 1
// Characters/line: 16
lcd_init(16);

TWCR=0x00;

Max = 1023-Mid;
max_e=Mid/4;

    while (1)
    {
        Udk = PID_control();
        if (Udk==Max)
        {
            am = Mid-Udk*10;
            if (am<0) am = 0; 
            duong = Mid+Udk;
        } else if (Udk==-Max)
        {            
            duong = Mid+Udk*10;
            if (duong<0) duong = 0; 
            am = Mid-Udk;
        }   
        else
        { 
            am = Mid - Udk;
            duong = Mid +Udk;
        }

        if((count==4)||(count==8)||(count==13)||(count>13))
        {
        //delay_ms(100);
           OCR1A=0;
           OCR1B=0;
           count++;
           delay_ms(1000);
        }
        else
        {
            OCR1A=am;
            OCR1B=duong;
        }
         //*  sprintf(lcd_buffer,"Vi tri xe = %d",vitrixe());
            
        sprintf(lcd_buffer,"%d %d %d %d",adc[0],adc[1],adc[2],adc[3]);
        lcd_gotoxy(0,0);
        lcd_puts(lcd_buffer);
        sprintf(lcd_buffer,"%d %d %d %d",adc[4],adc[5],adc[6],adc[7]);
        //sprintf(lcd_buffer,"Udk = %d",(int)Udk);
        lcd_gotoxy(0,1);
        lcd_puts(lcd_buffer);
      }
}

bác có thể giải thích cho em một số giá trị bác lấy đc không đó là:
level[chanel] = {400,400,400,400,400,400,400,400};
max_e=200, Mid=900,Max;
return temp-450;
Max = 1023-Mid;
max_e=Mid/4;

**tsmayruaxe** · 09-21-2014, 08:07 PM

Gửi bởi trantu93

e cúngc đang làm con này,nó vào cua thì ok nhưng đi đường thẳng thì bị lắc không chạy thẳng.có thuật toán nào giải quyết vấn đề không mấy bác chỉ e với.thanks trước

Dùng PID thì ngon lành chạy mượt và êm ru, thử đi bạn

**dunghoang** · 09-21-2014, 08:33 PM

Gửi bởi daotruongpro

Dùng PID thì ngon lành chạy mượt và êm ru, thử đi bạn

e ms tiếp xúc vs vi điển đc tầm 4 tháng mà là do tự tìm ,tự học nên phần thuật toán chưa học đc nhiều+e học toán ko đc tốt nên vẫn là gà chip.

.mong mấy bác chỉ bảo với.

**gaukhunglong** · 09-22-2014, 02:53 AM

Gửi bởi trantu93

e ms tiếp xúc vs vi điển đc tầm 4 tháng mà là do tự tìm ,tự học nên phần thuật toán chưa học đc nhiều+e học toán ko đc tốt nên vẫn là gà chip.

.mong mấy bác chỉ bảo với.

Em qua đây tham khảo , trước hết là bài đơn giản đã
Chia sẻ - Điều khiển động cơ bằng phương pháp PID hiển thị LCD | Cộng đồng cơ điện tử Việt Nam | Mechatronics

**diemkhtr** · 09-21-2015, 05:40 PM

Gửi bởi THUONGDTQB

Bác mta_cdt viết bài đi, em ủng hộ cả 2 tay luôn, về phần cơ khí thì em chịu á. Em sẻ ủng hộ phần mạch và code với bác, chắc luồng này mở ra sôi nổi lắm ý

e đang bắt tay vào làm con này nên có nhiều bỡ ngỡ..bác chia sẻ về code và mạch cho e được k ạ?

**annado9x** · 11-06-2015, 06:48 PM

Gửi bởi mta_cdt

Đây cũng là một kiểu PID viết hơi khác 1 chút.

Mã:

/*****************************************************
This program was produced by the
CodeWizardAVR V2.05.0 Professional
Automatic Program Generator
© Copyright 1998-2010 Pavel Haiduc, HP InfoTech s.r.l.
http://www.hpinfotech.com

Project :
Version :
Date    : 5/2/2014
Author  : NeVaDa
Company :
Comments:


Chip type               : ATmega16
Program type            : Application
AVR Core Clock frequency: 8.000000 MHz
Memory model            : Small
External RAM size       : 0
Data Stack size         : 256
*****************************************************/

#include <mega16.h>
#include <stdio.h>
#include <delay.h>
#include <alcd.h>
#define ADC_VREF_TYPE 0x00
#define chanel 8
//const char dir=0;
  int count=0, step=0, start=0;
  unsigned int adc[8];
  unsigned char status[chanel] = {1,2,4,8,16,32,64,128};
  int level[chanel] = {400,400,400,400,400,400,400,400};
  unsigned char outStatus;
  //float e, old_e=0,e_sum=0;
  float error, old_error, Kp=1.2, Kd=10, Ki=0;                          
  //int u;
  int max_e=200, Mid=900,Max;   

unsigned int read_adc(unsigned char adc_input)
{
    ADMUX=adc_input | (ADC_VREF_TYPE & 0xff);
    // Delay needed for the stabilization of the ADC input voltage
    delay_us(200);
    // Start the AD conversion
    ADCSRA|=0x40;
    // Wait for the AD conversion to complete
    while ((ADCSRA & 0x10)==0);
    ADCSRA|=0x10;
    return ADCW;
}
////////////////////////DOC TAT CA CAC KENH VAO CUNG NHO MIN/////////////////
void read_adc_all()
  {
    unsigned char i;
    unsigned int temp;
    outStatus = 0;
    for (i=0; i<chanel; i++)
    {
        temp = read_adc(i);
        if (temp<level[i])
        {
            adc[i]=0;
        }
        else
        {
            adc[i]=temp;
            outStatus |= status[i];         
        }
        delay_us(50);                               
    }         
    if (start ==0)
     {
        if ((adc[1]>0)&&(adc[2]>0)&&(adc[3]>0)&&(adc[4]>0)&&(adc[5]>0)&&(adc[6]>0))
        {
            start=1;
            count++;
        }
     }
    else
        if ((adc[2]==0)||(adc[3]==0)||(adc[4]==0)||(adc[5]==0))start=0;
  }
/////////////////////// TINH VI TRI XE/////////////////////////////////////
int vitrixe()
{ 

    char j;
    unsigned int sum1=0, sum2=0;
    float temp;
    read_adc_all();
    for (j=0; j<chanel;j++)
    {
        sum1=sum1+adc[j]*(j+1);
        sum2=sum2+adc[j];
    }
    if (sum2!=0)
    {
        temp = (float)sum1*100;
        temp = temp/sum2;
    }
    else temp=0;
    return temp-450;
}           

//---------------------------------------------------
int PID_control()
{
    float delta, Udk;
    int error_sum=0;
  
    error =(float)vitrixe();            // Sai so dieu khien
 
  
    if ((error<-350) && (old_error>=0)) {error=-error;}
  
    delta = (error - old_error);
    old_error = error;                  //Luu gia tri sai so 
    error_sum = error_sum+error; // Thanh phan tich phan
                                    
    // Khong cho thanh phan tich phan vuot qua gia tri max
    if (error_sum <-max_e) error_sum = -max_e;
    if (error_sum > max_e) error_sum = max_e;
  
    Udk = Kp*error + Ki*error_sum + Kd*delta;  //Tin hieu dieu khien

    // Giam sat gia tri dieu khien ko duoc vuot qua ngwong
    if (Udk <-Max) Udk = -Max;
    if (Udk > Max) Udk = Max;
  
    return (int)Udk;
}


void main(void)
{
    unsigned char lcd_buffer[16];
    int duong, am;
   // float delta;
    int Udk;
  //unsigned int temp=0;
// Declare your local variables here

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=In Func6=In Func5=Out Func4=Out Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=0 State4=0 State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD=0x30;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=0xFF
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 125.000 kHz
// Mode: Fast PWM top=ICR1
// OC1A output: Non-Inv.
// OC1B output: Non-Inv.
// Noise Canceler: Off
// Input Capture on Rising Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0xA2;
TCCR1B=0x5B;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x03;
ICR1L=0xFF;
OCR1AH=0x3;
OCR1AL=0xff;
OCR1BH=0x3;
OCR1BL=0xff;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=0xFF
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

// USART initialization
// USART disabled
UCSRB=0x00;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// ADC initialization
// ADC Clock frequency: 125.000 kHz
// ADC Voltage Reference: AREF pin
// ADC Auto Trigger Source: Free Running
ADMUX=ADC_VREF_TYPE & 0xff;
ADCSRA=0xA6;
SFIOR&=0x1F;

// SPI initialization
// SPI disabled
SPCR=0x00;

// TWI initialization
// TWI disabled

// Alphanumeric LCD initialization
// Connections specified in the
// Project|Configure|C Compiler|Libraries|Alphanumeric LCD menu:
// RS - PORTC Bit 7
// RD - PORTC Bit 6
// EN - PORTC Bit 5
// D4 - PORTC Bit 4
// D5 - PORTC Bit 3
// D6 - PORTC Bit 2
// D7 - PORTC Bit 1
// Characters/line: 16
lcd_init(16);

TWCR=0x00;

Max = 1023-Mid;
max_e=Mid/4;

    while (1)
    {
        Udk = PID_control();
        if (Udk==Max)
        {
            am = Mid-Udk*10;
            if (am<0) am = 0; 
            duong = Mid+Udk;
        } else if (Udk==-Max)
        {            
            duong = Mid+Udk*10;
            if (duong<0) duong = 0; 
            am = Mid-Udk;
        }   
        else
        { 
            am = Mid - Udk;
            duong = Mid +Udk;
        }

        if((count==4)||(count==8)||(count==13)||(count>13))
        {
        //delay_ms(100);
           OCR1A=0;
           OCR1B=0;
           count++;
           delay_ms(1000);
        }
        else
        {
            OCR1A=am;
            OCR1B=duong;
        }
         //*  sprintf(lcd_buffer,"Vi tri xe = %d",vitrixe());
            
        sprintf(lcd_buffer,"%d %d %d %d",adc[0],adc[1],adc[2],adc[3]);
        lcd_gotoxy(0,0);
        lcd_puts(lcd_buffer);
        sprintf(lcd_buffer,"%d %d %d %d",adc[4],adc[5],adc[6],adc[7]);
        //sprintf(lcd_buffer,"Udk = %d",(int)Udk);
        lcd_gotoxy(0,1);
        lcd_puts(lcd_buffer);
      }
}

a giải thích giúp e bi giờ muốn thay đổi tốc độ động cơ nhanh chậm vs thay đổi đc tín hiệu nhận mắt nhanh hơn thì phải thay đổi chỗ nào ạ

Chủ đề: thiết kế chế tạo robot dò đường.

Công cụ Chủ đề

Tìm Chủ đề

Hiển thị

Quyền viết bài

Đồ chơi sáng tạo - Người bạn nhỏ...

Lợi ích từ những món đồ chơi sáng...

Đổi mẫu miễn phí – Chỉ ở Sneaker...

Khám phá sneaker chính hãng – Chỉ...

Đàn ông sinh lực, sức khỏe...