向各位大侠求助:我用avr做两个步进电机控制一个悬挂物体运动,序调了好久也不理想!恳请
#include<avr/io.h>#include<math.h>
#include<avr/interrupt.h>
#include<avrbit.h>
#define uchar unsigned char
#define uint unsigned int
#define R_motor_state_0 0
#define R_motor_state_1 1
#define R_motor_state_2 2
#define R_motor_state_3 3
#define R_motor_state_4 4
#define R_motor_state_5 5
#define R_motor_state_6 6
#define R_motor_state_7 7
#define L_motor_state_0 0
#define L_motor_state_1 1
#define L_motor_state_2 2
#define L_motor_state_3 3
#define L_motor_state_4 4
#define L_motor_state_5 5
#define L_motor_state_6 6
#define L_motor_state_7 7
#define hl26.7
int lm=100,rm=141,ly=100,ry=141,tl,tr,m1=100,m2=10,m3=100; //坐标原点初始状态
uintNL,NR; //电机移动步数
int l1,r1,j; //两线的长度
ucharP_A;
int i=0;
uchar R_motor_state=0,L_motor_state=0,L_F_motor_state=0,R_F_motor_state=0;
uint R_count=0,L_count=0,R_motor_time=30,L_motor_time=30,R_flag=0,L_flag=0;
/*----函数声明-----*/
void xundian(uint x,uint y);
void huayuan(uint x,uint y,int R);
void R_motor();
void L_motor();
void R_F_motor();
void L_F_motor();
void delay(uint ms)
{
uint i,j;
for(i=0;i<ms;i++)
{
for(j=0;j<1141;j++);
}
}
/*----主函数(程序入口)----*/
int main(void)
{
DDRA=0XFF;//使能所在的PORTA端口
PORTA=0XFF; //设置所在管脚为输出
TCCR0=0X02;//8预分频1MHz
TCNT0=0XE1;// 设定时器初值30us
TIMSK=0X01; // 开中断
SREG|=BIT(7);
xundian(20,30);
delay(200); //延时约500ms
}
void R_motor() //占用A端口
{
switch(R_motor_state)
{
case R_motor_state_0:R_motor_state=R_motor_state_1;PORTA=(P_A&0X0F)|0XC0;P_A=PINA;break;
case R_motor_state_1:R_motor_state=R_motor_state_2;PORTA=(P_A&0X0F)|0X40;P_A=PINA;break;
case R_motor_state_2:R_motor_state=R_motor_state_3;PORTA=(P_A&0X0F)|0X60;P_A=PINA;break;
case R_motor_state_3:R_motor_state=R_motor_state_4;PORTA=(P_A&0X0F)|0X20;P_A=PINA;break;
case R_motor_state_4:R_motor_state=R_motor_state_5;PORTA=(P_A&0X0F)|0X30;P_A=PINA;break;
case R_motor_state_5:R_motor_state=R_motor_state_6;PORTA=(P_A&0X0F)|0X10;P_A=PINA;break;
case R_motor_state_6:R_motor_state=R_motor_state_7;PORTA=(P_A&0X0F)|0X90;P_A=PINA;break;
case R_motor_state_7:R_motor_state=R_motor_state_0;PORTA=(P_A&0X0F)|0X80;P_A=PINA;break;
}
}
void L_motor()
{
switch(L_motor_state)
{
case L_motor_state_0:L_motor_state=L_motor_state_1;PORTA=(P_A&0XF0)|0X08;P_A=PINA;break;
case L_motor_state_1:L_motor_state=L_motor_state_2;PORTA=(P_A&0XF0)|0X09;P_A=PINA;break;
case L_motor_state_2:L_motor_state=L_motor_state_3;PORTA=(P_A&0XF0)|0X01;P_A=PINA;break;
case L_motor_state_3:L_motor_state=L_motor_state_4;PORTA=(P_A&0XF0)|0X03;P_A=PINA;break;
case L_motor_state_4:L_motor_state=L_motor_state_5;PORTA=(P_A&0XF0)|0X02;P_A=PINA;break;
case L_motor_state_5:L_motor_state=L_motor_state_6;PORTA=(P_A&0XF0)|0X06;P_A=PINA;break;
case L_motor_state_6:L_motor_state=L_motor_state_7;PORTA=(P_A&0XF0)|0X04;P_A=PINA;break;
case L_motor_state_7:L_motor_state=L_motor_state_0;PORTA=(P_A&0XF0)|0X0C;P_A=PINA;break;
}
}
void R_F_motor()
{
switch(R_F_motor_state)
{
case R_motor_state_0:R_F_motor_state=R_motor_state_1;PORTA=(P_A&0X0F)|0X80;P_A=PINA;break;
case R_motor_state_1:R_F_motor_state=R_motor_state_2;PORTA=(P_A&0X0F)|0X90;P_A=PINA;break;
case R_motor_state_2:R_F_motor_state=R_motor_state_3;PORTA=(P_A&0X0F)|0X10;P_A=PINA;break;
case R_motor_state_3:R_F_motor_state=R_motor_state_4;PORTA=(P_A&0X0F)|0X30;P_A=PINA;break;
case R_motor_state_4:R_F_motor_state=R_motor_state_5;PORTA=(P_A&0X0F)|0X20;P_A=PINA;break;
case R_motor_state_5:R_F_motor_state=R_motor_state_6;PORTA=(P_A&0X0F)|0X60;P_A=PINA;break;
case R_motor_state_6:R_F_motor_state=R_motor_state_7;PORTA=(P_A&0X0F)|0X40;P_A=PINA;break;
case R_motor_state_7:R_F_motor_state=R_motor_state_0;PORTA=(P_A&0X0F)|0XC0;P_A=PINA;break;
}
}
void L_F_motor()
{
switch(L_F_motor_state)
{
case L_motor_state_0:L_F_motor_state=L_motor_state_1;PORTA=(P_A&0XF0)|0X0C;P_A=PINA;break;
case L_motor_state_1:L_F_motor_state=L_motor_state_2;PORTA=(P_A&0XF0)|0X04;P_A=PINA;break;
case L_motor_state_2:L_F_motor_state=L_motor_state_3;PORTA=(P_A&0XF0)|0X06;P_A=PINA;break;
case L_motor_state_3:L_F_motor_state=L_motor_state_4;PORTA=(P_A&0XF0)|0X02;P_A=PINA;break;
case L_motor_state_4:L_F_motor_state=L_motor_state_5;PORTA=(P_A&0XF0)|0X03;P_A=PINA;break;
case L_motor_state_5:L_F_motor_state=L_motor_state_6;PORTA=(P_A&0XF0)|0X01;P_A=PINA;break;
case L_motor_state_6:L_F_motor_state=L_motor_state_7;PORTA=(P_A&0XF0)|0X09;P_A=PINA;break;
case L_motor_state_7:L_F_motor_state=L_motor_state_0;PORTA=(P_A&0XF0)|0X08;P_A=PINA;break;
}
}
void xundian(uint x,uint y)
{
ly=lm;
ry=rm;
m1=m1-y;
m2=m2+x;
m3=m3-x;
lm=sqrt(pow(m1,2)+pow(m2,2)); //移动后到左滑轮的距离
rm=sqrt(pow(m1,2)+pow(m3,2)); //移动后到右滑轮的距离
l1=ly-lm;
r1=ry-rm;
if(ly>lm)
tl=1; //左电机正转收绳
else
tl=0; //左电机反转放绳
if(ry>rm)
tr=1; //右电机正转收绳
else
tr=0; //右电机反转放绳
NL=(uint)(fabs(l1)*hl);//左轮转动步数
NR=(uint)(fabs(r1)*hl);//右轮转动步数
while(NL>0||NR>0)
{
if( L_flag==1 && NL>0)
{
NL--;
L_flag=0;
if(tl==1)
{
L_motor();
}
else
{
L_F_motor();
}
}
if( R_flag==1 && NR>0)
{
NR--;
R_flag=0;
if(tr==1)
{
R_motor();
}
else
{
R_F_motor();
}
}
}
}
/*----定时器的中断服务函数----*/
ISR(TIMER0_OVF_vect)
{
TCNT0=0XE1;//定时器初值
if(NL>NR)
{
L_motor_time=30; //为使两电机同时完成
R_motor_time=30*NL/NR;
}
else
{
R_motor_time=30;
L_motor_time=30*NR/NL;
}
R_count++;
L_count++;
if(R_count>R_motor_time)
{
R_flag=1;
R_count=0;
}
if(L_count>L_motor_time)
{
L_flag=1;
L_count=0;
}
}
页:
[1]