//----------------------------------------------------------- // i8091.cpp // // compiled under large mode // // v1.0 6/20/2001 // //----------------------------------------------------------- #include #include #include #include #include #include "8000.h" //------------------------------------ #define i8091 0x0e #define YES 1 #define NO 0 #define ON 1 #define OFF 0 #define CW_CCW 0 #define PULSE_DIR 1 #define NORMAL_DIR 0 #define REVERSE_DIR 1 #define FW 0 #define BW 1 #define CW 0 #define CCW 1 #define X_axis 1 #define Y_axis 2 #define Z_axis 3 #define READY 0 #define BUSY 1 #define pi 3.141592654 //-------------- hardware ------------------------- #define ID 0x00 //R value=0x0E #define DI1 0x01 //R #define DI2 0x02 //R #define WRFF 0x01 //W #define RSTFF 0x02 //W //----state---------------------------------- #define PHASE1 1 #define PHASE2 2 #define PHASE3 3 #define sIDLE 0 #define sXYZ 1 #define sLINE 2 #define sCIRCLE 3 #define sARC 4 //----- command sets ----------------- #define sync_pattern1 0x55 //0101 0101 #define sync_pattern2 0xAA //1010 1010 //-- motion command #define LSP_ORG 0x01 #define HSP_ORG 0x02 #define HSD_ORG 0x03 #define LSP_PULSE_MOVE 0x04 #define HSP_PULSE_MOVE 0x05 #define LSP_MOVE 0x06 #define HSP_MOVE 0x07 #define CSP_MOVE 0x08 #define SLOW_DOWN 0x09 #define SLOW_STOP 0x0A //-- interpolation command #define INTP_PULSE 0x10 #define INTP_LINE 0x11 //-- set variable #define SET_VAR 0x20 #define SET_DEFDIR 0x21 #define SET_MODE 0x22 #define SET_SERVO_ON 0x23 // #define SET_REG 0x24 #define SET_NORMAL_CLOSE 0x25 //-- stop, break command #define RESET_SYSTEM 0x30 #define STOP_AXIS 0x31 #define STOP_ALL 0x32 //-- testing command #define RQST_LS 0x40 #define RQST_MSC 0x41 #define TEST_FIFO 0x42 //******************** for interpolation **************** #define PROFILE_STOP 1 #define PROFILE_BUSY 2 #define d_DDA_cycle 10 #define d_Acc_Dec 1 #define d_Low_Speed 10 #define d_High_Speed 100 //******************************************************** typedef struct { unsigned char exist; unsigned int base; unsigned char DDA_cycle; unsigned char Acc_Dec; unsigned int Low_Speed; unsigned int High_Speed; }i8091_cardtype; i8091_cardtype card[20]; //------ interpolation system parameter -------------------- typedef struct { unsigned char command; unsigned char cardNo; long x; long y; long R; unsigned char dir; unsigned int speed; unsigned char acc_mode; }system_type; system_type sys; //-------------------------------------------------------- unsigned int High_Speed=100; unsigned char fifo_full=0; //------ for interpolation command ------------------------ unsigned long total_pulseX=0; unsigned long total_pulseY=0; unsigned int dx=0; unsigned int dy=0; char x_dir=1; char y_dir=1; unsigned long total_pulse=0; unsigned long rem_pulse=0; unsigned int d_length=0; unsigned char Dec_Flag=0; unsigned char profile_state=0; unsigned char WorkFlag; unsigned long pulse_R=0; unsigned long pulse_R1=0; unsigned long pulse_R2=0; unsigned char MaxAxis=0; double total_length; double radius; double theta,d_theta; double Xn,Yn; double Xr,Yr; double th1,th2,th; double xx1,yy1; double xc,yc; double x_new,y_new; double x_old,y_old; unsigned char state=PHASE1; unsigned char workno=0; //------------------------------------------------------------- //************************************************************* void outportb_fifo(unsigned char cardNo, unsigned char cdata) { while ( (inportb(card[cardNo].base + DI1) & 0x40)==0) //fifo full { fifo_full=YES; }; while ( (inportb(card[cardNo].base + DI1) & 0x40)==0) //fifo full { fifo_full=YES; }; if (fifo_full==YES) Delay(5); outportb(card[cardNo].base + WRFF, cdata); } //---------------------------------------------------- void output_fifo(unsigned char cardNo, unsigned char com_byte0, unsigned char com_byte1, unsigned char com_byte2, unsigned char com_byte3, unsigned char com_byte4, unsigned char com_byte5, unsigned char com_byte6, unsigned char com_byte7, unsigned char com_byte8) { unsigned char parity; fifo_full=NO; parity= com_byte0 ^ com_byte1 ^ com_byte2 ^ com_byte3 ^ com_byte4 ^ com_byte5 ^ com_byte6 ^ com_byte7 ^ com_byte8; workno++; if (workno>0x50) workno=1; outportb_fifo(cardNo,0x00); outportb_fifo(cardNo,sync_pattern1); outportb_fifo(cardNo,sync_pattern2); outportb_fifo(cardNo,com_byte0); outportb_fifo(cardNo,com_byte1); outportb_fifo(cardNo,com_byte2); outportb_fifo(cardNo,com_byte3); outportb_fifo(cardNo,com_byte4); outportb_fifo(cardNo,com_byte5); outportb_fifo(cardNo,com_byte6); outportb_fifo(cardNo,com_byte7); outportb_fifo(cardNo,com_byte8); outportb_fifo(cardNo,parity); outportb_fifo(cardNo,workno); outportb_fifo(cardNo,0x00); outportb_fifo(cardNo,sync_pattern1); outportb_fifo(cardNo,sync_pattern2); outportb_fifo(cardNo,com_byte0); outportb_fifo(cardNo,com_byte1); outportb_fifo(cardNo,com_byte2); outportb_fifo(cardNo,com_byte3); outportb_fifo(cardNo,com_byte4); outportb_fifo(cardNo,com_byte5); outportb_fifo(cardNo,com_byte6); outportb_fifo(cardNo,com_byte7); outportb_fifo(cardNo,com_byte8); outportb_fifo(cardNo,~parity); outportb_fifo(cardNo,workno); } //---------------------------------------------------- void i8091_reset_fifo(unsigned char cardNo) { //1. cancel all of command pending in the fifo outportb(card[cardNo].base + RSTFF,0x00); outportb(card[cardNo].base + RSTFF,0x00); //2. output a null function to complete previous command, // if there is a residue command didn't accept output_fifo(cardNo,0,0,0,0,0,0,0,0,0); } //---------------------------------------------------- unsigned char i8091_REGISTRATION(unsigned char cardNo, unsigned int address) { card[cardNo].exist=NO; if (cardNo>19) return NO; if (inportb(address)!=i8091) return NO; card[cardNo].base = address; card[cardNo].DDA_cycle = d_DDA_cycle; card[cardNo].Acc_Dec = d_Acc_Dec; card[cardNo].Low_Speed = d_Low_Speed; card[cardNo].High_Speed= d_High_Speed; High_Speed = d_High_Speed; card[cardNo].exist=YES; WorkFlag =NO; state =PHASE1; sys.command=sIDLE; return card[cardNo].exist; } //---------------------------------------------------- unsigned char i8091_LIMIT_X(unsigned char cardNo) { return inportb(card[cardNo].base + DI1); } //---------------------------------------------------- unsigned char i8091_LIMIT_Y(unsigned char cardNo) { return inportb(card[cardNo].base + DI2); } //##################################################################### //---------------------------------------------------- void i8091_WAIT_X(unsigned char cardNo) { unsigned char stop1,stop2; if (card[cardNo].exist==NO) return; while ( (inportb(card[cardNo].base + DI1) & 0x20) == 0x20) {}; do { stop1 = inportb(card[cardNo].base + DI2) & 0x40; Delay(10); stop2 = inportb(card[cardNo].base + DI2) & 0x40; } while ( (stop1 == 0x40) || (stop2 == 0x40) ); } //---------------------------------------------------- unsigned char i8091_IS_X_STOP(unsigned char cardNo) { unsigned char stop1,stop2; if ( (inportb(card[cardNo].base + DI1) & 0x20) == 0x20) return NO; stop1 = inportb(card[cardNo].base + DI2) & 0x40; Delay(10); stop2 = inportb(card[cardNo].base + DI2) & 0x40; if ( (stop1 == 0x40) || (stop2 == 0x40) ) return NO; else return YES; } //---------------------------------------------------- void i8091_WAIT_Y(unsigned char cardNo) { unsigned char stop1,stop2; if (card[cardNo].exist==NO) return; while ( (inportb(card[cardNo].base + DI1) & 0x20) == 0x20) {}; do { stop1 = inportb(card[cardNo].base + DI2) & 0x80; Delay(10); stop2 = inportb(card[cardNo].base + DI2) & 0x80; } while ( (stop1 == 0x80) || (stop2 == 0x80) ); } //---------------------------------------------------- unsigned char i8091_IS_Y_STOP(unsigned char cardNo) { unsigned char stop1,stop2; if ( (inportb(card[cardNo].base + DI1) & 0x20) == 0x20) return NO; stop1 = inportb(card[cardNo].base + DI2) & 0x80; Delay(10); stop2 = inportb(card[cardNo].base + DI2) & 0x80; if ( (stop1 == 0x80) || (stop2 == 0x80) ) return NO; else return YES; } //##################################################################### //---------------------------------------------------- void i8091_SET_VAR(unsigned char cardNo, unsigned char set_DDA_cycle, unsigned char set_Acc_Dec, unsigned int set_Low_Speed, unsigned int set_High_Speed) { sys.command=sIDLE; if ((set_DDA_cycle==0) || (set_DDA_cycle>254)) return; if ((set_Acc_Dec==0) || (set_Acc_Dec > 200)) return; if ((set_Low_Speed==0) || (set_Low_Speed > 200)) return; if ((set_High_Speed <= set_Low_Speed) || (set_High_Speed > 2047)) return; card[cardNo].DDA_cycle = set_DDA_cycle; card[cardNo].Acc_Dec = set_Acc_Dec; card[cardNo].Low_Speed = set_Low_Speed; card[cardNo].High_Speed = set_High_Speed; output_fifo(cardNo,SET_VAR, set_DDA_cycle,set_Acc_Dec, *(&( *(char *)&set_Low_Speed )+1), *(&( *(char *)&set_Low_Speed )+0), *(&( *(char *)&set_High_Speed )+1), *(&( *(char *)&set_High_Speed )+0), 0,0); } //---------------------------------------------------- void i8091_SET_DEFDIR(unsigned char cardNo, unsigned char defdirX, unsigned char defdirY) { sys.command=sIDLE; output_fifo(cardNo,SET_DEFDIR, ((defdirY & 0x01) << 1) | (defdirX & 0x01) , 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_SET_MODE(unsigned char cardNo, unsigned char modeX, unsigned char modeY) { sys.command=sIDLE; output_fifo(cardNo,SET_MODE, ((modeY & 0x01) << 1) | (modeX & 0x01) , 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_SET_SERVO_ON(unsigned char cardNo, unsigned char sonX, unsigned char sonY) { sys.command=sIDLE; output_fifo(cardNo,SET_SERVO_ON, ((sonY & 0x01) << 1) | (sonX & 0x01) , 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_SET_NC(unsigned char cardNo, unsigned char sw) { sys.command=sIDLE; output_fifo(cardNo, SET_NORMAL_CLOSE, sw,0,0,0,0,0,0,0); } //##################################################################### //---------------------------------------------------- void i8091_STOP_X(unsigned char cardNo) { sys.command=sIDLE; output_fifo(cardNo,STOP_AXIS,X_axis & 0x03, 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_STOP_Y(unsigned char cardNo) { sys.command=sIDLE; output_fifo(cardNo,STOP_AXIS,Y_axis & 0x03, 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_RESET_SYSTEM(unsigned char cardNo) { unsigned char j; sys.command=sIDLE; WorkFlag=NO; outportb(card[cardNo].base + RSTFF,0x00); outportb(card[cardNo].base + RSTFF,0x00); for (j=0; j<15; j++) { outportb(card[cardNo].base + WRFF, 0x00); }; output_fifo(cardNo,STOP_ALL,0,0,0,0,0,0,0,0); output_fifo(cardNo,RESET_SYSTEM,0,0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_STOP_ALL(unsigned char cardNo) { unsigned char j; sys.command=sIDLE; WorkFlag=NO; //1. cancel all of command pending in the fifo outportb(card[cardNo].base + RSTFF,0x00); outportb(card[cardNo].base + RSTFF,0x00); for (j=0; j<15; j++) { outportb(card[cardNo].base + WRFF, 0x00); }; //2. to immediatelly stop all axes output_fifo(cardNo,STOP_ALL,0,0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_EMG_STOP(unsigned char cardNo) { unsigned char j; unsigned char parity; sys.command=sIDLE; WorkFlag=NO; //1. cancel all of command pending in the fifo outportb(card[cardNo].base + RSTFF,0x00); outportb(card[cardNo].base + RSTFF,0x00); for (j=0; j<15; j++) { outportb(card[cardNo].base + WRFF, 0x00); }; //2. to immediatelly stop all axes---------------- parity= STOP_ALL ^ 0 ^ 0 ^ 0 ^ 0 ^ 0 ^ 0 ^ 0 ^ 0; workno++; if (workno>0x50) workno=1; outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,sync_pattern1); outportb(card[cardNo].base + WRFF,sync_pattern2); outportb(card[cardNo].base + WRFF,STOP_ALL); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,parity); outportb(card[cardNo].base + WRFF,workno); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,sync_pattern1); outportb(card[cardNo].base + WRFF,sync_pattern2); outportb(card[cardNo].base + WRFF,STOP_ALL); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,0); outportb(card[cardNo].base + WRFF,~parity); outportb(card[cardNo].base + WRFF,workno); } //##################################################################### //---------------------------------------------------- void i8091_LSP_ORG(unsigned char cardNo, unsigned char DIR, unsigned char AXIS) { sys.command=sIDLE; output_fifo(cardNo,LSP_ORG,((DIR & 0x01) << 4) | (AXIS & 0x03), 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_HSP_ORG(unsigned char cardNo, unsigned char DIR, unsigned char AXIS) { sys.command=sIDLE; output_fifo(cardNo,HSP_ORG,((DIR & 0x01) << 4) | (AXIS & 0x03), 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_HSD_ORG(unsigned char cardNo, unsigned char DIR, unsigned char AXIS) { sys.command=sIDLE; output_fifo(cardNo,HSD_ORG,((DIR & 0x01) << 4) | (AXIS & 0x03), 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_LSP_PULSE_MOVE(unsigned char cardNo, unsigned char AXIS, long pulseN) { unsigned char DIR; sys.command=sIDLE; if (pulseN>0) DIR=CW; else DIR=CCW; pulseN=labs(pulseN); output_fifo(cardNo,LSP_PULSE_MOVE,((DIR & 0x01) << 4) | (AXIS & 0x03), *(&( *(char *)&pulseN )+3), *(&( *(char *)&pulseN )+2), *(&( *(char *)&pulseN )+1), *(&( *(char *)&pulseN )+0), 0,0,0); } //---------------------------------------------------- void i8091_HSP_PULSE_MOVE(unsigned char cardNo, unsigned char AXIS, long pulseN, unsigned int speed) { unsigned char DIR; sys.command=sIDLE; if (pulseN>0) DIR=CW; else DIR=CCW; pulseN=labs(pulseN); output_fifo(cardNo,HSP_PULSE_MOVE,((DIR & 0x01) << 4) | (AXIS & 0x03), *(&( *(char *)&pulseN )+3), *(&( *(char *)&pulseN )+2), *(&( *(char *)&pulseN )+1), *(&( *(char *)&pulseN )+0), *(&( *(char *)&speed )+1), *(&( *(char *)&speed )+0), 0); } //---------------------------------------------------- void i8091_LSP_MOVE(unsigned char cardNo, unsigned char DIR, unsigned char AXIS) { sys.command=sIDLE; output_fifo(cardNo,LSP_MOVE,((DIR & 0x01) << 4) | (AXIS & 0x03), 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_HSP_MOVE(unsigned char cardNo, unsigned char DIR, unsigned char AXIS) { sys.command=sIDLE; output_fifo(cardNo,HSP_MOVE,((DIR & 0x01) << 4) | (AXIS & 0x03), 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_CSP_MOVE(unsigned char cardNo, unsigned char DIR, unsigned char AXIS, unsigned int move_speed) { sys.command=sIDLE; if (move_speed > 2047) return; output_fifo(cardNo,CSP_MOVE,((DIR & 0x01) << 4) | (AXIS & 0x03), *(&( *(char *)&move_speed )+1), *(&( *(char *)&move_speed )+0), 0,0,0,0,0); } //---------------------------------------------------- void i8091_SLOW_DOWN(unsigned char cardNo, unsigned char AXIS) { sys.command=sIDLE; output_fifo(cardNo, SLOW_DOWN,AXIS & 0x03, 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_SLOW_STOP(unsigned char cardNo, unsigned char AXIS) { sys.command=sIDLE; output_fifo(cardNo,SLOW_STOP,AXIS & 0x03, 0,0,0,0,0,0,0); } //---------------------------------------------------- void i8091_INTP_PULSE(unsigned char cardNo, int Xpulse, int Ypulse) { if (abs(Xpulse) > 2047) return; if (abs(Ypulse) > 2047) return; if (Xpulse < 0) Xpulse = abs(Xpulse) | 0x8000; //CCW if (Ypulse < 0) Ypulse = abs(Ypulse) | 0x8000; //CCW output_fifo(cardNo,INTP_PULSE, *(&( *(char *)&Xpulse )+1), *(&( *(char *)&Xpulse )+0), *(&( *(char *)&Ypulse )+1), *(&( *(char *)&Ypulse )+0), 0,0,0,0); } //---------------------------------------------------- void i8091_INTP_LINE(unsigned char cardNo, long Xpulse, long Ypulse) { sys.command=sIDLE; if ((Xpulse==0) && (Ypulse==0)) return; if (labs(Xpulse) > 524287) return; if (labs(Ypulse) > 524287) return; if (Xpulse < 0) Xpulse = labs(Xpulse) | 0x80000000; //CCW if (Ypulse < 0) Ypulse = labs(Ypulse) | 0x80000000; //CCW output_fifo(cardNo,INTP_LINE, *(&( *(char *)&Xpulse )+3), *(&( *(char *)&Xpulse )+2), *(&( *(char *)&Xpulse )+1), *(&( *(char *)&Xpulse )+0), *(&( *(char *)&Ypulse )+3), *(&( *(char *)&Ypulse )+2), *(&( *(char *)&Ypulse )+1), *(&( *(char *)&Ypulse )+0) ); } //############################################################ //---------------------------------------------------- void i8091_INTP_PROFILE(unsigned char cardNo) { unsigned long Dec_Length; unsigned int CAD; if ( total_pulse < 4*card[cardNo].Low_Speed ) { if ( rem_pulse < card[cardNo].Low_Speed ) { d_length = (unsigned int) rem_pulse; profile_state = PROFILE_STOP; } else d_length = card[cardNo].Low_Speed; } else { Dec_Length = (d_length + card[cardNo].Acc_Dec); Dec_Length = Dec_Length*Dec_Length/(card[cardNo].Acc_Dec*2); if ((Dec_Flag == 0) && (rem_pulse > Dec_Length)) { if (d_length < High_Speed) d_length += card[cardNo].Acc_Dec; if (d_length >= High_Speed) d_length = High_Speed; } else { Dec_Flag=1; //if ((int)(d_length - card[cardNo].Acc_Dec) < (int)card[cardNo].Low_Speed ) // d_length = card[cardNo].Low_Speed; //else // d_length -= card[cardNo].Acc_Dec; CAD =(unsigned int)(((unsigned long)d_length* (unsigned long)d_length )/(2*rem_pulse)); if ((int)(d_length - CAD) < (int)card[cardNo].Acc_Dec ) d_length = card[cardNo].Acc_Dec; else d_length -= CAD; //------------------ if (rem_pulse <= d_length) { d_length = (unsigned int)rem_pulse; profile_state = PROFILE_STOP; } } } rem_pulse -= d_length; } //---------------------------------------------------- void i8091_INTP_PROFILE_CSPD(unsigned int spd) { if (rem_pulse > spd) { d_length = spd; } else { d_length = (unsigned int)rem_pulse; profile_state = PROFILE_STOP; } rem_pulse -= d_length; } //---------------------------------------------------------- unsigned char i8091_LINE_CORE(unsigned char cardNo, long x,long y, unsigned int speed, unsigned char acc_mode) { switch (state) { case PHASE1: WorkFlag=YES; if ((x==0) && (y==0)) return READY; if (speed>2040) speed=2040; High_Speed = speed; if (x<0) x_dir=-1; else x_dir=1; if (y<0) y_dir=-1; else y_dir=1; total_pulseX=labs(x); total_pulseY=labs(y); if (total_pulseX>total_pulseY) total_pulse=total_pulseX; else total_pulse=total_pulseY; total_length=sqrt( (double)(total_pulseX)*(double)(total_pulseX) + (double)(total_pulseY)*(double)(total_pulseY) ); speed = (unsigned int)(speed*(double)(total_pulse)/total_length); High_Speed = speed; rem_pulse=total_pulse; Dec_Flag=0; d_length=0; pulse_R=0; profile_state = PROFILE_BUSY; state=PHASE2; return BUSY; //break; case PHASE2: do { if (WorkFlag==NO) { profile_state=PROFILE_STOP; break; //break while loop }; //---------------------------- if (acc_mode==0) i8091_INTP_PROFILE(cardNo); else i8091_INTP_PROFILE_CSPD(speed); if (total_pulseX>total_pulseY) { dx=d_length; dy=(unsigned int) ((total_pulseY*dx+pulse_R)/total_pulseX); pulse_R =(unsigned int)((total_pulseY*dx+pulse_R)%total_pulseX); } else { dy=d_length; dx=(unsigned int) ((total_pulseX*dy+pulse_R)/total_pulseY); pulse_R =(unsigned int)((total_pulseX*dy+pulse_R)%total_pulseY); } i8091_INTP_PULSE(cardNo, x_dir*dx, y_dir*dy); } while ((profile_state==PROFILE_BUSY) && (fifo_full==NO) ); if (profile_state==PROFILE_STOP) { state=PHASE1; return READY; } else { state=PHASE2; return BUSY; } //break; }; return READY; } //----------------------------------------------------------- unsigned char i8091_CIRCLE_CORE(unsigned char cardNo, long x, long y, unsigned char dir, unsigned int speed, unsigned char acc_mode) { switch (state) { case PHASE1: WorkFlag=YES; if ((x==0) && (y==0)) return READY; if (speed>2040) speed=2040; High_Speed = speed; radius = sqrt((double)(x)*(double)(x)+(double)(y)*(double)(y)); total_pulse = (unsigned long) (2.0*pi*radius); Xr=(double)(-x); Yr=(double)(-y); if (Xr!=0.0) { theta = atan(Yr/Xr); if (Xr<0.0) theta += pi; } else if (Yr>0.0) theta=pi/2.0; else theta=-pi/2.0; Xn=0.0; Yn=0.0; rem_pulse=total_pulse; Dec_Flag=0; d_length=0; profile_state = PROFILE_BUSY; state=PHASE2; return BUSY; //break; case PHASE2: do { if (WorkFlag==NO) { profile_state=PROFILE_STOP; break; //break while loop } //---get d_length----------------------------- if (acc_mode==0) i8091_INTP_PROFILE(cardNo); else i8091_INTP_PROFILE_CSPD(speed); d_theta = (double)(d_length) / radius; if (dir == CW) { xx1 = x + radius*cos(theta - d_theta); yy1 = y + radius*sin(theta - d_theta); theta -= d_theta; } else { xx1 = x + radius*cos(theta + d_theta); yy1 = y + radius*sin(theta + d_theta); theta += d_theta; } if (profile_state == PROFILE_STOP) { xx1=0; yy1=0; } dx = (int)(xx1-Xn); dy = (int)(yy1-Yn); Xn += (double)(dx); Yn += (double)(dy); i8091_INTP_PULSE(cardNo, dx, dy); } while ((profile_state==PROFILE_BUSY) && (fifo_full==NO) ); if (profile_state==PROFILE_STOP) { state=PHASE1; return READY; } else { state=PHASE2; return BUSY; } //break; }; return READY; } //-------------------------------------------------------------------------------- unsigned char i8091_ARC_CORE(unsigned char cardNo, long x, long y, long R, unsigned char dir, unsigned int speed, unsigned char acc_mode) { switch (state) { case PHASE1: WorkFlag=YES; if (R==0) return READY; if (speed>2040) speed=2040; High_Speed = speed; radius = (double)(labs(R)); xx1 = (double)(x); yy1 = (double)(y); if (radius < sqrt(xx1*xx1+yy1*yy1)/2.0) return READY; th = 2.0*asin(sqrt(xx1*xx1+yy1*yy1)/2.0/radius); // calculate th1 if ((R > 0) && (dir == CW)) th1 = (pi-th)/2.0; if ((R > 0) && (dir == CCW)) th1 = -(pi-th)/2.0; if ((R < 0) && (dir == CCW)) th1 = (pi-th)/2.0; if ((R < 0) && (dir == CW)) th1 = -(pi-th)/2.0; // calculate th2 = atan(yy1/xx1) if (xx1!=0.0) { th2 = atan(yy1/xx1); if (xx1<0.0) th2 += pi; } else if (yy1>0.0) th2=pi/2.0; else th2=-pi/2.0; // calculate center of arc xc = xx1/2.0 + radius*sin(th1)*sin(th2); yc = yy1/2.0 - radius*sin(th1)*cos(th2); // calculate theta = atan(-yc/-xc) if (-xc!=0.0) { theta = atan(-yc/-xc); if (-xc<0.0) theta += pi; } else if (-yc>0.0) theta=pi/2.0; else theta=-pi/2.0; // calculate total pulse if (R > 0) total_pulse = (unsigned long) (th*radius); else total_pulse = (unsigned long) ((2.0*pi-th)*radius); x_old=0.0; y_old=0.0; rem_pulse=total_pulse; Dec_Flag=0; d_length=0; profile_state = PROFILE_BUSY; state=PHASE2; return BUSY; //break; case PHASE2: do { if (WorkFlag==NO) { profile_state=PROFILE_STOP; break; //break while loop } //---get d_length----------------------------- if (acc_mode==0) i8091_INTP_PROFILE(cardNo); else i8091_INTP_PROFILE_CSPD(speed); d_theta = (double)(d_length) / radius; if (dir == CW) { x_new = xc + radius*cos(theta - d_theta); y_new = yc + radius*sin(theta - d_theta); theta -= d_theta; } else { x_new = xc + radius*cos(theta + d_theta); y_new = yc + radius*sin(theta + d_theta); theta += d_theta; } if (profile_state == PROFILE_STOP) { x_new=xx1; y_new=yy1; } dx = (int)(x_new-x_old); dy = (int)(y_new-y_old); x_old += (double)(dx); y_old += (double)(dy); i8091_INTP_PULSE(cardNo, dx, dy); } while ((profile_state==PROFILE_BUSY) && (fifo_full==NO) ); if (profile_state==PROFILE_STOP) { state=PHASE1; return READY; } else { state=PHASE2; return BUSY; } //break; }; return READY; } //##################################################################### void intp_state_machine() { unsigned char status; switch (sys.command) { case sLINE: status= i8091_LINE_CORE(sys.cardNo, sys.x, sys.y, sys.speed, sys.acc_mode); if (status==READY) { state=PHASE1; sys.command=sIDLE; }; break; case sCIRCLE: status= i8091_CIRCLE_CORE(sys.cardNo, sys.x, sys.y, sys.dir, sys.speed, sys.acc_mode); if (status==READY) { state=PHASE1; sys.command=sIDLE; }; break; case sARC: status= i8091_ARC_CORE(sys.cardNo, sys.x, sys.y, sys.R, sys.dir, sys.speed, sys.acc_mode); if (status==READY) { state=PHASE1; sys.command=sIDLE; }; break; case sIDLE: default: state=PHASE1; break; } } //------------------------------------------------------------- void i8091_INTP_LINE02(unsigned char cardNo, long x,long y, unsigned int speed, unsigned char acc_mode) { sys.command=sLINE; sys.cardNo=cardNo; sys.x=x; sys.y=y; sys.speed=speed; sys.acc_mode=acc_mode; state=PHASE1; intp_state_machine(); } void i8091_INTP_CIRCLE02(unsigned char cardNo, long x, long y, unsigned char dir, unsigned int speed, unsigned char acc_mode) { sys.command=sCIRCLE; sys.cardNo=cardNo; sys.x=x; sys.y=y; sys.dir=dir; sys.speed=speed; sys.acc_mode=acc_mode; state=PHASE1; intp_state_machine(); } void i8091_INTP_ARC02(unsigned char cardNo, long x, long y, long R, unsigned char dir, unsigned int speed, unsigned char acc_mode) { sys.command=sARC; sys.cardNo=cardNo; sys.x=x; sys.y=y; sys.R=R; sys.dir=dir; sys.speed=speed; sys.acc_mode=acc_mode; state=PHASE1; intp_state_machine(); } unsigned char i8091_INTP_STOP() { intp_state_machine(); //check EMG if ((inportb(card[sys.cardNo].base + DI1)&0x80)==0x00) sys.command=sIDLE; //stop function if (sys.command==sIDLE) return READY; else return BUSY; } //####################################################################### void i8091_GET_CARD(unsigned char cardNo) { if (card[cardNo].exist==NO) return; #ifndef DOS //------------------------------------ // execute interpolation command intp_state_machine(); #endif };