#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;	// 指令位置
	s32 targetMotoPos;	// 目标位置
	s32 ecdPosition;	// 编码器位置

	s32 thisPosErr;		// 位置偏差
	s32 integral;		// 偏差积分
	s32 differential;	// 位置微分
	s32 thisactVel;		// 本次检测速度值

	s32 pospid;			// 位置PID

	s32 setMotoPos;		// 设置位置
	s32 effort;			// 设置电流

}DrvOutInfo;

#ifndef DEF_SPD_FIL
#define DEF_SPD_FIL		0
#endif

typedef struct
{
	//----------------------
	int motoIdx;					// 电机索引

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

	//----------------------
	DriverPara paras;		// 参数

	//----------------------
	int powerEnFlag;		// 使能标志
	//----------------------
	float stepAngle;		// 细分
	//----------------------
	s32 runspeed;			// 运行速度
	s32 speedadd;			// 内部运行加速度
	int runsta;
	//----------------------
	s32 cmdPosition;		// 指令位置统计（电角度）
	s32 ecdPosition;		// 编码器位置统计（电角度）
	s32 runPosition;		// 运行位置统计（电角度）

	s32 lastPosErr;			// 上次的偏差(电角度)
	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;			// 上上次的偏差

	s32 intev;
	s32 effort;				// 输出电流
	//----------------------
	s32 lastThetaGap;
	//----------------------
	s32 exCmdCounter;		// 外部指令计数
	//----------------------
	s32 posOffset;
	//----------------------

	int first;
	s32 oldTheta;			// 上次的角度(电角度)
	s32 oldCounter;			// 上次的指令计数
	s32 halfCounter;		// 开环半流计数器
	//----------------------
	int errorCode;			// 错误代码
	//----------------------
#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""测试电机驱动" 	"\r\n"
	"\t" "p1		p2" "\r\n"
	"\t" "0		x" "显示x驱动参数, 0 全部显示" "\r\n"
	"\t""x0d	n" "设置x驱动参数d的值为n" "\r\n"
	"\t""x000	m" "开环步进方式运动x电机" "\r\n"
	"\t\t"	"m=0" "失能" "\r\n"
	"\t\t"	"m=1" "使能" "\r\n"
	"\t\t"	"m=2" "正向整步" "\r\n"
	"\t\t"	"m=3" "负向整步" "\r\n"
	"\t\t"	"m=4" "正向微步" "\r\n"
	"\t\t"	"m=5" "负向微步" "\r\n"
	"\t" "x000d	n" "驱动器方式运动x电机,位移为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 参数分母

#define	PULSE_COUNTER_RELOAD	(S32_MAX-1)			// 外部脉冲计数器重装载值

#define	LOW_CIRCLE_TIME			2000						// 低电流锁定时间 (2k)
#define	ADVANCE_ADJ_MAX			((MOTO_PPC)/MOTO_NOPP/5)		// 调整偏差

#define	MIN_PE_LIMIT			((MOTO_PPC)/MOTO_NOPP/32)		// 最小偏差允许
#define	MAX_PE_LIMIT			(MOTO_PPC/10)					// 最大偏差限制

#define MAX_IG_LIMIT  			(RD_CALIB_PPC*PID_RANGE)	// 积分项目饱和限制

#define MAX_IS_LIMIT  			(MAX_IG_LIMIT*90/100)		// 积分分段最大限制
#define MIN_IS_LIMIT  			(MAX_IG_LIMIT*50/100)		// 积分分段最小限制

#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); /* 设置方向 */ \
 \
	SetMoto##id##CoilPwmEnable(); /* 允许PWM输出 */  \
 \
	g_driverCtrl[id-1].ReadEncoderCounter = ReadMoto##id##EncoderCounter; \
	g_driverCtrl[id-1].InitEncoder = InitMoto##id##Encoder; \
}while(0)

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

void InitDriversCtrl(void)
{
	int i;

	// 电机初始化
	InitStepMotos();		// 初始化步进电机控制

	InitDrvMotions();		// 初始化内置运动控制

	// 驱动控制
	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

	// 装载默认参数
	InitDriverParas();

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

		// 从参数文件装载参数值
		memcpy(&(g_driverCtrl[i].paras), GetDriverParas(i), sizeof(DriverPara));

		// 计算细分
		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);

		// 重置默认参数

	}

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

	//-----------------------
	// 闭环控制初始位置
	for (i = 0; i < DRIVERS_NUM; i++)
	{
		SetEnableCtrl(i, 0);
	}

	DelayMs(10);

	// 开始时固定在整步位置
	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);

	// 初始化编码器计数（根据固定整步位置）
	for (i = 0; i < DRIVERS_NUM; i++)
	{
		g_driverCtrl[i].InitEncoder();
	}

	DelayMs(10);

	// 使能电机
	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

	// 打开控制周期，开始控制
	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;		// 指令位置统计（电角度）
	pCtrl->ecdPosition = rdTheta;		// 编码器位置统计（电角度）
	pCtrl->runPosition = rdTheta;		// 运行位置统计（电角度）

	pCtrl->lastPosErr = 0;			// 上次的偏差
	pCtrl->integral = 0;			// 积分
