[Emc-commit] emc/src/emcmot segmentqueue.c,1.3,1.4 segmentqueue.h,1.3,1.4 emcsegmot.c,1.4,1.5 Makefile,1.38,1.39

[<pr...@us...>]

Update of /cvsroot/emc/emc/src/emcmot
In directory sc8-pr-cvs1:/tmp/cvs-serv24220

Modified Files:
	segmentqueue.c segmentqueue.h emcsegmot.c Makefile 
Log Message:
Updated segment queue files, so that they compile and link. In the
emcmot/Makefile, there's a new target, "seg", that builds the stepper
and servo real-time modules.

This was accomplished by taking the latest emcmot.c, and working in
a layer that lets you switch between TC and segment queue algorithms,
using the "use_tc" variable. Eventually this will be an ini file thing.

It's untested! It shouldn't interfere with anyone since it's not built
by default. To build, make ... seg in the emcmot directory. Edit the
"use_tc" variable and make it 0 for segment queue.


Index: segmentqueue.c
===================================================================
RCS file: /cvsroot/emc/emc/src/emcmot/segmentqueue.c,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** segmentqueue.c	21 Dec 2000 16:22:11 -0000	1.3
--- segmentqueue.c	15 Aug 2003 20:00:58 -0000	1.4
***************
*** 4,7 ****
--- 4,9 ----
  
    Modification history
+   14-Aug-2003  FMP converted EmcPose to PmPose for use with PmLineInit(),
+   PmCircleInit(), etc.
    30-Jun-1999 RSB changed the calculation of the maximum tangential
    acceleration for circles (in sqAddCircle)
***************
*** 188,192 ****
        pmCartScalDiv ( s1->circle.rHelix, s1->circle.angle, &helix);
        pmCartCartAdd ( v1, helix, &v1);
!       pmCartNorm ( v1, &v1 );
  
      }
--- 190,194 ----
        pmCartScalDiv ( s1->circle.rHelix, s1->circle.angle, &helix);
        pmCartCartAdd ( v1, helix, &v1);
!       pmCartUnit ( v1, &v1 );
  
      }
***************
*** 200,204 ****
        pmCartScalDiv ( s2->circle.rHelix, s2->circle.angle, &helix);
        pmCartCartAdd ( s2->circle.rPerp, helix, &v2);
!       pmCartNorm ( v2, &v2 );
      }
  
--- 202,206 ----
        pmCartScalDiv ( s2->circle.rHelix, s2->circle.angle, &helix);
        pmCartCartAdd ( s2->circle.rPerp, helix, &v2);
!       pmCartUnit ( v2, &v2 );
      }
  
***************
*** 1166,1169 ****
--- 1168,1192 ----
  }
  
