# 电动捕鼠

**Repository Path**: single-chip-development_1/electric-mouse-trap

## Basic Information

- **Project Name**: 电动捕鼠
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-08-21
- **Last Updated**: 2026-03-23

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 电方向及磁编码器零点校准

sysval.voltage_sensor_align校准时q轴分量一般设置0.5，针对50w左右的航模电机

也就是电流的额定功率的10分之1，或者20分之一

```
int alignSensor(void)
{


		//dbgprint("MOT: Align sensor.\r\n");
		//dbgprint("zero_electric_angle:%d\r\n",sysval.iap.zero_electric_angle );
		long i;
		float angle;
		float mid_angle,end_angle;
		float moved;
		sysval.iap.sensor_direction = UNKNOWN;
		sysval.iap.zero_electric_angle =0 ;
		//校准电机方向与磁编码的关系
		if(sysval.iap.sensor_direction == UNKNOWN)  //没有设置，需要检测
		{
			// find natural direction
			// move one electrical revolution forward
			for(i=0; i<=500; i++)
			{
				angle = _3PI_2 + _2PI * i / 500.0f;
				setPhaseVoltage(sysval.voltage_sensor_align, 0,  angle);
				delay_ms(10);
			}
			mid_angle=getAngle();

			for(i=500; i>=0; i--) 
			{
				angle = _3PI_2 + _2PI * i / 500.0f;
				setPhaseVoltage(sysval.voltage_sensor_align, 0,  angle);
				delay_ms(10);
			}
			end_angle=getAngle();
			setPhaseVoltage(0, 0, 0);
			delay_ms(200);

			dbgprint("mid_angle=%.4f\r\n",mid_angle);
			dbgprint("end_angle=%.4f\r\n",end_angle);

			moved = fabs(mid_angle - end_angle);
			if((mid_angle == end_angle)||(moved < 0.01f))  //相等或者几乎没有动
			{
				dbgprint("MOT: Failed to notice movement loop222.\r\n");
				tim1Stop(); //电机检测不正常，关闭驱动
				return 0;
			}else if(mid_angle < end_angle)
			{
				dbgprint("MOT: sensor_direction==CCW\r\n");
				sysval.iap.sensor_direction=CCW;
			}else{
				dbgprint("MOT: sensor_direction==CW\r\n");
				sysval.iap.sensor_direction=CW;
			}

		}else{dbgsegprint("MOT: Skip dir calib.\r\n");}
		dbgprint("pole_pairs:%d\r\n",sysval.pole_pairs);
		if(sysval.iap.zero_electric_angle == 0)  //没有设置，需要检测
		{
			//强行将电机q轴拉到3分2pi的位置读取零点偏移角度
			setPhaseVoltage(sysval.voltage_sensor_align, 0,  _3PI_2);	//计算零点偏移角度
			delay_ms(150);
			delay_ms(150);
			delay_ms(150);
			delay_ms(150);
			delay_ms(150);
			//sysval.iap.zero_electric_angle = (机械角*极对数)%一圈的数值
			sysval.iap.zero_electric_angle = _normalizeAngle(_electricalAngle(sysval.iap.sensor_direction*getAngle(), sysval.pole_pairs));
			delay_ms(20);
			dbgprint("Zero electric angle:%f  shaft angle:%f\r\n",sysval.iap.zero_electric_angle,sysval.f_angle);
			delay_ms(150);
		}else dbgprint("MOT: Skip offset calib.\r\n");
		setPhaseVoltage(0, 0, 0);	//关闭电机
		return 1;

}


```

---

# svpwm发波函数

