增加了平衡步兵的策略控制,修复了message_center的严重错误,务必同步此更新!
This commit is contained in:
NeoZng
parent
f3d47258cd
commit
a196a9cb59
|
|
@ -70,8 +70,6 @@ void StartDAEMONTASK(void const *argument);
|
|||
|
||||
void StartROBOTTASK(void const *argument);
|
||||
|
||||
void StartROBOTTASK(void const *argument);
|
||||
|
||||
void StartUITASK(void const *argument);
|
||||
/* USER CODE END FunctionPrototypes */
|
||||
|
||||
|
|
@ -175,7 +173,7 @@ void StartINSTASK(void const *argument)
|
|||
while (1)
|
||||
{
|
||||
// 1kHz
|
||||
// INS_Task();
|
||||
INS_Task();
|
||||
osDelay(1);
|
||||
}
|
||||
}
|
||||
|
|
@ -206,7 +204,7 @@ void StartROBOTTASK(void const *argument)
|
|||
{
|
||||
// 200Hz
|
||||
RobotTask();
|
||||
osDelay(5); // syh此处暂时将时间改<E997B4>?10ms，原因在于未使用缓冲区发送，发<EFBC8C>?<3F>时延时5ms
|
||||
osDelay(5);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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.wheel_dist = 0.5*imu_data->Accel[3];
|
||||
// right_side.wheel_dist = 0.5*imu_data->Accel[3];
|
||||
}
|
||||
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;
|
||||
} // 转向的时候不需要修正轮速
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -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,15 +98,14 @@ 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
|
||||
p->theta = p->phi5 - 0.5 * PI - imu_data->Pitch;
|
||||
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
|
||||
|
|
@ -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->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
|
||||
|
||||
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 + 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];
|
||||
}
|
||||
|
|
@ -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,17 +410,17 @@ 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,多机通信
|
||||
|
||||
|
|
|
|||
|
|
@ -1,11 +1,11 @@
|
|||
#pragma once
|
||||
|
||||
// 底盘参数
|
||||
#define CALF_LEN 0.25f // 小腿
|
||||
#define THIGH_LEN 0.15f // 大腿
|
||||
#define CALF_LEN 0.245f // 小腿
|
||||
#define THIGH_LEN 0.14f // 大腿
|
||||
#define JOINT_DISTANCE 0.108f // 关节间距
|
||||
#define WHEEL_RADIUS 0.032f // 轮子半径
|
||||
#define LIMIT_LINK_RAD 0.358 //
|
||||
#define WHEEL_RADIUS 0.069f // 轮子半径
|
||||
#define LIMIT_LINK_RAD 0.15149458 // 初始限位角度,见ParamAssemble
|
||||
|
||||
// 计算速度的方式(五连杆到单杆的映射)
|
||||
#define VELOCITY_DIFF_VMC // 通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
|
||||
|
|
|
|||
|
|
@ -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的二重积分，从而避免平衡控制器和转向控制器冲突
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue