HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修复PID堵转检测错误，修复LK电机位置环计算错误，更新文档，修改了消息中心的命名（不影响app层）

This commit is contained in:
NeoZng 2023-05-14 14:56:00 +08:00
parent 299073321b
commit ecb56ef935
12 changed files with 63 additions and 641 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
README.md
View File

@ -112,19 +112,18 @@
## BSP层(Board Sopport Package)
- TODO：
- 主要功能：实现映射功能。
- 主要功能：实现对STM HAL的封装功能，进一步抽象硬件。
- 在本框架中，BSP层与cubeMX初始化有一定程度的耦合，若没有在CUBEMX中开启某个外设，则在application不能初始化使用了对应外设的module。对该层的修改可能需要使用cube重新生成工程（主要是外设的配置，通信速度，时钟频率和分频数等）。该层也是唯一允许直接出现stm32HAL库函数的代码层，**在非BSP层编写代码时，如需使用HAL_...函数，请思考是否有同功能的BSP_...函数**。不过，由于ST的HAL已经对硬件进行较高的抽象（如以handle_xxx的方式描述一个硬件外设或功能引脚），因此即使需要更换开发板，必须修改的内容也极少。
- 最简单的(如gpio)仅是对HAL库函数的封装。较为复杂的则会进行一定程度的处理(如can)
**编写和使用指南**
- 补充与修改：某款主控对应的BSP层应保持相同，当认为该层可能缺少部分功能或有错误时，请联系组长确认后解决并更新整个框架，**请勿自行修改提交**。 请在你修改/增加的bsp_XXX.md中提供测试用例和使用示范以及任何其他需要注意的事项，并在代码必要的地方添加注释。
- 代码移植：BSP层也是在不同系列、型号的stm32间执行代码移植时主要需要关注的代码层。向功能更强系列移植一般只需要重配cube并重新组织BSP层的映射关系，而向功能较少的系列移植还需要去掉其不支持的功能。如果仅是对同一型号的开发板进行HAL初始化配置的修改，则BSP层**不需要**变动。
- 代码移植：BSP层也是在不同系列、型号的stm32间执行代码移植时主要需要关注的代码层。向功能更强系列移植一般只需要重配cube，而向功能较少的系列移植还需要去掉其不支持的功能。如果仅是对同一型号的开发板进行HAL初始化配置的修改，一般只需要给app层的应用重新分配外设和引脚，或修改波特率和通信频率等。
- 子文件与文件夹：
- bsp.c/h：该层用于bsp基础功能初始化的文件，其中.h被include至main.c中，以实现整个代码层的初始化。include了该层所有模块的.h并调用各模块的初始化函数。**注意**，有些外设如串口和CAN不需要在bsp.c中进行模块层的初始化，他们会在module层生成实例（即C语言中的结构体）并注册到bsp层时自动进行初始化。以此达到提高运行速度避免未使用的模块被加载的问题。
- bsp.c/h：该层用于bsp基础功能初始化的文件，其中.h被include至main.c中，以实现整个代码层的初始化。include了该层所有模块的.h并调用各模块的初始化函数。目前需要初始化的bsp只有log和dwt，**不同主频的MCU需要修改dwt初始化的参数**。**注意**，有些外设如串口和CAN不需要在bsp.c中进行模块层的初始化，他们会在module层生成实例（即C语言中的结构体）并注册到bsp层时自动进行初始化。以此达到提高运行速度避免未使用的模块被加载的问题。
- bsp_xxx.c/h：每一个成对的.c/h对应一种外设，当上面两个代码层需要使用某个外设时，这里的文件就是对应的交互接口。
- 注册回调函数与接收：通信类外设模块有的定义了回调函数类型(函数指针类型)，若调用bsp...h中的回调函数注册函数将其他位置(HAL层)定义的符合形式的函数注册为回调函数，该函数在接收到数据后或其他设定位置会被调用。在module对模块进行初始化的时候需要将对应的协议解析函数进行设置，代码中注释有对应提示。
- 注册回调函数与接收：通信类外设模块有的定义了回调函数(函数指针类型)，module层的模块需要自行处理接收回调函数，在注册bsp的时候应传入对应参数格式的回调函数指针，使得接收中断发生的时候bsp层可以自行找到对应的上层回调函数进行调用。这也是回调函数设计的初衷：为底层代码调用上层代码提供接口，当特定事件发生的时候完成触发（自行搜索hook函数）。
## Module层
@ -156,10 +155,9 @@
- 封装程度：
应尽可能使到上层使用时不考虑下层所需的操作。如在使用电机时，这个电机的数据该和哪些电机的数据在一个数据包中发送，can的过滤器设置，均属于应该自动处理的功能；
接收类的driver应该封装到只有初始化（用于初始化的`register`和发送控制命令`set_control`两个函数和一个实时更新的用于给app层提供该信息的数据结构体）。
应尽可能使到上层使用时不考虑下层所需的操作。如在使用电机时，这个电机的数据该和哪些电机的数据在一个数据包中发送，can的过滤器设置，均属于应该自动处理的功能；接收类的driver应该封装到只有初始化（用于初始化的`register`和发送控制命令`set_control`两个函数和一个实时更新的用于给app层提供该信息的数据结构体）。
Module层主要存放的是类型定义和实例指针数组，在该层没有进行实例化（定义或通过malloc分配空间），若在APP层没有实例化，则该模块的存在与否基本不会影响编译后的可执行文件，只会占用初始化和代码区所需的少量内存。module只会保存每个实例对象的指针，在没有初始化的时候仅仅占用一个指针数组的空间。因此，基于本框架的其他工程没有必要删除APP层未使用的module文件。
Module层主要存放的是类型定义和实例指针数组，在该层没有进行实例化（定义或通过malloc分配空间），若在APP层没有实例化，则该模块的存在与否不会影响编译后的可执行文件，只会占用初始化和代码区所需的少量内存。module只会保存每个实例对象的指针，在没有初始化的时候仅仅占用一个指针数组的空间。因此，基于本框架的其他工程没有必要删除APP层未使用的module文件。
务必为模块添加说明文档和使用范例，以及其他需要注意的事项（如果有）。

446
application/balance_chassis/balance.c
View File

@ -1,446 +0,0 @@
// app
#include "balance.h"
#include "gain_table.h"
#include "robot_def.h"
#include "general_def.h"
// module
#include "HT04.h"
#include "LK9025.h"
#include "bmi088.h"
#include "referee_task.h"
#include "super_cap.h"
#include "controller.h"
#include "can_comm.h"
#include "user_lib.h"
// standard
#include "stdint.h"
#include "arm_math.h" // 需要用到较多三角函数
#include "bsp_dwt.h"
static uint32_t balance_dwt_cnt;
static float balance_dt;
/* 底盘拥有的模块实例 */
static attitude_t *imu_data;
// static BMI088Instance *imu;
static SuperCapInstance *super_cap;
static referee_info_t *referee_data; // 裁判系统的数据会被不断更新
static Referee_Interactive_info_t my_uidata; // UI绘制数据,各种模式和状态/功率的显示
// 电机
static HTMotorInstance *lf, *rf, *lb, *rb;
static LKMotorInstance *l_driven, *r_driven;
// 底盘板和云台板通信
static CANCommInstance *chassis_comm; // 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率并降低阻塞
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Chassis_Upload_Data_s chassis_feed_send;
// 两个腿的参数,0为左腿,1为右腿
static LinkNPodParam left_side, right_side;
static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
static PIDInstance leg_length_pid; // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
/* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/
/**
* @brief 将电机和imu的数据组装为LinkNPodParam结构体
* @note 由于两个连杆和轮子连接时,只有一个使用了轴承,另外一个直接和电机的定子连接,
* 所以轮子的转速和电机的转速不一致,因此和地面的速度应为电机转速减去杆的转速,得到的才是轮子的转速
* @note HT04电机上电的编码器位置为零,请看Link2Pod()的note,确定限位LIMIT_LINK_RAD的正负号
*
* @todo angle direction to be verified
*/
static void ParamAssemble()
{
left_side.phi1 = PI2 - LIMIT_LINK_RAD + lb->measure.total_angle;
left_side.phi4 = lf->measure.total_angle + LIMIT_LINK_RAD;
left_side.phi1_w = lb->measure.speed_rads;
left_side.phi4_w = lf->measure.speed_rads;
left_side.wheel_dist = l_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
left_side.wheel_w = l_driven->measure.speed_rads;
// 电机的角度是逆时针为正,右侧全部取反
right_side.phi1 = PI2 - LIMIT_LINK_RAD - rb->measure.total_angle;
right_side.phi4 = -rf->measure.total_angle + LIMIT_LINK_RAD;
right_side.phi1_w = -rb->measure.speed_rads;
right_side.phi4_w = -rf->measure.speed_rads;
right_side.wheel_dist = -r_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
right_side.wheel_w = -r_driven->measure.speed_rads;
// 若有转向指令,则使用IMU积分得到的速度,否则左右轮相反会产生阻碍转向的力矩
if (chassis_cmd_recv.wz != 0) // 此时相当于lqr的平衡环只对机体的速度响应
{
float vel_divide_R = imu_data->Accel[3] * balance_dt / WHEEL_RADIUS; // todo: 确定速度方向
left_side.wheel_w = vel_divide_R;
right_side.wheel_w = vel_divide_R;
} // 转向的时候不需要修正轮速
}
/**
* @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
* @todo 测试两种的效果,留下其中一种;
* 若差分的效果好则不需要VELOCITY_DIFF_VMC内的代码
*
* @note 右侧视图
* ___x anti-clockwise is positive for motor
* | _____
* |y / \
* \ /
* \ /
*
* @param p 5连杆和腿的参数
*/
static void Link2Pod(LinkNPodParam *p)
{ // 拟将功能封装到vmc_project.h中
float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2t, phi5t;
xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(p->phi4);
yD = THIGH_LEN * arm_sin_f32(p->phi4);
xB = 0 + THIGH_LEN * arm_cos_f32(p->phi1);
yB = THIGH_LEN * arm_sin_f32(p->phi1);
BD = powf(xD - xB, 2) + powf(yD - yB, 2);
A0 = 2 * CALF_LEN * (xD - xB);
B0 = 2 * CALF_LEN * (yD - yB);
p->phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
xC = xB + CALF_LEN * arm_cos_f32(p->phi2);
yC = yB + CALF_LEN * arm_sin_f32(p->phi2);
p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2);
p->pod_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
p->phi3 = atan2(yC - yD, xC - xD); // 稍后用于计算VMC
p->theta = p->phi5 - 0.5 * PI - imu_data->Pitch * DEGREE_2_RAD; // 确定方向
p->height = p->pod_len * arm_cos_f32(p->theta);
#ifdef VELOCITY_DIFF_VMC
float phi1_pred = p->phi1 + p->phi1_w * balance_dt; // 预测下一时刻的关节角度(利用关节角速度)
float phi4_pred = p->phi4 + p->phi4_w * balance_dt;
// 重新计算腿长和腿角度
xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(phi4_pred);
yD = THIGH_LEN * arm_sin_f32(phi4_pred);
xB = 0 + THIGH_LEN * arm_cos_f32(phi1_pred);
yB = THIGH_LEN * arm_sin_f32(phi1_pred);
BD = powf(xD - xB, 2) + powf(yD - yB, 2);
A0 = 2 * CALF_LEN * (xD - xB);
B0 = 2 * CALF_LEN * (yD - yB);
phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
xC = xB + CALF_LEN * arm_cos_f32(phi2t);
yC = yB + CALF_LEN * arm_sin_f32(phi2t);
phi5t = atan2f(yC, xC - JOINT_DISTANCE / 2);
// 差分计算腿长变化率和腿角速度
p->phi2_w = (phi2t - p->phi2) / balance_dt; // 稍后用于修正轮速
p->pod_w = (phi5t - p->phi5) / balance_dt;
p->pod_v = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->pod_len) / balance_dt;
p->theta_w = (phi5t - 0.5 * PI - imu_data->Pitch * DEGREE_2_RAD - balance_dt * imu_data->Gyro[3] - p->theta) / balance_dt; //@todo 确定c板安装方向(BMI088);
p->height_v = p->pod_v * arm_cos_f32(p->theta) - p->pod_len * arm_sin_f32(p->theta) * p->theta_w; // 这很酷!PDE!
#endif
if (chassis_cmd_recv.wz == 0) // 没有转向控制指令,修正轮速. @todo 确定方向
p->wheel_w = (p->wheel_w - p->phi2_w + imu_data->Gyro[3]); // 注意此时单位仍然是rad/s, @todo 确定c板安装方向(BMI088);
// 此时使用的是电机编码器反馈的速度
}
/**
* @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
* 由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算
*
* @note 计算得到的腿部力矩输出还要再综合运动控制系统补偿后映射为两个关节电机
* 而轮子的输出则只经过转向PID的反馈增益计算
*
* @todo 确定使用查表还是多项式拟合
*
*/
static void CalcLQR(LinkNPodParam *p, float target_x)
{
float *gain_list = LookUpKgain(p->pod_len); // K11,K12... K21,K22... K26
float T[2]; // 0 T_wheel, 1 T_pod;
for (uint8_t i = 0; i < 2; i++)
{
T[i] = gain_list[i * 6 + 0] * -p->theta +
gain_list[i * 6 + 1] * -p->theta_w +
gain_list[i * 6 + 4] * -imu_data->Pitch * DEGREE_2_RAD +
gain_list[i * 6 + 5] * -imu_data->Gyro[1]; // @todo 待确定c板安装方向
if (chassis_cmd_recv.vx == 0 && chassis_cmd_recv.wz == 0) // 位置闭环仅在目标转向速度和水平速度都为零的情况下起作用
T[i] += gain_list[i * 6 + 2] * (target_x - p->wheel_dist); // 若速度为0,则加入位置闭环,以提供对外力更好的抵抗和复位
else
T[i] += gain_list[i * 6 + 3] * (chassis_cmd_recv.vx - p->wheel_w * WHEEL_RADIUS); // 若目标速度|角速度不为0,则只对速度闭环
} // 有转向指令的时候,轮速的来源是IMU,而不是编码器
p->T_wheel = T[0];
p->T_pod = T[1];
}
/**
* @brief 腿部控制:抗劈叉; 轮子控制:转向
* @todo 确定方向
*/
static void SynthesizeMotion()
{
PIDCalculate(&anti_crash_pid, left_side.phi5 - right_side.phi5, 0);
left_side.T_pod += anti_crash_pid.Output;
right_side.T_pod -= anti_crash_pid.Output;
PIDCalculate(&swerving_pid, imu_data->Yaw, chassis_cmd_recv.wz); // 对速度闭环还是使用角度增量闭环?
left_side.T_wheel -= swerving_pid.Output;
right_side.T_wheel += swerving_pid.Output;
}
/**
* @brief 腿部控制:长度.用PD模拟弹簧的传递函数
*
* @note 实际使用的是机体当前的高度,若使用腿长计算则会导致加速和减速时因腿倾斜故而机体高度变化
*/
static void LegControl(LinkNPodParam *p, float target_length)
{
p->F_pod += PIDCalculate(&leg_length_pid, p->height, target_length);
}
/**
* @brief roll轴补偿(保持机体水平)
*
*/
static void RollCompensate()
{
PIDCalculate(&roll_compensate_pid, imu_data->Roll, 0);
left_side.F_pod += roll_compensate_pid.Output;
right_side.F_pod -= roll_compensate_pid.Output;
}
/**
* @brief 将前面计算的T和F映射为关节电机输出
*
*/
static void VMCProject(LinkNPodParam *p)
{
float s23 = arm_sin_f32(p->phi2 - p->phi3);
float phi25 = p->phi2 - p->phi5;
float phi35 = p->phi3 - p->phi5;
float F_m_L = p->F_pod * p->pod_len;
p->T_back = -(THIGH_LEN * arm_sin_f32(p->phi1 - p->phi2) * (p->T_pod * arm_cos_f32(phi35) - F_m_L * arm_sin_f32(phi35))) / (p->pod_len * s23);
p->T_front = -(THIGH_LEN * arm_sin_f32(p->phi3 - p->phi4) * (p->T_pod * arm_cos_f32(phi25) - F_m_L * arm_sin_f32(phi25))) / (p->pod_len * s23);
}
/**
* @brief 离地监测和?跳跃控制?
* 通过模型解算地面的支持力完成离地检测
*
*/
static uint8_t air_flag;
static void FlyDetect()
{
}
/**
* @brief 功率限制,一般不需要;限制完成之后设定电机输出
*
*/
static void WattLimitSet()
{
// code to limit 9025's output
// ...
// 设定电机电流
HTMotorSetRef(lf, left_side.T_front);
HTMotorSetRef(lb, left_side.T_back);
HTMotorSetRef(rf, right_side.T_front);
HTMotorSetRef(rb, right_side.T_back);
LKMotorSetRef(l_driven, left_side.T_wheel);
LKMotorSetRef(r_driven, right_side.T_wheel);
}
void BalanceInit()
{ // IMU初始化
BMI088_Init_Config_s imu_config = {
.spi_acc_config = {
.GPIOx = GPIOC,
.cs_pin = GPIO_PIN_4,
.spi_handle = &hspi1,
},
.spi_gyro_config = {
.GPIOx = GPIOC,
.cs_pin = GPIO_PIN_4,
.spi_handle = &hspi1,
},
.heat_pid_config = {
.Kp = 0.0f,
.Kd = 0.0f,
.Ki = 0.0f,
.MaxOut = 0.0f,
.DeadBand = 0.0f,
},
.heat_pwm_config = {
.channel = TIM_CHANNEL_1,
.htim = &htim1,
},
.cali_mode = BMI088_CALIBRATE_ONLINE_MODE,
.work_mode = BMI088_BLOCK_PERIODIC_MODE,
};
// imu = BMI088Register(&imu_config);
imu_data = INS_Init();
SuperCap_Init_Config_s cap_conf = {
// 超级电容初始化
.can_config.can_handle = &hcan1,
.can_config.rx_id = 0x311,
.can_config.tx_id = 0x312,
};
super_cap = SuperCapInit(&cap_conf);
// ↓↓↓---------------关节电机初始化----------------↓↓↓
Motor_Init_Config_s joint_conf = {
// 写一个,剩下的修改方向和id即可
.can_init_config = {
.can_handle = &hcan1,
},
.controller_param_init_config = {
.current_PID = {
.Kp = 1,
},
},
.controller_setting_init_config = {
.close_loop_type = CURRENT_LOOP,
.outer_loop_type = CURRENT_LOOP,
.motor_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
},
.motor_type = HT04,
};
joint_conf.can_init_config.tx_id = 1;
joint_conf.can_init_config.rx_id = 11;
lf = HTMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 2;
joint_conf.can_init_config.rx_id = 12;
lb = HTMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 3;
joint_conf.can_init_config.rx_id = 13;
rf = HTMotorInit(&joint_conf);
joint_conf.can_init_config.tx_id = 4;
joint_conf.can_init_config.rx_id = 14;
rb = HTMotorInit(&joint_conf);
// ↓↓↓---------------驱动电机初始化----------------↓↓↓
Motor_Init_Config_s driven_conf = {
// 写一个,剩下的修改方向和id即可
.can_init_config.can_handle = &hcan1,
.controller_param_init_config = {
.current_PID = {
.Kp = 274.348,
},
},
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
.outer_loop_type = CURRENT_LOOP,
.close_loop_type = CURRENT_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = LK9025,
};
driven_conf.can_init_config.tx_id = 1;
l_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id = 2;
r_driven = LKMotorInit(&driven_conf);
CANComm_Init_Config_s chassis_comm_conf = {
// 底盘板和云台板通信
.can_config = {
.can_handle = &hcan1,
.rx_id = 0x201,
.tx_id = 0x200,
},
.send_data_len = sizeof(Chassis_Upload_Data_s),
.recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
};
chassis_comm = CANCommInit(&chassis_comm_conf);
referee_data = Referee_Interactive_init(&huart6, &my_uidata); // 裁判系统串口
// ↓↓↓---------------综合运动控制----------------↓↓↓
PID_Init_Config_s swerving_pid_conf = {
.Kp = 0.0f,
.Kd = 0.0f,
.Ki = 0.0f,
.MaxOut = 0.0f,
.DeadBand = 0.0f,
.Improve = PID_IMPROVE_NONE,
};
PIDInit(&swerving_pid, &swerving_pid_conf);
PID_Init_Config_s anti_crash_pid_conf = {
.Kp = 0.0f,
.Kd = 0.0f,
.Ki = 0.0f,
.MaxOut = 0.0f,
.DeadBand = 0.0f,
.Improve = PID_IMPROVE_NONE,
};
PIDInit(&swerving_pid, &swerving_pid_conf);
PID_Init_Config_s leg_length_pid_conf = {
.Kp = 0.0f,
.Kd = 0.0f,
.Ki = 0.0f,
.MaxOut = 0.0f,
.DeadBand = 0.0f,
.Improve = PID_IMPROVE_NONE,
};
PIDInit(&leg_length_pid, &leg_length_pid_conf);
PID_Init_Config_s roll_compensate_pid_conf = {
.Kp = 0.0f,
.Kd = 0.0f,
.Ki = 0.0f,
.MaxOut = 0.0f,
.DeadBand = 0.0f,
.Improve = PID_IMPROVE_NONE,
};
PIDInit(&roll_compensate_pid, &roll_compensate_pid_conf);
}
/* balanceTask可能需要以更高频率运行,以提高线性化的精确程度 */
void BalanceTask()
{
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chassis_comm);
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
{
HTMotorStop(lf);
HTMotorStop(rf);
HTMotorStop(lb);
HTMotorStop(rb);
LKMotorStop(l_driven);
LKMotorStop(r_driven);
}
else
{
HTMotorEnable(lf);
HTMotorEnable(rf);
HTMotorEnable(lb);
HTMotorEnable(rb);
LKMotorEnable(l_driven);
LKMotorEnable(r_driven);
}
ParamAssemble(); // 参数组装,将电机和IMU的参数组装到一起
// 将五连杆映射成单杆
Link2Pod(&left_side);
Link2Pod(&right_side);
// 根据单杆计算处的角度和杆长,计算反馈增益
CalcLQR(&left_side, chassis_cmd_recv.vx); // @todo,需要确定速度or位置闭环
CalcLQR(&right_side, chassis_cmd_recv.vx);
// 腿长控制
LegControl(&left_side, 0);
LegControl(&right_side, 0);
// 综合运动控制,转向+抗劈叉
SynthesizeMotion(); // 两边要一起
// 俯仰角补偿,保持机体水平
RollCompensate(); // 两边也要一起
// VMC映射成关节输出
VMCProject(&left_side);
VMCProject(&right_side);
FlyDetect(); // 滞空检测,尝试使用加速度计自由落体时量测值小?
WattLimitSet(); // 电机输出限幅
// code to go here... 裁判系统,UI,多机通信
CANCommSend(chassis_comm, (uint8_t *)&chassis_feed_send);
}

