最近在想,使用查表方式实现的S加减速就只能控制速度,不能控制位置,能否实现一个查表,可以同个脉冲的位置查找当前的速度?
比方说提供一个函数 给定 开始速度 ,结束速度,总步数, 当前步数, 即可查找到 当前步数对应的速度?
抛砖引玉,请问各位如何做一个根据当前步数实时计算S加减速的速度的方法?
我做了一个S曲线的的表格,表格是根据当前步数查找当前的速度,使用一下函数可实现该功能,但是该表格只能应用于升速中:初速度为0 的情况或者降速中:末尾速度为0的情况。
const uint16_t sigmoid_speed[1025] = { 1,384,765,1145,1522,1897,2269,2640,3008,3375,3739,4101,4461,4818,5174,5528,5879,6228,6576,6921,7265,7606,7945,8283,8618,8952,9283,9613,9940,10266,10590,10912,11232,11550,11866,12181,12494,12804,13113,13421,13726,14030,14331,14632,14930,15226,15521,15814,16106,16396,16684,16970,17255,17538,17819,18099,18377,18653,18928,19201,19473,19743,20011,20278,20544,20807,21070,21330,21589,21847,22103,22358,22611,22863,23113,23362,23609,23855,24100,24343,24584,24824,25063,25300,25536,25771,26004,26236,26467,26696,26924,27150,27375,27599,27822,28043,28263,28482,28699,28915,29130,29344,29556,29767,29977,30186,30393,30600,30805,31008,31211,31413,31613,31812,32010,32207,32402,32597,32790,32983,33174,33364,33553,33740,33927,34113,34297,34481,34663,34844,35024,35204,35382,35559,35735,35910,36084,36257,36429,36600,36770,36939,37107,37274,37440,37605,37769,37932,38094,38256,38416,38575,38734,38891,39048,39204,39358,39512,39665,39817,39968,40119,40268,40417,40564,40711,40857,41002,41147,41290,41433,41574,41715,41855,41995,42133,42271,42408,42544,42679,42813,42947,43080,43212,43343,43474,43604,43733,43861,43989,44116,44242,44367,44492,44615,44739,44861,44983,45104,45224,45344,45463,45581,45698,45815,45931,46047,46161,46276,46389,46502,46614,46725,46836,46946,47056,47165,47273,47381,47488,47594,47700,47805,47910,48013,48117,48219,48322,48423,48524,48624,48724,48823,48922,49020,49117,49214,49310,49406,49501,49596,49690,49783,49876,49969,50061,50152,50243,50333,50423,50512,50601,50689,50777,50864,50950,51036,51122,51207,51292,51376,51460,51543,51626,51708,51789,51871,51951,52032,52112,52191,52270,52348,52426,52504,52581,52657,52734,52809,52885,52959,53034,53108,53181,53254,53327,53399,53471,53543,53614,53684,53754,53824,53894,53962,54031,54099,54167,54234,54301,54368,54434,54500,54565,54630,54695,54759,54823,54886,54950,55012,55075,55137,55199,55260,55321,55381,55442,55502,55561,55620,55679,55738,55796,55854,55911,55968,56025,56082,56138,56194,56249,56304,56359,56414,56468,56522,56576,56629,56682,56734,56787,56839,56891,56942,56993,57044,57095,57145,57195,57244,57294,57343,57392,57440,57488,57536,57584,57631,57678,57725,57772,57818,57864,57910,57955,58001,58046,58090,58135,58179,58223,58266,58310,58353,58396,58438,58481,58523,58565,58607,58648,58689,58730,58771,58811,58851,58891,58931,58971,59010,59049,59088,59126,59165,59203,59241,59279,59316,59353,59390,59427,59464,59500,59536,59572,59608,59644,59679,59714,59749,59784,59819,59853,59887,59921,59955,59988,60022,60055,60088,60120,60153,60185,60218,60250,60281,60313,60344,60376,60407,60438,60468,60499,60529,60559,60589,60619,60649,60678,60708,60737,60766,60795,60823,60852,60880,60908,60936,60964,60992,61019,61046,61073,61100,61127,61154,61181,61207,61233,61259,61285,61311,61336,61362,61387,61412,61437,61462,61487,61512,61536,61560,61585,61609,61632,61656,61680,61703,61727,61750,61773,61796,61818,61841,61864,61886,61908,61930,61952,61974,61996,62018,62039,62060,62082,62103,62124,62145,62165,62186,62206,62227,62247,62267,62287,62307,62327,62347,62366,62386,62405,62424,62443,62462,62481,62500,62519,62537,62556,62574,62592,62610,62628,62646,62664,62682,62699,62717,62734,62752,62769,62786,62803,62820,62837,62853,62870,62886,62903,62919,62935,62951,62968,62983,62999,63015,63031,63046,63062,63077,63092,63108,63123,63138,63153,63168,63182,63197,63212,63226,63241,63255,63269,63283,63297,63311,63325,63339,63353,63367,63380,63394,63407,63421,63434,63447,63460,63473,63486,63499,63512,63525,63537,63550,63563,63575,63587,63600,63612,63624,63636,63648,63660,63672,63684,63696,63707,63719,63730,63742,63753,63765,63776,63787,63798,63809,63820,63831,63842,63853,63864,63875,63885,63896,63906,63917,63927,63937,63948,63958,63968,63978,63988,63998,64008,64018,64028,64037,64047,64057,64066,64076,64085,64095,64104,64113,64123,64132,64141,64150,64159,64168,64177,64186,64195,64203,64212,64221,64229,64238,64246,64255,64263,64272,64280,64288,64296,64305,64313,64321,64329,64337,64345,64353,64361,64368,64376,64384,64391,64399,64407,64414,64422,64429,64436,64444,64451,64458,64466,64473,64480,64487,64494,64501,64508,64515,64522,64529,64536,64542,64549,64556,64562,64569,64576,64582,64589,64595,64602,64608,64614,64621,64627,64633,64639,64646,64652,64658,64664,64670,64676,64682,64688,64694,64699,64705,64711,64717,64722,64728,64734,64739,64745,64751,64756,64762,64767,64772,64778,64783,64789,64794,64799,64804,64810,64815,64820,64825,64830,64835,64840,64845,64850,64855,64860,64865,64870,64874,64879,64884,64889,64893,64898,64903,64907,64912,64917,64921,64926,64930,64935,64939,64943,64948,64952,64957,64961,64965,64969,64974,64978,64982,64986,64990,64995,64999,65003,65007,65011,65015,65019,65023,65027,65031,65034,65038,65042,65046,65050,65053,65057,65061,65065,65068,65072,65076,65079,65083,65086,65090,65094,65097,65101,65104,65107,65111,65114,65118,65121,65124,65128,65131,65134,65138,65141,65144,65147,65151,65154,65157,65160,65163,65166,65169,65172,65175,65179,65182,65185,65187,65190,65193,65196,65199,65202,65205,65208,65211,65214,65216,65219,65222,65225,65227,65230,65233,65236,65238,65241,65244,65246,65249,65251,65254,65257,65259,65262,65264,65267,65269,65272,65274,65277,65279,65282,65284,65286,65289,65291,65294,65296,65298,65301,65303,65305,65307,65310,65312,65314,65316,65319,65321,65323,65325,65327,65329,65332,65334,65336,65338,65340,65342,65344,65346,65348,65350,65352,65354,65356,65358,65360,65362,65364,65366,65368,65370,65372,65374,65376,65377,65379,65381,65383,65385,65387,65388,65390,65392,65394,65395,65397,65399,65401,65402,65404,65406,65408,65409,65411,65413,65414,65416,65417,65419,65421,65422,65424,65425,65427,65429,65430,65432,65433,65435,65436,65438,65439,65441,65442,65444,65445,65447,65448,65450,65451,65452,65454,65455,65457,65458,65459,65461,65462,65464,65465,65466,65468,65469,65470,65472,65473,65474,65475,65477,65478,65479,65481,65482,65483,65484,65485,65487,65488,65489,65490,65492,65493,65494,65495,65496,65497,65499,65500,65501,65502,65503,65504,65505,65506,65508,65509,65510,65511,65512,65513,65514,65515,65516,65517,65518,65519,65520,65521,65522,65523,65524,65525,65526,65527,65528,65529,65530,65531,65532
};
static double lookup_step = 1024;
static int32_t total_counts = 1024;
static int32_t v_start = 0;
static int32_t v_end = 1;
static int32_t v_half = 0;
static int64_t v_delta = 0;
static int32_t step_half = 0;
void sigmoid_init(uint32_t vs, uint32_t ve, uint32_t step_event_count)
{
v_start = vs;
v_end = ve;
v_half = (v_start + v_end) / 2;
lookup_step = 1024.0f / step_event_count;
v_delta = (int64_t)(v_end - v_start);
step_half = (step_event_count + 1) / 2;
total_counts = step_event_count;
}
uint32_t sigmoid_get_speed_by_dstance(uint32_t steps)
{
if (steps >= total_counts) return v_end;
if (steps == 0) return v_start;
int32_t speed, st;
if(v_delta>0)
{
st = steps * 1024 / (total_counts );
speed = (v_delta*sigmoid_speed[st]) >> 16;
speed += v_start;
if (speed < 0) speed = 0;
}
else
{
st = steps * 1024 / (total_counts);
speed = (v_delta*sigmoid_speed[1024-st]) >> 16;
speed = v_end- speed;
if (speed < 0) speed = 0;
}
return speed;
} |