xm/god-yuan-hero
xm/god-yuan-hero
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

防止被王玉庆删掉备份

Browse Source
This commit is contained in:
yxming66 2025-11-02 16:20:15 +08:00
parent 873d5937cb
commit 218f8f62e7
10 changed files with 1173 additions and 328 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -76,7 +76,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/module/config.c
User/module/gimbal.c
User/module/shoot.c
User/module/cmd.c
# User/task sources
User/task/atti_esti.c
User/task/blink.c

461
User/module/chassis.c
View File

@ -1,208 +1,299 @@
/*
底盘模组
*/
#include "chassis.h"
/*
底盘模组
*/
#include "can.h"
#include "device/at9s_pro.h"
#include "component/pid.h"
#include "cmsis_os2.h"
#include <stdlib.h>
#include "bsp/mm.h"
#include "bsp/can.h"
#include "component/ahrs.h"
#include "device/motor_rm.h"
#include <math.h>
#include "bsp/time.h"
#include "device/motor.h"
#include "module/chassis.h"
KPID_t pid_wheel[4];
KPID_Params_t pid_wheel_params={1, 0.8, 0.0, 0.0, 0.0, 0.8,-1.0,-1.0};
KPID_t pid_lockagl[4];
KPID_Params_t pid_lockagl_params={1, 0.8, 0.05, 0.0, 0.1, 0.8,-1.0,M_2PI};
KPID_t pid_lockomg[4];
KPID_Params_t pid_lockomg_params={2, 1, 0.0, 0.01, 0.0, 0.8,-1.0,-1.0};
/**
* @brief 底盘小陀螺模式相关参数
*/
#define CHASSIS_ROTOR_WZ_MIN 0.6f //小陀螺最小速度
#define CHASSIS_ROTOR_WZ_MAX 0.8f //小陀螺最大速度
#define M_7OVER72PI (M_2PI * 7.0f / 72.0f) //角度偏移量（用在跟随云台35°）
#define CHASSIS_ROTOR_OMEGA 0.001f //角速度变化频率
KPID_t pid_follow;
KPID_Params_t pid_follow_params={0.5, 1, 0, 0.0, 0, 0.0, -1.0, -1};
MOTOR_RM_Param_t motor1to4_param[4]={
{.can=BSP_CAN_1,.id=0x201,.module=MOTOR_M3508,.reverse=false,.gear=true,},
{.can=BSP_CAN_1,.id=0x202,.module=MOTOR_M3508,.reverse=false,.gear=true,},
{.can=BSP_CAN_1,.id=0x203,.module=MOTOR_M3508,.reverse=false,.gear=true,},
{.can=BSP_CAN_1,.id=0x204,.module=MOTOR_M3508,.reverse=false,.gear=true,}
};
MOTOR_Feedback_t motor1to4_measure[4];
float lockagl_out[4];
float chassis_out[4];
float Wheel_status[4];
void Inverse_resolve(float vx,float vy,float w)
{
Wheel_status[0]=vx-vy+w;
Wheel_status[1]=vx+vy+w;
Wheel_status[2]=-vx+vy+w;
Wheel_status[3]=-vx-vy+w;
}
void Motor_Init(float freq)
{ for(int i=0;i<4;i++)
{
PID_Init(&pid_wheel[i], KPID_MODE_CALC_D, freq,&pid_wheel_params);
PID_Init(&pid_lockagl[i], KPID_MODE_CALC_D, freq,&pid_lockagl_params);
PID_Init(&pid_lockomg[i], KPID_MODE_CALC_D, freq,&pid_lockomg_params);
/**
* @brief 设置底盘模式
* @param c 底盘结构体指针
* @param mode 目标控制模式
* @param now 当前时间戳(ms)
* @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
*/
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, uint32_t now) {
if (!c)
return CHASSIS_ERR_NULL;
if (mode == c->mode)
return CHASSIS_OK;
//随机种子，小陀螺模式随机设置旋转方向
if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
srand(now);
c->wz_multi = (rand() % 2) ? -1 : 1;
}
//重置PID和滤波
for (uint8_t i = 0; i < c->num_wheel; i++) {
PID_Reset(&c->pid.motor[i]);
LowPassFilter2p_Reset(&c->filter.in[i], 0.0f);
LowPassFilter2p_Reset(&c->filter.out[i], 0.0f);
}
c->mode = mode;
return CHASSIS_OK;
}
PID_Init(&pid_follow, KPID_MODE_NO_D, freq,&pid_follow_params);
/**
* @brief 小陀螺模式动态角速度
* @param min 最小速度
* @param max 最大速度
* @param now 当前时间戳(ms)
* @return 角速度值
*/
static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
float wz_vary = fabsf(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
return (wz_vary > max) ? max : wz_vary;
}
/**
* @brief 底盘模式初始化
* @param c 底盘结构体指针
* @param param 底盘参数结构体指针
* @param mech_zero 机械零点欧拉角
* @param target_freq 控制频率(Hz)
* @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空 CHASSIS_ERR_TYPE:不支持的模式
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
float target_freq) {
if (!c) return CHASSIS_ERR_NULL;
//初始化CAN通信
BSP_CAN_Init();
for(int i=0;i<4;i++){
MOTOR_RM_Register(&motor1to4_param[i]);
c->param = param;
c->mode = CHASSIS_MODE_RELAX;
//根据底盘不同设置模式轮子与混合器
Mixer_Mode_t mixer_mode;
switch (param->type) {
case CHASSIS_TYPE_MECANUM://麦轮
c->num_wheel = 4;
mixer_mode = MIXER_MECANUM;
break;
case CHASSIS_TYPE_PARLFIX4:
c->num_wheel = 4;
mixer_mode = MIXER_PARLFIX4;
break;
case CHASSIS_TYPE_PARLFIX2:
c->num_wheel = 2;
mixer_mode = MIXER_PARLFIX2;
break;
case CHASSIS_TYPE_OMNI_CROSS:
c->num_wheel = 4;
mixer_mode = MIXER_OMNICROSS;
break;
case CHASSIS_TYPE_OMNI_PLUS: //全向轮（老步兵类型）
c->num_wheel = 4;
mixer_mode = MIXER_OMNIPLUS;
break;
case CHASSIS_TYPE_SINGLE:
c->num_wheel = 1;
mixer_mode = MIXER_SINGLE;
break;
default:
return CHASSIS_ERR_TYPE;
}
//初始化时间戳
c->last_wakeup = 0;
c->dt = 0.0f;
//初始化PID和滤波
for (uint8_t i = 0; i < c->num_wheel; i++) {
PID_Init(&c->pid.motor[i], KPID_MODE_NO_D, target_freq, &param->pid.motor_pid_param);
LowPassFilter2p_Init(&c->filter.in[i], target_freq, param->low_pass_cutoff_freq.in);
LowPassFilter2p_Init(&c->filter.out[i], target_freq, param->low_pass_cutoff_freq.out);
//清零电机反馈
c->feedback.motor[i].rotor_speed = 0;
c->feedback.motor[i].torque_current = 0;
c->feedback.motor[i].rotor_abs_angle = 0;
c->feedback.motor[i].temp = 0;
}
//初始化PID和混合器
PID_Init(&c->pid.follow, KPID_MODE_NO_D, target_freq, &param->pid.follow_pid_param);
Mixer_Init(&c->mixer, mixer_mode);
//清零运动向量和输出
c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;
for (uint8_t i = 0; i < c->num_wheel; i++) {
c->out.motor[i] = 0.0f;
}
//注册大疆电机
for (int i = 0; i < c->num_wheel; i++) {
MOTOR_RM_Register(&(c->param->motor_param[i]));
}
return CHASSIS_OK;
}
int64_t now,last_wakeup;
float dt;
float vx,vy,w;float chassis_yaw;
float speed[4];
float lockagl[4]={0,0,0,0};
void chassis_control(chassis_ctrl_eulr_t eulr, COMP_AT9S_CMD_t cmd_rc)//k:ת<><D7AA><EFBFBD>޷<EFBFBD>
{
//float vx,vy,w;
now =BSP_TIME_Get_us() / 1000000.0f;
dt =(BSP_TIME_Get_us() - last_wakeup) / 1000000.0f;
last_wakeup =BSP_TIME_Get_us();
float delta_angle;
float cos_delta_angle;
float sin_delta_angle;
for(int i=0;i<4;i++){
MOTOR_RM_Update(&motor1to4_param[i]);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&motor1to4_param[i]);
if(motor_fed!=NULL)
/**
* @brief 更新电机反馈(IMU+电机状态)
* @param c 底盘结构体指针
* @param feedback 底盘反馈指针结构体
* @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
//更新所有电机反馈
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_Update(&(c->param->motor_param[i]));
MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(c->param->motor_param[i]));
c->motors[i] = rm_motor;
MOTOR_RM_t *rm = c->motors[i];
if (rm_motor != NULL) {
c->feedback.motor[i] = rm_motor->feedback;
}else
{
motor1to4_measure[i]=motor_fed->motor.feedback;
return CHASSIS_ERR_NULL;
}
}
switch(cmd_rc.mode){
case 0:
vx=0;vy=0;w=0;
break;
case 1:
delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
cos_delta_angle = cosf(delta_angle);
sin_delta_angle = sinf(delta_angle);
vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw,
eulr.chassis_encoder_yaw, 0.0f, dt);
break;
case 2:
PID_ResetIntegral(&pid_follow);
delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
cos_delta_angle = cosf(delta_angle);
sin_delta_angle = sinf(delta_angle);
vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
w=PID_Calc(&pid_follow, eulr.chassis_mech_zero_yaw,
eulr.chassis_encoder_yaw, 0.0f, dt);
break;
case 3:
delta_angle = eulr.chassis_encoder_yaw - eulr.chassis_mech_zero_yaw;
cos_delta_angle = cosf(delta_angle);
sin_delta_angle = sinf(delta_angle);
vx =cos_delta_angle * cmd_rc.chassis_ctrl_vec.x - sin_delta_angle * cmd_rc.chassis_ctrl_vec.y;
vy =sin_delta_angle * cmd_rc.chassis_ctrl_vec.x + cos_delta_angle * cmd_rc.chassis_ctrl_vec.y;
w=0.6f;
break;
default :
break;
}
Inverse_resolve(vx, vy, w);
float abs_max = 0.0f;
for (int8_t i = 0; i < 4; i++) {
const float abs_val = fabsf(Wheel_status[i]);
abs_max = (abs_val > abs_max) ? abs_val : abs_max;
}
if (abs_max > 1.0f) {
for (int8_t i = 0; i < 4; i++) {
Wheel_status[i] /= abs_max;
}
}
// if(vx==0&&vy==0)PID_ResetIntegral(&pid_wheel);
switch(cmd_rc.mode){
case 0:
PID_ResetIntegral(&pid_wheel[0]);
PID_ResetIntegral(&pid_wheel[1]);
PID_ResetIntegral(&pid_wheel[2]);
PID_ResetIntegral(&pid_wheel[3]);
chassis_out[0] = 0;
chassis_out[1] = 0;
chassis_out[2] = 0;
chassis_out[3] = 0;
break;
case 1:
PID_ResetIntegral(&pid_wheel[0]);
PID_ResetIntegral(&pid_wheel[1]);
PID_ResetIntegral(&pid_wheel[2]);
PID_ResetIntegral(&pid_wheel[3]);
// chassis_out[0] = 0;
// chassis_out[1] = 0;
// chassis_out[2] = 0;
// chassis_out[3] = 0;
for(int i=0;i<4;i++){
speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
if(speed[i]>1)speed[i]=1;
else if(speed[i]<-1)speed[i]=-1;
lockagl_out[i] = PID_Calc(&pid_lockagl[i],lockagl[i],motor1to4_measure[i].rotor_abs_angle, 0,dt);
chassis_out[i] = PID_Calc(&pid_lockomg[i],lockagl_out[i],speed[i], 0,dt);
if(chassis_out[i]>0.8f)chassis_out[i]=0.8f;
}
break;
case 2:
for(int i=0;i<4;i++){
lockagl[i]=motor1to4_measure[i].rotor_abs_angle;
speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
if(speed[i]>1)speed[i]=1;
else if(speed[i]<-1)speed[i]=-1;
chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt);
}
break;
case 3:
for(int i=0;i<4;i++){
lockagl[i]=motor1to4_measure[i].rotor_abs_angle;
speed[i]=motor1to4_measure[i].rotor_speed*19.2032/8000.0f;
if(speed[i]>1)speed[i]=1;
else if(speed[i]<-1)speed[i]=-1;
chassis_out[i] = PID_Calc(&pid_wheel[i],Wheel_status[i],speed[i], 0,dt);
}
break;
default :
break;
}
// for(int i;i<4;i++){
// chassis_out[i]*=0.6;
// }
MOTOR_RM_SetOutput(&motor1to4_param[0], chassis_out[0]);
MOTOR_RM_SetOutput(&motor1to4_param[1], chassis_out[1]);
MOTOR_RM_SetOutput(&motor1to4_param[2], chassis_out[2]);
MOTOR_RM_SetOutput(&motor1to4_param[3], chassis_out[3]);
//MOTOR_RM_SetOutput(&motor1to4_param[1], 0);
// MOTOR_RM_SetOutput(&motor1to4_param[2], 0);
MOTOR_RM_Ctrl(&motor1to4_param[0]);
return CHASSIS_OK;
}
/**
* @brief 底盘电机控制
* @param c 底盘结构体指针
* @param c_cmd 控制命令
* @param now 当前时间戳(ms)
* @return CHASSIS_OK:成功 CHASSIS_ERR_NULL:空
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
if (!c || !c_cmd) return CHASSIS_ERR_NULL;
//计算控制周期
c->dt = (float)(now - c->last_wakeup) / 1000.0f;
c->last_wakeup = now;
if (!isfinite(c->dt) || c->dt <= 0.0f) {
c->dt = 0.001f;
}
if (c->dt < 0.0005f) c->dt = 0.0005f;
if (c->dt > 0.050f) c->dt = 0.050f;
//设置模式
Chassis_SetMode(c, c_cmd->mode, now);
//不同模式下对应解算（运动向量）
switch (c->mode) {
case CHASSIS_MODE_BREAK:
c->move_vec.vx = c->move_vec.vy = 0.0f;
break;
case CHASSIS_MODE_INDEPENDENT:
c->move_vec.vx = c_cmd->ctrl_vec.vx;
c->move_vec.vy = c_cmd->ctrl_vec.vy;
break;
default: { //遥控器坐标->机体坐标系
float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
float cosb = cosf(beta);
float sinb = sinf(beta);
c->move_vec.vx = cosb * c_cmd->ctrl_vec.vx - sinb * c_cmd->ctrl_vec.vy;
c->move_vec.vy = sinb * c_cmd->ctrl_vec.vx + cosb * c_cmd->ctrl_vec.vy;
break;
}
}
//根据模式计算底盘角速度
switch (c->mode) {
case CHASSIS_MODE_RELAX:
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_INDEPENDENT:
c->move_vec.wz = 0.0f;
break;
case CHASSIS_MODE_OPEN:
c->move_vec.wz = c_cmd->ctrl_vec.wz;
break;
//云台跟随
case CHASSIS_MODE_FOLLOW_GIMBAL:
c->move_vec.wz = PID_Calc(&c->pid.follow, c->mech_zero, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
break;
//云台跟随（偏移）
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
c->move_vec.wz = PID_Calc(&c->pid.follow,c->mech_zero +M_7OVER72PI, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
break;
//小陀螺
case CHASSIS_MODE_ROTOR:
c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
break;
}
//运动学逆解算，运动向量分解为电机转速
Mixer_Apply(&c->mixer, &c->move_vec, c->setpoint.motor_rpm, c->num_wheel, 500.0f);
for (uint8_t i = 0; i < c->num_wheel; i++) {
float rf = c->setpoint.motor_rpm[i];///目标转速
float fb = LowPassFilter2p_Apply(&c->filter.in[i], (float)c->feedback.motor[i].rotor_speed);
float out_current;
switch (c->mode) {
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_FOLLOW_GIMBAL:
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
case CHASSIS_MODE_ROTOR:
case CHASSIS_MODE_INDEPENDENT:
out_current = PID_Calc(&c->pid.motor[i], c->setpoint.motor_rpm[i], fb, 0.0f, c->dt);
break;
case CHASSIS_MODE_OPEN:
out_current = c->setpoint.motor_rpm[i] / 7000.0f;
break;
case CHASSIS_MODE_RELAX:
out_current = 0.0f;
break;
}
//低通滤波和限幅
c->out.motor[i] = LowPassFilter2p_Apply(&c->filter.out[i], out_current);
Clip(&c->out.motor[i], -c->param->limit.max_current, c->param->limit.max_current);
}
return CHASSIS_OK;
}
/**
* @brief 电机输出
* @param c 底盘结构体指针
*/
void Chassis_Output(Chassis_t *c) {
if (!c)
return;
//每个电机目标输出
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_t *rm = c->motors[i];
if (!rm) continue;
MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
}
//调用ctrl
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_t *rm = c->motors[i];
if (rm) {
MOTOR_RM_Ctrl(&rm->param);
}
}
}
/**
* @brief 重置底盘输出
* @param c 底盘结构体指针
*/
void Chassis_ResetOutput(Chassis_t *c) {
if (!c) return;
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_t *m = c->motors[i];
if (m) {
MOTOR_RM_Relax(&(m->param));
}
}
}

