学着使用PD调节做的循迹小车

donglaile

自己做的循迹小车，跑的比较慢，电机的减速比太大了。没有电路图，自己想到什么就做什么拼出来的，参考了好多人的，使用STC12C2052AD，主要是他有AD才用的，这样电路也简单。程序也是东抄西抄再加点自己写的凑出来的。

自己太笨，一直弄不懂PID 调节，现在做了这个实验，似乎明白了一点了






照片不见了，重新贴-_-!












程序：
/*********************************************************************
//     循迹小车
//单片机：STC12C2052AD
//晶体：  24MHz
//串口波特率：9600
//日期：2012.4.7 1:38   Write by donglaile
***********************************************************************/

#include <STC12C52.h>

#include <string.h>
#include <math.h>       

sbit Work_Display = P3^2;
       
sbit motor_L0=P3^2;
sbit motor_L1=P3^3;
sbit motor_R0=P3^4;
sbit motor_R1=P1^6;


#define  MY_ADDR  1

//定义ADC_CONTER控制常量
#define ADC_POWER        0x80
#define        ADC_FLAG        0x10
#define        ADC_START        0x08
#define        ADC_SPEEDLL        0x00
#define        ADC_SPEEDLH        0x20
#define        ADC_SPEEDHL        0x40
#define        ADC_SPEEDHH        0x60

#define SET_EADCI   0x10



unsigned char idata AD_Value[6];

unsigned char data CHANNEL = 0;

//串口数据定义区
unsigned char idata RX_Counter = 0;            //数据接收计数器
unsigned char idata RX_Status  = 0;                        //串口接收完成标志位
unsigned char idata RX_Buf[8];           //数据接收缓存
unsigned char idata RX_OK;                                         //串口接收完成标志位
unsigned char idata OK[5];
//---------------------------------------------
//PWM数据定义区
unsigned char Pwm_duty;                                 //pwm占空比

unsigned int   num=0;

//------------------------
unsigned char table[]={"lizhengdong"};


//------------------------
void IO_init(void);
void Timer0_Init(void) ;
void Uart_Init(void);
void PWM_Init(void);
void Sys_Init (void);

void Send_Byte(unsigned char i);
void Send_Str(unsigned char *Str);
void Delay(unsigned int us);
bit Data_Processing(void);
//------------------------

void Delay(unsigned int us)    //延时函数
{
        unsigned char i;
        while( us-- )
        {
                i = 220;
                while(i--);
        }
}

//------------------------------------------------------------------------
//函数功能：串口1初始化函数                        9600
//------------------------------------------------------------------------
void Uart_Init(void)                //9600bps@24MHz           误差0.16%
{
        PCON &= 0x7f;                //波特率不倍速
        SCON = 0x50;                //8位数据,可变波特率
        AUXR |= 0x40;                //定时器1时钟为Fosc,即1T
        AUXR &= 0xfe;                //串口1选择定时器1为波特率发生器
        TMOD &= 0x0f;                //清除定时器1模式位
        TMOD |= 0x20;                //设定定时器1为8位自动重装方式
        TL1 = 0xB2;                //设定定时初值
        TH1 = 0xB2;                //设定定时器重装值
        ET1 = 0;                //禁止定时器1中断
        TR1 = 1;                //启动定时器1

        ES  = 1;
}

//------------------------------------------------------------------------
//函数功能：定时器0初始化函数
//------------------------------------------------------------------------
void Timer0_Init( void )                            // 初始化定时器0  作为PWM的时钟源
{
        AUXR = AUXR | 0x80;                                    // 设置stc为1T模式

    TMOD |= 0x01;

    //TH0 = (65535 -20000 )/256;                                                //定时77US
        TH0 = (65535 -200 )>>8;
    TL0 = (65535 -20000 );

        ET0  = 1;                                     //开定时器中断
        TR0  = 1;                                                         //开启定时器0
}

