新增底盘电机功率限制
This commit is contained in:
parent
9e51196992
commit
fdcd89b265
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
}
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
.Kp = 3, // 10
|
||||
.Ki = 0, // 1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 5000,
|
||||
.MaxOut = 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)
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue