作 者：语言与存在的顶点.
时 间：2020.7.2
内容描述：实现电机控制定期定量缓慢加减速功能。
/* add_sub.c 文件 */ #include <string.h> #include <stdio.h> #include "add_sub.h" //梯形加速 /*梯形加速 加速 减速 4 ____ 3 / \ 2 / \ 1 / \ 0_ ___/ \___ _____ -1 \ / -2 \ / -4 \___/ 1 实现给定值得分布赋值； 2 主要放在PID输入之前，策略控制之后； */ // void ADDWork(ADDorLOW* V,u16 *Time_a) { V->error=V->Vpoint - V->Vout; //偏差 if((V->Vpoint >= 0)&&(V->Vout >0)) //正半轴 { if(V->error > V->AccError) //偏差 > 允许偏差 { if(*Time_a >= V->val) //加速过程 { *Time_a=0; V->Vout += V->Acc; //输出 + } } else if((-V->error)>V->AccError) //偏差绝对值 > 允许偏差 { if(*Time_a >= (V->sub)) //加速过程 { *Time_a = 0; V->Vout-= V->Acc; //输出 - } } else //偏差 在 允许偏差内 { V->Vout = V->Vpoint; //输出目标值 *Time_a=0; } } else if((V->Vpoint <= 0)&&(V->Vout <0)) //负半轴 { if(V->error > V->AccError) { if(*Time_a >= V->sub) { *Time_a=0; V->Vout += V->Acc; } } else if((-V->error)>V->AccError) { if(*Time_a >= (V->val)) { *Time_a = 0; V->Vout-= V->Acc; } } else { V->Vout = V->Vpoint; *Time_a=0; } } else //正负切换---- { if(V->error > V->AccError) { if(*Time_a >= V->val) { *Time_a=0; V->Vout += V->Acc; } } else if((-V->error)>V->AccError) { if(*Time_a >= (V->val)) { *Time_a = 0; V->Vout-= V->Acc; } } else { V->Vout = V->Vpoint; *Time_a=0; } } if(V->Vout<V->MinOut) //限副最小值 V->Vout=V->MinOut; else if(V->Vout>V->MaxOut) //限副最大值 V->Vout=V->MaxOut; } //梯形加速模块初始化 ADDorLOW MotorA; ADDorLOW MotorB; void addRunA_Init(void) //A电机 { if(MotorA.Init==0) { MotorA.Acc = 25; //加速度初值 MotorA.AccError = 30; //加速度允许偏差 > 加速度 MotorA.MaxOut = 2000; //最大速出 mm/s MotorA.MinOut =-2000; //最大速出 mm/s MotorA.val = 10; //加速度运行周期 * 10ms MotorA.sub = 5; //减速度运行周期 * 10ms MotorA.Init=1; } } void addRunB_Init(void) //B电机 { if(MotorB.Init==0) { MotorB.Acc = 25; //加速度初值 MotorB.AccError = 30; //加速度允许偏差 > 加速度 MotorB.MaxOut = 2000; //最大速出 mm/s MotorB.MinOut =-2000; //最大速出 mm/s MotorB.val = 10; //加速度运行周期 * 10ms MotorB.sub = 5; //减速度运行周期 * 10ms MotorB.Init=1; } } /*---------------------应用样例--------------- #include "add_sub.h" //梯形加速 void main(void) { addRunA_Init(); //A行进电机梯形加速程序 addRunB_Init(); //B行进电机梯形加速程序 while(1) { MotorA.Vpoint = dealControl.vInA ; //a电机v赋值 来源于控制目标 MotorB.Vpoint = dealControl.vInB ; //b电机v赋值 来源于控制目标 ADDWork(&MotorA,&TP_10ms.task02); //a行走 电机 梯形加速 ADDWork(&MotorB,&TP_10ms.task03); //b行走 电机 梯形加速 dealControl.vOutA = MotorA.Vout ; //a行走 电机 输出给PID函数 dealControl.vOutB = MotorB.Vout ; //b行走 电机 输出给PID函数 } } */ /* add_sub.h 文件 */ #ifndef __ADD_SUB_H #define __ADD_SUB_H #include "stdio.h" #include "sys.h" //梯形加速模块 typedef struct ADDorLOW{ //INPUT float Vpoint; // float MaxOut; // float MinOut; // //OUTPUT float Vout; // //System char Init; float error; // float AccError; // float Acc; // u16 val; u16 sub; }ADDorLOW; extern ADDorLOW MotorA; extern ADDorLOW MotorB; void ADDWork(ADDorLOW* V,u16 *Time_a); void addRunA_Init(void); void addRunB_Init(void); #endif