optical/NxFuncs/closedriver/driversctrl.c

#include "config.h"
#include "driversctrl.h"

#if (DRIVERS_NUM > 0)

#include "stepmotos.h"

#include "inout.h"
#include "delay.h"
#include "shell.h"
#include "trigger.h"
#include "cpuid.h"

#include "driverparas.h"

//------------------------------------------------------------------------

#include "drivermotion.h"

//------------------------------------------------------------------------

#ifndef MOTO_ECD_PER_CIRCLE
#define	MOTO_ECD_PER_CIRCLE		4000
#endif

//------------------------------------------------------------------------

#define OUT_INFO_NUM		0

typedef struct
{
	s32 cmdPosition;	// ָ<><D6B8>λ<EFBFBD><CEBB>
	s32 targetMotoPos;	// Ŀ<><C4BF>λ<EFBFBD><CEBB>
	s32 ecdPosition;	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>

	s32 thisPosErr;		// λ<><CEBB>ƫ<EFBFBD><C6AB>
	s32 integral;		// ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	s32 differential;	// λ<><CEBB>΢<EFBFBD><CEA2>
	s32 thisactVel;		// <20><><EFBFBD>μ<EFBFBD><CEBC><EFBFBD><EFBFBD>ٶ<EFBFBD>ֵ

	s32 pospid;			// λ<><CEBB>PID

	s32 setMotoPos;		// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
	s32 effort;			// <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>

}DrvOutInfo;

#ifndef DEF_SPD_FIL
#define DEF_SPD_FIL		0
#endif

typedef struct
{
	//----------------------
	int motoIdx;					// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

	void (*InitEncoder)(void);
	int (*ReadEncoderCounter)(void);

	//----------------------
	DriverPara paras;		// <20><><EFBFBD><EFBFBD>

	//----------------------
	int powerEnFlag;		// ʹ<>ܱ<EFBFBD>־
	//----------------------
	float stepAngle;		// ϸ<><CFB8>
	//----------------------
	s32 runspeed;			// <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
	s32 speedadd;			// <20>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD>м<EFBFBD><D0BC>ٶ<EFBFBD>
	int runsta;
	//----------------------
	s32 cmdPosition;		// ָ<><D6B8>λ<EFBFBD><CEBB>ͳ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>Ƕȣ<C7B6>
	s32 ecdPosition;		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ͳ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>Ƕȣ<C7B6>
	s32 runPosition;		// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ͳ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>Ƕȣ<C7B6>

	s32 lastPosErr;			// <20>ϴε<CFB4>ƫ<EFBFBD><C6AB>(<28><><EFBFBD>Ƕ<EFBFBD>)
	s32 integral;
	s32 differential;
#if (DEF_SPD_FIL > 1)
	s32 spdBuff[DEF_SPD_FIL];
	s32 spdIdx;
	s32 spdSum;
#endif

//	s32 lastVel;
//	s32 lastVErr;
//	s32 lastDErr;			// <20><><EFBFBD>ϴε<CFB4>ƫ<EFBFBD><C6AB>

	s32 intev;
	s32 effort;				// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	//----------------------
	s32 lastThetaGap;
	//----------------------
	s32 exCmdCounter;		// <20>ⲿָ<E2B2BF><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	//----------------------
	s32 posOffset;
	//----------------------

	int first;
	s32 oldTheta;			// <20>ϴεĽǶ<C4BD>(<28><><EFBFBD>Ƕ<EFBFBD>)
	s32 oldCounter;			// <20>ϴε<CFB4>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	s32 halfCounter;		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	//----------------------
	int errorCode;			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	//----------------------
#if (OUT_INFO_NUM != 0)
	int timeCount;
	int timeGap;
	int outInfoIdx;
	int outInfoEn;

	DrvOutInfo outInfoList[OUT_INFO_NUM+1];
#endif

}DriverCtrl;

//------------------------------------------------------------------------

DriverCtrl g_driverCtrl[DRIVERS_NUM];

//------------------------------------------------------------------------

void InitCloseCtrl(int driverIdx);

int GetThetaValue(int driverIdx);
void SetMotoDirectionCtrl(int driverIdx, int dirbit);
void SetMotoPulseCtrl(int driverIdx, int dirbit);

//------------------------------------------------------------------------

float CalcStepAngle(s32 stepPerCircle, s32 numerator, s32 denominator);

int GetThetaFromEcd(int ecdvalue);
int LoadCtrlPara();

//------------------------------------------------------------------------

const char * DRV_HLP_STR =
	"\t""<EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>" 	"\r\n"
	"\t" "p1		p2" "\r\n"
	"\t" "0		x" "<EFBFBD><EFBFBD>ʾx<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, 0 ȫ<><C8AB><EFBFBD><EFBFBD>ʾ" "\r\n"
	"\t""x0d	n" "<EFBFBD><EFBFBD><EFBFBD><EFBFBD>x<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>d<EFBFBD><EFBFBD>ֵΪn" "\r\n"
	"\t""x000	m" "<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD>˶<EFBFBD>x<EFBFBD><EFBFBD><EFBFBD><EFBFBD>" "\r\n"
	"\t\t"	"m=0" "ʧ<EFBFBD><EFBFBD>" "\r\n"
	"\t\t"	"m=1" "ʹ<EFBFBD><EFBFBD>" "\r\n"
	"\t\t"	"m=2" "<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>" "\r\n"
	"\t\t"	"m=3" "<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>" "\r\n"
	"\t\t"	"m=4" "<EFBFBD><EFBFBD><EFBFBD><EFBFBD>΢<EFBFBD><EFBFBD>" "\r\n"
	"\t\t"	"m=5" "<EFBFBD><EFBFBD><EFBFBD><EFBFBD>΢<EFBFBD><EFBFBD>" "\r\n"
	"\t" "x000d	n" "<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD>˶<EFBFBD>x<EFBFBD><EFBFBD><EFBFBD><EFBFBD>,λ<><CEBB>Ϊn" "\r\n"
;

void TestDriver(char * para1, char * para2);
void ShowDriver(char * para1, char * para2);

//------------------------------------------------------------------------

#define RD_CALIB_PPC			(100)	// (4000)

//-----------------------------------------------------------
#define IDX_RANGE				10
#define	PID_RANGE				1024			// PID <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĸ

#define	PULSE_COUNTER_RELOAD	(S32_MAX-1)			// <20>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0>ֵ

