# RMCOD2026_Sentry

**Repository Path**: cod_-control/rmcod2026_-sentry

## Basic Information

- **Project Name**: RMCOD2026_Sentry
- **Description**: COD26 赛季哨兵电控代码
- **Primary Language**: C
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 8
- **Forks**: 0
- **Created**: 2025-07-17
- **Last Updated**: 2026-04-04

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

#  RMCOD2026_Sentry

[TOC]

## 介绍

26赛季COD舵轮哨兵代码
---



## 环境依赖

- 开发工具：Keil V5.38a，VsCode
- 软件环境：Window11
- 硬件环境：达妙H7(STM32H723VGT6)
- 编译工具：Arm Compiler V6.22，C/C++编译

---



## 使用说明

#### 底盘控制

-   底盘控制任务主要放置在 [Control_Task.c](D:\CODsoftware\Infantry_2026\Infantry_333\chassis\COD_H7_Template\Task\Src\Control_Task.c) 
-   底盘和云台通信主要放置在 [CAN_Task.c](D:\CODsoftware\Infantry_2026\Infantry_333\chassis\COD_H7_Template\Task\Src\CAN_Task.c) 和 [bsp_can.c](D:\CODsoftware\Infantry_2026\Infantry_333\chassis\COD_H7_Template\BSP\Src\bsp_can.c) 

#### 云台控制

-   云台控制任务主要放置在  [Control_Task.c](D:\CODsoftware\Infantry_2026\Infantry_333\gimbal\Task\Src\Control_Task.c) 和 [Shoot.c](D:\CODsoftware\Infantry_2026\Infantry_333\gimbal\Task\Src\Shoot.c)  
-   云台和底盘通信主要放置在  [CAN_Task.c](D:\CODsoftware\Infantry_2026\Infantry_333\gimbal\Task\Src\CAN_Task.c) 和 [bsp_can.c](D:\CODsoftware\Infantry_2026\Infantry_333\gimbal\BSP\Src\bsp_can.c) 
-   云台和上位机通信主要放置在 [MiniPC_Task.c](D:\CODsoftware\Infantry_2026\Infantry_333\gimbal\Task\Src\MiniPC_Task.c) 和 [MiniPC.c](D:\CODsoftware\Infantry_2026\Infantry_333\gimbal\Components\Device\Src\MiniPC.c)  

---



## Chassis重要文件

#### 舵轮解算

```c#
	// 舵向轮解算  
	Chassis_Info->Target.Chassis_Angle[0] = -(atan2f((Control_Info.Move.Vx + Control_Info.Move.Vw * sin45 * R), (Control_Info.Move.Vy + Control_Info.Move.Vw * sin45 * R)) * RadiansToDegrees);
	Chassis_Info->Target.Chassis_Angle[1] = -(atan2f((Control_Info.Move.Vx + Control_Info.Move.Vw * sin45 * R), (Control_Info.Move.Vy - Control_Info.Move.Vw * sin45 * R)) * RadiansToDegrees);
	Chassis_Info->Target.Chassis_Angle[2] = -(atan2f((Control_Info.Move.Vx - Control_Info.Move.Vw * sin45 * R), (Control_Info.Move.Vy + Control_Info.Move.Vw * sin45 * R)) * RadiansToDegrees);
	Chassis_Info->Target.Chassis_Angle[3] = -(atan2f((Control_Info.Move.Vx - Control_Info.Move.Vw * sin45 * R), (Control_Info.Move.Vy - Control_Info.Move.Vw * sin45 * R)) * RadiansToDegrees);
		
	Chassis_Info->Measure.Absolute_Angle[0] = (Chassis_Info->trigger.Chassis_Angle_average[2][0] - Motor_Course[0].Data.Angle + Chassis_Info->Target.Chassis_Angle[0]);
	Chassis_Info->Measure.Absolute_Angle[1] = (Chassis_Info->trigger.Chassis_Angle_average[2][1] - Motor_Course[1].Data.Angle + Chassis_Info->Target.Chassis_Angle[1]);
	Chassis_Info->Measure.Absolute_Angle[2] = (Chassis_Info->trigger.Chassis_Angle_average[2][2] - Motor_Course[2].Data.Angle + Chassis_Info->Target.Chassis_Angle[2]);
	Chassis_Info->Measure.Absolute_Angle[3] = (Chassis_Info->trigger.Chassis_Angle_average[2][3] - Motor_Course[3].Data.Angle + Chassis_Info->Target.Chassis_Angle[3]);

	for (i = 0; i < 4; i++){
		// 限制舵向角度在+-180之间
	if(Chassis_Info->Measure.Absolute_Angle[i]>=180)  Chassis_Info->Measure.Absolute_Angle[i]-=360;   
  else if(Chassis_Info->Measure.Absolute_Angle[i]<=-180) Chassis_Info->Measure.Absolute_Angle[i]+=360;
		
		if (fabsf(Chassis_Info->Measure.Absolute_Angle[i]) < 90.f)
			Chassis_Info->trigger.K[i] = 1.0f;
		//优劣弧判断
		else if (fabsf(Chassis_Info->Measure.Absolute_Angle[i]) >= 90.f){
			if (Chassis_Info->Measure.Absolute_Angle[i] >= 90){
				Chassis_Info->Measure.Absolute_Angle[i] -= 180.0f;
				Chassis_Info->trigger.K[i] = -1.0f;
			}
			else 
				 if (Chassis_Info->Measure.Absolute_Angle[i] <= -90){
				Chassis_Info->Measure.Absolute_Angle[i] += 180.0f;
				Chassis_Info->trigger.K[i] = -1.0f;
			}
		}
	}
	// 行进轮解算
	Chassis_Info->Target.Chassis_Velocity[0] = -Chassis_Info->trigger.K[0]* (Q_sqrt(powf(Control_Info.Move.Vx + Control_Info.Move.Vw * sin45, 2.f) + powf(Control_Info.Move.Vy + Control_Info.Move.Vw * sin45, 2.f)));
	Chassis_Info->Target.Chassis_Velocity[1] = -Chassis_Info->trigger.K[1]* (Q_sqrt(powf(Control_Info.Move.Vx - Control_Info.Move.Vw * sin45, 2.f) + powf(Control_Info.Move.Vy - Control_Info.Move.Vw * sin45, 2.f)));
	Chassis_Info->Target.Chassis_Velocity[2] = Chassis_Info->trigger.K[2] * (Q_sqrt(powf(Control_Info.Move.Vx - Control_Info.Move.Vw * sin45, 2.f) + powf(Control_Info.Move.Vy + Control_Info.Move.Vw * sin45, 2.f)));
	Chassis_Info->Target.Chassis_Velocity[3] = Chassis_Info->trigger.K[3] * (Q_sqrt(powf(Control_Info.Move.Vx + Control_Info.Move.Vw * sin45, 2.f) + powf(Control_Info.Move.Vy - Control_Info.Move.Vw * sin45, 2.f)));
}
```

