学着使用PD调节做的循迹小车-hhxb的回答

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

2020-01-20 18:55发布

本帖最后由 donglaile 于 2012-4-11 19:48 编辑

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

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

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

Camera_20120331_201416-001.jpg (225.48 KB, 下载次数: 0) 下载附件 2012-4-11 19:47 上传

程序：
/*********************************************************************
//    循迹小车
//单片机：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 == ' ')
{

   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] = '';
//  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]=" 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;}
}

友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。

6条回答

hhxb
2020-01-20 21:06

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

加载中...

查看其它6个回答

一周热门更多>

相关问题

【东软载波ESF0654 PDS开发板活动】开箱
1 个回答

东软载波ESF0654 PDS开发板外部中断
1 个回答

东软载波ESF0654 PDS开发板高级控制定时器AD16C4T
1 个回答

用串口调试助手为什么只能在hex模式接收发送而在文本模式不行
9 个回答

触摸芯片SC02B/SC04B在地砖灯的设计方案
1 个回答

相关文章

51单片机与蓝牙模块连接
0个评论

51单片机的硬件结构
0个评论

基于51单片机的无线遥控器制作
0个评论

51单片机 AD转换
0个评论

51单片机数码管递增显示
0个评论

如何实现对单片机寄存器的访问
0个评论

基于51单片机的指纹密码锁
0个评论

采纳回答

向帮助了您的知道网友说句感谢的话吧!

非常感谢!

编辑标签

最多设置5个标签!

51单片机

举报内容

检举类型

检举内容

检举用户

检举原因

广告推广

恶意灌水

回答内容与提问无关

抄袭答案

其他

检举说明(必填)

打开微信“扫一扫”，打开网页后点击屏幕右上角分享按钮

付费偷看金额在0.1-10元之间

您已邀请0人回答查看邀请

擅长该话题的人

回答过该话题的人

我关注的人

Copyright © 2024 平头弟京ICP备15032243号-1 投诉举报邮箱：nettui@qq.com

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

打开微信“扫一扫”，打开网页后点击屏幕右上角分享按钮

一周热门 更多>

相关问题

相关文章

采纳回答

编辑标签

举报内容

检举类型

检举原因

检举说明(必填)

打开微信“扫一扫”，打开网页后点击屏幕右上角分享按钮

付费偷看金额在0.1-10元之间

一周热门更多>