kernel.c

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

                  if(angle2<0) {angle2=incang*(i+1)-slow_down_ang; endang=currentang-slow_down_ang;}
                  else {angle2=incang*(i+1)+slow_down_ang; endang=currentang+slow_down_ang;}
                 }
               else goto DONE2;
              }
           }
         y=cos(endang)*radius+y2-((*current).y_pos-y_start);
         z=sin(endang)*radius+z2-((*current).z_pos-z_start);
         x=x2-((*current).x_pos-x_start);
         if(a2!=0) a=a2-((*current).a_pos-a_start);
         total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

         //do backlash
         msg=backlash(hard_config, soft_config, &vect1, &vect2);
         if(msg!=COMPLETE) goto DONE2;

         //save backlash settings
         if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
         if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

         msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed_2, x, y, z, a, total_vect2);
         if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
           {break_flag=msg;
            if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                            (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
            temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
            temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
            temp5=(*soft_config).io4_cmm;
            (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
            (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
            max_accel=get_max_accel(hard_config,vect2.x, vect2.y, vect2.z, vect2.a, total_vect2);
            total=*cur_speed**cur_speed/(2*max_accel);
            factor=total/total_vect2;
            move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0,
                         x2*factor, y2*factor, z2*factor, a2*factor, total);
           }

         DONE2 :
         if(break_flag!=OFF)
           {(*soft_config).keyboard=temp1; (*soft_config).io1_cmm=temp2;
            (*soft_config).io2_cmm=temp3; (*soft_config).io3_cmm=temp4;
            (*soft_config).io4_cmm=temp5;
            msg=break_flag;
           }
         for(temp1=0;temp1<4;temp1++)
           {if(temp1==3) temp1++;
            update_display(soft_config, (*current).x_pos, (*current).y_pos,
                          (*current).z_pos, (*current).a_pos, 0, 0, 0, temp1);
           }
         break;

      case ZX_ARC_ENT:
         //get unit vector and total length end of arc and next object
         radius=sqrt(z2*z2+x2*x2);
         if(radius==0) currentang=endang=angle2=0;
         else {currentang=atan2(-x2,-z2); endang=currentang+angle2;}
         total_vect2_end=get_unit_vect(&vect2_end, -radius*sin(endang), y2, -radius*cos(endang), a2, angle2, ZX_ARC_ENT);
         total_vect3=get_unit_vect(&vect3, x3, y3, z3, a3, angle3, object3);

         //see if backlash at end of move
         if(backlash_to_come(hard_config, &vect2_end, &vect3, START_OVER, START_OVER) || object3==LINE_G0_ENT) max_speed_2=0;
         else //get maximum ending speed squared
           {if(object3==LINE_ENT)
            max_speed_2=min(max_speed_sq(hard_config, vect2_end.x, vect2_end.y,
                            vect2_end.z, vect2_end.a, total_vect2_end, &vect3),
                            max_speed_sq_line(hard_config, x3, y3, z3, a3, total_vect3));
            else max_speed_2=max_speed_sq(hard_config, vect2_end.x, vect2_end.y, vect2_end.z, 
                                          vect2_end.a, total_vect2_end, &vect3);
           }
         //max acceleration due to end vector
         max_accel=get_max_accel(hard_config, vect2_end.x, vect2_end.y, vect2_end.z, vect2_end.a, total_vect2_end);

         //get loop increments
         if(angle2!=0) 
           {incang=2*atan(10*min((*hard_config).z_per_step, (*hard_config).x_per_step)/(2*radius));
            if(angle2<0) incang=-incang; a_inc=a2*incang/angle2; y_inc=y2*incang/angle2;
           }
         else incang=1;
         
         //initialize ang begin angle loop
         x_start=(*current).x_pos;
         y_start=(*current).y_pos;
         z_start=(*current).z_pos;
         a_start=(*current).a_pos;
         for(i=0;i<labs(angle2/incang);i++)
           {currentang=currentang+incang;
            z=cos(currentang)*radius+z2-((*current).z_pos-z_start);
            x=sin(currentang)*radius+x2-((*current).x_pos-x_start);
            y=y_inc*(i+1)-((*current).y_pos-y_start);
            if(a2!=0) a=a_inc*(i+1)-((*current).a_pos-a_start);
            total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

            //do backlash
            msg=backlash(hard_config, soft_config, &vect1, &vect2);
            if(msg!=COMPLETE) goto DONE;

            //save backlash settings
            if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
            if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

            if(break_flag!=OFF) acceleration=DECEL;
            else acceleration=NORMAL;
            
            temp=*cur_speed**cur_speed*fabs(incang);
            //determine if backlash slow down angles have been exceeded
            if((*hard_config).back_z>.000009)
              {currentang_frac=fmod(currentang,PI);
               if(currentang_frac<0) currentang_frac=currentang_frac+PI;
               if(angle2>0) currentang_frac=PI-currentang_frac;
               if(calcang==ON) back_z_slow_down_ang=temp/(2*(*hard_config).accel_x*total_vect2);
               if(back_z_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            if((*hard_config).back_x>.000009)
              {currentang_frac=fmod(currentang+PI/2,PI);
               if(currentang_frac<0) currentang_frac=currentang_frac+PI;
               if(angle2>0) currentang_frac=PI-currentang_frac;
               if(calcang==ON) back_x_slow_down_ang=temp/(2*(*hard_config).accel_z*total_vect2);
               if(back_x_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            
            //determine if ending slow down angle has been exceeded
            if(calcang==ON) slow_down_ang=-(max_speed_2-*cur_speed**cur_speed)*fabs(incang)/(2*max_accel*total_vect2);
            if(slow_down_ang>=fabs(angle2-incang*(i+1))) acceleration=DECEL;
            
            if(acceleration==NORMAL)
              {max_speed=get_max_speed(hard_config, x, y, z, a, total_vect2);
               max_speed=max_speed*max_speed;
               msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed, x, y, z, a, total_vect2);
               calcang=ON;
              }

            if(acceleration==DECEL)
              {msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0, x, y, z, a, total_vect2);
               calcang=OFF;
              }

            if(msg!=COMPLETE)
              {if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
                 {break_flag=msg;
                  if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                                  (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
                  temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
                  temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
                  temp5=(*soft_config).io4_cmm;
                  (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
                  (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
                  //reset angle for stop
                  max_accel=get_max_accel(hard_config, vect2.x, vect2.y, vect2.z, vect2.a, total_vect2);
                  slow_down_ang=*cur_speed**cur_speed*fabs(incang)/(2*max_accel*total_vect2);
                  y2=y_inc*(i+1)+y2*slow_down_ang/fabs(angle2);
                  a2=a_inc*(i+1)+a2*slow_down_ang/fabs(angle2);
                  if(angle2<0) {angle2=incang*(i+1)-slow_down_ang; endang=currentang-slow_down_ang;}
                  else {angle2=incang*(i+1)+slow_down_ang; endang=currentang+slow_down_ang;}
                 }
               else goto DONE3;
              }
           }
         z=cos(endang)*radius+z2-((*current).z_pos-z_start);
         x=sin(endang)*radius+x2-((*current).x_pos-x_start);
         y=y2-((*current).y_pos-y_start);
         if(a2!=0) a=a2-((*current).a_pos-a_start);
         total_vect2=get_unit_vect(&vect2, x, y, z, a, NULL, LINE_ENT);

         //do backlash
         msg=backlash(hard_config, soft_config, &vect1, &vect2);
         if(msg!=COMPLETE) goto DONE3;

         //save backlash settings
         if(vect2.x!=0) vect1.x=vect2.x; if(vect2.y!=0) vect1.y=vect2.y;
         if(vect2.z!=0) vect1.z=vect2.z; if(vect2.a!=0) vect1.a=vect2.a;

         msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed_2, x, y, z, a, total_vect2);
         if((msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4) && break_flag==OFF)
           {break_flag=msg;
            if(msg!=BREAK) {(*current).pt_x=(*current).x_pos; (*current).pt_y=(*current).y_pos;
                            (*current).pt_z=(*current).z_pos; (*current).pt_a=(*current).a_pos;}
            temp1=(*soft_config).keyboard; temp2=(*soft_config).io1_cmm;
            temp3=(*soft_config).io2_cmm; temp4=(*soft_config).io3_cmm;
            temp5=(*soft_config).io4_cmm;
            (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
            (*soft_config).io3_cmm=(*soft_config).io4_cmm=OFF;
            max_accel=get_max_accel(hard_config, x, y, z, a, total_vect2);
            total=*cur_speed**cur_speed/(2*max_accel);
            factor=total/total_vect2;
            msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, 0,
                             x2*factor, y2*factor, z2*factor, a2*factor, total);
           }

         DONE3 :
         if(break_flag!=OFF)
           {(*soft_config).keyboard=temp1; (*soft_config).io1_cmm=temp2;
            (*soft_config).io2_cmm=temp3; (*soft_config).io3_cmm=temp4;
            (*soft_config).io4_cmm=temp5;
            msg=break_flag;
           }
         for(temp1=0;temp1<4;temp1++)
           {if(temp1==3) temp1++;
            update_display(soft_config, (*current).x_pos, (*current).y_pos,
                          (*current).z_pos, (*current).a_pos, 0, 0, 0, temp1);
           }
         break;
     }

   //reset backlash to come messages accordingly
   if((x3==0 && y3==0 && z3==0) || msg!=COMPLETE || msg!=PAUSE) fd_fwd_msg=START_OVER;
   else fd_fwd_msg=CONTINUE;
   if(a3==0 || msg!=COMPLETE || msg!=PAUSE) fd_fwd_msg_a=START_OVER;
   else fd_fwd_msg_a=CONTINUE;

   //restore keyboard interupt
   restore_key();
   return msg;
}

//backlash function
int backlash(struct hard_setup *hard_config, struct soft_setup *soft_config,
             struct unit_vect *vect1, struct unit_vect *vect2)
{
   double x=0, y=0, z=0, a=0, total;
   float max_speed, cur_speed=0;
   int msg, temp1, temp2, temp3, temp4, temp5, temp6, temp7, temp8, temp9, temp10, temp11, temp12;
   struct pos pt;
   pt.x_pos=pt.y_pos=pt.z_pos=pt.a_pos=0;

   //determine if backlash takes place
   if(((*vect1).x*(*vect2).x>=0 || (*hard_config).back_x<=.000009) &&
      ((*vect1).y*(*vect2).y>=0 || (*hard_config).back_y<=.000009) &&
      ((*vect1).z*(*vect2).z>=0 || (*hard_config).back_z<=.000009) &&
      ((*vect1).a*(*vect2).a>=0 || (*hard_config).back_a<=.000009))
      return COMPLETE;

   //determine backlash
   if((*vect1).x*(*vect2).x<0 && (*hard_config).back_x>.000009)
     {if((*vect2).x<0) x=-(*hard_config).back_x; else x=(*hard_config).back_x;}
   if((*vect1).y*(*vect2).y<0 && (*hard_config).back_y>.000009)
     {if((*vect2).y<0) y=-(*hard_config).back_y; else y=(*hard_config).back_y;}
   if((*vect1).z*(*vect2).z<0 && (*hard_config).back_z>.000009)
     {if((*vect2).z<0) z=-(*hard_config).back_z; else z=(*hard_config).back_z;}
   if((*vect1).a*(*vect2).a<0 && (*hard_config).back_a>.000009)
     {if((*vect2).a<0) a=-(*hard_config).back_a; else a=(*hard_config).back_a;}

   //determine total backlash vector
   total=total_vect(x, y, z, a);

   //determine max speed
   max_speed=get_max_speed(hard_config, x, y, z, a, total);

   //do backlash compensation
   temp1=(*soft_config).keyboard;
   temp2=(*soft_config).io1_cmm;
   temp3=(*soft_config).io2_cmm;
   temp4=(*soft_config).io3_cmm;
   temp5=(*soft_config).io4_cmm;
   temp6=(*soft_config).display_x;
   temp7=(*soft_config).display_y;
   temp8=(*soft_config).display_z;
   temp9=(*soft_config).display_s;
   temp10=(*soft_config).display_as;
   temp11=(*soft_config).display_a;
   temp12=(*soft_config).display_fr;
   (*soft_config).keyboard=(*soft_config).io1_cmm=(*soft_config).io2_cmm=
   (*soft_config).io3_cmm=(*soft_config).io4_cmm=
   (*soft_config).display_x=(*soft_config).display_y=(*soft_config).display_z=(*soft_config).display_a=
   (*soft_config).dis