dcm.c

基于DsPic30F4011的高精度位置伺服控制系统

#define	INITIALIZE
#include "motor.h"
#include "parms.h"
#include "encoder.h"
#include "SVGEN.h"
#include "pi.h"
#include "ReadADC.h"		//包含变量定义和函数声明
#include "UserParms.h"

/******** 引脚分配**********
单片机:dsPIC30F4011
//指示LED接口，设置为输出模式，低电平点亮
LED1:AN8/RB8
LED2:AN7/RB7
LED1:AN6/RB6
//数码管串入数据并出数据接口
SP_DAT:RD1
SP_RCLK:RD3
SP_SRCLK:RD2
//数码管位显接口
WX1:PWM2L/RE2
WX2:PWM2H/RE3
WX3:PWM3L/RE4
WX4:PWM3H/RE5
***********************/
const unsigned char ledtable[]=
{
	0x82,0xbb,0xa4,0xa1,0x99,0xc1,0xc0,0xab,0x80,0x81,0xff
};


struct 
{
	unsigned MotorRunning	:1;  // 
	unsigned MotorDir		:1;
	unsigned unused			:15;
}Flags;
//周期计算
unsigned int SteerEncoderValue;		//方向盘的旋转解码值
unsigned int LastSteerEncoderValue;	//前一次方向盘的旋转解码值
unsigned char Required_Direction;	//要求的电机转子方向
unsigned char Current_Direction;	//当前的电机转子方向

unsigned char Required_Direction;	//要求的电机转子方向
unsigned char Current_Direction;	//当前的电机转子方向

//根据霍耳中断计算电机速度和转动角度
unsigned int PastCapture, ActualCapture, Period; 
//根据旋转编码器的状态计算方向盘转动角度和方向
 int SteerCntValue; 	//方向盘计数初始值
 int WheelCntValue;			//转向轮计数值

unsigned int MotorStalledCounter = 0; // This variable gets incremented each 
                                      // 1 ms, and is cleared everytime a new
                                      // sector is detected. Used for 
                                      // ForceCommutation and MotorStalled 
                                      // protection functions

SFRAC16	_WHEELPOSITION = diEncoderCntsPerRev;
SFRAC16	_STEERPOSITION = diSteerCntsPer90Degree;
SFRAC16 MeasurePosition,RefPosition;		//测量位置，参考位置

//SFRAC16 _MINPERIOD = MINPERIOD - 1;
SFRAC16 MeasuredSpeed, RefSpeed;

//SFRAC16 ErrorPosition_1,ErrorPosition_2,ErrorPosition_3;
//SFRAC16 ControlDifference[3];
SFRAC16 ControlOutput = 0;

unsigned char Led_Show_Buf[4] = {};      //显示缓冲区
unsigned char Led_Show_Data;        	 //LED待显示数据
unsigned char Led_Show_Bit;				 //LED待显示数据位
unsigned char Nwei_Show = 0;	    	 //刷新控制寄存器


//tPIParm  PIParmPosition;				//位置PID参数
//tPIParm  PIParmVelocity;				//速度PID参数
//tPIParm  PIParmCurrent;					//电流PID参数

tReadADCParm  ReadADCParm;				//AD转换参数

unsigned int	OutPut;


long k;
int a,b,c;
unsigned int adcnt = 0;
unsigned int adshow = 0;
//----------------------------------------------------------
//函数: void __attribute__((__interrupt__)) _CNInterrupt (void)
//输入: None.
//输出: None.

//概括: 此中断程序为方向盘上编码器的QEA信号的变化通知中断服务程序，程序中累加中断次数
//	并根据此时连接于RB3上的QEB信号的状态判断方向盘的转动方向
//----------------------------------------------------------
void __attribute__((__interrupt__)) _CNInterrupt (void)
{
	IFS0bits.CNIF = 0;	// Clear interrupt flag

	register unsigned int temp;
	temp = PORTB & 0x0008;
	temp |= (PORTC & 0x4000);
	if(temp == 0x4008 || temp == 0)
	{
		EncoderParm.iSteerCnt ++;			//方向盘正转	
		if(EncoderParm.iSteerCnt >= diEnMaxSteerCnts)
			EncoderParm.iSteerCnt = diEnMaxSteerCnts;		
	}
	else if(temp == 0x0008 || temp == 0x4000)
	{

		EncoderParm.iSteerCnt --;			//方向盘反转
		if(EncoderParm.iSteerCnt <= -diEnMaxSteerCnts)
			EncoderParm.iSteerCnt = -diEnMaxSteerCnts;			
	}	

	return;
}