//	pCtrl->lastDErr = 0;			// 上上次的偏差
	pCtrl->effort = 0;				// 输出电流

	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);			// 行走到目标位置
		}
		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)	// 简化计算, 编码器和正弦表数量相同
	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)			// 非使能状态下
	{
		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;		// 理论目标位置
	s32 setMotoPos;			// 本次计算实际目标位置
	s32 effort;				// 电流
	s32 tmps32;
	s32 thisactVel = 0;		// 和上一次比较的电角度差值
	// 读取编码器位置
//	if (pCtrl->paras.closeCtrlMode != 0)
	{
		s32 rdTheta;		// 当前电角度
		s32 thisThetaGap;	// 电角度差值(和上一次比较)
		rdTheta = GetMotoThetaValue(pCtrl);		// 读取编码器, 转换为电角度

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

		thisThetaGap = rdTheta - pCtrl->oldTheta;	// 电角度差值
		// 转换为-180-180度之间
		if (thisThetaGap < -MOTO_PPC/2)		// 反转
		{
			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;		// 速度值
#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;
	}

	// 读取指令位置
	{
		s32 thisCmdGap;	// 本次和上次的指令位置偏差
		s32 rdCounter;	// 目标位置,指令计数
		rdCounter = pCtrl->exCmdCounter;

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

			if (pCtrl->paras.closeCtrlMode != 0)		// 闭环模式
			{
				pCtrl->cmdPosition = pCtrl->ecdPosition / pCtrl->stepAngle;
			}
		}

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

		// 累计指令位置
		pCtrl->cmdPosition += thisCmdGap;

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

	targetMotoPos = (s32)(pCtrl->cmdPosition * pCtrl->stepAngle);			// 计算位置初值为指令位置

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

	if (pCtrl->paras.closeCtrlMode == 0)		// 开环模式
	{
		if (targetMotoPos == pCtrl->runPosition)
		{
			pCtrl->halfCounter++;
		}
		else
		{
			pCtrl->halfCounter = 0;

			// 开环模式下的速度限制, 防止丢步
			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;	// 锁定电流
		}
		else
		{
			effort = pCtrl->paras.openRunCircle;	// 驱动电流
		}
	}
	else		// 闭环控制
	{
		// 位置闭环
		s32 pospid = 0;
		s32 thisPosErr = 0;		// 位置偏差(电角度)
		{
			int index = 0;
			thisPosErr = targetMotoPos - pCtrl->ecdPosition;

			// 最大偏差36度?
			if (thisPosErr > MAX_PE_LIMIT)				// 限制偏差范围
			{
				thisPosErr = MAX_PE_LIMIT;
			}
			else if (thisPosErr < -MAX_PE_LIMIT)
			{
				thisPosErr = -MAX_PE_LIMIT;
			}

			// 位置PID
			// 分段积分
			if ((thisPosErr >= 0 && pCtrl->lastPosErr <= 0) ||
				(thisPosErr <= 0 && pCtrl->lastPosErr >= 0)	)		// 本次与上次运动方向相反
			{
				pCtrl->integral /= 128;		// 积分快速下降
			}

			if (abs(thisPosErr + pCtrl->lastPosErr) < 10)	// 是否可以理解为接近目标位置或在目标位置附近抖动
			{
				index = IDX_RANGE;
				pCtrl->integral /= 16;		// 2 //  积分快速下降
			}
			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));
				}

				// 抗积分饱和
				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;
				}

				// 积分项饱和限制
				if (pCtrl->integral > MAX_IG_LIMIT)
				{
					pCtrl->integral = MAX_IG_LIMIT;
				}
				else if (pCtrl->integral < -MAX_IG_LIMIT)
				{
					pCtrl->integral = -MAX_IG_LIMIT;
				}
			}

			// 微分项, 微分滤波
			tmps32 = (thisPosErr - pCtrl->lastPosErr);
			pCtrl->differential = (pCtrl->differential * 26 + tmps32 * 230) / 256;

			pCtrl->lastPosErr = thisPosErr;			// 记录上次的误差

			// 计算位置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
		}

		// 位置前馈
		tmps32 = pCtrl->ecdPosition + pCtrl->paras.closeAdjust;	// 以编码器位置为基准位置

		s32 advance = thisactVel;		// 速度作为 前馈角度
		s32 basecircle = pCtrl->paras.basecircle;

#if (0)
		// 20240105 临时改进
		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度
		}
        else if(pospid < -50)
        {
        	advance -= (MAX_ADVANCE);
		}
        else if (pospid > 10)
        {
			advance += (MICO_ADVANCE);		// 0.36度
        }
        else if(pospid < -10)
        {
        	advance -= (MICO_ADVANCE);
        }
        else
        {
#if (0)// 临时测试使用
        	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;
		// 电流
#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 电流延迟下降
		if (effort < pCtrl->effort)
		{
			if (abs(effort - pCtrl->effort) > 10)
			{
				effort = pCtrl->effort - 10;
			}
		}
	}

	// 电流控制
	{
		SetMotoPosTo(pCtrl->motoIdx, setMotoPos, effort);	// 行走到目标位置

		pCtrl->runPosition = setMotoPos;					// 记录电机位置（电角度）
		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	// 测试利用率

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
}

