基于STM32 HAL库的平衡循迹小车技术详解：串级PID控制、MPU移植及电赛应用

1.硬件设计（仅限参考） 

先是原理图如下

主要模块：灰度，电源，mpu（陀螺仪），oled，tb6612，含有编码器的电机

2.mpu模块的使用和移植 

想要使平衡车保持平衡，mpu是最重要的模块，大家可以参考这个博主写的

http://t.csdnimg.cn/ITLI3

如果大家没有时间或者移植失败，也可以直接移植我代码之中的mpu模块：

链接: https://pan.baidu.com/s/1-9Vstj5v0Wgqkm1AFrCt_w?pwd=k8g8 提取码: k8g8

3.代码的主要逻辑

1.通过mpu获得pitch,roll,yaw的值来进行处理  2.获得编码器的数值3.通过处理之后的数据进行电机的控制

1.读取float pitch,roll,yaw;  //欧拉角    short aacx,aacy,aacz;   //加速度传感器原始数据

1..首先讲解cubmax上的配置

这里重点说明一下，mpu的移植代码中应该把其中的位置配置修改为与你的硬件相匹配

对于相应的引脚设置为input就可以了

然后要设置一个定时器中断，以此来随时获得mpu的数值

中断的代码：

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)

{

if(htim->Instance == TIM1)//每10ms进入定时器中断

{

    mpu_dmp_get_data(&pitch,&roll,&yaw);//读欧拉角

MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);//读陀螺仪原始数据

// MPU_Get_Accelerometer(&aacx,&aacy,&aacz);//读取加速度

Encoderleft = getEncoderleft();

Encoderright = getEncoderright();

gray_detect();

balance();

}

}

这里要特别说明一下，大家可以加一下下面这串代码，每次从当前位置获取mpu的零点，初始化成功之后在运行下面的代码，这样大家每次开始都放到同一位置就可以保持平衡了（放到main中）

  while (MPU_Init ())

{

OLED_ShowString (0,0,'mpu_err',12);

OLED_Refresh ();

}

  while(mpu_dmp_init())//初始化dmp，成功返回0

  {

OLED_ShowString(0,16,'error',12);

OLED_Refresh();

  }

到这里，我们应该就可以成功获取mpu的6个值了，大家可以试着坐一坐，这是平衡车最重要的准备工作

3.编码器数值的读取和驱动讲解

通常来说，如果我们只考虑mpu的值的话，即往那边到就往哪边跑，这样平衡效果不好而且平衡时间通常很短，如果要持续保持平衡，那么我们就还需要编码器的数据，这样如果摆动过大，可以以更大的速度保持平衡，在参数调好的情况下可以在原地保持平衡

1.cubmax上的配置

首先是电机的pwm波，这个和以往小车的配置一样

2.编码器数值的获取（单独使用两个定时器）将combined channels设置为encoder mode模式

接下来是代码部分：

获取编码器的数值

int16_t getEncoderleft(void)

{

    int16_t Encoder_left_raw = (short) __HAL_TIM_GET_COUNTER(&htim4); // 获取原始左编码器值

    __HAL_TIM_SET_COUNTER(&htim4, 0);    // 编码器脉冲清0

    return Encoder_left_raw;

}

int16_t getEncoderright(void)

{

    int16_t Encoder_right_raw = (short) __HAL_TIM_GET_COUNTER(&htim3); // 获取原始右编码器值

    __HAL_TIM_SET_COUNTER(&htim3, 0);    // 编码器脉冲清0

    return Encoder_right_raw;

}

驱动代码： 

void Load(int moto1,int moto2)

{

moto1 = moto1 > 5900? 5900: moto1;

moto1 = moto1 < -5900? -5900: moto1;

moto2 = moto2 > 5900? 5900: moto2;

moto2 = moto2 < -5900? -5900: moto2;

if(moto1>0)//Left

{

  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);

  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET);//反转

}

else

{

  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_RESET);//正转

moto1 = -moto1;

}

__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,moto1);

if(moto2>0)//Right

{

  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_RESET);

  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET);//正转

}

else

{

  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET);

  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET);//反转

moto2 = -moto2;

}

__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2,moto2);

}

到这里大家可以看看自己的编码器值知否正确，一般来说转起来后会稳定在几十到几百

如果上面的功能均实现了，那么恭喜你，制作平衡车的所有准备已经准备好了

4.pid的编写

这里使用的是串级pid

#include 'pid.h'

//float Balance_kp = 0,Balance_kd = 0;//0.9

float Balance_kp = 300,Balance_kd = 1.1;//430 0.9

//float speed_kp = 0,speed_ki = 0/200;   //0.064

float speed_kp = -0.007,speed_ki = -0.007/200;   //0.0075

//float turn_kp = 170,turn_kd = 0.5;//165

float turn_kp = -170,turn_kd = 0;

//直立环

float Balance( float target , float roll, float Gyro_x)//俯仰角，目标角度

{

int speed;

speed = Balance_kp*(roll - target)+Balance_kd*Gyro_x;

return speed;

}

//速度环

float speed1(int encoder_left,int encoder_right,int target )

{

static float err_lowout_last,enconder_s;

float a=0.7;

float err ,err_lowout ,speed;

//偏差值

err = (encoder_right + encoder_left) - target;

//低通滤波

err_lowout = (1-a)*err + a*err_lowout_last;

speed = speed_kp*err_lowout+speed_ki*enconder_s;

//积分

enconder_s+=err_lowout;

//限幅

if(enconder_s>20000)  enconder_s=20000;         

if(enconder_s<-20000) enconder_s=-20000; 

err_lowout_last = err_lowout;

return speed;

}

//转向环

//角速度，角度值

float turn(float data)

{

float err=0;

float target=0,result=0;

  static float sum_err =0;

err=data-target;

result=turn_kp*err;//+turn_kd*sum_err;

// if(result > 2500 ) result = 2500;

// if(result < -2500 ) result = -2500;

// sum_err+=err;

return result;

}

详细调参和代码讲解请参考：

【草履虫都能学会的STM32平衡小车教程（软件篇）】 https://www.bilibili.com/video/BV1zx4y1y7ZR/?share_source=copy_web&vd_source=495bd57f5ed50cfefc6cd2d6b1bd3f25

最后是我的代码部分，大家可以参考一下

链接: https://pan.baidu.com/s/1-9Vstj5v0Wgqkm1AFrCt_w?pwd=k8g8 提取码: k8g8