From 78c9a77cf4c0e47c0ec9dc5d5450803079c35a4b Mon Sep 17 00:00:00 2001 From: NeoZng Date: Sat, 3 Dec 2022 21:39:31 +0800 Subject: [PATCH] =?UTF-8?q?=E6=90=AD=E5=BB=BA=E4=BA=86chassis=E7=9A=84?= =?UTF-8?q?=E6=A1=86=E6=9E=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 +- Makefile | 3 +- application/chassis/chassis.c | 75 ++++++++++++++++++++++--- application/cmd/chassis_cmd.c | 4 +- application/gimbal/gimbal.c | 2 + modules/message_center/message_center.c | 3 +- modules/message_center/message_center.h | 3 +- modules/motor/dji_motor.c | 12 +++- modules/motor/dji_motor.h | 6 ++ 9 files changed, 98 insertions(+), 13 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 826f45c..d37caac 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -29,7 +29,8 @@ "remote_control.h": "c", "gimbal_cmd.h": "c", "chassis_cmd.h": "c", - "referee.h": "c" + "referee.h": "c", + "arm_math.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/Makefile b/Makefile index f1f3eac..504ed68 100644 --- a/Makefile +++ b/Makefile @@ -22,7 +22,8 @@ TARGET = basic_framework # debug build? DEBUG = 1 # optimization -OPT = -Og +OPT = -O0 # O0避免没有使用到的变量被优化,如果没有特殊的调试需求请修改成-Og. + # 为了更高的性能,正式上车不需要调试时修改成-O3 ####################################### diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 2b19b39..2af5dcc 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -3,6 +3,13 @@ #include "dji_motor.h" #include "super_cap.h" #include "message_center.h" +#include "arm_math.h" + +#define OFFSET_X_CENTER //纵向轮距(前进后退方向) +#define OFFSET_Y_CENTER //横向轮距(左右平移方向) +#define RADIUS_WHEEL //轮子半径 +#define PERIMETER_WHEEL +#define REDUCTION_RATIO 19 /* 底盘应用包含的模块和信息存储 */ #ifdef CHASSIS_BOARD // 使用板载IMU获取底盘转动角速度 @@ -16,15 +23,13 @@ static dji_motor_instance *lb; static dji_motor_instance *rb; 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 Subscriber_t* chassis_sub; +static Chassis_Upload_Data_s chassis_feedback_data; +// 将云台系的速度投影到底盘 +static float chassis_vx,chassis_vy; -static void mecanum_calculate() -{ - -} void ChassisInit() { @@ -144,7 +149,63 @@ void ChassisInit() chassis_pub=PubRegister("chassis_feed",sizeof(Chassis_Upload_Data_s)); } -void ChassisTask() + +/** + * @brief 计算每个轮毂电机的输出,正运动学解算 + * + */ +static void MecanumCalculate() { +} + +/** + * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算 + * + */ +static void EstimateSpeed() +{ + +} +// chassis_cmd_recv chassis_feedback_data +void ChassisTask() +{ + // 后续增加没收到消息的处理 + // 获取新的控制信息 + SubGetMessage(chassis_sub,&chassis_cmd_recv); + + if(chassis_cmd_recv.chassis_cmd.chassis_mode==CHASSIS_ZERO_FORCE) + { + DJIMotorStop(); + } + + // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上 + chassis_vx=chassis_cmd_recv.chassis_cmd.vx*arm_cos_f32(chassis_cmd_recv.chassis_cmd.offset_angle)- + chassis_cmd_recv.chassis_cmd.vy*arm_sin_f32(chassis_cmd_recv.chassis_cmd.offset_angle); + chassis_vy=chassis_cmd_recv.chassis_cmd.vx*arm_sin_f32(chassis_cmd_recv.chassis_cmd.offset_angle)- + chassis_cmd_recv.chassis_cmd.vy*arm_cos_f32(chassis_cmd_recv.chassis_cmd.offset_angle); + + // 根据控制模式设定旋转速度 + switch (chassis_cmd_recv.chassis_cmd.chassis_mode) + { + case CHASSIS_NO_FOLLOW: + chassis_cmd_recv.chassis_cmd.wz=0; + break; + case CHASSIS_FOLLOW_GIMBAL_YAW: + chassis_cmd_recv.chassis_cmd.wz=0.05f*powf(chassis_cmd_recv.chassis_cmd.wz,2.0f); // 不开pid,以误差角平方为输出 + break; + case CHASSIS_ROTATE: + //chassis_cmd_recv.chassis_cmd.wz 当前维持定值,后续增加不规则的变速策略 + break; + default: + break; + } + + //根据控制模式进行正运动学解算,计算底盘输出 + MecanumCalculate(); + + //根据电机的反馈速度计算 + EstimateSpeed(); + + PubPushMessage(chassis_pub,&chassis_feedback_data); } \ No newline at end of file diff --git a/application/cmd/chassis_cmd.c b/application/cmd/chassis_cmd.c index c208f8c..e7e64d2 100644 --- a/application/cmd/chassis_cmd.c +++ b/application/cmd/chassis_cmd.c @@ -34,5 +34,7 @@ void ChassisCMDInit() void ChassisCMDTask() { - + SubGetMessage(chassis_feed_sub,&chassis_fetch_data); + SubGetMessage(gimbal2chassis_sub,&data_from_gimbal_cmd); + } diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 411b586..37b32c1 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -5,6 +5,8 @@ #include "message_center.h" +#define YAW_ALIGN_ECD 0 + static attitude_t* Gimbal_IMU_data; // 云台IMU数据 static dji_motor_instance* yaw_motor; // yaw电机 static dji_motor_instance *pitch_motor; // pitch电机 diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c index 43e6399..0beded9 100644 --- a/modules/message_center/message_center.c +++ b/modules/message_center/message_center.c @@ -178,7 +178,7 @@ uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr) return 1; } -void PubPushMessage(Publisher_t *pub, void *data_ptr) +uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr) { Subscriber_t *iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该事件的所有订阅者;如果为空说明遍历结束 while (iter) // 遍历订阅了当前事件的所有订阅者,依次填入最新消息 @@ -195,4 +195,5 @@ void PubPushMessage(Publisher_t *pub, void *data_ptr) iter = iter->next_subs_queue; // 访问下一个订阅者 } + return 1; } \ No newline at end of file diff --git a/modules/message_center/message_center.h b/modules/message_center/message_center.h index 4143335..c6212bd 100644 --- a/modules/message_center/message_center.h +++ b/modules/message_center/message_center.h @@ -109,7 +109,8 @@ uint8_t SubGetMessage(Subscriber_t* sub,void* data_ptr); * * @param pub 发布者实例指针 * @param data_ptr 指向要发布的数据的指针 + * @return uint8_t 新消息成功推送给几个订阅者 */ -void PubPushMessage(Publisher_t* pub,void* data_ptr); +uint8_t PubPushMessage(Publisher_t* pub,void* data_ptr); diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 59ce6c3..57a1d96 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -4,6 +4,7 @@ #define ECD_ANGLE_COEF 0.043945f // 360/8192 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 +static uint8_t stop_flag=0; /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; @@ -256,7 +257,16 @@ void DJIMotorControl() { if (sender_enable_flag[i]) { + if(stop_flag) // 如果紧急停止,将所有发送信息置零 + { + memset(sender_assignment[i].tx_buff,0,8); + } CANTransmit(&sender_assignment[i]); } } -} \ No newline at end of file +} + +void DJIMotorStop() +{ + stop_flag=1; +} diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index 7aa2ce9..577bc27 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -106,4 +106,10 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba void DJIMotorControl(); +/** + * @brief 停止所有电机 + * + */ +void DJIMotorStop(); + #endif // !DJI_MOTOR_H