//------------------------------------------------------------------------
//函数功能：pwm0123初始化函数
//------------------------------------------------------------------------
void PWM_Init(void)   
{
        //舵机控制信号周期20ms，对应频率50Hz
        //
        //
        //
        //定时器的来源CPS1,CPS0;           
        //PWM频率 = PCA时钟输入源频率/256;
        //脉冲源选择T0溢出，可调PWM频率输出;
        //50Hz = Clk / 256;
        //Clk = 12800Hz,Tc = 0.000078125S = 78.125uS;
        //12T模式
        //机器周期 T = 12/主振频率,T = 12/(22.1184*10^6)S =0.542uS
        //实际定时时间Tc = x*T        ,x = Tc/T =78.125/0.542 = 144.22
        //定时器初值 = 2^n-x =65535 - 144
        //1T模式
        //机器周期 T = 1/主振频率,T = 1/(22.1184*10^6)S = 0.045uS
        //TH0 =; TL0 = ;
        CCON=0;//关闭PCA计数和所有中断位
        CMOD = PCACLK2;                                            // 设置PWM的时钟源 : T0溢出  
        CL   = 0x00;                                            // PCA初值
        CH   = 0x00;

        // PWM0 控制
        CCAP0L = 0x00;                                            // 设置比较值  控制脉宽
        CCAP0H = 0x00;                                            // CCAPnL 与 CCAPnH 值应相同
        CCAPM0 = 0x42;                                            // 模块工作模式设置为PWM输出
    PCA_PWM0 = 0x00;                                    // 清零PWM模式下的第9位

        // PWM1 控制
        CCAP1L = 0x00;                                            // 设置比较值  控制脉宽
        CCAP1H = 0x00;                                            // CCAPnL 与 CCAPnH 值应相同
        CCAPM1 = 0x42;                                            // 模块工作模式设置为PWM输出
    PCA_PWM1 = 0x00;                                    // 清零PWM模式下的第9位



//        EPCA_LVD = 1;                                            // 允许PCA中断及低压检测中断
//        IE |= 0x40;                                  /* 使能PCA定时功能 */效果同上

        CR = 1;                                                            // 开启PWM
}
//------------------------------------------------------------------------
//函数功能：端口始化函数
//------------------------------------------------------------------------
void IO_init(void)
{
        P1M0 = 0x3f;                                            // P1全部为准双向口（传统8051模式）           
        P1M1 = 0x00;
   
        P3M0 = 0x00;                                                // P3全部为准双向口
        P3M1 = 0x00;
}
//---------------------------------------
//名称: 初始化函数函数       

//日期：20081111  
//-----------------------------------------
void init_AD(void)
{
        P1M0=0x3f;        //0000 0111                //设置P1.012为高阻输入，以准备AD
        P1M1=0x00;        //0000 0000

//        AUXR |= SET_EADCI;          //允许ADC中断
        ADC_DATA = 0;  //数据寄存器清零
    ADC_CONTR = 0xf8; //1111,1000打开A/D转换电源
                                          //ADC_CONTR  ADC_POWER SPEED1 SPEED0 ADC_FLAG ADC_START CHS2 CHS1 CHS0
                                          //控制寄存器        AD上电          速度         速度  结束标志         AD开始            通道选择
                                          //speed1,speed0
                                          //  0                 0          ,270 时钟周期转换一次
                                          //  0                 1          ,540 时钟周期转换一次
                                          //  1                 0          ,810 时钟周期转换一次
                                          //  1                 1          ,1080时钟周期转换一次
                                          //
        //IPH |= 0x20;          //设置优先级
        //IP   = 0x00;          //
        Delay(1000);           //延时1ms
        ADC_CONTR = ADC_CONTR & 0xe0; //1110,0000 清ADC_FLAG,ADC_START位和低3位
//        EADC_SPI = 1;          //开ADC中断,
}
//---------------------------------------
//名称: 模数转换函数（8位）       
//说明：非中断方式
//-----------------------------------------
unsigned char Read_AD(unsigned char ch)
{
        ADC_DATA = 0;                 //清A/D转换结果寄存器                          
        ADC_CONTR = 0xF8|ch;   //1111，1000 ADCS ＝ 1，启动转换
        Delay(5);                   //延时1ms           不一定要延时1ms
        do
        {;}while((ADC_CONTR & 0x10) == 0);   //0001,0000等待A/D转换结束
        ADC_CONTR = ADC_CONTR&0xE7;   //1110,0111清ADC_FLAG位，停止A/D转换
        return ADC_DATA;        
}
//------------------------------------------------------------------------
//函数功能：系统初始化函数
//------------------------------------------------------------------------
void Sys_Init(void)          
{
        CLK_DIV = 0x00;                                    // 时钟分频 ：不分频

        IO_init();// 初始化IO口                       
        init_AD();
                                          
        Timer0_Init();                                    // 初始化 T0
        Uart_Init();                                        // 初始化 uart
        PWM_Init();                                                // 初始化 PWM
               
        EA = 1;                                                    // 开启总中断
}


