HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

功率控制（需要调参数）、拨弹反转（未验证）

This commit is contained in:
userName 2024-02-04 10:59:41 +08:00
parent 8be769bf3f
commit 35ad6c7055
5 changed files with 82 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
application/chassis/chassis.c
View File

@ -157,15 +157,28 @@ static void MecanumCalculate()
*/
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;
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);
}
/**

14
application/shoot/shoot.c
View File

@ -16,6 +16,7 @@ static Shoot_Ctrl_Cmd_s shoot_cmd_recv; // 来自cmd的发射控制信息
static Subscriber_t *shoot_sub;
static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
static float stop_location;
// dwt定时,计算冷却用
static float hibernate_time = 0, dead_time = 0;
@ -107,7 +108,16 @@ void ShootInit()
shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
}
static void Stop_Check()
{
if(loader->measure.total_angle < stop_location*0.9)
{
DJIMotorOuterLoop(loader, ANGLE_LOOP);
DJIMotorSetRef(loader,stop_location);
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 1000;
}
}
/* 机器人发射机构控制核心任务 */
void ShootTask()
{
@ -139,10 +149,12 @@ void ShootTask()
case LOAD_STOP:
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
break;
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
stop_location = loader->measure.total_angle;
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 2000; // 完成1发弹丸发射的时间

37
modules/motor/DJImotor/dji_motor.c
View File

@ -228,7 +228,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()
{
@ -290,6 +299,34 @@ 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;

12
modules/motor/motor_def.h
View File

@ -67,7 +67,11 @@ typedef enum
MOTOR_STOP = 0,
MOTOR_ENALBED = 1,
} Motor_Working_Type_e;
typedef enum
{
NO_POWER_LIMIT = 0,
POWER_LIMIT_ON = 1,
} Power_Limit_Type_e;
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct
{
@ -78,7 +82,7 @@ typedef struct
Feedback_Source_e angle_feedback_source; // 角度反馈类型
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 +99,10 @@ typedef struct
PIDInstance angle_PID;
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
float motor_power_max; //每个电机分配的功率上限
float motor_power_predict; //根据模型预测的电机功率
float motor_power_scale; //电机功率缩放比例
} Motor_Controller_s;
/* 电机类型枚举 */

1
modules/remote/remote_control.c
View File

@ -103,6 +103,7 @@ static void RemoteControlRxCallback()
*/
static void RCLostCallback(void *id)
{
DaemonReload(rc_daemon_instance); // 遥控器没数据也可以喂狗
memset(rc_ctrl, 0, sizeof(rc_ctrl)); // 清空遥控器数据
USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
LOGWARNING("[rc] remote control lost");