43
application/balance_chassis/balance.h
View File

@ -1,43 +0,0 @@
#pragma once
// 底盘参数
#define CALF_LEN 0.245f // 小腿
#define THIGH_LEN 0.14f // 大腿
#define JOINT_DISTANCE 0.108f // 关节间距
#define WHEEL_RADIUS 0.069f // 轮子半径
#define LIMIT_LINK_RAD 0.15149458 // 初始限位角度,见ParamAssemble
// 计算速度的方式(五连杆到单杆的映射)
#define VELOCITY_DIFF_VMC // 通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
// #define ANGLE_DIFF_VMC //直接保存上一次的值,通过差分计算腿长变化率和腿角速度
typedef struct
{
// joint
float phi1_w, phi4_w, phi2_w; // phi2_w used for calc real wheel speed
float T_back, T_front;
// link angle,phi1-ph5, phi5 is pod angle
float phi1, phi2, phi3, phi4, phi5;
// wheel
float wheel_dist;
float wheel_w;
float T_wheel;
// pod
float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一
float pod_len;
float height, height_v;
float pod_v, pod_w;
float F_pod, T_pod;
} LinkNPodParam;
/**
* @brief 平衡底盘初始化
*
*/
void BalanceInit();
/**
* @brief 平衡底盘任务
*
*/
void BalanceTask();

32
application/balance_chassis/balance.md
View File

@ -1,32 +0,0 @@
# balance
可以继续解耦,将VMC独立成模块.
目前默认使用平衡底盘时为双板.
## 工作流程
1. 获取控制信息和状态信息，组装到linkparam
2. 根据控制模式将控制指令转化为实际的参考输入
3. 使用lqr得出的反馈增益,计算二阶倒立摆模型的控制输出;需要根据当前腿长查gain table,或预先拟合K=f(Leg)的函数
4. 计算二阶倒立摆$[L0 phi0]$和轮腿[phi1 phi4]间的雅可比,根据VMC将lqr的输出[F Tp]映射成[T1 T2] ; 驱动轮不需要映射
5. 进行综合运动补偿,即转向控制和抗劈叉
6. 进行腿长控制计算,即长度控制和roll轴水平控制
7. 进行离地检测判断是否要让腿保持垂直,后续再加入跳跃功能
8. 根据裁判系统和超级电容的功率信息进行输出限幅
9. 设置反馈信息,包括裁判系统的数据,并通过电机反馈和IMU数据计算底盘实际运动状态等
10. 推送反馈信息
电机初始化为电流环即可,注意基于模型的控制需要正确设定单位
如果功率可能超限,需要判定降低功率输出后受影响最小的执行单元,并给予其较大的功率输出衰减(一般不会超功率)
另外, 选择平衡底盘有枪口冷却增益, 注意将这一部分改变反馈给cmd, 以使得shoot有更好的表现
## 优化环节
为了控制系统有更好的效果，对工程上的细节有一些微小的优化如下：
1. 腿长控制实际上对机体高度计算腿长闭环，使得云台能够保持恒定高度
2. 静止时开启位置反馈，若有速度输入则不使用位置反馈，从而避免底盘打滑的情况
3. 没有转向输入时使用轮式里程计计算位置x，有转向输入时使用imu的二重积分，从而避免平衡控制器和转向控制器冲突