//---------------------------------------------------------------------
// ADC 中断服务程序执行速度计算以及PWM更新循环。
// ADC 采样和转换由PWM 周期触发。
// 速度计算假定计算之间的间隔时间是固定的。
//---------------------------------------------------------------------
void __attribute__((__interrupt__)) _ADCInterrupt (void)
{
	IFS0bits.ADIF = 0;
	//计算电机速度
	//MotorStalledCounter ++;
	//LATBbits.LATB6 = 1; 

    CalcVelAngIrp();  
    CalcAng();
    PIParmPosition.qInRef = EncoderParm.qSteerMechAng;
    PIParmPosition.qInMeas = EncoderParm.qWheelMechAng;
    PIParmPosition.qInErr = PIParmPosition.qInRef - PIParmPosition.qInMeas;			//角度偏差
    if(PIParmPosition.qInErr > 1000  )
      PIParmPosition.qInErr = 1000;
    else if(PIParmPosition.qInErr < -1000) 	
      PIParmPosition.qInErr = -1000;

    CalcPosPI(&PIParmPosition); 
     
    if(EncoderParm.iVelCntDwn == EncoderParm.iIrpPerCalc) 
    {
	    CalcVel();
	    PIParmVelocity.qInRef = PIParmPosition.qOut;
	    PIParmVelocity.qInMeas = EncoderParm.qMotorVelMech;
		PIParmVelocity.qInErr = PIParmVelocity.qInRef - PIParmVelocity.qInMeas;
		CalcPosPI(&PIParmVelocity);
		
		if(PIParmVelocity.qOut < 0)
		{
			PIParmVelocity.qOut = -PIParmVelocity.qOut;
			PWMCON1bits.PEN1L = 0;
			Nop();
			PWMCON1bits.PEN1H = 1;
		}
		else if(PIParmVelocity.qOut > 0)
		{
				PWMCON1bits.PEN1H = 0;	
				Nop();
				PWMCON1bits.PEN1L = 1;	
		}	
		else
		{
			PIParmVelocity.qOut = 0;
			PWMCON1bits.PEN1L = 0;
			Nop();
			PWMCON1bits.PEN1H = 0;
		}
		PDC1 = (((unsigned long)PIParmVelocity.qOut * (unsigned long)PTPER * 2)>>15);       
	}  
	                    
 
    return;
}

//=====================================
//void InitUserParms(void)
//初始化设备参数
//
//
//=====================================
void InitUserParms(void)
{
	CORCONbits.SATA  = 0;			//禁止ACCA 和ACCB 饱和
	CORCONbits.IF = 0;

	//INTCON1bits.NSTDIS = 1;			//禁止中断嵌套
	//初始化电机参数
	MotorParm.iMechRPM = diMechMaxSpeed;		//2000RPM

	MotorParm.iCntsPerRev  = diEncoderCntsPerRev; 	//4096
	// Basic loop period (in sec).  (PWM interrupt period)
	MotorParm.fLoopPeriod = dLoopInTcy * dTcy;
	// Encoder velocity interrupt period (in sec). 
    MotorParm.fVelIrpPeriod = MotorParm.fLoopPeriod;	//中断周期0.0001s

	MotorParm.iIrpPerCalc = diIrpPerCalc;				//每次速度计算的AD中断次数40

	//mechanical speed of motor (in rev/sec)
    MotorParm.fMechRPS = (float)(MotorParm.iMechRPM/60.0);//电机转子最大速度33.33RPS
    
    // ---------------编码器----------------
    if(InitEncoderScaling() )
    	return True;
 
	//----------位置环PID参数---------------
	PIParmPosition.qKp = dPqKp;			//位置环比例系数0.3125,
										//实际使用扩大16倍定标，即比例系数为4.0
	PIParmPosition.qK1_p = dPqK1_p;			
							
	PIParmPosition.qKi = dPqKi;			//
	PIParmPosition.qKc = dPqKc;			//
	PIParmPosition.qOutMax = dPqOutMax;	//
	PIParmPosition.qOutMin = -dPqOutMax;
	InitPI(&PIParmPosition);
	
	PIParmVelocity.qKp = dVqKp;			//速度环比例系数0.3125,
										//实际使用扩大16倍定标，即比例系数为4.0
	PIParmVelocity.qKi = dVqKi;			
	PIParmVelocity.qKc = dVqKc;			
	PIParmVelocity.qOutMax = dVqOutMax;	
	PIParmVelocity.qOutMin = -dVqOutMax;
	InitPI(&PIParmVelocity);
	

	//---------ADC-测量电流和电位器---------
	ReadADCParm.qK = dqK;				//0.5
}

//=====================================
//外设初始化设置
//AD
//PWM
//QEI
//=====================================
SetupPeripherals(void)
{
	//--------电机PWM	-----------------
	TRISE = 0x0100;			//设置为输出模式
	PTPER = dLoopInTcy - 1;	//PWM周期计数值
	//OVDCON = 0x0000;		//禁止所有PWM输出
	PDC1 = 0;
	
	PWMCON1 = 0x0e11;			//配置PWM1H PWM1L为互补PWM输出模式，其余配置为通用I/O引脚
	PWMCON1bits.PMOD3 = 1;		//PWM配对I/O引脚处于独立输出模式
	PWMCON1bits.PMOD2 = 1;		//PWM配对I/O引脚处于独立输出模式
	PWMCON1bits.PMOD1 = 1;		//PWM配对I/O引脚处于独立输出模式
	PWMCON1bits.PEN3H = 0;
	PWMCON1bits.PEN2H = 0;		//PWMxH引脚禁止。I/O引脚成为通用I/O
	PWMCON1bits.PEN1H = 0;		//PWMxH引脚使能为PWM输出
	PWMCON1bits.PEN3L = 0;	
	PWMCON1bits.PEN2L = 0;
	PWMCON1bits.PEN1L = 0;		
	
	DTCON1 = 0;	
	DTCON1bits.DTAPS = 0;		//死区时间单元A的时钟周期为TCY
	//DTCON1bits.DTA = dDeadTime;	//死区时间单元A 的无符号6 位死区时间值位
	
	FLTACON = 0;
	
	PTCON = 0;				//PWM 时基控制寄存器		
	PTCONbits.PTEN = 1;		//PWM时基开启
	PTCONbits.PTSIDL = 0;	//PWM时基在CPU 空闲模式运行
	PTCONbits.PTOPS = 0;	//PWM 时基输出后分频比选择位
	PTCONbits.PTCKPS = 0;	//PWM 时基输入时钟周期预分频比为1 TCY（1:1 预分频）
	PTCONbits.PTMOD = 0;	//PWM 时基工作在自由运行模式

	SEVTCMP = 2;			//特殊事件比较值位

//------------ ADC - 测量电流和电位器给定值---------------------
	ADCON1 = 0;