//------------------------------------------------------------------------
//函数功能：发送数据到pc串口
//------------------------------------------------------------------------
void SendByte(unsigned char dat)
{
   ES = 0;
   TI = 0;
   if (dat == '\n')  
   {
     
      SBUF = 0x0d;      //output'CR'
          while(!TI);         
          return;
   }
   SBUF = dat;
   while(TI==0);
   TI = 0;
   ES = 1;
}
void Send_Str(unsigned char *Str)
{
        unsigned char Len,i;
        Len = strlen(Str);
        for(i=0;i<Len;i++)
        {
                SendByte(*Str);
                Str++;
        }
}
//-----------------------------------------------------------
/*
bit Data_Processing(void)
{
        unsigned char Ge,Shi,PWM;
        switch(RX_Buf[2])
        {
        case '0':
        case '1':
        case '2':
        case '3':
        case '4':
        case '5':
        case '6':
        case '7':
        case '8':
        case '9':Shi = RX_Buf[2] - '0';break;
        default :OK[1] = 1;break;
        }
        switch(RX_Buf[3])
        {
        case '0':
        case '1':
        case '2':
        case '3':
        case '4':
        case '5':
        case '6':
        case '7':
        case '8':
        case '9':Ge = RX_Buf[3] - '0';break;
        default :OK[2] = 1;break;
        }
        if((OK[1] == 0) && (OK[2] == 0))
        {
                PWM = Shi*10 + Ge;

                if(PWM > 100)
                {
                         Pwm_duty = 100;       
                }
                else if(PWM < 0)
                {
                    Pwm_duty = 0;
                }
                else
                {
                        Pwm_duty = PWM;
                }
                return 1;
        }
        else
        {
                return 0;
        }       
}
*/
/***************************************************
  把0--255的数值转化为3位字符串格式
****************************************************/
//void Byte2Str3(unsigned char zifu[],unsigned char val,unsigned char StartPtr)
//{
//  unsigned char characters[11]="0123456789";   
//  unsigned char tv=0;
//  
//  tv=val/100;
//  zifu[StartPtr++] = characters[tv];
//  tv=(val%100)/10;
//  zifu[StartPtr++] = characters[tv];
//  tv=val%10;
//  zifu[StartPtr] = characters[tv];
//  //zifu[3] = '\0';
//  return;
//}

//***************************************************
void motor_L(unsigned char p_on)  //255为停止 0为全速
{
    CCAP0L=255-p_on;//Set the initial value same as CCAP0H
    CCAP0H=255-p_on;//25% Duty Cycle ,0xc0
                                    //CCAPMn.1使能脉宽调制模式，EC0M必须=1
   // CCAPM2=0x42;//0100,0010 Setup PCA module 0 in PWM mode
}
void motor_R(unsigned char p_on)
{
    CCAP1L=255-p_on;//Set the initial value same as CCAP0H
    CCAP1H=255-p_on;//25% Duty Cycle ,0xc0
                            //CCAPMn.1使能脉宽调制模式，EC0M必须=1
   // CCAPM3=0x42;//0100,0010 Setup PCA module 0 in PWM mode
}
/******************************************************/
/******************************************************/
unsigned char LineInfo;
unsigned char PWMLine_L;
unsigned char PWMLine_R;
#define         NoInfo 0
#define  High_MarkLine         0x99
#define  Low_MarkLine         0x1a
void IR_adc_proc(void)        //1代表白线存在，0代表白线不存在
{
            unsigned char i,temp;

         LineInfo=0;
         for(i=0;i<6;i++)
         {         
                   temp = Read_AD(i);
                if (temp > High_MarkLine)
            {
                         //LineInfo = LineInfo |(0<<i);
                        LineInfo = LineInfo &(~(1<<(7-i)));
            }
            else
            {                               
                //LineInfo = LineInfo |(1<<i);
                        LineInfo = LineInfo |(1<<(7-i));
            }
                //SendByte(temp);
            }
         SendByte(0x05);
         SendByte(LineInfo);
}

