MCX312运动控制源码

#include "iom1280.h" #include <inavr.h> #include <ina90.h> #include "mcx312.h" #include "main.h" #include "X5sys.h" #include "IDT.h" unsigned int R,L; unsigned char ch; unsigned int LX[2],LY[2]; __flash float vsin[] = {0, 0.0087,0.0175,0.0262,0.0349,0.0436,0.0523,0.0611,0.0698,0.0785,0.0872, 0.0958,0.1045,0.1132,0.1219,0.1305,0.1392,0.1478,0.1564,0.165,0.1736, 0.1822,0.1908,0.1994,0.2079,0.2164,0.2250,0.2334,0.2419,0.2504,0.2588, 0.2672,0.2756,0.2840,0.2924,0.3007,0.3090,0.3173,0.3256,0.3338,0.3420, 0.3502,0.3584,0.3665,0.3746,0.3827,0.3907,0.3987,0.4067,0.4147,0.4226, 0.4305,0.4384,0.4462,0.4540,0.4617,0.4695,0.4772,0.4848,0.4924,0.5000, 0.5075,0.5150,0.5225,0.5299,0.5373,0.5446,0.5519,0.5592,0.5664,0.5736, 0.5807,0.5878,0.5948,0.6018,0.6088,0.6157,0.6225,0.6293,0.6361,0.6428, 0.6494,0.6561,0.6626,0.6691,0.6756,0.6820,0.6884,0.6947,0.7009,0.7071, 0.7133,0.7193,0.7254,0.7314,0.7373,0.7431,0.7490,0.7547,0.7604,0.7660, 0.7716,0.7771,0.7826,0.7880,0.7934,0.7986,0.8039,0.809,0.8141,0.8192, 0.8241,0.8290,0.8339,0.8387,0.8434,0.8480,0.8526,0.8572,0.8616,0.8660, 0.8704,0.8746,0.8788,0.8829,0.8870,0.8910,0.8949,0.8988,0.9026,0.9063, 0.9097,0.9135,0.9171,0.9205,0.9239,0.9272,0.9304,0.9336,0.9367,0.9397, 0.9426,0.9455,0.9483,0.9511,0.9537,0.9563,0.9588,0.9613,0.9636,0.9659, 0.9681,0.9703,0.9724,0.9744,0.9763,0.9781,0.9799,0.9816,0.9833,0.9848, 0.9863,0.9877,0.9890,0.9903,0.9914,0.9925,0.9938,0.9945,0.9954,0.9962, 0.9969,0.9976,0.9981,0.9986,0.9990,0.9994,0.9997,0.9998,0.9999,1}; extern void ReadLP(unsigned char axis); extern void ZXZbu(unsigned int n); extern void ZXFbu(unsigned int n); extern void FXZbu(unsigned int n); extern void FXFbu(unsigned int n); extern void ZYZbu(unsigned int n); extern void ZYFbu(unsigned int n); extern void FYZbu(unsigned int n); extern void FYFbu(unsigned int n); void delay(unsigned int n) {unsigned int i,j; for(i=0;i<n;i++) {__watchdog_reset(); for(j=0;j<1000;j++) _NOP(); } } void outpw(unsigned char * p,unsigned char ucData) { * p = ucData; } unsigned char inputw(unsigned char * p) {unsigned char count; count = *p; return(count); } void Write_WR0(unsigned char axis,unsigned char cmd) { outpw((unsigned char *)(adr+WR0H),axis); outpw((unsigned char *)(adr+WR0L),cmd); } void Write_WR1(unsigned char axis,long wdata) { outpw((unsigned char *)(adr+WR0H),axis); outpw((unsigned char *)(adr+WR0L),0x0f); outpw((unsigned char *)(adr+WR1H),wdata >> 8); outpw((unsigned char *)(adr+WR1L),wdata & 0xff); } void Write_WR2(unsigned char axis,long wdata) { outpw((unsigned char *)(adr+WR0H),axis); outpw((unsigned char *)(adr+WR0L),0x0f); outpw((unsigned char *)(adr+WR2H),wdata >> 8); outpw((unsigned char *)(adr+WR2L),wdata & 0xff); } void Write_WR3(unsigned char axis,long wdata) { outpw((unsigned char *)(adr+WR0H),axis); /* 轴设定 */ outpw((unsigned char *)(adr+WR0L),0x0f); /* 轴切换 */ outpw((unsigned char *)(adr+WR3H),wdata >> 8); outpw((unsigned char *)(adr+WR3L),wdata & 0xff); } void Write_WR4(long wdata) { outpw((unsigned char *)(adr+WR4H),wdata >> 8); outpw((unsigned char *)(adr+WR4L),wdata & 0xff); } void Write_WR5(long wdata) { outpw((unsigned char *)(adr+WR5H),wdata >> 8); outpw((unsigned char *)(adr+WR5L),wdata & 0xff); } void Write_data(long wdata1,long wdata2) { outpw((unsigned char *)(adr+WR7H),wdata1 >> 8); outpw((unsigned char *)(adr+WR7L),wdata1 & 0xff); outpw((unsigned char *)(adr+WR6H),wdata2 >> 8); outpw((unsigned char *)(adr+WR6L),wdata2 & 0xff); } void InitMcx312(void) { outpw((unsigned char *)(adr+WR0H),0x80); /* 复位 */ outpw((unsigned char *)(adr+WR0L),0x00); Write_WR2(0x03,0x140); /* X,Y轴设定为 PLUS/DIR 模式 */ delay(1000); } void Wait(unsigned char axis) { while(inputw((unsigned char *)(adr+RR0L)) & axis); /* 各轴输出状态 */ } void Wait_Next(void) { while(!(inputw((unsigned char *)(adr+RR0H)) & 0x02)); } void SetLP(unsigned axis,unsigned int dwdata1,unsigned int dwdata2) /* 设置逻辑位置计数器 */ { outpw((unsigned char *)(adr+WR7H),dwdata1 >> 8); outpw((unsigned char *)(adr+WR7L),dwdata1 & 0xff); outpw((unsigned char *)(adr+WR6H),dwdata2 >> 8); outpw((unsigned char *)(adr+WR6L),dwdata2 & 0xff); outpw((unsigned char *)(adr+WR0H),axis); outpw((unsigned char *)(adr+WR0L),0x09); } void SetEP(unsigned axis,unsigned int dwdata1,unsigned int dwdata2) /* 设置实际位置计数器 */ { outpw((unsigned char *)(adr+WR7H),dwdata1 >> 8); outpw((unsigned char *)(adr+WR7L),dwdata1 & 0xff); outpw((unsigned char *)(adr+WR6H),dwdata2 >> 8); outpw((unsigned char *)(adr+WR6L),dwdata2 & 0xff); outpw((unsigned char *)(adr+WR0H),axis); outpw((unsigned char *)(adr+WR0L),0x0a); } /* 获取逻辑位置计数器 */ void ReadLP(unsigned char axis) { if(axis==0x01) { outpw((unsigned char *)(adr+WR0H),0x01); outpw((unsigned char *)(adr+WR0L),0x10); LX[0]=(inputw((unsigned char *)(adr+RR7H)) << 8) | inputw((unsigned char *)(adr+RR7L)); LX[1]=(inputw((unsigned char *)(adr+RR6H)) << 8) | inputw((unsigned char *)(adr+RR6L)); } if(axis==0x02) { outpw((unsigned char *)(adr+WR0H),0x02); outpw((unsigned char *)(adr+WR0L),0x10); LY[0]=(inputw((unsigned char *)(adr+RR7H)) << 8) | inputw((unsigned char *)(adr+RR7L)); LY[1]=(inputw((unsigned char *)(adr+RR6H)) << 8) | inputw((unsigned char *)(adr+RR6L)); } } /* 获取实际位置计数器 */ void ReadEP(unsigned char axis) { if(axis==0x01) { outpw((unsigned char *)(adr+WR0H),0x01); outpw((unsigned char *)(adr+WR0L),0x11); AX[0]=(inputw((unsigned char *)(adr+RR7H)) << 8) | inputw((unsigned char *)(adr+RR7L)); AX[1]=(inputw((unsigned char *)(adr+RR6H)) << 8) | inputw((unsigned char *)(adr+RR6L)); } if(axis==0x02) { outpw((unsigned char *)(adr+WR0H),0x02); outpw((unsigned char *)(adr+WR0L),0x11); AY[0]=(inputw((unsigned char *)(adr+RR7H)) << 8) | inputw((unsigned char *)(adr+RR7L)); AY[1]=(inputw((unsigned char *)(adr+RR6H)) << 8) | inputw((unsigned char *)(adr+RR6L)); } } void Set_HigSpeed(void) { Write_WR3(0x01,0x01); Write_WR5(0x04); Write_data(0x7a,0x1200); /* 范围:8000000(M=1) */ Write_WR0(0x01,0x00); Write_data(0x00,0x40); Write_WR0(0x01,0x02); /* 设定加速度 */ Write_data(0x00,0x40); Write_WR0(0x01,0x03); /* 设定减速度 */ Write_data(0x00,0x3E8); Write_WR0(0x01,0x04); /* 初始速度 */ Write_data(0x00,0x1388); Write_WR0(0x01,0x05); /* 驱动速度 */ } void Set_LowSpeed(void) { outpw((unsigned char *)(adr+WR5H),0x01); /* 均直线速度 */ outpw((unsigned char *)(adr+WR5L),0x04); /* ax1为X轴,ax2为Y轴 */ Write_data(0x00,MTSpeed); // Write_data(0x00,0x5DC); Write_WR0(0x01,0x04); /* 初始速度 */ Write_data(0x00,MTSpeed); // Write_data(0x00,0x5DC); Write_WR0(0x01,0x05); /* 驱动速度 */ } void XCW(unsigned int n) { Set_LowSpeed(); INTflag &= ~BIT(0); while(1) { if(INTflag & BIT(0)) break; Write_data(0x00,0x01); Write_WR0(0x01,0x06); /* X轴间隙补偿 */ Write_data(0x00,0x00); Write_WR0(0x02,0x06); Write_WR0(0x00,0x30); Wait(0x03); } Set_HigSpeed(); Write_data(0x00,n); Write_WR0(0x01,0x06); /* 终点 X n mm */ Write_data(0x00,0x00); Write_WR0(0x02,0x06); /* 终点 Y 0 mm */ Write_data(0x00,0x2134); Write_WR0(0x01,0x07); /* 设置减速点 */ Write_WR0(0x00,0x3B); Write_WR0(0x00,0x30); Wait(0x03); } void XCCW(unsigned int n) { Set_LowSpeed(); INTflag &= ~BIT(0); while(1) { if(INTflag & BIT(0)) break; Write_data(0xFFFF,0xFFFE); Write_WR0(0x01,0x06); /* X轴间隙补偿 */ Write_data(0x00,0x00); Write_WR0(0x02,0x06); Wr