#define	LOW_CIRCLE_TIME			2000						// <20>͵<EFBFBD><CDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1> (2k)
#define	ADVANCE_ADJ_MAX			((MOTO_PPC)/MOTO_NOPP/5)		// <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>

#define	MIN_PE_LIMIT			((MOTO_PPC)/MOTO_NOPP/32)		// <20><>Сƫ<D0A1><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define	MAX_PE_LIMIT			(MOTO_PPC/10)					// <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

#define MAX_IG_LIMIT  			(RD_CALIB_PPC*PID_RANGE)	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

#define MAX_IS_LIMIT  			(MAX_IG_LIMIT*90/100)		// <20><><EFBFBD>ֶַ<D6B7><D6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define MIN_IS_LIMIT  			(MAX_IG_LIMIT*50/100)		// <20><><EFBFBD>ֶַ<D6B7><D6B6><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>

#define	EFF_MAX					(MAX_OUT_LEV *1)
#define	EFF_MIN					(MAX_OUT_LEV *-1)

#define MIN_OUT_EFF				0

#define DEF_OUT_EFF				50

#define POS_PER_CIRCLE			(MOTO_ECD_PER_CIRCLE)

//------------------------------------------------------------------------


//------------------------------------------------------------------------
#define MOTO_INIT(id)		do \
{\
	SetMotoCtrlFuncs(DRIVER##id##_IDX, \
			SetMoto##id##CoilAPwm, SetMoto##id##ACPOn, SetMoto##id##ACPOff, SetMoto##id##ACNOn, SetMoto##id##ACNOff, \
			SetMoto##id##CoilBPwm, SetMoto##id##BCPOn, SetMoto##id##BCPOff, SetMoto##id##BCNOn, SetMoto##id##BCNOff); \
 \
	SetMotoCCWDir(DRIVER##id##_IDX, MOTO##id##_CCWDIR); /* <20><><EFBFBD>÷<EFBFBD><C3B7><EFBFBD> */ \
 \
	SetMoto##id##CoilPwmEnable(); /* <20><><EFBFBD><EFBFBD>PWM<57><4D><EFBFBD><EFBFBD> */  \
 \
	g_driverCtrl[id-1].ReadEncoderCounter = ReadMoto##id##EncoderCounter; \
	g_driverCtrl[id-1].InitEncoder = InitMoto##id##Encoder; \
}while(0)

//------------------------------------------------------------------------

void InitDriversCtrl(void)
{
	int i;

	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
	InitStepMotos();		// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

	InitDrvMotions();		// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>

	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	memset(g_driverCtrl, 0, sizeof(DriverCtrl)*DRIVERS_NUM);

#if (DRIVERS_NUM >= 1)
	MOTO_INIT(1);
#endif

#if (DRIVERS_NUM >= 2)
	MOTO_INIT(2);
#endif

#if (DRIVERS_NUM >= 3)
	MOTO_INIT(3);
#endif

#if (DRIVERS_NUM >= 4)
	MOTO_INIT(4);
#endif

	// װ<><D7B0>Ĭ<EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD>
	InitDriverParas();

	for (i = 0; i < DRIVERS_NUM; i++)
	{
		g_driverCtrl[i].motoIdx = i;

		// <20>Ӳ<EFBFBD><D3B2><EFBFBD><EFBFBD>ļ<EFBFBD>װ<EFBFBD>ز<EFBFBD><D8B2><EFBFBD>ֵ
		memcpy(&(g_driverCtrl[i].paras), GetDriverParas(i), sizeof(DriverPara));

		// <20><><EFBFBD><EFBFBD>ϸ<EFBFBD><CFB8>
		g_driverCtrl[i].stepAngle = CalcStepAngle(g_driverCtrl[i].paras.stepPerCircle, g_driverCtrl[i].paras.numerator, g_driverCtrl[i].paras.denominator);
		printf("moto%d stepAngle=%.2f\r\n", i+1, g_driverCtrl[i].stepAngle);

		// <20><><EFBFBD><EFBFBD>Ĭ<EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD>

	}

	//-----------------------

	//-----------------------
	// <20>ջ<EFBFBD><D5BB><EFBFBD><EFBFBD>Ƴ<EFBFBD>ʼλ<CABC><CEBB>
	for (i = 0; i < DRIVERS_NUM; i++)
	{
		SetEnableCtrl(i, 0);
	}

	DelayMs(10);

	// <20><>ʼʱ<CABC>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
	for (i = 0; i < DRIVERS_NUM; i++)
	{
		if (g_driverCtrl[i].paras.closeCtrlMode != 0)
		{
			ResetMotoPos(i, 100);
		}
		else
		{
			g_driverCtrl[i].halfCounter = LOW_CIRCLE_TIME;
		}
	}

	DelayMs(200);

	// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݹ̶<DDB9><CCB6><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>
	for (i = 0; i < DRIVERS_NUM; i++)
	{
		g_driverCtrl[i].InitEncoder();
	}

	DelayMs(10);

	// ʹ<>ܵ<EFBFBD><DCB5><EFBFBD>
	for (i = 0; i < DRIVERS_NUM; i++)
	{
		SetEnableCtrl(i, 1);
	}

	DelayMs(100);

#if (OUT_INFO_NUM != 0)
	g_driverCtrl[0].outInfoIdx = 0;
	g_driverCtrl[0].timeGap = 0;
	g_driverCtrl[0].outInfoEn = 1;
#endif

	// <20>򿪿<EFBFBD><F2BFAABF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
	if (1)
	{
		printf("StartControlCycle\r\n");
		StartControlCycle();
	}

#if (0)
	AddShellCmd("DRIVER", DRV_HLP_STR, TestDriver);
#else
	AddShellCmd("DRIVER", "", ShowDriver);
#endif
}

//------------------------------------------------------------------------


void InitCloseCtrl(int driverIdx)
{
	s32 rdTheta = 0;
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];

	rdTheta = GetThetaValue(driverIdx);

	pCtrl->cmdPosition = rdTheta / pCtrl->stepAngle;		// ָ<><D6B8>λ<EFBFBD><CEBB>ͳ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>Ƕȣ<C7B6>
	pCtrl->ecdPosition = rdTheta;		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ͳ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>Ƕȣ<C7B6>
	pCtrl->runPosition = rdTheta;		// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>ͳ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>Ƕȣ<C7B6>

	pCtrl->lastPosErr = 0;			// <20>ϴε<CFB4>ƫ<EFBFBD><C6AB>
	pCtrl->integral = 0;			// <20><><EFBFBD><EFBFBD>
//	pCtrl->lastDErr = 0;			// <20><><EFBFBD>ϴε<CFB4>ƫ<EFBFBD><C6AB>
	pCtrl->effort = 0;				// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

	printf("\r\n InitCloseCtrl, g_driverCtrl[%d]:\r\n"
			"\t rdTheta=%d, \r\n"
			"\t stepAngle=%f, \r\n"
			"\t cmdPosition=%d, \r\n"
			"\t ecdPosition=%d, \r\n"
			"\t runPosition=%d, \r\n",
			driverIdx, rdTheta, pCtrl->stepAngle, pCtrl->cmdPosition, pCtrl->ecdPosition, pCtrl->runPosition);

	pCtrl->first = 1;
}

void SetEnableCtrl(int driverIdx, int enbit)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];

	if (enbit != 0)
	{
		if (pCtrl->powerEnFlag == 0 && pCtrl->paras.closeCtrlMode != 0)
		{
			InitCloseCtrl(driverIdx);
			SetMotoPosTo(pCtrl->motoIdx, pCtrl->ecdPosition, DEF_OUT_EFF);			// <20><><EFBFBD>ߵ<EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>
		}
		pCtrl->powerEnFlag = 1;

		printf("set moto%d en on\r\n", pCtrl->motoIdx+1);
	}
	else
	{
		if (pCtrl->powerEnFlag != 0)
		{
			pCtrl->powerEnFlag = 0;
			DelayUs(200);
			SetMotoEnable(pCtrl->motoIdx, 0);
		}
		printf("set moto%d en off\r\n", pCtrl->motoIdx+1);
	}
}


