现在我在做一个项目，有两根行程不等的导轨，导轨上有电机驱动的设备，，两设备在导轨的部分行程上能够自动跟踪，A设备有人操作运动，B设备根据A设备运动而自动跟踪对准，两设备均使用电位器来测量行程位置，我的想法先给个起始点，通过“总行程/总AD=实际走了的行程/当前AD值-起始AD”，求出设备走了多少CM的行程，再起控制设备B去走相同行程，程序如下
void AUTO_MOTION_CONTROL(void)
{
MASTER_AD_DATA=ADCMathAV(1); //设备A的当前AD值，
MASTER_AD_BASE=ADC6_A_BASE_EEPROM;//设备A起始点的AD值，调试时保存的，
SERVANT_AD_DATA=ADCMathAV(0);//设备B的当前AD值
SERVANT_AD_BASE=ADC5_B_BASE_EEPROM;//设备B起始点的AD值，调试时保存的，与设备A起始点水平对齐
SERVANT_AD_MAX=ADC1_B_EEPROM_H;//设备B的最大AD值
SERVANT_AD_MIN=ADC1_B_EEPROM_L;//设备B的最小AD值
BFIXData=(SERVANT_AD_MAX-SERVANT_AD_MIN)/10;//设备B的总AD
AFIXData=(ADC4_A_EEPROM_H-ADC6_A_BASE_EEPROM)/10;//设备A的总AD
TRAN_MASTER=FIX_A_UD_DATA;//设备A跟踪行程，调试时保存的
TRAN_SERVANT=FIX_B_UD_DATA;//设备B跟踪行程，调试时保存的
if (SERVANT_AD_DATA > SERVANT_AD_MIN)//算出当前设备B的位置
{
BRunRoat =(unsigned int ) (SERVANT_AD_DATA - SERVANT_AD_BASE);
BRunRoat =(unsigned int ) ( BRunRoat * TRAN_SERVANT);
BRunRoat =(unsigned int ) ( BRunRoat/ BFIXData);
}else
{
BRunRoat = 0;
}
if (MASTER_AD_DATA > MASTER_AD_BASE)//算出当前设备A的位置
{
ARunRoat =(unsigned int ) (MASTER_AD_DATA - MASTER_AD_BASE);
ARunRoat =(unsigned int ) (ARunRoat * TRAN_MASTER);
ARunRoat =(unsigned int ) (ARunRoat/ AFIXData);
}else
{
ARunRoat = 0;
}
if (MathBig( ARunRoat, BRunRoat, 20))//比较设备AB两个行程大小，超过20就开始跟踪
{
if (( ARunRoat >BRunRoat) && (!L_B_U) && (SERVANT_AD_DATA < SERVANT_AD_MAX))
{
OUT_B_U; //B上升
AUTO_FOLLOW_FLAG = 1;
}
if ((ARunRoat < BRunRoat) && (!L_B_D) && (SERVANT_AD_DATA> SERVANT_AD_MIN))
{
OUT_B_D; //B下降
AUTO_FOLLOW_FLAG = 2;
}
}
if ((L_B_U || (SERVANT_AD_DATA > SERVANT_AD_MAX)) && (AUTO_FOLLOW_FLAG == 1))//到上限位停止
{
OUT_B_U_STOP;
AUTO_FOLLOW_FLAG = 0;
RATE_INIT_SET = 0;
V0 = 0;
U0 = 0;
en = 0;
en1 = 0;
en2 = 0;
Vpwm = 0;
PID_START_STOP_FLAG = 0;
PID_SET_RATE = PID_SET_BOOT;
}
if ((L_B_D || (SERVANT_AD_DATA< SERVANT_AD_MIN)) && (AUTO_FOLLOW_FLAG == 2))//到下限位停止
{
OUT_B_D_STOP;
AUTO_FOLLOW_FLAG = 0;
RATE_INIT_SET = 0;
V0 = 0;
U0 = 0;
en = 0;
en1 = 0;
en2 = 0;
Vpwm = 0;
PID_START_STOP_FLAG = 0;
PID_SET_RATE = PID_SET_BOOT;
}
if ((MathSmall(ARunRoat,BRunRoat, 5)) //设备AB行程误差小于5就停止跟踪
&& ((AUTO_FOLLOW_FLAG == 1) || (AUTO_FOLLOW_FLAG == 2)))
{
OUT_B_U_STOP;
OUT_B_D_STOP;
OUT_B_UD_STOP;
AUTO_FOLLOW_FLAG = 0;
RATE_INIT_SET = 0;
V0 = 0;
U0 = 0;
en = 0;
en1 = 0;
en2 = 0;
Vpwm = 0;
PID_START_STOP_FLAG = 0;
PID_SET_RATE = PID_SET_BOOT;
}
if ((MathSmall(ARunRoat,BRunRoat, 200)) //设备B进入减速
&& ((AUTO_FOLLOW_FLAG == 1) || (AUTO_FOLLOW_FLAG == 2)))
{PID_START_STOP_FLAG=1;PID_SET_RATE = PID_SET_DEC;}
}
现在这程序实际运行中总是跟踪偏差大，离起始点越远，偏差越大，我个人分析电机控制部分应该没有问题，主要是在数据计算处理部分出错导致，但实在找不出具体原因，求好心的大神帮忙分析下，小弟感激不尽！ |
楼主
标题置顶
标题高亮