```


void setPhaseVoltage(float Uq, float Ud, float angle_el)
{


	float Uout,T0,T1,T2;
	float Ta,Tb,Tc,U_alpha,U_beta;

	angle_el =_normalizeAngle(angle_el);//电角度标准化在【0,2pi】

	U_alpha=Ud*arm_cos_f32(angle_el) - Uq*arm_sin_f32(angle_el);			  //反park变换
	U_beta=Ud*arm_sin_f32(angle_el) + Uq*arm_cos_f32(angle_el);

	Uout=_sqrt(U_alpha*U_alpha + U_beta*U_beta) / sysval.voltage_power_supply;	  //per unit值
	//0.577f是 1/√3（三分之一的平方根的倒数） 的浮点数常量
	if(Uout> 0.577f)Uout= 0.577f; 	//六边形的内切圆(SVPWM最大不失真旋转电压矢量赋值)
	if(Uout<-0.577f)Uout=-0.577f; 

	if(Uq>0)
	  	angle_el =_normalizeAngle(angle_el+_PI_2);			//加90度后是参考电压矢量的位置
	else
		angle_el =_normalizeAngle(angle_el-_PI_2);
	sysval.Sector =(uint8_t) ((angle_el / _PI_3) + 1);     //根据角度判断参考电压所在扇区                    			  
	T1 = _SQRT3* arm_sin_f32 (sysval.Sector*_PI_3 - angle_el) * Uout;           //计算两个相邻电压矢量作用时间
	T2 = _SQRT3* arm_sin_f32 (angle_el - (sysval.Sector-1.0)*_PI_3) * Uout;
	T0 = 1 - T1 - T2;                               	 //零矢量作用时间  


	// calculate the duty cycles(times)
	switch(sysval.Sector)
	{
		case 1:
			Ta = T1 + T2 + T0/2;
			Tb = T2 + T0/2;
			Tc = T0/2;
			break;
		case 2:
			Ta = T1 +  T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T0/2;
			break;
		case 3:
			Ta = T0/2;
			Tb = T1 + T2 + T0/2;
			Tc = T2 + T0/2;
			break;
		case 4:
			Ta = T0/2;
			Tb = T1+ T0/2;
			Tc = T1 + T2 + T0/2;
			break;
		case 5:
			Ta = T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T2 + T0/2;
			break;
		case 6:
			Ta = T1 + T2 + T0/2;
			Tb = T0/2;
			Tc = T1 + T0/2;
			break;
		default:  // possible error state
			Ta = 0;
			Tb = 0;
			Tc = 0;
	}

	TIM1->CCDAT1 = ((u16) (Ta * TIM1->AR));
	TIM1->CCDAT2 = ((u16) (Tb * TIM1->AR));
	TIM1->CCDAT3 = ((u16) (Tc * TIM1->AR));
}
/******************************************************************************/

```

---

# 电角度

```
float electricalAngle(void)
{
  //电角度 = (机械角*极对数)%一圈的数值  一般是2pi
  return _normalizeAngle((sysval.shaft_angle) * sysval.pole_pairs);
}
```

---

# 机械角

## 获取多圈角度函数

```
// 获取多圈角度函数 单位 ：弧度 (rad)
float getAngle(void)
{
	int32_t d_angle;
	uint16_t angle_data;
	angle_data = mt6701_ssi_read();
	d_angle = angle_data - angle_data_prev;
	if(fabs(d_angle) > (0.8f*Mt6701_cpr) ) full_rotation_offset += (d_angle > 0) ? -_2PI : _2PI; 
	angle_data_prev = angle_data;
	//转动圈数过多后数值过大导致计算下降，最终越来越慢，每隔一定圈数归零一次
	if(full_rotation_offset >= ( ANGLE2PIMAX)) 
	{                                        		//这个问题针对电机长时间连续一个方向转动
		full_rotation_offset = 0;              		//速度模式
		angle_prev = angle_prev - ANGLE2PIMAX;
		sysval.angleSetZeroCnt++;
		sysval.angleSetZeroFlage=1;

	}
	if(full_rotation_offset <= (-ANGLE2PIMAX))
	{
		full_rotation_offset =  0;
		angle_prev = angle_prev + ANGLE2PIMAX;
		sysval.angleSetZeroCnt--;
		sysval.angleSetZeroFlage=1;
	}
	sysval.f_angle = (full_rotation_offset + ( (float)angle_data * 3.835e-4f));
	return sysval.f_angle;
}

```

## 获取角速度函数

公式

系统时钟差分 = （上一个系统时钟数 - 当前系统时钟数）/系统时钟频率  ---108mhz分之1----

机械角差分 = (上一个机械角 - 当前机械角)

角速度 = 机械角差分 / 系统时钟差分      ----相当与速度等与路程除以时间----

```
{}// Shaft velocity calculation
// 获取角速度函数  单位 ：弧度/秒 (rad/s)
float getVelocity(void)
{
	long now_us;
	float Ts, angle_now;
	// calculate sample time
	now_us = SysTick->VAL;
	if(now_us < velocity_calc_timestamp){
		Ts = (float)(velocity_calc_timestamp - now_us)/ClkTs;
	}else{
		Ts = (float)(0xFFFFFF - now_us + velocity_calc_timestamp)/ClkTs;
	}
	if(Ts == 0 || Ts > 0.5f) Ts = 1e-3f; //系统时钟差分限幅
	angle_now = getAngle();
	if( 1 == sysval.angleSetZeroFlage){ //圈数归零时不计算速度
		velocity_angle_prev = angle_now;
		sysval.angleSetZeroFlage = 0;
	}else{
		sysval.vel = (angle_now - velocity_angle_prev)/Ts;
		velocity_angle_prev = angle_now;
	}
	velocity_calc_timestamp = now_us;
	return sysval.vel;
}

```



## 获取电角度

公式

电角度 = （机械角 * 极对数）% 一圈的数值

```
float electricalAngle(void)
{
  
  return _normalizeAngle((sysval.shaft_angle) * sysval.pole_pairs);
}
```


