TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into referee

Browse Source
This commit is contained in:
Kidenygood
2023-04-17 16:22:02 +08:00
parent 11655fbe46 1818edf117
commit 6f54bc201b
524 changed files with 20243 additions and 139813 deletions
Download Patch File Download Diff File Expand all files Collapse all files

277
application/balance_chassis/balance.c
View File

@@ -1,6 +1,5 @@
// app
#include "balance.h"
#include "vmc_project.h"
#include "gain_table.h"
#include "robot_def.h"
#include "general_def.h"
@@ -12,77 +11,194 @@
#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 BMI088Instance *imu;
static attitude_t *imu_data;
// static BMI088Instance *imu;
static SuperCapInstance *super_cap;
static referee_info_t *referee_data; // 裁判系统的数据会被不断更新
static HTMotorInstance *lf;
static HTMotorInstance *rf;
static HTMotorInstance *lb;
static HTMotorInstance *rb;
static LKMotorInstance *l_driven;
static LKMotorInstance *r_driven;
static CANCommInstance *chassis_comm; // 底盘板和云台板通信
// 电机
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;
// 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率
/* 方便函数间无开销传递参数的中间变量 */
// 若将下面的封装函数取缔,则可以将这些变量放入BalanceTask函数体内.
// static ...
static float leg_len_l, leg_len_r; // 左右腿长(虚拟)
static float leg_angle_l, leg_angle_r; // 左右腿角度(虚拟)
// 倒立摆的虚拟力和虚拟力矩
static float F_virtual_left, T_virtual_left, F_virtual_right, T_virtual_right;
// 左前,左后,右前,右后关节力矩
static float T_joint_lf, T_joint_lr, T_joint_rf, T_joint_rb;
static float T_leg_left, T_leg_right; // 左右驱动电机力矩
// 两个腿的参数,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 由于两个连杆和轮子连接时,只有一个使用了轴承,另外一个直接和电机的定子连接,
* 所以轮子的转速和电机的转速不一致,因此和地面的速度应为电机转速减去杆的转速,得到的才是轮子的转速
*
* @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];
}
}
/**
* @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
* @todo 测试两种的效果,留下其中一种;
* 若差分的效果好则不需要VELOCITY_DIFF_VMC内的代码
*
* @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);
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 = 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->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 - 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!
#endif
p->wheel_w = (p->wheel_w - p->phi2_w + imu_data->Gyro[3]) * WHEEL_RADIUS; // 修正轮速
}
/**
* @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
* 由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算
*
* @note 计算得到的腿部力矩输出还要再综合运动控制系统补偿后映射为两个关节电机
* 而轮子的输出则只经过转向PID的反馈增益计算
*
* @todo 确定使用查表还是多项式拟合
*
*/
static void CalcLQR()
{
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 + 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];
}
p->T_wheel = T[0];
p->T_pod = T[1];
}
/**
* @brief 将LQR的输出映射到关节和驱动电机的输出
*
* @brief 腿部控制:抗劈叉; 轮子控制:转向
* @todo 确定方向
*/
static void VMCProject()
{ // 拟将功能封装到vmc_project.h中
}
/**
* @brief 腿部角度控制:转向和抗劈叉
*
*/
static PIDInstance swerving_pid; // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
static PIDInstance anti_crash_pid; // 抗劈叉,将输出以相反的方向叠加到左右腿的上
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 腿长控制:长度 + roll轴补偿(保持机体水平),用PD模拟弹簧的传递函数
* @brief 腿部控制:长度.用PD模拟弹簧的传递函数
*
*/
static PIDInstance leg_length_pid; // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
static void LegControl()
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);
}
/**
@@ -90,6 +206,7 @@ static void LegControl()
* 通过模型解算地面的支持力完成离地检测
*
*/
static uint8_t air_flag;
static void FlyDetect()
{
}
@@ -100,12 +217,17 @@ static void FlyDetect()
*/
static void WattLimit()
{
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 = {
// IMU初始化
.spi_acc_config = {
.GPIOx = GPIOC,
.cs_pin = GPIO_PIN_4,
@@ -116,16 +238,6 @@ void BalanceInit()
.cs_pin = GPIO_PIN_4,
.spi_handle = &hspi1,
},
.acc_int_config = {
.exti_mode = EXTI_TRIGGER_FALLING,
.GPIO_Pin = GPIO_PIN_10,
.GPIOx = GPIOA,
},
.gyro_int_config = {
.exti_mode = EXTI_TRIGGER_FALLING,
.GPIO_Pin = GPIO_PIN_11,
.GPIOx = GPIOA,
},
.heat_pid_config = {
.Kp = 0.0f,
.Kd = 0.0f,
@@ -141,6 +253,7 @@ void BalanceInit()
.work_mode = BMI088_BLOCK_PERIODIC_MODE,
};
// imu = BMI088Register(&imu_config);
imu_data = INS_Init();
SuperCap_Init_Config_s cap_conf = {
// 超级电容初始化
@@ -165,7 +278,6 @@ void BalanceInit()
rb = HTMotorInit(&joint_conf);
// ↓↓↓---------------驱动电机初始化----------------↓↓↓
Motor_Init_Config_s driven_conf = {
// 写一个,剩下的修改方向和id即可
.can_init_config.can_handle = &hcan1,
@@ -184,12 +296,11 @@ void BalanceInit()
.close_loop_type = CURRENT_LOOP,
},
.motor_type = LK9025,
};
driven_conf.can_init_config.tx_id=1;
driven_conf.can_init_config.tx_id = 1;
l_driven = LKMotorInit(&driven_conf);
driven_conf.can_init_config.tx_id=2;
driven_conf.can_init_config.tx_id = 2;
r_driven = LKMotorInit(&driven_conf);
CANComm_Init_Config_s chassis_comm_conf = {
@@ -204,7 +315,7 @@ void BalanceInit()
};
chassis_comm = CANCommInit(&chassis_comm_conf);
referee_data = RefereeInit(&huart6); // 裁判系统串口
// referee_data = RefereeInit(&huart6); // 裁判系统串口
// ↓↓↓---------------综合运动控制----------------↓↓↓
PID_Init_Config_s swerving_pid_conf = {
@@ -251,4 +362,50 @@ void BalanceInit()
/* 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(); // 滞空检测
WattLimit(); // 电机输出限幅
// code to go here... 裁判系统,UI,多机通信
CANCommSend(chassis_comm, (uint8_t *)&chassis_feed_send);
}

38
application/balance_chassis/balance.h
View File

@@ -1,17 +1,43 @@
#pragma once
// 底盘参数
#define CALF_LEN 0.25f // 小腿
#define THIGH_LEN 0.15f // 大腿
#define JOINT_DISTANCE 0.108f // 关节间距
#define WHEEL_RADIUS 0.032f // 轮子半径
#define LIMIT_LINK_RAD 0.358 //
// 计算速度的方式(五连杆到单杆的映射)
#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();

4
application/balance_chassis/gain_table.h
View File

@@ -19,7 +19,7 @@ void EnalbeInterpolation(void)
}
/* 默认关闭插值,向下取整 */
float LookUpKgain(float leg_length)
float* LookUpKgain(float leg_length)
{
}

14
application/balance_chassis/vmc_project.h
View File

@@ -1,14 +0,0 @@
#ifndef VMC_PROJECT_H
#define VMC_PROJECT_H
#include "stm32f407xx.h"
#include "arm_math.h"
#include "math.h"
#include "general_def.h"
// 将五连杆和直杆的vmc映射放在此处,方便修改和调试,balance.c不会太长
#endif // !VMC_PROJECT_H

45
application/chassis/chassis.c
View File

@@ -43,18 +43,14 @@ attitude_t *Chassis_IMU_data;
#ifdef ONE_BOARD
static Publisher_t *chassis_pub; // 用于发布底盘的数据
static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令
#endif // !ONE_BOARD
#endif // !ONE_BOARD
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
// static referee_info_t *referee_data; // 裁判系统相关数据
static SuperCapInstance *cap; // 超级电容
static DJIMotorInstance *motor_lf; // left right forward back
static DJIMotorInstance *motor_rf;
static DJIMotorInstance *motor_lb;
static DJIMotorInstance *motor_rb;
static SuperCapInstance *cap; // 超级电容
static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
/* 用于自旋变速策略的时间变量,后续考虑查表加速 */
/* 用于自旋变速策略的时间变量 */
// static float t;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
@@ -68,19 +64,19 @@ void ChassisInit()
.can_init_config.can_handle = &hcan1,
.controller_param_init_config = {
.speed_PID = {
.Kp = 10,//4.5
.Ki = 0,//0
.Kd = 0,//0
.Kp = 10, // 4.5
.Ki = 0, // 0
.Kd = 0, // 0
.IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 12000,
},
.current_PID = {
.Kp = 0.5,//0.4
.Ki = 0,//0
.Kp = 0.5, // 0.4
.Ki = 0, // 0
.Kd = 0,
.IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 15000,
},
},
@@ -114,7 +110,6 @@ void ChassisInit()
// while (referee_data->GameRobotState.robot_id ==0);
// Referee_Interactive_init(referee_data);
SuperCap_Init_Config_s cap_conf = {
.can_config = {
.can_handle = &hcan2,
@@ -145,10 +140,10 @@ void ChassisInit()
#endif // ONE_BOARD
}
#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
/**
* @brief 计算每个轮毂电机的输出,正运动学解算
* 用宏进行预替换减小开销,运动解算具体过程参考教程
@@ -157,7 +152,7 @@ static void MecanumCalculate()
{
vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
vt_lb = chassis_vx - chassis_vy -chassis_cmd_recv.wz * LB_CENTER;
vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
}
@@ -224,10 +219,10 @@ void ChassisTask()
chassis_cmd_recv.wz = 0;
break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
chassis_cmd_recv.wz = -1.5*chassis_cmd_recv.offset_angle*abs(chassis_cmd_recv.offset_angle);
chassis_cmd_recv.wz = -1.5 * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz=4000;
chassis_cmd_recv.wz = 4000;
break;
default:
break;
@@ -236,8 +231,8 @@ void ChassisTask()
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
static float sin_theta, cos_theta;
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;

42
application/gimbal/gimbal.c
View File

@@ -7,16 +7,15 @@
#include "bmi088.h"
static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor; // yaw电机
static DJIMotorInstance *pitch_motor; // pitch电机
static attitude_t *gimba_IMU_data; // 云台IMU数据
static DJIMotorInstance *yaw_motor, *pitch_motor;
static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
BMI088Instance* imu;
BMI088Instance *imu;
void GimbalInit()
{
BMI088_Init_Config_s imu_config = {
@@ -54,8 +53,8 @@ void GimbalInit()
.cali_mode = BMI088_CALIBRATE_ONLINE_MODE,
.work_mode = BMI088_BLOCK_PERIODIC_MODE,
};
// imu=BMI088Register(&imu_config);
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
imu = BMI088Register(&imu_config);
// gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
// YAW
Motor_Init_Config_s yaw_config = {
.can_init_config = {
@@ -64,26 +63,26 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kp = 8, //8
.Kp = 8, // 8
.Ki = 0,
.Kd = 0,
.DeadBand = 0.1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100,
.MaxOut = 500,
},
.speed_PID = {
.Kp = 50,//50
.Ki = 200,//200
.Kp = 50, // 50
.Ki = 200, // 200
.Kd = 0,
.Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 20000,
},
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2],
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
},
.controller_setting_init_config = {
.angle_feedback_source = OTHER_FEED,
@@ -109,11 +108,11 @@ void GimbalInit()
.MaxOut = 500,
},
.speed_PID = {
.Kp=50,//50
.Ki =350,//350
.Kd =0,//0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
.IntegralLimit =2500,
.Kp = 50, // 50
.Ki = 350, // 350
.Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 2500,
.MaxOut = 20000,
},
.other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
@@ -136,7 +135,7 @@ void GimbalInit()
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
}
int aaaaaaa;
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
@@ -178,17 +177,14 @@ void GimbalTask()
default:
break;
}
// if(yaw_motor->motor_measure.total_angle>120)
// {
// aaaaaaa++;
// }
// 在合适的地方添加pitch重力补偿前馈力矩
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
// ...
// 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
// 推送消息
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);

21
application/robot.c
View File

@@ -10,7 +10,6 @@
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
#include "chassis.h"
#include "referee.h"
#endif
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
@@ -19,6 +18,11 @@
#include "robot_cmd.h"
#endif
#ifdef BALANCE_BAORD
#include "balance.h"
#endif // BALANCE_BOARD
void RobotInit()
{
// 关闭中断,防止在初始化过程中发生中断
@@ -35,9 +39,13 @@ void RobotInit()
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
// ChassisInit();
ChassisInit();
#endif
#ifdef BALANCE_BAORD
BalanceInit();
#endif // BALANCE_BA
// 初始化完成,开启中断
__enable_irq();
Referee_Interactive_init();
@@ -52,7 +60,10 @@ void RobotTask()
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
// ChassisTask();
// Referee_Interactive_task();
ChassisTask();
#endif
#ifdef BALANCE_BAORD
BalanceTask();
#endif // BALANCE_BA
}

15
application/robot_def.h
View File

@@ -131,11 +131,11 @@ typedef enum
LOAD_BURSTFIRE, // 连发
} loader_mode_e;
// 功率限制,从裁判系统获取
// 功率限制,从裁判系统获取,是否有必要保留?
typedef struct
{ // 功率控制
float chassis_power_mx;
} Chassis_Power_Data_s;
} Chassis_Power_Data_s;
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
/**
@@ -188,10 +188,9 @@ typedef struct
/* @todo : 对于平衡底盘,需要新增控制模式和控制数据 */
typedef struct
{
#ifdef CHASSIS_BOARD
attitude_t chassis_imu_data;
#endif // CHASSIS_BOARD
#if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要)
// attitude_t chassis_imu_data;
#endif
// 后续增加底盘的真实速度
// float real_vx;
// float real_vy;
@@ -200,9 +199,7 @@ typedef struct
uint8_t rest_heat; // 剩余枪口热量
Bullet_Speed_e bullet_speed; // 弹速限制
Enemy_Color_e enemy_color; // 0 for blue, 1 for red
// 是否需要剩余电量?(电容)
} Chassis_Upload_Data_s;
/* @todo : 对于平衡底盘,需要不同的反馈数据 */

49
application/shoot/shoot.c
View File

@@ -7,9 +7,7 @@
#include "general_def.h"
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *friction_l; // 左摩擦轮
static DJIMotorInstance *friction_r; // 右摩擦轮
static DJIMotorInstance *loader; // 拨盘电机
static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
// static servo_instance *lid; 需要增加弹舱盖
static Publisher_t *shoot_pub;
@@ -29,16 +27,16 @@ void ShootInit()
},
.controller_param_init_config = {
.speed_PID = {
.Kp = 0,//20
.Ki = 0,//1
.Kp = 0, // 20
.Ki = 0, // 1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 10000,
.MaxOut = 15000,
},
.current_PID = {
.Kp = 0,//0.7
.Ki = 0,//0.1
.Kp = 0, // 0.7
.Ki = 0, // 0.1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 10000,
@@ -54,13 +52,13 @@ void ShootInit()
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
},
.motor_type = M3508};
friction_config.can_init_config.tx_id = 1,
friction_l = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
friction_r = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 1,
friction_l = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
friction_r = DJIMotorInit(&friction_config);
// 拨盘电机
Motor_Init_Config_s loader_config = {
.can_init_config = {
@@ -70,22 +68,22 @@ void ShootInit()
.controller_param_init_config = {
.angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kp = 0,//10
.Kp = 0, // 10
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
},
.speed_PID = {
.Kp = 0,//10
.Ki = 0,//1
.Kp = 0, // 10
.Ki = 0, // 1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 5000,
},
.current_PID = {
.Kp = 0,//0.7
.Ki = 0,//0.1
.Kp = 0, // 0.7
.Ki = 0, // 0.1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
@@ -140,16 +138,16 @@ void ShootTask()
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
break;
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 150; // 完成1发弹丸发射的时间
break;
// 三连发,如果不需要后续可能删除
case LOAD_3_BULLET:
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 300; // 完成3发弹丸发射的时间
break;
@@ -170,7 +168,6 @@ void ShootTask()
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
}
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
{
@@ -193,7 +190,7 @@ void ShootTask()
DJIMotorSetRef(friction_l, 30000);
DJIMotorSetRef(friction_r, 30000);
break;
}
}
}
else // 关闭摩擦轮
{