DIY平衡小车

前期基本方案

预期效果

基本目标

• 根据指令实现精准的速度控制
• 自身坐标和姿态的解算
• 与上位机传输数据

更有意思的目标

• 双轮平衡车
• 控制雷达平稳

电机驱动

​ 电机使用12V供电，可以直接由电源模块供电。使用TB6612驱动模块可以满足两个电机的驱动需求。

位姿测量

​ 使用MPU6050可以获取角速度和加速度值（但是之前试用的时候发现其自带的库位姿解算会有不少时间的初始化延迟）。

​ MPU6050使用IIC通信，在获得了新的测量之后会在INT脚输出高电平，为了保证数据的即时性，将一个GPIO配置为外部中断，在中断函数里更新数据。

轮速获取

​ 将两个定时器配置为编码器接口模式，分别接收两个电机对应的编码器数据。我们使用的是AB双相输入的编码器，参考手册：

如果计数器只在TI2的边沿计数，则置TIMx_SMCR寄存器中的SMS=001；如果只在TI1边沿计数，则置SMS=010；如果计数器同时在TI1和TI2边沿计数，则置SMS=011。

对寄存器的配置已经被封装在库函数TIM_EncoderInterfaceConfig（ ）里了，只需要选择配置模式3即可，在这个情况下，TI1和TI2分别对应TIMx的channel1和channel2的引脚，默认情况下TIM2就是PA0 PA1,TIM3是PA6 PA7。

也可以与MPU6050传感器数据融合。

运动控制

直立环

直立环使小车保持直立，即保持在机械中值θ的位置，理想情况下是保持0°。

$$$PWM_{balance} = KP * θ + KD * \Delta θ$$

速度环

​ 根据上位机发布的速度指令或者由位置环计算出的目标速度计算出输出力矩。

$$PWM_{speed} = KP_s * Error + KD_s * \Delta Error$$

速度环（平衡车）

​ 但是由于安装等外界因素影响，小车以为的0°并非真正的平衡态，于是我们引入了速度环。当小车速度稳定时，便可以认为小车进入平衡态。速度环的输出可以看作是一个角度，作为直立环的输入，直立环便演变成了使小车保持一定角度，在保持角度的时候便会产生速度。

$$balance \_ IN \_ speed = KP_S * Error + KD_S * \Delta Error$$ $$PWM \_ balance \_ speed=KP * (θ + balance \_ IN \_ speed) + KD * \Delta θ$$

合并整理后便可得到

$$PWM \_ balance \_ speed = KP * θ + KD * \Delta θ + KP（ KP_S * Error + KD_S * \Delta Error）$$

两者其实是线性相加的，直接将KP KP_S合并为KP_S，KP KD_S合并为KD_S，最终简化为

$$PWM_{balance} = KP * \theta + KD * \Delta \theta$$ $$PWM_{speed} = KP_s *Error + KD_s * \Delta Error$$ $$PWM = PWM_{speed} + PWM_{balance}$$

转向环

​ 转向环比较简单，直接根据角度输出一个速度差，用PD控制即可。

控制雷达平稳

​ 根据车子姿态控制舵机角度以尽可能保证雷达平稳。

运动模型

​ 设：车子速度为v ； 两轮速度分别为v1，v2 ；两轮间距为d ；速度瞬心为点 P ； 速度瞬心离两轮距离分别为 r1 , r2 ；车身角速度为 w ； 车子运动轨迹半径为R ；逆时针转为正方向

​ 可以算得：

$$v =\frac{1}{2} ( v_1 + v_2 )$$ $$\omega = \frac{v_2 - v_1}{d}$$

​

$$R = d\frac{v_1 + v_2}{2(v_2 - v_1)}$$

目前的想法是建立一个世界坐标系和一个车子坐标系。

• 在车子坐标系中可以轻松地计算出下一时刻车子的位置和姿态 ；
• 而通过这一时刻车子的位置和姿态又可以得到此时车子坐标系与世界坐标系的变换矩阵；
• 利用变换矩阵可以将车子坐标系中下一时刻的位姿转换到世界坐标系。

​ 如此便可以完成状态递推。