239
User/module/chassis.h
View File

@ -1,21 +1,238 @@
/*
底盘模组
*/
#pragma once
#ifndef CHASSIS_CONTROL_H
#define CHASSIS_CONTROL_H
#ifdef __cplusplus
extern "C" {
#endif
#include "main.h"
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "bsp/can.h"
#include "component\filter.h"
#include "component\mixer.h"
#include "component\pid.h"
#include "component\ahrs.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* 运行正常 */
#define CHASSIS_ERR (-1) /* 运行时出现了一些小错误 */
#define CHASSIS_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define CHASSIS_ERR_MODE (-3) /* 运行时出配置了错误的ChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* 运行时出配置了错误的Chassis_Type_t */
#include "component/at9s_pro_cmd.h"
/* ŷ<><C5B7><EFBFBD>ǣ<EFBFBD>Euler angle<6C><65> */
#define MAX_MOTOR_CURRENT 20.0f
/* 底盘控制模式 */
typedef enum {
CHASSIS_MODE_RELAX, /* 放松模式,电机不输出。一般情况底盘初始化之后的模式 */
CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制静止。用于机器人停止状态 */
CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */
CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35°跟随云台 */
CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */
CHASSIS_MODE_INDEPENDENT, /*独立模式。底盘运行不受云台影响 */
CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */
} Chassis_Mode_t;
/* 小陀螺转动模式 */
typedef enum {
ROTOR_MODE_CW, /* 顺时针转动 */
ROTOR_MODE_CCW, /* 逆时针转动 */
ROTOR_MODE_RAND, /* 随机转动 */
} Chassis_RotorMode_t;
/* 底盘控制命令 */
typedef struct {
float chassis_mech_zero_yaw;
float chassis_encoder_yaw;
} chassis_ctrl_eulr_t;
Chassis_Mode_t mode; /* 底盘运行模式 */
Chassis_RotorMode_t mode_rotor; /* 小陀螺转动模式 */
MoveVector_t ctrl_vec; /* 底盘控制向量 */
} Chassis_CMD_t;
void Inverse_resolve(float vx,float vy,float w);
void Motor_Init(float freq);
void chassis_control(chassis_ctrl_eulr_t eulr, COMP_AT9S_CMD_t cmd_rc);
/* 限位 */
typedef struct {
float max;
float min;
} Chassis_Limit_t;
/* 底盘类型（底盘的物理设计） */
typedef enum {
CHASSIS_TYPE_MECANUM, /* 麦克纳姆轮 */
CHASSIS_TYPE_PARLFIX4, /* 平行摆放的四个驱动轮 */
CHASSIS_TYPE_PARLFIX2, /* 平行摆放的两个驱动轮 */
CHASSIS_TYPE_OMNI_CROSS, /* 叉型摆放的四个全向轮 */
CHASSIS_TYPE_OMNI_PLUS, /* 十字型摆设的四个全向轮 */
CHASSIS_TYPE_DRONE, /* 无人机底盘 */
CHASSIS_TYPE_SINGLE, /* 单个摩擦轮 */
} Chassis_Type_t;
/* 底盘参数结构体,ALL初始化参数 */
typedef struct {
MOTOR_RM_Param_t motor_param[4];
struct {
KPID_Params_t motor_pid_param; /* 底盘电机PID参数 */
KPID_Params_t follow_pid_param; /* 跟随云台PID参数 */
} pid;
Chassis_Type_t type; /* 底盘类型，底盘的机械设计和轮子选型 */
/* 低通滤波器截至频率*/
struct {
float in; /* 输入 */
float out; /* 输出 */
} low_pass_cutoff_freq;
/* 电机反装，应该和云台设置相同*/
struct {
bool yaw;
} reverse;
struct {
float max_vx, max_vy, max_wz;
float max_current;
} limit;
} Chassis_Params_t;
typedef struct {
AHRS_Gyro_t gyro;
AHRS_Eulr_t eulr;
} Chassis_IMU_t;
typedef struct {
MOTOR_Feedback_t motor[4]; // 四个 3508电机 反馈
float encoder_gimbalYawMotor;
} Chassis_Feedback_t;
/* 底盘输出结构体*/
typedef struct {
float motor[4];
} Chassis_Output_t;
/*
* 运行的主结构体￣
* 包含初始化参数,中间变量,输出变量
*/
typedef struct {
uint64_t last_wakeup;
float dt;
Chassis_Params_t *param; /* 底盘参数,用Chassis_Init设定 */
/* 模块通用 */
Chassis_Mode_t mode; /* 底盘模式 */
/* 底盘设计 */
int8_t num_wheel; /* 底盘轮子数量 */
Mixer_t mixer; /* 混合器,移动向量->电机目标值 */
MoveVector_t move_vec; /* 底盘实际的运动向量 */
MOTOR_RM_t *motors[4];/*指向底盘每个电机参数*/
float mech_zero;
float wz_multi; /* 小陀螺旋转模式 */
/* PID计算目标值 */
struct {
float motor_rpm[4]; /* 电机转速的动态数组,单位:RPM */
} setpoint;
/* 反馈控制用的PID */
struct {
KPID_t motor[4]; /* 控制轮子电机用的PID的动态数组 */
KPID_t follow; /* 跟随云台用的PID */
} pid;
struct {
Chassis_Limit_t vx, vy, wz;
} limit;
/* 滤波器 */
struct {
LowPassFilter2p_t in[4]; /* 反馈值滤波器 */
LowPassFilter2p_t out[4]; /* 输出值滤波器 */
} filter;
Chassis_Output_t out; /* 电机输出 */
Chassis_Feedback_t feedback;
//float out_motor[4];
} Chassis_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 底盘初始化
*
* \param c 包含底盘数据的结构体
* \param param 包含底盘参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 运行结果
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
float target_freq);
/**
* \brief 更新底盘反馈信息
*
* \param c 包含底盘数据的结构体
* \param can CAN设备结构体
*
* \return 运行结果
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c);
/**
* \brief 运行底盘控制逻辑
*
* \param c 包含底盘数据的结构体
* \param c_cmd 底盘控制指令
* \param dt_sec 两次调用的时间间隔
*
* \return 运行结果
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd,
uint32_t now);
/**
* \brief 复制底盘输出值
*
* \param s 包含底盘数据的结构体
* \param out CAN设备底盘输出结构体
*/
void Chassis_Output(Chassis_t *c);
/**
* \brief 清空Chassis输出数据
*
* \param out CAN设备底盘输出结构体
*/
void Chassis_ResetOutput(Chassis_t *c);
/**
* @brief 底盘功率限制
*
* @param c 底盘数据
* @param cap 电容数据
* @param ref 裁判系统数据
* @return 函数运行结果
*/
//还没有加入，waiting。。。。。。int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
// const Referee_ForChassis_t *ref);
/**
* @brief 导出底盘数据
*
* @param chassis 底盘数据结构体
* @param ui UI数据结构体
*/
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
#ifdef __cplusplus
}
#endif

