增加了hardfault调试支持,修复了消息队列在发布者先于订阅者创建话题时野指针的错误
This commit is contained in:
NeoZng
parent
b024d56bca
commit
bc1f928e02
|
|
@ -100,6 +100,8 @@ void HardFault_Handler(void)
|
|||
/* USER CODE END HardFault_IRQn 0 */
|
||||
while (1)
|
||||
{
|
||||
// 发生hardfault,点击step over会自动跳转回出错的指令,方便调试
|
||||
asm("bx lr");
|
||||
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
|
||||
/* USER CODE END W1_HardFault_IRQn 0 */
|
||||
}
|
||||
|
|
|
|||
|
|
@ -163,11 +163,11 @@ Module层主要存放的是类型定义和实例指针数组，在该层没有
|
|||
4. 增加机器人整车控制应用
|
||||
- 主要功能：实现机器人的控制
|
||||
|
||||
在完成BSP层和Module层后，如果在APP层没有控制代码，则代码并无实际功能。换言之，BSP层与Module层的存在是为了APP层更简单、更合理、更易于扩展和移植。本框架的初始目标即是实现：在APP层仅需思考逻辑并用无关硬件的C语言代码实现即可完成整个机器人的控制。所有需要使用的模块和算法都在Module层提供。
|
||||
在完成BSP层和Module层后，如果在APP层没有控制代码，则代码并无实际功能。换言之，BSP层与Module层的存在是为了APP层更简单、更合理、更易于扩展和移植。本框架的初始目标即是实现：在APP层仅需思考逻辑并用无关硬件的C语言代码实现即可完成整个机器人的控制。所有需要使用的模块和算法都在Module层提供，硬件的抽象在bsp层完成。
|
||||
|
||||
- APP层按照模块（如云台、发射、底盘）建立对应的子文件夹，在其中完成初始化和相关逻辑功能的编写。还有用于发布指令的云台指令应用和底盘指令应用，前者应该包含一个遥控器模块和一个视觉通信模块，后者包含裁判系统模块。它们包含的模块都会处理一些指令和控制信息，因此将这两个应用从云台和底盘应用中隔离出来。这样还可以方便兼容双板。
|
||||
- APP层按照机械设计结构（如云台、发射、底盘）建立对应的子文件夹，在其中完成初始化和相关逻辑功能的编写。还有用于发布指令的云台指令应用和底盘指令应用，前者应该包含一个遥控器模块和一个视觉通信模块，后者包含裁判系统模块。它们包含的模块都会处理一些指令和控制信息，因此将这两个应用从云台和底盘应用中隔离出来。这样还可以方便兼容双板。
|
||||
|
||||
- 单双板切换在application的`robot_def.h`中进行，修改宏定义可以切换开发板的设定模式。当设定为单板的时候，在`robot.c`中会对gimbal，chassis，shoot，gimbal_cmd，chassis_cmd五个应用都进行初始化。对于双板的情况，需要将上板配置为gimbal board，下板配置为chassis board，它们会分别初始化gimbal/shoot/gimbal_cmd和chassis/chassis_cmd。
|
||||
- 单双板切换在application的`robot_def.h`中进行，**修改宏定义可以切换开发板的设定模式**。当设定为单板的时候，在`robot.c`中会对gimbal，chassis，shoot，gimbal_cmd，chassis_cmd五个应用都进行初始化。对于双板的情况，需要将上板配置为gimbal board，下板配置为chassis board，它们会分别初始化gimbal/shoot/gimbal_cmd和chassis/chassis_cmd。
|
||||
|
||||
- 对于单板的情况，所有应用之间的信息交互通过message center完成。而使用双板时，需要通过板间通信传递控制信息（默认遥控器接收机和pc在云台板，裁判系统在底盘板，因此需要互发信息）。当前通过**条件编译**来控制信息的去向（发往message center/接收，还是通过can comm发送/接收），后续考虑将双板通信纳入message center的实现中，根据`robot_def.h`的开发板定义自动处理通信，降低应用层级的逻辑复杂度。
|
||||
|
||||
|
|
@ -357,7 +357,7 @@ HAL库初始化 --> BSP初始化 --> Application初始化 --> app调用其拥有
|
|||
|
||||
APP会调用其所有的模块的初始化函数（注册函数），这是因为本框架的设计思想是任何模块在被注册（构造/初始化）之前，都是不存在的，当且仅当定义了一个模块结构体（也称实例）的时候，才有一个实体的概念。
|
||||
|
||||
main函数唯一需要的函数是app层的`robot.c`中的`RobotInitia()`函数，它首先会调用BSP初始化，然后进行所有应用的初始化；每个应用会调用对应模块的初始化；一些依赖通信外设的模块会将通信支持相关的bsp进行初始化。初始化结束之后实时系统启动。
|
||||
main函数唯一需要的函数是app层的`robot.c`中的`RobotInit()`函数，它首先会调用BSP初始化，然后进行所有应用的初始化；每个应用会调用对应模块的初始化；一些依赖通信外设的模块会将通信支持相关的bsp进行初始化。初始化结束之后实时系统启动。
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -2,33 +2,149 @@
|
|||
#include "robot_def.h"
|
||||
#include "dji_motor.h"
|
||||
#include "super_cap.h"
|
||||
#include "message_center.h"
|
||||
|
||||
#ifdef CHASSIS_BOARD //使用板载IMU获取底盘转动角速度
|
||||
/* 底盘应用包含的模块和信息存储 */
|
||||
#ifdef CHASSIS_BOARD // 使用板载IMU获取底盘转动角速度
|
||||
#include "ins_task.h"
|
||||
IMU_Data_t* Chassis_IMU_data;
|
||||
IMU_Data_t *Chassis_IMU_data;
|
||||
#endif // CHASSIS_BOARD
|
||||
// static SuperCAP cap;
|
||||
static dji_motor_instance *lf; // left right forward back
|
||||
static dji_motor_instance *rf;
|
||||
static dji_motor_instance *lb;
|
||||
static dji_motor_instance *rb;
|
||||
|
||||
// SuperCAP cap;
|
||||
static dji_motor_instance* lf; //left right forward back
|
||||
static dji_motor_instance* rf;
|
||||
static dji_motor_instance* lb;
|
||||
static dji_motor_instance* rb;
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
|
||||
static Publisher_t* chassis_pub;
|
||||
static Chassis_Upload_Data_s chassis_feedback_data;
|
||||
static Subscriber_t* chassis_sub;
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
|
||||
|
||||
|
||||
static void mecanum_calculate()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void ChassisInit()
|
||||
{
|
||||
// 左前轮
|
||||
Motor_Init_Config_s left_foward_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 1,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
.current_PID = {
|
||||
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
// 右前轮
|
||||
Motor_Init_Config_s right_foward_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 2,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
.current_PID = {
|
||||
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
// 左后轮
|
||||
Motor_Init_Config_s left_back_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 3,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
.current_PID = {
|
||||
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
// 右后轮
|
||||
Motor_Init_Config_s right_back_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 4,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
.current_PID = {
|
||||
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
|
||||
lf = DJIMotorInit(&left_foward_config);
|
||||
rf = DJIMotorInit(&right_foward_config);
|
||||
lb = DJIMotorInit(&left_back_config);
|
||||
rb = DJIMotorInit(&right_back_config);
|
||||
|
||||
// SupercapInit();
|
||||
|
||||
chassis_sub=SubRegister("chassis_cmd",sizeof(Chassis_Ctrl_Cmd_s));
|
||||
chassis_pub=PubRegister("chassis_feed",sizeof(Chassis_Upload_Data_s));
|
||||
}
|
||||
|
||||
void ChassisTask()
|
||||
void ChassisTask()
|
||||
{
|
||||
|
||||
}
|
||||
|
|
@ -1,22 +1,38 @@
|
|||
#include "robot_def.h"
|
||||
#include "chassis_cmd.h"
|
||||
#include "referee.h"
|
||||
#include "message_center.h"
|
||||
|
||||
|
||||
/* chassis_cmd应用包含的模块和信息存储 */
|
||||
static referee_info_t *referee_data; // 裁判系统的数据
|
||||
#ifndef ONE_BOARD
|
||||
#include "can_comm.h"
|
||||
static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
|
||||
#endif // !ONE_BOARD
|
||||
|
||||
#endif // !ONE_BOARD
|
||||
//和chassis应用的交互
|
||||
static Publisher_t* chassis_cmd_pub;
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd; // 发送给chassis应用的控制信息
|
||||
static Subscriber_t* chassis_feed_sub;
|
||||
static Chassis_Upload_Data_s chassis_fetch_data; // chassis反馈的数据
|
||||
//和gimbal_cmd的交互
|
||||
static Publisher_t* chassis2gimbal_pub;
|
||||
static Chassis2Gimbal_Data_s data_to_gimbal_cmd; // 发送给gimbal_cmd的数据,包括热量限制,底盘功率等
|
||||
static Subscriber_t* gimbal2chassis_sub;
|
||||
static Gimbal2Chassis_Data_s data_from_gimbal_cmd; // 来自gimbal_cmd的数据,主要是底盘控制量
|
||||
static referee_info_t *referee_data; // 裁判系统的数据
|
||||
|
||||
|
||||
void ChassisCMDInit()
|
||||
{
|
||||
referee_data= RefereeInit(&huart6);
|
||||
|
||||
chassis_cmd_pub=PubRegister("chassis_cmd",sizeof(Chassis_Ctrl_Cmd_s));
|
||||
chassis_feed_sub=SubRegister("chassis_feed",sizeof(Chassis_Upload_Data_s));
|
||||
chassis2gimbal_pub=PubRegister("chassis2gimbal",sizeof(Chassis2Gimbal_Data_s));
|
||||
gimbal2chassis_sub=SubRegister("gimbal2chassis",sizeof(Gimbal2Chassis_Data_s));
|
||||
}
|
||||
|
||||
void ChassisCMDTask()
|
||||
{
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,22 +3,34 @@
|
|||
#include "remote_control.h"
|
||||
#include "ins_task.h"
|
||||
#include "master_process.h"
|
||||
#include "message_center.h"
|
||||
|
||||
|
||||
/* gimbal_cmd应用包含的模块实例指针和交互信息存储*/
|
||||
#ifndef ONE_BOARD
|
||||
#include "can_comm.h"
|
||||
static CANCommInstance *chasiss_can_comm;
|
||||
static CANCommInstance *chasiss_can_comm; //双板通信
|
||||
#endif // !ONE_BOARD
|
||||
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息
|
||||
static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息
|
||||
static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息
|
||||
static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
|
||||
static Gimbal2Chassis_Data_s data_to_chassis_cmd; // 发送给底盘CMD应用的控制信息,主要是遥控器和UI绘制相关
|
||||
static Chassis2Gimbal_Data_s data_from_chassis_cmd; // 从底盘CMD应用接收的控制信息,底盘功率枪口热量等
|
||||
static RC_ctrl_t *remote_control_data; // 遥控器数据
|
||||
static Vision_Recv_s *vision_recv_data; // 视觉接收数据
|
||||
static Vision_Send_s *vision_send_data; // 视觉发送数据
|
||||
|
||||
static Publisher_t* gimbal_cmd_pub;
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息
|
||||
static Subscriber_t* gimbal_cmd_feed_sub;
|
||||
static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息
|
||||
|
||||
static Publisher_t* shoot_cmd_pub;
|
||||
static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息
|
||||
static Subscriber_t* shoot_cmd_feed_sub;
|
||||
static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
|
||||
|
||||
static Publisher_t* gimbal2chassis_pub;
|
||||
static Gimbal2Chassis_Data_s data_to_chassis_cmd; // 发送给底盘CMD应用的控制信息,主要是遥控器和UI绘制相关
|
||||
static Subscriber_t* chassis2gimbal_sub;
|
||||
static Chassis2Gimbal_Data_s data_from_chassis_cmd; // 从底盘CMD应用接收的控制信息,底盘功率枪口热量等
|
||||
|
||||
|
||||
static void CalcOffsetAngle()
|
||||
{
|
||||
}
|
||||
|
|
@ -37,6 +49,15 @@ static void SetCtrlMessage()
|
|||
|
||||
void GimbalCMDInit()
|
||||
{
|
||||
remote_control_data=RC_init(&huart3);
|
||||
vision_recv_data=VisionInit(&huart1);
|
||||
|
||||
gimbal_cmd_pub=PubRegister("gimbal_cmd",sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
gimbal_cmd_feed_sub=SubRegister("gimbal_feed",sizeof(Gimbal_Upload_Data_s));
|
||||
shoot_cmd_pub=PubRegister("shoot_cmd",sizeof(Shoot_Ctrl_Cmd_s));
|
||||
shoot_cmd_feed_sub=SubRegister("shoot_feed",sizeof(Shoot_Upload_Data_s));
|
||||
gimbal2chassis_pub=PubRegister("gimbal2chassis",sizeof(Gimbal2Chassis_Data_s));
|
||||
chassis2gimbal_sub=SubRegister("chassis2gimbal",sizeof(Chassis2Gimbal_Data_s));
|
||||
}
|
||||
|
||||
void GimbalCMDTask()
|
||||
|
|
|
|||
|
|
@ -2,15 +2,76 @@
|
|||
#include "robot_def.h"
|
||||
#include "dji_motor.h"
|
||||
#include "ins_task.h"
|
||||
#include "message_center.h"
|
||||
|
||||
static IMU_Data_t Gimbal_IMU_data; // 云台IMU数据
|
||||
static dji_motor_instance yaw; // yaw电机
|
||||
static dji_motor_instance pitch; // pitch电机
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息
|
||||
|
||||
static attitude_t* Gimbal_IMU_data; // 云台IMU数据
|
||||
static dji_motor_instance* yaw_motor; // yaw电机
|
||||
static dji_motor_instance *pitch_motor; // pitch电机
|
||||
|
||||
static Publisher_t* gimbal_pub;
|
||||
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给gimbal_cmd的云台状态信息
|
||||
static Subscriber_t* gimbal_sub;
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息
|
||||
|
||||
|
||||
void GimbalInit()
|
||||
{
|
||||
Gimbal_IMU_data=INS_Init();
|
||||
|
||||
// YAW
|
||||
Motor_Init_Config_s yaw_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 1,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
.other_angle_feedback_ptr=&Gimbal_IMU_data->YawTotalAngle,
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = GM6020};
|
||||
// PITCH
|
||||
Motor_Init_Config_s pitch_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 2,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = GM6020};
|
||||
|
||||
yaw_motor=DJIMotorInit(&yaw_config);
|
||||
pitch_motor=DJIMotorInit(&pitch_config);
|
||||
|
||||
gimbal_pub=PubRegister("gimbal_feed",sizeof(Gimbal_Upload_Data_s));
|
||||
gimbal_sub=SubRegister("gimbal_cmd",sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
}
|
||||
|
||||
void GimbalTask()
|
||||
|
|
|
|||
|
|
@ -3,32 +3,40 @@
|
|||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
#include "chassis.h"
|
||||
#include "chassis_cmd.h"
|
||||
#endif
|
||||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
#include "gimbal.h"
|
||||
#include "shoot.h"
|
||||
#include "gimbal_cmd.h"
|
||||
#endif
|
||||
|
||||
|
||||
void RobotInit()
|
||||
{
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
||||
ChassisInit();
|
||||
ChassisCMDInit();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
|
||||
GimbalCMDInit();
|
||||
GimbalInit();
|
||||
ShootInit();
|
||||
#endif
|
||||
}
|
||||
|
||||
void RobotTask()
|
||||
{
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
||||
ChassisTask();
|
||||
ChassisCMDTask();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
|
||||
GimbalCMDTask();
|
||||
GimbalTask();
|
||||
ShootTask();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,15 +1,106 @@
|
|||
#include "shoot.h"
|
||||
#include "robot_def.h"
|
||||
#include "dji_motor.h"
|
||||
#include "message_center.h"
|
||||
|
||||
static dji_motor_instance *friction_l; // 左摩擦轮
|
||||
static dji_motor_instance *friction_r; // 右摩擦轮
|
||||
static dji_motor_instance *loader; // 拨盘电机
|
||||
static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自gimbal_cmd的发射控制信息
|
||||
static Shoot_Upload_Data_s shoot_feedback_data; // 反馈回gimbal_cmd的发射状态信息
|
||||
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例
|
||||
*/
|
||||
static dji_motor_instance *friction_l; // 左摩擦轮
|
||||
static dji_motor_instance *friction_r; // 右摩擦轮
|
||||
static dji_motor_instance *loader; // 拨盘电机
|
||||
|
||||
static Publisher_t *shoot_pub;
|
||||
static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自gimbal_cmd的发射控制信息
|
||||
static Subscriber_t *shoot_sub;
|
||||
static Shoot_Upload_Data_s shoot_feedback_data; // 来自gimbal_cmd的发射控制信息
|
||||
|
||||
void ShootInit()
|
||||
{
|
||||
// 左摩擦轮
|
||||
Motor_Init_Config_s left_friction_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 1,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
.current_PID = {
|
||||
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
// 右摩擦轮
|
||||
Motor_Init_Config_s right_friction_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 2,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
.current_PID = {
|
||||
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
// 拨盘电机
|
||||
Motor_Init_Config_s loader_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 3,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kd = 10,
|
||||
.Ki = 1,
|
||||
.Kd = 2,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
||||
},
|
||||
.current_PID = {
|
||||
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M2006};
|
||||
|
||||
friction_l = DJIMotorInit(&left_friction_config);
|
||||
friction_r = DJIMotorInit(&right_friction_config);
|
||||
loader = DJIMotorInit(&loader_config);
|
||||
|
||||
shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
|
||||
shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
|
||||
}
|
||||
|
||||
void ShootTask()
|
||||
|
|
|
|||
|
|
@ -33,11 +33,6 @@ float RefTemp = 40; // 恒温设定温度
|
|||
|
||||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
|
||||
|
||||
/* 调用此函数获得姿态数据指针 */
|
||||
attitude_t *GetINSptr()
|
||||
{
|
||||
return (attitude_t *)&INS.Roll;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 温度控制
|
||||
|
|
@ -49,7 +44,7 @@ static void IMU_Temperature_Ctrl(void)
|
|||
imu_pwm_set(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
|
||||
}
|
||||
|
||||
void INS_Init(void)
|
||||
attitude_t* INS_Init(void)
|
||||
{
|
||||
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR);
|
||||
IMU_Param.scale[X] = 1;
|
||||
|
|
|
|||
|
|
@ -71,18 +71,11 @@ typedef struct
|
|||
float Roll;
|
||||
} IMU_Param_t;
|
||||
|
||||
/**
|
||||
* @brief 反馈位姿结构体指针,apps通过初始化时保存该指针以实现数据访问
|
||||
*
|
||||
* @return attitude_t*
|
||||
*/
|
||||
attitude_t *GetINSptr();
|
||||
|
||||
/**
|
||||
* @brief 初始化惯导解算系统
|
||||
*
|
||||
*/
|
||||
void INS_Init(void);
|
||||
attitude_t *INS_Init(void);
|
||||
|
||||
/**
|
||||
* @brief 此函数放入实时系统中,以1kHz频率运行
|
||||
|
|
|
|||
|
|
@ -72,19 +72,21 @@ static Publisher_t message_center = {
|
|||
.first_subs = NULL,
|
||||
.next_event_node = NULL};
|
||||
|
||||
static void CheckName(char* name)
|
||||
static void CheckName(char *name)
|
||||
{
|
||||
if(strnlen(name,MAX_EVENT_NAME_LEN+1)>=MAX_EVENT_NAME_LEN)
|
||||
if (strnlen(name, MAX_EVENT_NAME_LEN + 1) >= MAX_EVENT_NAME_LEN)
|
||||
{
|
||||
while (1); // 进入这里说明事件名超出长度限制
|
||||
while (1)
|
||||
; // 进入这里说明事件名超出长度限制
|
||||
}
|
||||
}
|
||||
|
||||
static void CheckLen(uint8_t len1,uint8_t len2)
|
||||
static void CheckLen(uint8_t len1, uint8_t len2)
|
||||
{
|
||||
if(len1!=len2)
|
||||
if (len1 != len2)
|
||||
{
|
||||
while(1); // 相同事件的消息长度不同
|
||||
while (1)
|
||||
; // 相同事件的消息长度不同
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -97,14 +99,8 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len)
|
|||
node = node->next_event_node; // 指向下一个发布者(发布者发布的事件)
|
||||
if (strcmp(name, node->event_name) == 0) // 如果事件名相同就订阅这个事件
|
||||
{
|
||||
CheckLen(data_len,node->data_len);
|
||||
Subscriber_t *sub = node->first_subs; // 指向订阅了该事件的第一个订阅者
|
||||
while (sub->next_subs_queue) // 遍历订阅了该事件的链表
|
||||
{
|
||||
sub = sub->next_subs_queue; // 移动到下一个订阅者
|
||||
} // 遇到空指针停下,说明到了链表尾部,创建新的订阅节点
|
||||
|
||||
// 申请内存,注意要memset因为新空间不一定是空的,可能有之前留存的垃圾值
|
||||
CheckLen(data_len, node->data_len);
|
||||
// 创建新的订阅者结点,申请内存,注意要memset因为新空间不一定是空的,可能有之前留存的垃圾值
|
||||
Subscriber_t *ret = (Subscriber_t *)malloc(sizeof(Subscriber_t));
|
||||
memset(ret, 0, sizeof(Subscriber_t));
|
||||
// 对新建的Subscriber进行初始化
|
||||
|
|
@ -113,11 +109,22 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len)
|
|||
{ // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度
|
||||
ret->queue[i] = malloc(sizeof(data_len));
|
||||
}
|
||||
|
||||
sub->next_subs_queue = ret; // 接到链表尾部,延长该订阅该事件的subs链表
|
||||
//如果之前没有订阅者,特殊处理一下
|
||||
if(node->first_subs==NULL)
|
||||
{
|
||||
node->first_subs=ret;
|
||||
return ret;
|
||||
}
|
||||
// 遍历订阅者链表,直到到达尾部
|
||||
Subscriber_t *sub = node->first_subs; // iterator
|
||||
while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表
|
||||
{
|
||||
sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部
|
||||
}
|
||||
sub->next_subs_queue = ret; // 把刚刚创建的订阅者接上
|
||||
return ret;
|
||||
}
|
||||
// 时间名不同,在下一轮循环访问下一个结点
|
||||
// 事件名不同,在下一轮循环访问下一个结点
|
||||
}
|
||||
// 遍历完,发现尚未注册事件(还没有发布者);那么创建一个事件,此时node是publisher链表的最后一个结点
|
||||
node->next_event_node = (Publisher_t *)malloc(sizeof(Publisher_t));
|
||||
|
|
@ -146,7 +153,7 @@ Publisher_t *PubRegister(char *name, uint8_t data_len)
|
|||
node = node->next_event_node; // 切换到下一个发布者(事件)结点
|
||||
if (strcmp(node->event_name, name) == 0) // 如果已经注册了相同的事件,直接返回结点指针
|
||||
{
|
||||
CheckLen(data_len,node->data_len);
|
||||
CheckLen(data_len, node->data_len);
|
||||
return node;
|
||||
}
|
||||
} // 遍历完发现尚未创建name对应的事件
|
||||
|
|
|
|||
|
|
@ -11,22 +11,23 @@
|
|||
static usart_instance* referee_usart_instance;
|
||||
|
||||
/**************裁判系统数据******************/
|
||||
referee_info_t referee_info;
|
||||
uint8_t Judge_Self_ID; // 当前机器人的ID
|
||||
uint16_t Judge_SelfClient_ID; // 发送者机器人对应的客户端ID
|
||||
static referee_info_t referee_info;
|
||||
static uint8_t Judge_Self_ID; // 当前机器人的ID
|
||||
static uint16_t Judge_SelfClient_ID; // 发送者机器人对应的客户端ID
|
||||
|
||||
static void ReceiveCallback()
|
||||
{
|
||||
JudgeReadData(referee_usart_instance->recv_buff);
|
||||
}
|
||||
|
||||
void RefereeInit(UART_HandleTypeDef *referee_usart_handle)
|
||||
referee_info_t* RefereeInit(UART_HandleTypeDef *referee_usart_handle)
|
||||
{
|
||||
USART_Init_Config_s conf;
|
||||
conf.module_callback = ReceiveCallback;
|
||||
conf.usart_handle = referee_usart_handle;
|
||||
conf.recv_buff_size = RE_RX_BUFFER_SIZE;
|
||||
referee_usart_instance=USARTRegister(&conf);
|
||||
return &referee_info;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -120,15 +121,4 @@ void JudgeReadData(uint8_t *ReadFromUsart)
|
|||
JudgeReadData(ReadFromUsart + sizeof(xFrameHeader) + LEN_CMDID + referee_info.FrameHeader.DataLength + LEN_TAIL);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 读取瞬时功率
|
||||
* @param void
|
||||
* @retval 实时功率值
|
||||
* @attention
|
||||
*/
|
||||
float JudgeGetChassisPower(void)
|
||||
{
|
||||
return (referee_info.PowerHeatData.chassis_power);
|
||||
}
|
||||
}
|
||||
|
|
@ -340,11 +340,17 @@ typedef struct
|
|||
ext_CommunatianData_t CommuData; // 队友通信信息
|
||||
} referee_info_t;
|
||||
|
||||
extern referee_info_t referee_info;
|
||||
|
||||
void RefereeInit(UART_HandleTypeDef *referee_usart_handle);
|
||||
void JudgeReadData(uint8_t *ReadFromUsart);
|
||||
float JudgeGetChassisPower(void);
|
||||
|
||||
#pragma pack()
|
||||
|
||||
/**
|
||||
* @brief
|
||||
*
|
||||
* @param referee_usart_handle
|
||||
* @return referee_info_t*
|
||||
*/
|
||||
referee_info_t* RefereeInit(UART_HandleTypeDef *referee_usart_handle);
|
||||
|
||||
|
||||
|
||||
|
||||
#endif // !REFEREE_H
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
|
||||
#include <stdint-gcc.h>
|
||||
#include "main.h"
|
||||
#include "usart.h"
|
||||
|
||||
#define RC_CH_VALUE_MIN ((uint16_t)364)
|
||||
#define RC_CH_VALUE_OFFSET ((uint16_t)1024)
|
||||
|
|
|
|||
Loading…
Reference in New Issue