motor.c

双轴步进电机同步动作

#include "common.h"
#include "global.h"
#include "GpioDriver.h"
#include "motor.h"
#include "Keyb_glb.h"
#include "lwDrvKeyb_scan.h"

S32 curx, cury;//当前坐标XY值
char str[6];




CONST U16 Cos[999] =
{
2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,
2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,
2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,
2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,
2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,2048,

2048,2048,2048,2048,2048,2048,2048,2048,2048,2047,2047,2047,2047,2047,2047,2047,2046,2046,2046,2046,
2046,2045,2045,2045,2044,2044,2044,2043,2043,2043,2042,2042,2042,2041,2041,2040,2040,2039,2039,2039,
2038,2038,2037,2036,2036,2035,2035,2034,2034,2033,2032,2032,2031,2031,2030,2029,2028,2028,2027,2026,
2026,2025,2024,2023,2023,2022,2021,2020,2019,2018,2018,2017,2016,2015,2014,2013,2012,2011,2010,2009,
2008,2007,2006,2005,2004,2003,2002,2001,2000,1999,1998,1997,1996,1995,1993,1992,1991,1990,1989,1987,
1986,1985,1984,1983,1981,1980,1979,1977,1976,1975,1973,1972,1971,1969,1968,1967,1965,1964,1962,1961,
1959,1958,1957,1955,1954,1952,1951,1949,1947,1946,1944,1943,1941,1940,1938,1936,1935,1933,1931,1930,
1928,1926,1925,1923,1921,1920,1918,1916,1914,1913,1911,1909,1907,1905,1904,1902,1900,1898,1896,1894,
1892,1891,1889,1887,1885,1883,1881,1879,1877,1875,1873,1871,1869,1867,1865,1863,1861,1859,1857,1855,
1852,1850,1848,1846,1844,1842,1840,1837,1835,1833,1831,1829,1827,1824,1822,1820,1818,1815,1813,1811,
1808,1806,1804,1801,1799,1797,1794,1792,1790,1787,1785,1783,1780,1778,1775,1773,1770,1768,1766,1763,
1761,1758,1756,1753,1751,1748,1746,1743,1740,1738,1735,1733,1730,1728,1725,1722,1720,1717,1714,1712,
1709,1707,1704,1701,1698,1696,1693,1690,1688,1685,1682,1679,1677,1674,1671,1668,1666,1663,1660,1657,
1654,1652,1649,1646,1643,1640,1637,1635,1632,1629,1626,1623,1620,1617,1614,1611,1608,1605,1603,1600,
1597,1594,1591,1588,1585,1582,1579,1576,1573,1570,1567,1564,1561,1558,1554,1551,1548,1545,1542,1539,
1536,1533,1530,1527,1524,1520,1517,1514,1511,1508,1505,1502,1498,1495,1492,1489,1486,1483,1479,1476,
1473,1470,1466,1463,1460,1457,1454,1450,1447,1444,1440,1437,1434,1431,1427,1424,1421,1418,1414,1411,
1408,1404,1401,1398,1394,1391,1388,1384,1381,1378,1374,1371,1368,1364,1361,1357,1354,1351,1347,1344,
1340,1337,1334,1330,1327,1323,1320,1317,1313,1310,1306,1303,1299,1296,1292,1289,1286,1282,1279,1275,
1272,1268,1265,1261,1258,1254,1251,1247,1244,1240,1237,1233,1230,1226,1223,1219,1216,1212,1209,1205,
1202,1198,1195,1191,1188,1184,1181,1177,1174,1170,1167,1163,1159,1156,1152,1149,1145,1142,1138,1135,
1131,1127,1124,1120,1117,1113,1110,1106,1103,1099,1095,1092,1088,1085,1081,1078,1074,1070,1067,1063,
1060,1056,1053,1049,1045,1042,1038,1035,1031,1028,1024,1020,1017,1013,1010,1006,1003,999,995,992,
988,985,981,978,974,970,967,963,960,956,953,949,945,942,938,935,931,928,924,921,
917,913,910,906,903,899,896,892,889,885,881,878,874,871,867,864,860,857,853,850,
846,843,839,836,832,829,825,822,818,815,811,808,804,801,797,794,790,787,783,780,
776,773,769,766,762,759,756,752,749,745,742,738,735,731,728,725,721,718,714,711,
708,704,701,697,694,691,687,684,680,677,674,670,667,664,660,657,654,650,647,644,
640,637,634,630,627,624,621,617,614,611,608,604,601,598,594,591,588,585,582,578,
575,572,569,565,562,559,556,553,550,546,543,540,537,534,531,528,524,521,518,515,
512,509,506,503,500,497,494,490,487,484,481,478,475,472,469,466,463,460,457,454,
451,448,445,443,440,437,434,431,428,425,422,419,416,413,411,408,405,402,399,396,
394,391,388,385,382,380,377,374,371,369,366,363,360,358,355,352,350,347,344,341,
339,336,334,331,328,326,323,320,318,315,313,310,308,305,302,300,297,295,292,290,
287,285,282,280,278,275,273,270,268,265,263,261,258,256,254,251,249,247,244,242,
240,237,235,233,230,228,226,224,221,219,217,215,213,211,208,206,204,202,200,198,
196,193,191,189,187,185,183,181,179,177,175,173,171,169,167,165,163,161,159,157,
156,154,152,150,148,146,144,143,141,139,137,135,134,132,130,128,127,125,123,122,
120,118,117,115,113,112,110,108,107,105,104,102,101,99,97,96,94,93,91,90,
89,87,86,84,83,81,80,79,77,76,75,73,72,71,69,68,67,65,64,63,
62,61,59,58,57,56,55,53,52,51,50,49,48,47,46,45,44,43,42,41,
40,39,38,37,36,35,34,33,32,31,30,30,29,28,27,26,25,25,24,23,
22,22,21,20,20,19,18,17,17,16,16,15,14,14,13,13,12,12,11,10,
10,9,9,9,8,8,7,7,6,6,6,5,5,5,4,4,4,3,3,3,
2,2,2,2,2,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0

};

