|
本帖最后由 donglaile 于 2012-4-11 19:48 编辑
自己做的循迹小车,跑的比较慢,电机的减速比太大了。没有电路图,自己想到什么就做什么拼出来的,参考了好多人的,使用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;}
}
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
该献的血还是要献的。你不献他不献。难道让我去献? --- 出自坛友:lovejp1981
|