int GetThetaFromEcd(int ecdvalue)
{
#if (SIN_TAB_SIZE == POS_PER_CIRCLE)	// <20>򻯼<EFBFBD><F2BBAFBC><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ұ<EFBFBD><D2B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ
	ecdvalue *= MOTO_NOPP;
#else
	ecdvalue *= MOTO_PPC;
	ecdvalue /= POS_PER_CIRCLE;
#endif
	return ecdvalue;
}

int GetMotoThetaValue(DriverCtrl * pCtrl)
{
	if (pCtrl == NULL)
	{
		return 0;
	}

	int ecdvalue = pCtrl->ReadEncoderCounter();
	ecdvalue += pCtrl->paras.ecdAdjust;

	return GetThetaFromEcd(ecdvalue);
}

//
int GetThetaValue(int driverIdx)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return 0;
	}
	pCtrl = &g_driverCtrl[driverIdx];
	return GetMotoThetaValue(pCtrl);
}


float CalcStepAngle(s32 stepPerCircle, s32 numerator, s32 denominator)
{
	float stepAngle = 1.0f;
	if (stepPerCircle != 0)
	{
		stepAngle = (1.0f*MOTO_PPC/stepPerCircle);
	}
	else if (denominator != 0)
	{
		stepAngle = (1.0f*MOTO_PPC*numerator/denominator);
	}

	return stepAngle;
}
//------------------------------------------------------------------------
//------------------------------------------------------------------------------------

void MotoCloseCtrlProc(DriverCtrl * pCtrl)
{
	if (pCtrl == NULL)
	{
		return;
	}
	if (pCtrl->powerEnFlag <= 0)			// <20><>ʹ<EFBFBD><CAB9>״̬<D7B4><CCAC>
	{
		return;
	}

#if (OUT_INFO_NUM != 0)
	pCtrl->timeCount++;

	if (pCtrl->outInfoEn != 0)
	{
		if (pCtrl->timeGap <= 0)
		{
			pCtrl->timeGap = 1;
		}
		if ((pCtrl->timeCount % pCtrl->timeGap) == 0)
		{
			pCtrl->outInfoIdx++;
			if (pCtrl->outInfoIdx >= OUT_INFO_NUM)
			{
				pCtrl->outInfoEn = 0;
			}
		}
	}
#endif

	s32 targetMotoPos;		// <20><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>
	s32 setMotoPos;			// <20><><EFBFBD>μ<EFBFBD><CEBC><EFBFBD>ʵ<EFBFBD><CAB5>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>
	s32 effort;				// <20><><EFBFBD><EFBFBD>
	s32 tmps32;
	s32 thisactVel = 0;		// <20><><EFBFBD><EFBFBD>һ<EFBFBD>αȽϵĵ<CFB5><C4B5>ǶȲ<C7B6>ֵ
	// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
//	if (pCtrl->paras.closeCtrlMode != 0)
	{
		s32 rdTheta;		// <20><>ǰ<EFBFBD><C7B0><EFBFBD>Ƕ<EFBFBD>
		s32 thisThetaGap;	// <20><><EFBFBD>ǶȲ<C7B6>ֵ(<28><><EFBFBD><EFBFBD>һ<EFBFBD>αȽ<CEB1>)
		rdTheta = GetMotoThetaValue(pCtrl);		// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD>Ƕ<EFBFBD>

		if (pCtrl->first == 1)
		{
			pCtrl->oldTheta = rdTheta;
		}

		thisThetaGap = rdTheta - pCtrl->oldTheta;	// <20><><EFBFBD>ǶȲ<C7B6>ֵ
		// ת<><D7AA>Ϊ-180-180<38><30>֮<EFBFBD><D6AE>
		if (thisThetaGap < -MOTO_PPC/2)		// <20><>ת
		{
			thisThetaGap += MOTO_PPC;
		}
		else if (thisThetaGap > MOTO_PPC/2)
		{
			thisThetaGap -= MOTO_PPC;
		}

#if (DEF_SPD_FIL > 1)
		pCtrl->spdIdx++;
		if (pCtrl->spdIdx >= DEF_SPD_FIL)
		{
			pCtrl->spdIdx = 0;
		}
		pCtrl->spdSum -= pCtrl->spdBuff[pCtrl->spdIdx];
		pCtrl->spdBuff[pCtrl->spdIdx] = thisThetaGap;
		pCtrl->spdSum += thisThetaGap;
		thisactVel = pCtrl->spdSum/DEF_SPD_FIL;		// <20>ٶ<EFBFBD>ֵ
#else
		thisactVel = thisThetaGap;
#endif
		pCtrl->ecdPosition += thisThetaGap;

#if (OUT_INFO_NUM != 0)
		pCtrl->outInfoList[pCtrl->outInfoIdx].thisactVel = thisactVel;
		pCtrl->outInfoList[pCtrl->outInfoIdx].ecdPosition = pCtrl->ecdPosition;
#endif

		pCtrl->oldTheta = rdTheta;
	}

	// <20><>ȡָ<C8A1><D6B8>λ<EFBFBD><CEBB>
	{
		s32 thisCmdGap;	// <20><><EFBFBD>κ<EFBFBD><CEBA>ϴε<CFB4>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>ƫ<EFBFBD><C6AB>
		s32 rdCounter;	// Ŀ<><C4BF>λ<EFBFBD><CEBB>,ָ<><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		rdCounter = pCtrl->exCmdCounter;

		if (pCtrl->first == 1)
		{
			pCtrl->oldCounter = rdCounter;

			if (pCtrl->paras.closeCtrlMode != 0)		// <20>ջ<EFBFBD>ģʽ
			{
				pCtrl->cmdPosition = pCtrl->ecdPosition / pCtrl->stepAngle;
			}
		}

		thisCmdGap = rdCounter - pCtrl->oldCounter;
		if (thisCmdGap < -1*(s32)(PULSE_COUNTER_RELOAD/2))	// <20><>ת
		{
			thisCmdGap += PULSE_COUNTER_RELOAD;
		}
		else if (thisCmdGap > PULSE_COUNTER_RELOAD/2)
		{
			thisCmdGap -= PULSE_COUNTER_RELOAD;
		}
		pCtrl->oldCounter = rdCounter;

		// <20>ۼ<EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
		pCtrl->cmdPosition += thisCmdGap;

#if (OUT_INFO_NUM != 0)
		pCtrl->outInfoList[pCtrl->outInfoIdx].cmdPosition = pCtrl->cmdPosition;
#endif
	}

	targetMotoPos = (s32)(pCtrl->cmdPosition * pCtrl->stepAngle);			// <20><><EFBFBD><EFBFBD>λ<EFBFBD>ó<EFBFBD>ֵΪָ<CEAA><D6B8>λ<EFBFBD><CEBB>

#if (OUT_INFO_NUM != 0)
		pCtrl->outInfoList[pCtrl->outInfoIdx].targetMotoPos = targetMotoPos;
#endif

	if (pCtrl->paras.closeCtrlMode == 0)		// <20><><EFBFBD><EFBFBD>ģʽ
	{
		if (targetMotoPos == pCtrl->runPosition)
		{
			pCtrl->halfCounter++;
		}
		else
		{
			pCtrl->halfCounter = 0;

			// <20><><EFBFBD><EFBFBD>ģʽ<C4A3>µ<EFBFBD><C2B5>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>, <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>
			tmps32 = targetMotoPos - pCtrl->runPosition;
			if (tmps32 > pCtrl->paras.openMaxOffset)
			{
				targetMotoPos = pCtrl->runPosition + pCtrl->paras.openMaxOffset;
			}
			else if (tmps32 < -1*pCtrl->paras.openMaxOffset)
			{
				targetMotoPos = pCtrl->runPosition - pCtrl->paras.openMaxOffset;
			}
		}
		setMotoPos = targetMotoPos;

		if (pCtrl->halfCounter > LOW_CIRCLE_TIME)
		{
			pCtrl->halfCounter = LOW_CIRCLE_TIME;
			effort = pCtrl->paras.openLockCircle;	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		}
		else
		{
			effort = pCtrl->paras.openRunCircle;	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		}
	}
	else		// <20>ջ<EFBFBD><D5BB><EFBFBD><EFBFBD><EFBFBD>
	{
		// λ<>ñջ<C3B1>
		s32 pospid = 0;
		s32 thisPosErr = 0;		// λ<><CEBB>ƫ<EFBFBD><C6AB>(<28><><EFBFBD>Ƕ<EFBFBD>)
		{
			int index = 0;
			thisPosErr = targetMotoPos - pCtrl->ecdPosition;

			// <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>36<33><36>?
			if (thisPosErr > MAX_PE_LIMIT)				// <20><><EFBFBD><EFBFBD>ƫ<EFBFBD>Χ
			{
				thisPosErr = MAX_PE_LIMIT;
			}
			else if (thisPosErr < -MAX_PE_LIMIT)
			{
				thisPosErr = -MAX_PE_LIMIT;
			}

			// λ<><CEBB>PID
			// <20>ֶλ<D6B6><CEBB><EFBFBD>
			if ((thisPosErr >= 0 && pCtrl->lastPosErr <= 0) ||
				(thisPosErr <= 0 && pCtrl->lastPosErr >= 0)	)		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD><CFB4>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>෴
			{
				pCtrl->integral /= 128;		// <20><><EFBFBD>ֿ<EFBFBD><D6BF><EFBFBD><EFBFBD>½<EFBFBD>
			}

			if (abs(thisPosErr + pCtrl->lastPosErr) < 10)	// <20>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ӽ<EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD>û<EFBFBD><C3BB><EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD>ø<EFBFBD><C3B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			{
				index = IDX_RANGE;
				pCtrl->integral /= 16;		// 2 //  <20><><EFBFBD>ֿ<EFBFBD><D6BF><EFBFBD><EFBFBD>½<EFBFBD>
			}
			else if (abs(thisPosErr) > MAX_IS_LIMIT)
			{
				index = 0;
			}
			else
			{
				if (abs(thisPosErr) < MIN_IS_LIMIT)
				{
					index = IDX_RANGE;
				}
				else
				{
					index = ((MAX_IS_LIMIT) - abs(thisPosErr)) * IDX_RANGE / ((MAX_IS_LIMIT - MIN_IS_LIMIT));
				}

				// <20><><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>
				if (pCtrl->effort > EFF_MAX)
				{
					if (thisPosErr < 0)
					{
						pCtrl->integral += thisPosErr;
					}
				}
				else if (pCtrl->effort < EFF_MIN)
				{
					if (thisPosErr > 0)
					{
						pCtrl->integral += thisPosErr;
					}
				}
				else
				{
					pCtrl->integral += thisPosErr;
				}

				// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEB1A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
				if (pCtrl->integral > MAX_IG_LIMIT)
				{
					pCtrl->integral = MAX_IG_LIMIT;
				}
				else if (pCtrl->integral < -MAX_IG_LIMIT)
				{
					pCtrl->integral = -MAX_IG_LIMIT;
				}
			}

			// ΢<><CEA2><EFBFBD><EFBFBD>, ΢<><CEA2><EFBFBD>˲<EFBFBD>
			tmps32 = (thisPosErr - pCtrl->lastPosErr);
			pCtrl->differential = (pCtrl->differential * 26 + tmps32 * 230) / 256;

			pCtrl->lastPosErr = thisPosErr;			// <20><>¼<EFBFBD>ϴε<CFB4><CEB5><EFBFBD><EFBFBD><EFBFBD>

			// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>PID
			pospid = ( ( (pCtrl->paras.kp * thisPosErr)
					   + (pCtrl->paras.ki * pCtrl->integral * index / PID_RANGE)
					   + (pCtrl->paras.kd * pCtrl->differential) 	) 		/ PID_RANGE);
#if (OUT_INFO_NUM != 0)
			pCtrl->outInfoList[pCtrl->outInfoIdx].thisPosErr = thisPosErr;
			pCtrl->outInfoList[pCtrl->outInfoIdx].integral = pCtrl->integral;
			pCtrl->outInfoList[pCtrl->outInfoIdx].differential = pCtrl->differential;

			pCtrl->outInfoList[pCtrl->outInfoIdx].pospid = pospid;
#endif
		}

		// λ<><CEBB>ǰ<EFBFBD><C7B0>
		tmps32 = pCtrl->ecdPosition + pCtrl->paras.closeAdjust;	// <20>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>Ϊ<EFBFBD><CEAA>׼λ<D7BC><CEBB>

		s32 advance = thisactVel;		// <20>ٶ<EFBFBD><D9B6><EFBFBD>Ϊ ǰ<><C7B0><EFBFBD>Ƕ<EFBFBD>
		s32 basecircle = pCtrl->paras.basecircle;

#if (0)
		// 20240105 <20><>ʱ<EFBFBD>Ľ<EFBFBD>
		if (pospid > 200)
        {
			advance += (MAX_ADVANCE+MICO_ADVANCE+MIN_ADVANCE);
		}
        else if(pospid < -200)
        {
        	advance -= (MAX_ADVANCE+MICO_ADVANCE+MIN_ADVANCE);
		}
        else if (pospid > 100)
        {
			advance += (MAX_ADVANCE+MIN_ADVANCE);
		}
        else if(pospid < -100)
        {
        	advance -= (MAX_ADVANCE+MIN_ADVANCE);
		}
        else
#endif
        if (pospid > 50)
        {
			advance += (MAX_ADVANCE);		// 1.8<EFBFBD><EFBFBD>
		}
        else if(pospid < -50)
        {
        	advance -= (MAX_ADVANCE);
		}
        else if (pospid > 10)
        {
			advance += (MICO_ADVANCE);		// 0.36<EFBFBD><EFBFBD>
        }
        else if(pospid < -10)
        {
        	advance -= (MICO_ADVANCE);
        }
        else
        {
#if (0)// <20><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
        	static int flag = 0;
    		if (abs(advance) < MIN_ADVANCE)
    		{
    			if (flag == 0)
    			{
    				printf("pid=%d,A:%d\r\n", pospid, advance);
        			flag = 1;
    			}
            	basecircle = 0;
    			advance = 0;
    		}
    		else
    		{
    			flag = 0;
        	}
#else
    		if (abs(advance) <= MIN_ADVANCE)
    		{
            	basecircle = 0;
    			advance = 0;
    		}
#endif
        }

		setMotoPos = tmps32 + advance;
		// <20><><EFBFBD><EFBFBD>
#if (0) // 20240826
		effort = abs(pospid);
		if (effort > 4000)
		{
			effort /= 8;
		}
		else if (effort > 2000)
		{
			effort /= 6;
		}
		else if (effort > 800)
		{
			effort /= 4;
		}
		else if (effort > 200)
		{
			effort /= 2;
		}
		effort = basecircle + effort * pCtrl->paras.closeCircle / PID_RANGE;
#else
		effort = basecircle + abs(pospid)/8 * pCtrl->paras.closeCircle / PID_RANGE;
#endif
		// 20240109 ljs <20><><EFBFBD><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3>½<EFBFBD>
		if (effort < pCtrl->effort)
		{
			if (abs(effort - pCtrl->effort) > 10)
			{
				effort = pCtrl->effort - 10;
			}
		}
	}

	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	{
		SetMotoPosTo(pCtrl->motoIdx, setMotoPos, effort);	// <20><><EFBFBD>ߵ<EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>

		pCtrl->runPosition = setMotoPos;					// <20><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD>Ƕȣ<C7B6>
		pCtrl->effort = effort;

#if (OUT_INFO_NUM != 0)
		pCtrl->outInfoList[pCtrl->outInfoIdx].setMotoPos = setMotoPos;
		pCtrl->outInfoList[pCtrl->outInfoIdx].effort = effort;
#endif
	}
