driver.cpp

这是在windows环境下用VC开发的MCX运动控制芯片的演示程序

	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
			Reg.wr2[i] = wdata;
	}
}

void SetWR3(short axis, unsigned int wdata)
{
	_OUTW(WR0, (axis << 8) | SET_NOP);	// WR1-3必须选择轴
	_OUTW(WR3, wdata);
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
			Reg.wr3[i] = wdata;
	}
}

void SetWR4(unsigned int wdata)
{
	_OUTW(WR4, wdata);
	Reg.wr4 = wdata;
}

void SetWR5(unsigned int wdata)
{
	_OUTW(WR5, wdata);
	Reg.wr5 = wdata;
}

void SetWR67(long dwdata)
{
	_OUTW(WR7, (dwdata >> 16) & 0xffff);
	_OUTW(WR6, dwdata & 0xffff);
}

// 寄存器WR1-WR5置/复位
void EnableWR1(short axis, unsigned int ctrl, int bEnabled)
{
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			if (bEnabled)
				SetWR1(1 << i, Reg.wr1[i] | ctrl);
			else
				SetWR1(1 << i, Reg.wr1[i] & ~ctrl);
		}
	}
}

void EnableWR2(short axis, unsigned int ctrl, int bEnabled)
{
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			if (bEnabled)
				SetWR2(1 << i, Reg.wr2[i] | ctrl);
			else
				SetWR2(1 << i, Reg.wr2[i] & ~ctrl);
		}
	}
}

void EnableWR3(short axis, unsigned int ctrl, int bEnabled)
{
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			if (bEnabled)
				SetWR3(1 << i, Reg.wr3[i] | ctrl);
			else
				SetWR3(1 << i, Reg.wr3[i] & ~ctrl);
		}
	}
}

void EnableWR4(unsigned int ctrl, int bEnabled)
{
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (bEnabled)
			SetWR4(Reg.wr4 | ctrl);
		else
			SetWR4(Reg.wr4 & ~ctrl);
	}
}

void EnableWR5(unsigned int ctrl, int bEnabled)
{
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (bEnabled)
			SetWR5(Reg.wr5 | ctrl);
		else
			SetWR5(Reg.wr5 & ~ctrl);
	}
}

// 读取寄存器RR0-RR5
unsigned int GetRR0()
{
	return _INW(RR0);
}

unsigned int GetRR1(short axis)
{
	_OUTW(WR0, (axis << 8) | SET_NOP);	// RR1-3必须选择轴
	return _INW(RR1);
}

unsigned int GetRR2(short axis)
{
	_OUTW(WR0, (axis << 8) | SET_NOP);	// RR1-3必须选择轴
	return _INW(RR2);
}

unsigned int GetRR3(short axis)
{
	_OUTW(WR0, (axis << 8) | SET_NOP);	// RR1-3必须选择轴
	return _INW(RR3);
}

unsigned int GetRR4()
{
	return _INW(RR4);
}

unsigned int GetRR5()
{
	return _INW(RR5);
}

// 获取设定的系统、驱动参数、当前状态参数
SYSLIST& GetSysList()
{
	return SysList;
}

DRVLIST& GetDrvList()
{
	return DrvList;
}

CSVLIST& GetCurStatus(short axis)
{
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			CsvList.lp[i] = ReadLP(1 << i);
			CsvList.ep[i] = ReadEP(1 << i);
			CsvList.cv[i] = ReadCV(1 << i);
			CsvList.ca[i] = ReadCA(1 << i);
		}
	}
	return CsvList;
}

// 安装中断
void HookInt(void interrupt (*NewInt)(...), unsigned int irq)
{
	irq_adr = irq;
	disable();
		OldInt = getvect(irq_adr);
		setvect(irq_adr, NewInt);
		outportb(0x21, 0);
		IntMask = inportb(0x21);
	enable();
	EnableWR5(0xc000);	// 连续、位插补中断使能
}

// 取消中断 
void UnhookInt()
{
	disable();
		setvect(irq_adr, OldInt);
		outportb(0x21, IntMask);
	enable();
}

/////////////////////////////////////////////////////////////////////
//
// 参数转换函数(Jerk-K, Acc-A, Speed-V) 
// Jerk pps/s^2,	Acc pps/s,		Speed pps
long JerkToK(short axis, long dwdata)
{
	long ret;
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			ret = 62.5e6 * SysList.m[i] / dwdata;
			break;
		}
	}
	return ret;
}

short AccToA(short axis, long wdata)
{
	short ret;
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			ret = wdata / SysList.m[i] / 125;
			break;
		}
	}
	return ret;
}

short SpeedToV(short axis, long wdata)
{
	short ret;
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			ret = wdata / SysList.m[i];
			break;
		}
	}
	return ret;
}

long KToJerk(short axis, long dwdata)
{
	long ret;
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			ret = 62.5e6 * SysList.m[i] / dwdata;
			break;
		}
	}
	return ret;
}

long AToAcc(short axis, short wdata)
{
	long ret;
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			ret = wdata * 125 * SysList.m[i];
			break;
		}
	}
	return ret;
}

long VToSpeed(short axis, short wdata)
{
	long ret;
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			ret = wdata * SysList.m[i];
		}
	}
	return ret;
}

// 等待驱动停止(不能用于插补)
void WaitForStop(short axis)
{
	unsigned int flag;
	while (TRUE)
	{
		flag = GetRR0();
		if (flag & 0xf0)
		{
			SetError();		// 驱动出错退出
			break;
		}
		if ((flag & axis) == 0)
		{
			break;			// 驱动正常退出
		}
	}
}