# 两电阻foc电流重构

## 配定时器触发adc采样时机

TIM_TimeBaseStructure.RepetCnt = 2 

相当于三个pwm周期产生一次更新事件，tim使用的是中心对齐模式3（上下计数）

相当于每个pwm周期的上升沿和下降沿都会触发一次adc采样

24khz的pwm那么 adc采样频率 = 24*2/3 = 16khz

```
    /* 定时器基础配置 */
	freq = 108000000 / (2 * freq);
    TIM_TimeBaseStructure.Prescaler = 0;		// 预分频器设置为0，时钟直接使用108MHz
    TIM_TimeBaseStructure.CntMode   = TIM_CNT_MODE_CENTER_ALIGN3; // 中心对齐模式3（上下计数）
    TIM_TimeBaseStructure.Period    = freq;		// 设置自动重装载值决定PWM频率
    TIM_TimeBaseStructure.ClkDiv    = 0;		// 时钟分频器设置为0，时钟直接使用108MHz
    TIM_TimeBaseStructure.RepetCnt  = 2;		// 重复计数器设置为2，用于生成3次更新事件
```


## 相电流计算

```
static void  getPhaseCurrents(void)
{

	// (零电流adc - 有电流adc)*(3.3/4096)/采样放大器增益/采样电阻阻值 = 相电流 
	// 零电流adc相当于把运放偏置电压1.65v算进去了，所以上面公式没有出现运放偏置电压1.65v
	// 采样放大器增益 50
	// 采样电阻阻值 0.002
	// (3.3/4096)/采样放大器增益/采样电阻阻值 = 0.008056f
	// FOCPHASEVOLTSAGE = 0.008056f
	current.a =  0;
	current.b =  LPFoperator(&LPF_current_b,(float)adcValueArray[VADCIONUM]);
	current.c =  LPFoperator(&LPF_current_c,(float)adcValueArray[WADCIONUM]);
	current.b = (current.b - (float)sysval.adcValueArray[VADCIONUM]) * FOCPHASEVOLTSAGE; // 
	current.c = (current.c - (float)sysval.adcValueArray[WADCIONUM]) * FOCPHASEVOLTSAGE; //

}
```


## 重构母线直流电流

```

float getDCCurrent(float motor_electrical_angle)
{

	// read current phase currents
	getPhaseCurrents();
	// calculate clarke transform
	//只采a,b两相电流
	if(!sysval.currentabc.c) 
	{
		// if only two measured currents
		i_alpha = sysval.currentabc.a;
		i_beta = _1_SQRT3 * sysval.currentabc.a + _2_SQRT3 * sysval.currentabc.b;
	}
	//只采b，c两相电流
	if(!sysval.currentabc.a)
	{
		// if only two measured currents
		sysval.ua = -sysval.currentabc.c - sysval.currentabc.b;
		i_alpha = sysval.ua;
		i_beta = _1_SQRT3 * sysval.ua + _2_SQRT3 * sysval.currentabc.b;
	}
	//只采a，c两相电流
	if(!sysval.currentabc.b)
	{
		// if only two measured currents
		sysval.vb = -sysval.currentabc.a - sysval.currentabc.c;
		i_alpha = sysval.currentabc.a; 
		i_beta = _1_SQRT3 * sysval.currentabc.a + _2_SQRT3 * sysval.vb;
	//只采a，b,c三相电流
	}else{
		// signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
		sysval.mid = (1.0f/3) * (sysval.currentabc.a + sysval.currentabc.b + sysval.currentabc.c);
		sysval.ua = sysval.currentabc.a - sysval.mid;
		sysval.vb = sysval.currentabc.b - sysval.mid;
		i_alpha = sysval.ua;
		i_beta = _1_SQRT3 * sysval.ua + _2_SQRT3 * sysval.vb;
	}
	sysval.wc = -(sysval.ua + sysval.vb );
	// if motor angle provided function returns signed value of the current
	// determine the sign of the current
	// sign(atan2(sysval.currentabc.q, sysval.currentabc.d)) is the same as c.q > 0 ? 1 : -1  
	if(motor_electrical_angle)sign = (i_beta * arm_cos_f32(motor_electrical_angle) - i_alpha*arm_sin_f32(motor_electrical_angle)) > 0 ? 1 : -1;  
	// return current magnitude
	return (float)(sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta));
}
```


## foc控制三相电流重构q,d轴电流


