很早以前在RTT上做的S型电机加减速表计算,为了配合理解,做了一个EXCEL的数学模型传上来:
EXCEL文件:
注释已经很详细了,呵呵,我就不多说了!在RTT平台上实现的,虽然跟飞思卡尔没关系,但是看到有很多做电机驱动的,就发出来给大家!
做这段代码的时候用到高中物理中的知识,发点感慨,人不知道什么时候学的东西就能用的上,呵呵!
#ifndef __SPEED_H__ #define __SPEED_H__ #include "rtthread.h" #include "finsh.h" /* 三分之一加减速法则: 加速路程不超过三分之一总路程, 这样做的目的是给减速留有足够的时间去检测减速点, 进而实现平稳的将速度减到初速度, 如果在处理速度够快的情况下, 可以选择二分之一加减速 */ //加减速法则,2分之一法则或3分之一法则 #define SPEEDPRINCIPLE 3 //电机最大转速,单位转/min #define MOTOMAXSPEED 1500 //电机每转对应的传动距离,单位:mm #define MOTOROUNDLENGTH 36 //电机最大初速度,初速度超过该值会对电机造成伤害,单位转/min #define MOTOMAXSTART 200 //驱动器细分数 #define DRIVERSFRACTION 1 //电机细分数 #define MOTOFRACTION 200 //根据初速度,末速度,以及时间,计算加速路程 #define ACCELERATESPACE(V0,Vt,t) (((V0) + (Vt)) * (t) / 2) //根据加速路程,初速度,以及时间,计算末速度 #define LASTVELOCITY(S,V0,t) (2 * (S) / (t) - (V0)) //根据电机转速(转/min),计算电机步速度(step/s) #define STEPSPEED(RV) ((RV) * MOTOFRACTION * DRIVERSFRACTION / 60) //根据长度计算电机所需走的步数,S的单位为mm #define TOTALSTEP(S) S * MOTOFRACTION * DRIVERSFRACTION / MOTOROUNDLENGTH struct SPEED { int V0; //初速度,单位:step/s int Vt; //末速度,单位:step/s long long S; //路程,单位:step double t; //加速时间,单位:s int time; //加速次数 单位:次 int *SpeedTab; //加速速度表,速度单位:step/s int SecSpeedPoint; //减速点 单位:step(在电机运动过程中,如果剩余路程小于等于该值,那么电机开始减速) }; typedef struct SPEED * Speed_t; //各参数范围值,可以在此设置参数范围 //初速度大于0小于电机最高起速,末速度大于初速度,小于电机最高转速 #define IS_SPEED(V0,Vt) (((V0) >= 0) && ((V0) <= STEPSPEED(MOTOMAXSTART)) \ && ((Vt) >= (V0)) && ((Vt) <= STEPSPEED(MOTOMAXSPEED))) #define IS_SPACE(S) ((S) > 0) #define IS_ADDTIMING(t) ((t) > 0) #define IS_ADDTIME(time) ((time) >= 32) //让加速次数必须大于等于32,否则计算表格就没什么意义 #endif #include "Speed.h" //函数名称: CalculateSpeedTab //函数功能:计算加减速表 //参数参数:Speed 速度结构体 // V0; //初速度,单位:step/s // Vt; //末速度,单位:step/s // S; //路程,单位:step // t; //加速时间,单位:s // time; //加速次数 单位:次 // *SpeedTab; //加速速度表,速度单位:step/s // SecSpeedPoint; //减速点 单位:step //返回输出:速度结构体 static Speed_t CalculateSpeedTab(Speed_t Speed) { int i; double aa; //加加速 int DeltaV; //速度变化量 int tempVt; //如果速度记录表的指针为空,说明没有分配内存 RT_ASSERT(Speed->SpeedTab != RT_NULL); //各参数是否超范围 RT_ASSERT(IS_SPEED(Speed->V0, Speed->Vt)); RT_ASSERT(IS_SPACE(Speed->S)); RT_ASSERT(IS_ADDTIMING(Speed->t)); RT_ASSERT(IS_ADDTIME(Speed->time)); //根据加速法则计算末速度 tempVt = LASTVELOCITY(Speed->S / SPEEDPRINCIPLE, Speed->V0, Speed->t); Speed->Vt = (tempVt > Speed->Vt) ? Speed->Vt : tempVt; Speed->SecSpeedPoint = ACCELERATESPACE(Speed->V0, Speed->Vt, Speed->t); //计算减速点位置,剩余路程为该值时开始减速 aa = (double)((Speed->Vt - Speed->V0) / 2) //加速度变化拐点时的速度值 a| /|\ * 2 //知道直角三角形面积,逆推三角型高度时将面积*2 | / | \面积为DeltaV / (Speed->time / 2) //除以底边 | / | \ / (Speed->time / 2); //再除以底边,得到斜率,即加加速 |/___|___\__time //开始速度计算 for(i = 0; i < ((Speed->time / 2) + 1); i++) { DeltaV = (aa * i * i) / 2; //V = V0 + a * t / 2; a = aa * t; *(Speed->SpeedTab + i) = Speed->V0 + DeltaV; //当前点的速度 *(Speed->SpeedTab + Speed->time - i) = Speed->Vt - DeltaV; //对称点的速度 } return Speed; } //函数名称:TestSpeed //函数功能:测试速度表计算函数 //函数参数:V0 初速度,单位:转/min // Vt 末速度,单位:转/min // S 路程,单位:mm // time 加速次数 //函数返回:无 void TestSpeed(int V0, int Vt, int S, int time) { int i; Speed_t Speed = (Speed_t)rt_malloc(sizeof(struct SPEED)); Speed->V0 = STEPSPEED(V0); //起速 Speed->Vt = STEPSPEED(Vt); //末速 Speed->S = TOTALSTEP(S); //路程 Speed->t = 0.2; //加速时间设为0.2秒 Speed->time = time; //加速次数 Speed->SpeedTab = (int *)rt_malloc(sizeof(int) * (Speed->time + 1)); //根据加速次数申请表格内存,加1,以防用的时候超界 CalculateSpeedTab(Speed); //开始计算 for(i = 0; i < Speed->time; i++) { rt_kprintf("SpeedTab[%d] = %d, Acceleration = %d\n" , i , *(Speed->SpeedTab + i) , *(Speed->SpeedTab + i + 1) - *(Speed->SpeedTab + i)); } rt_kprintf("SpeedTab[%d] = %d\n", i, *(Speed->SpeedTab + i)); rt_free(Speed->SpeedTab); rt_free(Speed); } FINSH_FUNCTION_EXPORT(TestSpeed, Test Speed table calculate);
最新回复 [0]