#if (0)
	if (pCtrl->first == 1)
	{
		printf("MotoCloseCtrlProc first run\r\n");
	}
#endif
	pCtrl->first = 0;
}

//------------------------------------------------------------------------

#define CPU_RATE	0	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

u32 g_workGap = 0;
float g_cpuRate = 0;

void CPURateIntProc(int section)
{
#if (CPU_RATE != 0)
#if (1)
	if (section == 0)
	{
		SetDebugOn();
	}
	else if (section == 1)
	{
		SetDebugOff();
	}
#else
	static u32 workBeg = 0;
	static u32 workEnd = 0;
	static u32 lastEnd = 0;

	if (section == 0)
	{
		SetDebugOn();
		workBeg = GetUsSoftTimer();
	}
	else if (section == 1)
	{
		workEnd = GetUsSoftTimer();
		g_workGap = workEnd - lastEnd;
		lastEnd = workEnd;
		if (g_workGap != 0)
		{
			g_cpuRate = (workEnd - workBeg) * 100.0 / g_workGap;
		}

		SetDebugOff();
	}
#endif
#endif
}

// <20><><EFBFBD><EFBFBD><EFBFBD>жϣ<D0B6> 10K
void DriversCtrlIntProc(void)
{
#if (1)
	// <20>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>Ƽ<EFBFBD><C6BC><EFBFBD>
	DrvMotionCalcProc();
#endif

#if (1)
	// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	DriverCtrl * pCtrl;
	for (int i = 0; i < DRIVERS_NUM; i++)
	{
		pCtrl = &g_driverCtrl[i];
		if (pCtrl->paras.cmdSource == 0)	// <20>ڲ<EFBFBD>ָ<EFBFBD><D6B8>
		{
			pCtrl->exCmdCounter = GetDrvMotionRealPos(i) + pCtrl->posOffset;	// <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>õ<EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
		}
		MotoCloseCtrlProc(pCtrl);
	}

#endif
}