// 等待下一次插补
void WaitForNext()
{
	unsigned int flag;
	while (TRUE)
	{
		flag = GetRR0();
		if (flag & 0xf0)
		{
			SetError();		// 驱动出错退出
			break;
		}
		if ((flag & 0x200) == 0)
		{
			break;			// 插补完毕，准备进行下一次插补
		}
	}
}

/////////////////////////////////////////////////////////////////////
//
// 单轴正负向运动(轴号，终点，初始速度，驱动速度，加速度，减速度，加加速，倍率)可带梯形、S曲线加/减速
// 注意：1.根据p的符号来判断方向
//		 2.在使用d,k后如不想使用还需要手动关掉独立减速度和S曲线开关
//       3.所有参数单位均使用真实值 PPS, PPS/s, PPS/s^2
//		 4.不能同时选择2个轴
void SingleAxisDrive(short axis, long p, long sv, long v, long a, long d, long k, float m)
{
	int i, j;
	for (i = 0; i < AXIS_NUM; i++)
	{
		j = 1 << i;
		if (axis & j)
		{
			if (m != NULL)
				SetM(j, m);
			
			if (sv != NULL)
				SetSV(j, SpeedToV(j, sv));
			if (v != NULL)
				SetV(j, SpeedToV(j, v));
			
			if (a != NULL)
				SetA(j, AccToA(j, a));
			if (d != NULL)
			{
				EnableWR3(j, 0x2);			// 使用独立减速度
				SetD(j, AccToA(j, d));
			}
			
			if (k != NULL)
			{
				EnableWR3(j, 0x4);			// 使用S曲线加减速
				SetK(j, JerkToK(j, k));
			}

			if (p > 0)
			{
				SetP(j, p);
				Command(j, DRIVE_P);	// 正向驱动
			}
			else
			{
				SetP(j, -p);
				Command(j, DRIVE_N);	// 负向驱动
			}
			break;
		} // end if
	} // end for
}

// 单轴定速正负向运动(根据v的符号来判断方向)
void ConstSpeedDrive(short axis, long v, long p)
{
	for (int i = 0; i < AXIS_NUM; i++)
	{
		if (axis & (1 << i))
		{
			if (v < 0)		// 负向驱动
			{
				SetSV(1 << i, SpeedToV(1 << i, -v));
				SetV(1 << i, SpeedToV(1 << i, -v));
				if (p == NULL)
				{
					Command(1 << i, DRIVE_CN);
				}
				else
				{
					SetP(1 << i, p);
					Command(1 << i, DRIVE_N);
				}
			}
			else			// 正向驱动
			{
				SetSV(1 << i, SpeedToV(1 << i, v));
				SetV(1 << i, SpeedToV(1 << i, v));
				if (p == NULL)
				{
					Command(1 << i, DRIVE_CP);
				}
				else
				{
					SetP(1 << i, p);
					Command(1 << i, DRIVE_P);
				}
			}
			break;
		} // end if
	} // end for
}

// 两轴直线插补(1轴终点，2轴终点，初始速度，驱动速度，加速度，减速度，手动减速点)
// 注意：1.两轴匀速插补时M2 = 1.414M1;
//		 2.在使用d后如不想使用还需要手动关掉独立减速度开关
//       3.所有参数单位均使用真实值 PPS, PPS/s, PPS/s^2
void Line2D(long p1, long p2, long sv, long v, long a, long d, short dp)
{
	for (int i = 1; i < 3; i++)
	{
		if (sv != NULL)
			SetSV(i, SpeedToV(i, sv));
		if (v != NULL)
			SetV(i, SpeedToV(i, v));
		if (a != NULL)
			SetA(i, AccToA(i, a));
		if (d != NULL)
		{
			EnableWR3(i, 0x2);			// 使用独立减速度
			SetD(i, AccToA(i, d));
		}
		if (dp != NULL)
		{
			EnableWR3(i, 0x1);			// 手动减速
			SetDP(i, dp);
		}
	}
	SetP(AXIS_X, p1);
	SetP(AXIS_Y, p2);
	Command(AXIS_XY, CMD_LINE);
}

// 两轴圆弧插补
void Circle2D(short mode, long c1, long c2, long p1, long p2, long sv, long v, long a, long d, short dp)
{
	for (int i = 1; i < 3; i++)
	{
		if (sv != NULL)
			SetSV(i, SpeedToV(i, sv));
		if (v != NULL)
			SetV(i, SpeedToV(i, v));
		if (a != NULL)
			SetA(i, AccToA(i, a));
		if (d != NULL)
		{
			EnableWR3(i, 0x2);			// 使用独立减速度
			SetD(i, AccToA(i, d));
		}
		if (dp != NULL)
		{
			EnableWR3(i, 0x1);			// 手动减速
			SetDP(i, dp);
		}
	}
	SetC(AXIS_X, c1);
	SetC(AXIS_Y, c2);
	SetP(AXIS_X, p1);
	SetP(AXIS_Y, p2);
	
	if (mode == CW)
		Command(AXIS_XY, CMD_CW);		// CW
	else if (mode == CCW)
		Command(AXIS_XY, CMD_CCW);		// CCW
	else
	{
		SetError();
		return;
	}
}

//
//	CMEE of UESTC CopyRight (C) 2003
//
//////////////////////////////////////////////////////////////////////