static void motor_step(U32 kGpio, S32 kSpeed)
{
	int k;
	if(GpioDrv_Read(GPIO_9) && GpioDrv_Read(GPIO_24) && GpioDrv_Read(GPIO_22) && GpioDrv_Read(GPIO_23))
	{
		GpioDrv_SetLow(kGpio);
		for(k = 0; k < 768; k ++);
		GpioDrv_SetHigh(kGpio);
		for(k = 0; k < kSpeed; k ++);
	}
}

Void motor_init()
{
    GpioDrv_SetHigh(GPIO_X_DATA | GPIO_Y_DATA);
    GpioDrv_SetOut(GPIO_X_DATA | GPIO_Y_DATA);
    GpioDrv_SetHigh(GPIO_X_DIR | GPIO_Y_DIR);
    GpioDrv_SetOut(GPIO_X_DIR | GPIO_Y_DIR);
    
   	GpioDrv_SetLow(GPIO_DRAG_VALVE | GPIO_SUCK_VALVE | GPIO_SUCK_CTRL);
    GpioDrv_SetOut(GPIO_DRAG_VALVE | GPIO_SUCK_VALVE | GPIO_SUCK_CTRL);
    
    GpioDrv_SetIn(GPIO_9 | GPIO_24 | GPIO_22 | GPIO_23);
}

