STM32 双轮差速里程计计算
ODOMETER_EST_PULSE_PER_METER 表示每米距离需要的脉冲数目,参考电机里程计属性设置
#define ODOMETER_EST_PULSE_PER_METER 1320 //通常11线
#define ODOMETER_EST_PULSE_PER_METER 1560 //通常13线
R_param 表示两个轮子分别到双轮中心的距离
leftcnt/ rightcnt 表示左轮在固定时间脉冲计数值,前进为正数,后退为负数。
/*
* 固定一段时间(10ms or 20ms)调用
*/
void odom_cal_trigger(s32 leftcnt, s32 rightcnt)
{
float dl = 0.0, dr = 0.0;
float sinval = 0.0, cosval = 0.0;
dl = leftcnt * (1000.f / ODOMETER_EST_PULSE_PER_METER); // mm
dr = rightcnt * (1000.f / ODOMETER_EST_PULSE_PER_METER); // mm
float d_yaw = (dr - dl) / 2.0f / R_param; //rad
float displacement = (dl + dr) / 2.0f;
float dx = cos(d_yaw)*displacement; //mm
float dy = sin(d_yaw)*displacement; //mm
sinval = sin(odom.yaw), cosval = cos(odom.yaw);
odom.x += (cosval * dx - sinval * dy)/1000.f; //m