//------------------------------------------------------------------------

u32 GetDrvErrorCode(int driverIdx)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return 0;
	}
	pCtrl = &g_driverCtrl[driverIdx];
	return pCtrl->errorCode;
}

void CleanDrvErrorCode(int driverIdx)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];
	pCtrl->errorCode = 0;
}

void SetDrvErrorCode(int driverIdx, int code)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];
	pCtrl->errorCode = code;
}

//--------------------------------------------------------------------------
#ifndef CLOSEMOTO_POSERR_MAXVAL
#define CLOSEMOTO_POSERR_MAXVAL		(MOTO_PPC*2)
#endif

void DriverRunTask(void)
{
	s32 poserr = 0;
	DriverCtrl * pCtrl;
	for (int i = 0; i < DRIVERS_NUM; i++)
	{
		pCtrl = &g_driverCtrl[i];
		if (pCtrl->errorCode == 0 &&
				pCtrl->paras.closeCtrlMode == CTRLMODE_CLOSE &&
				1)
		{
			// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			poserr = abs(pCtrl->cmdPosition*pCtrl->stepAngle - pCtrl->ecdPosition);
			if (poserr > CLOSEMOTO_POSERR_MAXVAL)
			{
				printf("driver %d pos error, cmdPosition=%d, ecdPosition=%d, sub=%d\r\n", i+1, pCtrl->cmdPosition, pCtrl->ecdPosition, poserr);
				pCtrl->errorCode = DR_ERR_POS;	// <20><><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD>
				SetEnableCtrl(i, 0);			// <20><>ʹ<EFBFBD><CAB9>
			}
		}
	}
}

//------------------------------------------------------------------------

// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
void ResetDriverPos(int driverIdx, s32 pos)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];

	printf("reset pos to=%d\r\n", pos);

	EnterCritical();
	s32 cpos = GetDrvMotionRealPos(driverIdx);
	SetDrvMotionRealPos(driverIdx, pos);
	s32 ofst = cpos - pos;
	pCtrl->posOffset += ofst;
	ExitCritical();
}

void SetDriverTargetPos(int driverIdx, s32 tpos)
{
	SetDrvMotionRealPos(driverIdx, tpos);
}

void SetDriverOffset(int driverIdx, s32 ofst)
{
	s32 cpos = GetDrvMotionRealPos(driverIdx);
	s32 tpos = cpos + ofst;
	SetDrvMotionRealPos(driverIdx, tpos);
}

//------------------------------------------------------------------------------------

void SetOpenRunCircle(int driverIdx, int circle)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];

	pCtrl->paras.openRunCircle = circle;
}

void SetOpenLockCircle(int driverIdx, int circle)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];

	pCtrl->paras.openLockCircle = circle;
}

void SetOpenMaxOffset(int driverIdx, int maxofst)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];

	pCtrl->paras.openMaxOffset = maxofst;
}

s32 GetClosePositionFromEcd(int driverIdx)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return 0;
	}
	pCtrl = &g_driverCtrl[driverIdx];
	return pCtrl->ecdPosition / pCtrl->stepAngle;
}

void SetCloseCircle(int driverIdx, int circle)
{
	DriverCtrl * pCtrl;
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM)
	{
		return;
	}
	pCtrl = &g_driverCtrl[driverIdx];

	pCtrl->paras.closeCircle = circle;
}

//------------------------------------------------------------------------------------

#define ANGLE_PER_CIRCLE 	36000	// 360 <20><>

s32 CalcAngleByEcdValue(s32 ecd)
{
	s32 angle = -1;
	float counter;
	counter = ecd;
	if (counter < 0)
	{
	}
	counter = (counter * ANGLE_PER_CIRCLE / MOTO_ECD_PER_CIRCLE);	// <20><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>

	angle = counter + (counter>0?(counter!=0?1:0):-1) * (0.500001f);

	// <20>Ƕ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD> [0, ANGLE_PER_CIRCLE)
	while (angle < 0)
	{
		angle += ANGLE_PER_CIRCLE;
	}
	while (angle >= ANGLE_PER_CIRCLE)
	{
		angle -= ANGLE_PER_CIRCLE;
	}
	return angle;
}

//------------------------------------------------------------------------------------


//------------------------------------------------------------------------------------

// <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ, 0<><30><EFBFBD><EFBFBD>	1<>ջ<EFBFBD>
void SetDriverCtrlMode(int driverIdx, int mode)
{
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM ||
			(mode != 0 && mode != 1) ||
			0)
	{
		return;
	}

	// <20><><EFBFBD>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥʹ<C8A5><CAB9>
	SetEnableCtrl(driverIdx, 0);
	int ctrlmod = g_driverCtrl[driverIdx].paras.closeCtrlMode;

	if (mode == 0)
	{
		printf("set moto%d ctrlmode to open\r\n", driverIdx+1);
	}
	else if (mode == 1)
	{
		printf("set moto%d ctrlmode to close\r\n", driverIdx+1);
	}

	g_driverCtrl[driverIdx].paras.closeCtrlMode = mode;

	if (ctrlmod != g_driverCtrl[driverIdx].paras.closeCtrlMode &&	// <20><><EFBFBD><EFBFBD>ģʽ<C4A3><CABD>ͬ
			g_driverCtrl[driverIdx].paras.closeCtrlMode != 0 &&		// <20>ջ<EFBFBD><D5BB><EFBFBD><EFBFBD><EFBFBD>
			1)
	{
		ResetMotoPos(driverIdx, 100);
		DelayMs(200);
		g_driverCtrl[driverIdx].InitEncoder();
		DelayMs(10);
	}

	DelayMs(1);

	SetEnableCtrl(driverIdx, 1);
}

//------------------------------------------------------------------------------------

void DlBlk(u32 dl)
{

}

