close_loop.c

dsPIC30f3010的PID控制代码

{
	InitUserInt();										// 初始化 I/O
	InitADC10();										// 初始化 ADC，小数格式
	InitTMR1();											// 初始化 TMR1，1 ms 周期ISR
	InitTMR3();											// 初始化 TMR3，产生捕捉时基
	Init_IC_and_CN();									// 初始化霍尔传感器输入 ISR	
	InitMCPWM();										// 初始化 PWM @ 20 kHz, 中心对齐, 500 ns 死区 

	for(;;)
	{
		if ((Switch_2) && (!Flags.MotorRunning))
		{
			while(Switch_2);
			RunMotor();									// 电机停止的时候有按键，启动电机
		}
		else if ((Switch_2) && (Flags.MotorRunning))
		{
			while(Switch_2);
			StopMotor();								// 电机运行的时候有按键，停止电机
		}
	}
	return 0;
}

//***********************************************************************************
// 	这种拓扑结构需要对升压电容（bootstrap cap）充电：
//      经过一段未知时间后，电机第一次运行前//  都要打开充电函数，下桥晶体管10 ms ，
//      保证这些电容上有电荷,然后把控制权交给PWM
//***********************************************************************************

void ChargeBootstraps(void)
{
	unsigned int i;
	OVDCON = 0x0015;									// 打开下桥晶体管进行充电
	for (i = 0; i < 33330; i++) 						// 延迟10 ms（20 MIPS）
		;
	OVDCON = 0x0000;
	return;
}

//*********************************************************************
// 启动电机：
//   RunMotor 将对升压电容充电、初始化变量、使能ISR
//*********************************************************************

void RunMotor(void)
{
	ChargeBootstraps();
	ControlDifference[0] = 0;							// K时刻的Error(最近的)
	ControlDifference[1] = 0;							// K-1时刻的Error
	ControlDifference[2] = 0;							// K-2时刻的Error
	PIDCoefficients[0] = Kp + Ki + Kd;					// 调整后的参数，方便使用 MAC指令
	PIDCoefficients[1] = -(Kp + 2*Kd);		 
	PIDCoefficients[2] = Kd;				 
	TMR1 = 0;											// 复位TMR1，用于速度控制
	TMR3 = 0;											// 复位TMR3，用于速度测量
	ActualCapture = MAXPERIOD; 							// 初始化IC，最低速度60 RPM
	PastCapture = 0;
	Period = MAXPERIOD;
	while (IC7CONbits.ICBNE)
		IC7BUF;
														// 初始化方向，注意ADC没有停止
	HallValue = (unsigned int)((PORTB >> 3) & 0x0007);	// 读取霍尔信号
	LastSector = Sector = SectorTable[HallValue];		// 初始化扇区

							// 只有启动的时候，RefSpeed的符号根据正转CW (+RefSpeed)或反转CCW (-RefSpeed)
    						// 因为电机启动后，方向将由控制输出变量决定，以便工作在四个象限
	if (RefSpeed > 0)
	{
		ControlOutput = 0;
		Current_Direction = Required_Direction = CW;
		Speed = MINABSSPEED;
	}
	else
	{
		ControlOutput = 0;
		Current_Direction = Required_Direction = CCW;
		Speed = -MINABSSPEED;
	}
	SpeedAbsValue = MINABSSPEED;
	MotorStalledCounter = 0;							// 复位电机停止保护计数器
	IFS0bits.T1IF = 0;									// 清除所有中断标志 
	IFS0bits.CNIF = 0;	 								// 禁止 TMR1、CN5、IC7、IC8 中断
	IFS1bits.IC7IF = 0;	 
	IFS1bits.IC8IF = 0;	 
	__asm__ volatile("DISI #5");						// 禁止所有中断
	IEC0bits.T1IE = 1;	 
	IEC0bits.CNIE = 1; 
	IEC1bits.IC7IE = 1;	 
	IEC1bits.IC8IE = 1;	 
	Flags.MotorRunning = 1;								// 设置电机运行标志
	return;
}

//*********************************************************************
//  关闭电机：
//  清除中断、关闭PWM
//*********************************************************************

void StopMotor(void)
{
	OVDCON = 0x0000;									// 关闭所有晶体管											
	__asm__ volatile("DISI #5");						// 禁止所有中断
	IEC0bits.T1IE = 0;	
	IEC0bits.CNIE = 0;									// 禁止 TMR1、CN5、IC7、IC8 中断
	IEC1bits.IC7IE = 0;	 
	IEC1bits.IC8IE = 0;						 
	Flags.MotorRunning = 0;								// 指示电机停止
	return;
}

//********************************************************************
// 六步换向程序：
//     根据扇区的位置查找表格，决定晶体管对开关顺序
//********************************************************************

void SixStepComm (int _Sector, int _Voltage)
{
	if (_Voltage >= 0)
	{																// PDC寄存器装载一样的数值，使用强制控制				
																	// 逻辑（OVDCON)选择相应的晶体管对
		PDC1 = PDC2 = PDC3 = (unsigned int)_Voltage / 16;
		if (_Sector == -1)
			OVDCON = 0x0000; 										// 假如遇到非法扇区，关闭晶体管
		else
			OVDCON = StateLoTable[_Sector];
	}
	else
	{
		PDC1 = PDC2 = PDC3 = (unsigned int)(-(_Voltage+1)) / 16;
		if (_Sector == -1)
			OVDCON = 0x0000; 										// 假如遇到非法扇区，关闭晶体管
		else
			OVDCON = StateLoTable[(_Sector + 3) % 6];
	}
	return;
}