---



### 3508角度轮校准

  <mark>3508舵轮使用接近开关进行校准</mark>  
- 洛施达微小型方形接近开关传感器82.5NOF/82.5NOT可替代GX-F8AH8B

```c#
 Chassis_Cal(Chassis_Info_Typedef *Chassis_Info)
     
     
    PowerCtrl_Info[0].K3 = 3;
    PowerCtrl_Info[0].Err_Lower = 0.01;
    PowerCtrl_Info[0].Err_Upper = 50;
    PowerCtrl_Info[1].K3 = 3.5;
    PowerCtrl_Info[1].Err_Lower = 0.001;
    PowerCtrl_Info[1].Err_Upper = 500;	
	RLS_Init(&RLS_Power_Info, 4, 1, 0.99999, 1e-5);
    RLS_Power_Info.Data.W[0] = 1.453e-07;	
    RLS_Power_Info.Data.W[1] = 1.23e-07; 
    RLS_Power_Info.Data.W[2] =  1.453e-07;	   
    RLS_Power_Info.Data.W[3] = 1.23e-07;
	
	start = 0;
 
//接近开关高低电平获取
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    /* Prevent unused argument(s) compilation warning */
    UNUSED(GPIO_Pin);
    if (GPIO_Pin == GPIO_PIN_13)
      	Chassis_Info.trigger.now_state[0] = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_13); // 13  
    if (GPIO_Pin == GPIO_PIN_9)
        Chassis_Info.trigger.now_state[1] = HAL_GPIO_ReadPin(GPIOE, GPIO_PIN_9);  // 9
    if (GPIO_Pin == GPIO_PIN_2)
        Chassis_Info.trigger.now_state[2] = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_2);  // 0
    if (GPIO_Pin == GPIO_PIN_0)
       	Chassis_Info.trigger.now_state[3] = HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0);  // 2

    /* NOTE: This function Should not be modified, when the callback is needed,
             the HAL_GPIO_EXTI_Callback could be implemented in the user file
     */
}
 
 static void Chassis_Cal(Chassis_Info_Typedef *Chassis_Info){
	if (Chassis_Info->trigger.now_state[0] == 0 && Chassis_Info->trigger.last_state[0] == 1){
		Chassis_Info->trigger.Grab[0][0] += 1;
		Chassis_Info->trigger.Chassis_Angle_now[0] = Motor_Course[0].Data.Angle;
	}
	if (Chassis_Info->trigger.now_state[1] == 0 && Chassis_Info->trigger.last_state[1] == 1){
		Chassis_Info->trigger.Grab[0][1] += 1;
		Chassis_Info->trigger.Chassis_Angle_now[1] = Motor_Course[1].Data.Angle;
	}

	if (Chassis_Info->trigger.now_state[2] == 0 && Chassis_Info->trigger.last_state[2] == 1){
		Chassis_Info->trigger.Grab[0][2] += 1;
		Chassis_Info->trigger.Chassis_Angle_now[2] = Motor_Course[2].Data.Angle;
	}

	
	if (Chassis_Info->trigger.now_state[3] == 0 && Chassis_Info->trigger.last_state[3] == 1){
		Chassis_Info->trigger.Grab[0][3] += 1;
		Chassis_Info->trigger.Chassis_Angle_now[3] = Motor_Course[3].Data.Angle;
	}
	
	//**********************************//
	//**********************************//
	if (Chassis_Info->trigger.now_state[0] == 1 && Chassis_Info->trigger.last_state[0] == 0){
		Chassis_Info->trigger.Grab[1][0] += 1;
		Chassis_Info->trigger.Chassis_Angle_last[0] = Motor_Course[0].Data.Angle;
		Chassis_Info->trigger.Chassis_Angle_average[0][0] = (Chassis_Info->trigger.Chassis_Angle_now[0] + Chassis_Info->trigger.Chassis_Angle_last[0]) * 0.5;
	}
	if (Chassis_Info->trigger.now_state[1] == 1 && Chassis_Info->trigger.last_state[1] == 0){
		Chassis_Info->trigger.Grab[1][1] += 1;
		Chassis_Info->trigger.Chassis_Angle_last[1] = Motor_Course[1].Data.Angle;
		Chassis_Info->trigger.Chassis_Angle_average[0][1] = (Chassis_Info->trigger.Chassis_Angle_now[1] + Chassis_Info->trigger.Chassis_Angle_last[1]) * 0.5;
	}
	if (Chassis_Info->trigger.now_state[2] == 1 && Chassis_Info->trigger.last_state[2] == 0){
		Chassis_Info->trigger.Grab[1][2] += 1;
		Chassis_Info->trigger.Chassis_Angle_last[2] = Motor_Course[2].Data.Angle;
		Chassis_Info->trigger.Chassis_Angle_average[0][2] = (Chassis_Info->trigger.Chassis_Angle_now[2] + Chassis_Info->trigger.Chassis_Angle_last[2]) * 0.5;
	}
	if (Chassis_Info->trigger.now_state[3] == 1 && Chassis_Info->trigger.last_state[3] == 0){
		Chassis_Info->trigger.Grab[1][3] += 1;
		Chassis_Info->trigger.Chassis_Angle_last[3] = Motor_Course[3].Data.Angle;
		Chassis_Info->trigger.Chassis_Angle_average[0][3] = (Chassis_Info->trigger.Chassis_Angle_now[3] + Chassis_Info->trigger.Chassis_Angle_last[3]) * 0.5;
	}
	
	for (i = 0; i < 4; i++){
		Chassis_Info->trigger.Error[i] = Chassis_Info->trigger.Chassis_Angle_average[2][i] - Motor_Course[i].Data.Angle;
		if ((Chassis_Info->trigger.Grab[0][i] > 1) && (Chassis_Info->trigger.Grab[1][i] > 1)){
			Chassis_Info->trigger.Chassis_Angle_average[1][i] = (Chassis_Info->trigger.Chassis_Angle_now[i] + Chassis_Info->trigger.Chassis_Angle_last[i]) * 0.5;
			if (Chassis_Info->trigger.need_trigger == 1){
				Chassis_Info->trigger.Chassis_Angle_average[2][i] = ((Chassis_Info->trigger.Chassis_Angle_average[0][i] + Chassis_Info->trigger.Chassis_Angle_average[1][i]) * 0.5) - 3.0f;
			}
			PID_Calculate(&PID_Chassis_Cal[i], Chassis_Info->trigger.Chassis_Angle_average[2][i], Motor_Course[i].Data.Angle);
			PID_Calculate(&PID_Chassis_Cal_V[i], PID_Chassis_Cal[i].Output, Motor_Course[i].Data.Velocity);
			Chassis_Info->trigger.Calibrate_State[i] = 1;
		}
       
		else{
			if (Chassis_Info->trigger.Grab[1][i] < 1){
				Chassis_Info->trigger.Chassis_Angle[i] = 100;
				PID_Calculate(&PID_Chassis_Cal[i], Chassis_Info->trigger.Chassis_Angle[i], 0);
				PID_Calculate(&PID_Chassis_Cal_V[i], PID_Chassis_Cal[i].Output, Motor_Course[i].Data.Velocity);
			}
			if ((Chassis_Info->trigger.Grab[1][i] == 1)){
				Chassis_Info->trigger.Chassis_Angle[i] = -90;
				PID_Calculate(&PID_Chassis_Cal[i], Chassis_Info->trigger.Chassis_Angle[i], 0);
				PID_Calculate(&PID_Chassis_Cal_V[i], PID_Chassis_Cal[i].Output, Motor_Course[i].Data.Velocity);
			}
		}
	}
}
```

