|
|
楼主 |
发表于 2015-3-17 23:11:17
|
显示全部楼层
上传一个我最引以为自豪的加减速算法，当时想怎么最简单写个加减速算法，灵机一动出来了，超EASY，类S型。
void Goto_posr(unsigned int rpos) //转台运动到某个位置 带加减速
{
unsigned int posr;
unsigned int aaaa;
unsigned int aaa=0;
unsigned int spedr=140;
unsigned int sspr=1; //假设130加速到最高速度
unsigned int ssup,ssdn; //一半的路径
////////////////////////////////////直线
if(rpos>2300)
rpos=2300;
if(rpos<40)
rpos=40;
if(rpos>RPOS) //计算运动方向
{
RDIR=1;
posr=rpos-RPOS;
}
else
{
RDIR=0;
posr=RPOS-rpos;
}
if((posr/2)>130) //判断加减速阀值
{
ssup= 130;
ssdn= posr-130;
}
else
{
ssup= posr/2;
ssdn= posr/2;
}
for(aaaa=0;aaaa<posr;aaaa++)
{
if(aaaa<ssup) //加速
{
aaa++;
if(aaa==sspr)
{
sspr=sspr+1;
spedr=spedr-7;
aaa=0;
}
}
if(aaaa>ssdn) //减速
{
aaa++;
if(aaa==sspr)
{
sspr=sspr-1;
spedr=spedr+7;
aaa=0;
}
}
RCLK=1;
delay_10us(spedr);
RCLK=0;
delay_10us(spedr);
if(RDIR==1)
RPOS++;
else
RPOS--;
}
}
bool is_Lorg(void)
{
if(IN5==0)
{
delay(5);
if(IN5==0)
return 0;
}
return 1;
}
bool is_Rorg(void)
{
if(IN6==0)
{
delay(5);
if(IN6==0)
return 0;
}
return 1;
}
void rest_orig(void)//滚回原点
{
unsigned char aaa;
LDIR=0;
RDIR=0;
while(is_Lorg())
{
LCLK=1;
delay_10us(30);
LCLK=0;
delay_10us(30);
}
LDIR=1;
delay(200);
while(IN5==0)
{
LCLK=1;
delay_10us(30);
LCLK=0;
delay_10us(30);
}
for(aaa=0;aaa<100;aaa++)
{
LCLK=1;
delay_10us(30);
LCLK=0;
delay_10us(30);
}
while(is_Rorg())
{
RCLK=1;
delay_10us(100);
RCLK=0;
delay_10us(100);
}
RDIR=1;
delay(200);
while(IN6==0)
{
RCLK=1;
delay_10us(100);
RCLK=0;
delay_10us(100);
}
for(aaa=0;aaa<40;aaa++)
{
RCLK=1;
delay_10us(100);
RCLK=0;
delay_10us(100);
}
LPOS=100;
RPOS=40;
Goto_posr(1000); //运动?到某个位置直线滑台最远2300
Goto_posl(22500); //运动?到某个位置? 转台 a 最远45000
} |
|
[复制链接]
发表于 2015-3-17 23:06:14
