本文将概述常见机器人底盘的电机及其编码器并结合Breakout 36.0中STM32的参考代码，对SLAMWARE系统中用到的里程计进行说明。

仅以两轮差动电机为例，三轮全向轮电机请参考KBSW180145 基于Slamware解决方案的服务型机器人底盘电机里程计概述及代码示例KBSW180148 Instruction for Integrating SLAMWARE Solution in Tri-omini-wheeled Base

typedef struct _base_set_motorvelocity_request
{    
    _s32 velocity_x_q16;
    _s32 velocity_y_q16;
    _s32 motorangular_speed_mm[4]velocity_q16;
} __attribute__((packed)) base_set_motorvelocity_request_t;

typedef struct _base_deadreckon_response
{
    case SLAMWARECORECB_CMD__s32 base_dx_mm_q16;
   _s32 base_dy_mm_q16;
   _s32 base_dtheta_degree_q16;
   _u8 status_bitmap; 
} __attribute__((packed)) base_deadreckon_response_t;

(int32_t)bumpermonitor_clamp_motorcmd(speed_l_mm), (int32_t)bumpermonitor_clamp_motorcmd(speed_r_mm));
        if(speed_l_mm != 0)
        {
            last_motor_l_speed = (int)speed_l_mm;
        }
        if(speed_r_mm != 0)
        {
            last_motor_r_speed = (int)speed_r_mm;
        }
        ans_pkt.status_bitmap |= (is_ontheground()?0:BASE_MOTOR_TRACTION_LOST);  // 该字段用以报告电机异常状态，用户可根据自身需求参考代码添加判断比如判断电机是否离地等，如无异常请务必都填0即ans_pkt.status_bitmap = 0;
        net_send_ans(channel, &ans_pkt, sizeof(base_deadreckon_response_t));
     }
     break;

//每米编码器脉冲数
#define ODOMETER_EST_PULSE_PER_METER  6390UL


//行走电机速度控制频率：60hz
#define CONF_MOTOR_HEARTBEAT_FREQ     60
#define CONF_MOTOR_HEARTBEAT_DURATION (1000/(CONF_MOTOR_HEARTBEAT_FREQ))


/*
 * 刷新行走电机的里程数据函数
 */
static void _refresh_walkingmotor_odometer(_u32 durationMs)
{
    _u8 cnt;
    float dist_mm;    // disable interrupt   
    _u32 irqSave = enter_critical_section();                                    //临界资源保护
    for (cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt)                       //得到这段时间内的编码器数据
    {
        _lastEncoderTicksDelta[cnt] = _encoderTicksDelta[cnt];
        _encoderTicksDelta[cnt] = 0;
    }
    leave_critical_section(irqSave);

    if (durationMs ==           0)                                                        //临界资源保护防止除零
        durationMs = 1;

    for (size_t cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt)                       {//根据这段时间内的编码器数据计算这段时间内速度，即当前速度
    {
        dist_mm = (float)_lastEncoderTicksDelta[cnt] * (1000.0/ODOMETER_EST_PULSE_PER_METER);
        _lastOdometerSpeedAbs[cnt] = dist_mm * 1000.0 / durationMs;
        dist_mm      
        _lastEncoderTicksDelta+= _motorDistTailing[cnt];
        _motorDistAccumulated[cnt] += _encoderTicksDelta(_u32)dist_mm;
        _motorDistTailing[cnt]; = dist_mm                //获得delta时间内编码器的脉冲数
        _motorAccumulatedTicks- (_u32)dist_mm;
        _motorDeltaTicks[cnt] += _encoderTicksDeltalastEncoderTicksDelta[cnt];
    }
}

/*
 * 计算左行走电机里程数据
 */
float walkingmotor_delta_ldist_mm_f(void)
{
    _u32 delta_dist = _motorDeltaTicks[WALKINGMOTOR_LEFT_ID];        //获得累计编码器的脉冲数
        _encoderTicksDelta[cnt计算左行走电机的累计编码器值
    _motorDeltaTicks[WALKINGMOTOR_LEFT_ID] = 0;
    return delta_dist  }
    leave_critical_section(irqSave);
    if (durationMs == 0)                                                        //防止除零
        durationMs = 1;
    for (size_t cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt) {                       //根据delta的编码器数据计算这段时间内速度，即当前速度
        _lastOdometerSpeedAbs[cnt] = (float) _lastEncoderTicksDelta[cnt] * (1000.0 / ODOMETER_EST_PULSE_PER_METER) * 1000.0 / durationMs;
    }
}
/*
 * 计算左行走电机累计里程函数
 * 单位：mm
 */
_u32 cumulate_walkingmotor_ldist_mm(void)
{
    return (_motorAccumulatedTicks[WALKINGMOTOR_LEFT_ID] * 1000) / ODOMETER_EST_PULSE_PER_METER;
}
/*
 * 计算右行走电机累计里程函数
 * 单位：mm
 */
_u32 cumulate_walkingmotor_rdist_mm(void)
{
    return (_motorAccumulatedTicks[WALKINGMOTOR_RIGHT_ID] * 1000) / ODOMETER_EST_PULSE_PER_METER;
}

* (1000.f / ODOMETER_EST_PULSE_PER_METER);
}


/*
 * 计算右行走电机里程数据
 */
float walkingmotor_delta_rdist_mm_f(void)
{
    _u32 delta_dist = _motorDeltaTicks[WALKINGMOTOR_RIGHT_ID]; ////计算右行走电机的累计编码器值
    _motorDeltaTicks[WALKINGMOTOR_RIGHT_ID] = 0;
    return delta_dist * (1000.f / ODOMETER_EST_PULSE_PER_METER);
}



#ifdef FEATURE_SET_V
static const float robot_radius_mm = 118.f;
static bool is_first_motor_data = true;
static int last_motor_l_speed = 0;
static int last_motor_r_speed = 0;


/*
 *通过里程累积计算位移量
 */
static void calculateMotorStatus(base_deadreckon_response_t* ans_pkt)
{
    memset(ans_pkt, 0, sizeof(*ans_pkt));
    float d_dist_l_mm_f = walkingmotor_delta_ldist_mm_f();  //计算左行走电机累计里程
    float d_dist_r_mm_f = walkingmotor_delta_rdist_mm_f();  //计算右行走电机累计里程
    
    if(is_first_motor_data)
    {
       is_first_motor_data = false;
       ans_pkt->base_dx_mm_q16 = 0;
       ans_pkt->base_dy_mm_q16 = 0;
       ans_pkt->base_dtheta_degree_q16 = 0;
    }
    else
    {
        if(last_motor_l_speed < 0)
        {
            d_dist_l_mm_f = -d_dist_l_mm_f;
        }
        if(last_motor_r_speed < 0)
        {
            d_dist_r_mm_f = -d_dist_r_mm_f;
        }
        float d_yaw = (d_dist_r_mm_f - d_dist_l_mm_f) / 2.0f / robot_radius_mm;
        float displacement = (d_dist_l_mm_f + d_dist_r_mm_f) / 2.0f;
        
        float dx = cos(d_yaw)*displacement;
        float dy = sin(d_yaw)*displacement;
        
        ans_pkt->base_dx_mm_q16 = (_s32)(dx * (1<<16));
        ans_pkt->base_dy_mm_q16 = (_s32)(dy * (1<<16));
        ans_pkt->base_dtheta_degree_q16 = (_s32)(d_yaw / M_PIf * 180 * (1<<16));
           }
   }
#endif