红外公循迹+PID控制电机的小车

苏三岁 · 发表于 2018-1-6 14:24:32

想学习做一个红外光循迹黑白线来控制的小车，pid控制电机的方法
有没有大神能够分享一下的心得，资料的

正点原子 · 发表于 2018-1-7 01:30:22

帮顶

xy123 · 发表于 2018-1-7 17:54:38

#include <reg51.H>
#define SPEEDMAX 1000
sbit ENA = P1^2;
sbit CS  = P1^3;
sbit CLK = P1^4;
sbit DO  = P1^5;
unsigned char seg_data[10]={0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f};  //¹²Òõ¼«ÊýÂë¹Ü¶ÎÖµ
unsigned char bit_tab[4]={0xfe,0xfd,0xfb,0xf7};    //Î»Ñ¡¿ØÖÆÎ»
unsigned int mod[4]={1000,100,10,1};  //È¡Ä£¿ØÖÆÁ¿£¬ÎªÁË·½±ã¼ÆËãÊ®½øÖÆÊýµÄÃ¿Ò»Î»Êý×Ö
float s1,s2,s3,s4,s5;                      //Ëæ»ú¶¨ÒåµÄËÙ¶È±äÁ¿£¬±¸ÓÃ
unsigned int rate;                               //Î´Ê¹ÓÃ
unsigned int timer0_cnt;                         //¶¨Ê±Æ÷0¶¨Ê±ÖÐ¶Ï¼ÆÊý£¬ÎªÁË¿ØÖÆÕ¼¿Õ±ÈÊ¹ÓÃ
      int button_set_data;                      //°´¼üÉèÖÃËÙ¶ÈÖµ
unsigned char button;                            //°´¼üÖµ
unsigned char flag;                               //±êÖ¾Î»£¬Ä¿Ç°Î´Ê¹ÓÃ
unsigned char timer2_cnt;                         //¶¨Ê±Æ÷2¼ÆÊýÆ÷£¬ÓÃÀ´¼ÆÊ±500ms
unsigned char pot;                               //»¬¶¯±ä×èÆ÷µçÑ¹Öµ
unsigned int motor_plus;                         //1sÄÚ£¬µç»ú±àÂëÆ÷Âö³å¸öÊý
unsigned int driver_data;                         //Õ¼¿Õ±ÈÇý¶¯µ÷Õû
unsigned int pot_data;                            //»¬¶¯±ä×èÆ÷µçÑ¹Öµ¶ÔÓ¦µÄ×ªËÙ
unsigned int speed;                               //µç»ú×ªËÙ
      int pid_val;                            //PID¿ØÖÆÊä³öÖµ
typedef struct
{
   unsigned int set_point;

   float P;
   float I;
   float D;

   unsigned int last_error;
   unsigned int prev_error;
   unsigned int sum_error;
}
PID;                                        //PIDËã·¨½á¹¹Ìå
PID motor_pid;                               //¶¨Òåµç»úPID¿ØÖÆ±äÁ¿
//PID¼ÆËã
int PidCalculate(PID *ppid, int next_point)    //PID¼ÆËãº¯Êý
{
   float derror,error;
   float val;

   error=ppid->set_point-next_point;          //µ±Ç°Ê±¿Ì²îÖµ
   ppid->sum_error+=error;                   //»ý·ÖÖµ£¬»ý·ÖÖµ¾ÍÊÇ²îÖµµÄÀÛ¼Ó
   derror=ppid->last_error - ppid->prev_error; //Î¢·ÖÖµ£¬Î¢·Ö¾ÍÊÇµ±Ç°Ê±¿ÌµÄ²îÖµ-Ç°Ò»Ê±¿ÌµÄ²îÖµ

   ppid->prev_error=ppid->last_error;       //´æ´¢µ±Ç°Ê±¿ÌµÄ²îÖµ£¬±ä³ÉÇ°Ò»Ê±¿Ì²îÖµ£¬ÎªÏÂ´Î¼ÆËã×ö×¼±¸
   ppid->last_error=error;

   val=ppid->P*error + ppid->I * ppid->sum_error + ppid->D*derror;
   return (int)val;
}
void PidInit (PID *ppid)             //PID³õÊ¼»¯º¯Êý
{
   ppid->set_point=0;
   ppid->P=0.6;
   ppid->I=1;
   ppid->D=0.2;
}

void delay(unsigned int x)             //ÑÓÊ±º¯Êý
{
   unsigned int i;
   for(i=0;i<x;i++);
}
unsigned char AD0831_ReadChar(void)    //ADC¶ÁÈ¡Êý¾Ý
{
   unsigned char val;
   unsigned char i;
   val=0;
   CS=0;
   for(i=0;i<9;i++)
   {
      CLK=1;
      delay(1);
      CLK=0;
      val<<=1;
      val|=DO;
   }
   CS=1;
   delay(1);
   return val;
}
void main(void)
{
   unsigned char i,j;

   IT0=1;
   EX0=1;
   TMOD=0X12;
   TH0=256-70;
   TL0=256-70;
   ET0=1;
   EA=1;
   TR0=1;
   TH1=(65536-50000)/256;
   TL1=(65536-50000)%256;
   ET1=1;
   TR1=1;
   P1=0x00;
   DO=1;

   PidInit(&motor_pid);

   while(1)
   {
      P1|=0x01;
      j++;
      if(j==100)
      {
         pot = AD0831_ReadChar();
         pot_data=(unsigned int)(((float)(pot)/256)*1000);
         j=0;
      }

      button=P3;
      button=button & 0x03;
      if(button!=0x03)
      {
         delay(10);
         button=P3;
         button=button & 0x03;
         if(button!=0x03)
         {
               if(button==0x02)
               {
                  button_set_data=button_set_data+100;
               }
               if(button==0x01)
               {
                  button_set_data=button_set_data-100;
               }
               if(button_set_data>1000)
                  button_set_data=1000;
               if(button_set_data<0)
                  button_set_data=0;

         }
         while(button!=0x03)
         {
               button=P3;
               button=button & 0x03;
         }
      }


      for(i=0;i<4;i++)
      {
         P0=seg_data[(speed/mod[i])%10];
         P2=bit_tab[i];
         delay(100);
         P2=0xff;
      }

      motor_pid.set_point=pot_data;
   }
}
void timer0() interrupt 1
{
   timer0_cnt++;
   if(timer0_cnt==1000)
   {
      timer0_cnt=0;
   }
   if(timer0_cnt<driver_data)
   {
      ENA=1;
   }
   else
   {
      ENA=0;
   }
}
void int0() interrupt 0
{
   motor_plus++;
}
void timer1() interrupt 3
{
   TH1=(65536-50000)/256;
   TL1=(65536-50000)%256;
   timer2_cnt++;
   if(timer2_cnt==10)
   {
      timer2_cnt=0;

      speed=((float)(motor_plus)*(float)60)/(float)24.0;
      motor_plus=0;

      pid_val=PidCalculate(&motor_pid,speed);
      if(pid_val>1000)
         pid_val=1000;
      if(pid_val<0)
         pid_val=0;
      driver_data=(unsigned int)pid_val;
   }

}