kernel.c

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

   if(total==0) return 0;
   return total/get_max_4(fabs(x)/(*hard_config).accel_x, fabs(y)/(*hard_config).accel_y,
                          fabs(z)/(*hard_config).accel_z, fabs(a)/(*hard_config).accel_a);
}

float get_max_speed(struct hard_setup *hard_config, float x, float y, float z, float a, float total)
{
   if(total==0) return 0;
   return total/get_max_4(fabs(x)/(*hard_config).vel_x, fabs(y)/(*hard_config).vel_y,
                          fabs(z)/(*hard_config).vel_z, fabs(a)/(*hard_config).vel_a);
}

float get_min_speed(struct hard_setup *hard_config, float x, float y, float z, float a, float total)
{
   if(total==0) return 0;
   return total/get_max_4(fabs(x)/(*hard_config).min_vel_x, fabs(y)/(*hard_config).min_vel_y,
                          fabs(z)/(*hard_config).min_vel_z, fabs(a)/(*hard_config).min_vel_a);
}

float get_max_4(float a, float b, float c, float d)
{
   if(a>=b&&a>=c&&a>=d) return a;
   if(b>=a&&b>=c&&b>=d) return b;
   if(c>=a&&c>=b&&c>=d) return c;
   return d;
}

int pause(struct hard_setup *hardware, struct soft_setup *software, float t, float *frate)
{
   float count=0;
   int msg=CONTINUE;
   BYTE s_port_read;

   init_key();
   while(count<t)
     {while(tick)
        {if(key==0x39 || key==0x01) {key=0; msg=BREAK; break;}
         if(key==0x21) {key=0; *frate=*frate+(*software).speed_inc; update_display(software, 0, 0, 0, 0, 0, 0, *frate, 6);}  //'F' key
         if(key==0x1f) {key=0; if(*frate-(*software).speed_inc>0) *frate=*frate-(*software).speed_inc; update_display(software, 0, 0, 0, 0, 0, 0, *frate, 6);} //'S' key
         if(key==0x19) {key=0; msg=PAUSE;}
        }
      if(msg==BREAK) break;
      tick=1;
      count=count+(*software).dt;
      if((*software).e_stop!=OFF) s_port_read=(248&inportb((*hardware).port+1))^128;
      if((*software).e_stop!=OFF && (*hardware).e_trip*(*hardware).e_bit-(s_port_read&(*hardware).e_bit)==0) {msg=E_STOP; break;}
     }
   restore_key();
   return msg;
}

unsigned long guage(struct soft_setup *software)
{
   float count=0;
   unsigned long i=0;

   while(count<1.0)
     {while(tick){sqrt(3); i++;}
      tick=1;
      count=count+(*software).dt;
     }
   i=i*.0083;
   if (i>100000) i=100000;
   return i;
}

//software interrupt functions
volatile int  tick_limit;            /*  The number of ticks before we call DOS ISR */
volatile int  tick_count;            /*  Current DOS tick counter                   */

void set_timer_freq(unsigned long freq)
{
   unsigned int new_timer_number ;
   _asm   cli              //disable interrupts

   if (freq<18) freq=18;
   tick_limit   = freq/18;         //number of ticks to give original
   tick_count   = 0;

    /*  Calculate the 'count down' value for the timer chip itself...  */
   new_timer_number=(freq==18) ? 0 : ((unsigned int) (1193180L/freq));
   /*  Re-program the timer chip...  */
   _asm    mov    dx, 43H
   _asm    mov    al, 34H
   _asm    out    dx, al
   _asm    mov    dx, 40H
   _asm    mov    ax, new_timer_number
   _asm    out    dx, al
   _asm    mov    al, ah
   _asm    out    dx, al

   _asm    sti       //enable interrupts
}

void (interrupt far *old_direct_timer)(void);

void _interrupt _far new_direct_timer(void)
{
   tick=0;
   if (--tick_count<=0)
     {tick_count=tick_limit;
      old_direct_timer();
     }
   else
   outp(0x20,0x20);
}

void begin_realtime(struct soft_setup *software, unsigned long freq)
{
   if(freq<18) freq=18;
   (*software).dt=1.0/freq;

   _asm   cli    //disable interrupts

   old_direct_timer  = _dos_getvect(0x08);
   set_timer_freq(freq);
   _dos_setvect(0x08,new_direct_timer);

   _asm   sti    //enable interrupts
}

