126 lines
4.5 KiB
C
126 lines
4.5 KiB
C
/*
|
||
chassis Task
|
||
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "task/user_task.h"
|
||
/* USER INCLUDE BEGIN */
|
||
#include "module/chassis.h"
|
||
#include "module/config.h"
|
||
#include "device/ai.h"
|
||
#include "device/supercap.h"
|
||
#include "component/limiter.h"
|
||
/* USER INCLUDE END */
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* USER STRUCT BEGIN */
|
||
Chassis_t chassis;
|
||
Chassis_CMD_t cmd_chassis;
|
||
AI_result_t c_cmd_ai_result; /* 新增的 ai 结果变量 主要是给底盘接收自瞄相关的命令*/
|
||
float temp_for_cap[7];//cap-add//supercap-add
|
||
float P3508[4],P6020[4],I6020[4],S6020[4],I3508[4],S3508[4],power_limit_3508=100;
|
||
/* USER STRUCT END */
|
||
|
||
/* Private function --------------------------------------------------------- */
|
||
/* Exported functions ------------------------------------------------------- */
|
||
void Task_chassis(void *argument) {
|
||
(void)argument; /* 未使用argument，消除警告 */
|
||
|
||
|
||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||
const uint32_t delay_tick = osKernelGetTickFreq() / CHASSIS_FREQ;
|
||
|
||
osDelay(CHASSIS_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||
|
||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||
/* USER CODE INIT BEGIN */
|
||
/*底盘初始化*/
|
||
chassis_init(&chassis,&Config_GetRobotParam()->chassis,CHASSIS_FREQ);
|
||
|
||
/* USER CODE INIT END */
|
||
|
||
while (1) {
|
||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||
/* USER CODE BEGIN */
|
||
|
||
osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0);
|
||
|
||
osMessageQueueGet(task_runtime.msgq.gimbal.yaw4310,&chassis.motorfeedback.gimbal_yaw_encoder,NULL,0);
|
||
/*接受cmd任务数据*/
|
||
if(osMessageQueueGet(task_runtime.msgq.chassis.cmd, &cmd_chassis, NULL, 0)==osOK)
|
||
{
|
||
if(cmd_chassis.ctrl_mode == CHASSIS_MODE_DAOHANG){
|
||
cmd_chassis.Vx = c_cmd_ai_result.chassis_t.Vx;
|
||
cmd_chassis.Vy = c_cmd_ai_result.chassis_t.Vy;
|
||
cmd_chassis.Vw = c_cmd_ai_result.chassis_t.Vw;
|
||
}
|
||
else{
|
||
cmd_chassis.Vx = cmd_chassis.Vx;
|
||
cmd_chassis.Vy = cmd_chassis.Vy;
|
||
cmd_chassis.Vw = cmd_chassis.Vw;
|
||
}
|
||
Chassis_update(&chassis);
|
||
Chassis_Control(&chassis, &cmd_chassis,tick);
|
||
}else
|
||
{
|
||
Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
|
||
Chassis_Control(&chassis, &safe_cmd,tick);
|
||
}
|
||
|
||
// float P3=0,P6=0;
|
||
// for(int i=0;i<4;i++)
|
||
// {
|
||
// S6020[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&chassis.param->motor_6020_param[i])->motor));
|
||
// I6020[i] = MOTOR_GetTorqueCurrent(&(MOTOR_RM_GetMotor(&chassis.param->motor_6020_param[i])->motor));
|
||
// S3508[i] = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&chassis.param->motor_3508_param[i])->motor));
|
||
// I3508[i] = MOTOR_GetTorqueCurrent(&(MOTOR_RM_GetMotor(&chassis.param->motor_3508_param[i])->motor));
|
||
// P3508[i]=fabsf(S3508[i]*I3508[i]);
|
||
// P6020[i]=fabsf(S6020[i]*I6020[i]);
|
||
// P3+=P3508[i];
|
||
// P6+=P6020[i];
|
||
// }
|
||
// temp_for_cap[0] = P6*0.000012; // 计算6020总功率 P6
|
||
// temp_for_cap[1] = P3*0.0000048;
|
||
// float P_3508 = 0; // 计算3508总功率 P3
|
||
// for(int i=0;i<4;i++)
|
||
// {
|
||
// P_3508+=chassis.out.rotor3508_out[i]*S3508[i]*0.0000048;
|
||
// }
|
||
|
||
|
||
// if(CAN_SuperCapRXData.SuperCapReady == 1)
|
||
// {
|
||
// if(CAN_SuperCapRXData.SuperCapEnergy<=10)
|
||
// power_limit_3508 = 100;
|
||
// else
|
||
// power_limit_3508 = 320;
|
||
|
||
// if(CAN_SuperCapRXData.ChassisPower>power_limit_3508&&power_limit_3508>=0)
|
||
// {
|
||
// for(int i=0;i<4;i++)
|
||
// {
|
||
// chassis.out.rotor3508_out[i]*=(power_limit_3508)/
|
||
// CAN_SuperCapRXData.ChassisPower>power_limit_3508; // 如果总功率超过限制，按比例降低3508的输出
|
||
// }
|
||
|
||
// }
|
||
// }
|
||
// else
|
||
// {
|
||
// //启动老方案进行限制
|
||
// PowerLimit_ChassicOutput(power_limit_3508,chassis.out.rotor3508_out,
|
||
// chassis.motorfeedback.rotor_rpm3508, 4); // 根据功率限制调整3508输出
|
||
|
||
// }
|
||
|
||
|
||
Chassis_Setoutput(&chassis);
|
||
/* USER CODE END */
|
||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||
}
|
||
|
||
} |