From 4658a0b5ea7239b301e5d1684645887db8664354 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Thu, 20 Apr 2023 17:54:12 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=B9=B3=E8=A1=A1?= =?UTF-8?q?=E6=AD=A5=E5=85=B5=E7=9A=84=E7=AD=96=E7=95=A5=E6=8E=A7=E5=88=B6?= =?UTF-8?q?,=E4=BF=AE=E5=A4=8D=E4=BA=86message=5Fcenter=E7=9A=84=E4=B8=A5?= =?UTF-8?q?=E9=87=8D=E9=94=99=E8=AF=AF,=E5=8A=A1=E5=BF=85=E5=90=8C?= =?UTF-8?q?=E6=AD=A5=E6=AD=A4=E6=9B=B4=E6=96=B0!?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Src/freertos.c | 2 +- application/balance_chassis/balance.c | 113 ++++++++++++++---------- application/balance_chassis/balance.h | 2 +- application/balance_chassis/balance.md | 10 ++- modules/message_center/message_center.c | 2 +- 5 files changed, 77 insertions(+), 52 deletions(-) diff --git a/Src/freertos.c b/Src/freertos.c index a743c86..c8ca5c9 100644 --- a/Src/freertos.c +++ b/Src/freertos.c @@ -206,7 +206,7 @@ void StartROBOTTASK(void const *argument) { // 200Hz RobotTask(); - osDelay(5); // syh此处暂时将时间改�?10ms，原因在于未使用缓冲区发送，发�?�时延时5ms + osDelay(5); } } diff --git a/application/balance_chassis/balance.c b/application/balance_chassis/balance.c index 7f9ec0e..f4bc528 100644 --- a/application/balance_chassis/balance.c +++ b/application/balance_chassis/balance.c @@ -7,11 +7,11 @@ #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" -#include "rm_referee.h" // standard #include "stdint.h" #include "arm_math.h" // 需要用到较多三角函数 @@ -25,6 +25,7 @@ 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; @@ -45,30 +46,32 @@ static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水 * @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; - - if(chassis_cmd_recv.wz != 0) // 若有转向指令,则使用IMU积分得到的位置,否则左右轮位置相反会产生阻碍转向的力矩 +{ + 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的平衡环只对机体的速度响应 { - // left_side.wheel_dist = 0.5*imu_data->Accel[3]; - // right_side.wheel_dist = 0.5*imu_data->Accel[3]; - } + 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; + } // 转向的时候不需要修正轮速 } /** @@ -76,6 +79,13 @@ static void ParamAssemble() * @todo 测试两种的效果,留下其中一种; * 若差分的效果好则不需要VELOCITY_DIFF_VMC内的代码 * + * @note 右侧视图 + * ___x anti-clockwise is positive for motor + * | _____ + * |y / \ + * \ / + * \ / + * * @param p 5连杆和腿的参数 */ static void Link2Pod(LinkNPodParam *p) @@ -88,11 +98,10 @@ static void Link2Pod(LinkNPodParam *p) 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); - xC = xB + CALF_LEN * arm_cos_f32(phi2t); - yC = yB + CALF_LEN * arm_sin_f32(phi2t); + 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->phi2 = phi2t; 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 @@ -118,11 +127,12 @@ static void Link2Pod(LinkNPodParam *p) 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 - p->theta) / balance_dt; - p->height_v = p->pod_v * arm_cos_f32(p->theta) - p->pod_len * arm_sin_f32(p->theta) * p->theta_w; // 这很酷!PDE! + p->theta_w = (phi5t - 0.5 * PI - imu_data->Pitch - 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 - - p->wheel_w = (p->wheel_w - p->phi2_w + imu_data->Gyro[3]) * WHEEL_RADIUS; // 修正轮速 + 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); + // 此时使用的是电机编码器反馈的速度 } /** @@ -143,11 +153,13 @@ static void CalcLQR(LinkNPodParam *p, float target_x) { T[i] = gain_list[i * 6 + 0] * -p->theta + gain_list[i * 6 + 1] * -p->theta_w + - gain_list[i * 6 + 2] * (target_x - p->wheel_dist) + - gain_list[i * 6 + 3] * -p->wheel_w + gain_list[i * 6 + 4] * -imu_data->Pitch + - gain_list[i * 6 + 5] * -imu_data->Gyro[3]; - } + 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]; } @@ -170,6 +182,7 @@ static void SynthesizeMotion() /** * @brief 腿部控制:长度.用PD模拟弹簧的传递函数 * + * @note 实际使用的是机体当前的高度,若使用腿长计算则会导致加速和减速时因腿倾斜故而机体高度变化 */ static void LegControl(LinkNPodParam *p, float target_length) { @@ -212,17 +225,21 @@ static void FlyDetect() } /** - * @brief 功率限制,一般不需要 + * @brief 功率限制,一般不需要;限制完成之后设定电机输出 * */ -static void WattLimit() +static void WattLimitSet() { - 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); + // 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() @@ -315,7 +332,7 @@ void BalanceInit() }; chassis_comm = CANCommInit(&chassis_comm_conf); - // referee_data = RefereeInit(&huart6); // 裁判系统串口 + referee_data = Referee_Interactive_init(&huart6,&my_uidata); // 裁判系统串口 // ↓↓↓---------------综合运动控制----------------↓↓↓ PID_Init_Config_s swerving_pid_conf = { @@ -393,19 +410,19 @@ void BalanceTask() // 腿长控制 LegControl(&left_side, 0); LegControl(&right_side, 0); - - SynthesizeMotion(); // 综合运动控制,转向+抗劈叉 - - RollCompensate(); // 俯仰角补偿,保持机体水平 + // 综合运动控制,转向+抗劈叉 + SynthesizeMotion(); // 两边要一起 + // 俯仰角补偿,保持机体水平 + RollCompensate(); // 两边也要一起 // VMC映射成关节输出 VMCProject(&left_side); VMCProject(&right_side); - FlyDetect(); // 滞空检测 + FlyDetect(); // 滞空检测,尝试使用加速度计自由落体时量测值小? - WattLimit(); // 电机输出限幅 + WattLimitSet(); // 电机输出限幅 // code to go here... 裁判系统,UI,多机通信 CANCommSend(chassis_comm, (uint8_t *)&chassis_feed_send); -} +} \ No newline at end of file diff --git a/application/balance_chassis/balance.h b/application/balance_chassis/balance.h index f6e0e11..138e0a0 100644 --- a/application/balance_chassis/balance.h +++ b/application/balance_chassis/balance.h @@ -5,7 +5,7 @@ #define THIGH_LEN 0.15f // 大腿 #define JOINT_DISTANCE 0.108f // 关节间距 #define WHEEL_RADIUS 0.032f // 轮子半径 -#define LIMIT_LINK_RAD 0.358 // +#define LIMIT_LINK_RAD 0.15149458 // 初始限位角度,见ParamAssemble // 计算速度的方式(五连杆到单杆的映射) #define VELOCITY_DIFF_VMC // 通过速度计算增量,然后通过差分计算腿长变化率和腿角速度 diff --git a/application/balance_chassis/balance.md b/application/balance_chassis/balance.md index 6c5392d..e653b73 100644 --- a/application/balance_chassis/balance.md +++ b/application/balance_chassis/balance.md @@ -6,7 +6,7 @@ ## 工作流程 -1. 获取控制信息 +1. 获取控制信息和状态信息，组装到linkparam 2. 根据控制模式将控制指令转化为实际的参考输入 3. 使用lqr得出的反馈增益,计算二阶倒立摆模型的控制输出;需要根据当前腿长查gain table,或预先拟合K=f(Leg)的函数 4. 计算二阶倒立摆$[L0 phi0]$和轮腿[phi1 phi4]间的雅可比,根据VMC将lqr的输出[F Tp]映射成[T1 T2] ; 驱动轮不需要映射 @@ -22,3 +22,11 @@ 如果功率可能超限,需要判定降低功率输出后受影响最小的执行单元,并给予其较大的功率输出衰减(一般不会超功率) 另外, 选择平衡底盘有枪口冷却增益, 注意将这一部分改变反馈给cmd, 以使得shoot有更好的表现 + +## 优化环节 + +为了控制系统有更好的效果，对工程上的细节有一些微小的优化如下： + +1. 腿长控制实际上对机体高度计算腿长闭环，使得云台能够保持恒定高度 +2. 静止时开启位置反馈，若有速度输入则不使用位置反馈，从而避免底盘打滑的情况 +3. 没有转向输入时使用轮式里程计计算位置x，有转向输入时使用imu的二重积分，从而避免平衡控制器和转向控制器冲突 \ No newline at end of file diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c index ce62f64..4d09a96 100644 --- a/modules/message_center/message_center.c +++ b/modules/message_center/message_center.c @@ -62,7 +62,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len) ret->data_len = data_len; // 设定数据长度 for (size_t i = 0; i < QUEUE_SIZE; ++i) { // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度 - ret->queue[i] = malloc(sizeof(data_len)); + ret->queue[i] = malloc(data_len); } // 如果是第一个订阅者,特殊处理一下,将first_subs指针指向新建的订阅者(详见文档) if (pub->first_subs == NULL)