//******************************************************************************************
//  强制换向：
//      当电机不产生霍尔信号的时候就会调用该函数, 也即：电机旋转过慢或已经静止
//      假如电机慢，这时会根据实际的传感器位置和旋转方向调用该函数强制换向程序。
//******************************************************************************************

void ForceCommutation(void)
{
	HallValue = (unsigned int)((PORTB >> 3) & 0x0007);	// 读取霍尔信息
	Sector = SectorTable[HallValue];					// 根据霍尔信息读取扇区
	if (Sector != -1)									// 假如在非法扇区，不做任何操作
	{
		if (Required_Direction == CW)					// 根据旋转方向, 取得新的相位
		{ SixStepComm(Sector, ControlOutput); }
		else
		{ SixStepComm(Sector, -(ControlOutput + 1)); }
	}
	return;
}

//******************************************************************************************
//  配置 ADC :
//	   一个通道 （RB2/AN2)、使用PWM触发、电位器连接到CH0和RB2、手动停止采样、启动转换
//	   手动检查转换结束、数据格式为有符号小数（signed fractional）			
//******************************************************************************************

void InitADC10(void)
{
	ADPCFG = 0xFFF8;				// RB3, RB4, RB5 数字口
	ADCON1 = 0x026E;				// PWM 触发，小数格式									
	ADCON2 = 0x0000;
	ADCHS = 0x0002;					// 电位器连接到 AN2
	ADCON3 = 0x0003;
	IFS0bits.ADIF = 0;			 
	IEC0bits.ADIE = 1;			 	
	ADCON1bits.ADON = 1;			// 打开ADC 
	return;
}

//****************************************************************************************
//	InitMCPWM, 初始化PWM:
//	   FPWM = 20000 Hz、沿对齐PWMs，独立工作模式、占空比设置为0、使用PWM触发 ADC 
//****************************************************************************************

void InitMCPWM(void)
{
	TRISE = 0x0100;				// PWM 管脚为输出, FLTA脚为输入
	PTPER = FCY/FPWM - 1;		// 根据CPU速度计算周期
	OVDCON = 0x0000;			// 禁止 PWM 输出
	PWMCON1 = 0x0777;			// 使能 PWM ，配置为互补对称模式		 
	PDC1 = 0;					// 占空比为 0  
	PDC2 = 0;				 
	PDC3 = 0;				 
	SEVTCMP = PTPER;			 
	PWMCON2 = 0x0F00;			// 16 后分频（获得20 kHz）
	PTCON = 0x8000;				// 沿对齐
	return;				 
}

//********************************************************************
//  配置霍尔传感器输入：
//      电平变换中断（CN5） 、输入捕捉（IC7，IC8）  
//      在IC7 口捕捉的数值用于计算下一个周期
//********************************************************************

void Init_IC_and_CN(void)
{
	//Hall A -> CN5. Hall A 只用于换向
	//Hall B -> IC7. Hall B 用作换向、速度检测
	//Hall C -> IC8. Hall C 只用作换向
								// 设置 CN 5 （电平变换中断）
	TRISB |= 0x38;				// 所有霍尔连接端口设置为输入
	CNPU1 = 0;					// 禁止所有 CN 上拉
	CNEN1 = 0x20;				// 允许 CN5
	IFS0bits.CNIF = 0;	 
								// 输入捕捉 7
	IC7CON = 0x0001;			// 在每个沿进行输入捕捉（结合中断和TMR3）
	IFS1bits.IC7IF = 0;		 
								// 输入捕捉 8
	IC8CON = 0x0001;			// 在每个沿进行输入捕捉（结合中断和TMR3）
	IFS1bits.IC8IF = 0;			// 清中断标记
	return;
}

//***********************************************************************
//	初始化 Timer 1 ：
//      每 1 ms 中断一次，以便进行速度控制, 电机停止保护包括：
//      电机太慢的时候强制换向 , 电机速度为0的时候停止电机运行
//***********************************************************************

void InitTMR1(void)
{
	T1CON = 0x0020;			// 采用内部时钟Tcy/64 
	TMR1 = 0;
	PR1 = 313;				// 1 ms 中断（在20 MIPS时）
	T1CONbits.TON = 1;		// 打开TMR1 
	return;
}

//************************************************************************
//	初始化 timer 3 ：
//      作为计算霍尔周期的捕捉通道的时基。
//************************************************************************

void InitTMR3(void)
{
	T3CON = 0x0020;			// 采用内部时钟 Tcy/64  
	TMR3 = 0;
	PR3 = 0xFFFF;
	T3CONbits.TON = 1;		// 打开 TMR3 
	return;
}

//************************************************************************
//	初始化 IO：
//************************************************************************

void InitUserInt(void)
{
	TRISC |= 0x4000;	// S2/RC14 为输入
	PORTF = 0x0008;		// RS232 口初始化
	TRISF = 0xFFF7;		// TX脚作为输出
	return;
}

/*
void SpeedControl(void)
{ 
  unsigned int i;
  ControlDifference[0] = RefSpeed - Speed ;
  for (i = 0; i < 3; i++)
     {
      ControlOutput += ControlDifference[i] * PIDCoefficients[i] ;
     }
   ControlDifference[2] = ControlDifference[1];
   ControlDifference[1] = ControlDifference[0];
 }
*/