From fdcd89b26567ad97c2fbfb51b18b00c02d61df78 Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Sat, 20 Jan 2024 16:16:13 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=BA=95=E7=9B=98=E7=94=B5?= =?UTF-8?q?=E6=9C=BA=E5=8A=9F=E7=8E=87=E9=99=90=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- application/chassis/chassis.c | 81 ++++++++++++++++++++++++------ application/cmd/robot_cmd.c | 3 ++ application/gimbal/gimbal.c | 58 ++++++++++++--------- application/robot.c | 10 ++-- application/robot_def.h | 3 +- application/shoot/shoot.c | 54 ++++++++++++++++---- modules/motor/DJImotor/dji_motor.c | 42 +++++++++++++++- modules/motor/motor_def.h | 12 +++++ modules/power_meter/power_meter.c | 38 ++++++++++++++ modules/power_meter/power_meter.h | 39 ++++++++++++++ 11 files changed, 284 insertions(+), 58 deletions(-) create mode 100644 modules/power_meter/power_meter.c create mode 100644 modules/power_meter/power_meter.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ca44ab..2b6306a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310 modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd - modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa + modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/power_meter modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor modules/motor/ECmotor application application/chassis application/cmd application/gimbal application/shoot Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 29b5401..741581a 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -17,6 +17,7 @@ #include "super_cap.h" #include "message_center.h" #include "referee_task.h" +#include "power_meter.h" #include "general_def.h" #include "bsp_dwt.h" @@ -46,6 +47,7 @@ static referee_info_t* referee_data; // 用于获取裁判系统的数据 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI static SuperCapInstance *cap; // 超级电容 +static PowerMeterInstance *power_meter; //功率计 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back /* 用于自旋变速策略的时间变量 */ @@ -55,6 +57,8 @@ static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left righ static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 +static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f}; + void ChassisInit() { // 四个轮子的参数一样,改tx_id和反转标志位即可 @@ -82,7 +86,9 @@ void ChassisInit() .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, - .close_loop_type = SPEED_LOOP,//| CURRENT_LOOP, + .close_loop_type = SPEED_LOOP| CURRENT_LOOP, + + .power_limit_flag = POWER_LIMIT_ON, //开启功率限制 }, .motor_type = M3508, }; @@ -114,6 +120,15 @@ void ChassisInit() }}; cap = SuperCapInit(&cap_conf); // 超级电容初始化 + PowerMeter_Init_Config_s power_conf = { + .can_config = { + .can_handle = &hcan1, + .rx_id = 0x212, + } + }; + + power_meter = PowerMeterInit(&power_conf); + // 发布订阅初始化,如果为双板,则需要can comm来传递消息 #ifdef CHASSIS_BOARD Chassis_IMU_data = INS_Init(); // 底盘IMU初始化 @@ -165,21 +180,47 @@ static void OmniCalculate() vt_lf/=(WHEEL_BASE*1.414f); } + +//依据3508电机功率模型，预测电机输出功率 +static float EstimatePower(DJIMotorInstance* chassis_motor) +{ + + float I_cmd = chassis_motor->motor_controller.current_PID.Output; + float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm + + float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd; + + return power; +} +float vofa_send_data[6]; /** * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值 * */ static void LimitChassisOutput() { - // 功率限制待添加 - // referee_data->PowerHeatData.chassis_power; - // referee_data->PowerHeatData.chassis_power_buffer; + float P_cmd = motor_rf->motor_controller.motor_power_predict + + motor_rb->motor_controller.motor_power_predict + + motor_lb->motor_controller.motor_power_predict + + motor_lf->motor_controller.motor_power_predict + 3.6f; + float P_max = 60 - 10; + float K = P_max/P_cmd; + vofa_send_data[2] = P_cmd; + + motor_rf->motor_controller.motor_power_scale = K; + motor_rb->motor_controller.motor_power_scale = K; + motor_lf->motor_controller.motor_power_scale = K; + motor_lb->motor_controller.motor_power_scale = K; + + { + DJIMotorSetRef(motor_lf, vt_lf); + DJIMotorSetRef(motor_rf, vt_rf); + DJIMotorSetRef(motor_lb, vt_lb); + DJIMotorSetRef(motor_rb, vt_rb); + } + + - // 完成功率限制后进行电机参考输入设定 - DJIMotorSetRef(motor_lf, vt_lf); - DJIMotorSetRef(motor_rf, vt_rf); - DJIMotorSetRef(motor_lb, vt_lb); - DJIMotorSetRef(motor_rb, vt_rb); } /** @@ -246,16 +287,28 @@ void ChassisTask() chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta; chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta; + motor_rf->motor_controller.motor_power_max = 10; + // 根据控制模式进行正运动学解算,计算底盘输出 //MecanumCalculate(); OmniCalculate(); - + //vt_rf = 5000; // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 LimitChassisOutput(); -// float vofa_send_data[2]; -// vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref; -// vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure; -// vofa_justfloat_output(vofa_send_data,8,&huart1); + + //DJIMotorSetRef(motor_rf, 5000); + + + vofa_send_data[0] = motor_rf->motor_controller.speed_PID.Ref; + vofa_send_data[1] = motor_rf->motor_controller.speed_PID.Measure; + + vofa_send_data[3] = PowerMeterGet(power_meter); + vofa_send_data[4] = 60; + + + + + vofa_justfloat_output(vofa_send_data,24,&huart1); // 根据电机的反馈速度和IMU(如果有)计算真实速度 EstimateSpeed(); diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 326454c..1fc484f 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -157,6 +157,9 @@ static void RemoteControlSet() shoot_cmd_send.load_mode = LOAD_STOP; // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, shoot_cmd_send.shoot_rate = 8; + //检测到卡弹 拨弹盘反转 + if(shoot_fetch_data.stalled_flag == 1) + shoot_cmd_send.load_mode = LOAD_REVERSE; } /** diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index f1b76fc..a45e046 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -9,10 +9,13 @@ #include "bmi088.h" #include "vofa.h" +#include "power_meter.h" + + static attitude_t *gimba_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor, *pitch_motor; -static ECMotorInstance *big_yaw_motor; +static DJIMotorInstance *big_yaw_motor; static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd) static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 @@ -110,14 +113,16 @@ void GimbalInit() .close_loop_type = OPEN_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, - .motor_type = ECA8210 + .motor_type = M3508 }; + // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 // yaw_motor = DJIMotorInit(&yaw_config); // pitch_motor = DJIMotorInit(&pitch_config); - big_yaw_motor = ECMotorInit(&big_yaw_config); + //big_yaw_motor = ECMotorInit(&big_yaw_config); + //big_yaw_motor = DJIMotorInit(&big_yaw_config); gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); @@ -172,41 +177,43 @@ void GimbalTask() // ... //DJIMotorEnable(pitch_motor); - ECMotorEnable(big_yaw_motor); - - float input = step_input_generate(&sinInputGenerate); + //ECMotorEnable(big_yaw_motor); + float input = sin_input_generate(&sinInputGenerate); + //DJIMotorEnable(big_yaw_motor); //DJIMotorSetRef(pitch_motor,input); - ECMotorSetRef(big_yaw_motor,30); + //ECMotorSetRef(big_yaw_motor,30); //ANODT_SendF1(input*1000,pitch_motor->measure.speed_aps*1000,0,0); float theta = pitch_motor->measure.angle_single_round - 6200 * ECD_ANGLE_COEF_DJI; float gravity_feed = 3800*arm_cos_f32(theta/180*PI); - //DJIMotorSetRef(pitch_motor,gravity_feed); - float vofa_send_data[4]; - vofa_send_data[0]=big_yaw_motor->measure.speed_rads; - vofa_send_data[1]=big_yaw_motor->measure.angle_single_round; - vofa_send_data[2]=big_yaw_motor->measure.real_current; - vofa_send_data[3]=big_yaw_motor->measure.temperature; -// vofa_send_data[3]=yaw_motor->motor_controller.angle_PID.Measure; - vofa_justfloat_output(vofa_send_data,16,&huart1); + //DJIMotorSetRef(big_yaw_motor,input+2000); +// float vofa_send_data[6]; +// vofa_send_data[0]=big_yaw_motor->measure.speed_aps; +// vofa_send_data[1]=big_yaw_motor->measure.angle_single_round; +// vofa_send_data[2]=big_yaw_motor->measure.real_current; +// vofa_send_data[3]=power_meter->power_msg.vol; +// vofa_send_data[4]=power_meter->power_msg.current; +// vofa_send_data[5]=big_yaw_motor->motor_controller.pid_ref; +// +// vofa_justfloat_output(vofa_send_data,24,&huart1); - vofa_send_data[0] = pitch_motor->motor_controller.pid_ref; - vofa_send_data[1] = pitch_motor->measure.real_current; - vofa_send_data[2] = theta; - vofa_send_data[3] = gravity_feed; - vofa_justfloat_output(vofa_send_data,16,&huart1); +// vofa_send_data[0] = pitch_motor->motor_controller.pid_ref; +// vofa_send_data[1] = pitch_motor->measure.real_current; +// vofa_send_data[2] = theta; +// vofa_send_data[3] = gravity_feed; +// vofa_justfloat_output(vofa_send_data,16,&huart1); // 设置反馈数据,主要是imu和yaw的ecd - gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; - gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; - - // 推送消息 - PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); +// gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; +// gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; +// +// // 推送消息 +// PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); } @@ -260,6 +267,7 @@ float sin_input_generate(sin_input_generate_t* InputGenerate) else InputGenerate->input = arm_sin_f32(2*PI*InputGenerate->frequency[InputGenerate->frequency_index]*InputGenerate->time); //float input = arm_sin_f32(2*PI*frequency*time); + InputGenerate->input *= 2000; return InputGenerate->input; } diff --git a/application/robot.c b/application/robot.c index 7de82b4..1f11e52 100644 --- a/application/robot.c +++ b/application/robot.c @@ -31,13 +31,13 @@ void RobotInit() #if defined(ONE_BOARD) || defined(GIMBAL_BOARD) RobotCMDInit(); - //暂时注释云台和发射任务 调试底盘 + GimbalInit(); -// ShootInit(); + ShootInit(); #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - //ChassisInit(); + ChassisInit(); #endif OSTaskInit(); // 创建基础任务 @@ -52,11 +52,11 @@ void RobotTask() RobotCMDTask(); //暂时注释云台和发射任务 调试底盘 GimbalTask(); -// ShootTask(); + ShootTask(); #endif #if defined(ONE_BOARD) || defined(CHASSIS_BOARD) - //ChassisTask(); + ChassisTask(); #endif } \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index e66ac8f..d1b21dd 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -19,7 +19,7 @@ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ #define ONE_BOARD // 单板控制整车 // #define CHASSIS_BOARD //底盘板 -// #define GIMBAL_BOARD //云台板 +//#define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 // #define VISION_USE_UART // 使用串口发送视觉数据 @@ -204,6 +204,7 @@ typedef struct { // code to go here // ... + uint8_t stalled_flag; //堵转标志 } Shoot_Upload_Data_s; #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1) diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index e19af5c..36d5fed 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -16,7 +16,7 @@ static Subscriber_t *shoot_sub; static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息 // dwt定时,计算冷却用 -static float hibernate_time = 0, dead_time = 0; +static float hibernate_time = 0,last_hibernate_time = 0, dead_time = 0; void ShootInit() { @@ -48,7 +48,7 @@ void ShootInit() .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, - .close_loop_type = SPEED_LOOP | CURRENT_LOOP, + .close_loop_type = SPEED_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = M3508}; @@ -74,12 +74,12 @@ void ShootInit() .MaxOut = 200, }, .speed_PID = { - .Kp = 0, // 10 - .Ki = 0, // 1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 5000, - .MaxOut = 5000, + .Kp = 3, // 10 + .Ki = 0, // 1 + .Kd = 0, + .Improve = PID_Integral_Limit, + .IntegralLimit = 5000, + .MaxOut = 10000, }, .current_PID = { .Kp = 0, // 0.7 @@ -103,6 +103,31 @@ void ShootInit() shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); } +//卡弹检测 +void stalled_detect() +{ + static float last_detect_time = 0,detect_time = 0; + static float last_total_angle = 0; + static uint8_t stalled_cnt = 0; + + last_detect_time = detect_time; + detect_time = DWT_GetTimeline_ms(); + + if(detect_time - last_detect_time < 200) // 200ms 检测一次 + return; + + if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE) + { + float reference_angle = (detect_time - last_detect_time) * loader->motor_controller.pid_ref * 1e3f; + float real_angle = loader->measure.total_angle - last_total_angle; + if(real_angle < reference_angle * 0.2f) + { + //stalled_cnt ++; + shoot_feedback_data.stalled_flag = 1; + } + } +} + /* 机器人发射机构控制核心任务 */ void ShootTask() @@ -126,8 +151,8 @@ void ShootTask() // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) - // if (hibernate_time + dead_time > DWT_GetTimeline_ms()) - // return; + if (hibernate_time + dead_time > DWT_GetTimeline_ms()) + return; // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 switch (shoot_cmd_recv.load_mode) @@ -155,12 +180,17 @@ void ShootTask() case LOAD_BURSTFIRE: DJIMotorOuterLoop(loader, SPEED_LOOP); DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8); - // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second) + + // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second) break; // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流) // 也有可能需要从switch-case中独立出来 case LOAD_REVERSE: DJIMotorOuterLoop(loader, SPEED_LOOP); + DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转 + hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + dead_time = 500; // 翻转500ms + shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志 // ... break; default: @@ -207,6 +237,8 @@ void ShootTask() { //... } + //卡弹检测 + stalled_detect(); // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈 PubPushMessage(shoot_pub, (void *)&shoot_feedback_data); diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index eb14da8..d70c4b1 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -198,6 +198,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr; // 后续增加电机前馈控制器(速度和电流) + // 电机分组,因为至多4个电机可以共用一帧CAN控制报文 MotorSenderGrouping(instance, &config->can_init_config); @@ -251,6 +252,16 @@ void DJIMotorSetRef(DJIMotorInstance *motor, float ref) { motor->motor_controller.pid_ref = ref; } +static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f}; +//依据3508电机功率模型，预测电机输出功率 +static float EstimatePower(DJIMotorInstance* chassis_motor,float output) +{ + float I_cmd = chassis_motor->motor_controller.current_PID.Output; + float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm + + float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd; + return power; +} // 为所有电机实例计算三环PID,发送控制报文 void DJIMotorControl() @@ -274,7 +285,6 @@ void DJIMotorControl() pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改 if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) pid_ref *= -1; // 设置反转 - // pid_ref会顺次通过被启用的闭环充当数据的载体 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) @@ -308,6 +318,7 @@ void DJIMotorControl() { //现在3508和6020电调都内置电流闭环 无需PID计算 //pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref); + motor_controller->current_PID.Output = pid_ref; pid_ref = pid_ref; } @@ -316,6 +327,35 @@ void DJIMotorControl() if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) pid_ref *= -1; + //功率限制 + if(motor_setting->power_limit_flag == POWER_LIMIT_ON) + { + float I_cmd = pid_ref; + float w = measure->speed_aps /6 ;//aps to rpm + motor_controller->motor_power_predict = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd; + //这里K应该使用所有底盘电机一起计算 (在底盘任务中) + //float K = motor_controller->motor_power_max / motor_controller->motor_power_predict; + float K = motor_controller->motor_power_scale; + if(K>=1 || motor_controller->motor_power_predict < 0)//未超过最大功率 或做负功的电机 不做限制 + { + + } + else if(K<1) + { + float P_cmd = K * motor_controller->motor_power_predict; //对输出功率进行缩放 + + float a = motor_power_K[2] ; + float b = motor_power_K[0] * w; + float c = motor_power_K[1] * w * w - P_cmd; + + if(pid_ref < 0) + pid_ref = (-b - sqrtf(b*b-4*a*c))/(2*a); + else + pid_ref = (-b + sqrtf(b*b-4*a*c))/(2*a); + } + //if( motor_controller->motor_power_predict < ) + } + // 获取最终输出 set = (int16_t)pid_ref; diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index e7d4f61..bf5b782 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -68,6 +68,11 @@ typedef enum MOTOR_ENALBED = 1, } Motor_Working_Type_e; +typedef enum +{ + NO_POWER_LIMIT = 0, + POWER_LIMIT_ON = 1, +} Power_Limit_Type_e; /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */ typedef struct { @@ -79,6 +84,8 @@ typedef struct Feedback_Source_e speed_feedback_source; // 速度反馈类型 Feedfoward_Type_e feedforward_flag; // 前馈标志 + Power_Limit_Type_e power_limit_flag; //功率限制标志 + } Motor_Control_Setting_s; /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/ @@ -95,6 +102,10 @@ typedef struct PIDInstance angle_PID; float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环 + + float motor_power_max; //每个电机分配的功率上限 + float motor_power_predict; //根据模型预测的电机功率 + float motor_power_scale; //电机功率缩放比例 } Motor_Controller_s; /* 电机类型枚举 */ @@ -143,6 +154,7 @@ typedef struct Motor_Type_e motor_type; CAN_Init_Config_s can_init_config; Motor_Control_Type_e motor_control_type; + } Motor_Init_Config_s; #endif // !MOTOR_DEF_H diff --git a/modules/power_meter/power_meter.c b/modules/power_meter/power_meter.c new file mode 100644 index 0000000..595af75 --- /dev/null +++ b/modules/power_meter/power_meter.c @@ -0,0 +1,38 @@ +// +// Created by SJQ on 2023/12/19. +// + +#include "power_meter.h" +#include "memory.h" +#include "stdlib.h" + +static PowerMeterInstance *power_meter_instance = NULL; // 可以由app保存此指针 + +static void PowerMeterRxCallback(CANInstance *_instance) +{ + uint8_t *rxbuff; + PowerMeter_Msg_s *Msg; + rxbuff = _instance->rx_buff; + Msg = &power_meter_instance->power_msg; + Msg->vol = (uint16_t)(rxbuff[1] << 8 | rxbuff[0]); + Msg->current = (uint16_t)(rxbuff[3] << 8 | rxbuff[2]); + +} + +PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *PowerMeter_config) +{ + power_meter_instance = (PowerMeterInstance *)malloc(sizeof(PowerMeterInstance)); + memset(power_meter_instance, 0, sizeof(PowerMeterInstance)); + + PowerMeter_config->can_config.can_module_callback = PowerMeterRxCallback; + power_meter_instance->can_ins = CANRegister(&PowerMeter_config->can_config); + return power_meter_instance; +} + +float PowerMeterGet(PowerMeterInstance *instance) +{ + float power = instance->power_msg.current*instance->power_msg.vol/1e4; + return power; +} + +#include "power_meter.h" diff --git a/modules/power_meter/power_meter.h b/modules/power_meter/power_meter.h new file mode 100644 index 0000000..f5464ee --- /dev/null +++ b/modules/power_meter/power_meter.h @@ -0,0 +1,39 @@ +// +// Created by SJQ on 2023/12/19. +// + +#ifndef BASIC_FRAMEWORK_POWER_METER_H +#define BASIC_FRAMEWORK_POWER_METER_H +#include "bsp_can.h" + +#pragma pack(1) +typedef struct +{ + uint16_t vol; // 电压 + uint16_t current; // 电流 +} PowerMeter_Msg_s; +#pragma pack() + +/* 超级电容实例 */ +typedef struct +{ + CANInstance *can_ins; // CAN实例 + PowerMeter_Msg_s power_msg; // 功率计反馈信息 +} PowerMeterInstance; + +/* 超级电容初始化配置 */ +typedef struct +{ + CAN_Init_Config_s can_config; +} PowerMeter_Init_Config_s; + +/** + * @brief 初始化功率计 + * + * @param PowerMeter_config 功率计初始化配置 + * @return PowerMeterInstance* 功率计实例指针 + */ +PowerMeterInstance *PowerMeterInit(PowerMeter_Init_Config_s *PowerMeter_config); +float PowerMeterGet(PowerMeterInstance *instance); + +#endif //BASIC_FRAMEWORK_POWER_METER_H