444
User/module/cmd.c
View File

@ -2,18 +2,271 @@
控制命令
*/
#include "module/cmd.h"
#include <stdint.h>
#include <string.h>
/*************************************************************************/
/*********************************仲裁器**********************************/
/*************************************************************************/
/*************************************************************************************************************************************/
/***************************************************************仲裁器****************************************************************/
/*************************************************************************************************************************************/
/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
bool isREFOnline(CMD_t *c){return c->input_ref.online;}
bool isNUCOnline(CMD_t *c){return c->input_nuc.online;}
bool isRCOnline(CMD_t *c){return c->input_rc.online;}
bool isPCOnline(CMD_t *c){return c->input_pc.online;}
void PriorityConfigsRanking(CMD_Param_t *param) {
static bool init=false;
if(init) return;
for (int i = 0; i < CMD_SRC_NUM - 1; i++) {
for (int j = 0; j < CMD_SRC_NUM - i - 1; j++) {
if (param->rcSourcePriorityConfigs[j].priority < param->rcSourcePriorityConfigs[j + 1].priority) {
CMD_RCSourcePriorityConfig_t temp = param->rcSourcePriorityConfigs[j];
param->rcSourcePriorityConfigs[j] = param->rcSourcePriorityConfigs[j + 1];
param->rcSourcePriorityConfigs[j + 1] = temp;
}
}
}
init=!init;
}
CMD_InputSource_t getHighestPrioritySource(CMD_t *c) {
for (int i = 0; i < CMD_SRC_NUM; i++) {
CMD_InputSource_t source = c->params.rcSourcePriorityConfigs[i].source;
switch (source) {
case CMD_SRC_REFEREE:
if (isREFOnline(c)) {
return CMD_SRC_REFEREE;
}
break;
case CMD_SRC_NUC:
if (isNUCOnline(c)) {
return CMD_SRC_NUC;
}
break;
case CMD_SRC_RC:
if (isRCOnline(c)) {
return CMD_SRC_RC;
}
break;
case CMD_SRC_PC:
if (isPCOnline(c)) {
return CMD_SRC_PC;
}
}
}
return CMD_SRC_PC; /*默认使用键盘鼠标*/
}
int8_t Cmd_Arbiter(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
PriorityConfigsRanking(&c->params);
CMD_InputSource_t source = getHighestPrioritySource(c);
c->out_chassis.source = source;
c->out_gimbal.source = source;
c->out_shoot.source = source;
return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */
/*************************************************************************************************************************************/
/****************************************************************RC*******************************************************************/
/*************************************************************************************************************************************/
/* API ---------------------------------------------------------------------- */
/* Mrobot生成 */
#define RC_SELECT_Index 1
/* 扩展接口 */
#if RC_SELECT_Index == 0
#define FOR_EACH_RC(_) _(dr16, DR16)
#elif RC_SELECT_Index == 1
#define FOR_EACH_RC(_) _(at9s, AT9S)
#endif
/* Includes ----------------------------------------------------------------- */
#if RC_SELECT_Index == 0
#include "device/dr16.h"
#elif RC_SELECT_Index == 1
#include "device/at9s_pro.h"
#endif
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
#define RC_X_FIELD_MACRO(name, NAME) DEVICE_##NAME##_t name;
#define RC_X_EXTERN_MACRO(name, NAME) extern DEVICE_##NAME##_t name##_out;
#define RC_X_COPYDEFINE_MACRO(name, NAME) \
static void copy_##name(rc_inputdata_u *dst) { dst->name = name##_out; }
#define RC_X_COPYREFERENCE_MACRO(name, NAME) copy_##name,
union rc_inputdata_u{
FOR_EACH_RC(RC_X_FIELD_MACRO)
};
FOR_EACH_RC(RC_X_EXTERN_MACRO)
FOR_EACH_RC(RC_X_COPYDEFINE_MACRO)
/* Private variables -------------------------------------------------------- */
/*静态缓冲区*/
static rc_inputdata_u rc_buffer;
/* Private function -------------------------------------------------------- */
int8_t Cmd_Get_rc(CMD_InputData_RC_t *rc)
{
FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer);
rc->inputData = &rc_buffer;
rc->type = RC_SELECT_Index;
return CMD_OK;
}
/*************************************************************************/
/**********************************RC*************************************/
/*************************************************************************/
int8_t Cmd_BuildChassisCommandFromInput_rc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
#if RC_SELECT_Index == 0
c->input_rc.inputData->dr16.data =
#elif RC_SELECT_Index == 1
switch (c->input_rc.inputData->at9s.data.key_E) {
case AT9S_CMD_SW_DOWN:
c->out_chassis.cmd.mode = CHASSIS_MODE_RELAX;
break;
case AT9S_CMD_SW_MID:
c->out_chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
break;
case AT9S_CMD_SW_UP:
c->out_chassis.cmd.mode = CHASSIS_MODE_ROTOR;
break;
default:
c->out_chassis.cmd.mode = CHASSIS_MODE_RELAX;
break;
}
c->out_chassis.cmd.ctrl_vec.vx = c->input_rc.inputData->at9s.data.ch_l_y;
c->out_chassis.cmd.ctrl_vec.vy = c->input_rc.inputData->at9s.data.ch_l_x;
c->out_chassis.cmd.ctrl_vec.wz = c->input_rc.inputData->at9s.data.ch_r_x;
#endif
return CMD_OK;
}
int8_t Cmd_BuildGimbalCommandFromInput_rc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
#if RC_SELECT_Index == 0
c->input_rc.inputData->dr16.data =
#elif RC_SELECT_Index == 1
switch (c->input_rc.inputData->at9s.data.key_G) {
case AT9S_CMD_SW_DOWN:
c->out_gimbal.cmd.mode = GIMBAL_MODE_RELAX;
c->out_gimbal.cmd.delta_yaw = 0.0f;
c->out_gimbal.cmd.delta_pit = 0.0f;
break;
case AT9S_CMD_SW_MID:
c->out_gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
c->out_gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
c->out_gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
break;
case AT9S_CMD_SW_UP:
c->out_gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
c->out_gimbal.cmd.delta_yaw = -at9s_out.data.ch_l_x * 2.0f;
c->out_gimbal.cmd.delta_pit = -at9s_out.data.ch_l_y * 1.5f;
break;
default:
c->out_gimbal.cmd.mode = GIMBAL_MODE_RELAX;
c->out_gimbal.cmd.delta_yaw = 0.0f;
c->out_gimbal.cmd.delta_pit = 0.0f;
break;
}
#endif
return CMD_OK;
}
int8_t Cmd_BuildShootCommandFromInput_rc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
#if RC_SELECT_Index == 0
c->input_rc.inputData->dr16.data =
#elif RC_SELECT_Index == 1
switch (c->input_rc.inputData->at9s.data.key_C) {
case AT9S_CMD_SW_DOWN:
c->out_shoot.cmd.online = at9s_out.online;
c->out_shoot.cmd.ready = true;
c->out_shoot.cmd.firecmd = true;
break;
case AT9S_CMD_SW_MID:
c->out_shoot.cmd.online = at9s_out.online;
c->out_shoot.cmd.ready = true;
c->out_shoot.cmd.firecmd = false;
break;
case AT9S_CMD_SW_UP:
c->out_shoot.cmd.online = at9s_out.online;
c->out_shoot.cmd.ready = false;
c->out_shoot.cmd.firecmd = false;
break;
default:
c->out_shoot.cmd.online = at9s_out.online;
c->out_shoot.cmd.ready = false;
c->out_shoot.cmd.firecmd = false;
break;
}
#endif
return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */
/*************************************************************************************************************************************/
/*****************************************************************PC******************************************************************/
/*************************************************************************************************************************************/
/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
int8_t Cmd_Get_pc(CMD_InputData_PC_t *pc)
{
FOR_EACH_RC(RC_X_COPYREFERENCE_MACRO)(&rc_buffer);
rc->inputData = &rc_buffer;
rc->type = RC_SELECT_Index;
return CMD_OK;
}
/* Exported functions ------------------------------------------------------- */
int8_t Cmd_BuildChassisCommandFromInput_pc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
return CMD_OK;
}
int8_t Cmd_BuildGimbalCommandFromInput_pc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
return CMD_OK;
}
int8_t Cmd_BuildShootCommandFromInput_pc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
return CMD_OK;
}
/*************************************************************************************************************************************/
/****************************************************************NUC******************************************************************/
/*************************************************************************************************************************************/
/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
@ -23,63 +276,146 @@
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
#define RC_SELECT_Index 1
int8_t Cmd_BuildChassisCommandFromInput_nuc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
/* 扩展接口 */
#if RC_SELECT_Index == 0
#define FOR_EACH_RC(_) _(dr16, DR16)
#elif RC_SELECT_Index == 1
#define FOR_EACH_RC(_) _(at9s, AT9S)
#endif
#if RC_SELECT_Index == 0
#include "device/dr16.h"
#elif RC_SELECT_Index == 1
#include "device/at9s_pro.h"
#endif
#define X_FIELD(name, NAME) DEVICE_##NAME##_t name;
#define X_EXTERN(name, NAME) extern DEVICE_##NAME##_t name##_out;
#define X_COPY(name, NAME) \
static void copy_##name(rc_u *dst) { dst->name = name##_out; }
#define X_REF(name, NAME) copy_##name,
union rc_u{
FOR_EACH_RC(X_FIELD)
};
FOR_EACH_RC(X_EXTERN)
FOR_EACH_RC(X_COPY)
/*静态缓冲区，供返回使用*/
static rc_u rc_buffer;
CMD_RCInputData_t rc;
int8_t Cmd_get_rc(CMD_RCInputData_t *dst)
{
FOR_EACH_RC(X_REF)(&rc_buffer);
dst->rc = &rc_buffer;
dst->rc_type = RC_SELECT_Index;
return CMD_OK;
}
/*************************************************************************/
/**********************************PC*************************************/
/*************************************************************************/
int8_t Cmd_BuildGimbalCommandFromInput_nuc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
return CMD_OK;
}
int8_t Cmd_BuildShootCommandFromInput_nuc(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
return CMD_OK;
}
/*************************************************************************/
/**********************************NUC*************************************/
/*************************************************************************/
/*************************************************************************************************************************************/
/***************************************************************REF*******************************************************************/
/*************************************************************************************************************************************/
/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t Cmd_BuildChassisCommandFromInput_referee(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
/*************************************************************************/
/**********************************REF*************************************/
/*************************************************************************/
return CMD_OK;
}
int8_t Cmd_BuildGimbalCommandFromInput_referee(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
/*************************************************************************/
/*********************************分发命令*********************************/
/*************************************************************************/
return CMD_OK;
}
int8_t Cmd_BuildShootCommandFromInput_referee(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
return CMD_OK;
}
/*************************************************************************************************************************************/
/***************************************************************分发命令***************************************************************/
/*************************************************************************************************************************************/
/* Includes ----------------------------------------------------------------- */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
typedef int8_t (*Cmd_BuildCommandFunc)(CMD_t *c);
typedef struct {
CMD_InputSource_t source;
Cmd_BuildCommandFunc chassisFunc;
Cmd_BuildCommandFunc gimbalFunc;
Cmd_BuildCommandFunc shootFunc;
} CMD_SourceHandler_t;
CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
{CMD_SRC_RC, Cmd_BuildChassisCommandFromInput_rc, Cmd_BuildGimbalCommandFromInput_rc, Cmd_BuildShootCommandFromInput_rc},
{CMD_SRC_PC, Cmd_BuildChassisCommandFromInput_pc, Cmd_BuildGimbalCommandFromInput_pc, Cmd_BuildShootCommandFromInput_pc},
{CMD_SRC_NUC, Cmd_BuildChassisCommandFromInput_nuc, Cmd_BuildGimbalCommandFromInput_nuc, Cmd_BuildShootCommandFromInput_nuc},
{CMD_SRC_REFEREE, Cmd_BuildChassisCommandFromInput_referee, Cmd_BuildGimbalCommandFromInput_referee, Cmd_BuildShootCommandFromInput_referee},
};
int8_t Cmd_GenerateCommand(CMD_t *c) {
if (c == NULL) {
return CMD_ERR_NULL; // 参数错误
}
Cmd_Arbiter(c);
switch (c->out_chassis.source) {
case CMD_SRC_RC:
sourceHandlers[CMD_SRC_RC].chassisFunc(c);
break;
case CMD_SRC_PC:
sourceHandlers[CMD_SRC_PC].chassisFunc(c);
break;
case CMD_SRC_NUC:
sourceHandlers[CMD_SRC_NUC].chassisFunc(c);
break;
case CMD_SRC_REFEREE:
sourceHandlers[CMD_SRC_REFEREE].chassisFunc(c);
break;
default:
break;
}
switch (c->out_gimbal.source) {
case CMD_SRC_RC:
sourceHandlers[CMD_SRC_RC].gimbalFunc(c);
break;
case CMD_SRC_PC:
sourceHandlers[CMD_SRC_PC].gimbalFunc(c);
break;
case CMD_SRC_NUC:
sourceHandlers[CMD_SRC_NUC].gimbalFunc(c);
break;
case CMD_SRC_REFEREE:
sourceHandlers[CMD_SRC_REFEREE].gimbalFunc(c);
break;
default:
break;
}
switch (c->out_shoot.source) {
case CMD_SRC_RC:
sourceHandlers[CMD_SRC_RC].shootFunc(c);
break;
case CMD_SRC_PC:
sourceHandlers[CMD_SRC_PC].shootFunc(c);
break;
case CMD_SRC_NUC:
sourceHandlers[CMD_SRC_NUC].shootFunc(c);
break;
case CMD_SRC_REFEREE:
sourceHandlers[CMD_SRC_REFEREE].shootFunc(c);
break;
default:
break;
}
return CMD_OK;
}

176
User/module/cmd.h
View File

@ -22,57 +22,193 @@ extern "C" {
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef union rc_u rc_u;
typedef struct {
rc_u *rc;
enum { DR16, AT9S } rc_type;
} CMD_RCInputData_t;
typedef union rc_inputdata_u rc_inputdata_u;
#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */
/* 输入源枚举 */
typedef enum {
CMD_SRC_RC, /* 遥控器 */
CMD_SRC_RC=0, /* 遥控器 */
CMD_SRC_PC, /* 键盘鼠标 */
CMD_SRC_NUC, /* 上位机 */
CMD_SRC_REFEREE, /* 裁判系统 */
CMD_SRC_NUM
CMD_SRC_NUM,
CMD_SRC_ERR
} CMD_InputSource_t;
/* RC part begin-------------------------------------- */
typedef struct {
bool online;
enum {DR16, AT9S} type;
rc_inputdata_u *inputData;
} CMD_InputData_RC_t;
}CMD_PCInputData_t;
typedef struct {
}CMD_NUCInputData_t;
} CMD_RC_Param_t;
typedef struct {
CMD_InputData_RC_t input;
} CMD_RC_t;
/* RC part end---------------------------------------- */
/* PC part begin-------------------------------------- */
/* 键盘按键值 */
typedef enum {
CMD_KEY_W = 0,
CMD_KEY_S,
CMD_KEY_A,
CMD_KEY_D,
CMD_KEY_SHIFT,
CMD_KEY_CTRL,
CMD_KEY_Q,
CMD_KEY_E,
CMD_KEY_R,
CMD_KEY_F,
CMD_KEY_G,
CMD_KEY_Z,
CMD_KEY_X,
CMD_KEY_C,
CMD_KEY_V,
CMD_KEY_B,
CMD_L_CLICK,
CMD_R_CLICK,
CMD_KEY_NUM,
} CMD_KeyValue_t;
typedef enum {
CMD_ACTIVE_RISING_EDGE, /* 按下时触发 */
CMD_ACTIVE_FALLING_EDGE, /* 抬起时触发 */
CMD_ACTIVE_PRESSED, /* 按住时触发 */
} CMD_TriggerType_t;
/* 行为值序列 */
typedef enum {
CMD_BEHAVIOR_FORE = 0, /* 向前 */
CMD_BEHAVIOR_BACK, /* 向后 */
CMD_BEHAVIOR_LEFT, /* 向左 */
CMD_BEHAVIOR_RIGHT, /* 向右 */
CMD_BEHAVIOR_ACCELERATE, /* 加速 */
CMD_BEHAVIOR_DECELEBRATE, /* 减速 */
CMD_BEHAVIOR_FIRE, /* 开火 */
CMD_BEHAVIOR_FIRE_MODE, /* 切换开火模式 */
CMD_BEHAVIOR_BUFF, /* 打符模式 */
CMD_BEHAVIOR_AUTOAIM, /* 自瞄模式 */
CMD_BEHAVIOR_OPENCOVER, /* 弹舱盖开关 */
CMD_BEHAVIOR_ROTOR, /* 小陀螺模式 */
CMD_BEHAVIOR_REVTRIG, /* 反转拨弹 */
CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */
CMD_BEHAVIOR_NUM,
} CMD_Behavior_t;
typedef struct {
CMD_KeyValue_t key;
CMD_TriggerType_t trigger_type;
} CMD_KeyMapItem_t;
/* 行为映射的对应按键数组 */
typedef struct {
CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM];
} CMD_KeyMap_Params_t;
typedef struct {
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse; /* 鼠标值 */
uint16_t key; /* 按键值 */
uint16_t res; /* 保留，未启用 */
} CMD_PC_data_t;
typedef struct {
bool online;
CMD_PC_data_t data;
}CMD_InputData_PC_t;
typedef struct {
}CMD_REFInputData_t;
} CMD_PC_Param_t;
typedef struct {
CMD_InputData_PC_t input;
} CMD_PC_t;
/* PC part end---------------------------------------- */
/* NUC part begin------------------------------------- */
typedef struct {
bool online;
}CMD_InputData_NUC_t;
typedef struct {
} CMD_NUC_Param_t;
typedef struct {
CMD_InputData_NUC_t input;
} CMD_NUC_t;
/* NUC part end--------------------------------------- */
/* REF part begin------------------------------------- */
typedef struct {
bool online;
}CMD_InputData_REF_t;
typedef struct {
} CMD_REF_Param_t;
typedef struct {
CMD_InputData_REF_t input;
} CMD_REF_t;
/* REF part begin------------------------------------- */
/* 底盘控制命令 */
typedef struct {
} CMD_cmdForChassisCtrl_t;
CMD_InputSource_t source;
Chassis_CMD_t cmd;
} CMD_OutputCmd_CHASSIS_t;
/* 云台控制命令 */
typedef struct {
} CMD_cmdForGimbalCtrl_t;
CMD_InputSource_t source;
Gimbal_CMD_t cmd;
} CMD_OutputCmd_GIMBAL_t;
/* 射击控制命令 */
typedef struct {
CMD_InputSource_t src;
Shoot_CMD_t shoot_cmd;
} CMD_Shoot_t;
CMD_InputSource_t source;
Shoot_CMD_t cmd;
} CMD_OutputCmd_SHOOT_t;
typedef struct {
bool pc_ctrl; /* 是否使用键鼠控制 */
bool host_overwrite; /* 是否Host控制 */
CMD_InputSource_t source;
uint8_t priority;
} CMD_SourcePriorityConfig_t;
typedef struct {
CMD_SourcePriorityConfig_t SourcePriorityConfigs[CMD_SRC_NUM];
} CMD_Param_t;
typedef struct {
uint16_t key_last; /* 上次按键键值 */
CMD_Param_t params;
CMD_RC_t rc;
CMD_PC_t pc;
CMD_NUC_t nuc;
CMD_REF_t ref;
CMD_OutputCmd_CHASSIS_t out_chassis;
CMD_OutputCmd_GIMBAL_t out_gimbal;
CMD_OutputCmd_SHOOT_t out_shoot;
} CMD_t;
/* Exported functions prototypes -------------------------------------------- */