$$\Delta \theta_k = \frac{v_{k-1}}{R_{k-1}} \Delta t$$ $$\Delta x_k = R_{k-1} (1 - \cos {\Delta \theta_k})$$ $$\Delta y_k = R_{k-1} \sin{\Delta \theta_k}$$ $$x_k = x_{k-1} + \Delta x_k \sin{\theta_{k-1}} + \Delta y_k \cos{\theta_{k-1}}$$ $$y_k = y_{k-1} - \Delta x_k \cos{\theta_{k-1}} + \Delta y_k \sin{\theta_{k-1}}$$ $$\theta_k = \theta_{k-1} + \Delta \theta_k$$

结构设计

STM32代码

运动控制调试

​ 调试里程计正解出位姿的时候，由于方向不一致，所以需要修改读取速度的极性。不过改完之后一定要记得把几个控制函数里的极性也做修改。比如角度控制环可能变成正反馈，速度环又可能变成逆反馈。

串口调试

​ DEBUG或者远程控制的时候选择使用串口外设。其中用到了三个串口，可以根据情况选择使用。

接收中断问题

​ 程序主要由一个核心中断函数构成，在调试的过程中我发现由于这个中断函数的存在，串口通讯会经常出现通讯错误。显然是因为串口中断函数和这个核心中断函数冲突了，不过是中断优先级配置的问题。

​ 可是我发现了这样一个问题，USART_IRQn配置为0x00,0x00，EXTI9_5_IRQn配置为0x00,0x01会严重影响串口数据接收的正确性，但是将后者配置为0x01,0x01就不会。同样是优先级低于串口，为什么改变了后者的**抢断优先级**就会产生这样的区别呢？

​ 当中断优先级配置完成后，若多个中断同时发生则先比较抢占优先级谁的值小谁先进行，若抢占优先级相同则比较子优先级谁的值小谁先进行，若两者都相同，就比较硬件中断编号（该编号由硬件自身决定），值得注意的是当一中断正在进行时又一中断发生若后者的抢占优先级的值比前者小则后者可以打断正在发生的中断执行后者的中断进程，需要注意 若后者的抢占优先级与前者相等但子优先级的值比前者小此时后者不能打断正在发生的中断, 必须等其完成后才能进行， 至于后者的优先级比正在发生的中断优先级低的情况，那就肯定是等待正在进行的中断完成后再进行。

​ 所以说“抢占”一词，还是比较生动的。

​ 当然，如果直接用DMA的话就没这些问题了。

USART2_DMA

​ 在DMA初始化函数的最后，设置的TX与RX的开闭状态是不一样的，为什么呢？在理顺了具体的DMA收发过程之后便很容易理解了。

发送

判断DMA是否处于发送完毕的空闲状态。flag=1，正在发送

设置发送数据的地址与大小

CMD使能开始发送

（可执行其他操作）

DMA发送完成中断

清除标志位（包括flag）

CMD失能关闭通道

接收

（DMA事先保持开启）

串口输入，DMA同时转运数据

传输完毕，串口空闲中断

清除标志位

CMD失能关闭DMA通道

获取DMA缓存中数据长度与数据

（切换接收缓冲器）

更新DMA剩余缓存（CNDTR）

CMD使能开启通道等待接收

sendbyte(u8 t)，串口会以对应的字符显示

USART1_DMA

​ 因为usart2的引脚可能要拿去做别的，给USART1也配个DMA。

给usart1配置DMA，想用这些控制代码编译的,这样只用改一个宏定义就可以选择配置或不配置DMA，但是出现了莫名其妙的错误，先不用了。

#define USART1_DMA 1
#if
#else
#endif

​ 配置了DMA后，串口1的printf就不能用了，会卡死循环。不过可以根据自己的通讯需求写几个函数，用起来反而更加方便。

sizeof( )

返回的是不算结束符的字节数，而不是位数。如

char str[5] = "1234";
则
//printf_s(char *s, u8 newline)
{
    int size = sizeof(s) == 4;

​ 我天真的以为这是字符串的大小，我是后来我发现这个size一直是4，我愕然，这其实是指针的大小。

malloc(n)

n也是分配的字节数，所以也常常会看到

something_t *p = (something *)malloc(sizeof(something_t));

sprintf( )

会直接覆盖原字符串，而不是向末尾添加，如

char str[5] = "1234";
sprintf(s,"\r\n");
则
s[0]='\r', s[1]='\n', s[2]='\0', s[3]='4';

strcat( ) & strncat( )

参数传递

//其它位调用
char str[5] = "1234";
printf_s(str,1);//方法一
printf_s("1234",1);//方法二
//printf_s(char *s, u8 newline)
{
    int size = sizeof(s);
    ......
    strcat(s,"\r\n");
    sprintf(s,"1234\r\n");
}

​ 方法一可以正常运行，但是太麻烦；方法二会让32停止运行，个人认为是因为传递过去的是一个常量，不能够修改。但是size能正常返回，如果不尝试对数据进行修改，也可以正常打印。

​ 想换行的话，如果printf_s("1234\r\n")，sizeof(s)返回的值依然是4，比较奇怪。为了调用起来方便，干脆直接新建一个字符串指针算了。

我在debug里面一步步进行就没有问题，结果全速运行的时候就出问题，我真的栓q了。

​

​ 发送字符串的时候计算len的时候别忘了多算一个，是留给结束符的空间。如果没有算上的话，串口打印出来会有一些乱码，加上之后就没有了，应该是这个原因吧。

蓝牙SPP

基本使用方法

手机发消息的时候别忘了加回车

调试

​ 之前UART2，UART3都是没问题的，但是今天调试的时候发现插在UART3上面，连接到蓝牙的时候单片机就会停止运行。

​ DEBUG发现竟然卡在15_10的中断函数里面，查询手册发现UART3的RX是PB11，而这个代码正好把PB11配置为了超声波模块的中断引脚，中断函数里会等待超声波模块回复的高电平，导致一直在等待。一般这两个功能是没有一起使用的必要的，所以直接关闭其中一个即可。

​ 但是还有一个问题，就是串口接收到的数据解析出现错误。结果是因为USART3_RX_BUF写成了复制的USART_RX_BUF没改过来😂。

通讯协议

USART1——STM32与ROS

消息类型

#define WARN_MSG 1 //warn
#define SPD_MSG  2 //speed
#define POS_MSG  3 //position
#define PARA_MSG 4 // param
#define DES_MSG  5 //destination
#define ACK_MSG  6 //ACK
#define INFO_MSG 7 //

DES_MSG

stm32发送目的地及当前位置

示例：”5+1234-4321+1111-2222+3333”

• 帧头：5
• 目的地横坐标+1234：1.234m
• 目的地纵坐标-4321：4.321m
• 当前横坐标+1111：1111mm
• 当前纵坐标-2222：2222mm
• 当前朝向+3333：弧度制3.333(不会超过±π)
• 结束符：\r\n

POS_MSG

stm32发送当前坐标与朝向角度

示例：“3+0282+9999+0000”

• 帧头

ROS发送需要到达的位置

SPD_MSG

ROS发送小车运动指令

运动消息类型

• for_bac=1：前后
• turn=2：转向

示例：”21+1325\r\n”

• 帧头：2
• 运动类型1：直线速度为13.25
• value+1325：

ACK

ROS发送ACK表示已经接收DES_MSG

示例：”6\r\n”

• 帧头：6
• 结束符：\r\n

USART3——STM32与蓝牙模块

帧头不能用简单的字符，因为蓝牙模块在初始化的时候会发送大量的信息。

DES_MSG

手机通过蓝牙SPP发送小车目的地

示例：66612344321\r\n

• 帧头：666
• 目的地横坐标：1234——1.234m
• 目的地纵坐标：4321——4.321m
• 结束符：\r\n

SPD_MSG

示例：“668 +1673 +1293\r\n”

• 帧头：668
• 间隔：‘ ‘
• 直线速度+1673：16.73
• 角速度+1293：12.93
• 结束符：\r\n

PARAM_MSG

参数消息类型：

• 1：
• 2：
• 3：
• 4：
• 5:
• 6:
• 7：转向环KP，倍率3
• 8：转向环KI，倍率2

示例：“667 8 +1000\r\n”

• 帧头：668

• 间隔：‘ ‘

• 参数类型8：转向环KI

• value+1000：KI=10

控制代码

#