main.c

自动控制电机程序

							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((disknum - DiskNumNow) < 0)
				{
					for(i=0;i<(disknum + 20 - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
			else if((DiskNumNow > 20) & (DiskNumNow < 41))
			{
				M3Position_ini();	//左右电机初始化
				Motor(3,0,80,4);	//顺转，走固定80步
				while((IO1PIN & M3P)>>21 == 1)	//逆转，走到3不挡为止
				{
					Motor(3,1,1,4);
				}
				Motor(3,1,DiskNum01,4);	//逆转，走固定380步
				if((disknum + 20 - DiskNumNow) > 0)
				{
					for(i=0;i<(disknum + 20 - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((disknum + 20 - DiskNumNow) < 0)
				{
					for(i=0;i<(disknum + 40 - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
			else if((DiskNumNow > 40) & (DiskNumNow < 46))
			{
				M3Position_ini();	//左右电机初始化
				Motor(3,0,80,4);	//顺转，走固定80步
				while((IO1PIN & M3P)>>21 == 1)	//逆转，走到3不挡为止
				{
					Motor(3,1,1,4);
				}
				Motor(3,1,DiskNum01,4);	//逆转，走固定380步
				if((disknum - ((DiskNumNow - 41) * 4 + 2)) > 0)
				{
					for(i=0;i<(disknum - ((DiskNumNow - 41) * 4 + 2));i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((disknum - ((DiskNumNow - 41) * 4 + 2)) < 0)
				{
					for(i=0;i<(disknum + 20 - ((DiskNumNow - 41) * 4 + 2));i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
		}
		else if((disknum > 20) & (disknum < 41))	//21～40号杯
		{
			if(DiskNumNow < 21)
			{
				//M3Position_ini();	//左右电机初始化
				//Motor(3,0,80,4);	//顺转，走固定80步
				//while((IO1PIN & M3P)>>21 == 1)	//逆转，走到3不挡为止
				//{
				//	Motor(3,1,1,4);
				//}
				Motor(3,1,DiskNum21-DiskNum01,4);	//逆转，走固定460步
				if((disknum - 20 - DiskNumNow) > 0)
				{
					for(i=0;i<(disknum - 20 - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((disknum - 20 - DiskNumNow) < 0)
				{
					for(i=0;i<(disknum - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
			else if((DiskNumNow > 20) & (DiskNumNow < 41))
			{
				if((disknum - DiskNumNow) > 0)
				{
					for(i=0;i<(disknum - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((disknum - DiskNumNow) < 0)
				{
					for(i=0;i<(disknum + 20 - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
			else if((DiskNumNow > 40) & (DiskNumNow < 46))
			{
				M3Position_ini();	//左右电机初始化
				Motor(3,0,80,4);	//顺转，走固定80步
				while((IO1PIN & M3P)>>21 == 1)	//逆转，走到3不挡为止
				{
					Motor(3,1,1,4);
				}
				Motor(3,1,DiskNum21,4);	//逆转，走固定460步
				if((disknum - ((DiskNumNow - 41) * 4 + 22)) > 0)
				{
					for(i=0;i<(disknum - ((DiskNumNow - 41) * 4 + 22));i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((disknum - ((DiskNumNow - 41) * 4 + 22)) < 0)
				{
					for(i=0;i<(disknum + 20 - ((DiskNumNow - 41) * 4 + 22));i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
		}
		else if((disknum > 40) & (disknum < 46))	//41～45号杯
		{
			if(DiskNumNow < 21)
			{
				//M3Position_ini();	//左右电机初始化
				//Motor(3,0,80,4);	//顺转，走固定80步
				//while((IO1PIN & M3P)>>21 == 1)	//逆转，走到3不挡为止
				//{
				//	Motor(3,1,1,4);
				//}
				Motor(3,1,DiskNum41-DiskNum01,4);	//逆转，走固定590步
				if((((disknum - 41) * 4 + 2) - DiskNumNow) > 0)
				{
					for(i=0;i<(((disknum - 41) * 4 + 2) - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((((disknum - 41) * 4 + 2) - DiskNumNow) < 0)
				{
					for(i=0;i<(((disknum - 41) * 4 + 2) + 20 - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
			else if((DiskNumNow > 20) & (DiskNumNow < 41))
			{
				//M3Position_ini();	//左右电机初始化
				//Motor(3,0,80,4);	//顺转，走固定80步
				//while((IO1PIN & M3P)>>21 == 1)	//逆转，走到3不挡为止
				//{
				//	Motor(3,1,1,4);
				//}
				Motor(3,1,DiskNum41-DiskNum21,4);	//逆转，走固定590步
				if((((disknum - 41) * 4 + 22) - DiskNumNow) > 0)
				{
					for(i=0;i<(((disknum - 41) * 4 + 22) - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((((disknum - 41) * 4 + 22) - DiskNumNow) < 0)
				{
					for(i=0;i<(((disknum - 41) * 4 + 22) + 20 - DiskNumNow);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
			else if((DiskNumNow > 40) & (DiskNumNow < 46))
			{
				if((disknum - DiskNumNow) > 0)
				{
					for(i=0;i<((disknum - DiskNumNow) * 4);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
				else if((disknum - DiskNumNow) < 0)
				{
					for(i=0;i<((disknum + 5 - DiskNumNow) * 4);i++)
					{
						if((IO1PIN & M4D)>>20 == 0)//正好对着光耦2了（为了接下来的“逆转，走到2不挡为止”顺利进行）
						{
							if((60-CDiskStep) >= 0)
							{
								Motor(4,1,60-CDiskStep,4);//逆走60-CDiskStep步
							}
						}
						while((IO1PIN & M4D)>>20 == 1)	//逆转，走到2不挡为止
						{
							Motor(4,1,1,4);
						}
						Motor(4,1,90-60+CDiskStep,4);
					}
				}
			}
		}
		DiskNumNow = disknum;
	}
}
void GetSample(int samplenum,int sample,int add1num,int add1,int add2num,int add2,int add3num,int add3)	//取样
{
	int i;
	if(SetFlag == 1)
	{
		if(CleanFlag == 1)
		{
			CL();	//对准清洗槽
			Motor(2,1,SetCLStep,4);	//向下走SetCLStep步，进入清洗槽
			UART1_SendStr(PumpIniQ);	//向串口发送字符串，泵初始化
			DelayNS(800);	//延时
			for(i=0;i<CleanNumber;i++)
			{
				UART1_SendStr(PumpCln);
				DelayNS(900);	//延时
				UART1_SendStr(PumpCln);
				DelayNS(900);	//延时
			}
			UART1_SendStr(PumpIni);	//向串口发送字符串，泵初始化
			DelayNS(800);	//延时

			M2Position_ini();	//往上走到初始位置

			CleanNumber = 0;
			CleanFlag   = 0;
		}

		M2Position_ini();	//往上走到初始位置

		UART1_SendStr(PumpAir);	//吸40ul空气
		DelayNS(400);	//延时
		
		//SD();	//对准样品盘
		M2Position_ini();	//上下电机初始化
		M3Position_ini();	//左右电机初始化
		//M4Position_ini();	//盘电机初始化
		Motor(3,0,80,4);	//顺转，走固定80步
		while((IO1PIN & M3P)>>21 == 1)	//逆转，走到3不挡为止
		{
			Motor(3,1,1,4);
		}

		if((DiskNumNow > 0) & (DiskNumNow < 21))
		{
			Motor(3,1,DiskNum01,4);	//逆转，走固定380步
		}
		else if((DiskNumNow > 20) & (DiskNumNow < 41))
		{
			Motor(3,1,DiskNum21,4);	//逆转，走固定460步
		}
		else if((DiskNumNow > 40) & (DiskNumNow < 46))
		{
			Motor(3,1,DiskNum41,4);	//逆转，走固定590步
		}

		Motor(2,1,80,4);	//先向下走80步
		while((IO1PIN & M2D)>>17 == 1)	//往下，走到6不挡为止
		{
			Motor(2,1,1,4);
		}
		Motor(2,1,40,4);	//再向下走40步

		if(Add1Flag == 1)
		{
			MoveDisk(add1num);
			Motor(2,1,SetSDStep,4);	//向下走SetSDStep步，进入样品盘

			DelayNS(100);	//延时
			inStep(add1+40,PumpGet);
			UART1_SendStr(PumpGet);	//取样
			DelayNS(600);	//延时

			M2Position_ini();	//往上走到初始位置
			Motor(2,1,80,4);	//先向下走80步
			while((IO1PIN & M2D)>>17 == 1)	//往下，走到6不挡为止
			{
				Motor(2,1,1,4);