kernel.c

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

         //set backlash to come vector
         backlash_to_come(hard_config, &vect2, &vect3, fd_fwd_msg, fd_fwd_msg_a); 
         
         //move machine accordingly
         msg=move_machine(hard_config, soft_config, current, cur_speed, &high_speed, 0,
                          x2, y2, z2, a2, total_vect2);
         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 XY_ARC_ENT:
         //get unit vector and total length end of arc and next object
         radius=sqrt(x2*x2+y2*y2);
         if(radius==0) currentang=endang=angle2=0;
         else {currentang=atan2(-y2,-x2); endang=currentang+angle2;}
         total_vect2_end=get_unit_vect(&vect2_end, -radius*cos(endang), -radius*sin(endang), z2, a2, angle2, XY_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).x_per_step, (*hard_config).y_per_step)/(2*radius));
            if(angle2<0) incang=-incang; a_inc=a2*incang/angle2; z_inc=z2*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;
            x=cos(currentang)*radius+x2-((*current).x_pos-x_start);
            y=sin(currentang)*radius+y2-((*current).y_pos-y_start);
            z=z_inc*(i+1)-((*current).z_pos-z_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_x>.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_x_slow_down_ang=temp/(2*(*hard_config).accel_y*total_vect2);
               if(back_x_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            if((*hard_config).back_y>.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_y_slow_down_ang=temp/(2*(*hard_config).accel_x*total_vect2);
               if(back_y_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, x, y, z, a, total_vect2);
                  slow_down_ang=*cur_speed**cur_speed*fabs(incang)/(2*max_accel*total_vect2);
                  z2=z_inc*(i+1)+z2*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 DONE;
              }
           }
         x=cos(endang)*radius+x2-((*current).x_pos-x_start);
         y=sin(endang)*radius+y2-((*current).y_pos-y_start);
         z=z2-((*current).z_pos-z_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 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;

         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);
           }

         DONE :
         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 YZ_ARC_ENT:
         //get unit vector and total length end of arc and next object
         radius=sqrt(y2*y2+z2*z2);
         if(radius==0) currentang=endang=angle2=0;
         else {currentang=atan2(-z2,-y2); endang=currentang+angle2;}
         total_vect2_end=get_unit_vect(&vect2_end, x2, -radius*cos(endang), -radius*sin(endang), a2, angle2, YZ_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).y_per_step, (*hard_config).z_per_step)/(2*radius));
            if(angle2<0) incang=-incang; a_inc=a2*incang/angle2; x_inc=x2*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;
            y=cos(currentang)*radius+y2-((*current).y_pos-y_start);
            z=sin(currentang)*radius+z2-((*current).z_pos-z_start);
            x=x_inc*(i+1)-((*current).x_pos-x_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_y>.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_y_slow_down_ang=temp/(2*(*hard_config).accel_z*total_vect2);
               if(back_y_slow_down_ang>=currentang_frac) acceleration=DECEL;
              }
            if((*hard_config).back_z>.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_z_slow_down_ang=temp/(2*(*hard_config).accel_y*total_vect2);
               if(back_z_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, x, y, z, a, total_vect2);
                  slow_down_ang=*cur_speed**cur_speed*fabs(incang)/(2*max_accel*total_vect2);
                  x2=x_inc*(i+1)+x2*slow_down_ang/fabs(angle2);
                  a2=a_inc*(i+1)+a2*slow_down_ang/fabs(angle2);