kernel.c

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

         //check for limit switch release
         if(x_limit==OFF)
           {if((*hard_config).x_ltrip*(*hard_config).x_lbit-(s_port_read&(*hard_config).x_lbit)!=0)
              {if(*sp_limit>0) cur_debo_x=cur_debo_x+*cur_speed*(*soft_config).dt*dist_x*xs/total_vect;
               else cur_debo_x=cur_debo_x+cur_x_sp*(*soft_config).dt;
               if(cur_debo_x>(*hard_config).debo_x) {cur_debo_x=0; x_limit=ON; msg=UNLIMIT; break;}}
            else cur_debo_x=0;}
         if(y_limit==OFF)
           {if((*hard_config).y_ltrip*(*hard_config).y_lbit-(s_port_read&(*hard_config).y_lbit)!=0)
              {if(*sp_limit>0) cur_debo_y=cur_debo_y+*cur_speed*(*soft_config).dt*dist_y*ys/total_vect;
               else cur_debo_y=cur_debo_y+cur_y_sp*(*soft_config).dt;
               if(cur_debo_y>(*hard_config).debo_y) {cur_debo_y=0; y_limit=ON; msg=UNLIMIT; break;}}
            else cur_debo_y=0;}
         if(z_limit==OFF)
           {if((*hard_config).z_ltrip*(*hard_config).z_lbit-(s_port_read&(*hard_config).z_lbit)!=0)
              {if(*sp_limit>0) cur_debo_z=cur_debo_z+*cur_speed*(*soft_config).dt*dist_z*zs/total_vect;
               else cur_debo_z=cur_debo_z+cur_z_sp*(*soft_config).dt;
               if(cur_debo_z>(*hard_config).debo_z) {cur_debo_z=0; z_limit=ON; msg=UNLIMIT; break;}}
            else cur_debo_z=0;}
         if(a_limit==OFF && (*soft_config).a_status==ON)
           {if((*hard_config).a_ltrip*(*hard_config).a_lbit-(s_port_read&(*hard_config).a_lbit)!=0)
              {if(*sp_limit>0) cur_debo_a=cur_debo_a+*cur_speed*(*soft_config).dt*dist_a*as/total_vect;
               else cur_debo_a=cur_debo_a+cur_a_sp*(*soft_config).dt;
               if(cur_debo_a>(*hard_config).debo_a) {cur_debo_a=0; a_limit=ON; msg=UNLIMIT; break;}}
            else cur_debo_a=0;}
        }

      if(*sp_limit>0){
      //set acceleration flag to decel if applicable
      if(accelerate==NORMAL && total_vect>current_vect+.0001)
        {cur_decel=(max_end_speed_sq-*cur_speed**cur_speed)/(2*(total_vect-current_vect));
         if(cur_decel<=-max_accel) accelerate=DECEL;}

      //do acceleration routine and define speed
      if(accelerate==NORMAL)
        {if(*cur_speed<=*sp_limit)
           {*cur_speed=*cur_speed+max_accel*(*soft_config).dt;
            if(*cur_speed>min(*sp_limit, max_speed)) *cur_speed=min(*sp_limit, max_speed);}
         else
           {*cur_speed=*cur_speed-max_accel*(*soft_config).dt;
            if(*cur_speed<min(*sp_limit, max_speed)) *cur_speed=min(*sp_limit, max_speed);}
        }
      if(accelerate==DECEL)
        {*cur_speed=*cur_speed-max_accel*(*soft_config).dt;
         if(*cur_speed<min_speed) *cur_speed=min_speed;}

      //increment position
      current_vect=current_vect+*cur_speed*(*soft_config).dt;
      temp=*cur_speed*(*soft_config).dt/total_vect;
      if(current_x*xs<dist_x*xs) {current_x=current_x+temp*dist_x; inc_x=inc_x+temp*dist_x*xs;}
      if(current_y*ys<dist_y*ys) {current_y=current_y+temp*dist_y; inc_y=inc_y+temp*dist_y*ys;}
      if(current_z*zs<dist_z*zs) {current_z=current_z+temp*dist_z; inc_z=inc_z+temp*dist_z*zs;}
      if(current_a*as<dist_a*as) {current_a=current_a+temp*dist_a; inc_a=inc_a+temp*dist_a*as;}
      if(dist_x*xs<=current_x*xs) {inc_x=inc_x-(current_x*xs-dist_x*xs); current_x=dist_x;}
      if(dist_y*ys<=current_y*ys) {inc_y=inc_y-(current_y*ys-dist_y*ys); current_y=dist_y;}
      if(dist_z*zs<=current_z*zs) {inc_z=inc_z-(current_z*zs-dist_z*zs); current_z=dist_z;}
      if(dist_a*as<=current_a*as) {inc_a=inc_a-(current_a*as-dist_a*as); current_a=dist_a;}
      }
      else{  //if fast non-coordinated move
      //set acceleration flag to decel if applicable
      if(acc_x==NORMAL && dist_x*xs>current_x*xs+.0001)
        {cur_decel=((*hard_config).min_vel_x*(*hard_config).min_vel_x-cur_x_sp*cur_x_sp)/(2*(dist_x*xs-current_x*xs));
         if(cur_decel<=-(*hard_config).accel_x) acc_x=DECEL;}
      if(acc_y==NORMAL && dist_y*ys>current_y*ys+.0001)
        {cur_decel=((*hard_config).min_vel_y*(*hard_config).min_vel_y-cur_y_sp*cur_y_sp)/(2*(dist_y*ys-current_y*ys));
         if(cur_decel<=-(*hard_config).accel_y) acc_y=DECEL;}
      if(acc_z==NORMAL && dist_z*zs>current_z*zs+.0001)
        {cur_decel=((*hard_config).min_vel_z*(*hard_config).min_vel_z-cur_z_sp*cur_z_sp)/(2*(dist_z*zs-current_z*zs));
         if(cur_decel<=-(*hard_config).accel_z) acc_z=DECEL;}
      if(acc_a==NORMAL && dist_a*as>current_a*as+.0001)
        {cur_decel=((*hard_config).min_vel_a*(*hard_config).min_vel_a-cur_a_sp*cur_a_sp)/(2*(dist_a*as-current_a*as));
         if(cur_decel<=-(*hard_config).accel_a) acc_a=DECEL;} 
      
      //do acceleration routine and define speed
      if(acc_x==NORMAL && cur_x_sp<(*hard_config).vel_x && dist_x*xs>current_x*xs)
        {cur_x_sp=max(cur_x_sp+(*hard_config).accel_x*(*soft_config).dt,(*hard_config).min_vel_x);
         if(cur_x_sp>(*hard_config).vel_x) cur_x_sp=(*hard_config).vel_x;}
      if(acc_x==DECEL && cur_x_sp>(*hard_config).min_vel_x && dist_x*xs>current_x*xs)
        {cur_x_sp=cur_x_sp-(*hard_config).accel_x*(*soft_config).dt;
         if(cur_x_sp<(*hard_config).min_vel_x) cur_x_sp=(*hard_config).min_vel_x;}
      if(acc_y==NORMAL && cur_y_sp<(*hard_config).vel_y && dist_y*ys>current_y*ys)
        {cur_y_sp=max(cur_y_sp+(*hard_config).accel_y*(*soft_config).dt,(*hard_config).min_vel_y);
         if(cur_y_sp>(*hard_config).vel_y) cur_y_sp=(*hard_config).vel_y;}
      if(acc_y==DECEL && cur_y_sp>(*hard_config).min_vel_y  && dist_y*ys>current_y*ys)
        {cur_y_sp=cur_y_sp-(*hard_config).accel_y*(*soft_config).dt;
         if(cur_y_sp<(*hard_config).min_vel_y) cur_y_sp=(*hard_config).min_vel_y;}
      if(acc_z==NORMAL && cur_z_sp<(*hard_config).vel_z && dist_z*zs>current_z*zs)
        {cur_z_sp=max(cur_z_sp+(*hard_config).accel_z*(*soft_config).dt,(*hard_config).min_vel_z);
         if(cur_z_sp>(*hard_config).vel_z) cur_z_sp=(*hard_config).vel_z;}
      if(acc_z==DECEL && cur_z_sp>(*hard_config).min_vel_z && dist_z*zs>current_z*zs)
        {cur_z_sp=cur_z_sp-(*hard_config).accel_z*(*soft_config).dt;
         if(cur_z_sp<(*hard_config).min_vel_z) cur_z_sp=(*hard_config).min_vel_z;}
      if(acc_a==NORMAL && cur_a_sp<(*hard_config).vel_a && dist_a*as>current_a*as)
        {cur_a_sp=max(cur_a_sp+(*hard_config).accel_a*(*soft_config).dt,(*hard_config).min_vel_a);
         if(cur_a_sp>(*hard_config).vel_a) cur_a_sp=(*hard_config).vel_a;}
      if(acc_a==DECEL && cur_a_sp>(*hard_config).min_vel_a && dist_a*as>current_a*as)
         {cur_a_sp=cur_a_sp-(*hard_config).accel_a*(*soft_config).dt;
         if(cur_a_sp<(*hard_config).min_vel_a) cur_a_sp=(*hard_config).min_vel_a;}

      //increment position
      current_x=current_x+cur_x_sp*(*soft_config).dt*xs;
      current_y=current_y+cur_y_sp*(*soft_config).dt*ys;
      current_z=current_z+cur_z_sp*(*soft_config).dt*zs;
      current_a=current_a+cur_a_sp*(*soft_config).dt*as;
      inc_x=inc_x+cur_x_sp*(*soft_config).dt;
      inc_y=inc_y+cur_y_sp*(*soft_config).dt;
      inc_z=inc_z+cur_z_sp*(*soft_config).dt;
      inc_a=inc_a+cur_a_sp*(*soft_config).dt;
      if(dist_x*xs<=current_x*xs) {inc_x=inc_x-(current_x*xs-dist_x*xs); cur_x_sp=0; current_x=dist_x;}
      if(dist_y*ys<=current_y*ys) {inc_y=inc_y-(current_y*ys-dist_y*ys); cur_y_sp=0; current_y=dist_y;}
      if(dist_z*zs<=current_z*zs) {inc_z=inc_z-(current_z*zs-dist_z*zs); cur_z_sp=0; current_z=dist_z;}
      if(dist_a*as<=current_a*as) {inc_a=inc_a-(current_a*as-dist_a*xs); cur_a_sp=0; current_a=dist_a;}
      }
      
      //check to see if time to take a step
      if(inc_x>(*hard_config).x_per_step/2)
        {step_flag=ON;
         inc_x=inc_x-(*hard_config).x_per_step/2;
         if(act_step==HIGH)
           {if((step&(*hard_config).x_sbit)==(*hard_config).x_sbit) step=step-(*hard_config).x_sbit;
            else if(x_step==LOW) step=step+(*hard_config).x_sbit;}
         else
           {if((step&(*hard_config).x_sbit)==0) step=step+(*hard_config).x_sbit;
            else if(x_step==HIGH) step=step-(*hard_config).x_sbit;}
         if(x_step==HIGH) x_step=LOW; else x_step=HIGH;
        }
      if(inc_y>(*hard_config).y_per_step/2)
        {step_flag=ON;
         inc_y=inc_y-(*hard_config).y_per_step/2;
         if(act_step==HIGH)
           {if((step&(*hard_config).y_sbit)==(*hard_config).y_sbit) step=step-(*hard_config).y_sbit;
            else if(y_step==LOW) step=step+(*hard_config).y_sbit;}
         else
           {if((step&(*hard_config).y_sbit)==0) step=step+(*hard_config).y_sbit;
            else if(y_step==HIGH) step=step-(*hard_config).y_sbit;}
         if(y_step==HIGH) y_step=LOW; else y_step=HIGH;
        }
      if(inc_z>(*hard_config).z_per_step/2)
        {step_flag=ON;
         inc_z=inc_z-(*hard_config).z_per_step/2;
         if(act_step==HIGH)
           {if((step&(*hard_config).z_sbit)==(*hard_config).z_sbit) step=step-(*hard_config).z_sbit;
            else if(z_step==LOW) step=step+(*hard_config).z_sbit;}
         else
           {if((step&(*hard_config).z_sbit)==0) step=step+(*hard_config).z_sbit;
            else if(z_step==HIGH) step=step-(*hard_config).z_sbit;}
         if(z_step==HIGH) z_step=LOW; else z_step=HIGH;
        }
      if(inc_a>(*hard_config).a_per_step/2)
        {step_flag=ON;
         inc_a=inc_a-(*hard_config).a_per_step/2;
         if(act_step==HIGH)
           {if((step&(*hard_config).a_sbit)==(*hard_config).a_sbit) step=step-(*hard_config).a_sbit;
            else if(a_step==LOW) step=step+(*hard_config).a_sbit;}
         else
           {if((step&(*hard_config).a_sbit)==0) step=step+(*hard_config).a_sbit;
            else if(a_step==HIGH) step=step-(*hard_config).a_sbit;}
         if(a_step==HIGH) a_step=LOW; else a_step=HIGH;
        }
      if(step_flag)
        {step_flag=OFF;
         outportb((*hard_config).port, dir+step);
        }

      //check for completion of move
      if(dist_x*xs<=current_x*xs && dist_y*ys<=current_y*ys &&
         dist_z*zs<=current_z*zs && dist_a*as<=current_a*as)
        {cur_x_sp=cur_y_sp=cur_z_sp=cur_a_sp=0;
         if(*cur_speed<=min_speed && max_end_speed_sq<=min_speed*min_speed) *cur_speed=sqrt(max_end_speed_sq);
         break;
        }

      //check for keyboard speed control
      if((*soft_config).keyboard==ON && *sp_limit>0){
      if(key==0x21) {key=0; *sp_limit=*sp_limit+speed_inc; update_display(soft_config, 0, 0, 0, 0, 0, 0, *sp_limit, 6);}  //'F' key
      if(key==0x1f)
        {key=0; if(*sp_limit-speed_inc>0) *sp_limit=*sp_limit-speed_inc; update_display(soft_config, 0, 0, 0, 0, 0, 0, *sp_limit, 6);}} //'S' key

      //check for machine pause
      if(key==0x19) {key=0; msg=PAUSE;}

      if(*sp_limit>0){
      //check for io trip
      if((*soft_config).io1_cmm==ON && (c_port_read&(*hard_config).io1_bit)==LOW) {msg=IO1; break;}
      if((*soft_config).io2_cmm==ON && (c_port_read&(*hard_config).io2_bit)==LOW) {msg=IO2; break;}
      if((*soft_config).io3_cmm==ON && (c_port_read&(*hard_config).io3_bit)==LOW) {msg=IO3; break;}
      if((*soft_config).io4_cmm==ON && (c_port_read&(*hard_config).io4_bit)==LOW) {msg=IO4; break;}
      
      //check for keyboard trip (space bar or esc key)
      if((*soft_config).keyboard==ON && (key==0x39 || key==0x01)) {key=0; msg=BREAK; break;}

      //check for jog release
      if((*soft_config).keyboard==JOG && jogging==OFF) break;
      }
      if(*sp_limit<0 && break_flag==OFF){
      //check for io trip
      if((*soft_config).io1_cmm==ON && (c_port_read&(*hard_config).io1_bit)==LOW) msg=IO1;
      if((*soft_config).io2_cmm==ON && (c_port_read&(*hard_config).io2_bit)==LOW) msg=IO2;
      if((*soft_config).io3_cmm==ON && (c_port_read&(*hard_config).io3_bit)==LOW) msg=IO3;
      if((*soft_config).io4_cmm==ON && (c_port_read&(*hard_config).io4_bit)==LOW) msg=IO4;
      
      //check for keyboard trip on space bar or ESC
      if(key==0x39 || key==0x01) {key=0; msg=BREAK;}

      if(msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4)
        {break_flag=ON;
         if(msg!=BREAK) {(*current).pt_x=(*current).x_pos+current_x; (*current).pt_y=(*current).y_pos+current_y; 
                         (*current).pt_z=(*current).z_pos+current_z; (*current).pt_a=(*current).a_pos+current_a;}
         dist_x=current_x+sign(dist_x)*min(fabs(dist_x-current_x),cur_x_sp*cur_x_sp/(2*(*hard_config).accel_x));
         dist_y=current_y+sign(dist_y)*min(fabs(dist_y-current_y),cur_y_sp*cur_y_sp/(2*(*hard_config).accel_y));
         dist_z=current_z+sign(dist_z)*min(fabs(dist_z-current_z),cur_z_sp*cur_z_sp/(2*(*hard_config).accel_z));
         dist_a=current_a+sign(dist_a)*min(fabs(dist_a-current_a),cur_a_sp*cur_a_sp/(2*(*hard_config).accel_a));
        }}          

      if((*soft_config).update_after==OFF)
        {//update screen every .016 seconds
         display_count=display_count+(*soft_config).dt;
         if(display_count>.015)
           {display_count=0;
            if(*sp_limit>0){
            if(dist_x==0 && dist_y==0 && dist_z==0) speed=0;
            else speed=*cur_speed;
            update_display(soft_config, (*current).x_pos+current_x, (*current).y_pos+current_y,
                           (*current).z_pos+current_z, (*current).a_pos+current_a,
                           speed, (dist_a*as**cur_speed/total_vect), 0, display);
            }
            else {
            update_display(soft_config, (*current).x_pos+current_x, (*current).y_pos+current_y,
                           (*current).z_pos+current_z, (*current).a_pos+current_a,
                           sqrt(cur_x_sp*cur_x_sp+cur_y_sp*cur_y_sp+cur_z_sp*cur_z_sp),
                           cur_a_sp, 0, display);}
            display++; if(display==6) display=0;
           }
        }
     }

   if(msg==UNLIMIT) cur_x_sp=cur_y_sp=cur_z_sp=cur_a_sp=0;
   if((msg==COMPLETE || msg==PAUSE) && (*soft_config).keyboard!=JOG)
     {(*current).x_pos=(*current).x_pos+dist_x;
      (*current).y_pos=(*current).y_pos+dist_y;
      (*current).z_pos=(*current).z_pos+dist_z;
      (*current).a_pos=(*current).a_pos+dist_a;}
   else
     {(*current).x_pos=(*current).x_pos+current_x;
      (*current).y_pos=(*current).y_pos+current_y;
      (*current).z_pos=(*current).z_pos+current_z;
      (*current).a_pos=(*current).a_pos+current_a;}
   return msg;
}

int sign(float num)
{
   if(num<0) return -1;
   else return 1;
}

double total_vect(double x, double y, double z, double a)
{
  if(x==0 && y==0 && z==0) return fabs(a);
  return sqrt(x*x+y*y+z*z);
}

float get_max_accel(struct hard_setup *hard_config, float x, float y, float z, float a, float total)
{