+ static void emcpose_to_pmpose(EmcPose e, PmPose * p)
+ {
+   p->tran = e.tran;
+   p->rot.s = 1;
+   p->rot.x = e.a;
+   p->rot.y = e.b;
+   p->rot.z = e.c;
+ 
+   return;
+ }
+ 
+ static void pmpose_to_emcpose(PmPose p, EmcPose * e)
+ {
+   e->tran = p.tran;
+   e->a = p.rot.x;
+   e->b = p.rot.y;
+   e->c = p.rot.z;
+ 
+   return;
+ }
+ 
  int sqAddLine(SEGMENTQUEUE  *sq, EmcPose end, int ID)
  {
***************
*** 1171,1175 ****
    SEGMENT *newseg;
    EmcPose start;
! 
  
    /* check if segment queue has been initialized */
--- 1194,1198 ----
    SEGMENT *newseg;
    EmcPose start;
!   PmPose start_pose, end_pose;
  
    /* check if segment queue has been initialized */
***************
*** 1232,1236 ****
  
    /* initialize line */
!   pmLineInit( &newseg->line, newseg->start, newseg->end);
  
    /* set the maximum tangential acceleration for this line */
--- 1255,1261 ----
  
    /* initialize line */
!   emcpose_to_pmpose(newseg->start, &start_pose);
!   emcpose_to_pmpose(newseg->end, &end_pose);
!   pmLineInit( &newseg->line, start_pose, end_pose);
  
    /* set the maximum tangential acceleration for this line */
***************
*** 1261,1264 ****
--- 1286,1290 ----
    PmCartesian helix;
    double absHelix;
+   PmPose start_pose, end_pose;
  
    /* used to calculate the maximum tangential acceleration */
***************
*** 1284,1288 ****
      start=sq->queue[(sq->end+sq->size-1)%sq->size].end;
  
!   pmCircleInit(&circle, start, end, center, normal, turn);
  
    if (circle.angle==0)
--- 1310,1316 ----
      start=sq->queue[(sq->end+sq->size-1)%sq->size].end;
  
!   emcpose_to_pmpose(start, &start_pose);
!   emcpose_to_pmpose(end, &end_pose);
!   pmCircleInit(&circle, start_pose, end_pose, center, normal, turn);
  
    if (circle.angle==0)
***************
*** 1387,1390 ****
--- 1415,1420 ----
    int turn;
    PmCartesian normal, center;
+   PmPose last_point_pose;
+   PmPose start_pose, end_pose;
  
    int npow1, npow2, npow3; /* to speed up cubic calculations */
***************
*** 1528,1537 ****
        sq->currentID=sq->cursor->ID;
  
!       if ( sq->cursor->type == SQ_LINEAR )
!         pmLinePoint( &sq->cursor->line, sq->dist - sq->offset, &sq->lastPoint);
!       else
!         pmCirclePoint( &sq->cursor->circle, \
!                        (sq->dist - sq->offset)/sq->cursor->helixRadius,\
!                        &sq->lastPoint );
      }
  
--- 1558,1569 ----
        sq->currentID=sq->cursor->ID;
  
!       if ( sq->cursor->type == SQ_LINEAR ) {
!         pmLinePoint( &sq->cursor->line, sq->dist - sq->offset, &last_point_pose);
!       } else {
!         pmCirclePoint( &sq->cursor->circle,
!                        (sq->dist - sq->offset)/sq->cursor->helixRadius,
!                        &last_point_pose);
!       }
!       pmpose_to_emcpose(last_point_pose, &sq->lastPoint);
      }
  
***************
*** 1540,1547 ****
         things much easier... */
      {
!       if ( sq->cursor->type == SQ_LINEAR )
!         pmLinePoint(&as->line, sq->dist, &sq->lastPoint);
!       else
!         pmCirclePoint(&as->circle, sq->dist/as->helixRadius, &sq->lastPoint);
      }
  
--- 1572,1581 ----
         things much easier... */
      {
!       if ( sq->cursor->type == SQ_LINEAR ) {
!         pmLinePoint(&as->line, sq->dist, &last_point_pose);
!       } else {
!         pmCirclePoint(&as->circle, sq->dist/as->helixRadius, &last_point_pose);
!       }
!       pmpose_to_emcpose(last_point_pose, &sq->lastPoint);
      }
  
***************
*** 1606,1610 ****
                as->length=sqGiveLength(as->start,as->end);
                as->totLength=as->length;
!               pmLineInit(&as->line,as->start,as->end);
              }
            else
--- 1640,1646 ----
                as->length=sqGiveLength(as->start,as->end);
                as->totLength=as->length;
! 	      emcpose_to_pmpose(as->start, &start_pose);
! 	      emcpose_to_pmpose(as->end, &end_pose);
!               pmLineInit(&as->line, start_pose, end_pose);
              }
            else
***************
*** 1615,1619 ****
                normal = as->circle.normal;
                center = as->circle.center;
!               pmCircleInit( &as->circle, as->start, as->end, \
                              center, normal, turn);
                as->length=as->circle.angle* as->helixRadius;
--- 1651,1657 ----
                normal = as->circle.normal;
                center = as->circle.center;
! 	      emcpose_to_pmpose(as->start, &start_pose);
! 	      emcpose_to_pmpose(as->end, &end_pose);
!               pmCircleInit( &as->circle, start_pose, end_pose,
                              center, normal, turn);
                as->length=as->circle.angle* as->helixRadius;
