3333

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_TX_ID 0x30F  //C板发给超级电容的ID
-#define SUPERCAP_RX_ID 0x100  //超级电容发给C板的ID
+#define SUPERCAP_RX_ID 0x300  //超级电容发给C板的ID
 
 
 //超级电容的状态标志位，超级电容运行或者保护的具体状态反馈

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]);

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); /* 运行结束，等待下一次唤醒 */

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

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，消除警告 */

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_Init();
-	// SuperCap_CanTX.Enable = 1 ;              //超级电容使能。1使能，0失能
+	SuperCap_CanTX.Enable = 1 ;              //超级电容使能。1使能，0失能
-	// SuperCap_CanTX.Charge = 0 ;              //此标志位无效，超电的充放电是自动的
+	SuperCap_CanTX.Charge = 0 ;              //此标志位无效，超电的充放电是自动的
-	// SuperCap_CanTX.Powerlimit = 120 ;        //裁判系统功率限制
+	SuperCap_CanTX.Powerlimit = 120 ;        //裁判系统功率限制
-	// SuperCap_CanTX.ChargePower = 1 ;         //此参数无效，超电的充电功率随着底盘功率变化
+	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();
+      SuperCap_Update();
-      // CAN_TX_SuperCapData(&SuperCap_CanTX);
+      CAN_TX_SuperCapData(&SuperCap_CanTX);
 		 /* 锁住RTOS内核防止控制过程中断，造成错误 */
 			// osKernelLock();
       /* 根据裁判系统数据计算输出功率 */

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",

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)