78
User/module/config.c
View File

@ -17,6 +17,84 @@
// 机器人参数配置
Config_RobotParam_t robot_config = {
.chassis_param = {
.motor_param = {
{
.can = BSP_CAN_1,
.id = 0x201,
.module = MOTOR_M3508,
.reverse = false,
.gear = true
},
{
.can = BSP_CAN_1,
.id = 0x202,
.module = MOTOR_M3508,
.reverse = false,
.gear = true
},
{
.can = BSP_CAN_1,
.id = 0x203,
.module = MOTOR_M3508,
.reverse = false,
.gear = true
},
{
.can = BSP_CAN_1,
.id = 0x204,
.module = MOTOR_M3508,
.reverse = false,
.gear = true
},
},
.type = CHASSIS_TYPE_OMNI_PLUS,
.pid = {
.motor_pid_param = {
.k = 0.001f,
.p = 0.01f,
.i = 0.01f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = -1.0f,
},
.follow_pid_param = {
.k = 0.5f,
.p = 1.0f,
.i = 0.0f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
.d_cutoff_freq = -1.0f,
.range = M_2PI,
},
},
.low_pass_cutoff_freq = {
.in = 50.0f,
.out = 50.0f,
},
.reverse = {
.yaw = true,
},
.limit = {
.max_vx = 3.0f,
.max_vy = 3.0f,
.max_wz = 2.0f,
.max_current = 16000.0f
},
},
.gimbal_param = {
.pid = {

2
User/module/config.h
View File

@ -18,7 +18,7 @@ extern "C" {
typedef struct {
Shoot_Params_t shoot_param;
Gimbal_Params_t gimbal_param;
// Chassis_Params_t chassis_param;
Chassis_Params_t chassis_param;
} Config_RobotParam_t;
/* Exported functions prototypes -------------------------------------------- */

25
User/task/ctrl_chassis.c
View File

@ -4,11 +4,10 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include <can.h>
#include "module/chassis.h"
#include "component/at9s_pro_cmd.h"
#include "module/config.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -16,8 +15,10 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
chassis_ctrl_eulr_t chassis_ctrl_eulr;
COMP_AT9S_CMD_t chassis_ctrl_cmd_rc;
Chassis_t chassis;
Chassis_CMD_t chassis_cmd;
Chassis_IMU_t chassis_imu;
static AHRS_Eulr_t mech_zero = {0};
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -33,18 +34,20 @@ void Task_ctrl_chassis(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
Motor_Init(CTRL_CHASSIS_FREQ);
Config_RobotParam_t *cfg = Config_GetRobotParam();
Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ);
chassis.mech_zero=4.13f;
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_ctrl_cmd_rc, NULL, 0);
osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis_ctrl_eulr.chassis_encoder_yaw, NULL, 0);
osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis.feedback.encoder_gimbalYawMotor, NULL, 0);
osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0);//遥控器
chassis_ctrl_eulr.chassis_mech_zero_yaw=4.13f;
//// chassis_ctrl_eulr.chassis_encoder_yaw=motor_gimbal_yaw.total_angle;
chassis_control (chassis_ctrl_eulr,chassis_ctrl_cmd_rc);
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount());
Chassis_Output(&chassis);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