/********************************************************
*  函数说明：获取偏离轨迹线的数值                       *
*  输入：    表明寻线状态的字节                         *
*  说明:                                               *
*           通过类质心算法获取当前机器人偏离轨迹线的量  *
*           - 表示偏左 + 表示偏右                       *
********************************************************/
/*
signed char IR_GetCG_X(unsigned char AdcValues)
{
        signed char a = 0;
        signed char Temp = 0;
        signed char Totals = 0;
       
        for (a = 0;a<8;a++)           //00 001100
        {
           if (((AdcValues <<a)>>7) == 1)   //那种更好？
           //if(AdcValues & (1<<(7-a)))
           {
                       Temp +=((-5)+(a<<1));
                    Totals++;
                 }
        }
        if (Totals ==0)
        {
             return  0;
        }
        return (Temp / Totals);
}
*/
/*********************************************************/
unsigned char IR_GET_POS(unsigned char line_pos)
{
          unsigned char Temp = 0;
        unsigned char i ;
        unsigned char T = 0;
        for (i=0;i<8;i++)           //11111100
        {
           if (((line_pos <<i)>>7) == 1)   
           {
              Temp =(i+1)*10;
                  T++;
           }
        }
        if (T == 0)
        {
             return  0;
        }
  return Temp;
}