25
application/balance_chassis/gain_table.h
View File

@ -1,25 +0,0 @@
/* 平衡底盘lqr反馈增益和腿长的关系表,可以选择查找精度和插值 */
#pragma once
#include "stdint.h"
#include "stm32f407xx.h"
#include "arm_math.h"
#include "math.h"
#define GAIN_TABLE_SIZE 100 // 增益表大小
// K 2x6,6个状态变量2个输出(Tp关节电机和T驱动轮电机)
static float leglen2gain [GAIN_TABLE_SIZE][2][6] = {0};
static interpolation_flag = 0; // 插值方式:1 线性插值 0 关闭插值
void EnalbeInterpolation(void)
{
interpolation_flag = 1;
}
/* 默认关闭插值,向下取整 */
float* LookUpKgain(float leg_length)
{
}

17
application/cmd/robot_cmd.c
View File

@ -274,22 +274,7 @@ static void EmergencyHandler()
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
}
switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_C]%2) //ctrl+c 进入急停
{
case 0:
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
break;
default:
robot_state = ROBOT_STOP;
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
shoot_cmd_send.shoot_mode = SHOOT_OFF;
shoot_cmd_send.friction_mode = FRICTION_OFF;
shoot_cmd_send.load_mode = LOAD_STOP;
break;
}
}
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */

7
application/robot.c
View File

@ -42,10 +42,6 @@ void RobotInit()
ChassisInit();
#endif
#ifdef BALANCE_BAORD
BalanceInit();
#endif // BALANCE_BA
// 初始化完成,开启中断
__enable_irq();
}
@ -62,7 +58,4 @@ void RobotTask()
ChassisTask();
#endif
#ifdef BALANCE_BAORD
BalanceTask();
#endif // BALANCE_BA
}

2
modules/algorithm/controller.c
View File

