📄 motion.c
字号:
/****************************************************
*---------------------------------------------------*
* 功能描述: X轴脉冲输出控制(T0发脉冲、T2控制加减速)*
* Y轴脉冲输出控制(T1发脉冲、T2控制加减速) *
* 输出: *
* 输入: *
* 参数: *
*---------------------------------------------------*/
#include "config.h"
#include "motion.h"
#include "mpa101.h"
// 梯形加减速用寄存器 T0,T2有效
volatile U32 xtimer_set; // TxMRC 设置值
volatile U32 xtimer_max; // TxMRC最大值
volatile U32 xupacc; // 加速度Hz/s
volatile U32 xdownacc; // 减速度Hz/s
volatile U32 xaccsum; // ms累计值
volatile U32 xsp_low; // 起步速度Hz/s
volatile U32 xsp_hi; // 最高速度Hz/s
volatile U32 xsp_v; // 当前速度Hz/s
volatile U32 xdistance_goal; // 运行距离
volatile U32 xdistance_count; // 输出脉冲计数器
volatile U32 xdistance_do; // 输出脉冲减速长
volatile U8 xtrape_mode; // 梯形模式
volatile U8 xdir; // 方向
volatile U8 xmode; // 定长模式or速度模式
// 梯形加减速用寄存器 T1,T2有效
volatile U32 ytimer_set; // TxMRC 设置值
volatile U32 ytimer_max; // TxMRC最大值
volatile U32 yupacc; // 加速度Hz/s
volatile U32 ydownacc; // 减速度Hz/s
volatile U32 yaccsum; // ms累计值
volatile U32 ysp_low; // 起步速度Hz/s
volatile U32 ysp_hi; // 最高速度Hz/s
volatile U32 ysp_v; // 当前速度Hz/s
volatile U32 ydistance_goal; // 运行距离
volatile U32 ydistance_count; // 输出脉冲计数器
volatile U32 ydistance_do; // 输出脉冲减速长
volatile U8 ytrape_mode; // 梯形模式
volatile U8 ydir; // 方向
volatile U8 ymode; // 定长模式or速度模式
volatile U8 In4old; // IN4上一次输入
volatile U8 In4new; // IN4输入
volatile U16 times_500ms;
volatile U8 Flag_500ms;
volatile U8 times_min=5;
volatile U8 Flag_min;
extern U32 out1dtime_c;
extern U32 alarmtime_c;
extern U32 alarmtime_d;
extern S32 xposi;
extern S32 yposi;
extern U8 in_count;
extern U8 pause;
//------------------------------------------//
// Function : X_Motion //
// Description: x轴开始运行 //
// Parameter : //
//------------------------------------------//
U8 X_Motion(MOTIONPARA para)
{
U32 dis1, dis2, ubuf;
if(para.distance == 0) return(0);
if(para.lowsp > MAXSPEED) para.lowsp = MAXSPEED;
if(para.hisp > MAXSPEED) para.hisp = MAXSPEED;
if(para.hisp < para.lowsp) para.lowsp = para.hisp;
if(para.hisp == 0) return(0);
xdistance_goal = para.distance;
xsp_low = para.lowsp;
xsp_hi = para.hisp;
xupacc = para.upsp;
xdownacc = para.downsp;
xdir = para.dir;
xmode= para.mode;
// 计算加减速脉冲数
ubuf = (xsp_hi*xsp_hi-xsp_low*xsp_low)/2; // U32 hispeed不能超过50K
if(xupacc>0)
dis1 = (U32)(ubuf/xupacc);
else dis1 = 0;
if(xdownacc>0)
dis2 = (U32)(ubuf/xdownacc);
else dis2 = 0;
if((dis1+dis2)>xdistance_goal){
xdistance_do = (U32)(xdistance_goal*(1000*xdownacc/
(xupacc+xdownacc))/1000-(xsp_hi-xsp_low)*8/1000);
} else {
xdistance_do = xdistance_goal-dis2-(xsp_hi-xsp_low)*16/1000;
}
xdistance_count = 0;
if(xdir==CW) FIO0CLR = Y3; else FIO0SET = Y3;
xtimer_set = (U32)(PCLK/xsp_low);
xaccsum = 0;
xtimer_max = xtimer_set;
xtrape_mode = CP_UP;
xsp_v = xsp_low;
T0MR0 = xtimer_set;
T0TCR = 0x03;
T0TCR = 0x01;
return(1);
}
//------------------------------------------//
// Function : X_Stop //
// Description: x轴停止运行 //
//------------------------------------------//
void X_Stop(void)
{
T0TCR = 0x00;
T0TCR = 0x02;
xtrape_mode = CP_STOP;
}
//------------------------------------------//
// Function : X_Stop2 //
// Description: x轴减速停止运行 //
//------------------------------------------//
void X_Stop2(void)
{
xaccsum = 0;
xmode = 1;
xtrape_mode = CP_DOWN;
xsp_hi = xsp_v;
}
//------------------------------------------//
// Function : X_Status //
// Description: 查询x轴状态 //
//------------------------------------------//
U8 X_Status(void)
{
return(xtrape_mode);
}
//------------------------------------------//
// Function : Y_Motion //
// Description: y轴开始运行 //
// Parameter : //
//------------------------------------------//
U8 Y_Motion(MOTIONPARA para)
{
U32 dis1, dis2, ubuf;
if(para.distance == 0) return(0);
if(para.lowsp > MAXSPEED) para.lowsp = MAXSPEED;
if(para.hisp > MAXSPEED) para.hisp = MAXSPEED;
if(para.hisp < para.lowsp) para.lowsp = para.hisp;
if(para.hisp == 0) return(0);
ydistance_goal = para.distance;
ysp_low = para.lowsp;
ysp_hi = para.hisp;
yupacc = para.upsp;
ydownacc = para.downsp;
ydir = para.dir;
ymode= para.mode;
// 计算加减速脉冲数
ubuf = (ysp_hi*ysp_hi-ysp_low*ysp_low)/2; // U32 hispeed不能超过50K
if(yupacc>0)
dis1 = (U32)(ubuf/yupacc);
else dis1 = 0;
if(ydownacc>0)
dis2 = (U32)(ubuf/ydownacc);
else dis2 = 0;
if((dis1+dis2)>ydistance_goal){
ydistance_do = (U32)(ydistance_goal*(1000*ydownacc/
(yupacc+ydownacc))/1000-(ysp_hi-ysp_low)*8/1000);
} else {
ydistance_do = ydistance_goal-dis2-(ysp_hi-ysp_low)*16/1000;
}
ydistance_count = 0;
if(ydir==CW) FIO0SET = Y4; else FIO0CLR = Y4;
ytimer_set = (U32)(PCLK/ysp_low);
yaccsum = 0;
ytimer_max = ytimer_set;
ytrape_mode = CP_UP;
ysp_v = ysp_low;
In4old = 0;
In4new = 0;
T1MR0 = ytimer_set;
T1TCR = 0x03;
T1TCR = 0x01;
return(1);
}
//------------------------------------------//
// Function : Y_Stop //
// Description: y轴停止运行 //
//------------------------------------------//
void Y_Stop(void)
{
T1TCR = 0x00;
T1TCR = 0x02;
ytrape_mode = CP_STOP;
}
//------------------------------------------//
// Function : Y_Stop2 //
// Description: y轴减速停止运行 //
//------------------------------------------//
void Y_Stop2(void)
{
yaccsum = 0;
ymode = 1;
ytrape_mode = CP_DOWN;
ysp_hi = ysp_v;
}
//------------------------------------------//
// Function : Y_Status //
// Description: 查询y轴状态 //
//------------------------------------------//
U8 Y_Status(void)
{
return(ytrape_mode);
}
//------------------------------------------//
// Function : xinit //
// Description: x轴脉冲设置初始化 //
//------------------------------------------//
void xinit(void)
{
xtrape_mode = CP_STOP; // 停止X轴运行
T0PR = 0; // 设置定时器分频为0分频
T0MCR = 0x03; // 匹配通道0匹配中断并复位T0TC
ytrape_mode = CP_STOP; // 停止Y轴运行
T1PR = 0; // 设置定时器分频为1分频，得16000000Hz
T1MCR = 0x03; // 匹配通道0匹配复位T1TC
// TIMER2 中断时间=1ms
T2PR = 99; // 设置定时器分频为100分频，得160000Hz
T2MCR = 0x03; // 匹配通道0匹配中断并复位T0TC
T2MR0 = 160; // 比较值 16000k/100/160=1ms
T2TCR = 0x03; // 启动并复位T0TC
T2TCR = 0x01;
FIO0SET = Y1 | Y1EN;
FIO0CLR = Y2;
times_500ms = 0;
}
//------------------------------------------//
// Function : IRQ_Timer0 //
// Description: X轴脉冲 //
//------------------------------------------//
void __irq IRQ_Timer0(void)
{
U32 bak;
bak = VICIntEnable; // 备份当前VICIntEnable的值
VICIntEnClr = (1<<4)|(1<<5)|(1<<6);
VICVectAddr = 0x00; // 清除中断逻辑，以便VIC可以响应更高优先级IRQ中断
if(pause==0){
if(FIO0PIN&Y1){
FIO0CLR = Y1;
xdistance_count++;
if(xdir==CW)xposi++;else xposi--;
} else {
FIO0SET = Y1;
}
}
if(xmode==0){
if(!(xdistance_goal^xdistance_count)){
T0TCR = 0x00;
T0TCR = 0x02;
xtrape_mode = CP_STOP;
}
}
T0MR0 = xtimer_set; // 重新设定
T0IR = 0x01; // 打开MR1中断
VICIntEnable = bak;
}
//------------------------------------------//
// Function : IRQ_Timer1 //
// Description: Y轴脉冲 //
//------------------------------------------//
void __irq IRQ_Timer1(void)
{
U32 bak;
bak = VICIntEnable; // 备份当前VICIntEnable的值
VICIntEnClr = (1<<5)|(1<<6);
VICVectAddr = 0x00; // 清除中断逻辑，以便VIC可以响应更高优先级IRQ中断
if(pause==0){
if(FIO0PIN&Y2){
FIO0CLR = Y2;
ydistance_count++;
if(ydir==CW)yposi++;else yposi--;
} else {
FIO0SET = Y2;
}
}
if(ymode==0){
if(!(ydistance_goal^ydistance_count)){
T1TCR = 0x00;
T1TCR = 0x02;
ytrape_mode = CP_STOP;
}
}
T1MR0 = ytimer_set; // 重新设定
T1IR = 0x01; // 打开MR1中断
VICIntEnable = bak;
}
//------------------------------------------//
// Function : IRQ_Timer2 //
// Description: 1ms中断计算加减速值 //
//------------------------------------------//
void __irq IRQ_Timer2(void)
{
U32 bak;
bak = VICIntEnable; // 备份当前VICIntEnable的值
VICIntEnClr = (1<<26)|(1<<6)|(1<<4)|(1<<5);
VICVectAddr = 0x00; // 清除中断逻辑，以便VIC可以响应更高优先级IRQ中断
// FIO0SET = Y4;
// X轴梯形加减速设置
if(xtrape_mode>0){
//if(T0TC > xtimer_max) T0TC = 0; // 防止TC>MR0
xaccsum++;
if(xtrape_mode==CP_UP){ // 加速段
xsp_v = (U32)(xupacc*xaccsum/1000+xsp_low);
xtimer_set = (U32)(PCLK/xsp_v);
if(xsp_v >= xsp_hi){
xtrape_mode++;
}
if(xdistance_count > xdistance_do){ // 超过减速段时减速模式3
xtrape_mode++;
// xtrape_mode = CP_DOWN;
xsp_hi = xsp_v;
// xaccsum = 0;
}
} else if(xtrape_mode == CP_STAND){ // 稳速段
if((xmode==0) && (xdistance_count > xdistance_do)){
xtrape_mode++;
xaccsum = 0;
}
} else if(xtrape_mode == CP_DOWN){ // 减速段
if(xsp_v > xsp_low) xsp_v = (U32)(xsp_hi-xdownacc*xaccsum/1000);
else {
xsp_v = xsp_low;
if(xmode==1){
T0TCR = 0x00;
T0TCR = 0x02;
xtrape_mode = CP_STOP;
}
}
xtimer_set = (U32)(PCLK/xsp_v);
}
xtimer_max = xtimer_set; // 取最大值防止T0不工作
}
// Y轴梯形加减速设置
if(ytrape_mode>0){
//if(T1TC > ytimer_max) T1TC = 0; // 防止TC>MR0
yaccsum++;
if(ytrape_mode==CP_UP){ // 加速段
ysp_v = (U32)(yupacc*yaccsum/1000+ysp_low);
ytimer_set = (U32)(PCLK/ysp_v);
if(ysp_v >= ysp_hi){
ytrape_mode++;
}
if(ydistance_count > ydistance_do){ // 超过减速段时减速模式3
ytrape_mode++;
// ytrape_mode = CP_DOWN;
ysp_hi = ysp_v;
// yaccsum = 0;
}
} else if(ytrape_mode == CP_STAND){ // 稳速段
if(((ymode==0)||(ymode==2)) && (ydistance_count > ydistance_do)){
ytrape_mode++;
yaccsum = 0;
}
} else if(ytrape_mode == CP_DOWN){ // 减速段
if(ysp_v > ysp_low) ysp_v = (U32)(ysp_hi-ydownacc*yaccsum/1000);
else {
ysp_v = ysp_low;
if(ymode==1){
T1TCR = 0x00;
T1TCR = 0x02;
ytrape_mode = CP_STOP;
}
}
ytimer_set = (U32)(PCLK/ysp_v);
}
ytimer_max = ytimer_set; // 取最大值防止T0不工作
}
times_500ms++;
if(times_500ms>499) Flag_500ms = 1;
if(times_500ms>1000){
times_500ms = 0;
Flag_500ms = 0;
times_min++;
if(times_min >= 60){ times_min = 0; Flag_min = 1;}
}
if(out1dtime_c>0) out1dtime_c--;
if(alarmtime_c>0) alarmtime_c--;
if(alarmtime_d>0) alarmtime_d--;
in_count++;
// FIO0CLR = Y4;
T2IR = 0x01; // 打开MR2中断
VICIntEnable = bak;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -