轮腿平衡步兵总结 - 代码设计篇
代码框架
.
│ .gitignore
│ LICENSE.md
│ README.md
│
├───Algorithm //算法封装
│ ├───inc
│ │ algorithmOfCRC.h
│ │ crc32.h
│ │ kalman_filter.h
│ │ my_filter.h
│ │ Observer.h
│ │ pid.h
│ │ QuaternionEKF.h
│ │ RLS_Identification.h
│ │ SignalGenerator.h
│ │ SystemIdentification.h
│ │ TD.h
│ │ user_lib.h
│ │ wheel_ins.h
│ │
│ └───src
│ algorithmOfCRC.c // CRC16,CRC8算法
│ crc32.c // CRC32算法
│ kalman_filter.c // 卡尔曼滤波(感谢玺佬开源)
│ my_filter.c // IIR算法以及RC滤波器
│ Observer.c // 状态观测器(未完善)
│ pid.c // PID算法封装(再次感谢玺佬开源)
│ QuaternionEKF.c // IMU滤波算法封装(再再次感谢玺佬开源)
│ RLS_Identification.c // 递推最小二乘法封装
│ SignalGenerator.c // 信号发生器封装，可以产生各种类型的锯齿波，正弦波以及方波
│ SystemIdentification.c // 连续阶跃波生成以及连续频率正弦波生成封装
│ TD.c // 跟踪微分器封装(第n次感谢玺佬开源)
│ user_lib.c // 最小二乘法封装，死区控制算法等(第n次感谢玺佬开源)
│ wheel_ins.c // 轮毂电机反馈与加速度计数据融合算法
│
├───docs
│ │ ina260.pdf // 电流计
│ │ 轮腿底盘控制V2.0.pdf // 底盘控制板原理图
│ │
│ ├───SD Association
│ │
│ └───SD协议2.0
├───Mylib // 硬件标准库封装
│ ├───inc
│ │ bluetooth.h
│ │ bsp_referee.h
│ │ bsp_spi_sdcard.h
│ │ bsp_superpower.h
│ │ can1.h
│ │ can1_receive.h
│ │ can2.h
│ │ can2_receive.h
│ │ can_send.h
│ │ counter.h
│ │ i2c.h
│ │ iwdg.h
│ │ led.h
│ │ motor_rs485.h
│ │ os_tick.h
│ │ pc_uart.h
│ │ pin_affine.h
│ │ wifi.h
│ │
│ └───src
│ bluetooth.c // 蓝牙
│ bsp_referee.c // 裁判系统接口
│ bsp_spi_sdcard.c // SD卡接口
│ bsp_superpower.c // 超级电容接口
│ can1.c // CAN1
│ can1_receive.c
│ can2.c // CAN2
│ can2_receive.c
│ can_send.c // CAN发送封装
│ counter.c // 定时器，主要用来计算程序耗时，测试通信是否正常等功能
│ i2c.c // I2C通信实现
│ iwdg.c // 看门狗实现
│ led.c // led灯驱动
│ motor1_rs485.c // 左(右)关节电机RS485驱动
│ motor2_rs485.c // 右(左)关节电机RS485驱动
│ os_tick.c // 用于FreeRTOS性能测试的定时器
│ pc_uart.c // 与PC通信串口驱动
│ wifi.c // wifi模块驱动
├───Task // 任务
│ ├───inc
│ │ ActionTask.h
│ │ BlueToothTask.h
│ │ ChasisControlTask.h
│ │ ChasisEstimateTask.h
│ │ CPU_Task.h
│ │ GimbalTask.h
│ │ iwdgTask.h
│ │ MF9025_IdentifyTask.h
│ │ Offline_Task.h
│ │ PC_Task.h
│ │ PowerControlTask.h
│ │ RefereeTask.h
│ │ SDCardTask.h
│ │ Start_Task.h
│ │ Test_Task.h
│ │ WheelsAccelFusionTask.h
│ │
│ └───src
│ ActionTask.c // 完成机器人状态切换逻辑任务
│ BlueToothTask.c // 蓝牙传输任务(配合VOFA+实现无线debug)
│ ChasisControlTask.c // 底盘控制任务
│ ChasisEstimateTask.c // 底盘姿态估计任务
│ CPU_Task.c // FreeRTOS CPU占用耗时测试任务
│ GimbalTask.c // 热量控制以及与云台控制任务
│ iwdgTask.c // 看门狗任务
│ MF9025_IdentifyTask.c // MF9025电机系统辨识任务
│ Offline_Task.c // 掉线检测任务
│ PC_Task.c // 与PC通信任务
│ PowerControlTask.c // 功率控制测试任务
│ RefereeTask.c // 裁判系统读取任务
│ SDCardTask.c // SD卡读写任务
│ Start_Task.c // 启动任务
│ Test_Task.c // 电路测试任务
│ WheelsAccelFusionTask.c // 轮毂电机与加速度计数据融合测试任务
│
└───User
├───inc
│ ├───app
│ │ ChasisController.h
│ │ Gimbal.h
│ │ GimbalReceive.h
│ │ GimbalSend.h
│ │ HeatControl.h
│ │ ins.h
│ │ main.h
│ │ pc_serial.h
│ │ PowerLimit.h
│ │ RobotAbnormalDetector.h
│ │ robotObserver.h
│ │ ToggleBullet.h
│ │
│ ├───config
│ │ can_config.h // CAN ID配置
│ │
│ ├───FATFS // SD卡相关
│ ├───motor
│ │ GM6020.h
│ │ M2006.h
│ │ M3508.h
│ │ MF9025.h
│ │ UniTreeA1.h
│ │
│ ├───os
│ │ FreeRTOSConfig.h
│ │
│ ├───peripheral
│ │ icm20602.h
│ │ ina260.h
│ │ Referee.h
│ │ remote_control.h
│ │ SuperPower.h
│ │
│ ├───sys
│ │ stm32f4xx.h
│ │ stm32f4xx_conf.h
│ │ stm32f4xx_it.h
│ │ system_stm32f4xx.h
│ │
│ └───tools
│ debug.h
│ protocol.h
│ tools.h
│ ZeroCheck.h
│
└───src
├───app
│ ChasisController.c // 底盘控制算法
│ Gimbal.c // 云台控制算法(下供弹控yaw轴用，已弃用)
│ GimbalReceive.c // 接收云台信息
│ GimbalSend.c // 发送给云台信息
│ HeatControl.c // 热量控制
│ ins.c // INS,IMU滤波算法
│ main.c // 程序进口
│ pc_serial.c // 与PC通信接口
│ PowerLimit.c // 功率限制，电容控制接口
│ RobotAbnormalDetector.c // 异常观测
│ robotObserver.c // 机器人状态观测器
│ ToggleBullet.c // 拨弹电机控制
│
├───motor
│ GM6020.c // 6020电机驱动
│ M2006.c // 2006电机驱动
│ M3508.c // 3508电机驱动
│ MF9025.c // 9025电机驱动
│ UniTreeA1.c // A1电机驱动
│
├───peripheral
│ icm20602.c // IMU
│ ina260.c // 电流计
│ Referee.c // 裁判系统
│ remote_control.c // 遥控器
│ SuperPower.c // 超级电容
│
├───sys //系统文件
│ startup_stm32f40_41xxx.s
│ stm32f4xx_it.c
│ system_stm32f4xx.c
│
└───tools
debug.c // 全局debug器，用于快速定位传感器错误
tools.c // 延时封装
ZeroCheck.c // 过零检测
代码启动逻辑
标准库封装
在实际应用中，可能会因为板子上引脚复用不同而使标准库修改十分麻烦，故在平衡步兵代码中，统一封装了标准库，即将大部分需要修改的东西都放在.h 文件中，防止大面积修改代码引发的错误。
例如 can1.h 中，若需要修改复用的 GPIO 口，修改以下宏定义即可。
/* params */
#define CAN1_RCC_AHBx_GPIOx RCC_AHB1Periph_GPIOA
#define CAN1_GPIOx GPIOA
#define CAN1_GPIO_PinSource_x1 GPIO_PinSource11
#define CAN1_GPIO_PinSource_x2 GPIO_PinSource12
#define CAN1_GPIO_Pin_x1 GPIO_Pin_11
#define CAN1_GPIO_Pin_x2 GPIO_Pin_12
/* GPIO特性设置到特定的位置修改 */
/* NVIC优先级到特定位置修改 */
/* CAN1特性配置到特定位置修改 */
/* 过滤器的其它设置到特定位置改 */
串口接发设计
以串口发送给 VOFA+的数据为例，#pragma pack(push, 1)设定一字节对齐(否则会默认四字节对齐，不能使用 memcpy 操作)。
#pragma pack(push, 1)
#define CH_COUNT_B 6
typedef struct BlueToothSendData
{
float fdata[CH_COUNT_B];
unsigned char tail[4];
} BlueToothSendData;
#pragma pack(pop)
这样的话，便可以使用 memcpy 函数直接将数据一次性复制到发送的缓冲区中，操作十分“优雅”。
void BLUE_TOOTHSendData(BlueToothSendData *data)
{
while (DMA_GetCmdStatus(BLUE_TOOTH_SEND_DMAx_Streamx) == ENABLE)
;
memcpy(BlueToothSend_Buff, data, BlueTooth_SENDBUF_SIZE);
BlueToothSend_Buff[BlueTooth_SENDBUF_SIZE - 1] = 0x7f;
BlueToothSend_Buff[BlueTooth_SENDBUF_SIZE - 2] = 0x80;
BlueToothSend_Buff[BlueTooth_SENDBUF_SIZE - 3] = 0x00;
BlueToothSend_Buff[BlueTooth_SENDBUF_SIZE - 4] = 0x00;
DMA_SetCurrDataCounter(BLUE_TOOTH_SEND_DMAx_Streamx, BlueTooth_SENDBUF_SIZE);
DMA_Cmd(BLUE_TOOTH_SEND_DMAx_Streamx, ENABLE);
}
裁判系统数据接收设计
接受使用 DMA_Mode_Circular 模式，当数据过一圈时圈数加一。
referee_data.decoder.receive_data_len = REFEREE_RECVBUF_SIZE - DMA_GetCurrDataCounter(REFEREE_RECV_DMAx_Streamx) + referee_data.decoder.judgementFullCount * REFEREE_RECVBUF_SIZE;
if (referee_data.decoder.receive_data_len - referee_data.decoder.decode_data_len > 2 * REFEREE_RECVBUF_SIZE)
{
referee_data.decoder.decode_data_len = referee_data.decoder.receive_data_len - 2 * REFEREE_RECVBUF_SIZE;
}
int read_arr = referee_data.decoder.decode_data_len % REFEREE_RECVBUF_SIZE;
代码通过对比收到的数据长度以及已经解码的数据长度进行处理，解码数与接受数相差过大，则可以使解码长度=接收长度-圈长度来使不重复解码。
该解码采用的是单字节解码形式，好处在于大大降低了丢包的几率。
UI_PushUp_Counter++;
UI_PushUp_Counter_500 = UI_PushUp_Counter % 500;
UI_PushUp_Counter_20 = UI_PushUp_Counter % 20;
if (UI_PushUp_Counter_500 == 37)
{
UI_Draw_String(&referee_data.UI_String.String, "001", UI_Graph_Add, 2, UI_Color_Green, 15, 8, 3, 1600, 800, " FRIC ");
UI_PushUp_String(&referee_data.UI_String, referee_data.Game_Robot_State.robot_id);
}
发送绘制 UI 则是通过定时发送完成，这样可以不需要按键对 UI 进行初始化，但缺点是有时候 UI 更新不太及时(这是一个比较重要的问题，目前还没找到解决方案)。
全局 Debug 器
代码定义了一个可以进行全局 debug 的结构体，通过 Keil 可以观察到结构体的内容，以判断传感器是否存在丢帧等现象，以及收发是否正常等等。
GlobalDebugger global_debugger;
typedef struct Motor_SendReceive_Debugger
{
uint16_t send_msgs_num;
uint16_t recv_msgs_num;
uint16_t loss_num; //丢帧数
/* 接收帧率计算定义 */
uint32_t last_can_cnt;
float can_dt; //前后两帧时间差
} Motor_SendReceive_Debugger;