// DRIVER
void TestDriver(char * para1, char * para2)
{
	int p1, p2;

	p1 = p2 = 0;
	if (para1 != NULL && strcmp(para1, "") != 0)
	{
		p1 = atoi(para1);
	}
	if (para2 != NULL && strcmp(para2, "") != 0)
	{
		p2 = atoi(para2);
	}
	if (p1 == p2)
	{
	}
	//--------------------------------
	DriverPara * pParas;

	if (p1 >= 0 && p1 < 500)
	{
		int i;
		int alllop = 0;
		int dridx = p1/100;
		int paidx = p1%100;

		int lopcount;
		int didxcount;

		if (dridx == 0)
		{
			if (p2 == 0)
			{
				printf("gap=%d, cpu rate=%.2f%%\r\n", g_workGap, g_cpuRate);
				alllop = DRIVERS_NUM;
			}
		}
		else
		{
			dridx -= 1;
		}

		lopcount = alllop;
		didxcount = dridx;
		do
		{
			if (didxcount >= 0 && didxcount < DRIVERS_NUM)
			{
				pParas = &(g_driverCtrl[didxcount].paras);

				if (paidx == 0)
				{
					printf("\r\nmoto%d paras:\r\n", didxcount+1);
					for (i = 0; i < DRV_PARA_NUMS; i++)
					{
						const char * parastr = GetDriverParaName(i);
						if (parastr != NULL && strlen(parastr) != 0)
						{
							printf("  %d. %s = %d\r\n", i+1, parastr, *((int*)(((u32)pParas) + i*4)));
						}
					}

					printf("\r\n g_driverCtrl[%d] values:\r\n"
							"\t cmdPosition=%d, \r\n"
							"\t ecdPosition=%d, \r\n"
							"\t runPosition=%d, \r\n",
							didxcount, g_driverCtrl[didxcount].cmdPosition, g_driverCtrl[didxcount].ecdPosition, g_driverCtrl[didxcount].runPosition);
				}
				else
				{
					// <20><><EFBFBD>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȥʹ<C8A5><CAB9>
					SetEnableCtrl(didxcount, 0);
					int ctrlmod = g_driverCtrl[dridx].paras.closeCtrlMode;

					printf("set %s = %d, old = %d\r\n", GetDriverParaName(paidx-1), p2, *((int*)(((u32)pParas) + (paidx-1)*4)));

					*((int*)(((u32)pParas) + (paidx-1)*4)) = p2;
					g_driverCtrl[dridx].stepAngle = CalcStepAngle(g_driverCtrl[dridx].paras.stepPerCircle, g_driverCtrl[dridx].paras.numerator, g_driverCtrl[dridx].paras.denominator);

					if (ctrlmod != g_driverCtrl[dridx].paras.closeCtrlMode &&
							g_driverCtrl[dridx].paras.closeCtrlMode != 0	)
					{
						ResetMotoPos(dridx, 50);
						DelayMs(200);
						g_driverCtrl[dridx].InitEncoder();
						DelayMs(10);
					}
					printf("moto%d stepAngle=%.2f\r\n", dridx+1, g_driverCtrl[dridx].stepAngle);

					DelayMs(1);

#if (OUT_INFO_NUM != 0)
					g_driverCtrl[dridx].outInfoIdx = 0;
					g_driverCtrl[dridx].timeGap = 0;
					g_driverCtrl[dridx].outInfoEn = 1;
#endif
					SetEnableCtrl(didxcount, 1);
				}
			}
			if (lopcount != 0)
			{
				lopcount--;
				didxcount++;
				DelayMs(100);
				continue;
			}
			else
			{
				break;
			}

		}while(1);
	}
#if (1)
	if (p1 >= 1000 && p1 < 5000)
	{// <20><><EFBFBD>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		int dridx = p1/1000 - 1;
		int paidx = p1%1000;

		if (dridx < 0 || dridx >= DRIVERS_NUM)
		{
			return;
		}

		int sel, count;
		sel = p2%10;
		count = p2/10;

		DriverCtrl * pCtrl;
		pCtrl = &(g_driverCtrl[dridx]);

		int rslt, arsult;
		int ang;

		SetEnableCtrl(dridx, 0);

		DelayMs(1);

		do
		{
			rslt = pCtrl->ReadEncoderCounter();
			ang = CalcAngleByEcdValue(rslt);
			printf("before run moto%d, rslt=%d(0x%x), ang=%.2f\r\n", dridx+1, rslt, rslt, ang/100.0);

			if (paidx == 0)
			{
				if (sel == 0)
				{
					ResetMotoPos(dridx, 0);
				}
				else if (sel == 1)
				{
					ResetMotoPos(dridx, 50);
				}
				else if (sel == 2)
				{
					MotoOneFullStep(dridx, 1, 50);
				}
				else if (sel == 3)
				{
					MotoOneFullStep(dridx, -1, 50);
				}
				else if (sel == 4)
				{
					MotoOneMicoStep(dridx, 1, 50);
				}
				else if (sel == 5)
				{
					MotoOneMicoStep(dridx, -1, 50);
				}
				else if (sel == 100)
				{
				}
			}
			DelayMs(50);
			arsult = pCtrl->ReadEncoderCounter();
			ang = CalcAngleByEcdValue(arsult);
			printf("after run moto%d, rslt=%d(0x%x), ang=%.2f\r\n", dridx+1, arsult, arsult, ang/100.0, arsult-rslt);
			count--;
		}while(count > 0);
	}
#endif
	if (p1 >= 10000 && p1 < 50000)
	{// <20><><EFBFBD>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD>˶<EFBFBD>
		int dridx = p1/10000 - 1;
		u32 timer;
		int rslt, arsult, ang;
		if (dridx < 0 || dridx >= DRIVERS_NUM)
		{
			return;
		}

		DriverCtrl * pCtrl;
		pCtrl = &(g_driverCtrl[dridx]);

		rslt = pCtrl->ReadEncoderCounter();
		ang = CalcAngleByEcdValue(rslt);
		printf("before run moto%d, rslt=%d(0x%x), ang=%.2f, sub=%d\r\n", dridx+1, rslt, rslt, ang/100.0);

		printf("test run start, idx=%d, ", dridx);

		timer = GetMsSoftTimer();

		{
			DrvMotionPara drpara;
			memset(&drpara, 0, sizeof(DrvMotionPara));

			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			drpara.movement = p2;			// λ<><CEBB><EFBFBD><EFBFBD>
			drpara.startPPS = pCtrl->paras.startPPS;	// <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
			drpara.runPPS = pCtrl->paras.runPPS;		// <20>˶<EFBFBD><CBB6>ٶ<EFBFBD>
			drpara.addPPSS = pCtrl->paras.addPPSS;		// <20><>ͣ<EFBFBD><CDA3><EFBFBD>ٶ<EFBFBD>
			drpara.brkPPSS = pCtrl->paras.brkPPSS;		// ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
			drpara.DelayWhenBlock = DlBlk;

			drpara.blockRunflag = 1;			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־, =1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,ͬ<><CDAC><EFBFBD>˶<EFBFBD>	=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C>첽<EFBFBD>˶<EFBFBD>

			DrvMotionStart(dridx, &drpara);		// <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
		}

		timer = GetMsSoftTimer() - timer;
		printf("test run over, time=%dms\r\n", timer);

		DelayMs(100);

		arsult = pCtrl->ReadEncoderCounter();
		ang = CalcAngleByEcdValue(arsult);
		printf("after run moto%d, rslt=%d(0x%x), ang=%.2f\r\n", dridx+1, arsult, arsult, ang/100.0, arsult-rslt);
	}

	if (p1 >= 100000 && p1 < 200000)
	{
		int dridx = 0;

		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		DrvMotionPara drpara;
		DriverCtrl * pCtrl;

		for (dridx = 0; dridx < DRIVERS_NUM; dridx++)
		{
			printf("test run start, idx=%d, ", dridx);
			pCtrl = &(g_driverCtrl[dridx]);

			memset(&drpara, 0, sizeof(DrvMotionPara));

			drpara.movement = p2;			// λ<><CEBB><EFBFBD><EFBFBD>
			drpara.startPPS = pCtrl->paras.startPPS;	// <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
			drpara.runPPS = pCtrl->paras.runPPS;		// <20>˶<EFBFBD><CBB6>ٶ<EFBFBD>
			drpara.addPPSS = pCtrl->paras.addPPSS;		// <20><>ͣ<EFBFBD><CDA3><EFBFBD>ٶ<EFBFBD>
			drpara.brkPPSS = pCtrl->paras.brkPPSS;		// ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>

			drpara.funTestTime = 0; 					// <20><><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

			drpara.blockRunflag = 0;			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־, =1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,ͬ<><CDAC><EFBFBD>˶<EFBFBD>	=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C>첽<EFBFBD>˶<EFBFBD>

			DrvMotionStart(dridx, &drpara);		// <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
		}

		do
		{
			printf("gap=%d, cpu rate=%.2f%%\r\n", g_workGap, g_cpuRate);
			if (IsConsoleCancel() != 0)
			{
				break;
			}

			DelayMs(10);

		}while(1);
	}

	if (p1 >= 200000 && p1 <= 240000)
	{
#if (OUT_INFO_NUM != 0)
		int dridx = (p1-200000)/10000;
		if (dridx < 0 || dridx >= DRIVERS_NUM)
		{
			return;
		}

		DriverCtrl * pCtrl;
		pCtrl = &(g_driverCtrl[dridx]);
		int gap = p1 % 10000;

		if (p2 < 0)
		{
			int i = 0;
			int num = pCtrl->outInfoIdx;
			printf("out info num=%d\r\n", num);

			for (i = 1; i < num; i++)
			{
				printf(	"%d "
						"%d "
						"%d "
						"%d "
						"%d "
						"%d "
						"%d "
						"%d "

						"%d "
						"%d "
						"%d "
						"\r\n"
						,i
						,pCtrl->outInfoList[i].cmdPosition		// ָ<><D6B8>λ<EFBFBD><CEBB>
						,pCtrl->outInfoList[i].targetMotoPos	// Ŀ<><C4BF>λ<EFBFBD><CEBB>
						,pCtrl->outInfoList[i].ecdPosition		// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
						,pCtrl->outInfoList[i].thisPosErr		// λ<><CEBB>ƫ<EFBFBD><C6AB>
						,pCtrl->outInfoList[i].integral			// ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
						,pCtrl->outInfoList[i].differential		// λ<><CEBB>΢<EFBFBD><CEA2>
						,pCtrl->outInfoList[i].pospid			// λ<><CEBB>PID
						,pCtrl->outInfoList[i].thisactVel		// <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>

						,pCtrl->outInfoList[i].setMotoPos		// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
						,pCtrl->outInfoList[i].effort			// <20><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>
				);

				if (IsConsoleCancel() != 0)
				{
					break;
				}
				DelayMs(10);
			}
		}
		else
		{
			pCtrl->outInfoIdx = 0;
			pCtrl->timeGap = gap;
			pCtrl->outInfoEn = 1;

			printf("test out begin, idx=%d\r\n", dridx);

			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
			DrvMotionPara drpara;
			memset(&drpara, 0, sizeof(DrvMotionPara));

			drpara.movement = p2;			// λ<><CEBB><EFBFBD><EFBFBD>
			drpara.startPPS = pCtrl->paras.startPPS;	// <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
			drpara.runPPS = pCtrl->paras.runPPS;		// <20>˶<EFBFBD><CBB6>ٶ<EFBFBD>
			drpara.addPPSS = pCtrl->paras.addPPSS;		// <20><>ͣ<EFBFBD><CDA3><EFBFBD>ٶ<EFBFBD>
			drpara.brkPPSS = pCtrl->paras.brkPPSS;		// ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>

			drpara.blockRunflag = 0;			// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>־, =1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,ͬ<><CDAC><EFBFBD>˶<EFBFBD>	=0,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C>첽<EFBFBD>˶<EFBFBD>

			DrvMotionStart(dridx, &drpara);		// <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>

			do
			{
				if (pCtrl->outInfoEn == 0)
				{
					break;
				}

				if (IsConsoleCancel() != 0)
				{
					break;
				}
			}while(1);

			printf("test out end\r\n");
		}
#endif
	}

}

void ShowDriver(char * para1, char * para2)
{
	int p1, p2;

	p1 = p2 = 0;
	if (para1 != NULL && strcmp(para1, "") != 0)
	{
		p1 = atoi(para1);
	}
	if (para2 != NULL && strcmp(para2, "") != 0)
	{
		p2 = atoi(para2);
	}
	if (p1 == p2)
	{
	}

	if (p1 == 1)
	{
		if (p2 > 0 && p2 <= DRIVERS_NUM)
		{
			printf("%d.cmdPosition=%d, ecdPosition=%d, runPosition=%d\r\n",
					p2, g_driverCtrl[p2-1].cmdPosition, g_driverCtrl[p2-1].ecdPosition, g_driverCtrl[p2-1].runPosition);
		}
	}
}

#endif