kernel.c

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

#include <stdio.h>
#include <stdlib.h>
#include <dos.h>
#include <math.h>
#include <conio.h>
#include "definitions.h"
#include "kernel.h"

extern int version;

void video_write(BYTE, BYTE, double, int, int);
int move_machine(struct hard_setup *, struct soft_setup *, struct pos *,
                 float *, float *, float, double, double, double, double, double);
double get_unit_vect(struct unit_vect *, double, double, double, double, double, int);
float max_speed_sq(struct hard_setup *,
                   float, float, float, float, float, 
                   struct unit_vect *);
float max_speed_sq_line(struct hard_setup *, double, double, double, double, double);
int backlash(struct hard_setup *, struct soft_setup *, struct unit_vect*, struct unit_vect*);
int backlash_to_come(struct hard_setup *, struct unit_vect *, struct unit_vect *, int, int);
float get_max_speed(struct hard_setup *, float, float, float, float, float);
float get_max_accel(struct hard_setup *, float, float, float, float, float);
float get_min_speed(struct hard_setup *, float, float, float, float, float);
float get_max_4(float, float, float, float);
int sign(float);
double total_vect(double, double, double, double);
void _interrupt _far new_direct_timer(void);
void set_timer_freq(unsigned long);
void _interrupt _far MyKeyboardRoutine(void);
void init_key(void);
void restore_key(void);

//hardware bit manipulation functions
void clear_data_bits(struct hard_setup *hard_config)
{
   outportb((*hard_config).port, 0);
}

void set_io_bit(struct hard_setup *hard_config, int bit)
{
   outportb((*hard_config).port+2, ((inportb((*hard_config).port+2)^11)|bit)^11);
}

void clear_io_bit(struct hard_setup *hard_config, int bit)
{
   BYTE iobits;

   iobits=(inportb((*hard_config).port+2)^11)|bit;
   outportb((*hard_config).port+2, (iobits&(iobits-bit))^11);
}

void toggle_io_bit(struct hard_setup *hard_config, int bit)
{
   bit=pow(2,bit);
   outportb((*hard_config).port+2, ((inportb((*hard_config).port+2)^11)^bit)^11);
}

//update the display
void update_display(struct soft_setup *soft_config, double x, double y, double z,
                    double a, float speed, float a_speed, float f_rate, int item)
{
   if((*soft_config).display_x && (item==0 || item==ALL)) 
      video_write((*soft_config).x_xpos,(*soft_config).x_ypos, x, RED, LONG);
   if((*soft_config).display_y && (item==1 || item==ALL)) 
      video_write((*soft_config).y_xpos,(*soft_config).y_ypos, y, RED, LONG);
   if((*soft_config).display_z && (item==2 || item==ALL)) 
      video_write((*soft_config).z_xpos,(*soft_config).z_ypos, z, RED, LONG);
   if((*soft_config).display_s && (item==3 || item==ALL))
      video_write((*soft_config).s_xpos,(*soft_config).s_ypos, speed*(*soft_config).time, BLACK, SHORT);
   if((*soft_config).display_a && (item==4 || item==ALL)) 
      video_write((*soft_config).a_xpos,(*soft_config).a_ypos, a, RED, LONG);
   if((*soft_config).display_as && (item==5 || item==ALL))
      video_write((*soft_config).as_xpos,(*soft_config).as_ypos, a_speed*(*soft_config).a_time, BLACK, SHORT);
   if((*soft_config).display_fr && (item==6 || item==ALL))
      video_write((*soft_config).fr_xpos,(*soft_config).fr_ypos, f_rate*(*soft_config).time, BLACK, SHORT);
}

void video_write(BYTE x, BYTE y, double dnum, int bkg, int lgth)
{
   BYTE _far *video=(BYTE _far *)0xB8000000L;       /*  Ptr to base of VGA text RAM  */
   BYTE unit;
   WORD yoff=(y-1)*160, xoff=(x-1)*2;
   int s=sign(dnum), i;
   double temp;
   unsigned long lnum=(unsigned long)(temp=s*dnum*(10000)), den=10000000;

   if(temp-lnum>=.5)lnum++;
   if(lgth==SHORT && lnum/100.0-lnum/100>=.5)lnum=lnum+100;
   if(lnum==0) s=1;
   if(lnum>99999999) lnum=0;
   for(i=1; i<17+lgth*4; i+=2) *(video+(WORD)(xoff+i+yoff))=WHITE+bkg*16;
   *(video+(WORD)(xoff+10+yoff))='.';
   if(s<0) *(video+(WORD)(xoff+yoff))='-';
   else *(video+(WORD)(xoff+yoff))=' ';
   for(i=2; i<16+lgth*4; i+=2)
     {if(i==10) i+=2; 
      unit=lnum/den; lnum=lnum-unit*den; den=den/10;
      *(video+(WORD)(xoff+i+yoff))=unit+48;
     }
}

