//-------------------------------------------------------------------------------
// File Name:	sewhead.c
// Brief:
// Version:		1.0.0
// Create Date: 2024/10/11
// Create by: 	W.X
// Copyright:
//				TIANJIN GETONAGAIN TECHNOLOGY CO., LIMITED
//				All rights reserved.
//
// Modify by:	W.X
// Modify Date:	2024/10/17
//-------------------------------------------------------------------------------

#define	_IN_HEADBOARD_C

#include "sewhead.h"
#if (BOARD_USE_FOR == SEW_HEAD)

#include "trigger.h"
#include "inout.h"
#include "debug.h"
#include "workctrl.h"
#include "shell.h"
#include "delay.h"
#include "modbus_def.h"
#include "modbus_s.h"
#include "modbus_m.h"
#include "canwork.h"
#include "para.h"

typedef void (*PwmFunc)(void);
PwmFunc D29PwmProc = NULL;

void RefreshOutputSta(void);
void OutputCtrl(u8 num, u8 onof);
void PwmCtrlTask(void);
void InitSTM32PwmTimer(void);
void InitSoftPwmCtrl(PwmFunc proc);
void SoftPwmIntProc(void);
void InOutStaRefreshTask(void);

#if(CONTROL_BUS == COMM_485)
void InOutWorkTask(Rs485Cmd * pCmd);
#endif

void ModBusCmdProcTask(Rs485Cmd * pCmd);
void RefreshElasticMoto(void);
void TopThreadBreakCheck(void);
void ModBusReceiveProc(Rs485Cmd *cmd);
int MotoMoveAsOfst(int mode, int ofst, spdCtrl *spd, int block);
void FootMotoDis(void);
void FootMotoEn(void);
void FollowEcdOutPulseOn(void);
void FollowEcdOutPulseOff(void);
void FollowEcdOutDirOn(void);
void FollowEcdOutDirOff(void);
void FootMotoToZero(void);
void CheckParasValue(void);
void PFootCtrlUp(void);
void PFootCtrlDown(void);
void PFootCtrlBack(void);
void SetMotoPosValue(u16 pos,u16 value);

void TestAction(char * para1, char * para2);

#define	CHK_TIME_GAP		10		// 发送时间间隔, 单位 100us
#define	MODRSV_TIMEOUT		820		// 读取超时时间, 单位为 CHK_TIME_GAP * 100us
#define	MODSEND_GAP			50		// 发送和读取的间隔, 单位为 CHK_TIME_GAP * 100us
//-------------------------------------------------------------------------------

#define	BUF_TAB_SIZE		1000		// 缓冲区大小

typedef struct
{
	u8 boardId;
	u8 outputSta;
	u8 inputSta;
	u8 outputCtrl;
	u8 pwmOutputFlag;
	u8 softLockSta;
//	u16 d29cmdReg[MODBUS_REG_MAX-MODBUS_REG_MIN];

	u16 pwmCtrl;		// 输出占空比 0~100
	u16 pwmFreq;		// 定时器PWM频率
	u16 pwmCount;
	u32 timeCount;
	u32 pwmPulsTime;
	u32 startTime;		// 启动时间
	u32 chgTime;		// 变化次数
	u32 cmprbuf;		// 分频系数缓存
	int workingSta;

	u32 tabIdx;						// 缓存表索引
	u32 chgTimeTab[BUF_TAB_SIZE];	// 变更次数缓存表
	u32 curTimeTab[BUF_TAB_SIZE];	// 变更次数缓存表
	u8 pwmTab[BUF_TAB_SIZE];		// PWM缓存缓存表

	u8 outsta;
	u8 outOtFlag;
	u8 workFlag;

	u16 enable;
	u16 adcvalue;
	u16 thrdBreakFlag;
	u16 mtWkSta[8];
	u32 outTime;

	u32 enflag;
	u32 stopAngle;
	u32 encoderValue;
	s32 footMtPos;

	u32 followRunNum;
	int followStartPPos;
	int followStartNPos;
	u32 followConPAng;
	u32 followConNAng;

	u32 pfootFlLowPos;

	u32 followEcdEn;
	u32 followCountr;
	u32 followStepNum;
	u32 followNumLow;
	u32 followNumHigh;
	int followTableIdx;
	int followEcdDir;
	int followEcdCheck;
	int followChangeFlag;
	u32 followChangeNum;
	int setParasFlag;
	u32 setParasTime;
	int pfootSta;

	u32 pfootRunHighPos;
	u32 pfootFlowPAng;			// 压脚跟踪上升位
	u32 pfootFlowNAng;			// 压脚跟踪下降位
	u32 pfootFlowPuls;			// 压脚跟踪脉冲数
	u32 pfootRunLowPos;			// 压脚运行低位
	u32 pfootRunBackPos;		// 压脚运行回针位
	s32 ecOfstValue;			// 编码器差值
	u32 pfootRunConAng;			// 跟随持续角度
	//------------
	void (*FollowMotoPulseOn)(void);
	void (*FollowMotoPulseOff)(void);
	void (*FollowMotoSignOn)(void);
	void (*FollowMotoSignOff)(void);
	//------------

	u32 thrdBreakCount;
	u32 motocoordinate;
	u16 headCmdReg[MODBUS_REG_MAX-MODBUS_REG_MIN];
}HeadCtrl;

HeadCtrl g_headCtrl;

//-------------------------------------------------------------------------------
u8 GetBoardSwitchValue(void)
{
	u8 val = 0;
	if(GetSwitch1Status() == SENSOR_ON)
	{
		val |= 0x01;
	}
	if(GetSwitch2Status() == SENSOR_ON)
	{
		val |= 0x02;
	}
	if(GetSwitch3Status() == SENSOR_ON)
	{
		val |= 0x04;
	}
	if(GetSwitch4Status() == SENSOR_ON)
	{
		val |= 0x08;
	}
	return val;
}

// 初始化
void InitSewHeadBoard(void)
{
	memset(&g_headCtrl, 0, sizeof(HeadCtrl));
	g_headCtrl.boardId = GetBoardSwitchValue();

	if((g_headCtrl.boardId & 0x04) == 0)
	{
		InitSoftPwmCtrl(PwmCtrlTask);
#if(PWM_TIMER_CTRL == 1)
		g_headCtrl.pwmOutputFlag = 1;
#endif
		printf("initsoftpwmctrl.");
	}

#if(CONTROL_BUS == COMM_485)
#if(COM_485_RTU == 1)
	InitModbusS(COMM_USART2, B115200, '8', 'N', '1','R');	// 初始化485
#else
	if((g_headCtrl.boardId & 0x08) != 0)
	{
		InitModbusS(COMM_USART2, B115200, '8', 'N', '1','R');	// 初始化485
	}
	else
	{
		InitModbusS(COMM_USART2, B115200, '8', 'N', '1','A');	// 初始化485
	}
#endif
	//InitModbusS(COMM_USART1,B115200, '9', 'E', '1', 'A');  // 串口3,115200波特率
	InitModbusReg(MODBUS_REG_MIN, &g_headCtrl.d29cmdReg[0], (MODBUS_REG_MAX-MODBUS_REG_MIN));
	InitModbusSCtrl(D29_BOARD_ID,30);	// 配置modbus(收包超时时间:2ms)

	RegModbusSCmdProc(ModBusCmdProcTask);	// 外部函数

#endif
	InitModbus_m(0,COMM_USART3, B115200, '8', 'N', '1','R');	// 初始化485
	InitRs485Ctrl(MODRSV_TIMEOUT);
	RegRsCmdProc(ModBusReceiveProc);

	CheckParasValue();
	g_headCtrl.pfootFlLowPos = g_headCtrl.pfootRunLowPos;
	g_headCtrl.enflag = EN_FALG;
	g_headCtrl.FollowMotoPulseOn = FollowEcdOutPulseOn;
	g_headCtrl.FollowMotoPulseOff = FollowEcdOutPulseOff;
	g_headCtrl.FollowMotoSignOn = FollowEcdOutDirOn;
	g_headCtrl.FollowMotoSignOff = FollowEcdOutDirOff;
	DelayRef(500);
	FootMotoToZero();
	SetEn485OutOff();

	AddShellCmd("KEY", "test action", TestAction);
}


void InitSTM32PwmTimer(void)
{
#if(PWM_TIMER_CTRL == 1)

#else
	NVIC_InitTypeDef NVIC_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef TIM_TimeOCStructure;
/* 中断配置 */
	NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;					/* 通道 */
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;			/* 抢占式中断优先级 */
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; 					/* 响应式中断优先级 */
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;						/* 使能中断 */
	NVIC_Init(&NVIC_InitStructure);
/*IO配置*/
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_APB2PeriphClockCmd(OUT7_RCC_APB2Periph, ENABLE);

	GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3,ENABLE);

	GPIO_InitStructure.GPIO_Pin	= OUT6_GPIO_Pin | OUT7_GPIO_Pin;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;

//	GPIO_InitStructure.GPIO_Pin	= OUT7_GPIO_Pin;
//	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;

	GPIO_Init(OUT7_GPIO_PORT, &GPIO_InitStructure);

///* 定时器配置 */
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);				/* 启动定时器时钟 */
	TIM_DeInit(TIM3);												/* 重新将Timer设置为缺省值 */
///* 时基配置 */
	TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
	TIM_TimeBaseStructure.TIM_Prescaler = (72-1);				/* 预分频 */
	TIM_TimeBaseStructure.TIM_Period = (3334-1);				/* 自动装载值  500us中断*/
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;				/* 采样时钟 */
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;			/* 向上计数模式 */
	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);				/* 初始化TIMx */

	TIM_TimeOCStructure.TIM_OCMode = TIM_OCMode_PWM2;
	TIM_TimeOCStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_TimeOCStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_TimeOCStructure.TIM_Pulse = 0;

	TIM_OC1Init(TIM3,&TIM_TimeOCStructure);
	TIM_OC2Init(TIM3,&TIM_TimeOCStructure);

	TIM_CtrlPWMOutputs(TIM3,DISABLE);

	TIM_OC1PreloadConfig(TIM3,ENABLE);
	TIM_OC2PreloadConfig(TIM3,ENABLE);
	TIM_ARRPreloadConfig(TIM3,ENABLE);

	//TIM_ClearFlag(TIM3, TIM_FLAG_Update);							/* 清除溢出中断标志 */
	//TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);						/* 开启中断 */
	TIM_Cmd(TIM3, ENABLE);											/* 开启定时器 */

#endif
}

void SoftPwmIntProc(void)
{
#if(PWM_TIMER_CTRL == 1)
	if (D29PwmProc != NULL)
	{
		D29PwmProc();
	}
#else
	if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
	{
		TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
		if (D29PwmProc != NULL)
		{
			D29PwmProc();
		}
	}
#endif
}

void InitSoftPwmCtrl(PwmFunc proc)
{
//	InitSTM32PwmTimer();
#if(PWM_TIMER_CTRL == 1)
//	Tim1IntProc = SoftPwmIntProc;
	D29PwmProc = proc;
	g_headCtrl.pwmFreq = 100;
	g_headCtrl.pwmCount = 0;
	g_headCtrl.pwmCtrl = 0;			// 初始占空比为0
#else
	Tim3IntProc = SoftPwmIntProc;
	D29PwmProc = proc;
	g_headCtrl.pwmCtrl = 0;
	g_headCtrl.pwmFreq = 3334;
#endif
}

void PwmCtrlTask(void)
{
#if(PWM_TIMER_CTRL == 1)
	if(g_headCtrl.pwmCount < g_headCtrl.pwmCtrl)
	{
		g_headCtrl.pwmCount ++;
//		if((g_headCtrl.pwmIOSta & 0x20) != 0)
		{
			SetOutputPWM1On();
			SetOutputPWM2On();
		}
	}
	else if(g_headCtrl.pwmCount < g_headCtrl.pwmFreq)
	{
		g_headCtrl.pwmCount ++;
		if(g_headCtrl.pwmCount >= g_headCtrl.pwmFreq)
		{
			g_headCtrl.pwmCount = 0;
			if(g_headCtrl.timeCount != 0)
			{
				if(g_headCtrl.timeCount == 1)
				{
					g_headCtrl.pwmPulsTime = GetUsSoftTimer();
				}

				g_headCtrl.timeCount ++;
				if(g_headCtrl.timeCount >= 11)
				{
					g_headCtrl.timeCount = 0;
//					printf("puls Freq:%d.\r\n",10000000/(GetUsSoftTimer() - g_headCtrl.pwmPulsTime));
				}
			}
		}
		SetOutputPWM1Off();
		SetOutputPWM2Off();
	}
	else
	{
		g_headCtrl.pwmCount = 0;
//		SetOutput6Off();
//		SetOutput7Off();
	}

//	g_headCtrl.timeCount ++;
//	if(g_headCtrl.timeCount > 20000)
//	{
//		u32 ntime = 0;
//		ntime = GetMsSoftTimer();
//		g_headCtrl.timeCount = 0;
//
//		printf("timer init 1s,pwmCount = %d,time:%d.\r\n",g_headCtrl.pwmCount,ntime);
//	}
#else
//	g_headCtrl.timeCount ++;
//	if(g_headCtrl.timeCount > 1000000/g_headCtrl.pwmFreq)
//	{
//		u32 ntime = 0;
//		ntime = GetMsSoftTimer();
//		g_headCtrl.timeCount = 0;
//
//		printf("timer 1s,time:%d,pwm频率 = %dHz,pwm占空比 %d.\r\n",ntime,1000000/g_headCtrl.pwmFreq,g_headCtrl.pwmCtrl);
//	}
#endif
}


void SysLedTask(void)
{
	static u32 ledsta = 0;
	static u32 ledtime = 0;
	if(GetMsSoftTimer10() - ledtime >= 1000)
	{
		if(ledsta == 0)
		{
			SetSysLedOn();
			ledsta = 1;
		}
		else
		{
			SetSysLedOff();
			ledsta = 0;
		}
		ledtime = GetMsSoftTimer10();
	}
}


void SewHeadBoardTask(void)
{
	//SetParasTask();
	Rs485Task();
#if(CONTROL_BUS == COMM_485)
	ModbusSTask();
#endif
	InOutStaRefreshTask();
	SysLedTask();
}

void ModBusReceiveProc(Rs485Cmd *cmd)
{
	if (cmd == NULL)
	{
		return;
	}

	memcpy(g_headCtrl.mtWkSta,cmd->rddat,16);

}

void SendMotoCtrlCmd(u16 cmd,u16 act)
{
	Rs485Cmd ccmd;
	memset(&ccmd,0,sizeof(Rs485Cmd));

	ccmd.index = 10;
	ccmd.cmd = MODBUS_WR;
	ccmd.addr = cmd;
	ccmd.wrdat[0] = act;
	ccmd.wlen = 1;
	ccmd.rlen = 0;

	AddRs485Cmd(&ccmd);
}

void FootMotoCmdRst(void)
{
	SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_CMD_RST);
}

void FootMotoToZero(void)
{
	SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_TO_ZERO);
//	SetSTM32Moto1RealPos(0);
}

void FootMotoDis(void)
{
	SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_DIS);
}

void FootMotoEn(void)
{
	SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_EN);
}

void FootMotoReset(void)
{
	SendMotoCtrlCmd(MOTO_CTRL_CMD,MOTO_RESET);
}

void OutputStepOn(void)
{
	if(g_headCtrl.followEcdDir == 1)
	{
		g_headCtrl.footMtPos ++;
	}
	else
	{
		g_headCtrl.footMtPos --;
	}
	SetMoto1StepOn();
}

#define	FOLLOW_TAB_NUM	6

int FOLLOW_TABLE[FOLLOW_TAB_NUM] =
{
	20,
	16,
	12,
	8,
	6,
	3,
};

void GetFollowTableValue(u32 angle,u32 puls)
{
	int i, temp,value,followec,sum = 0;
	followec = angle*1000/360;
	value = followec/puls;

	for(i = 0; i<FOLLOW_TAB_NUM-1; i++)
	{
		FOLLOW_TABLE[i] = value + (4*(FOLLOW_TAB_NUM -2 - i));
		sum += FOLLOW_TABLE[i];
	}
	temp = puls-((FOLLOW_TAB_NUM-1)*2);
	FOLLOW_TABLE[FOLLOW_TAB_NUM-1] = (followec - sum*2)/temp;
}

void FollowEcdOutPulseOn(void)
{
	if (g_headCtrl.followEcdEn != 0)
	{
		g_headCtrl.followCountr++;

		if (g_headCtrl.followCountr >= g_headCtrl.followStepNum)
		{
			if(g_headCtrl.followNumHigh > g_headCtrl.followRunNum)
			{
				g_headCtrl.followNumHigh = 0;
				g_headCtrl.followNumLow = 0;
				g_headCtrl.followEcdEn = 0;
				return;
			}

			OutputStepOn();
			g_headCtrl.followCountr = 0;
			g_headCtrl.followNumHigh++;

			// 加速
			if (g_headCtrl.followTableIdx < FOLLOW_TAB_NUM && g_headCtrl.followNumHigh < FOLLOW_TAB_NUM)
			{
				g_headCtrl.followTableIdx++;	// 跟随运动的索引
				g_headCtrl.followStepNum = FOLLOW_TABLE[g_headCtrl.followTableIdx];		// 装载新的值
			}

			// 降速
			if(g_headCtrl.followNumHigh >= g_headCtrl.followRunNum - FOLLOW_TAB_NUM)
			{
				if(g_headCtrl.followTableIdx <= 0)
				{
					g_headCtrl.followTableIdx = 0;
				}
				g_headCtrl.followStepNum = FOLLOW_TABLE[g_headCtrl.followTableIdx];		// 装载新的值
				g_headCtrl.followTableIdx--;	// 跟随运动的索引
			}
		}
	}
}

void FollowEcdOutPulseOff(void)
{
	if (g_headCtrl.followEcdEn != 0)
	{
		if (g_headCtrl.followCountr * 2 + 1 >= g_headCtrl.followStepNum)
		{
			OutputStepOff();
			g_headCtrl.followNumLow++;
		}
	}
}

void FollowEcdOutDirOn(void)
{
	if (g_headCtrl.followEcdCheck != 0)
	{
		if (g_headCtrl.followEcdDir == 1)
		{
			OutputDirOn();
		}
		else
		{
			OutputDirOff();
		}
	}
}
void FollowEcdOutDirOff(void)
{
	if (g_headCtrl.followEcdCheck != 0)
	{
		if (g_headCtrl.followEcdDir == 1)
		{
			OutputDirOff();
		}
		else
		{
			OutputDirOn();
		}
	}
}

void CheckParasValue(void)
{
	g_headCtrl.followRunNum = g_headCtrl.pfootFlowPuls;
	if(g_headCtrl.followRunNum == 0)
	{
		g_headCtrl.followRunNum = 40;
	}
	g_headCtrl.followStepNum = 3;
	g_headCtrl.followStartPPos = g_headCtrl.pfootFlowPAng*1000/360 + g_headCtrl.ecOfstValue;
	if(g_headCtrl.followStartPPos >= 1000)
	{
		g_headCtrl.followStartPPos -= 1000;
	}
	if(g_headCtrl.followStartPPos < 0)
	{
		g_headCtrl.followStartPPos += 1000;
	}
	if(g_headCtrl.followStartPPos == 0)
	{
		g_headCtrl.followStartPPos = 100;
	}
	g_headCtrl.followStartNPos = g_headCtrl.pfootFlowNAng*1000/360 + g_headCtrl.ecOfstValue;
	if(g_headCtrl.followStartNPos >= 1000)
	{
		g_headCtrl.followStartNPos -= 1000;
	}
	if(g_headCtrl.followStartNPos < 0)
	{
		g_headCtrl.followStartNPos += 1000;
	}
	if(g_headCtrl.followStartNPos == 0)
	{
		g_headCtrl.followStartNPos = 600;
	}
	g_headCtrl.pfootRunLowPos = g_headCtrl.pfootRunLowPos;
	if(g_headCtrl.pfootRunLowPos == 0)
	{
		g_headCtrl.pfootRunLowPos = 10;
	}
	g_headCtrl.pfootRunBackPos = g_headCtrl.pfootRunBackPos;
	if(g_headCtrl.pfootRunBackPos == 0)
	{
		g_headCtrl.pfootRunBackPos = 10;
	}
	g_headCtrl.pfootRunHighPos = g_headCtrl.pfootRunLowPos + g_headCtrl.followRunNum;
	if(g_headCtrl.pfootRunHighPos == 0)
	{
		g_headCtrl.pfootRunHighPos = 50;
	}

	g_headCtrl.followConPAng = g_headCtrl.pfootRunConAng;
	if(g_headCtrl.followConPAng == 0)
	{
		g_headCtrl.followConPAng = 140;
	}
	else if(g_headCtrl.followConPAng >160)
	{
		g_headCtrl.followConPAng = 160;
	}

	GetFollowTableValue(g_headCtrl.followConPAng,g_headCtrl.followRunNum);
	//GetFollowTableValue(g_headCtrl.followConNAng,g_headCtrl.followRunNum);
}

void SetMotoPosValue(u16 pos,u16 value)
{
	SendMotoCtrlCmd(pos,value);
}

void PFootCtrlUp(void)
{
	//MotoMoveAsOfst(1,100,NULL,0);
	//SendMotoCtrlCmd(MOTO_CTRL_TOPOS,PFOOT_HIGH_POS);
	SendMotoCtrlCmd(MOTO_CTRL_TOZPOS,g_headCtrl.pfootRunHighPos);
	g_headCtrl.pfootSta = 1;
}

void PFootCtrlDown(void)
{
	//MotoMoveAsOfst(1,30,NULL,0);
	//SendMotoCtrlCmd(MOTO_CTRL_TOPOS,PFOOT_LOW_POS);
	SendMotoCtrlCmd(MOTO_CTRL_TOZPOS,g_headCtrl.pfootRunLowPos);
	g_headCtrl.pfootSta = 2;
	g_headCtrl.followRunNum = g_headCtrl.pfootRunHighPos - g_headCtrl.pfootRunLowPos;
//	g_headCtrl.followChangeNum = g_headCtrl.pfootRunHighPos - g_headCtrl.pfootRunLowPos;
//	if(g_headCtrl.followChangeNum != g_headCtrl.followRunNum)
//	{
//		g_headCtrl.followChangeFlag = 1;
//		g_headCtrl.pfootFlLowPos = g_headCtrl.pfootRunLowPos;
//	}
}

void PFootCtrlBack(void)
{
	//MotoMoveAsOfst(1,g_headCtrl.pfootRunBackPos,NULL,0);
	SendMotoCtrlCmd(MOTO_CTRL_TOZPOS,g_headCtrl.pfootRunBackPos);
	g_headCtrl.pfootSta = 2;
	g_headCtrl.followRunNum = g_headCtrl.pfootRunHighPos - g_headCtrl.pfootRunBackPos;
//	g_headCtrl.followChangeNum = g_headCtrl.pfootRunHighPos - g_headCtrl.pfootRunBackPos;
//	if(g_headCtrl.followChangeNum != g_headCtrl.followRunNum)
//	{
//		g_headCtrl.followChangeFlag = 1;
//		g_headCtrl.pfootFlLowPos = g_headCtrl.pfootRunBackPos;
//	}
}

#if(CONTROL_BUS == COMM_485)
void ModBusCmdProcTask(Rs485Cmd * pCmd)
{
	Rs485Cmd rCmd;

	if (pCmd == NULL)
	{
		return;
	}
	memcpy(&rCmd, pCmd, sizeof(Rs485Cmd));

	InOutWorkTask(pCmd);
	if (rCmd.cmd == MODBUS_06WR)
	{
		switch (rCmd.addr)
		{
			case 0:
			{
				break;
			}

			case 1:
			{
				if (rCmd.wrdat[0] == 1)
				{
					g_headCtrl.followStepNum = FOLLOW_TABLE[0];
					g_headCtrl.followEcdCheck = 1;
					g_headCtrl.followEcdDir = 1;
					g_headCtrl.footMtPos = GetSTM32Moto1RealPos();

					g_headCtrl.followEcdEn = 0;
				}
				else
				{
					g_headCtrl.followEcdCheck = 0;
					//g_headCtrl.pfootSta = 0;
				}
				break;
			}
			case 5:
			{
				g_headCtrl.followStepNum = FOLLOW_TABLE[0];
				g_headCtrl.followEcdCheck = 1;
				g_headCtrl.followEcdDir = 1;
				g_headCtrl.footMtPos = GetSTM32Moto1RealPos();
				//if(g_headCtrl.pfootSta == 0)
				{
					PFootCtrlDown();
				}
				g_headCtrl.followEcdEn = 0;
				break;
			}
			case 2:		// 压脚升降
			{
				if (rCmd.wrdat[0] == 1)
				{
					PFootCtrlUp();
				}
				else if (rCmd.wrdat[0] == 2)
				{
					PFootCtrlDown();
				}
				else if (rCmd.wrdat[0] == 3)
				{
					PFootCtrlBack();
				}
				g_headCtrl.followEcdEn = 0;
				break;
			}

			case 3:
			{
				g_headCtrl.followStartNPos = rCmd.wrdat[0];
				break;
			}
			case 4:
			{
				g_headCtrl.followStartPPos = rCmd.wrdat[0];
				break;
			}
			case 11:
			{
				MotoMoveAsOfst(0,(s16)rCmd.wrdat[0],NULL,0);
				break;
			}
			case 12:
			{
				MotoMoveAsOfst(1,(s16)rCmd.wrdat[0],NULL,0);
				break;
			}
			case 20:
			{
				if(rCmd.wrdat[0] == 0)
				{
					FootMotoToZero();
				}
				else if(rCmd.wrdat[0] == 1)
				{
					FootMotoDis();
				}
				else if(rCmd.wrdat[0] == 2)
				{
					FootMotoEn();
				}
				else if(rCmd.wrdat[0] == 10)
				{
					FootMotoReset();
				}
				else
				{
					FootMotoCmdRst();
				}
				break;
			}
			case 50:		// 设定压脚工作下位
			{
				if(rCmd.wrdat[0] == 0)
				{
					g_headCtrl.followChangeNum = g_headCtrl.followRunNum;
					//g_headCtrl.followChangeFlag = 1;
				}
				else
				{
					g_headCtrl.followChangeNum = g_headCtrl.pfootRunHighPos - rCmd.wrdat[0];
					g_headCtrl.pfootFlLowPos = rCmd.wrdat[0];
					if(g_headCtrl.followChangeNum != g_headCtrl.followRunNum)
					{
						g_headCtrl.followChangeFlag = 1;
					}
				}
				break;
			}
			case 100:		// 校正编码器位置
			{
				int lpos;
				lpos = rCmd.wrdat[0]*1000/36000;
				g_headCtrl.ecOfstValue = g_headCtrl.encoderValue - lpos;
				CheckParasValue();
				SaveD32McPara();
				break;
			}
			case 200:		// 校正电机位置
			{

				break;
			}
			case 1000:		// 配置参数
			{

				break;
			}

			case 10:
			{// 输出
				if (rCmd.wrdat[0] == 1)
				{
					SetOutput1On();
				}
				else// if (rCmd.wrdat[0] == 2)
				{
					SetOutput1Off();
				}

				if (rCmd.wrdat[0] == 1)
				{
					SetOutput1On();
				}
				else// if (rCmd.wrdat[0] == 2)
				{
					SetOutput1Off();
				}
				break;
			}
			default:
				break;
		}
	}
	else if (rCmd.cmd == MODBUS_10WR)
	{
		g_headCtrl.pfootFlowPAng = rCmd.wrdat[0];
		g_headCtrl.pfootFlowNAng = rCmd.wrdat[1];
		g_headCtrl.pfootFlowPuls = rCmd.wrdat[2];
		g_headCtrl.pfootRunLowPos = rCmd.wrdat[3];
		g_headCtrl.pfootRunConAng = rCmd.wrdat[4];
		g_headCtrl.pfootRunBackPos = rCmd.wrdat[5];
		CheckParasValue();
		//g_headCtrl.setParasFlag = 1;
		//SaveD32McPara();
	}
}
#elif (CONTROL_BUS == COMM_CAN)
//-------------------------------------------------------------------------------
// CAN 命令处理
void CanCmdProc(void)
{
	u32 exid;
	int dev, tid, sid, cmd;

	if (g_workCtrl.curBdCtrl.cmdFlag != 0)
	{
		g_workCtrl.curBdCtrl.cmdFlag = 0;
		exid = g_workCtrl.curBdCtrl.ctrlCmd.cancomm.exId.id;

		dev = GET_PRT_FROM_EXID(exid);
		tid = GET_TID_FROM_EXID(exid);
		sid = GET_SID_FROM_EXID(exid);
		cmd = GET_CMD_FROM_EXID(exid);

		if (cmd == BN_CMD_TRAVERSE)		// 遍历命令
		{
			if (dev != BN_PROTOCOL_ALL || tid != BN_ID_BROADCAST)
			{
				return;
			}
			g_workCtrl.hostId = sid;

			// ......功能待开发
		}
		else if (cmd == BN_CMD_EXCTRL)	// 控制命令
		{
			SewHeadCommand * pCmd = (SewHeadCommand *)(&(g_workCtrl.curBdCtrl.ctrlCmd.cancomm.data.buff));
		//	printf("exctrl cmd=0x%x\r\n", pCmd->headCtrl.cmd);
			switch(pCmd->headCtrl.cmd)
			{
#if (BOARD_USE_FOR == SEW_HEAD)
				case CTRL_CMD_SEW_HEAD_CTRL:
#endif
				{
					switch(pCmd->headCtrl.subcmd)
					{
						case CTRL_SUBCMD_SEW_DISABLE		:		// 机头控制功能关闭
						{
							printf("ctrl func close\r\n");
							g_headCtrl.softLockSta = 1;

							break;
						}
						case CTRL_SUBCMD_SEW_ENABLE			:		// 机头控制功能打开
						{
							printf("ctrl func en\r\n");
							g_headCtrl.softLockSta = 0;

							break;
						}
						case CTRL_SUBCMD_SEW_WORK_STA:		// 工作状态刷新
						{
							u8 pwmval;
							pwmval = pCmd->headCtrl.para[0];
#if (PWM_TIMER_CTRL == 1)
							if(pwmval > 0 && pwmval <= 100)
							{
								if(g_headCtrl.pwmCtrl != pwmval)
								{
									if(pwmval <= 10 && g_headCtrl.workingSta == 1)
									{
										printf("little clip level in working.lv:%d\r\n",pwmval);
										return;
									}

									g_headCtrl.pwmCount = 0;
									g_headCtrl.pwmCtrl = pwmval;

//									if (g_headCtrl.cmprbuf != pwmval)
//									{// 打印是否丢包
//										g_headCtrl.chgTime++;
//										g_headCtrl.cmprbuf = pwmval;
//										printf("[%d][%d]\tpwm:%d\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),pwmval);
//
//										// 将变化次数加入到缓冲区中
//										g_headCtrl.tabIdx = g_headCtrl.tabIdx % BUF_TAB_SIZE;
//										g_headCtrl.chgTimeTab[g_headCtrl.tabIdx] = g_headCtrl.chgTime;
//										g_headCtrl.curTimeTab[g_headCtrl.tabIdx] = GetMsSoftTimer10() - g_headCtrl.startTime + 40;
//										g_headCtrl.pwmTab[g_headCtrl.tabIdx] = pwmval;
//										g_headCtrl.tabIdx++;
//									}
								}
							}
#endif
							u8 sta = pCmd->headCtrl.para[1];
							OutputCtrl(0,sta);
							break;
						}
						case CTRL_SUBCMD_SEW_OUTPUT_CTRL:	// IO输出控制
						{
							u8 ionum,sta;
							ionum = pCmd->headCtrl.para[0];
							//sta = rCmd.wrdat[1];
							sta = pCmd->headCtrl.para[1];
							OutputCtrl(ionum,sta);
							break;
						}
						case CTRL_SUBCMD_SEW_ELCLIP_CTRL:	// 电子夹线控制
						{
#if(PWM_TIMER_CTRL == 1)
							if(pCmd->headCtrl.para[0] > 0 && pCmd->headCtrl.para[0] <= g_headCtrl.pwmFreq)
							{
								g_headCtrl.pwmCtrl = pCmd->headCtrl.para[0];
								g_headCtrl.pwmCount = 0;
							}
							else
							{
								g_headCtrl.pwmCtrl = 0;
								SetOutputPWM1Off();
								SetOutputPWM2Off();
							}
#endif
							break;
						}
						case CTRL_SUBCMD_SEW_IN_WORK:			// 进入工作状态
						{
							if(pCmd->headCtrl.para[0] == 0)
							{
								g_headCtrl.workingSta = 0;
							}
							else
							{
								g_headCtrl.workingSta = 1;
							}
							break;
						}
						case CTRL_SUBCMD_SEW_PFOOT_CTRL:		// 压脚电机控制
						{
							u8 pfoot_cmd= pCmd->headCtrl.para[0];
							if(pfoot_cmd == CTRL_PFOOT_CMD_TO_ZERO)
							{
								FootMotoToZero();
							}
							else if(pfoot_cmd == CTRL_PFOOT_CMD_DIS)
							{
								FootMotoDis();
							}
							else if(pfoot_cmd == CTRL_PFOOT_CMD_EN)
							{
								FootMotoEn();
							}
							else if(pfoot_cmd == CTRL_PFOOT_CMD_RESET)
							{
								FootMotoReset();
							}
							else if (pfoot_cmd == CTRL_PFOOT_CMD_UP)
							{
								PFootCtrlUp();
								g_headCtrl.followEcdEn = 0;
							}
							else if (pfoot_cmd == CTRL_PFOOT_CMD_DOWN)
							{
								PFootCtrlDown();
								g_headCtrl.followEcdEn = 0;
							}
							else if (pfoot_cmd == CTRL_PFOOT_CMD_BACK)
							{
								PFootCtrlBack();
								g_headCtrl.followEcdEn = 0;
							}
							else
							{
								FootMotoCmdRst();
							}
							break;
						}
						case CTRL_SUBCMD_SEW_PFOOT_FOLLOW_CTRL:		// 压脚电机跟随
						{
							if (pCmd->headCtrl.para[0] == 1)
							{
								g_headCtrl.followStepNum = FOLLOW_TABLE[0];
								g_headCtrl.followEcdCheck = 1;
								g_headCtrl.followEcdDir = 1;
//								g_headCtrl.footMtPos = GetSTM32Moto1RealPos();

								if (pCmd->headCtrl.para[1] != 0)
								{
									PFootCtrlDown();
								}

								g_headCtrl.followEcdEn = 0;
							}
							else
							{
								g_headCtrl.followEcdCheck = 0;
								//g_headCtrl.pfootSta = 0;
							}
							break;
						}

						case CTRL_SUBCMD_SEW_PFOOT_SET_POS:		// 设定压脚工作下位
						{
							if(pCmd->headCtrl.para[0] == 0)
							{
								g_headCtrl.followChangeNum = g_headCtrl.followRunNum;
								//g_headCtrl.followChangeFlag = 1;
							}
							else
							{
								g_headCtrl.followChangeNum = g_headCtrl.pfootRunHighPos - pCmd->headCtrl.para[0];
								g_headCtrl.pfootFlLowPos = pCmd->headCtrl.para[0];
								if(g_headCtrl.followChangeNum != g_headCtrl.followRunNum)
								{
									g_headCtrl.followChangeFlag = 1;
								}
							}
							break;
						}
						case CTRL_SUBCMD_SEW_PFOOT_SET_ECD:		// 校正编码器位置
						{
							int lpos;
							lpos = pCmd->headCtrl.para[0]*1000/36000;
							g_headCtrl.ecOfstValue = g_headCtrl.encoderValue - lpos;
							CheckParasValue();
							break;
						}
						default:break;
					}
					break;
				}
				case CTRL_CMD_SEW_HEAD_PARA:		// 缝纫机头板参数设置
				{
//					g_pHeadPara->pfootFlowPAng = pCmd->headPara.para[0];
//					g_pHeadPara->pfootFlowNAng = pCmd->headPara.para[1];
//					g_pHeadPara->pfootFlowPuls = pCmd->headPara.para[2];
//					g_pHeadPara->pfootRunLowPos = pCmd->headPara.para[3];
//					g_pHeadPara->pfootRunConAng = pCmd->headPara.para[4];
//					g_pHeadPara->pfootRunBackPos = pCmd->headPara.para[5];
					g_HeadPara.buf[pCmd->headPara.pataId] = pCmd->headPara.paraVal;
					if (pCmd->headPara.cmd != 0)
					{
						CheckParasValue();
					}
				}
				default:break;
			}
		}
		else
		{

		}
	}
}
#endif

#if (0)
int MotoNormalStop(u32 para1, u32 para2)
{
	int rslt = 0;


	return rslt;
}

int MotoQuickStop(u32 para1, u32 para2)
{
	int rslt = 0;


	return rslt;
}

int MotoMoveAsOfst(int mode, int ofst, spdCtrl *spd, int block)
{
	int rslt;
	MotoCtrl mtcfg;
	rslt = 0;

	memset(&mtcfg, 0, sizeof(MotoCtrl));

	if(mode == 1)
	{
		ofst = ofst - GetSTM32Moto1RealPos();
	}
	// 速度
	if(spd == NULL)
	{
		mtcfg.startPPS = 100;
		mtcfg.runPPS = 200;
		mtcfg.addPPSG = 10;
		mtcfg.brkPPSG = 50;
	}
	else
	{
		mtcfg.startPPS = spd->stspd;
		mtcfg.runPPS = spd->runspd;
		mtcfg.addPPSG = spd->add;
		mtcfg.brkPPSG = spd->brkadd;
	}

	mtcfg.movement = ofst;// 位移量
	mtcfg.GetNormalStop = MotoNormalStop;
	mtcfg.GetQuickStop = MotoQuickStop;
	mtcfg.condPara1 = 0;
	mtcfg.condPara2 = 0;
	mtcfg.blockRunflag = block;
	mtcfg.funTestTime = 1;
	mtcfg.ExecWhenRun = NULL;
	mtcfg.ExecWhenStart = NULL;
	mtcfg.ExecWhenStop = NULL;

	STM32Moto1Stop();

    DelayRef(50);

    STM32Moto1Start(&mtcfg);

	return rslt;
}
#endif
// 得到主轴角度，单位0.01度
int GetAngleFromEncoder(void)
{
	int ang = g_headCtrl.encoderValue * 36000 / ENCODER_PNUM + g_headCtrl.stopAngle;

	// 转化为 0 -- 35999
	while (ang >= 36000)
	{
		ang -= 36000;
	}

	while (ang <= -36000)
	{
		ang += 36000;
	}
	return ang;
}


#if(CONTROL_BUS == COMM_485)
// 输入输出工作任务
void InOutWorkTask(Rs485Cmd * pCmd)
{
	Rs485Cmd rCmd;
	int sendstacmd = 0;

	if (pCmd == NULL)
	{
		return;
	}

	memcpy(&rCmd, pCmd, sizeof(Rs485Cmd));

	rCmd.index &= 0x7f;
	if (rCmd.index != D29_BOARD_ID && rCmd.index != D29_ALL_BOARD_ID)
	{
		return;
	}

	if (rCmd.cmd == MODBUS_WR)
	{
		if(rCmd.addr != 0)
		{
			printf("get modbus cmd:%d,data:%d.\r\n",rCmd.addr,rCmd.wrdat[0]);
		}

		if(rCmd.addr == 0)		// 多IO控制
		{
			u8 ionum,sta;
			ionum = 0;

#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
			if (D29_BOARD_ID == 1)
			{// 同步刷新PWM占空比
				u8 pwmval;
				pwmval = (rCmd.wrdat[0] & 0xFF00) >> 8;
				if ((pwmval & 0x80) == 0x80)	// 收到PWM占空比标志
				{
					pwmval = pwmval & 0x7F;
#if (PWM_TIMER_CTRL == 1)
					if(pwmval > 0 && pwmval <= 100)
					{
						if(g_headCtrl.pwmCtrl != pwmval)
						{
							if(pwmval <= 10 && g_headCtrl.workingSta == 1)
							{
								printf("little clip level in working.lv:%d\r\n",pwmval);
								return;
							}

							//g_headCtrl.pwmCtrl = g_headCtrl.pwmFreq;
							g_headCtrl.pwmCount = 0;
//							TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);						/* 开启中断 */
//							TIM_Cmd(TIM1, ENABLE);
							g_headCtrl.pwmCtrl = pwmval;

							if (g_headCtrl.cmprbuf != pwmval)
							{// 打印是否丢包
								g_headCtrl.chgTime++;
								g_headCtrl.cmprbuf = pwmval;
								printf("[%d][%d]\tpwm:%d\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),pwmval);

								// 将变化次数加入到缓冲区中
								g_headCtrl.tabIdx = g_headCtrl.tabIdx % BUF_TAB_SIZE;
								g_headCtrl.chgTimeTab[g_headCtrl.tabIdx] = g_headCtrl.chgTime;
								g_headCtrl.curTimeTab[g_headCtrl.tabIdx] = GetMsSoftTimer10() - g_headCtrl.startTime + 40;
								g_headCtrl.pwmTab[g_headCtrl.tabIdx] = pwmval;
								g_headCtrl.tabIdx++;
							}
						}
					}
						//printf("P:%d\tF:%d\t",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
#else
				#if (0)
					if (pwmval >= 0 && pwmval <= 100)
					{
						PWMoutputTask(pwmval,1);
					}
				#else
					if ((pwmval >= 0 && pwmval <= 100) &&
						//(g_headCtrl.pwmCtrl != pwmval) &&
						1 )
					{
						u32 cmpr;
						if(g_headCtrl.pwmCtrl != pwmval)
						{
							if(pwmval <= 10 && g_headCtrl.workingSta == 1)
							{
								printf("little clip level in working.lv:%d\r\n",pwmval);
								return;
							}
							cmpr = g_headCtrl.pwmFreq * pwmval / 100;
							TIM_SetCompare1(TIM3,cmpr);
							TIM_SetCompare2(TIM3,cmpr);
							g_headCtrl.pwmCtrl = pwmval;
						}

						if (g_headCtrl.cmprbuf != cmpr)
						{// 打印是否丢包
							g_headCtrl.chgTime++;
							g_headCtrl.cmprbuf = cmpr;
							printf("[%d][%d]\tpwm:%d\tcmpr:%d (0)\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),pwmval,cmpr);

							// 将变化次数加入到缓冲区中
							g_headCtrl.tabIdx = g_headCtrl.tabIdx % BUF_TAB_SIZE;
							g_headCtrl.chgTimeTab[g_headCtrl.tabIdx] = g_headCtrl.chgTime;
							g_headCtrl.curTimeTab[g_headCtrl.tabIdx] = GetMsSoftTimer10() - g_headCtrl.startTime + 40;
							g_headCtrl.pwmTab[g_headCtrl.tabIdx] = pwmval;
							g_headCtrl.tabIdx++;
						}
						//printf("P:%d\tF:%d\t",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
					}
				#endif
#endif
				}
			}
#endif
			sta = (rCmd.wrdat[0]>>(8*(D29_BOARD_ID-1))) & 0x00FF;
			OutputCtrl(ionum,sta);
		}
		else if(rCmd.addr == 1)		// IO控制ON
		{
			u8 ionum,sta;
			ionum = rCmd.wrdat[0];
			//sta = rCmd.wrdat[1];
			sta = 1;
			OutputCtrl(ionum,sta);
		}
		else if(rCmd.addr == 2)		// IO控制OFF
		{
			u8 ionum,sta;
			ionum = rCmd.wrdat[0];
			//sta = rCmd.wrdat[1];
			sta = 0;
			OutputCtrl(ionum,sta);
		}
#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
		else if(rCmd.addr == 3 || rCmd.addr == 30)	//PWM控制
		{
#if(PWM_TIMER_CTRL == 1)
			if(rCmd.wrdat[0] > 0 && rCmd.wrdat[0] <= g_headCtrl.pwmFreq)
			{
				g_headCtrl.pwmCtrl = rCmd.wrdat[0];
				g_headCtrl.pwmCount = 0;
				//TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);						/* 开启中断 */
				//TIM_Cmd(TIM1, ENABLE);
			}
			else
			{
				g_headCtrl.pwmCtrl = 0;
				//TIM_ITConfig(TIM1, TIM_IT_Update, DISABLE);						/* 开启中断 */
				//TIM_Cmd(TIM1, DISABLE);			// 关闭定时器
				SetOutput6Off();
				SetOutput7Off();
				//SetOutput8Off();
			}
#else
			if (rCmd.wrdat[0] >= 0 && rCmd.wrdat[0] <= 100)
			{
			#if (0)
				PWMoutputTask(pwmval,1);
			#else
				u32 cmpr;
				u16 pwmval;
				pwmval = rCmd.wrdat[0];
				if(g_headCtrl.pwmCtrl != pwmval)
				{
					if(pwmval <= 10 && g_headCtrl.workingSta == 1)
					{
						printf("little clip level in working.lv:%d\r\n",pwmval);
						return;
					}
					cmpr = g_headCtrl.pwmFreq * pwmval / 100;
					if(rCmd.addr == 3)
					{
						TIM_SetCompare1(TIM3,cmpr);
					}
					TIM_SetCompare2(TIM3,cmpr);
					g_headCtrl.pwmCtrl = pwmval;
				}

				if (g_headCtrl.cmprbuf != cmpr)
				{// 打印是否丢包
					g_headCtrl.chgTime++;
					g_headCtrl.cmprbuf = cmpr;
					printf("[%d][%d]\tpwm:%d\tcmpr:%d (3)\r\n",g_headCtrl.chgTime,(GetMsSoftTimer10() - g_headCtrl.startTime + 40),rCmd.wrdat[0],cmpr);

					// 将变化次数加入到缓冲区中
					g_headCtrl.tabIdx = g_headCtrl.tabIdx % BUF_TAB_SIZE;
					g_headCtrl.chgTimeTab[g_headCtrl.tabIdx] = g_headCtrl.chgTime;
					g_headCtrl.curTimeTab[g_headCtrl.tabIdx] = GetMsSoftTimer10() - g_headCtrl.startTime + 40;
					g_headCtrl.pwmTab[g_headCtrl.tabIdx] = rCmd.wrdat[0];
					g_headCtrl.tabIdx++;
				}

				g_headCtrl.pwmCtrl = rCmd.wrdat[0];

				//printf("pwm:%d,Freq:%dHz\r\n",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
				//printf("占空比：%d,频率：%d Hz\r\n",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
			#endif
			}
			else
			{// 通讯异常,不响应

			}
#endif
		}
#endif
		else if(rCmd.addr == 4)		// 计时器复位
		{
			printf("startTime reset\r\n");

			g_headCtrl.chgTime = 0;
			g_headCtrl.startTime = GetMsSoftTimer10();
		}
		else if(rCmd.addr == 5)		// 进入工作状态
		{
			if(rCmd.wrdat[0] == 0)
			{
				g_headCtrl.workingSta = 0;
			}
			else
			{
				g_headCtrl.workingSta = 1;
			}
		}
		else if(rCmd.addr == 100)		// 更改IO配置
		{
			if(rCmd.wrdat[0] == 1)
			{
				TIM_Cmd(TIM3, DISABLE);		// 关闭定时器
				GPIO_InitTypeDef GPIO_InitStructure;
				OUT_OD_CONFIG(OUT6);
				OUT_OD_CONFIG(OUT7);
				g_headCtrl.pwmOutputFlag = 0;
			}
			else
			{
				InitSTM32PwmTimer();
				g_headCtrl.pwmOutputFlag = 1;
			}
		}
		else
		{
//			RefreshOutputSta();
//			sendstacmd = 1;
		}
	}

	if(sendstacmd == 1)
	{

	}
}
#endif
// 刷新输出状态
void RefreshOutputSta(void)
{
	if(GetOutput1Status() == SENSOR_ON)
	{
		g_headCtrl.outputSta |= 0x01;
	}
	else
	{
		g_headCtrl.outputSta &= ~0x01;
	}

	if(GetOutput2Status() == SENSOR_ON)
	{
		g_headCtrl.outputSta |= 0x02;
	}
	else
	{
		g_headCtrl.outputSta &= ~0x02;
	}

	if(GetOutput3Status() == SENSOR_ON)
	{
		g_headCtrl.outputSta |= 0x04;
	}
	else
	{
		g_headCtrl.outputSta &= ~0x04;
	}

	if(GetOutput4Status() == SENSOR_ON)
	{
		g_headCtrl.outputSta |= 0x08;
	}
	else
	{
		g_headCtrl.outputSta &= ~0x08;
	}

	if(GetOutput5Status() == SENSOR_ON)
	{
		g_headCtrl.outputSta |= 0x10;
	}
	else
	{
		g_headCtrl.outputSta &= ~0x10;
	}

	if(GetOutput6Status() == SENSOR_ON)
	{
		g_headCtrl.outputSta |= 0x20;
	}
	else
	{
		g_headCtrl.outputSta &= ~0x20;
	}

	if(GetOutput7Status() == SENSOR_ON)
	{
		g_headCtrl.outputSta |= 0x40;
	}
	else
	{
		g_headCtrl.outputSta &= ~0x40;
	}

	if(GetOutput8Status() == SENSOR_ON)
	{
		g_headCtrl.outputSta |= 0x80;
	}
	else
	{
		g_headCtrl.outputSta &= ~0x80;
	}
}

// 刷新输入输出状态
void InOutStaRefreshTask(void)
{
	if(GetInput1Status() == SENSOR_ON)
	{
		g_headCtrl.inputSta |= 0x01;
	}
	else
	{
		g_headCtrl.inputSta &= ~0x01;
	}

	if(GetInput2Status() == SENSOR_ON)
	{
		g_headCtrl.inputSta |= 0x02;
	}
	else
	{
		g_headCtrl.inputSta &= ~0x02;
	}

	if(GetInput3Status() == SENSOR_ON)
	{
		g_headCtrl.inputSta |= 0x04;
	}
	else
	{
		g_headCtrl.inputSta &= ~0x04;
	}

	if(GetInput4Status() == SENSOR_ON)
	{
		g_headCtrl.inputSta |= 0x08;
	}
	else
	{
		g_headCtrl.inputSta &= ~0x08;
	}

	if(GetInput5Status() == SENSOR_ON)
	{
		g_headCtrl.inputSta |= 0x10;
	}
	else
	{
		g_headCtrl.inputSta &= ~0x10;
	}

	if(GetInput6Status() == SENSOR_ON)
	{
		g_headCtrl.inputSta |= 0x20;
	}
	else
	{
		g_headCtrl.inputSta &= ~0x20;
	}

	if(GetInput7Status() == SENSOR_ON)
	{
		g_headCtrl.inputSta |= 0x40;
	}
	else
	{
		g_headCtrl.inputSta &= ~0x40;
	}

	if(GetInput8Status() == SENSOR_ON)
	{
		g_headCtrl.inputSta |= 0x80;
	}
	else
	{
		g_headCtrl.inputSta &= ~0x80;
	}
	g_headCtrl.headCmdReg[0] = g_headCtrl.inputSta;
}

// 输出控制
void OutputCtrl(u8 num, u8 onof)
{
	if(num == 0)
	{
		if((onof & 0x01) != 0)
		{
			SetOutput1On();
		}
		else
		{
			SetOutput1Off();
		}

		if((onof & 0x02) != 0)
		{
			SetOutput2On();
		}
		else
		{
			SetOutput2Off();
		}

		if((onof & 0x04) != 0)
		{
			SetOutput3On();
		}
		else
		{
			SetOutput3Off();
		}

		if((onof & 0x08) != 0)
		{
			SetOutput4On();
		}
		else
		{
			SetOutput4Off();
		}

		if((onof & 0x10) != 0)
		{
			SetOutput5On();
		}
		else
		{
			SetOutput5Off();
		}

//#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
//#else
		if(g_headCtrl.pwmOutputFlag == 0)
		{
			if((onof & 0x20) != 0)
			{
				SetOutput6On();
			}
			else
			{
				SetOutput6Off();
			}

			if((onof & 0x40) != 0)
			{
				SetOutput7On();
			}
			else
			{
				SetOutput7Off();
			}
		}
//#endif
		if((onof & 0x80) != 0)
		{
			SetOutput8On();
		}
		else
		{
			SetOutput8Off();
		}
	}
	else if(num == 1)
	{
		if(onof == 1)
		{
			SetOutput1On();
		}
		else
		{
			SetOutput1Off();
		}
	}
	else if(num == 2)
	{
		if(onof == 1)
		{
			SetOutput2On();
		}
		else
		{
			SetOutput2Off();
		}
	}
	else if(num == 3)
	{
		if(onof == 1)
		{
			SetOutput3On();
		}
		else
		{
			SetOutput3Off();
		}
	}
	else if(num == 4)
	{
		if(onof == 1)
		{
			SetOutput4On();
		}
		else
		{
			SetOutput4Off();
		}
	}
	else if(num == 5)
	{
		if(onof == 1)
		{
			SetOutput5On();
		}
		else
		{
			SetOutput5Off();
		}
	}
//#if (STM32_MOTOS_NUM == 0 && STM32_SOFT_PWM != 0)
//#else
	else if(num == 6)
	{
		if(g_headCtrl.pwmOutputFlag == 0)
		{
			if(onof == 1)
			{
				SetOutput6On();
			}
			else
			{
				SetOutput6Off();
			}
		}
	}
	else if(num == 7)
	{
		if(g_headCtrl.pwmOutputFlag == 0)
		{
			if(onof == 1)
			{
				SetOutput7On();
			}
			else
			{
				SetOutput7Off();
			}
		}
	}
//#endif
	else if(num == 8)
	{
		if(onof == 1)
		{
			SetOutput8On();
		}
		else
		{
			SetOutput8Off();
		}
	}
	//printf("N:%d\tO:%d\r\n",num,onof);
}

void TestOutputTask(void)
{
	static u32 thistime = 0;
	static int outsta = 0;

	if(GetMsSoftTimer10() - thistime >= 500)
	{
		if(outsta != 1)
		{
			outsta = 1;
		}
		else
		{
			outsta = 0;
		}
		thistime = GetMsSoftTimer10();
	}

	if(outsta == 1)
	{
		SetOutput1On();
		SetOutput2On();
		SetOutput3On();
		SetOutput4On();
		SetOutput5On();
		SetOutput6On();
		SetOutput7On();
		SetOutput8On();
	}
	else
	{
		SetOutput1Off();
		SetOutput2Off();
		SetOutput3Off();
		SetOutput4Off();
		SetOutput5Off();
		SetOutput6Off();
		SetOutput7Off();
		SetOutput8Off();

	}
}

//-------------------------------------------------------------------------------
// AP中断
void APIntProc(void)
{
	if (g_headCtrl.enflag != EN_FALG)
	{
		return;
	}
#if (TRIGGER_TYPE == TRG_ECD_AP || TRIGGER_TYPE == TRG_ECD_AB2)
	if (GetInputAPStatus() == SENSOR_ON)	// 下降沿触发
	{
		if (GetInputBPStatus() == SENSOR_OFF)
		{
			g_headCtrl.encoderValue++;
			if(g_headCtrl.followEcdCheck == 1)
			{
				if(g_headCtrl.encoderValue == g_headCtrl.followStartPPos)	// 上升
				{
					if(g_headCtrl.pfootSta == 2 || g_headCtrl.pfootSta == 3)
					{
						g_headCtrl.pfootSta = 3;
						g_headCtrl.followEcdDir = 1;
						g_headCtrl.followEcdEn = 1;
						g_headCtrl.followTableIdx = 0;
						g_headCtrl.followStepNum = FOLLOW_TABLE[0];
						g_headCtrl.followNumHigh = 0;
						g_headCtrl.followNumLow = 0;
					}
				}
				else if(g_headCtrl.encoderValue == g_headCtrl.followStartNPos)	// 下降
				{
					if(g_headCtrl.pfootSta == 1 || g_headCtrl.pfootSta == 3)
					{
						g_headCtrl.pfootSta = 3;
						g_headCtrl.followEcdDir = -1;
						g_headCtrl.followEcdEn = 1;
						g_headCtrl.followTableIdx = 0;
						g_headCtrl.followStepNum = FOLLOW_TABLE[0];
						g_headCtrl.followNumHigh = 0;
						g_headCtrl.followNumLow = 0;
						if(g_headCtrl.followChangeFlag == 1)
						{
							g_headCtrl.followRunNum = g_headCtrl.followChangeNum;
							g_headCtrl.followChangeFlag = 0;
							GetFollowTableValue(g_headCtrl.followConPAng,g_headCtrl.followRunNum);
						}
					}
				}
			}
			g_headCtrl.FollowMotoSignOn();
		}
		else
		{
			g_headCtrl.encoderValue--;
			g_headCtrl.FollowMotoSignOff();
		}
		g_headCtrl.FollowMotoPulseOn();
	}
	else
	{
		g_headCtrl.FollowMotoPulseOff();
	}
#elif (TRIGGER_TYPE == TRG_ECD_AB4)
	if (GetInputAPStatus() == GetInputBPStatus())
	{
		g_headCtrl.encoderValue++;
		g_headCtrl.FollowMotoSignOn();
	}
	else
	{
		g_headCtrl.encoderValue--;
		g_headCtrl.FollowMotoSignOff();
	}
	g_headCtrl.FollowMotoPulseOn();
#endif
//	printf("AP, evalue=%d\r\n", g_headCtrl.encoderValue);
}

void BPIntProc(void)
{
	if (g_headCtrl.enflag != EN_FALG)
	{
		return;
	}
#if (TRIGGER_TYPE == TRG_ECD_BP || TRIGGER_TYPE == TRG_ECD_AB2)
	if (GetInputBPStatus() == SENSOR_ON)	// 下降沿触发
	{
		if (GetInputAPStatus() == SENSOR_OFF)
		{
			g_headCtrl.encoderValue--;
			g_headCtrl.FollowMotoSignOff();
		}
		else
		{
			g_headCtrl.encoderValue++;
			g_headCtrl.FollowMotoSignOn();
		}
		g_headCtrl.FollowMotoPulseOn();
	}
	else
	{
		g_headCtrl.FollowMotoPulseOff();
	}
#elif (TRIGGER_TYPE == TRG_ECD_AB4)

	if (GetInputAPStatus() == GetInputBPStatus())
	{
		g_headCtrl.encoderValue--;
		g_headCtrl.FollowMotoSignOff();
	}
	else
	{
		g_headCtrl.encoderValue++;
		g_headCtrl.FollowMotoSignOn();
	}
	g_headCtrl.FollowMotoPulseOff();

#endif

//	printf("BP\r\n");
}

// ZP中断
void ZPIntProc(void)
{
	if (g_headCtrl.enflag != EN_FALG)
	{
		return;
	}
//	printf("zp, evalue=%d\r\n", g_headCtrl.encoderValue);
	g_headCtrl.encoderValue = 0;
}


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

	if (para1 == NULL || para2 == NULL)
	{
		return;
	}

	printf("para1=%s, para2=%s\r\n", para1, para2);

	if (strcmp(para1, "") == 0 || strcmp(para2, "") == 0)
	{
		return;
	}
	p1 = atoi(para1);
	p2 = atoi(para2);

	if (p1 == p2)
	{
	}
	//--------------------------------

	//--------------------------------
#if(PWM_TIMER_CTRL == 0)
	if (p1 == 0)
	{ // 打印历史数据
		int i,j;
		j = g_headCtrl.tabIdx;

		for (i = 0; i<BUF_TAB_SIZE;i++)
		{
			if ((i % 5) == 0)
			{
				DelayRef(10);
			}

			j = j % BUF_TAB_SIZE;
			printf("[%d][%d]\tpwm:%d\r\n",g_headCtrl.chgTimeTab[j],g_headCtrl.curTimeTab[j],g_headCtrl.pwmTab[j]);
			j++;
		}
	}
	else if (p1 == 1)	// 设置占空比
	{
		int cmpr;
		if(p2 < 0 || p2 > 100)
		{
			return;
		}

		cmpr = g_headCtrl.pwmFreq * p2 / 100;
		g_headCtrl.pwmCtrl = p2;
		TIM_SetCompare1(TIM3,cmpr);
		TIM_SetCompare2(TIM3,cmpr);

		TIM_Cmd(TIM3, ENABLE);
		printf("占空比：%d,频率：%d Hz\r\n",p2,1000000/g_headCtrl.pwmFreq);
	}
	else if (p1 == 2)
	{
		int freq,cmpr;
		TIM_Cmd(TIM3, DISABLE);
		DelayRef(100);
		freq = (1000000/p2)-1;
		g_headCtrl.pwmFreq = freq+1;
		cmpr = g_headCtrl.pwmFreq * g_headCtrl.pwmCtrl / 100;
		TIM_SetCompare1(TIM3,cmpr);
		TIM_SetCompare2(TIM3,cmpr);
		TIM_SetAutoreload(TIM3,freq);
		TIM_Cmd(TIM3, ENABLE);
		printf("占空比：%d,频率：%d Hz\r\n",g_headCtrl.pwmCtrl,1000000/g_headCtrl.pwmFreq);
	}
	else if (p1 == 3)	// 关闭
	{
		if(p2 == 1)
		{
			TIM_SetCompare1(TIM3,0);
		}
		else if(p2 == 2)
		{
			TIM_SetCompare2(TIM3,0);
		}
	}
	else if (p1 == 4)	// 开
	{
		u32 cmpr;
		cmpr = g_headCtrl.pwmFreq * g_headCtrl.pwmCtrl / 100;
		if(p2 == 1)
		{
			TIM_SetCompare1(TIM3,cmpr);
		}
		else if(p2 == 2)
		{
			TIM_SetCompare2(TIM3,cmpr);
		}
	}
	else if (p1 == 5)
	{

	}
	else if (p1 == 6)
	{

	}
	else if (p1 == 10)	// 开
	{
		if(p2 == 1)
		{
			TIM_Cmd(TIM3, ENABLE);
		}
		else
		{
			TIM_Cmd(TIM3, DISABLE);
		}
	}
#else
	else if(p1 == 1)
	{
		g_headCtrl.pwmCtrl = p2;
		LL_TIM_EnableCounter(TIM1);
		printf("占空比：%d \r\n",g_headCtrl.pwmCtrl);
	}
	else if(p1 == 2)
	{
		LL_TIM_EnableIT_UPDATE(TIM1);			/* 开启中断 */
		LL_TIM_DisableCounter(TIM1);			// 关闭定时器
		printf("关闭定时器 \r\n");
		SetOutput7Off();
		SetOutput6Off();
	}
	else if(p1 == 3)
	{
		g_headCtrl.pwmFreq = p2;
	}
	else if(p1 == 4)
	{
		g_headCtrl.timeCount = 1;
	}
#endif
	else if(p1 == 11)
	{
		if(p2 == 1)
		{
			SetOutput1On();
		}
		else if(p2 == 2)
		{
			SetOutput2On();
		}
		else if(p2 == 6)
		{
			SetOutput6On();
		}
		else if(p2 == 7)
		{
			SetOutput7On();
		}
		else if(p2 == 8)
		{
			SetOutput8On();
		}
	}
	else if(p1 == 12)
	{
		if(p2 == 1)
		{
			SetOutput1Off();
		}
		else if(p2 == 2)
		{
			SetOutput2Off();
		}
		else if(p2 == 6)
		{
			SetOutput6Off();
		}
		else if(p2 == 7)
		{
			SetOutput7Off();
		}
		else if(p2 == 8)
		{
			SetOutput8Off();
		}
	}
}
#endif