STM32 双轮差速里程计计算

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
    odom.y += (sinval * dx + cosval * dy)/1000.f; //m
    odom.yaw += d_yaw;  //rad
}