@ -97,7 +97,7 @@ static void f_Output_Limit(PIDInstance *pid)
static void f_PID_ErrorHandle(PIDInstance *pid)
{
/*Motor Blocked Handle*/
if (pid->Output < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
if (fabsf(pid->Output) < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
return;
if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)

38
modules/message_center/message_center.c
View File

@ -5,17 +5,17 @@
/* message_center是fake head node,是方便链表编写的技巧,这样就不需要处理链表头的特殊情况 */
static Publisher_t message_center = {
.event_name = "Message_Manager",
.topic_name = "Message_Manager",
.first_subs = NULL,
.next_event_node = NULL};
.next_topic_node = NULL};
static void CheckName(char *name)
{
if (strnlen(name, MAX_EVENT_NAME_LEN + 1) >= MAX_EVENT_NAME_LEN)
if (strnlen(name, MAX_TOPIC_NAME_LEN + 1) >= MAX_TOPIC_NAME_LEN)
{
LOGERROR("EVENT NAME TOO LONG:%s", name);
while (1)
; // 进入这里说明事件名超出长度限制
; // 进入这里说明话题名超出长度限制
}
}
@ -25,7 +25,7 @@ static void CheckLen(uint8_t len1, uint8_t len2)
{
LOGERROR("EVENT LEN NOT SAME:%d,%d", len1, len2);
while (1)
; // 进入这里说明相同事件的消息长度却不同
; // 进入这里说明相同话题的消息长度却不同
}
}
@ -33,28 +33,28 @@ Publisher_t *PubRegister(char *name, uint8_t data_len)
{
CheckName(name);
Publisher_t *node = &message_center;
while (node->next_event_node) // message_center会直接跳过,不需要特殊处理,这被称作dumb_head(编程技巧)
while (node->next_topic_node) // message_center会直接跳过,不需要特殊处理,这被称作dumb_head(编程技巧)
{
node = node->next_event_node; // 切换到下一个发布者(事件)结点
if (strcmp(node->event_name, name) == 0) // 如果已经注册了相同的事件,直接返回结点指针
node = node->next_topic_node; // 切换到下一个发布者(话题)结点
if (strcmp(node->topic_name, name) == 0) // 如果已经注册了相同的话题,直接返回结点指针
{
CheckLen(data_len, node->data_len);
node->pub_registered_flag = 1;
return node;
}
} // 遍历完发现尚未创建name对应的事件
// 在链表尾部创建新的事件并初始化
node->next_event_node = (Publisher_t *)malloc(sizeof(Publisher_t));
memset(node->next_event_node, 0, sizeof(Publisher_t));
node->next_event_node->data_len = data_len;
strcpy(node->next_event_node->event_name, name);
} // 遍历完发现尚未创建name对应的话题
// 在链表尾部创建新的话题并初始化
node->next_topic_node = (Publisher_t *)malloc(sizeof(Publisher_t));
memset(node->next_topic_node, 0, sizeof(Publisher_t));
node->next_topic_node->data_len = data_len;
strcpy(node->next_topic_node->topic_name, name);
node->pub_registered_flag = 1;
return node->next_event_node;
return node->next_topic_node;
}
Subscriber_t *SubRegister(char *name, uint8_t data_len)
{
Publisher_t* pub = PubRegister(name, data_len); // 查找或创建该事件的发布者
Publisher_t *pub = PubRegister(name, data_len); // 查找或创建该话题的发布者
// 创建新的订阅者结点,申请内存,注意要memset因为新空间不一定是空的,可能有之前留存的垃圾值
Subscriber_t *ret = (Subscriber_t *)malloc(sizeof(Subscriber_t));
memset(ret, 0, sizeof(Subscriber_t));
@ -72,7 +72,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len)
}
// 若该话题已经有订阅者, 遍历订阅者链表,直到到达尾部
Subscriber_t *sub = pub->first_subs; // 作为iterator
while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表
while (sub->next_subs_queue) // 遍历订阅了该话题的订阅者链表
{
sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部
}
@ -96,8 +96,8 @@ uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr)
uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr)
{
static Subscriber_t *iter;
iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该事件的所有订阅者;如果为空说明遍历结束
// 遍历订阅了当前事件的所有订阅者,依次填入最新消息
iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该话题的所有订阅者;如果为空说明遍历结束
// 遍历订阅了当前话题的所有订阅者,依次填入最新消息
while (iter)
{
if (iter->temp_size == QUEUE_SIZE) // 如果队列已满,则需要删除最老的数据(头部),再填入

28
modules/message_center/message_center.h
View File

@ -15,8 +15,8 @@
#include "stdint.h"
#define MAX_EVENT_NAME_LEN 32 // 最大的事件名长度,每个事件都有字符串来命名
#define MAX_EVENT_COUNT 12 // 最多支持的事件数量
#define MAX_TOPIC_NAME_LEN 32 // 最大的话题名长度,每个话题都有字符串来命名
#define MAX_TOPIC_COUNT 12 // 最多支持的话题数量
#define QUEUE_SIZE 1
typedef struct mqt
@ -28,30 +28,30 @@ typedef struct mqt
uint8_t back_idx;
uint8_t temp_size; // 当前队列长度
/* 指向下一个订阅了相同的事件的订阅者的指针 */
struct mqt *next_subs_queue; // 使得发布者可以通过链表访问所有订阅了相同事件的订阅者
/* 指向下一个订阅了相同的话题的订阅者的指针 */
struct mqt *next_subs_queue; // 使得发布者可以通过链表访问所有订阅了相同话题的订阅者
} Subscriber_t;
/**
* @brief 发布者类型.每个发布者拥有发布者实例,并且可以通过链表访问所有订阅了自己发布的事件的订阅者
* @brief 发布者类型.每个发布者拥有发布者实例,并且可以通过链表访问所有订阅了自己发布的话题的订阅者
*
*/
typedef struct ent
{
/* 事件名称 */
char event_name[MAX_EVENT_NAME_LEN + 1]; // 1个字节用于存放字符串结束符 '\0'
uint8_t data_len; // 该事件的数据长度
/* 指向第一个订阅了该事件的订阅者,通过链表访问所有订阅者 */
/* 话题名称 */
char topic_name[MAX_TOPIC_NAME_LEN + 1]; // 1个字节用于存放字符串结束符 '\0'
uint8_t data_len; // 该话题的数据长度
/* 指向第一个订阅了该话题的订阅者,通过链表访问所有订阅者 */
Subscriber_t *first_subs;
/* 指向下一个Publisher的指针 */
struct ent *next_event_node;
struct ent *next_topic_node;
uint8_t pub_registered_flag; // 用于标记该发布者是否已经注册
} Publisher_t;
/**
* @brief 订阅name的事件消息
* @brief 订阅name的话题消息
*
* @param name 事件名称
* @param name 话题名称
* @param data_len 消息长度,通过sizeof()获取
* @return Subscriber_t* 返回订阅者实例
*/
@ -60,7 +60,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len);
/**
* @brief 注册成为消息发布者
*
* @param name 发布者发布的话题名称(事件)
* @param name 发布者发布的话题名称(话题)
* @return Publisher_t* 返回发布者实例
*/
Publisher_t *PubRegister(char *name, uint8_t data_len);
@ -75,7 +75,7 @@ Publisher_t *PubRegister(char *name, uint8_t data_len);
uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr);
/**
* @brief 发布者给所有订阅了事件的订阅者推送消息
* @brief 发布者给所有订阅了话题的订阅者推送消息
*
* @param pub 发布者实例指针
* @param data_ptr 指向要发布的数据的指针

50
modules/message_center/message_center.md
View File

@ -4,15 +4,7 @@
> TODO:
>
> 1. 增加互斥锁或在读写数据的时候进入临界区，防止数据竞争
>
> 2. 降低代码冗杂度，将`PubRegister`和`SubRegister`的共同部分抽象出来编写新的函数
>
> 3. 支持自定义队列长度，使得订阅者可以自行确定需要的队列长度，适应不同的需求
>
> 4. ==**！！！重要！！！**==
>
> ==**Legacy support：将在正式版发布的时候剔除第三方指针传递者的实现，请迁移到新的链表实现。**==
> 支持自定义队列长度，使得订阅者可以自行确定需要的队列长度，适应不同的需求
@ -20,13 +12,13 @@
**重要定义：**
- 发布者：发布消息的对象。发布者会将自己的消息推送给所有订阅了某个特定**事件**的订阅者。
- 订阅者：获取消息的对象。订阅者在订阅了某个事件之后，可以通过接口获得该事件的消息。
- 事件（event / topic）：用于区分消息来源的对象。可以将一个事件看作一刊杂志，不同的发布者会将文章汇集到杂志上，而订阅者选择订阅一种杂志，然后就可以获取所有写在杂志上的文章。
- 发布者：发布消息的对象。发布者会将自己的消息推送给所有订阅了某个特定**话题**的订阅者。
- 订阅者：获取消息的对象。订阅者在订阅了某个话题之后，可以通过接口获得该话题的消息。
- 话题（topic）：用于区分消息来源的对象。可以将一个话题看作一刊杂志，不同的发布者会将文章汇集到杂志上，而订阅者选择订阅一种杂志，然后就可以获取所有写在杂志上的文章。
Message Center不同应用间进行消息传递的中介，它的存在可以在相当大的程度上解耦不同的app，使得不同的应用之间**不存在包含关系**，让代码的自由度更大，将不同模块之间的关系降为**松耦合**。在以往，如果一个.c文件中的数据需要被其他任务/源文件共享，那么其他模块应该要包含前者的头文件，且头文件中应当存在获取该模块数据的接口（即函数，一般是返回数据指针或直接返回数据，**强烈建议不要使用全局变量**）；但现在，不同的应用之间完全隔离，他们不需要了解彼此的存在，而是只能看见一个**消息中心**以及一些事件（有些地方也称**话题**）。
Message Center不同应用间进行消息传递的中介，它的存在可以在相当大的程度上解耦不同的app，使得不同的应用之间**不存在包含关系**，让代码的自由度更大，将不同模块之间的关系降为**松耦合**。在以往，如果一个.c文件中的数据需要被其他任务/源文件共享，那么其他模块应该要包含前者的头文件，且头文件中应当存在获取该模块数据的接口（即函数，一般是返回数据指针或直接返回数据，**强烈建议不要使用全局变量**）；但现在，不同的应用之间完全隔离，他们不需要了解彼此的存在，而是只能看见一个**消息中心**以及一些**话题**。
需要被共享的消息，将会被**发布者**（publisher）发送到消息中心；要获取消息，则由**订阅者**（subscriber）从消息中心根据订阅的消息名（事件）获取。在这之前，发布者要在消息中心完成**注册**，将自己要发布的消息类型和消息名称提交到消息中心；订阅者同样要先在消息中心完成订阅，将自己要接收的消息类型和消息名称提交到订阅中心。消息中心会根据**消息名称**，把订阅者绑定到发布相同名称的发布者上。
需要被共享的消息，将会被**发布者**（publisher）发送到消息中心；要获取消息，则由**订阅者**（subscriber）从消息中心根据订阅的话题获取。在这之前，发布者要在消息中心完成**注册**，将自己要发布的消息类型和话题名称提交到消息中心；订阅者同样要先在消息中心完成订阅，将自己要接收的消息类型和话题名称提交到订阅中心。消息中心会根据**话题名称**，把订阅者绑定到发布相同名称的发布者上。
> 为了节省空间，数据结构上采用了链表+循环数组模拟队列的方式。C没有哈希表，因此让发布者保存所有订阅者的地址（实际上只保存首地址，然后通过链表访问所有订阅者）。
@ -42,7 +34,7 @@ Message Center对外提供了四个接口，所有原本要进行信息交互的
## 外部接口
**在代码实现上，事件实际上就是通过一个字符串体现的。**
**在代码实现上，话题名实际上就是通过一个字符串体现的。**
```c
Subscriber_t* SubRegister(char* name,uint8_t data_len);
@ -56,7 +48,7 @@ void PubPushMessage(Publisher_t* pub,void* data_ptr);
### 订阅者
订阅者应该保存一个订阅者类型的指针`Subscriber_t*`，在初始化的时候调用`SubRegister()`并传入要订阅的事件名和该事件对应消息的长度，可以直接输入字符串，示例如下，将从event_name订阅float数据：
订阅者应该保存一个订阅者类型的指针`Subscriber_t*`，在初始化的时候调用`SubRegister()`并传入要订阅的话题名和该话题对应消息的长度，可以直接输入字符串，示例如下，将从event_name订阅float数据：
```c
Subscriber_t* my_sub;
@ -69,18 +61,18 @@ my_sub=SubRegister("event_name",sizeof(float));
### 发布者
发布者应该保存一个发布者类型的指针，在初始化的时候传入要发布的事件名和该事件对应的消息长度。
发布者应该保存一个发布者类型的指针，在初始化的时候传入要发布的话题名和该话题对应的消息长度。
完成注册后，通过`PubPushMessage()`发布新的消息。所有订阅了该事件的订阅者都会收到新的消息推送。
完成注册后，通过`PubPushMessage()`发布新的消息。所有订阅了该话题的订阅者都会收到新的消息推送。
### 可修改的宏
```c
#define MAX_EVENT_NAME_LEN 32 //最大的事件名长度,每个事件都由字符串来命名
#define MAX_EVENT_NAME_LEN 32 //最大的话题名长度,每个话题都由字符串来命名
#define QUEUE_SIZE 1 //消息队列的长度
```
修改第一个可以扩大事件名长度，第二个确定消息队列的长度，数量越大可以保存的消息越多。
修改第一个可以扩大话题名长度，第二个确定消息队列的长度，数量越大可以保存的消息越多。
@ -96,14 +88,14 @@ static void CheckName(char* name)
{
if(strnlen(name,MAX_EVENT_NAME_LEN+1)>=MAX_EVENT_NAME_LEN)
{
while (1); // 进入这里说明事件名超出长度限制
while (1); // 进入这里说明话题名超出长度限制
}
}
```
`message_center`内部保存了指向第一个发布者的指针，可以看作整个消息中心的抽象。通过这个变量，可以访问所有发布者和订阅者。它将会在各个函数中作为dumb_head（哑结点）以简化逻辑，这样不需要对链表头进行特殊处理。
`CheckName()`在发布者/订阅者注册的时候被调用，用于检查事件名是否超过长度限制。超长后会进入死循环，方便开发者检查。
`CheckName()`在发布者/订阅者注册的时候被调用，用于检查话题名是否超过长度限制。超长后会进入死循环，方便开发者检查。
> 四个外部接口的实现都有详细的注释，有兴趣的同学可以自行阅读。下方也提供了流程图。
@ -117,23 +109,23 @@ static void CheckName(char* name)
<center>建议打开原图查看</center>
**多个publisher可以绑定同一个事件，往该事件推送消息。但一个subscriber只能订阅一个事件，如果应用需要订阅多个事件，则要创建对应数量的订阅者。**
**多个publisher可以绑定同一个话题，往该话题推送消息。但一个subscriber只能订阅一个话题，如果应用需要订阅多个话题，则要创建对应数量的订阅者。**
> 对于电控程序目前的情况，不存在多个publisher向同一个事件推送消息的情况。
> 对于电控程序目前的情况，不存在多个publisher向同一个话题推送消息的情况。
**对于一个事件，其消息长度必须相同**。发布者和订阅者在注册时都会传入消息长度，用`sizof(your_data)`获取。应当保证不同的模块在进行交互式，使用相同的数据长度。
**对于相同话题，其消息长度必须相同**。发布者和订阅者在注册时都会传入消息长度，用`sizof(your_data)`获取。应当保证不同的模块在进行交互式，使用相同的数据长度。
### 发布者和订阅者注册的流程
- **发布者：**
遍历发布者的事件结点，如果发现相同的事件，直接返回指针即可；遍历完成后发现尚未创建则创建新的事件。
遍历发布者的话题结点，如果发现相同的话题，直接返回指针即可；遍历完成后发现尚未创建则创建新的话题。
<img src="../../assets/image-20221201152530558.png" alt="image-20221201152530558" style="zoom: 80%;" />
- **订阅者：**
需要注意，由于不同应用/模块的初始化顺序不同，可能出现订阅者先于发布者订阅某一消息的情况，所以要进行发布者链表的遍历，判断是否已经存在相同事件名的发布者，不存在则要先创建发布者结点再将新建订阅者结点并挂载到前者上。
需要注意，由于不同应用/模块的初始化顺序不同，可能出现订阅者先于发布者订阅某一消息的情况，所以要进行发布者链表的遍历，判断是否已经存在相同话题名的发布者，不存在则要先创建发布者结点再将新建订阅者结点并挂载到前者上。
<img src="../../assets/image-20221201152904044.png" alt="image-20221201152904044" style="zoom:80%;" />
@ -160,9 +152,9 @@ front=(front+1)%SIZE_OF_ARRAY;
- **发布者推送消息到指定事件**
- **发布者推送消息到指定话题**
通过发布者指针，将订阅了该事件的所有订阅者遍历，将新数据入队。
通过发布者指针，将订阅了该话题的所有订阅者遍历，将新数据入队。
- **订阅者获取消息**

2
modules/motor/LKmotor/LK9025.c
View File

@ -91,7 +91,7 @@ void LKMotorControl()
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_measure = measure->total_angle;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;