8
ozone/hero.jdebug
View File

@ -5,7 +5,7 @@
**********************************************************************
File :
Created : 14. Oct 2025 21:13
Created : 02. Nov 2025 13:12
Ozone Version : V3.40b
*/
@ -22,8 +22,8 @@ void OnProjectLoad (void) {
//
// Dialog-generated settings
//
Project.AddPathSubstitute ("D:/CUBEMX/hero/ozone", "$(ProjectDir)");
Project.AddPathSubstitute ("d:/cubemx/hero/ozone", "$(ProjectDir)");
Project.AddPathSubstitute ("D:/CUBEMX/hero/god-yuan-hero/ozone", "$(ProjectDir)");
Project.AddPathSubstitute ("d:/cubemx/hero/god-yuan-hero/ozone", "$(ProjectDir)");
Project.SetDevice ("STM32F407IG");
Project.SetHostIF ("USB", "");
Project.SetTargetIF ("SWD");
@ -32,7 +32,7 @@ void OnProjectLoad (void) {
//
// User settings
//
File.Open ("D:/CUBEMX/hero/MDK-ARM/hero/hero.axf");
File.Open ("D:/CUBEMX/hero/god-yuan-hero/build/Debug/hero.elf");
}
/*********************************************************************

50
ozone/hero.jdebug.user
View File

@ -1,42 +1,26 @@
GraphedExpression="((shoot).target_variable).target_rpm", Color=#e56a6f, Show=0
GraphedExpression="(shoot).errtosee", Color=#35792b, Show=0
GraphedExpression="chassis_out[2]", Color=#769dda
OpenDocument="startup_stm32f407xx.s", FilePath="D:/CUBEMX/hero/MDK-ARM/startup_stm32f407xx.s", Line=161
OpenDocument="chassis.h", FilePath="D:/CUBEMX/hero/User/module/chassis.h", Line=0
OpenDocument="ctrl_chassis.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_chassis.c", Line=4
OpenDocument="pid.h", FilePath="D:/CUBEMX/hero/User/component/pid.h", Line=0
OpenDocument="shoot.h", FilePath="D:/CUBEMX/hero/User/module/shoot.h", Line=166
OpenDocument="bmi088.c", FilePath="D:/CUBEMX/hero/User/device/bmi088.c", Line=159
OpenDocument="stm32f4xx_it.c", FilePath="D:/CUBEMX/hero/Core/Src/stm32f4xx_it.c", Line=87
OpenDocument="shoot.c", FilePath="D:/CUBEMX/hero/User/module/shoot.c", Line=364
OpenDocument="ctrl_shoot.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_shoot.c", Line=0
OpenDocument="can.c", FilePath="D:/CUBEMX/hero/User/bsp/can.c", Line=485
OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/User/task/ctrl_gimbal.c", Line=0
OpenDocument="chassis.c", FilePath="D:/CUBEMX/hero/User/module/chassis.c", Line=20
OpenDocument="main.c", FilePath="D:/CUBEMX/hero/Core/Src/main.c", Line=48
OpenDocument="ctrl_shoot.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_shoot.c", Line=0
OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_gimbal.c", Line=14
OpenDocument="ctrl_chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_chassis.c", Line=7
OpenDocument="user_task.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/user_task.c", Line=0
OpenDocument="main.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/main.c", Line=60
OpenToolbar="Debug", Floating=0, x=0, y=0
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=609, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=599, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=309, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=609, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=319, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=599, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1279, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=BOTTOM, x=0, y=0, w=1280, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="1 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="758;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1053;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="1004;0"
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=1279, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=1276, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=BOTTOM, x=0, y=0, w=1283, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1056;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1056;-69", CodeGraphLegendShown=1, CodeGraphLegendPosition="1007;0"
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=1276, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;482]
TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100]
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;100;100;100;100;110;126;422]
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;294]
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;103]
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;726]
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;100]
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((shoot).target_variable).target_rpm";" (shoot).errtosee";" chassis_out[2]"], ColWidths=[100;100;100;100;100]
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;144;134;124;134;110;134;281]
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
WatchedExpression="gimbal", RefreshRate=5, Window=Watched Data 1
WatchedExpression="shoot", RefreshRate=5, Window=Watched Data 1
WatchedExpression="shoot.feedback.fric[2]", RefreshRate=2, Window=Watched Data 1
WatchedExpression="chassis_out", RefreshRate=5, Window=Watched Data 1
WatchedExpression="shoot_cmd", RefreshRate=5, Window=Watched Data 1
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;366]