diff --git a/User/device/supercap.h b/User/device/supercap.h index 2641d3b..652b6d3 100644 --- a/User/device/supercap.h +++ b/User/device/supercap.h @@ -8,10 +8,10 @@ extern "C" { #include "device\device.h" //#include "referee.h" -#define SUPERCAP_CAN BSP_FDCAN_3 +#define SUPERCAP_CAN BSP_CAN_2 -#define SUPERCAP_TX_ID 0x001 //C板发给超级电容的ID -#define SUPERCAP_RX_ID 0x100 //超级电容发给C板的ID +#define SUPERCAP_TX_ID 0x30F //C板发给超级电容的ID +#define SUPERCAP_RX_ID 0x300 //超级电容发给C板的ID //超级电容的状态标志位，超级电容运行或者保护的具体状态反馈 diff --git a/User/module/chassis.c b/User/module/chassis.c index 809c317..7ff1377 100644 --- a/User/module/chassis.c +++ b/User/module/chassis.c @@ -488,10 +488,10 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now) // } return CHASSIS_OK; } - /*电机输出设定和发送*/ void Chassis_Setoutput(Chassis_t *c) { + for (int i = 0; i < 4; i++) { MOTOR_RM_SetOutput(&(c->param->motor_3508_param[i]), c->out.rotor3508_out[i]); diff --git a/User/task/chassis.c b/User/task/chassis.c index ca321f0..64973f5 100644 --- a/User/task/chassis.c +++ b/User/task/chassis.c @@ -9,6 +9,7 @@ #include "module/chassis.h" #include "module/config.h" #include "device/ai.h" +#include "device/supercap.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -19,6 +20,8 @@ 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=300; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -66,6 +69,47 @@ osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0); 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; + } + else + { + //启动老方案进行限制 + } + 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的输出 + } + + } + Chassis_Setoutput(&chassis); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ diff --git a/User/task/config.yaml b/User/task/config.yaml index a9b6e52..de7e2dd 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -64,7 +64,7 @@ - delay: 0 description: '' freq_control: true - frequency: 500.0 + frequency: 10.0 function: Task_super_cap name: super_cap stack: 256 diff --git a/User/task/referee.c b/User/task/referee.c index 1580224..8b3e41f 100644 --- a/User/task/referee.c +++ b/User/task/referee.c @@ -25,9 +25,6 @@ int hhh; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ -/* USER PRIVATE CODE BEGIN */ - -/* USER PRIVATE CODE END */ /* Exported functions ------------------------------------------------------- */ void Task_referee(void *argument) { (void)argument; /* 未使用argument，消除警告 */ diff --git a/User/task/super_cap.c b/User/task/super_cap.c index 64505f3..275b991 100644 --- a/User/task/super_cap.c +++ b/User/task/super_cap.c @@ -23,9 +23,6 @@ Referee_CapUI_t cap_ui; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ -/* USER PRIVATE CODE BEGIN */ - -/* USER PRIVATE CODE END */ /* Exported functions ------------------------------------------------------- */ void Task_super_cap(void *argument) { (void)argument; /* 未使用argument，消除警告 */ @@ -38,19 +35,19 @@ void Task_super_cap(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - // SuperCap_Init(); - // SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能 - // SuperCap_CanTX.Charge = 0 ; //此标志位无效，超电的充放电是自动的 - // SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制 - // SuperCap_CanTX.ChargePower = 1 ; //此参数无效，超电的充电功率随着底盘功率变化 + SuperCap_Init(); + SuperCap_CanTX.Enable = 1 ; //超级电容使能。1使能，0失能 + SuperCap_CanTX.Charge = 0 ; //此标志位无效，超电的充放电是自动的 + SuperCap_CanTX.Powerlimit = 120 ; //裁判系统功率限制 + SuperCap_CanTX.ChargePower = 1 ; //此参数无效，超电的充电功率随着底盘功率变化 /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ // osMessageQueueGet(task_runtime.msgq.referee.cap, &SuperCap_CanTX.Powerlimit , NULL, 0); - // SuperCap_Update(); - // CAN_TX_SuperCapData(&SuperCap_CanTX); + SuperCap_Update(); + CAN_TX_SuperCapData(&SuperCap_CanTX); /* 锁住RTOS内核防止控制过程中断，造成错误 */ // osKernelLock(); /* 根据裁判系统数据计算输出功率 */ diff --git a/User/task/user_task.c b/User/task/user_task.c index 55ddce4..a0111e8 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -17,7 +17,7 @@ const osThreadAttr_t attr_atti_esti = { const osThreadAttr_t attr_rc = { .name = "rc", .priority = osPriorityNormal, - .stack_size = 512 * 4, + .stack_size = 256 * 4, }; const osThreadAttr_t attr_chassis = { .name = "chassis", @@ -27,7 +27,7 @@ const osThreadAttr_t attr_chassis = { const osThreadAttr_t attr_cmd = { .name = "cmd", .priority = osPriorityNormal, - .stack_size = 512 * 4, + .stack_size = 256 * 4, }; const osThreadAttr_t attr_gimbal = { .name = "gimbal", diff --git a/User/task/user_task.h b/User/task/user_task.h index 4fdbb11..4495933 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -22,7 +22,7 @@ extern "C" { #define AI_FREQ (250.0) #define REFEREE_FREQ (500.0) #define TASK9_FREQ (500.0) -#define SUPER_CAP_FREQ (500.0) +#define SUPER_CAP_FREQ (10.0) /* 任务初始化延时ms */ #define TASK_INIT_DELAY (100u)