// 控制中断， 10K
void DriversCtrlIntProc(void)
{
#if (1)
	// 运动控制计算
	DrvMotionCalcProc();
#endif

#if (1)
	// 电机控制
	DriverCtrl * pCtrl;
	for (int i = 0; i < DRIVERS_NUM; i++)
	{
		pCtrl = &g_driverCtrl[i];
		if (pCtrl->paras.cmdSource == 0)	// 内部指令
		{
			pCtrl->exCmdCounter = GetDrvMotionRealPos(i) + pCtrl->posOffset;	// 更新运动控制位置到指令位置
		}
		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)
		{
			// 检测位置误差过大错误
			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;	// 出现错误
				SetEnableCtrl(i, 0);			// 断使能
			}
		}
	}
}

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

// 重置驱动器位置
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 度

s32 CalcAngleByEcdValue(s32 ecd)
{
	s32 angle = -1;
	float counter;
	counter = ecd;
	if (counter < 0)
	{
	}
	counter = (counter * ANGLE_PER_CIRCLE / MOTO_ECD_PER_CIRCLE);	// 计算角度

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

	// 角度值在区间 [0, ANGLE_PER_CIRCLE)
	while (angle < 0)
	{
		angle += ANGLE_PER_CIRCLE;
	}
	while (angle >= ANGLE_PER_CIRCLE)
	{
		angle -= ANGLE_PER_CIRCLE;
	}
	return angle;
}

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


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

// 设置电机控制模式, 0开环	1闭环
void SetDriverCtrlMode(int driverIdx, int mode)
{
	if (driverIdx < 0 || driverIdx >= DRIVERS_NUM ||
			(mode != 0 && mode != 1) ||
			0)
	{
		return;
	}

	// 设置参数，先去使能
	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 &&	// 控制模式不同
			g_driverCtrl[driverIdx].paras.closeCtrlMode != 0 &&		// 闭环控制
			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
				{
					// 设置参数，先去使能
					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)
	{// 测试电机编码器
		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)
	{// 测试电机运动
		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));

			// 输入参数
			drpara.movement = p2;			// 位移量
			drpara.startPPS = pCtrl->paras.startPPS;	// 启动速度
			drpara.runPPS = pCtrl->paras.runPPS;		// 运动速度
			drpara.addPPSS = pCtrl->paras.addPPSS;		// 启停加速度
			drpara.brkPPSS = pCtrl->paras.brkPPSS;		// 刹车加速度
			drpara.DelayWhenBlock = DlBlk;

			drpara.blockRunflag = 1;			// 阻塞运行标志, =1阻塞运行,同步运动	=0,非阻塞,异步运动

			DrvMotionStart(dridx, &drpara);		// 启动运动控制
		}

		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;

		// 输入参数
		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;			// 位移量
			drpara.startPPS = pCtrl->paras.startPPS;	// 启动速度
			drpara.runPPS = pCtrl->paras.runPPS;		// 运动速度
			drpara.addPPSS = pCtrl->paras.addPPSS;		// 启停加速度
			drpara.brkPPSS = pCtrl->paras.brkPPSS;		// 刹车加速度

			drpara.funTestTime = 0; 					// 运行过程中外部函数检测间隔时间计数器

			drpara.blockRunflag = 0;			// 阻塞运行标志, =1阻塞运行,同步运动	=0,非阻塞,异步运动

			DrvMotionStart(dridx, &drpara);		// 启动运动控制
		}

		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		// 指令位置
						,pCtrl->outInfoList[i].targetMotoPos	// 目标位置
						,pCtrl->outInfoList[i].ecdPosition		// 编码器位置
						,pCtrl->outInfoList[i].thisPosErr		// 位置偏差
						,pCtrl->outInfoList[i].integral			// 偏差积分
						,pCtrl->outInfoList[i].differential		// 位置微分
						,pCtrl->outInfoList[i].pospid			// 位置PID
						,pCtrl->outInfoList[i].thisactVel		// 本次速度

						,pCtrl->outInfoList[i].setMotoPos		// 设置位置
						,pCtrl->outInfoList[i].effort			// 设置电流
				);

				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);

			// 输入参数
			DrvMotionPara drpara;
			memset(&drpara, 0, sizeof(DrvMotionPara));

			drpara.movement = p2;			// 位移量
			drpara.startPPS = pCtrl->paras.startPPS;	// 启动速度
			drpara.runPPS = pCtrl->paras.runPPS;		// 运动速度
			drpara.addPPSS = pCtrl->paras.addPPSS;		// 启停加速度
			drpara.brkPPSS = pCtrl->paras.brkPPSS;		// 刹车加速度

			drpara.blockRunflag = 0;			// 阻塞运行标志, =1阻塞运行,同步运动	=0,非阻塞,异步运动

			DrvMotionStart(dridx, &drpara);		// 启动运动控制

			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