计算里程计

const double PI = 3.14159265358979323846;  
double yiquan=650;
double ban_jing=0.0335;
double zspeed;
double yspeed;

int velocity_PID_valuez(int velocity_measure,int velocity_calcu);
int velocity_PID_valuey(int velocity_measure,int velocity_calcu);
void Motor_Control(int pwm1,int pwm2)
{
    if(pwm1>0)
    {
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET);
    }
    if(pwm1<0)
    {
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_SET);
    }
    
    if(pwm2>0)
    {
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_SET);
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET);
    }
    if(pwm2<0)
    {
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_SET);
    }
    __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_1,pwm1);
    __HAL_TIM_SetCompare(&htim2, TIM_CHANNEL_2,pwm2);
}
int encoder_left;//×ó±àÂëÆ÷µç»úËÙ¶È
int iencoder_left;//×ó±àÂëÆ÷ËٶȻý·Ö
int encoder_right;//ÓÒ±àÂëÆ÷µç»úËÙ¶È
int iencoder_right;//ÓÒ±àÂëÆ÷ËٶȻý·Ö
float distance;
int cout;
int speedz,speedy;
int mubiao=3;
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    static uint8_t key_count,oled_count; 
     if(htim->Instance == TIM1)   //10ms
     {
         
             encoder_left = -(short)(__HAL_TIM_GET_COUNTER(&htim3));        
            iencoder_left+=encoder_left;
          encoder_right = (short)(__HAL_TIM_GET_COUNTER(&htim4));        
            iencoder_right+=encoder_right;
            //distance=(iencoder_left/36.0);
          zspeed=(encoder_left*(2*PI*ban_jing))/6.5;
          yspeed=(encoder_right*(2*PI*ban_jing))/6.5;//m/s
          distance+=((zspeed+yspeed)/2)*0.01;
          speedz=velocity_PID_valuez(zspeed,mubiao);
          speedy=velocity_PID_valuey(yspeed,mubiao);
          if(distance>1){
                mubiao=0;
                Motor_Control(0,0);
            }else{
             Motor_Control(speedz,speedy);
            }
          
          __HAL_TIM_SET_COUNTER(&htim3,0);
          __HAL_TIM_SET_COUNTER(&htim4,0);
          HAL_TIM_Base_Start_IT(&htim1);
     }
}