gcode.c

This is a CNCPro source file. It is include the G-Code interpreter source.

#include <stdio.h>
#include <math.h>
#include <conio.h>
#include "definitions.h"
#include "utilities.h"
#include "setup.h"
#include "kernel.h"
#include "gcode.h"
#include "control.h"
#include "operator.h"

extern int version;

int do_gcode(struct hard_setup *hardware, struct soft_setup *software, struct gcode_setup *gcode,
             struct control *ctrl, struct pos *world, char ch, double num)
{
   static double x_now, y_now, z_now, a_now, angle_now;
   static double X, Y, Z, A, I, J, K, R, Q, dcycleZ, dcycleR, dcycleQ, factor=1;
   static float fd_now, cur_sp;
   static int ENT_TYPE=LINE_ENT, ent_type_now=LINE_ENT, par;
   static int arc=XY_ARC_ENT, rot=CW, dwell, fd_ch, set_coord;
   static int new_X, new_Y, new_Z, new_A, new_I, new_J, new_K, new_R, new_Q;
   static int new_tool, init_M00, init_M30=ON, new_scale;
   static int dcycle81, dcycle83, dcycle_init, ent_holder, drilling;
   static int line_by_line;
   static BYTE buffer[1000];
   static struct pos pt_now, world_now, scale;
   struct pos pt, begin;
   double x, y, z, a, xe, ye, ze, dx, dy, dz, da, angle, radius, ms, dtemp;
   float fast_speed;
   int msg=COMPLETE, ent_type, temp;
   
   if(init_M30==ON)
      {dcycleR=dcycleZ=dcycleQ=scale.x_pos=scale.y_pos=scale.z_pos=scale.a_pos=0;
       factor=1;
       dcycle81=dcycle83=new_tool=dwell=new_scale=OFF;
       ENT_TYPE=ent_type_now=LINE_ENT; par=FALSE;
       arc=XY_ARC_ENT; rot=CW;
      }
   if(init_M00==ON || init_M30==ON)
      {init_M00=init_M30=OFF;
       x_now=y_now=z_now=a_now=angle_now=0; fd_ch=TRUE;
      }
   if(ch==1) line_by_line=ON;
   if(ch==2) line_by_line=OFF;