---



### 功率控制

参考了西交的功率控制模型，具体可参考[【RM2023-电机功率模型与功率控制开源】西交利物浦大学](https://gitee.com/link?target=http%3A%2F%2Fgithub.com%2FMaxwellDemonLin%2FMotor-modeling-and-power-control)
首先我们读取电机的电流值Current与转速Velocity（rpm），将电流映射为电机的转矩，将转速转换单位为（m/s）。
通过西交的模型得出具体的预测底盘功率
部分代码如下：

```c#
PowerCtrl_Info[1].Sum.Torque2_Sum = PowerCtrl_Info[1].Target.Torque_2[0] + PowerCtrl_Info[1].Target.Torque_2[1] + PowerCtrl_Info[1].Target.Torque_2[2] + PowerCtrl_Info[1].Target.Torque_2[3];
PowerCtrl_Info[1].Sum.Omiga2_Sum = PowerCtrl_Info[1].Target.Omiga_2[0] + PowerCtrl_Info[1].Target.Omiga_2[1] + PowerCtrl_Info[1].Target.Omiga_2[2] + PowerCtrl_Info[1].Target.Omiga_2[3];
PowerCtrl_Info[1].Sum.Power_Sum = PowerCtrl_Info[1].Target.Power_In[0] + PowerCtrl_Info[1].Target.Power_In[1] + PowerCtrl_Info[1].Target.Power_In[2] + PowerCtrl_Info[1].Target.Power_In[3] + PowerCtrl_Info[1].K3;
PowerCtrl_Info[1].Power_Allin = PowerCtrl_Info[1].Sum.Power_Sum;
PowerCtrl_Info[1].Power_Max = Chassis_Info.Power_Max - fabs(PowerCtrl_Info[0].Power_Allin);


```



#####  RLS功率模型拟合:

通过将初始化的常数Torque_2、Omiga_2、Power_In求和,得Torque2_Sum、Omiga2_Sum、Power_Allin...进行功率拟合，
通过RLS_Update对计算出的功率与实际功率进行递归最小二乘拟合，将模型中的参数K1，K2...进行更新。
舵向与行进的功率分配：通过Chassis_Info.Power_Max设定底盘最大输出功率，对舵向优先分配80%的功率，使其优先提供转向，将剩余的功率分配给行进向。<mark> //RLS_Update(&RLS_Power_Info);</mark>  通过开启自整定确定参数
部分代码如下：

```c#
VAL_LIMIT(PowerCtrl_Info[1].Power_Allin, -PowerCtrl_Info[1].Power_Max, PowerCtrl_Info[1].Power_Max);
LowPassFilter1p_Update(&LPF_ChassisPower,Chassis_Info.Supercap.Chassis_Power);
RLS_Power_Info.Data.U[0] = PowerCtrl_Info[0].Power_Allin + PowerCtrl_Info[1].Power_Allin;
RLS_Power_Info.Data.Y[0] =LPF_ChassisPower.Output;
RLS_Power_Info.Data.X[0] = PowerCtrl_Info[0].Sum.Torque2_Sum;
RLS_Power_Info.Data.X[1] = PowerCtrl_Info[0].Sum.Omiga2_Sum;
RLS_Power_Info.Data.X[2] = PowerCtrl_Info[1].Sum.Torque2_Sum;
RLS_Power_Info.Data.X[3] = PowerCtrl_Info[1].Sum.Omiga2_Sum;

//RLS_Update(&RLS_Power_Info);
PowerCtrl_Info[0].K1 = RLS_Power_Info.Data.W[0];
PowerCtrl_Info[0].K2 = RLS_Power_Info.Data.W[1];
PowerCtrl_Info[1].K1 = RLS_Power_Info.Data.W[2];
PowerCtrl_Info[1].K2 = RLS_Power_Info.Data.W[3];
```



#####  功率分配：

读取PID误差，将误差累加，根据单电机PID误差的占比和预测功率的占总输出功率的占比运用分配因子，通过平滑函数将其化为一个隶属度。 部分代码如下：

```c#
	for (int i = 0; i < 4; i++)
		PowerCtrl_Info[0].Err[i] = PID_Chassis_angle_v[i].Err[0];
	// 输入PID误差
	PowerCtrl_Info[0].Sum.Err_Sum = fabsf(PowerCtrl_Info[0].Err[FL]) + fabsf(PowerCtrl_Info[0].Err[FB]) + fabsf(PowerCtrl_Info[0].Err[RB]) + fabsf(PowerCtrl_Info[0].Err[RL]);
	// 计算PID误差和
	if (PowerCtrl_Info[0].Sum.Err_Sum > PowerCtrl_Info[0].Err_Upper)
		PowerCtrl_Info[0].K = 1;
	else if (PowerCtrl_Info[0].Sum.Err_Sum < PowerCtrl_Info[0].Err_Lower)
		PowerCtrl_Info[0].K = 0;
	else
		PowerCtrl_Info[0].K = (PowerCtrl_Info[0].Sum.Err_Sum - PowerCtrl_Info[0].Err_Lower) / (PowerCtrl_Info[0].Err_Upper - PowerCtrl_Info[0].Err_Lower);

	for (int i = 0; i < 4; i++)
		PowerCtrl_Info[1].Err[i] = PID_Chassis_velocity[i].Err[0];
	// 输入PID误差
	PowerCtrl_Info[1].Sum.Err_Sum = fabsf(PowerCtrl_Info[1].Err[FL]) + fabsf(PowerCtrl_Info[1].Err[FB]) + fabsf(PowerCtrl_Info[1].Err[RB]) + fabsf(PowerCtrl_Info[1].Err[RL]);
	// 计算PID误差和
	if (PowerCtrl_Info[1].Sum.Err_Sum > PowerCtrl_Info[1].Err_Upper)
		PowerCtrl_Info[1].K = 1;
	else if (PowerCtrl_Info[0].Sum.Err_Sum < PowerCtrl_Info[1].Err_Lower)
		PowerCtrl_Info[1].K = 0;
	else
		PowerCtrl_Info[1].K = (PowerCtrl_Info[1].Sum.Err_Sum - PowerCtrl_Info[1].Err_Lower) / (PowerCtrl_Info[1].Err_Upper - PowerCtrl_Info[1].Err_Lower);
	// 计算分配因子K

	for (int i = 0; i < 4; i++){
		PowerCtrl_Info[0].Menbership[i] = (PowerCtrl_Info[0].K * (fabs(PowerCtrl_Info[0].Err[i]) / PowerCtrl_Info[0].Sum.Err_Sum) + (1 - PowerCtrl_Info[0].K) * (fabs(PowerCtrl_Info[0].Target.Power_In[i]) / PowerCtrl_Info[0].Sum.Power_Sum));
		VAL_LIMIT(PowerCtrl_Info[0].Menbership[i], 0, 1);
		PowerCtrl_Info[1].Menbership[i] = (PowerCtrl_Info[1].K * (fabs(PowerCtrl_Info[1].Err[i]) / PowerCtrl_Info[1].Sum.Err_Sum) + (1 - PowerCtrl_Info[1].K) * (fabs(PowerCtrl_Info[1].Target.Power_In[i]) / PowerCtrl_Info[1].Sum.Power_Sum));
		VAL_LIMIT(PowerCtrl_Info[1].Menbership[i], 0, 1);
		PowerCtrl_Info[0].Power_Limit[i] = PowerCtrl_Info[0].Menbership[i] * PowerCtrl_Info[0].Power_Max; // 港科大P分配
		PowerCtrl_Info[1].Power_Limit[i] = PowerCtrl_Info[1].Menbership[i] * PowerCtrl_Info[1].Power_Max; // 港科大P分配
	}
```

##### 求解受限力矩电流：

如果拟合出的功率Power_Sum大于被分配的Power_Max， 则将根据上述分配的Power_Limit进行功率限制的求解，运用求根公式，求出限制后的力矩电流值。
如若所拟合的功率Power_Sum并未超过我们所给定的Power_Max,则直接沿用PID计算所得输出即可。
部分代码如下：

```c#
	if (PowerCtrl_Info[0].Sum.Power_Sum >= PowerCtrl_Info[0].Power_Max){
		for (int i = 0; i < 4; i++){
			PowerCtrl_Info[0].A = PowerCtrl_Info[0].K1;
			PowerCtrl_Info[0].B = PowerCtrl_Info[0].Target.Omiga[i];
			PowerCtrl_Info[0].C = PowerCtrl_Info[0].Target.Omiga_2[i] * PowerCtrl_Info[0].K2 + PowerCtrl_Info[0].K3 - PowerCtrl_Info[0].Power_Limit[i];
			PowerCtrl_Info[0].Delta = powf(PowerCtrl_Info[0].B, 2.f) - 4 * PowerCtrl_Info[0].A * PowerCtrl_Info[0].C;
			if (isnan(PowerCtrl_Info[0].Delta) == 1 || isinf(PowerCtrl_Info[0].Delta) == 1 || (PowerCtrl_Info[0].Delta) < 0)
				PowerCtrl_Info[0].Delta = 0;
			if (PowerCtrl_Info[0].Delta >= 0){
				PowerCtrl_Info[0].Sqrt = sqrt(PowerCtrl_Info[0].Delta);

				if (Chassis_Info.Output.Chassis_Angel[i] >= 0){
					PowerCtrl_Info[0].Torque[i] = (-PowerCtrl_Info[0].B + PowerCtrl_Info[0].Sqrt) / (2 * PowerCtrl_Info[0].A);
					Chassis_Info.Output.Chassis_Angel[i] = (PowerCtrl_Info[0].Torque[i] / 4.577e-5);
				}
				else{
					PowerCtrl_Info[0].Torque[i] = (-PowerCtrl_Info[0].B + PowerCtrl_Info[0].Sqrt) / (2 * PowerCtrl_Info[0].A);
					Chassis_Info.Output.Chassis_Angel[i] = (PowerCtrl_Info[0].Torque[i] / 4.577e-5);
				}
			}
			else{
				if (Chassis_Info.Output.Chassis_Angel[i] >= 0){
					PowerCtrl_Info[0].Torque[i] = (-PowerCtrl_Info[0].B) / (2 * PowerCtrl_Info[0].A);
					Chassis_Info.Output.Chassis_Angel[i] = (PowerCtrl_Info[0].Torque[i] / 4.577e-5);
				}
				else{
					PowerCtrl_Info[0].Torque[i] = (-PowerCtrl_Info[0].B) / (2 * PowerCtrl_Info[0].A);
					Chassis_Info.Output.Chassis_Angel[i] =(PowerCtrl_Info[0].Torque[i] / 4.577e-5);
				}
			}
		}
	}
	else{
		for (int i = 0; i < 4; i++)
			Chassis_Info.Output.Chassis_Angel[i] = PID_Chassis_angle_v[i].Output;
	}

		if (PowerCtrl_Info[1].Sum.Power_Sum >= PowerCtrl_Info[1].Power_Max){
			for (i = 0; i < 4; i++){
				PowerCtrl_Info[1].A = PowerCtrl_Info[1].K1;
				PowerCtrl_Info[1].B = PowerCtrl_Info[1].Target.Omiga[i];
				PowerCtrl_Info[1].C = PowerCtrl_Info[1].Target.Omiga_2[i] * PowerCtrl_Info[1].K2 + PowerCtrl_Info[1].K3 - PowerCtrl_Info[1].Power_Limit[i];
				PowerCtrl_Info[1].Delta = powf(PowerCtrl_Info[1].B, 2.f) - 4 * PowerCtrl_Info[1].A * PowerCtrl_Info[1].C;

				if (isnan(PowerCtrl_Info[1].Delta) == 1 || isinf(PowerCtrl_Info[1].Delta) == 1 || (PowerCtrl_Info[1].Delta) < 0)
					PowerCtrl_Info[1].Delta = 0;
				if (PowerCtrl_Info[1].Delta >= 0)
				{
					PowerCtrl_Info[1].Sqrt =Q_sqrt(PowerCtrl_Info[1].Delta);
					if (Chassis_Info.Output.Chassis_Velocity[i] >= 0)
					{
						PowerCtrl_Info[1].Torque[i] = (-PowerCtrl_Info[1].B + PowerCtrl_Info[1].Sqrt) / (2 * PowerCtrl_Info[1].A);
						Chassis_Info.Output.Chassis_Velocity[i] = (PowerCtrl_Info[1].Torque[i] / 4.577e-5);//
					}
					else
					{
						PowerCtrl_Info[1].Torque[i] = (-PowerCtrl_Info[1].B - PowerCtrl_Info[1].Sqrt) / (2 * PowerCtrl_Info[1].A);
						Chassis_Info.Output.Chassis_Velocity[i] =(PowerCtrl_Info[1].Torque[i] / 4.577e-5);//PowerCtrl_Info[1].Torque[i];
					}
				}
				else
				{
					if (Chassis_Info.Output.Chassis_Velocity[i] >= 0)
					{
						PowerCtrl_Info[1].Torque[i] = (-PowerCtrl_Info[1].B) / (2 * PowerCtrl_Info[1].A);
						Chassis_Info.Output.Chassis_Velocity[i] =(PowerCtrl_Info[1].Torque[i] / 4.577e-5);// 
					}
					else
					{
						PowerCtrl_Info[1].Torque[i] = (-PowerCtrl_Info[1].B) / (2 * PowerCtrl_Info[1].A);
						Chassis_Info.Output.Chassis_Velocity[i] =(PowerCtrl_Info[1].Torque[i] / 4.577e-5);
					}
				}
			}
		}
		else
			for (int i = 0; i < 4; i++)
			{
				Chassis_Info.Output.Chassis_Velocity[i] = PID_Chassis_velocity[i].Output;
			}
```



---

#### 导航模式

-   Chassis_Auto 为离线导航

```c#
        case Auto:
        case Chassis_Auto: {
float _Vy;
float _Vx;					
float Area_Status = (Referee_System_Info.rfid_status.rfid_status>>23)&0x01;					
			// 自动模式
		if ((MiniPc_Receive_Auto.Fire == 1)&&(MiniPc_Receive_Auto.Vision_Grap == 1))  Chassis_Info->Target.Shoot_Speed = Shoot_Info.Shoot.Shoot_DP * 540.0f;
    else   Chassis_Info->Target.Shoot_Speed = 0;  
						
 if((Referee_System_Info.robot_status.current_HP>280)&&(MiniPc_Receive_Auto.Vision_Grap==1)&&(Area_Status==1) &&(MiniPc_Receive_Auto.Fire_Switch_Spin==1)){
				_Vy =5;
				_Vx =5;	
			}	
else{ 
			 _Vy = -MiniPc_Receive_Auto.Vy *  2233.866;
			 _Vx = MiniPc_Receive_Auto.Vx * 2233.866;
}		  				
				
	Control_Info.Move.Vx = (-arm_sin_f32(Control_Info.Angle_gap * DegreesToRadians) * _Vx + arm_cos_f32(Control_Info.Angle_gap * DegreesToRadians) * _Vy);
	Control_Info.Move.Vy = arm_cos_f32(Control_Info.Angle_gap * DegreesToRadians) * _Vx + arm_sin_f32(Control_Info.Angle_gap * DegreesToRadians) * _Vy;

			float Auto_Vx = fabs(_Vx);
			float Auto_Vy = fabs(_Vy);
    static unsigned int Vw_Time =0;
static unsigned char is_odd_second = 0; 
			// Vw平滑处理
			static float target_vw;
			if ((Auto_Vx < 35) && (Auto_Vy < 35)) {		
 if(++Vw_Time >= 1200) {
    Vw_Time = 0;
    static int cycle_count = 0;
    cycle_count = (cycle_count + 1) % 3;
    if(cycle_count == 0) target_vw = 6500.0f;
    else if(cycle_count == 1) target_vw = 4000.0f;
    else target_vw =2000.0f;
}
}
			
			else 	target_vw = 6000.0f; //3000.f;		
						
			Control_Info.Move.Vw = target_vw;
			if ((MiniPc_Receive_Auto.Auto_to_Vision == 1) && (MiniPc_Receive_Auto.Vision_Grap == 1))
				Chassis_Info->Target.Vision_Yaw_Angle = MiniPc_Receive_Auto.Vision_Yaw;
			else		
			    Chassis_Info->Target.Vision_Yaw_Angle = Gimbal.Yaw_Angle;			
			break;
		}			
```

---

#### 哨兵自动开关超电

-   因为防止在超级电容关闭不及时导致的超功率，所以把阈值给的较高

```c#
//超电
void Control_SuperCap_Mode_Update(Control_Info_Typedef *Control_Info)        //当开启超电时底盘功率上限设置
{
			if(Chassis_Info.Supercap.Persent >= 30){
		Chassis_Info.Supercap.Power_Max  =Referee_System_Info.robot_status.chassis_power_limit-PID_Buffer.Output+100;
				Chassis_Info.Supercap.Mode_State=1;
		 }
			else{
					Chassis_Info.Supercap.Power_Max = Referee_System_Info.robot_status.chassis_power_limit-PID_Buffer.Output;
				Chassis_Info.Supercap.Mode_State=1;
			}
			
			Chassis_Info.Power_Max= Chassis_Info.Supercap.Power_Max;
}
```

----

#### 热量控制

-    如果使用中科视频中的虚拟枪口进行控制效果更佳（实力不足，在发射数量较多是会产生误差）

```c#
//热量控制
static void Fire_Ctrl(){
//剩余热量
	Shoot_Info.HeatCtrl.SYS_Qres = Referee_System_Info.robot_status.shooter_barrel_heat_limit - Referee_System_Info.power_heat_data.shooter_17mm_1_barrel_heat;
	
//	if( Shoot_Info.HeatCtrl.Last_SYS_Qrse != Shoot_Info.HeatCtrl.SYS_Qres){	 //虚拟枪口
//Shoot_Info.HeatCtrl.SYS_Qres = Referee_System_Info.robot_status.shooter_barrel_heat_limit - Referee_System_Info.power_heat_data.shooter_17mm_1_barrel_heat ;
//	}
//	else{
//				Shoot_Info.HeatCtrl.SYS_Qres =Shoot_Info.HeatCtrl.Qres;
//	}
//		
if(Shoot_Info.HeatCtrl.SYS_Qres > 100){
	Shoot_Info.Shoot.Shoot_DP = 19.0f;  //正常拨弹盘速度	
	}
else if((Shoot_Info.HeatCtrl.SYS_Qres > 20) &&((Shoot_Info.HeatCtrl.SYS_Qres <100))){ 
float a = (float)(Referee_System_Info.robot_status.shooter_barrel_cooling_value);
float m = (float)Shoot_Info.HeatCtrl.SYS_Qres;
float d = Shoot_Info.HeatCtrl.BulletHeat17;
	   if (Shoot_Info.HeatCtrl.ShootCount == 0){
     Shoot_Info.HeatCtrl.ShootTime = (m + 2 * a) * 10;  //正常射击时间
        VAL_LIMIT(Shoot_Info.HeatCtrl.ShootTime, 100, 5600);
        if (m < 50){
         Shoot_Info.HeatCtrl.Shoot_Speed = (d * m - a - 3 * d) / (d * (Shoot_Info.HeatCtrl.ShootTime / 100.0f)) + a / d;
        }
        else{
         Shoot_Info.HeatCtrl.Shoot_Speed = (d * m - a - 7 * d) / (d * (Shoot_Info.HeatCtrl.ShootTime / 100.0f)) + a / d;
        }
    }
    else if (0 < Shoot_Info.HeatCtrl.ShootCount && Shoot_Info.HeatCtrl.ShootCount < Shoot_Info.HeatCtrl.ShootTime){
     Shoot_Info.Shoot.Shoot_DP = Shoot_Info.HeatCtrl.Shoot_Speed ;
        VAL_LIMIT(Shoot_Info.Shoot.Shoot_DP, 0.0f, 20.0f);
    }
    else{
        Shoot_Info.Shoot.Shoot_DP = (a / d);
        if (Shoot_Info.Shoot.Shoot_DP < 1.f)
            Shoot_Info.Shoot.Shoot_DP = 0;
        VAL_LIMIT(Shoot_Info.Shoot.Shoot_DP, 0.0f, 20.0f);
    }
    if (Shoot_Info.HeatCtrl.ShootCount < Shoot_Info.HeatCtrl.ShootTime){
        Shoot_Info.HeatCtrl.ShootCount++;
    }
    Shoot_Info.HeatCtrl.Last_Shoot_time = Shoot_Info.HeatCtrl.ShootTime;
    if (m>= 40){
        if (Shoot_Info.HeatCtrl.ShootCount >= Shoot_Info.HeatCtrl.ShootTime){
            Shoot_Info.HeatCtrl.ShootCount = 0;
        }
    }
    else if (m<= 32){
        Shoot_Info.HeatCtrl.ShootCount = Shoot_Info.HeatCtrl.Last_Shoot_time;
    }

}
else if (Shoot_Info.HeatCtrl.SYS_Qres <= 30){
	Shoot_Info.Shoot.Shoot_DP = 0.0f;  //停止拨弹盘
}
Shoot_Info.HeatCtrl.Last_SYS_Qrse  = Shoot_Info.HeatCtrl.SYS_Qres;
}
```

---



## Gimbal重要文件

#### 射速控制

-   VAL_LIMIT((Control_Info->Gimbal.Target.Shoot_Speed),6180,6400); 这个需要根据当地当时的环境进行略微调整

```c#
  //保持弹频1
if(systick%121==0){
if (Shoot_Info.Shoot.initial_speed!= Control_Info->Gimbal.Target.Shoot_Speed_last){   //Shoot_Info.Shoot.initial_speed =21.8
Shoot_Info.Shoot.Fire_Trigger += SpeedAdapt(Shoot_Info.Shoot.initial_speed, 22.8,23.8,6.0,9.0);
Control_Info->Gimbal.Target.Shoot_Speed += Shoot_Info.Shoot.Fire_Trigger;
}
else{
Shoot_Info.Shoot.Fire_Trigger  = 0;
}
 Control_Info->Gimbal.Target.Shoot_Speed_last= Shoot_Info.Shoot.initial_speed;
}
  //限制摩擦轮转速
	VAL_LIMIT((Control_Info->Gimbal.Target.Shoot_Speed),6180,6400);
	Control_Info->Gimbal.Target.Left_Shoot=  -Control_Info->Gimbal.Target.Shoot_Speed;
  Control_Info->Gimbal.Target.Right_Shoot= Control_Info->Gimbal.Target.Shoot_Speed;
 
}
```

---



#### 自瞄下位机火控

```c#
static void Control_Fire(Control_Info_Typedef *Control_Info){
	
	if(MiniPc_Rx_Vision.Vision_Grap==1)
MiniPc_Rx_Vision.Fire_Switch = IsOnTarget(INS_Info.Yaw_Angle*DegreesToRadians,-INS_Info.Roll_Angle*DegreesToRadians,MiniPc_Rx_Vision.Fire_Yaw*DegreesToRadians, MiniPc_Rx_Vision.Fire_Pitch*DegreesToRadians,MiniPc_Rx_Vision.Distance);
  else MiniPc_Rx_Vision.Fire_Switch=0;
		
}

```


```c#
static bool IsOnTarget(const double cur_yaw,const double cur_pitch,const double target_yaw,const double target_pitch,const double distance) {                         
  // Judge whether to shoot
   shooting_range_yaw = fabs(atan2(0.185 / 2, distance));  //0.135
   shooting_range_pitch = fabs(atan2(3.0 / 2, distance));    //3

   pitch_ok =  fabs(cur_pitch - target_pitch) < shooting_range_pitch;
   yaw_ok = fabs(cur_yaw - target_yaw) < shooting_range_yaw;

  if (yaw_ok ) {
       
    return true;
  }
	  return false;
}

```

## 参考开源

1. [2025RoboMaster辽宁科技大学COD战队电控通用控制系统(达妙MC02 STM32H723VGT6)](https://gitee.com/wangcaofan/cod-h7-template)
2. [【RM2024-功率控制算法开源（含舵轮，轮腿，全自动调参）】香港科技大学ENTERPRIZE战队](https://gitee.com/link?target=http%3A%2F%2Fgithub.com%2Fhkustenterprize%2FRM2024-PowerModule%3Ftab%3Dreadme-ov-file)
3. [【RM2023-电机功率模型与功率控制开源】西交利物浦大学](https://gitee.com/link?target=http%3A%2F%2Fgithub.com%2FMaxwellDemonLin%2FMotor-modeling-and-power-control)
4. [【RM2025-超级电容与无线充电系统开源】香港科技大学ENTERPRIZE战队](https://bbs.robomaster.com/article/761385?source=4)
5. 在此十分感谢以上的个人、战队、院校。参考开源

## 致谢

-   此代码模板源于王芃学长。感谢我的电控学长，付增波，陈泓翔，王芃。是他们留下来的达妙H7通用控制系统让我能在此基础上修改完善。尤其是感谢付增波学长，我许多的代码都是对他的拙劣模仿; 感谢陈泓翔项管，对我代码不断完善提供指导和建议。
-   感谢导航组的代雨欣，叶少龙同学和齐志昂学长为哨兵赋予灵魂。感谢视觉组张济仁同学和韩宇学长带来的自瞄策略。感谢硬件组尹纪兴和尹贻菲同学的超电，让哨兵体验到150w的功率。感谢王兴瑞同学的机械设计舵轮哨兵，让我能将此项目运用在实际机器人上。
-    致敬传奇操作手————朱君豪同学
-   **感谢和我一路走来的COD成员！**
-   感谢沈阳工业大学（辽阳分校）赫兹矩阵战队、沈阳理工大学 Ambition战队、东北大学T-DT战队、南京航空航天大学长空御风战队、大连理工大学凌Bug战队、辽宁石油化工大学TCN战队（排名不分先后）的开源或交流，使我可以更好的 完善此项目。

## 展望
- 我们的队员王浩同学提出可以使用4310MIT模式作为角度轮
- 后续发现我们哨兵启动慢的原因可能是裁判系统接收数据较慢，可以尝试把裁判系统的数据接收单独分配一个Freertos任务


## 联系方式

QQ：1634789078