Void NEW_XY_RUN(S32 X_coordinate, S32 Y_coordinate)
{
	S32 k;
	S32 tarx, tary;
	S32 tmr, xtmr, ytmr;

	S32 jkjk;
	S32 kSpeed;
	U32 tmptmr;
	
	tarx = X_coordinate;
	tary = Y_coordinate;

	if((tarx == curx) && (tary == cury))
		return;
	
	else if((tarx != curx) && (tary == cury))	
	{
		if(tarx > curx)	
		{
			GpioDrv_SetHigh(GPIO_X_DIR);
			tmr = tarx - curx;
		}
		else
		{
			GpioDrv_SetLow(GPIO_X_DIR);
			tmr =curx  - tarx;
		}
	

		if (tmr<=ADD_STEP*3)
			{
				kSpeed=S_SPEED;
				while(tmr --)
				{
					motor_step(GPIO_X_DATA, kSpeed);
				}
			}		
		else
			{
				jkjk=tmr-ADD_STEP;
				while(tmr --)
				{
					if(tmr > jkjk-1)
						kSpeed=H_SPEED+Cos[(int)((ADD_STEP+jkjk-tmr)*999/ADD_STEP)]*DIST_SPEED/2048;
					else if(tmr < (ADD_STEP*2))
						kSpeed=H_SPEED+2*(Cos[(int)(tmr*999/(ADD_STEP*2))]*DIST_SPEED/2048);
					else
						kSpeed=H_SPEED;
					
					motor_step(GPIO_X_DATA, kSpeed);
				}
			}
			
		curx = tarx;
		sprintf(str,"%05D",curx);
		drvLcd_PrintfEnglishWord(0,8,(U8*)str);
	}
	
	else if((tarx == curx) && (tary != cury))
	{
		if(tary > cury)
		{
			GpioDrv_SetHigh(GPIO_Y_DIR);
			tmr = tary - cury;
		}
		else
		{
			GpioDrv_SetLow(GPIO_Y_DIR);
			tmr =cury  - tary;
		}
	
		if (tmr<=ADD_STEP*3)
			{
				kSpeed=S_SPEED;
				while(tmr --)
				{
					motor_step(GPIO_Y_DATA, kSpeed);
				}
			}		
		else
			{
				jkjk=tmr-ADD_STEP;
				while(tmr --)
				{
					if(tmr > jkjk-1)
						kSpeed=H_SPEED+Cos[(int)((ADD_STEP+jkjk-tmr)*999/ADD_STEP)]*DIST_SPEED/2048;
					else if(tmr < (ADD_STEP*2))
						kSpeed=H_SPEED+2*(Cos[(int)(tmr*999/(ADD_STEP*2))]*DIST_SPEED/2048);
					else
						kSpeed=H_SPEED;
					
					motor_step(GPIO_Y_DATA, kSpeed);
				}
			}
			
		cury = tary;
		sprintf(str,"%05D",cury);
		drvLcd_PrintfEnglishWord(2,8,(U8*)str);
	}
	
	else
	{
		if(tarx > curx)
		{
			GpioDrv_SetHigh(GPIO_X_DIR);
			xtmr = tarx - curx;
		}
		else
		{
			GpioDrv_SetLow(GPIO_X_DIR);
			xtmr =curx  - tarx;
		}
		
		if(tary > cury)
		{
			GpioDrv_SetHigh(GPIO_Y_DIR);
			ytmr = tary - cury;
		}
		else
		{
			GpioDrv_SetLow(GPIO_Y_DIR);
			ytmr =cury  - tary;
		}
		
		if(xtmr == ytmr)
		{
			tmr = xtmr;
			if (tmr<=ADD_STEP*3)
			{
				kSpeed=S_SPEED;
				while(tmr --)
				{
					motor_step(GPIO_Y_DATA | GPIO_X_DATA, kSpeed);
				}
			}		
			else
			{
				jkjk=tmr-ADD_STEP;
				while(tmr --)
				{
					if(tmr > jkjk-1)
						kSpeed=H_SPEED+Cos[(int)((ADD_STEP+jkjk-tmr)*999/ADD_STEP)]*DIST_SPEED/2048;
					else if(tmr < ADD_STEP*2)
						kSpeed=H_SPEED+2*(Cos[(int)(tmr*999/(ADD_STEP*2))]*DIST_SPEED/2048);
					else
						kSpeed=H_SPEED;
					
					motor_step(GPIO_Y_DATA | GPIO_X_DATA, kSpeed);
				}
			}
		}
		
		else if(xtmr > ytmr)
		{
			
			tmr = xtmr;
			tmptmr = 0;
			
			if (tmr<=ADD_STEP*3)
				{
					kSpeed=S_SPEED;
					while(tmr --)
					{
						if((xtmr-tmr)*ytmr*10 > ((tmptmr+1)*10-5)*xtmr)
						{	
							motor_step(GPIO_Y_DATA |GPIO_X_DATA, kSpeed);
							tmptmr = tmptmr + 1;
						}
						else
							motor_step(GPIO_X_DATA, kSpeed);
					}
				}		
			else
				{
					jkjk=tmr-ADD_STEP;
					while(tmr --)
					{
						if(tmr > jkjk-1)
							kSpeed=H_SPEED+Cos[(int)((ADD_STEP+jkjk-tmr)*999/ADD_STEP)]*DIST_SPEED/2048;
						else if(tmr < (ADD_STEP*2))
							kSpeed=H_SPEED+2*(Cos[(int)(tmr*999/(ADD_STEP*2))]*DIST_SPEED/2048);
						else
							kSpeed=H_SPEED;
						
						if((xtmr-tmr)*ytmr*10 > ((tmptmr+1)*10-5)*xtmr)
						{	
							motor_step(GPIO_Y_DATA |GPIO_X_DATA, kSpeed);
							tmptmr = tmptmr + 1;
						}
						else
							motor_step(GPIO_X_DATA, kSpeed);
					}
				}
		
		}
		
		else
		{

			
			tmr = ytmr;
			tmptmr = 0;
			
			if (tmr<=ADD_STEP*3)
				{
					kSpeed=S_SPEED;
					while(tmr --)
					{
						if((ytmr-tmr)*xtmr*10 > ((tmptmr+1)*10-5)*ytmr)
						{
							motor_step(GPIO_Y_DATA |GPIO_X_DATA, kSpeed);
							tmptmr = tmptmr + 1;
						}
						else
							motor_step(GPIO_Y_DATA, kSpeed);
					}
				}		
			else
				{
					jkjk=tmr-ADD_STEP;
					while(tmr --)
					{
						if(tmr > jkjk-1)
							kSpeed=H_SPEED+Cos[(int)((ADD_STEP+jkjk-tmr)*999/ADD_STEP)]*DIST_SPEED/2048;
						else if(tmr < (ADD_STEP*2))
							kSpeed=H_SPEED+2*(Cos[(int)(tmr*999/(ADD_STEP*2))]*DIST_SPEED/2048);
						else
							kSpeed=H_SPEED;
						
						if((ytmr-tmr)*xtmr*10 > ((tmptmr+1)*10-5)*ytmr)
						{
							motor_step(GPIO_Y_DATA |GPIO_X_DATA, kSpeed);
							tmptmr = tmptmr + 1;
						}
						else
							motor_step(GPIO_Y_DATA, kSpeed);
					}
				}
		
				
		}
	
		curx = tarx;						
		cury = tary;
		sprintf(str,"%05D",curx);
		drvLcd_PrintfEnglishWord(0,8,(U8*)str);
		sprintf(str,"%05D",cury);
		drvLcd_PrintfEnglishWord(2,8,(U8*)str);

	
	}
}