//realtime function for moving device
volatile int tick;
volatile int key, jogging;
int move_machine(struct hard_setup *hard_config, struct soft_setup *soft_config, struct pos *current,
                 float *cur_speed, float *sp_limit, float max_end_speed_sq,
                 double dist_x, double dist_y, double dist_z, double dist_a, double total_vect)
{
   double current_x=0, current_y=0, current_z=0, current_a=0, current_vect=0, temp;
   static double inc_x, inc_y, inc_z, inc_a;
   static float cur_debo_x, cur_debo_y, cur_debo_z, cur_debo_a;
   static float display_count;
   static float cur_x_sp, cur_y_sp, cur_z_sp, cur_a_sp;
   float speed, max_accel, cur_decel, max_speed, min_speed, speed_inc;
   static int x_limit=ON, y_limit=ON, z_limit=ON, a_limit=ON, display;
   static int pxs=1, pys=1, pzs=1, pas=1, x_step=LOW, y_step=LOW, z_step=LOW, a_step=LOW, act_step=HIGH;
   int xs=sign(dist_x), ys=sign(dist_y), zs=sign(dist_z), as=sign(dist_a);
   int msg=COMPLETE, step_flag=OFF, break_flag=OFF, accelerate=NORMAL;
   int acc_x=NORMAL, acc_y=NORMAL, acc_z=NORMAL, acc_a=NORMAL;
   BYTE dir=0, s_port_read, c_port_read;
   static BYTE step;

   //check step activate parameter and change if needed
   if(act_step!=(*hard_config).act_step)
     {act_step=(*hard_config).act_step;
      if(x_step==LOW) x_step=HIGH; else x_step=LOW;
      if(y_step==LOW) y_step=HIGH; else y_step=LOW;
      if(z_step==LOW) z_step=HIGH; else z_step=LOW;
      if(a_step==LOW) a_step=HIGH; else a_step=LOW;
      if((step&(*hard_config).x_sbit)==0) step=step+(*hard_config).x_sbit; else step=step-(*hard_config).x_sbit;
      if((step&(*hard_config).y_sbit)==0) step=step+(*hard_config).y_sbit; else step=step-(*hard_config).y_sbit;
      if((step&(*hard_config).z_sbit)==0) step=step+(*hard_config).z_sbit; else step=step-(*hard_config).z_sbit;
      if((step&(*hard_config).a_sbit)==0) step=step+(*hard_config).a_sbit; else step=step-(*hard_config).a_sbit;
      outportb((*hard_config).port, step);
     }

   //set up auxiliary channel for move
   if((*soft_config).a_status==OFF) dist_a=0;
   else if(dist_a==0)
     {if((*soft_config).a_follow==FOLLOW_X) {dist_a=dist_x; as=xs;}
      if((*soft_config).a_follow==FOLLOW_Y) {dist_a=dist_y; as=ys;}
      if((*soft_config).a_follow==FOLLOW_Z) {dist_a=dist_z; as=zs;}
      if((*soft_config).a_follow!=INDEPENDANT)
        {if((*soft_config).units==IN && (*soft_config).a_units==MM) dist_a=dist_a*25.4;
         else if((*soft_config).units==MM && (*soft_config).a_units==IN) dist_a=dist_a/25.4;
        }
     }

   //return if vector is zero length
   if(dist_x==0 && dist_y==0 && dist_z==0 && dist_a==0) return COMPLETE;

   //reset limit switches to original state if limits are turned off
   if((*soft_config).limit==OFF) 
     {x_limit=y_limit=z_limit=a_limit=ON;
      cur_debo_x=cur_debo_y=cur_debo_z=cur_debo_a=0;
     }

   //check for invalid speed parameter
   if(fabs(*sp_limit)<.00001) return INVALID_PARAMETER;
   
   //define directions
   if((*hard_config).x_pdir==HIGH && dist_x>0) dir=dir+(*hard_config).x_dbit;
   if((*hard_config).x_pdir==LOW  && dist_x<0) dir=dir+(*hard_config).x_dbit;
   if((*hard_config).y_pdir==HIGH && dist_y>0) dir=dir+(*hard_config).y_dbit;
   if((*hard_config).y_pdir==LOW  && dist_y<0) dir=dir+(*hard_config).y_dbit;
   if((*hard_config).z_pdir==HIGH && dist_z>0) dir=dir+(*hard_config).z_dbit;
   if((*hard_config).z_pdir==LOW  && dist_z<0) dir=dir+(*hard_config).z_dbit;
   if((*hard_config).a_pdir==HIGH && dist_a>0 && (*soft_config).a_status==ON) dir=dir+(*hard_config).a_dbit;
   if((*hard_config).a_pdir==LOW  && dist_a<0 && (*soft_config).a_status==ON) dir=dir+(*hard_config).a_dbit;
   if((*soft_config).a_status==OFF) dir=dir+(*hard_config).a_dbit;
   
   //set up increment for new direction if sign change
   if(pxs!=xs) {if(act_step==HIGH && (step&(*hard_config).x_sbit)==(*hard_config).x_sbit) step=step-(*hard_config).x_sbit;
                if(act_step==LOW && (step&(*hard_config).x_sbit)==0) step=step+(*hard_config).x_sbit;
                if(x_step==HIGH) x_step=LOW; else x_step=HIGH;
                inc_x=(*hard_config).x_per_step/2-inc_x;}
   if(pys!=ys) {if(act_step==HIGH && (step&(*hard_config).y_sbit)==(*hard_config).y_sbit) step=step-(*hard_config).y_sbit;
                if(act_step==LOW && (step&(*hard_config).y_sbit)==0) step=step+(*hard_config).y_sbit;
                if(y_step==HIGH) y_step=LOW; else y_step=HIGH;
                inc_y=(*hard_config).y_per_step/2-inc_y;}
   if(pzs!=zs) {if(act_step==HIGH && (step&(*hard_config).z_sbit)==(*hard_config).z_sbit) step=step-(*hard_config).z_sbit;
                if(act_step==LOW && (step&(*hard_config).z_sbit)==0) step=step+(*hard_config).z_sbit;
                if(z_step==HIGH) z_step=LOW; else z_step=HIGH;
                inc_z=(*hard_config).z_per_step/2-inc_z;}
   if(pas!=as) {if(act_step==HIGH && (step&(*hard_config).a_sbit)==(*hard_config).a_sbit) step=step-(*hard_config).a_sbit;
                if(act_step==LOW && (step&(*hard_config).a_sbit)==0) step=step+(*hard_config).a_sbit;
                if(a_step==HIGH) a_step=LOW; else a_step=HIGH;
                inc_a=(*hard_config).a_per_step/2-inc_a;}
   pxs=xs; pys=ys; pzs=zs; pas=as;
   outportb((*hard_config).port, dir+step);

   if(*sp_limit>0){
   //define speed inc.
   if(dist_x==0 && dist_y==0 && dist_z==0) speed_inc=(*soft_config).a_speed_inc;
   else speed_inc=(*soft_config).speed_inc;

   //define maximum acceleration
   max_accel=get_max_accel(hard_config, dist_x, dist_y, dist_z, dist_a, total_vect);

   //define maximum speed
   max_speed=get_max_speed(hard_config, dist_x, dist_y, dist_z, dist_a, total_vect);

   //define minimum speed
   min_speed=get_min_speed(hard_config, dist_x, dist_y, dist_z, dist_a, total_vect);

   //reset speeds accordingly
   if(*cur_speed<min_speed && *sp_limit>=min_speed) *cur_speed=min_speed;
   if(*cur_speed<*sp_limit && *sp_limit<min_speed) *cur_speed=min_speed=*sp_limit;
   }
   else *cur_speed=0;

   //begin control loop
   while(1)
     {while(tick);
      tick=1;

      //read status and control ports
      if((*soft_config).limit==ON || (*soft_config).e_stop!=OFF) s_port_read=(248&inportb((*hard_config).port+1))^128;
      if((*soft_config).io1_cmm==ON || (*soft_config).io2_cmm==ON ||
         (*soft_config).io3_cmm==ON || (*soft_config).io4_cmm==ON)
         c_port_read=(15&inportb((*hard_config).port+2))^11;
      
      //check for e-stop button or hold line trip
      if((*soft_config).e_stop!=OFF && (*hard_config).e_trip*(*hard_config).e_bit-(s_port_read&(*hard_config).e_bit)==0)
        {if((*soft_config).e_stop==E_STOP_L) {cur_x_sp=cur_y_sp=cur_z_sp=cur_a_sp=0; msg=E_STOP;}
         else while((*hard_config).e_trip*(*hard_config).e_bit-(s_port_read&(*hard_config).e_bit)==0)
           {s_port_read=(248&inportb((*hard_config).port+1))^128;
            //check for ESC key
            if(key==0x01) {key=0; msg=E_STOP; break;}
           }
         if(msg==E_STOP) break;
        }

      if((*soft_config).limit==ON)
        {//check for limit switch trip
         if(x_limit==ON)
           {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_x_sp=0; cur_debo_x=0; x_limit=OFF; if(xs>0) msg=X_PLIMIT; else msg=X_NLIMIT; break;}}
            else cur_debo_x=0;} 
         if(y_limit==ON)
           {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_y_sp=0; cur_debo_y=0; y_limit=OFF; if(ys>0) msg=Y_PLIMIT; else msg=Y_NLIMIT; break;}}
            else cur_debo_y=0;}
         if(z_limit==ON)
           {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_z_sp=0; cur_debo_z=0; z_limit=OFF; if(zs>0) msg=Z_PLIMIT; else msg=Z_NLIMIT; break;}}
            else cur_debo_z=0;}
         if(a_limit==ON && (*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_a_sp=0; cur_debo_a=0; a_limit=OFF; if(as>0) msg=A_PLIMIT; else msg=A_NLIMIT; break;}}
            else cur_debo_a=0;}