void end_realtime(void)
{
   _asm   cli    //disable interrupts

   set_timer_freq(18);             /*  Ensures correct rate!  */

   _dos_setvect(0x08,old_direct_timer);

   _asm   sti    //enable interrupts
}

//keyboard interrupt functions
void (interrupt far *OldKeyboardRoutine)(void);

void _interrupt _far MyKeyboardRoutine(void)
{
   key = inportb(0x60);
   if(key==140 || key==141 || key==206 || key==202 || key==200 || 
      key==203 || key==205 || key==208 || key==201 || key==209) jogging=OFF;
   outportb(0x20, 0x20);  /* Issue EOI */
}

void init_key(void)
{
   _asm   cli    //disable interrupts

   OldKeyboardRoutine = _dos_getvect(9);
   _dos_setvect(0x09, MyKeyboardRoutine);
   key=0;

   _asm   sti    //enable interrupts
}

void restore_key(void)
{
   _asm   cli    //disable interrupts

   _dos_setvect(0x09, OldKeyboardRoutine);

   _asm   sti    //enable interrupts
}


//more high level user functions
int move(struct hard_setup *hard_config, struct soft_setup *soft_config, struct pos *current,
         float *cur_speed, float *sp_limit,
         double x2, double y2, double z2, double a2, double angle2, int object2,
         double x3, double y3, double z3, double a3, double angle3, int object3)
{
   double total_vect2, total_vect3, total_vect2_end, factor, total, temp;
   double radius, currentang, endang, incang, a_inc, z_inc, x_inc, y_inc;
   double x_start, y_start, z_start, a_start, x, y, z, a=0, slow_down_ang, i;
   double back_x_slow_down_ang, back_y_slow_down_ang, back_z_slow_down_ang, currentang_frac;
   float max_speed_2, max_accel, max_speed, high_speed=HIGH_SPEED;
   int msg, temp1, temp2, temp3, temp4, temp5;
   int acceleration=NORMAL, calcang=ON, break_flag=OFF;
   static int fd_fwd_msg=START_OVER, fd_fwd_msg_a=START_OVER;
   struct unit_vect vect2, vect3, vect2_end;
   static struct unit_vect vect1;
   static int demo_inc;

   if(version==DEMO)
     {demo_inc++;
      if(demo_inc>40) return QUIT;
     }
   
   if(*sp_limit<0 && object2!=LINE_ENT && object2!=LINE_G0_ENT) return INVALID_PARAMETER;

   //hook keyboard interupt
   init_key();
   
   switch(object2)
     {case LINE_ENT:
         //get the max speed if HIGH_SPEED is selected
         if(*sp_limit<0) *sp_limit=get_max_speed(hard_config,x2,y2,z2,a2,total_vect(x2,y2,z2,a2));

         //get unit vector and total length for current object
         total_vect2=get_unit_vect(&vect2, x2, y2, z2, a2, angle2, object2);

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

         //save current backlash status
         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;

         //get unit vector and total length for next object
         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, &vect3, fd_fwd_msg, fd_fwd_msg_a) || 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, x2, y2, z2, a2, total_vect2, &vect3),
                            max_speed_sq_line(hard_config, x3, y3, z3, a3, total_vect3));
            else max_speed_2=max_speed_sq(hard_config, x2, y2, z2, a2, total_vect2, &vect3);
           }

         //move machine accordingly
         msg=move_machine(hard_config, soft_config, current, cur_speed, sp_limit, max_speed_2,
                          x2, y2, z2, a2, total_vect2);

         //continue to stop if break or io trip
         if(msg==BREAK || msg==IO1 || msg==IO2 || msg==IO3 || msg==IO4)
           {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, x2, y2, z2, a2, 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);
            (*soft_config).keyboard=temp1; (*soft_config).io1_cmm=temp2;
            (*soft_config).io2_cmm=temp3; (*soft_config).io3_cmm=temp4;
            (*soft_config).io4_cmm=temp5;
           }
         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 LINE_G0_ENT:
         //get backlash vector for current object
         vect2.x=x2; vect2.y=y2; vect2.z=z2; vect2.a=a2;

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

         //save current backlash status
         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;

         //get unit vector and total length for next object
         total_vect3=get_unit_vect(&vect3, x3, y3, z3, a3, angle3, object3);