|
|
适合作为移动平台的小车驱动控制程序
/***************************FileName:motionctl.C************************/
/******************************ICCAVR6.30编译***************************/
/*******************************editor:sq.cu****************************/
/********************************2007.09.29*****************************/
#include <iom16v.h>
#include <macros.h>
#include <stdio.h>
//***************************全局变量定义**************************//
#define uint unsigned int
#define uchar unsigned char
#define U1 12 //电机额定电压
float k[]={0.002,0.001,0}; //PID参数记录
long int position;
long int count=0;
long int cycle;
unsigned int current,setcurrent,seti;//current为A/D电流实际值，setcurrent为A/D限流值,seti为限流实际电流值
int speed=0;
uint realspd;
int motorstatus=0;
int commandflag=0;
int time_rcv=0;
int rcv_flag=0;
unsigned int cmd_fro_arm[30]; //上位机命令数组缓存
unsigned int command[6]; //接收上位机命令数组
int a=0; //采样时间次数
int b=0;
int c=0;
float t; //采样时间.s
int sampletime=10; //给定采样时间的次数
int EK; //本次偏差
int EK_1; //上次偏差
int EK_2; //上上次偏差
char flage=0; //监控标志
void delayNms(unsigned int n)
{
unsigned int i=0;
unsigned int b=0;
for(i=0;i<n;i++)
for(b=0;b<1152;b++) ;
}
void IO_init(void)
{
DDRA&=0xBF; //PA6作为检测电流输入端
DDRC = 0x09; //sen2,sen3输入;pwm sign，SD输出
PORTC &= ~BIT(3); //刚启动程序时确保电机停止
DDRB|=0x08; //PB3为PWM信号输出
DDRD &= 0xBF; //码盘信号CHA作为捕获输入
PORTD&=0xBF; //PD6(ICP)为输入捕获端,设置该pin为 不 带上拉电阻输入
}
//*************************** 串口初始化***************************//
void uart_init(void)
{
UCSRB=(1<<RXEN)|(1<<TXEN)|(1<<RXCIE);//8位数据 接收查询接受中断
UBRRL=51; //波特率为9600
UBRRH=0;
UCSRC=(1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0);//选UCSRC寄存器，数据长度为8位,1位停止位；
}
void USART_Transmit( unsigned char data )
{
while ( !( UCSRA & (1<<UDRE)) )
;
UDR = data;
}
unsigned char USART_Receive( void ){
while ( !(UCSRA & (1<<RXC)) )
;
return UDR;
}
//**************************capture函数捕获码盘转速信号周期***************************//
unsigned int capture(void)
{
unsigned int t1;
t1=0;
TCNT1=0;
TCCR1B=0x02; //8分频定时方式,1us计数一次，开始计时
switch(PIND&0x40)
{
case 0:
while(!(PIND&0x40))
{if(TCNT1>=1800)
{
break; //空操作计时，若1920us后还未检测到脉冲信号，说明电机未启动或堵转！（电机速度为1r/min时周期为3.5ms）
}
}
if(TCNT1<=1800)
{
TCNT1=0;
}
else
{
realspd=0;
break;
}
while(PIND&0x40)
{if(TCNT1>=1800)
{
break;
}
}
if(TCNT1>=1800)
{
realspd=0;
break;
}
else
{
t1=TCNT1;
TCNT1=0;
}
while(!(PIND&0x40))
{if(TCNT1>=1800)
{
break;
}
}
if(TCNT1<=1800)
{
t1+=TCNT1;
realspd=234375/t1;} //常数为6e7/256=234375,速度单位为r/min,未经减速器的电机转速
else
{
realspd=0;
}
break;
case 0x40:
while(PIND&0x40)
{if(TCNT1>=2000)
{
break; //空操作计时，若1920us后还未检测到脉冲信号，说明电机未启动或堵转！（电机速度为1r/min时周期为3.5ms）
}
}
if(TCNT1<=2000)
{
TCNT1=0;
}
else
{
realspd=0;
break;
}
while(!(PIND&0x40))
{if(TCNT1>=2000)
{
break;
}
}
if(TCNT1>=2000)
{
realspd=0;
break;
}
else
{
t1=TCNT1;
TCNT1=0;
}
while(PIND&0x40)
{if(TCNT1>=2000)
{
break;
}
}
if(TCNT1<=2000)
{
t1+=TCNT1;
realspd=234375/t1;}
else
{
realspd=0;
}
break;
default:
break;
}
TCCR1B=0x00; //检测一个周期后结束定时
TCNT1=0;
/*
if(realspd==0)
{
printf("realspd= %d%\n",realspd);
}
*/
return realspd;
}
//***************************PID调节函数***************************//
void PIDB(void)
{
float u; //电压差值
int z; //输出增量
int t; //采样时间
int temp1; //暂存
int i;
//t = sampletime*0.000032;
EK=speed*66-realspd; //减速比为66:1
//printf("realspd= %d%\n",realspd/66);
if((EK>=66)||(EK<=-66))
{
//u=k[0]*((EK-EK_1)+(t/k[1])*EK+(k[2]/t)*(EK-2*EK_1+EK_2));
u=k[0]*EK;
z=256*u/U1;
temp1=OCR0;
temp1=temp1+z;
if(temp1<=0) //结果小于0时输出0(输出0x10防止因为PID调节导致电机停机)
temp1 = 0x00;
if(temp1>=0xFF) //结果大于0xF0时输出0xF0
temp1 = 0xFF;
OCR0=temp1;
//printf("OCR0= %d \n",OCR0);
EK_2 = EK_1;
EK_1 = EK;
}
}
//*************************电流检测AD转换程序段****************************//
void AD_CURRENT(void)
{
ADMUX=0xC6; //选ADC6为模拟比较器的输入，参考电压为片内基准电压2.56v；
ADCSRA=0xC6; //A/D单次转换使能，分频为CLK/64，ADC时钟为125KHz(50-200);
while(!(ADCSRA & 0x10) ); //等待AD测量完成
current=ADC&0x03ff; // 返回转换结果,转换结果为10位
//USART_Transmit(current>>8);
//USART_Transmit(current);
if(current<=setcurrent)//ACS704反接，电流越大电压越小，检测电流超值，关电机；
{
PORTC &= ~BIT(3);
motorstatus=0;
//printf("The current is bigger than set current!");
}
}
//***********************************main函数****************************************//
void main(void)
{
IO_init();
uart_init();
seti=6; //过流检测限流值，为seti/2(A)
setcurrent=1000-20*seti;
MCUCR|=0x08; //INT1下降沿触发，记录脉冲数
TIMSK|=0x40; //T/C2溢出中断使能
TCCR2=0x02; //8预分频，1us计数一次
TCNT2=0xCE; //0.05ms溢出一次
TCCR0=0x61; //PWM相位修正模式，在升序计数时发生比较匹配将清零OC0，
//降序计数时发生比较匹配将置位OC0，一分频抑制高频噪音（八分频为0x62，1us计数一次）
SEI(); //使用串行接收中断首先需要打开全局中断，输入捕获需在子函数关全局中断
while(1)
{
if(commandflag==0) continue; //不能用return
else
{
commandflag=0;
if(command[3]>=100) command[3]=100;
if(command[3]<= 5 )
{
k[0]=0.004;
}
else
{
k[0]=0.002;
}
switch(command[2]) //command[2]为电机控制命令，command[3]，command[4]为电机命令参数的低8位和高8位
{
case 0x01: //速度控制：正转，顺时针
PORTC |= BIT(3);
/*if(!PINC0) //延时避免直通短路
{
delayNms(1);
}*/
PORTC |= BIT(0);
speed=command[3];
motorstatus=1;
break;
case 0x02: //速度控制：反转，逆时针
PORTC |= BIT(3);
/*if(PINC0)
{
delayNms(1);
}*/
PORTC &= ~BIT(0);
speed=command[3];
motorstatus=1;
break;
case 0x03: //停止,反向制动
/*if(!(PINC0))
{
PORTC |= BIT(0);
}
if((PINC&0x01)==0x01)
{
PORTC &= ~BIT(0);
}
delayNms(1);*/
PORTC &= ~BIT(3);
motorstatus=0;
break;
case 0x04: //位置控制，前进一定距离
PORTC |= BIT(3);
PORTC |= BIT(0);
motorstatus=1;
count=0;
if(command[3]!=0)
{
speed=command[3];
}
position=command[4]; //单位为cm
cycle=position*40744/100; //cycle=position*66*256/(pai*D),D为轮子直径132mm
OCR0=5*speed/2;
GICR|=0x80; //使能外部中断
while(count<=cycle);
PORTC &= ~BIT(0); //反向制动
delayNms(1); //延时不能太长，否则会跑飞
PORTC &= ~BIT(3);
motorstatus=0;
count=0;
GICR&=0x7F; //关外部中断
break;
case 0x05: //位置控制，后退一定距离
PORTC |= BIT(3);
PORTC &= ~BIT(0);
motorstatus=1;
count=0;
if(command[3]!=0) speed=command[3];
position=command[4];
cycle=position*40744/100;
OCR0=5*speed/2;
GICR|=0x80;
while(count<=cycle);
PORTC |= BIT(0);
delayNms(1);
PORTC &= ~BIT(3);
motorstatus=0;
count=0;
GICR&=0x7F;
break;
default:
break;
}
}
}
}
///////////////////////////////////////////////////////////////////////////
#pragma interrupt_handler UART_RXC:12
void UART_RXC(void)
{
int i=0;
char chkcode1=0;
char chkcode2=0;
TIMSK&=0xBF; //关闭定时器2溢出中断使能
UCSRB&=0x7F; //关闭串口接收中断使能
rcv_flag=0;
for(i=0;i<6;i++)
{
while(!(UCSRA&(1<<RXC)))
{
time_rcv++;
if(time_rcv>=8000)
{
time_rcv=0;
rcv_flag=1;
//printf("data lost!");
break;
}
}
if(rcv_flag==1) break;
cmd_fro_arm=UDR;
USART_Transmit(cmd_fro_arm);//
} //接受主机通过485总线发出的命令，被选中时回发指定数据
if((rcv_flag==0)&&(cmd_fro_arm[1]==0x01))
{
{
for(i=0;i<6;i++)
{
command=cmd_fro_arm;
}
}
for(i=0;i<5;i++)
chkcode1+=command; //校验码设为前面5个命令字节的相加值
chkcode2=command[1]+0x82; //校验码为数据类型和从机地址码相加
if((command[5]==chkcode1)&&(command[1]==0x01)) //在所发命令校验正确的情况下，一旦从机被选中，回发指定格式的代码，包括数据类型，从机地址和校验码
{
USART_Transmit(0x82);
USART_Transmit(command[1]);
USART_Transmit(chkcode2);
commandflag=1;
}
/*
else //主机命令错误时返回数据
{
USART_Transmit(0x82);
USART_Transmit(command[1]);
USART_Transmit(0xFF);
}
*/
}
TIMSK|=0x40; //定时器2溢出中断使能
UCSRB|=0x80; //串口接收中断使能
}
//***************************定时器2中断服务程序*******************//
#pragma interrupt_handler TIM2_OVF:5
void TIM2_OVF(void)
{
char sreg;
sreg=SREG;
TIMSK&=0xBF; //关闭定时器2溢出中断使能
TCCR2=0x00;
TCNT2=0x00;
//SEI();
a++;
if(a==sampletime) //PID采样周期为0.05ms*10=0.5ms
{
a=0;
if(motorstatus==1)
{
capture();
PIDB();
//AD_CURRENT();
}
}
//CLI();
TIMSK|=0x40; //重新打开定时器2溢出中断使能
TCCR2=0x02;
TCNT2=0xCE;
SREG=sreg;
}
//***************************INT1外部中断服务程序*******************//
#pragma interrupt_handler INT1_CAP:3
void INT1_CAP(void)
{
if(!(PIND&0x08))
{
count++;
/*b++;
if(b==20)
{
b=0;
capture();
PIDB();
}*/
}
}
点击此处下载 ourdev_544397.rar(文件大小:72K) (原文件名:控制程序.rar) |
阿莫论坛20周年了！感谢大家的支持与爱护！！
月入3000的是反美的。收入3万是亲美的。收入30万是移民美国的。收入300万是取得绿卡后回国，教唆那些3000来反美的！
|
发表于 2010-4-8 08:17:48