/**************************************************************/
unsigned int lastError = 0;
#define  KP  5
#define  KD  9
#define  M1         200
#define         M2         200
void PD_calculate(void)
{
//         unsigned int sensors[3];
        unsigned char position;
        unsigned int error;
        long integral=0;
        int derivative;
        int motorSpeed;
        unsigned char m1Speed;
        unsigned char m2Speed;
        //unsigned int sensors[3];
// get calibrated sensor values returned in the sensors array, along with the line position
// position will range from 0 to 2000, with 1000 corresponding to the line over the middle sensor
    //int position = qtr.readLine(sensors);
        IR_adc_proc();
        position= IR_GET_POS(LineInfo);
// compute our "error" from the line position.  We will make it so that the error is zero when
// the middle sensor is over the line, because this is our goal.  Error will range from
// -1000 to +1000.  If we have sensor 0 on the left and sensor 2 on the right,  a reading of -1000
// means that we see the line on the left and a reading of +1000 means we see the line on
// the right.
        //int error = position - 1000;
        error = position - 30;
        integral += error;
        derivative=error - lastError;
       
// set the motor speed based on proportional and derivative PID terms
// KP is the a floating-point proportional constant (maybe start with a value around 0.1)
// KD is the floating-point derivative constant (maybe start with a value around 5)
// note that when doing PID, it's very important you get your signs right, or else the
// control loop will be unstable
        //int motorSpeed = KP * error + KD * (error - lastError);
        motorSpeed = KP * error + KD * (error - lastError);
        //motorSpeed = error/20 + integral/10000 + (3/2) * derivative;
        lastError = error;
// M1 and M2 are base motor speeds.  That is to say, they are the speeds the motors should
// spin at if you are perfectly on the line with no error.  If your motors are well matched,
// M1 and M2 will be equal.  When you start testing your PID loop, it might help to start with
// small values for M1 and M2.  You can then increase the speed as you fine-tune your
// PID constants KP and KD.
        //int m1Speed = M1 + motorSpeed;
//        int m2Speed = M2 - motorSpeed;
        m1Speed = M1 + motorSpeed;
        m2Speed = M2 - motorSpeed;
// it might help to keep the speeds positive (this is optional)
// note that you might want to add a similiar line to keep the speeds from exceeding
// any maximum allowed value
        if (m1Speed < 0)
        {
                m1Speed = 0;
        }
        if (m2Speed < 0)
        {
                m2Speed = 0;
        }
        motor_R(m1Speed);
        motor_L(m2Speed);

// set motor speeds using the two motor speed variables above

}
/*
void SpeedPWM(char PWMLine)

{
    unsigned char PWMLine_L = PWMLine;
        unsigned char PWMLine_R = PWMLine;
        char Temp = 0;
        char CG_X;
        char p=51;


        CG_X=IR_GetCG_X(LineInfo);
       
    Temp = (char)fabs((float)CG_X);
           SendByte(0x11);
        SendByte(Temp);
    //if (CG_X <0)  //偏右
        if(LineInfo >= 0x20)
           {
//                if ((Temp<<3) <= PWMLine_R)
//            {
//                       PWMLine_R -= ((Temp<<5)+Temp<<2);
//                   }                         
//            else
//            {
//                       PWMLine_R = 0;
//            }
//                motor_R(PWMLine_R);
                motor_R(255-p*Temp);
                motor_L(255-(p-8)*Temp);

           }
        if(LineInfo <= 0x10)
           //else  //偏左
           {
//               if ((Temp<<3) <= PWMLine_L)
//            {
//                  PWMLine_L -= (Temp<<5);
//            }
//            else
//            {
//                       PWMLine_L = 0;
//            }
//                motor_L(PWMLine_L);
                motor_L(255-p*Temp);
                motor_R(255-(p-8)*Temp);
       
        }       
}
*/
/****************************************************************/
//void pwm_test(void)
//{
//           if(RX_OK == 1)
//        {
//                        
//                //Send_Str("ok!");
//                ES=0;
//                Data_Processing();
//                RX_OK = 0;                                  //预装标志
//
//                if(RX_Buf[1] == 0)
//                {          
//                    motor_L(Pwm_duty);
//                }
//                if(RX_Buf[1] == 1)
//                {
//                        motor_R(Pwm_duty);
//                }
//                ES=1;
//                Send_Str("ok!");
//                SendByte(Pwm_duty);               
//   }
//}
//------------------------------------------------------------------------
//函数功能：主函数
//------------------------------------------------------------------------

