From 78cc27ee1abd540d29237ffe5bf38446fce74f06 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sun, 11 Dec 2022 14:59:45 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8F=91=E5=B8=83beta=E7=89=88=E6=9C=AC,?= =?UTF-8?q?=E7=9B=AE=E5=89=8D=E5=9F=BA=E6=9C=AC=E5=8A=9F=E8=83=BD=E9=83=BD?= =?UTF-8?q?=E5=8F=AF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 +- Makefile | 3 +- VSCode+Ozone使用方法.md | 2 +- application/chassis/chassis.c | 89 +++++++++++++++++++++++---------- application/chassis/chassis.h | 8 +++ application/cmd/robot_cmd.c | 74 +++++++++++++++++++-------- application/gimbal/gimbal.c | 16 +++--- application/robot.c | 15 +++--- application/robot_def.h | 14 +++--- application/shoot/shoot.c | 46 +++++++++++------ modules/algorithm/controller.c | 6 +++ modules/motor/HT04.c | 2 +- modules/motor/HT04.h | 2 +- modules/motor/LK9025.c | 2 +- modules/motor/LK9025.h | 2 +- modules/motor/dji_motor.c | 35 ++++++------- modules/motor/dji_motor.h | 17 ++++--- modules/remote/remote_control.c | 17 +++++++ stm32.jflash | 82 ++++++++++++++++++++++++++++++ 19 files changed, 320 insertions(+), 115 deletions(-) create mode 100644 stm32.jflash diff --git a/.vscode/settings.json b/.vscode/settings.json index d01c3b5..e9a8cc4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -39,7 +39,8 @@ "general_def.h": "c", "super_cap.h": "c", "motor_def.h": "c", - "quaternionekf.h": "c" + "quaternionekf.h": "c", + "shoot.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/Makefile b/Makefile index 6b3fde3..3fa9f3b 100644 --- a/Makefile +++ b/Makefile @@ -312,8 +312,7 @@ download_dap: openocd -f openocd_dap.cfg -c init -c halt -c "flash write_image erase $(BUILD_DIR)/$(TARGET).hex $(OPENOCD_FLASH_START)" -c reset -c shutdown download_jlink: - openocd -f openocd_jlink.cfg -c init -c halt -c "flash write_image erase $(BUILD_DIR)/$(TARGET).hex $(OPENOCD_FLASH_START)" -c reset -c shutdown - + JFlash -openprj'stm32.jflash' -open'$(BUILD_DIR)/$(TARGET).hex',0x8000000 -auto -startapp -exit ####################################### # dependencies ####################################### diff --git a/VSCode+Ozone使用方法.md b/VSCode+Ozone使用方法.md index 019a4e3..98f1c13 100644 --- a/VSCode+Ozone使用方法.md +++ b/VSCode+Ozone使用方法.md @@ -526,7 +526,7 @@ VSCode `ctrl+,`进入设置，通过`搜索`找到cortex-debug插件的设置。 ```shell Project.SetOSPlugin(“plugin_name”) # plugin_name是启用的实时系统支持插件名 -# 我们要使用的命令是Project.SetOSPlugin(“FreeRTOSPlugin_CM4”) +# 我们要使用的命令是Project.SetOSPlugin ("FreeRTOSPlugin_CM4") ``` 支持的插件在Ozone的安装目录下的`Plugins/OS`目录： diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 2141ff0..6d63e1f 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -32,8 +32,14 @@ #include "can_comm.h" #include "ins_task.h" static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm -IMU_Data_t *Chassis_IMU_data; +attitude_t *Chassis_IMU_data; #endif // CHASSIS_BOARD +#ifdef ONE_BOARD +static Publisher_t *chassis_pub; +static Subscriber_t *chassis_sub; +#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; // 超级电容 @@ -42,12 +48,6 @@ static DJIMotorInstance *motor_rf; static DJIMotorInstance *motor_lb; static DJIMotorInstance *motor_rb; -/* chassis 包含的信息交互模块和数据*/ -static Publisher_t *chassis_pub; -static Chassis_Ctrl_Cmd_s chassis_cmd_recv; -static Subscriber_t *chassis_sub; -static Chassis_Upload_Data_s chassis_feedback_data; - /* 用于自旋变速策略的时间变量,后续考虑查表加速 */ static float t; @@ -65,13 +65,13 @@ void ChassisInit() .Kp = 10, .Ki = 0, .Kd = 0, - .MaxOut = 200, + .MaxOut = 2000, }, .current_PID = { - .Kp = 10, + .Kp = 1.2, .Ki = 0, .Kd = 0, - .MaxOut = 200, + .MaxOut = 2000, }, }, .controller_setting_init_config = { @@ -84,22 +84,22 @@ void ChassisInit() }; chassis_motor_config.can_init_config.tx_id = 1; - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE; + chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; motor_lf = DJIMotorInit(&chassis_motor_config); chassis_motor_config.can_init_config.tx_id = 2, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE; + chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; motor_rf = DJIMotorInit(&chassis_motor_config); chassis_motor_config.can_init_config.tx_id = 3, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE; + chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; motor_lb = DJIMotorInit(&chassis_motor_config); chassis_motor_config.can_init_config.tx_id = 4, - chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_REVERSE; + chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL; motor_rb = DJIMotorInit(&chassis_motor_config); - referee_data = RefereeInit(&huart6); + referee_data = RefereeInit(&huart6); //裁判系统初始化 SuperCap_Init_Config_s cap_conf = { .can_config = { @@ -107,10 +107,28 @@ void ChassisInit() .tx_id = 0x302, .rx_id = 0x301, }}; - cap = SuperCapInit(&cap_conf); + cap = SuperCapInit(&cap_conf); //超级电容初始化 + // 发布订阅初始化,如果为双板,则需要can comm来传递消息 +#ifdef CHASSIS_BOARD + Chassis_IMU_data=INS_Init(); // 底盘IMU初始化 + + CANComm_Init_Config_s comm_conf = { + .can_config={ + .can_handle=&hcan2, + .tx_id=0x311, + .rx_id=0x312, + }, + .recv_data_len=sizeof(Chassis_Ctrl_Cmd_s), + .send_data_len=sizeof(Chassis_Upload_Data_s), + }; + chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化 +#endif // CHASSIS_BOARD + +#ifdef ONE_BOARD chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); +#endif // ONE_BOARD } #define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD) @@ -125,8 +143,8 @@ 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_rb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * RB_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; } /** @@ -161,18 +179,32 @@ void ChassisTask() { // 后续增加没收到消息的处理 // 获取新的控制信息 +#ifdef ONE_BOARD SubGetMessage(chassis_sub, &chassis_cmd_recv); +#endif +#ifdef CHASSIS_BOARD + chassis_cmd_recv=*(Chassis_Ctrl_Cmd_s*)CANCommGet(chasiss_can_comm); +#endif // CHASSIS_BOARD + + if (chassis_cmd_recv.chassis_mode==CHASSIS_ZERO_FORCE) + { + DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止 + DJIMotorStop(motor_rf); + DJIMotorStop(motor_lb); + DJIMotorStop(motor_rb); + } + else + { + DJIMotorEnable(motor_lf); + DJIMotorEnable(motor_rf); + DJIMotorEnable(motor_lb); + DJIMotorEnable(motor_rb); + } // 根据控制模式设定旋转速度 // 后续增加不同状态的过渡模式? switch (chassis_cmd_recv.chassis_mode) { - case CHASSIS_ZERO_FORCE: - DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止 - DJIMotorStop(motor_rf); - DJIMotorStop(motor_lb); - DJIMotorStop(motor_rb); - break; case CHASSIS_NO_FOLLOW: chassis_cmd_recv.wz = 0; // 底盘不旋转,但维持全向机动,一般用于调整云台姿态 break; @@ -196,10 +228,10 @@ void ChassisTask() // 根据控制模式进行正运动学解算,计算底盘输出 MecanumCalculate(); - // 根据裁判系统的反馈数据和电容数据对输出限幅 + // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 LimitChassisOutput(); - // 根据电机的反馈速度计算 + // 根据电机的反馈速度和IMU(如果有)计算真实速度 EstimateSpeed(); // 获取裁判系统数据 @@ -210,5 +242,10 @@ void ChassisTask() chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; // 推送反馈消息 +#ifdef ONE_BOARD PubPushMessage(chassis_pub, &chassis_feedback_data); +#endif +#ifdef CHASSIS_BOARD + CANCommSend(chasiss_can_comm,(void*)&chassis_feedback_data); +#endif // CHASSIS_BOARD } \ No newline at end of file diff --git a/application/chassis/chassis.h b/application/chassis/chassis.h index 9aa9f0f..ea8bd50 100644 --- a/application/chassis/chassis.h +++ b/application/chassis/chassis.h @@ -1,8 +1,16 @@ #ifndef CHASSIS_H #define CHASSIS_H +/** + * @brief 底盘应用初始化,请在开启rtos之前调用 + * + */ void ChassisInit(); +/** + * @brief 底盘应用任务,放入实时系统以一定频率运行 + * + */ void ChassisTask(); #endif // CHASSIS_H \ No newline at end of file diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index ba965fe..273fa43 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -12,10 +12,16 @@ #define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF) /* gimbal_cmd应用包含的模块实例指针和交互信息存储*/ -#ifndef ONE_BOARD +#ifdef GIMBAL_BOARD #include "can_comm.h" -static CANCommInstance *chasiss_can_comm; // 双板通信 -#endif // !ONE_BOARD +static CANCommInstance *cmd_can_comm; // 双板通信 +#endif +#ifdef ONE_BOARD +static Publisher_t *chassis_cmd_pub +static Subscriber_t *chassis_feed_sub; +#endif // ONE_BOARD +static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 +static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回 static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回 @@ -31,10 +37,7 @@ static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息 static Subscriber_t *shoot_feed_sub; static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息 -static Publisher_t *chassis_cmd_pub; -static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关 -static Subscriber_t *chassis_feed_sub; -static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等 + static Robot_Status_e robot_state; @@ -47,8 +50,25 @@ void GimbalCMDInit() gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); shoot_feed_sub = SubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); + +#ifdef ONE_BOARD chassis_cmd_pub = PubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s)); chassis_feed_sub = SubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s)); +#endif // ONE_BOARD +#ifdef GIMBAL_BOARD + CANComm_Init_Config_s comm_conf={ + .can_config={ + .can_handle=&hcan1, + .tx_id=0x312, + .rx_id=0x311, + }, + .recv_data_len=sizeof(Chassis_Upload_Data_s), + .send_data_len=sizeof(Chassis_Ctrl_Cmd_s), + }; + cmd_can_comm=CANCommInit(&comm_conf); +#endif // GIMBAL_BOARD + + robot_state=ROBOT_WORKING; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } /** @@ -86,7 +106,7 @@ static void RemoteControlSet() // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据? if (switch_is_down(rc_data[TEMP].rc.s[0])) // 右侧开关状态[下],底盘跟随云台 chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; - if (switch_is_mid(rc_data[TEMP].rc.s[0])) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 + else if (switch_is_mid(rc_data[TEMP].rc.s[0])) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; // 云台参数,确定云台控制数据 @@ -104,24 +124,27 @@ static void RemoteControlSet() } // 底盘参数,目前没有加入小陀螺(调试似乎没有必要),系数需要调整 - chassis_cmd_send.vx = 1.0f * (float)rc_data[TEMP].rc.joystick[0]; - chassis_cmd_send.vy = 1.0f * (float)rc_data[TEMP].rc.joystick[1]; + chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.joystick[0]; + chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.joystick[1]; // 发射参数 if (switch_is_up(rc_data[TEMP].rc.s[0])) // 右侧开关状态[上],弹舱打开 - ; // 弹舱舵机控制,待添加servo_motor模块,开启 + {// 弹舱舵机控制,待添加servo_motor模块,开启 + + } else ; // 弹舱舵机控制,待添加servo_motor模块,关闭 // 摩擦轮控制,后续可以根据左侧拨轮的值大小切换射频 - if (rc_data[TEMP].rc.joystick[4] > 100) + if (rc_data[TEMP].rc.joystick[4] < -100) shoot_cmd_send.friction_mode = FRICTION_ON; else shoot_cmd_send.friction_mode = FRICTION_OFF; // 拨弹控制,目前固定为连发 - if (rc_data[TEMP].rc.joystick[4] > 500) + if (rc_data[TEMP].rc.joystick[4] <-500) shoot_cmd_send.load_mode = LOAD_BURSTFIRE; else shoot_cmd_send.load_mode = LOAD_STOP; + shoot_cmd_send.shoot_rate=1; } /** @@ -140,25 +163,29 @@ static void MouseKeySet() static void EmergencyHandler() { // 拨轮的向下拨超过一半,注意向下拨轮是正 - if (rc_data[TEMP].rc.joystick[4] > 300) // 还需添加重要应用和模块离线的判断 + if (rc_data[TEMP].rc.joystick[4] > 300 || robot_state==ROBOT_STOP) // 还需添加重要应用和模块离线的判断 { robot_state = ROBOT_STOP; // 遥控器左上侧拨轮打满,进入紧急停止模式 gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE; chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; shoot_cmd_send.shoot_mode = SHOOT_OFF; - return; } - // if(rc_data[TEMP].rc.joystick[4]<-300 && 各个模块正常) - // { - // //恢复运行 - // //... - // } + if(switch_is_up(rc_data[TEMP].rc.s[0])) + { + robot_state = ROBOT_WORKING; // 遥控器右侧开关为[上],恢复正常运行 + shoot_cmd_send.shoot_mode = SHOOT_ON; + } } void GimbalCMDTask() { // 从其他应用获取回传数据 +#ifdef ONE_BOARD SubGetMessage(chassis_feed_sub, &chassis_fetch_data); +#endif // ONE_BOARD +#ifdef GIMBAL_BOARD + chassis_fetch_data=*(Chassis_Upload_Data_s*)CANCommGet(cmd_can_comm); +#endif // GIMBAL_BOARD SubGetMessage(shoot_feed_sub, &shoot_fetch_data); SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data); @@ -181,7 +208,12 @@ void GimbalCMDTask() // 推送消息,双板通信,视觉通信等 // 应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置 - PubPushMessage(chassis_cmd_pub, &chassis_cmd_send); +#ifdef ONE_BOARD + SubGetMessage(chassis_feed_sub, &chassis_fetch_data); +#endif // ONE_BOARD +#ifdef GIMBAL_BOARD + CANCommSend(cmd_can_comm,(void*)&chassis_cmd_send); +#endif // GIMBAL_BOARD PubPushMessage(shoot_cmd_pub, &shoot_cmd_send); PubPushMessage(gimbal_cmd_pub, &gimbal_cmd_send); VisionSend(&vision_send_data); diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 957dc2d..d1853d4 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -54,21 +54,21 @@ void GimbalInit() Motor_Init_Config_s pitch_config = { .can_init_config = { .can_handle = &hcan1, - .tx_id = 3, + .tx_id = 2, }, .controller_param_init_config = { .angle_PID = { - .Kp = 10, + .Kp = 30, .Ki = 0, .Kd = 0, - .MaxOut = 2000, + .MaxOut = 4000, .DeadBand=0.3, }, .speed_PID = { .Kp = 10, .Ki = 0, .Kd = 0, - .MaxOut = 2000, + .MaxOut = 4000, }, .other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch, // 还需要增加角速度额外反馈指针 @@ -117,6 +117,8 @@ void GimbalTask() break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: + DJIMotorEnable(yaw_motor); + DJIMotorEnable(pitch_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED); @@ -126,6 +128,8 @@ void GimbalTask() break; // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 case GIMBAL_FREE_MODE: + DJIMotorEnable(yaw_motor); + DJIMotorEnable(pitch_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED); DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED); DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, MOTOR_FEED); @@ -153,8 +157,8 @@ void GimbalTask() // 设置反馈数据 gimbal_feedback_data.gimbal_imu_data = *Gimbal_IMU_data; - gimbal_feedback_data.yaw_motor_single_round_angle = pitch_motor->motor_measure.angle_single_round; + gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round; - // 推送消息 + // 推送消息 PubPushMessage(gimbal_pub, &gimbal_feedback_data); } \ No newline at end of file diff --git a/application/robot.c b/application/robot.c index 9a11c9a..3fbbc92 100644 --- a/application/robot.c +++ b/application/robot.c @@ -14,26 +14,27 @@ void RobotInit() { BSPInit(); -#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisInit(); -#endif #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) GimbalCMDInit(); GimbalInit(); ShootInit(); #endif + +#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) + ChassisInit(); +#endif } void RobotTask() { -#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - ChassisTask(); -#endif - #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) GimbalCMDTask(); GimbalTask(); ShootTask(); #endif + +#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) + ChassisTask(); +#endif } diff --git a/application/robot_def.h b/application/robot_def.h index 17b23f4..67b1933 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -16,13 +16,12 @@ #include "master_process.h" #include "stdint-gcc.h" -/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译 */ -/* 只能存在一个宏定义! */ -#define ONE_BOARD // 单板控制整车 +/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ +// #define ONE_BOARD // 单板控制整车 // #define CHASSIS_BOARD //底盘板 -// #define GIMBAL_BOARD //云台板 +#define GIMBAL_BOARD //云台板 -/* 重要参数定义,注意根据不同机器人进行修改 */ +/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 #define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_ECD_GREATER_THAN_4096 0 // yaw电机的初始编码器值是否大于4096,是为1,否为0 @@ -39,11 +38,12 @@ #define RADIUS_WHEEL 60 // 轮子半径 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换 +// 检查是否出现定义冲突 #if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \ (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \ (defined(CHASSIS_BOARD) && defined(GIMBAL_BOARD)) #error Conflict board definition! You can only define one type. -#endif // 检查是否出现定义冲突 +#endif #pragma pack(1) // 压缩结构体,取消字节对齐 @@ -92,7 +92,7 @@ typedef enum // 发射模式设置 typedef enum { - SHOOT_ON, + SHOOT_ON=0, SHOOT_OFF, }shoot_mode_e; typedef enum diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 7aa4f10..8a8d40c 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -38,7 +38,7 @@ void ShootInit() .Kp = 10, .Ki = 0, .Kd = 0, - .MaxOut = 200, + .MaxOut = 2000, }, }, .controller_setting_init_config = { @@ -47,7 +47,7 @@ void ShootInit() .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, + .reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = M3508}; // 右摩擦轮 @@ -58,16 +58,16 @@ void ShootInit() }, .controller_param_init_config = { .speed_PID = { - .Kp = 1, + .Kp = 10, .Ki = 0, .Kd = 0, .MaxOut = 200, }, .current_PID = { - .Kp = 1, + .Kp = 5, .Ki = 0, .Kd = 0, - .MaxOut = 200, + .MaxOut = 2000, }, }, .controller_setting_init_config = { @@ -75,7 +75,7 @@ void ShootInit() .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, + .reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = M3508}; // 拨盘电机 @@ -88,28 +88,28 @@ void ShootInit() .angle_PID = { // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 .Kp = 10, - .Ki = 1, - .Kd = 2, + .Ki = 0, + .Kd = 0, .MaxOut = 200, }, .speed_PID = { .Kp = 1, .Ki = 0, .Kd = 0, - .MaxOut = 200, + .MaxOut = 2000, }, .current_PID = { - .Kp = 1, + .Kp = 10, .Ki = 0, .Kd = 0, - .MaxOut = 200, + .MaxOut = 3000, }, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 - .close_loop_type = ANGLE_LOOP | SPEED_LOOP | CURRENT_LOOP, - .reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向 + .close_loop_type = ANGLE_LOOP | SPEED_LOOP , + .reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 }, .motor_type = M2006 // 英雄使用m3508 }; @@ -134,6 +134,13 @@ void ShootTask() DJIMotorStop(friction_r); DJIMotorStop(loader); } + else // 恢复运行 + { + DJIMotorEnable(friction_l); + DJIMotorEnable(friction_r); + DJIMotorEnable(loader); + + } // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 if (hibernate_time + dead_time > DWT_GetTimeline_ms()) @@ -147,7 +154,7 @@ void ShootTask() DJIMotorOuterLoop(loader, SPEED_LOOP); DJIMotorSetRef(loader, 0); break; - // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入)F + // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入) case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. DJIMotorOuterLoop(loader, ANGLE_LOOP); DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 增加一发弹丸的角度 @@ -164,7 +171,7 @@ void ShootTask() // 连发模式,对速度闭环,射频后续修改为可变 case LOAD_BURSTFIRE: DJIMotorOuterLoop(loader, SPEED_LOOP); - DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_WHEEL / NUM_PER_CIRCLE); + DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8); // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度 break; // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈) @@ -193,8 +200,17 @@ void ShootTask() DJIMotorSetRef(friction_r, 0); break; default: + DJIMotorSetRef(friction_l, 200); + DJIMotorSetRef(friction_r, 200); break; + } // 关闭摩擦轮 + if (shoot_cmd_recv.friction_mode==FRICTION_OFF) + { + DJIMotorSetRef(friction_l, 0); + DJIMotorSetRef(friction_r, 0); } + + // 开关弹舱盖 if (shoot_cmd_recv.lid_mode == LID_CLOSE) diff --git a/modules/algorithm/controller.c b/modules/algorithm/controller.c index 836a88b..f972b72 100644 --- a/modules/algorithm/controller.c +++ b/modules/algorithm/controller.c @@ -85,6 +85,12 @@ float PID_Calculate(PIDInstance *pid, float measure, float ref) // 输出限幅 f_Output_Limit(pid); } + else // 进入死区,清空积分和输出 + { + pid->Output=0; + pid->ITerm=0; + } + pid->Last_Measure = pid->Measure; pid->Last_Output = pid->Output; pid->Last_Dout = pid->Dout; diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c index 0511c15..3c47aec 100644 --- a/modules/motor/HT04.c +++ b/modules/motor/HT04.c @@ -30,7 +30,7 @@ static void DecodeJoint(CANInstance *motor_instance) tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4); joint_motor_info[i]->speed_rpm = uint_to_float(tmp, V_MAX, V_MIN, 12); tmp = ((motor_instance->rx_buff[4] & 0xf) << 8) | motor_instance->rx_buff[5]; - joint_motor_info[i]->given_current = uint_to_float(tmp, T_MAX, T_MIN, 12); + joint_motor_info[i]->real_current = uint_to_float(tmp, T_MAX, T_MIN, 12); break; } } diff --git a/modules/motor/HT04.h b/modules/motor/HT04.h index c3608fe..904f96f 100644 --- a/modules/motor/HT04.h +++ b/modules/motor/HT04.h @@ -20,7 +20,7 @@ typedef struct // HT04 float last_ecd; float ecd; float speed_rpm; - float given_current; + float real_current; PIDInstance pid; CANInstance *motor_can_instace; diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c index 2efa7ac..6d5d4e1 100644 --- a/modules/motor/LK9025.c +++ b/modules/motor/LK9025.c @@ -11,7 +11,7 @@ static void DecodeDriven(CANInstance *_instance) driven_motor_info[i]->last_ecd = driven_motor_info[i]->ecd; driven_motor_info[i]->ecd = (uint16_t)((_instance->rx_buff[7] << 8) | _instance->rx_buff[6]); driven_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]); - driven_motor_info[i]->given_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]); + driven_motor_info[i]->real_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]); driven_motor_info[i]->temperate = _instance->rx_buff[1]; break; } diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h index 88529f6..8c8b8d1 100644 --- a/modules/motor/LK9025.h +++ b/modules/motor/LK9025.h @@ -15,7 +15,7 @@ typedef struct // 9025 uint16_t last_ecd; uint16_t ecd; int16_t speed_rpm; - int16_t given_current; + int16_t real_current; uint8_t temperate; PIDInstance *pid; diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index ab8617b..0c3655b 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -130,15 +130,15 @@ static void DecodeDJIMotor(CANInstance *_instance) { rxbuff = _instance->rx_buff; measure = &dji_motor_info[i]->motor_measure; // measure要多次使用,保存指针减小访存开销 - + uint8_t nice; // resolve data and apply filter to current and speed measure->last_ecd = measure->ecd; - measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); - measure->angle_single_round = ECD_ANGLE_COEF * measure->ecd; - measure->speed_angle_per_sec = (1 - SPEED_SMOOTH_COEF) * measure->speed_angle_per_sec + - RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]); - measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + - RPM_2_ANGLE_PER_SEC * CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]); + measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1]; + measure->angle_single_round = ECD_ANGLE_COEF * (float)measure->ecd; + measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + RPM_2_ANGLE_PER_SEC * + SPEED_SMOOTH_COEF *(float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])) ; + measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current + + CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5])); measure->temperate = rxbuff[6]; // multi rounds calc,计算的前提是两次采样间电机转过的角度小于180° @@ -176,6 +176,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config); + DJIMotorEnable(dji_motor_info[idx]); return dji_motor_info[idx++]; } @@ -223,7 +224,7 @@ void DJIMotorControl() static Motor_Control_Setting_s *motor_setting; static Motor_Controller_s *motor_controller; static DJI_Motor_Measure_s *motor_measure; - static float pid_measure,pid_ref; + static float pid_measure, pid_ref; // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 for (size_t i = 0; i < idx; i++) { @@ -233,7 +234,7 @@ void DJIMotorControl() motor_setting = &motor->motor_settings; motor_controller = &motor->motor_controller; motor_measure = &motor->motor_measure; - pid_ref=motor_controller->pid_ref; //保存设定值,防止motor_controller->pid_ref在计算过程中被修改 + pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改 // pid_ref会顺次通过被启用的闭环充当数据的载体 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 @@ -253,7 +254,7 @@ void DJIMotorControl() if (motor_setting->speed_feedback_source == OTHER_FEED) pid_measure = *motor_controller->other_speed_feedback_ptr; else // MOTOR_FEED - pid_measure = motor_measure->speed_angle_per_sec; + pid_measure = motor_measure->speed_aps; // 更新pid_ref进入下一个环 pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, pid_ref); } @@ -261,7 +262,7 @@ void DJIMotorControl() // 计算电流环,只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈 if (motor_setting->close_loop_type & CURRENT_LOOP) { - pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, pid_ref); + pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->real_current, pid_ref); } // 获取最终输出 @@ -272,14 +273,14 @@ void DJIMotorControl() // 分组填入发送数据 group = motor->sender_group; num = motor->message_num; - sender_assignment[group].tx_buff[2 * num] = 0xff & set >> 8; - sender_assignment[group].tx_buff[2 * num + 1] = 0xff & set; + sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); + sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 电机是否停止运行 - // if (motor->stop_flag == MOTOR_STOP) - // { // 若该电机处于停止状态,直接将buff置零 - // memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u); - // } + if (motor->stop_flag == MOTOR_STOP) + { // 若该电机处于停止状态,直接将buff置零 + memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u); + } } } diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index ee93f00..e155d43 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -29,14 +29,15 @@ /* DJI电机CAN反馈信息*/ typedef struct { - uint16_t ecd; // 0-8191,刻度总共有8192格 - uint16_t last_ecd; // 上一次读取的编码器值 - float angle_single_round; // 单圈角度 - float speed_angle_per_sec; // 角速度,单位为:度/秒 rounds per minute - int16_t given_current; // 实际电流 - uint8_t temperate; // 温度 Celsius - int16_t total_round; // 总圈数,注意方向 - int32_t total_angle; // 总角度,注意方向 + uint16_t last_ecd; // 上一次读取的编码器值 + uint16_t ecd; // 0-8191,刻度总共有8192格 + float angle_single_round; // 单圈角度 + float speed_aps; // 角速度,单位为:度/秒 rounds per minute + int16_t real_current; // 实际电流 + uint8_t temperate; // 温度 Celsius + + float total_angle; // 总角度,注意方向 + int32_t total_round; // 总圈数,注意方向 } DJI_Motor_Measure_s; /** diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 9382476..9af9d9d 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -9,6 +9,22 @@ static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据,[1]:上一次的数据.用于按 // 遥控器拥有的串口实例 static USARTInstance *rc_usart_instance; + +/** + * @brief 矫正遥控器摇杆的值 + * + */ +static void RectifyRCjoystick() +{ + for (uint8_t i = 0; i < 5; i++) + { + if(rc_ctrl[TEMP].rc.joystick[i]>660 || rc_ctrl[TEMP].rc.joystick[i]<-660) + rc_ctrl[TEMP].rc.joystick[i]=0; + } + +} + + /** * @brief remote control protocol resolution * @param[in] sbus_buf: raw data point @@ -24,6 +40,7 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf) rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 2 rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 3 rc_ctrl[TEMP].rc.joystick[4] = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF)- RC_CH_VALUE_OFFSET; // 左侧拨轮 + RectifyRCjoystick(); // 开关,0左1右 rc_ctrl[TEMP].rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left rc_ctrl[TEMP].rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right diff --git a/stm32.jflash b/stm32.jflash new file mode 100644 index 0000000..d9c38ea --- /dev/null +++ b/stm32.jflash @@ -0,0 +1,82 @@ + AppVersion = 72202 + FileVersion = 2 +[GENERAL] + aATEModuleSel[10] = 0,0,0,0,0,0,0,0,0,0 + ConnectMode = 0 + CurrentFile = "" + DataFileSAddr = 0x00000000 + GUIMode = 0 + HostName = "" + TargetIF = 1 + USBPort = 0 + USBSerialNo = 0x00000000 + UseATEModuleSelection = 0 +[JTAG] + IRLen = 0 + MultipleTargets = 0 + NumDevices = 0 + Speed0 = 4000 + Speed1 = 4000 + TAP_Number = 0 + UseAdaptive0 = 0 + UseAdaptive1 = 0 + UseMaxSpeed0 = 0 + UseMaxSpeed1 = 0 +[CPU] + NumInitSteps = 1 + InitStep0_Action = "Reset" + InitStep0_Value0 = 0x00000000 + InitStep0_Value1 = 0x00000000 + InitStep0_Comment = "Reset and halt target" + NumExitSteps = 0 + UseScriptFile = 0 + ScriptFile = "" + UseRAM = 1 + RAMAddr = 0x20000000 + RAMSize = 0x00020000 + CheckCoreID = 1 + CoreID = 0x4BA00477 + CoreIDMask = 0x0F000FFF + UseAutoSpeed = 0x00000001 + ClockSpeed = 0x00000000 + EndianMode = 0 + ChipName = "ST STM32F407IG" +[FLASH] + aRangeSel[1] = 0-11 + BankName = "Internal flash" + BankSelMode = 1 + BaseAddr = 0x08000000 + NumBanks = 1 +[PRODUCTION] + AutoPerformsDisconnect = 0 + AutoPerformsErase = 1 + AutoPerformsProgram = 1 + AutoPerformsSecure = 0 + AutoPerformsStartApp = 0 + AutoPerformsUnsecure = 0 + AutoPerformsVerify = 1 + EnableFixedVTref = 0 + EnableTargetPower = 0 + EraseType = 1 + FixedVTref = 0x00000CE4 + MonitorVTref = 0 + MonitorVTrefMax = 0x0000157C + MonitorVTrefMin = 0x000003E8 + OverrideTimeouts = 0 + ProgramSN = 0 + SerialFile = "" + SNAddr = 0x00000000 + SNInc = 0x00000001 + SNLen = 0x00000004 + SNListFile = "" + SNValue = 0x00000001 + StartAppType = 0 + TargetPowerDelay = 0x00000014 + TimeoutErase = 0x00003A98 + TimeoutProgram = 0x00002710 + TimeoutVerify = 0x00002710 + VerifyType = 1 +[PERFORMANCE] + DisableSkipBlankDataOnProgram = 0x00000000 + PerfromBlankCheckPriorEraseChip = 0x00000001 + PerfromBlankCheckPriorEraseSelectedSectors = 0x00000001