```
void getFOCCurrents(float angle_el)
{
	// read current phase currents
	getPhaseCurrents();

	// calculate clarke transform
	//只采a,b两相电流
	if(!sysval.currentabc.c)
	{
		// if only two measured currents
		i_alpha = sysval.currentabc.a;  
		i_beta = _1_SQRT3 * sysval.currentabc.a + _2_SQRT3 * sysval.currentabc.b;
	}
	//只采c,b两相电流
	if(!sysval.currentabc.a)
	{
		// if only two measured currents
		sysval.ua = -sysval.currentabc.c - sysval.currentabc.b;
		i_alpha = sysval.ua;
		i_beta = _1_SQRT3 * sysval.ua + _2_SQRT3 * sysval.currentabc.b;
	}
	//只采a,c两相电流
	if(!sysval.currentabc.b)
	{
		// if only two measured currents
		sysval.vb = -sysval.currentabc.a - sysval.currentabc.c;
		i_alpha = sysval.currentabc.a; 
		i_beta = _1_SQRT3 * sysval.currentabc.a + _2_SQRT3 * sysval.vb;
	}else{
		//只采a,b,c三相电流
		// signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
		sysval.mid = (1.f/3) * (sysval.currentabc.a + sysval.currentabc.b + sysval.currentabc.c);
		sysval.ua = sysval.currentabc.a - sysval.mid;
		sysval.vb = sysval.currentabc.b - sysval.mid;
		i_alpha = sysval.ua;
		i_beta = _1_SQRT3 * sysval.ua + _2_SQRT3 * sysval.vb;
	}
	sysval.wc = -(sysval.ua + sysval.vb );
	// calculate park transform
	ct = arm_cos_f32(angle_el);
	st = arm_sin_f32(angle_el);
	sysval.currentdq.d = i_alpha * ct + i_beta * st;
	sysval.currentdq.q = i_beta * ct - i_alpha * st;

}

```

## 三电阻采样的另一种电流重构算法

### 三电阻采样重构母线直流电流

```
float getDCCurrent(float motor_electrical_angle)
{
	getPhaseCurrents();
	switch (sysval.Sector)
	{
	  case 4:
	  case 5:

		i_alpha =  sysval.currentabc.b;
		i_beta  =  sysval.currentabc.c ;		  
		break;

	  case 6:
	  case 1:

		i_beta	=  sysval.currentabc.a;
		i_alpha = sysval.currentabc.c - i_beta;
		break;

	  case 2:
	  case 3:
		  i_alpha = sysval.currentabc.a;
		  i_beta = sysval.currentabc.b - i_alpha;
		break;
	}
	if(motor_electrical_angle)sign = (i_beta * arm_cos_f32(motor_electrical_angle) - i_alpha*arm_sin_f32(motor_electrical_angle)) > 0 ? 1 : -1;  
	return (float)(sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta));
}
```


### 三电阻采样重构foc控制的q,d轴电流

```
void getFOCCurrents(float angle_el)
{

	getPhaseCurrents();
	switch (sysval.Sector)
	{
	  case 4:
	  case 5:

		i_alpha =  sysval.currentabc.b;
		i_beta  =  sysval.currentabc.c ;		  
		break;

	  case 6:
	  case 1:

		i_beta	= sysval.currentabc.a;
		i_alpha = sysval.currentabc.c - i_beta;
		break;



	  case 2:
	  case 3:
		  i_alpha = sysval.currentabc.a;
		  i_beta = sysval.currentabc.b - i_alpha;
		break;
	 }

	//Park变换
	ct = arm_cos_f32(angle_el);
	st = arm_sin_f32(angle_el);
	sysval.currentdq.d = i_alpha * ct + i_beta * st;
	sysval.currentdq.q = i_beta * ct - i_alpha * st;


}
```


# foc控制Park  Clarke  和 反Park  反Clarke的作用

* Park 变换：将两相静止坐标系（αβ）的交流量转换为两相旋转坐标系（dq）的直流量，便于像控制直流电机一样进行闭环控制。
* 反 Park 变换：将两相旋转坐标系（dq）的直流量转换为两相静止坐标系（αβ）的交流量，用于将控制量转化为实际可施加的电压 / 电流。
* Clarke 变换：将三相坐标系（ABC）的交流量转换为两相静止坐标系（αβ）的交流量，减少变量维度，简化计算。
* 反 Clarke 变换：将两相静止坐标系（αβ）的交流量转换为三相坐标系（ABC）的交流量，用于驱动三相电机的实际绕组。

## 电机控制dq轴分量的作用

在电机控制（尤其是矢量控制 FOC）中，dq 轴的作用主要有：

1. **实现解耦控制** ：将交流电机的磁场（d 轴）和转矩（q 轴）分量分离，可像控制直流电机一样独立调节。
2. **简化控制逻辑** ：把三相交流量转换为 dq 坐标系下的直流量，便于使用 PI 调节器等简单控制算法实现高精度闭环控制。
3. **提升动态性能** ：通过独立控制 d 轴（励磁）和 q 轴（转矩）分量，可快速响应负载变化，提高电机的动态响应速度。
4. **优化运行效率** ：可通过调节 d 轴电流控制磁场强度，在不同工况下实现效率最优。