   if(ch=='\n' && par==TRUE)
     {MOVE:
      par=FALSE;
      a=get_move_val(&A,&(*world).a_pos,&(*gcode).a_coord[(*ctrl).cs],&scale.a_pos,&factor,&(*ctrl).pty)*new_A;
      if(ENT_TYPE==LINE_ENT || ENT_TYPE==LINE_G0_ENT)
        {x=get_move_val(&X,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_X;
         y=get_move_val(&Y,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_Y;
         z=get_move_val(&Z,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_Z;
         angle=NULL; ent_type=ENT_TYPE;}
      if(ENT_TYPE==XY_ARC_ENT)
        {xe=get_move_val(&X,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_X;
         ye=get_move_val(&Y,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_Y;
         if((*gcode).relarc==ON) {temp=(*ctrl).pty; (*ctrl).pty=REL;}
         x=get_move_val(&I,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_I;
         y=get_move_val(&J,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_J;
         if((*gcode).relarc==ON) (*ctrl).pty=temp;
         z=get_move_val(&Z,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_Z;
         angle=get_ang(rot,-x,-y,xe-x,ye-y); ent_type=ENT_TYPE;}
      if(ENT_TYPE==YZ_ARC_ENT)
        {ye=get_move_val(&Y,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_Y;
         ze=get_move_val(&Z,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_Z;
         if((*gcode).relarc==ON) {temp=(*ctrl).pty; (*ctrl).pty=REL;}
         y=get_move_val(&I,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_J;
         z=get_move_val(&K,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_K;
         if((*gcode).relarc==ON) (*ctrl).pty=temp;
         x=get_move_val(&X,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_X;
         angle=get_ang(rot,-y,-z,ye-y,ze-z); ent_type=ENT_TYPE;}
      if(ENT_TYPE==ZX_ARC_ENT)
        {ze=get_move_val(&Z,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_Z;
         xe=get_move_val(&X,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_X;
         if((*gcode).relarc==ON) {temp=(*ctrl).pty; (*ctrl).pty=REL;}
         x=get_move_val(&I,&(*world).x_pos,&(*gcode).x_coord[(*ctrl).cs],&scale.x_pos,&factor,&(*ctrl).pty)*new_I;
         z=get_move_val(&K,&(*world).z_pos,&(*gcode).z_coord[(*ctrl).cs],&scale.z_pos,&factor,&(*ctrl).pty)*new_K;
         if((*gcode).relarc==ON) (*ctrl).pty=temp;
         y=get_move_val(&Y,&(*world).y_pos,&(*gcode).y_coord[(*ctrl).cs],&scale.y_pos,&factor,&(*ctrl).pty)*new_Y;
         angle=get_ang(rot,-z,-x,ze-z,xe-x); ent_type=ENT_TYPE;}

      START_MOVE:
      pt.x_pos=((*world).x_pos-(*gcode).x_coord[(*ctrl).cs])*(*ctrl).pty;
      pt.y_pos=((*world).y_pos-(*gcode).y_coord[(*ctrl).cs])*(*ctrl).pty;
      pt.z_pos=((*world).z_pos-(*gcode).z_coord[(*ctrl).cs])*(*ctrl).pty;
      pt.a_pos=((*world).a_pos-(*gcode).a_coord[(*ctrl).cs])*(*ctrl).pty;

      if(dcycle_init==ON) {dcycle_init=OFF;
                           if(new_Z) dcycleZ=Z;
                           if(new_R) dcycleR=R;
                           if(new_Q) dcycleQ=fabs(Q);
                           ch='S';
                           if(dcycle81==ON) num=151;
                           if(dcycle83==ON) num=156;
                           goto DONE;}
      if(set_coord==ON) {pt.x_pos=pt.x_pos-(pt.x_pos*(*ctrl).pty-X)*new_X;
                         pt.y_pos=pt.y_pos-(pt.y_pos*(*ctrl).pty-Y)*new_Y;
                         pt.z_pos=pt.z_pos-(pt.z_pos*(*ctrl).pty-Z)*new_Z;
                         pt.a_pos=pt.a_pos-(pt.a_pos*(*ctrl).pty-A)*new_A;
                         goto DONE;}
      if(new_scale==ON) {new_scale=OFF;
                         scale.x_pos=(X+((*world).x_pos-(*gcode).x_coord[(*ctrl).cs])-
                                     ((*world).x_pos-(*gcode).x_coord[(*ctrl).cs])*(*ctrl).pty)*new_X;
                         scale.y_pos=(Y+((*world).y_pos-(*gcode).y_coord[(*ctrl).cs])-
                                     ((*world).y_pos-(*gcode).y_coord[(*ctrl).cs])*(*ctrl).pty)*new_Y;
                         scale.z_pos=(Z+((*world).z_pos-(*gcode).z_coord[(*ctrl).cs])-
                                     ((*world).z_pos-(*gcode).z_coord[(*ctrl).cs])*(*ctrl).pty)*new_Z;
                         scale.a_pos=(A+((*world).a_pos-(*gcode).a_coord[(*ctrl).cs])-
                                     ((*world).a_pos-(*gcode).a_coord[(*ctrl).cs])*(*ctrl).pty)*new_A;
                         goto DONE;}

      if((*ctrl).cvv==OFF || line_by_line==ON || dcycle81==ON || dcycle83==ON)
        {begin=pt;

         CONTINUE_PTP_MOVE:
         if(ent_type==LINE_G0_ENT && (*gcode).safer==ON)
           {fast_speed=HIGH_SPEED;
            msg=move(hardware, software, &pt, &cur_sp, &fast_speed, x, y, z, a, angle, LINE_ENT,
                     0, 0, 0, 0, NULL, LINE_ENT);
           }
         else
           {msg=move(hardware, software, &pt, &cur_sp, &(*ctrl).frate, x, y, z, a, angle, ent_type,
                     0, 0, 0, 0, NULL, LINE_ENT);
           }
         if((msg==PAUSE || msg==BREAK || msg==E_STOP || (msg>=X_PLIMIT && msg<=A_NLIMIT)) && drilling==ON)
           {drilling=OFF; ENT_TYPE=ent_holder;}
         if(msg==BREAK)
           {msg=break_msg(hardware, software, (*gcode).rp[(*ctrl).cs], (*gcode).a_rp[(*ctrl).cs],
                          &begin, &pt, &x, &y, &z, &a, &angle, ent_type);
            if(msg==COMPLETE) goto CONTINUE_PTP_MOVE; if(msg==BREAK) {ch='S'; num=100;}
           }
         if(msg==E_STOP) init_M00=ON;
         if(msg>=X_PLIMIT && msg<=A_NLIMIT) init_M00=ON;
         if(dcycle81==ON && ch!='S') {ch='S'; num=151;}
         if(dcycle83==ON && ch!='S') {ch='S'; num=156;}
        }

      else  //cvv move
        {begin=pt_now;

         CONTINUE_CVV_MOVE:
         if(ent_type_now==LINE_G0_ENT && (*gcode).safer==ON)
           {fast_speed=HIGH_SPEED;
            msg=move(hardware, software, &pt_now, &cur_sp, &fast_speed, x_now, y_now, z_now, a_now,
                     angle_now, LINE_ENT, x, y, z, a, angle, LINE_ENT);
           }
         else
           {msg=move(hardware, software, &pt_now, &cur_sp, &fd_now, x_now, y_now, z_now, a_now,
                     angle_now, ent_type_now, x, y, z, a, angle, ent_type);
           }

         if(msg==BREAK)
           {msg=break_msg(hardware, software, (*gcode).rp[(*ctrl).cs], (*gcode).a_rp[(*ctrl).cs],
                          &begin, &pt_now, &x_now, &y_now, &z_now, &a_now, &angle_now, ent_type_now);
            if(msg==COMPLETE) goto CONTINUE_CVV_MOVE; if(msg==BREAK) {msg=M0; ch='S'; num=100; pt=pt_now; goto DONE;}
           }
         if(msg==E_STOP) {init_M00=ON; pt=pt_now; goto DONE;}
         if(msg>=X_PLIMIT && msg<=A_NLIMIT) {init_M00=ON; pt=pt_now; goto DONE;}

         pt_now=pt; x_now=x; y_now=y; z_now=z; a_now=a; angle_now=angle; ent_type_now=ent_type;
         world_now=*world;
         if(fd_ch==TRUE) {fd_ch=FALSE; fd_now=(*ctrl).frate; update_display(software, 0, 0, 0, 0, 0, 0, (*ctrl).frate, 6);}
         else (*ctrl).frate=fd_now;

         if(ent_type_now==LINE_ENT || ent_type_now==LINE_G0_ENT)
           {dx=x; dy=y; dz=z;}
         if(ent_type_now==XY_ARC_ENT)
           {radius=sqrt(x*x+y*y); dx=x+radius*cos(ang_of(-y,-x)+angle); dy=y+radius*sin(ang_of(-y,-x)+angle); dz=z;}
         if(ent_type_now==YZ_ARC_ENT)
           {radius=sqrt(y*y+z*z); dy=y+radius*cos(ang_of(-z,-y)+angle); dz=z+radius*sin(ang_of(-z,-y)+angle); dx=x;}
         if(ent_type_now==ZX_ARC_ENT)
           {radius=sqrt(z*z+x*x); dz=z+radius*cos(ang_of(-x,-z)+angle); dx=x+radius*sin(ang_of(-x,-z)+angle); dy=y;}
         if(a==0 && (*software).a_follow!=INDEPENDANT)
           {if((*software).a_follow==FOLLOW_X) da=dx; if((*software).a_follow==FOLLOW_Y) da=dy; if((*software).a_follow==FOLLOW_Z) da=dz;}
         else da=a;
         pt.x_pos=pt_now.x_pos+dx; pt.y_pos=pt_now.y_pos+dy; pt.z_pos=pt_now.z_pos+dz; pt.a_pos=pt_now.a_pos+da;
        }
      if(msg==PAUSE) {ch='M'; num=0;}

      DONE:
      (*world).x_pos=(*world).x_pos+pt.x_pos+((*gcode).x_coord[(*ctrl).cs]-(*world).x_pos)*(*ctrl).pty;
      (*world).y_pos=(*world).y_pos+pt.y_pos+((*gcode).y_coord[(*ctrl).cs]-(*world).y_pos)*(*ctrl).pty;
      (*world).z_pos=(*world).z_pos+pt.z_pos+((*gcode).z_coord[(*ctrl).cs]-(*world).z_pos)*(*ctrl).pty;
      (*world).a_pos=(*world).a_pos+pt.a_pos+((*gcode).a_coord[(*ctrl).cs]-(*world).a_pos)*(*ctrl).pty;
      if(par==FALSE) {new_X=new_Y=new_Z=new_A=new_I=new_J=new_K=new_R=FALSE;}

      if(set_coord==ON)
        {set_coord=OFF;
         for(temp=0;temp<4;temp++)
           {if(temp==3) temp++;
            update_display(software, (*world).x_pos*(*ctrl).pty, (*world).y_pos*(*ctrl).pty,
                           (*world).z_pos*(*ctrl).pty, (*world).a_pos*(*ctrl).pty, 0, 0, 0, temp);
           }
        }
     }

   if(ch=='X') {new_X=TRUE; X=num; par=TRUE;}
   if(ch=='Y') {new_Y=TRUE; Y=num; par=TRUE;}
   if(ch=='Z') {new_Z=TRUE; Z=num; par=TRUE;}
   if(ch==(*software).a_label && (*software).a_status==ON) {new_A=TRUE; A=num; par=TRUE;}
   if(ch=='I') {new_I=TRUE; I=num; par=TRUE;}
   if(ch=='J') {new_J=TRUE; J=num; par=TRUE;}
   if(ch=='K') {new_K=TRUE; K=num; par=TRUE;}
   if(ch=='R') {new_R=TRUE; R=num; par=TRUE;}
   if(ch=='Q') {new_Q=TRUE; Q=num; par=TRUE;}
   if(ch=='F')
     {if((*software).time==MIN) (*ctrl).frate=num/60;
      else (*ctrl).frate=num;
      if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) fd_ch=TRUE;
      else update_display(software, 0, 0, 0, 0, 0, 0, (*ctrl).frate, 6);
     }
   if(dwell==ON && ch!='P') dwell=OFF;
   if(dwell==ON && ch=='P') {ms=num; ch='S'; num=142;
                             if((*ctrl).cvv==ON && line_by_line==OFF && dcycle81==OFF && dcycle83==OFF) {x=y=z=a=0; angle=NULL; ent_type=LINE_ENT; goto START_MOVE;}
                            }

   if(new_scale==ON && (ch=='\n' || line_by_line==ON)) {par=FALSE; new_scale=OFF;}
   if(new_scale==ON && ch=='P') factor=num;

   if(new_tool==ON) 
     {new_tool=OFF;
      if(ch=='P' && num<=(*gcode).tnum_max)
        {(*world).z_pos=(*world).z_pos-((*gcode).tool_len[num]-(*gcode).tool_len[(*gcode).tnum]);
         (*gcode).tnum=num; GMTtext(software, ctrl, gcode);