encoder.c

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

// Scaling for encoder routines
//根据电机参数和电机编码器线数，确定电机速度计算的比例系数
//
//根据方向盘的编码器线数和旋转的圈数计算角度计算比例系数
#include "general.h"
#include "parms.h"
#include "encoder.h"
#include "UserParms.h"
//---------------------------------------------------------------------
//名称：bool InitEncoderScaling(void)
//说明：编码器计算比例初始化
//包括：
//1、电机角度比例计算
//所需数据：
//
//2、电机速度比例计算
//已知参数：iIrpPerCalc 、
//输出参数：EncoderParm.qKvel
//
//3、方向盘角度比例计算
//已知参数：iSteerCntsPer90Degree
//输出参数：EncoderParm.qKang_S
//
//---------------------------------------------------------------------
bool InitEncoderScaling(void)
{
	float 	fVelCalcPeriod, fMaxCntRate;
	long	MaxDeltaCnt;
	long	K;
	
	EncoderParm.iMotorCntsPerRev = MotorParm.iCntsPerRev ;	//4096
	EncoderParm.iSteerCntsPerWheel90Degree = diSteerCntsPer90Degree;	//8192
	//电机角度比例计算
	//qKang_w = (2^15)*(2^Nang_w)/iEncoderCntsPerRev/dfScaleDownVel.
	K = 32768;
	K *= (1 << Nang_W);
	K = K/diEncoderCntsPerRev;
	EncoderParm.qKang_Pos = K/dfWheelQuarterTurn;		//
	//qKang_w = (2^15)*(2^Nang_w)/iEncoderCntsPerRev/dfScaleDownVel.
	K = 32768;
	EncoderParm.qKang_Indx = K/dfWheelQuarterTurn;		//

	//方向盘角度比例计算
	//qKang_S = (2^15)*(2^Nang_W)/iWheelCntsPerRev.
	K = 32768;
	K *= (1 << Nang_S);
	EncoderParm.qKang_S = K/EncoderParm.iSteerCntsPerWheel90Degree;		//

	//电机转速比例计算
	EncoderParm.iIrpPerCalc = MotorParm.iIrpPerCalc;	//40
	fVelCalcPeriod = MotorParm.fVelIrpPeriod * MotorParm.iIrpPerCalc;	//0.004
	fMaxCntRate =EncoderParm.iMotorCntsPerRev * MotorParm.fMechRPS;//204800.0
	MaxDeltaCnt = fVelCalcPeriod * fMaxCntRate;		//每次速度计算最大计数脉冲819

	// qKvel = (2^15)*(2^Nvel)/MaxDeltaCnt
	K = 32768;
	K *= (1 << Nvel);
	K /= MaxDeltaCnt;
	if(K >= 32768)
		//Error
		return True;
	EncoderParm.qKvel = K;
	
	InitCalcVel();

	return False;
}