***************
*** 1749,1752 ****
--- 1787,1791 ----
    int turn;
    PmCartesian normal, center;
+   PmPose start_pose, end_pose;
  
    if ( sq==0 || sq->queue==0 )
***************
*** 1946,1950 ****
            as->length=sqGiveLength(as->start,as->end);
            as->totLength=as->length;
!           pmLineInit(&as->line,as->start,as->end);
          }
        else
--- 1985,1991 ----
            as->length=sqGiveLength(as->start,as->end);
            as->totLength=as->length;
! 	  emcpose_to_pmpose(as->start, &start_pose);
! 	  emcpose_to_pmpose(as->end, &end_pose);
!           pmLineInit(&as->line, start_pose, end_pose);
          }
        else
***************
*** 1954,1958 ****
            normal = as->circle.normal;
            center = as->circle.center;
!           pmCircleInit( &as->circle, as->start, as->end, center, normal, turn);
            as->length=as->circle.angle* as->helixRadius;
            as->totLength= as->length;
--- 1995,2001 ----
            normal = as->circle.normal;
            center = as->circle.center;
! 	  emcpose_to_pmpose(as->start, &start_pose);
! 	  emcpose_to_pmpose(as->end, &end_pose);
!           pmCircleInit(&as->circle, start_pose, end_pose, center, normal, turn);
            as->length=as->circle.angle* as->helixRadius;
            as->totLength= as->length;

Index: segmentqueue.h
===================================================================
RCS file: /cvsroot/emc/emc/src/emcmot/segmentqueue.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** segmentqueue.h	21 Dec 2000 16:22:11 -0000	1.3
--- segmentqueue.h	15 Aug 2003 20:00:58 -0000	1.4
***************
*** 29,32 ****
--- 29,34 ----
  
    Modification history:
+   
+   13-Aug-2003  FMP added #include "emcpos.h" for EmcPose
     8-Jun-1999  RSB added sqSetMaxFeedOverride()
     8-Jun-1999  RSB changed arguments of sqInitQueue, now also the start point
***************
*** 53,56 ****
--- 55,60 ----
  #include "posemath.h"
  /*  #include <linux/cons.h> */
+ 
+ #include "emcpos.h"		/* EmcPose */
  
  #define max(A,B)  ( (A) > (B) ? (A) : (B))

Index: emcsegmot.c
===================================================================
RCS file: /cvsroot/emc/emc/src/emcmot/emcsegmot.c,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** emcsegmot.c	19 May 2002 01:02:48 -0000	1.4
--- emcsegmot.c	15 Aug 2003 20:00:58 -0000	1.5
***************
*** 1,34 ****
! #ifdef rtlinux
! #define MODULE                  /* make it a module for RT-Linux */
! #endif
! 
  /*
!   emcsegmot.c
! 
!   Real-time motion controller
  
!   Queued 3-axis motion planning and cubic subinterpolation.
[...8718 lines suppressed...]
!     emcmotController(0);
!     cur_time = etime();
!     delta = cur_time - delta;
!     delta = emcmotConfig->servoCycleTime - delta;
  
!     if (delta > 0.0) {
!       esleep(delta);
      }
+   }
  
    cleanup_module();
***************
*** 3086,3088 ****
  }
  
! #endif /* not EMCMOT_NO_MAIN */
--- 6027,6029 ----
  }
  
! #endif /* not rtlinux */

Index: Makefile
===================================================================
RCS file: /cvsroot/emc/emc/src/emcmot/Makefile,v
retrieving revision 1.38
retrieving revision 1.39
diff -C2 -d -r1.38 -r1.39
*** Makefile	31 May 2003 11:12:25 -0000	1.38
--- Makefile	15 Aug 2003 20:00:59 -0000	1.39
***************
*** 699,702 ****
--- 699,704 ----
  	-o $@
  
+ seg : $(DEVP_LIB_DIR)/steppersegmod.o $(DEVP_LIB_DIR)/stgsegmod.o
+ 
  # Mini-Tetra, stepper motors with hexapod kinematics
  $(DEVP_LIB_DIR)/minitetra.o: \