void main(void)           
{       

        //unsigned char i;
//        unsigned char val_t;
        unsigned char txtbuf[17]="\n ADC Val:  ";  
        unsigned char characters[8]="01234567";
        Sys_Init();                                    // 系统初始化

        //Data_Processing();
        Send_Str("ok!");
        while(1)
        {
         
                 //pwm_test();
//                 for(i=0;i<6;i++)
//                 {
//                         val_t=Read_AD(i);
//                         SendByte(characters);
//                        Byte2Str3(txtbuf,val_t,11);
//                         Send_Str(txtbuf);
//                        SendByte(0x0d);
//                       
//                 }
                 //IR_adc_proc();
                 //SpeedPWM(255);
                 PD_calculate();
//         adc_proc();
//                 //val_t=Read_AD(0);
//                 //SendByte(val_t);
//       
//                 
//                 switch(LineInfor)
//                 {
//                 //右
//                 case 0x33:           //00 110011
//                      motor_L(0);
//                  motor_R(0);
//                          break;
//             case 0x23:           //00 100011
//                      motor_L(0);
//                  motor_R(100);
//                          break;
//                 case 0x27:           //00 100111
//                      motor_L(0);
//                  motor_R(150);
//                          break;
//                 case 0x07:           //00 000111
//                      motor_L(0);
//                  motor_R(200);
//                          break;
//                 case 0x0f:           //00 001111
//                      motor_L(0);
//                  motor_R(220);
//                          break;
//                 case 0x1f:           //00 011111
//                      motor_L(100);
//                  motor_R(240);
//                          break;
//
//                 //停止
//                 case 0x3f:         //00 111111
//                      motor_L(255);
//                  motor_R(255);
//                          break;
//                 //左
//                 case 0x31:         //00 110001
//                      motor_L(100);
//                  motor_R(0);
//                          break;
//                 case 0x39:         //00 111001
//                      motor_L(150);
//                  motor_R(0);
//                          break;
//                 case 0x38:         //00 111000
//                      motor_L(200);
//                  motor_R(0);
//                          break;
//                 case 0x3c:         //00 111100
//                      motor_L(220);
//                  motor_R(0);
//                          break;
//                 case 0x3e:         //00 111110
//                      motor_L(240);
//                  motor_R(0);
//                          break;
//
//                 default:
//                      motor_L(150);
//                  motor_R(0);
//                          break;
//                 }
        //        Send_Str(table);
                //Delay(5000);

//                pulse_1(0x0c);
//                pulse_2(0x0c);
//                Delay(50000);
//
//                pulse_1(0xa6);
//                pulse_2(0xa6);
//                Delay(50000);
//
//                pulse_1(0xb3);
//                pulse_2(0xb3);
//                Delay(50000);
   }
}
//------------------------------------------------------------------------
//函数功能：定时器0中断服务函数
//------------------------------------------------------------------------
void Timer0_ISR(void) interrupt 1 using 1 // 定时器中断
{
    TH0 = 0xf8;                                                         //修改重装值可改变pwm的频率
    TL0 = 0xd0;
        num++;
        if(num == 500)
        {
           num=0;
           Work_Display=~Work_Display;
        }
}
//void ADC_ISR(void)  interrupt 5 using 3
//{
//        ADC_CONTR = !ADC_FLAG;  //清ADC_FLAG位
//       
//        AD_Value[CHANNEL] = ADC_DATA;                              //读取8位AD结果 ADC_Value = 256*(Vin/Vcc)       
//        SendByte(CHANNEL);
//        SendByte(ADC_DATA);

//        if(++CHANNEL > 6)          //改变AD通道
//        {CHANNEL = 0;}
//        ADC_CONTR = ADC_POWER | ADC_SPEEDLL | ADC_START | CHANNEL;
//}
//------------------------------------------------------------------------
//函数功能：串口中断服务函数
//------------------------------------------------------------------------                       
/********************************************************/
void Uart_ISR(void) interrupt 4 using 2   //主机接收单片机数据
{
         unsigned char Temp_dat;
         if(RI == 1)                                                        //收到数据
         {                                                          
                  RI=0;                                             //清中断请求
                  Temp_dat = SBUF;         
                  /*---------接收数据------------------*/
                  if(Temp_dat == 'S')
                  {
                          RX_Status  = 1;                                  //数据接收开始标志
                          RX_Counter = 0;                                  //清零累加器
                          RX_Buf[0] = 'S';                              //纠错          
                  }
                  if(RX_Status == 1)
                  {
                          RX_Buf[RX_Counter]=Temp_dat;
                          if((RX_Buf[0] == 'S') && (Temp_dat == 'E') && (RX_Counter == 4))       
                            {
                                  RX_Status = 0;         //清零计数器
                                  RX_Counter= 0;
                                  RX_OK     = 1;        //数据接收完成标志
                                  OK[1]                = 0;
                                  OK[2]                = 0;
                                  OK[3]                = 0;
                      }
                          RX_Counter++;
                  }
          }
          if(TI)
          {TI=0;}
}

hhxb

没有电路图，这个代码实在没耐心看；楼主能不能贴几张小车照片呀

donglaile

hhxb 发表于 2012-4-11 17:52
没有电路图，这个代码实在没耐心看；楼主能不能贴几张小车照片呀

代码你复制到编辑器里看就好看了，，，这样写是为了好调试

linken

果断抄走代码研究....

fengkehy

好复杂的赶脚

gooodboooy

先收藏了