Main Page   Class Hierarchy   Alphabetical List   Data Structures   File List   Data Fields   Globals  

emccanon.cc File Reference

#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#include <string.h>
#include <ctype.h>
#include "emc.hh"
#include "canon.hh"
#include "interpl.hh"
#include "emcglb.h"
#include "emcpos.h"

Include dependency graph for emccanon.cc:

Include dependency graph

Go to the source code of this file.

Defines

#define __attribute__(x)
#define TO_EXT_LEN(mm)   ((mm) * GET_EXTERNAL_LENGTH_UNITS())
#define TO_EXT_ANG(deg)   ((deg) * GET_EXTERNAL_ANGLE_UNITS())
#define FROM_EXT_LEN(ext)   ((ext) / GET_EXTERNAL_LENGTH_UNITS())
#define FROM_EXT_ANG(ext)   ((ext) / GET_EXTERNAL_ANGLE_UNITS())
#define TO_PROG_LEN(mm)   ((mm) / (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define TO_PROG_ANG(deg)   (deg)
#define FROM_PROG_LEN(prog)   ((prog) * (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
#define FROM_PROG_ANG(prog)   (prog)
#define MSGLEN   256

Functions

char __attribute__ ((unused)) ident[]="$Id
int properVelocity (double vel)
void SET_ORIGIN_OFFSETS (double x, double y, double z, double a, double b, double c)
void USE_LENGTH_UNITS (CANON_UNITS in_unit)
void SET_TRAVERSE_RATE (double rate)
void STRAIGHT_TRAVERSE (double x, double y, double z, double a, double b, double c)
void SET_FEED_RATE (double rate)
void SET_FEED_REFERENCE (CANON_FEED_REFERENCE reference)
void SET_MOTION_CONTROL_MODE (CANON_MOTION_MODE mode)
CANON_MOTION_MODE GET_MOTION_CONTROL_MODE ()
void SELECT_PLANE (CANON_PLANE in_plane)
void SET_CUTTER_RADIUS_COMPENSATION (double radius)
void START_CUTTER_RADIUS_COMPENSATION (int side)
void STOP_CUTTER_RADIUS_COMPENSATION ()
void START_SPEED_FEED_SYNCH ()
void STOP_SPEED_FEED_SYNCH ()
void SELECT_MOTION_MODE (CANON_MOTION_MODE mode)
void ARC_FEED (double first_end, double second_end, double first_axis, double second_axis, int rotation, double axis_end_point, double a, double b, double c)
void STRAIGHT_FEED (double x, double y, double z, double a, double b, double c)
void STRAIGHT_PROBE (double x, double y, double z, double a, double b, double c)
void DWELL (double seconds)
void SPINDLE_RETRACT_TRAVERSE ()
void START_SPINDLE_CLOCKWISE ()
void START_SPINDLE_COUNTERCLOCKWISE ()
void SET_SPINDLE_SPEED (double r)
void STOP_SPINDLE_TURNING ()
void SPINDLE_RETRACT ()
void ORIENT_SPINDLE (double orientation, CANON_DIRECTION direction)
void USE_NO_SPINDLE_FORCE ()
void USE_TOOL_LENGTH_OFFSET (double length)
void CHANGE_TOOL (int slot)
void SELECT_TOOL (int slot)
void CLAMP_AXIS (CANON_AXIS axis)
char * setString (char *dst, const char *src, int maxlen)
char * addString (char *dst, const char *src, int maxlen)
void COMMENT (char *comment)
void DISABLE_FEED_OVERRIDE ()
void DISABLE_SPEED_OVERRIDE ()
void ENABLE_FEED_OVERRIDE ()
void ENABLE_SPEED_OVERRIDE ()
void FLOOD_OFF ()
void FLOOD_ON ()
void MESSAGE (char *s)
void MIST_OFF ()
void MIST_ON ()
void PALLET_SHUTTLE ()
void TURN_PROBE_OFF ()
void TURN_PROBE_ON ()
void UNCLAMP_AXIS (CANON_AXIS axis)
void PROGRAM_STOP ()
void OPTIONAL_PROGRAM_STOP ()
void PROGRAM_END ()
CANON_VECTOR GET_PROGRAM_ORIGIN ()
CANON_UNITS GET_LENGTH_UNITS ()
CANON_PLANE GET_PLANE ()
double GET_TOOL_LENGTH_OFFSET ()
void INIT_CANON ()
void CANON_ERROR (const char *fmt,...)
CANON_TOOL_TABLE GET_EXTERNAL_TOOL_TABLE (int pocket)
CANON_POSITION GET_EXTERNAL_POSITION ()
CANON_POSITION GET_EXTERNAL_PROBE_POSITION ()
double GET_EXTERNAL_PROBE_VALUE ()
int IS_EXTERNAL_QUEUE_EMPTY ()
double GET_EXTERNAL_FEED_RATE ()
double GET_EXTERNAL_TRAVERSE_RATE ()
double GET_EXTERNAL_LENGTH_UNITS ()
double GET_EXTERNAL_ANGLE_UNITS ()
int GET_EXTERNAL_TOOL ()
int GET_EXTERNAL_MIST ()
int GET_EXTERNAL_FLOOD ()
int GET_EXTERNAL_POCKET ()
double GET_EXTERNAL_SPEED ()
CANON_DIRECTION GET_EXTERNAL_SPINDLE ()
int GET_EXTERNAL_TOOL_MAX ()

Variables

CANON_MOTION_MODE canonMotionMode = 0
double spindleSpeed = 0.0
int preppedTool = 0
double currentLinearFeedRate = 1.0
double currentLinearTraverseRate = 1.0
double currentAngularFeedRate = 1.0
double currentAngularTraverseRate = 1.0
int pure_angular_move = 0
double currentToolLengthOffset = 0.0
double lastVelSet = -1
int spindleOn = 0
FILE * probefile = NULL

Define Documentation

#define FROM_EXT_ANG (  ext    )     ((ext) / GET_EXTERNAL_ANGLE_UNITS())
 

Definition at line 145 of file emccanon.cc.

Referenced by STRAIGHT_FEED(), and STRAIGHT_TRAVERSE().

#define FROM_EXT_LEN (  ext    )     ((ext) / GET_EXTERNAL_LENGTH_UNITS())
 

Definition at line 144 of file emccanon.cc.

Referenced by ARC_FEED(), GET_EXTERNAL_FEED_RATE(), GET_EXTERNAL_POSITION(), GET_EXTERNAL_PROBE_POSITION(), GET_EXTERNAL_TOOL_TABLE(), GET_EXTERNAL_TRAVERSE_RATE(), INIT_CANON(), SET_FEED_RATE(), SET_TRAVERSE_RATE(), STRAIGHT_FEED(), STRAIGHT_PROBE(), and STRAIGHT_TRAVERSE().

#define FROM_PROG_ANG (  prog    )     (prog)
 

Definition at line 153 of file emccanon.cc.

Referenced by SET_FEED_RATE(), SET_TRAVERSE_RATE(), STRAIGHT_FEED(), and STRAIGHT_TRAVERSE().

#define FROM_PROG_LEN (  prog    )     ((prog) * (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
 

Definition at line 152 of file emccanon.cc.

Referenced by ARC_FEED(), SET_FEED_RATE(), SET_ORIGIN_OFFSETS(), SET_TRAVERSE_RATE(), STRAIGHT_FEED(), STRAIGHT_PROBE(), STRAIGHT_TRAVERSE(), and USE_TOOL_LENGTH_OFFSET().

#define MSGLEN   256
 

#define TO_EXT_ANG (  deg    )     ((deg) * GET_EXTERNAL_ANGLE_UNITS())
 

Definition at line 141 of file emccanon.cc.

Referenced by STRAIGHT_FEED(), STRAIGHT_TRAVERSE(), and properVelocity().

#define TO_EXT_LEN (  mm    )     ((mm) * GET_EXTERNAL_LENGTH_UNITS())
 

Definition at line 140 of file emccanon.cc.

Referenced by ARC_FEED(), GET_TOOL_LENGTH_OFFSET(), SET_ORIGIN_OFFSETS(), STRAIGHT_FEED(), STRAIGHT_PROBE(), STRAIGHT_TRAVERSE(), USE_TOOL_LENGTH_OFFSET(), and properVelocity().

#define TO_PROG_ANG (  deg    )     (deg)
 

Definition at line 149 of file emccanon.cc.

Referenced by GET_EXTERNAL_POSITION(), and GET_EXTERNAL_PROBE_POSITION().

#define TO_PROG_LEN (  mm    )     ((mm) / (lengthUnits == CANON_UNITS_INCHES ? 25.4 : lengthUnits == CANON_UNITS_CM ? 10.0 : 1.0))
 

Definition at line 148 of file emccanon.cc.

Referenced by GET_EXTERNAL_FEED_RATE(), GET_EXTERNAL_POSITION(), GET_EXTERNAL_PROBE_POSITION(), GET_EXTERNAL_TOOL_TABLE(), GET_EXTERNAL_TRAVERSE_RATE(), and GET_PROGRAM_ORIGIN().

#define __attribute__ (  x    ) 
 

Definition at line 85 of file emccanon.cc.

Function Documentation

void ARC_FEED (  double    first_end,
double    second_end,
double    first_axis,
double    second_axis,
int    rotation,
double    axis_end_point,
double    a = 0,
double    b = 0,
double    c = 0
) 
 

Definition at line 462 of file emccanon.cc.

00466 {
00467   EmcPose end;
00468   PM_CARTESIAN center, normal;
00469   EMC_TRAJ_CIRCULAR_MOVE circularMoveMsg;
00470   EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
00471   int full_circle_in_active_plane = 0;
00472   double v1, v2, vel;
00473 
00474   // convert to absolute mm units
00475   first_axis = FROM_PROG_LEN(first_axis);
00476   second_axis = FROM_PROG_LEN(second_axis);
00477   first_end = FROM_PROG_LEN(first_end);
00478   second_end = FROM_PROG_LEN(second_end);
00479   axis_end_point = FROM_PROG_LEN(axis_end_point);
00480 
00481   // associate x with x, etc., offset by program origin, and set normals
00482   switch (activePlane) {
00483   case CANON_PLANE_XY:
00484 
00485     // offset and align args properly
00486     end.tran.x = first_end + programOrigin.x;
00487     end.tran.y = second_end + programOrigin.y;
00488     end.tran.z = axis_end_point + programOrigin.z;
00489     end.tran.z += currentToolLengthOffset;
00490     if (canonEndPoint.x == end.tran.x &&
00491         canonEndPoint.y == end.tran.y)
00492       {
00493         full_circle_in_active_plane = 1;
00494       }
00495     center.x = first_axis + programOrigin.x;
00496     center.y = second_axis + programOrigin.y;
00497     center.z = end.tran.z;
00498     normal.x = 0.0;
00499     normal.y = 0.0;
00500     normal.z = 1.0;
00501 
00502     // limit vel to min of X-Y-F
00503     vel = currentLinearFeedRate;
00504     v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]);
00505     v2 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]);
00506     if (vel > v1) {
00507       vel = v1;
00508     }
00509     if (vel > v2) {
00510       vel = v2;
00511     }
00512 
00513     break;
00514 
00515   case CANON_PLANE_YZ:
00516 
00517     // offset and align args properly
00518     end.tran.y = first_end + programOrigin.y;
00519     end.tran.z = second_end + programOrigin.z;
00520     end.tran.x = axis_end_point + programOrigin.x;
00521     end.tran.z += currentToolLengthOffset;
00522     if (canonEndPoint.z == end.tran.z &&
00523         canonEndPoint.y == end.tran.y) {
00524       full_circle_in_active_plane = 1;
00525     }
00526 
00527     center.y = first_axis + programOrigin.y;
00528     center.z = second_axis + programOrigin.z;
00529     center.x = end.tran.x;
00530     normal.y = 0.0;
00531     normal.z = 0.0;
00532     normal.x = 1.0;
00533 
00534     // limit vel to min of Y-Z-F
00535     vel = currentLinearFeedRate;
00536     v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]);
00537     v2 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]);
00538     if (vel > v1) {
00539       vel = v1;
00540     }
00541     if (vel > v2) {
00542       vel = v2;
00543     }
00544 
00545     break;
00546 
00547   case CANON_PLANE_XZ:
00548 
00549     // offset and align args properly
00550     end.tran.z = first_end + programOrigin.z;
00551     end.tran.x = second_end + programOrigin.x;
00552     end.tran.y = axis_end_point + programOrigin.y;
00553     end.tran.z += currentToolLengthOffset;
00554     if (canonEndPoint.x== end.tran.x &&
00555         canonEndPoint.z == end.tran.z)
00556       {
00557         full_circle_in_active_plane = 1;
00558       }
00559 
00560     center.z = first_axis + programOrigin.z;
00561     center.x = second_axis + programOrigin.x;
00562     center.y = end.tran.y;
00563     normal.z = 0.0;
00564     normal.x = 0.0;
00565     normal.y = 1.0;
00566 
00567     // limit vel to min of X-Z-F
00568     vel = currentLinearFeedRate;
00569     v1 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]);
00570     v2 = FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]);
00571     if (vel > v1) {
00572       vel = v1;
00573     }
00574     if (vel > v2) {
00575       vel = v2;
00576     }
00577 
00578     break;
00579   }
00580 
00581   // set proper velocity
00582   properVelocity(vel);
00583 
00584   /*
00585     mapping of rotation to turns:
00586 
00587     rotation  turns
00588     --------  -----
00589     0         none (linear move)
00590     1         0
00591     2         1
00592     -1        -1
00593     -2        -2
00594   */
00595 
00596   if (rotation == 0) {
00597     // linear move
00598 
00599     linearMoveMsg.end.tran.x = TO_EXT_LEN(end.tran.x);
00600     linearMoveMsg.end.tran.y = TO_EXT_LEN(end.tran.y);
00601     linearMoveMsg.end.tran.z = TO_EXT_LEN(end.tran.z);
00602 
00603     // fill in the orientation
00604     linearMoveMsg.end.a = a;
00605     linearMoveMsg.end.b = b;
00606     linearMoveMsg.end.c = c;
00607     
00608 
00609     interp_list.append(linearMoveMsg);
00610   }
00611   else if (rotation > 0) {
00612 
00613 #if 0
00614     // This should not be needed anymore with fix in _posemath.c 
00615     // If starting and ending on same point move around the
00616     // circle, don't just stay put.
00617     if (full_circle_in_active_plane) {
00618       rotation++;
00619     }
00620 #endif
00621 
00622     circularMoveMsg.end.tran.x = TO_EXT_LEN(end.tran.x);
00623     circularMoveMsg.end.tran.y = TO_EXT_LEN(end.tran.y);
00624     circularMoveMsg.end.tran.z = TO_EXT_LEN(end.tran.z);
00625 
00626     circularMoveMsg.center.x = TO_EXT_LEN(center.x);
00627     circularMoveMsg.center.y = TO_EXT_LEN(center.y);
00628     circularMoveMsg.center.z = TO_EXT_LEN(center.z);
00629 
00630     circularMoveMsg.normal.x = TO_EXT_LEN(normal.x);
00631     circularMoveMsg.normal.y = TO_EXT_LEN(normal.y);
00632     circularMoveMsg.normal.z = TO_EXT_LEN(normal.z);
00633 
00634     circularMoveMsg.turn = rotation - 1;
00635 
00636     // fill in the orientation
00637     circularMoveMsg.end.a = a;
00638     circularMoveMsg.end.b = b;
00639     circularMoveMsg.end.c = c;
00640 
00641 
00642     interp_list.append(circularMoveMsg);
00643   }
00644   else {
00645     // reverse turn
00646 
00647 #if 0
00648     // This should not be needed anymore with fix in _posemath.c 
00649     // If starting and ending on same point move around the
00650     // circle, don't just stay put.
00651     if (full_circle_in_active_plane) {
00652       rotation--;
00653     }
00654 #endif
00655     circularMoveMsg.end.tran.x = TO_EXT_LEN(end.tran.x);
00656     circularMoveMsg.end.tran.y = TO_EXT_LEN(end.tran.y);
00657     circularMoveMsg.end.tran.z = TO_EXT_LEN(end.tran.z);
00658 
00659     circularMoveMsg.center.x = TO_EXT_LEN(center.x);
00660     circularMoveMsg.center.y = TO_EXT_LEN(center.y);
00661     circularMoveMsg.center.z = TO_EXT_LEN(center.z);
00662 
00663     circularMoveMsg.normal.x = TO_EXT_LEN(normal.x);
00664     circularMoveMsg.normal.y = TO_EXT_LEN(normal.y);
00665     circularMoveMsg.normal.z = TO_EXT_LEN(normal.z);
00666 
00667     circularMoveMsg.turn = rotation;
00668 
00669 
00670     // fill in the orientation
00671     circularMoveMsg.end.a = a;
00672     circularMoveMsg.end.b = b;
00673     circularMoveMsg.end.c = c;
00674 
00675     interp_list.append(circularMoveMsg);
00676   }
00677 
00678 
00679   // update the end point
00680   canonUpdateEndPoint(end.tran.x, end.tran.y, end.tran.z, a, b, c);
00681 }

void CANON_ERROR (  const char *    fmt,
...   
) 
 

Definition at line 1240 of file emccanon.cc.

Referenced by GET_EXTERNAL_ANGLE_UNITS(), and GET_EXTERNAL_LENGTH_UNITS(). 

01241 {
01242   va_list ap;
01243   EMC_OPERATOR_ERROR operator_error_msg;
01244 
01245   operator_error_msg.id = 0;
01246   if (fmt != NULL) {
01247     va_start(ap, fmt);
01248     vsprintf(operator_error_msg.error, fmt, ap);
01249     va_end(ap);
01250   }
01251   else {
01252     operator_error_msg.error[0] = 0;
01253   }
01254 
01255   interp_list.append(operator_error_msg);
01256 }

void CHANGE_TOOL (  int    slot ) 
 

Definition at line 949 of file emccanon.cc.

00950 {
00951   EMC_TOOL_LOAD load_tool_msg;
00952 
00953   interp_list.append(load_tool_msg);
00954 }

void CLAMP_AXIS (  CANON_AXIS    axis ) 
 

Definition at line 967 of file emccanon.cc.

00968 {
00969   // FIXME-- unimplemented
00970 }

void COMMENT (  char *    s ) 
 

Definition at line 1012 of file emccanon.cc.

01013 {
01014   // nothing need be done here, but you can play tricks with hot comments
01015 
01016 #define MSGLEN 256
01017   char msg[MSGLEN];
01018   char probefilename[MSGLEN];
01019   char *ptr;
01020 
01021   // set RPY orientation for subsequent moves
01022   if (!strncmp(comment, "RPY", strlen("RPY"))) {
01023     PM_RPY rpy;
01024     // it's RPY <R> <P> <Y>
01025     if (3 != sscanf(comment, "%*s %lf %lf %lf", &rpy.r, &rpy.p, &rpy.y)) {
01026       // print current orientation
01027       printf("rpy = %f %f %f, quat = %f %f %f %f\n",
01028              rpy.r, rpy.p, rpy.y,
01029              quat.s, quat.x, quat.y, quat.z);
01030     }
01031     else {
01032       // set and print orientation
01033       quat = rpy;
01034       printf("rpy = %f %f %f, quat = %f %f %f %f\n",
01035              rpy.r, rpy.p, rpy.y,
01036              quat.s, quat.x, quat.y, quat.z);
01037     }
01038     return;
01039   }
01040 
01041   // open probe output file
01042   if (!strncmp(comment, "PROBEOPEN", strlen("PROBEOPEN"))) {
01043     // position ptr to first char after PROBEOPEN
01044     ptr = &comment[strlen("PROBEOPEN")];
01045     // and step over white space to name, or NULL
01046     while (isspace(*ptr)) {
01047       ptr++;
01048     }
01049     setString(probefilename, ptr, MSGLEN);
01050     if (NULL == (probefile = fopen(probefilename, "w"))) {
01051       // pop up a warning message
01052       setString(msg, "can't open probe file ", MSGLEN);
01053       addString(msg, probefilename, MSGLEN);
01054       MESSAGE(msg);
01055       probefile = NULL;
01056     }
01057     return;
01058   }
01059 
01060   // close probe output file
01061   if (!strncmp(comment, "PROBECLOSE", strlen("PROBECLOSE"))) {
01062     if (probefile != NULL) {
01063       fclose(probefile);
01064       probefile = NULL;
01065     }
01066     return;
01067   }
01068 
01069   return;
01070 }

void DISABLE_FEED_OVERRIDE (    ) 
 

Definition at line 1072 of file emccanon.cc.

01073 {
01074   // FIXME-- unimplemented
01075 }

void DISABLE_SPEED_OVERRIDE (    ) 
 

Definition at line 1077 of file emccanon.cc.

01078 {
01079   // FIXME-- unimplemented
01080 }

void DWELL (  double    seconds ) 
 

Definition at line 844 of file emccanon.cc.

00845 {
00846   EMC_TRAJ_DELAY delayMsg;
00847 
00848   delayMsg.delay = seconds;
00849 
00850   interp_list.append(delayMsg);
00851 }

void ENABLE_FEED_OVERRIDE (    ) 
 

Definition at line 1082 of file emccanon.cc.

01083 {
01084   // FIXME-- unimplemented
01085 }

void ENABLE_SPEED_OVERRIDE (    ) 
 

Definition at line 1087 of file emccanon.cc.

01088 {
01089   // FIXME-- unimplemented
01090 }

void FLOOD_OFF (    ) 
 

Definition at line 1092 of file emccanon.cc.

01093 {
01094   EMC_COOLANT_FLOOD_OFF flood_off_msg;
01095 
01096   interp_list.append(flood_off_msg);
01097 }

void FLOOD_ON (    ) 
 

Definition at line 1099 of file emccanon.cc.

01100 {
01101   EMC_COOLANT_FLOOD_ON flood_on_msg;
01102 
01103   interp_list.append(flood_on_msg);
01104 }

double GET_EXTERNAL_ANGLE_UNITS (    ) 
 

Definition at line 1418 of file emccanon.cc.

01419 {
01420   double u;
01421 
01422   u = emcStatus->motion.traj.angularUnits;
01423 
01424   if (u == 0) {
01425     CANON_ERROR("external angle units are zero");
01426     return 1.0;
01427   }
01428   else {
01429     return u;
01430   }
01431 }

double GET_EXTERNAL_FEED_RATE (    ) 
 

Definition at line 1376 of file emccanon.cc.

01377 {
01378   double feed;
01379 
01380   // convert from external to program units
01381   feed = TO_PROG_LEN(FROM_EXT_LEN(emcStatus->motion.traj.velocity));
01382 
01383   // now convert from per-sec to per-minute
01384   feed *= 60.0;
01385 
01386   return feed;
01387 }

int GET_EXTERNAL_FLOOD (    ) 
 

Definition at line 1443 of file emccanon.cc.

01444 {
01445   return emcStatus->io.coolant.flood;
01446 }

double GET_EXTERNAL_LENGTH_UNITS (    ) 
 

Definition at line 1403 of file emccanon.cc.

01404 {
01405   double u;
01406 
01407   u = emcStatus->motion.traj.linearUnits;
01408 
01409   if (u == 0) {
01410     CANON_ERROR("external length units are zero");
01411     return 1.0;
01412   }
01413   else {
01414     return u;
01415   }
01416 }

int GET_EXTERNAL_MIST (    ) 
 

Definition at line 1438 of file emccanon.cc.

01439 {
01440   return emcStatus->io.coolant.mist;
01441 }

int GET_EXTERNAL_POCKET (    ) 
 

Definition at line 1448 of file emccanon.cc.

01449 {
01450   return emcStatus->io.tool.toolPrepped;
01451 }

CANON_POSITION GET_EXTERNAL_POSITION (    ) 
 

Definition at line 1288 of file emccanon.cc.

01289 {
01290   CANON_POSITION position;
01291   EmcPose pos;
01292 
01293   pos = emcStatus->motion.traj.position;
01294 
01295   // first update internal record of last position
01296   canonEndPoint.x = FROM_EXT_LEN(pos.tran.x) - programOrigin.x;
01297   canonEndPoint.y = FROM_EXT_LEN(pos.tran.y) - programOrigin.y;
01298   canonEndPoint.z = FROM_EXT_LEN(pos.tran.z) - programOrigin.z;
01299   canonEndPoint.z = FROM_EXT_LEN(pos.tran.z) - programOrigin.z;
01300   canonEndPoint.z -= currentToolLengthOffset;
01301 
01302 
01303   canonEndPoint.a = FROM_EXT_ANG(pos.a) - programOrigin.a;
01304   canonEndPoint.b = FROM_EXT_ANG(pos.b) - programOrigin.b;
01305   canonEndPoint.c = FROM_EXT_ANG(pos.c) - programOrigin.c;
01306 
01307   
01308 
01309   // now calculate position in program units, for interpreter
01310   position.x = TO_PROG_LEN(canonEndPoint.x);
01311   position.y = TO_PROG_LEN(canonEndPoint.y);
01312   position.z = TO_PROG_LEN(canonEndPoint.z);
01313   position.z -= TO_PROG_LEN(currentToolLengthOffset);
01314 
01315 
01316   position.a = TO_PROG_ANG(canonEndPoint.a);
01317   position.b = TO_PROG_ANG(canonEndPoint.b);
01318   position.c = TO_PROG_ANG(canonEndPoint.c);
01319 
01320   return position;
01321 }

CANON_POSITION GET_EXTERNAL_PROBE_POSITION (    ) 
 

Definition at line 1323 of file emccanon.cc.

01324 {
01325   CANON_POSITION position;
01326   EmcPose pos;
01327   static CANON_POSITION last_probed_position;
01328 
01329   pos = emcStatus->motion.traj.probedPosition;
01330 
01331   // first update internal record of last position
01332   canonEndPoint.x = FROM_EXT_LEN(pos.tran.x) - programOrigin.x;
01333   canonEndPoint.y = FROM_EXT_LEN(pos.tran.y) - programOrigin.y;
01334   canonEndPoint.z = FROM_EXT_LEN(pos.tran.z) - programOrigin.z;
01335   canonEndPoint.z -= currentToolLengthOffset;
01336 
01337   canonEndPoint.a = FROM_EXT_ANG(pos.a) - programOrigin.a;
01338   canonEndPoint.b = FROM_EXT_ANG(pos.b) - programOrigin.b;
01339   canonEndPoint.c = FROM_EXT_ANG(pos.c) - programOrigin.c;  
01340 
01341   // now calculate position in program units, for interpreter
01342   position.x = TO_PROG_LEN(canonEndPoint.x);
01343   position.y = TO_PROG_LEN(canonEndPoint.y);
01344   position.z = TO_PROG_LEN(canonEndPoint.z);
01345   position.z -= TO_PROG_LEN(currentToolLengthOffset);
01346 
01347   position.a = TO_PROG_ANG(canonEndPoint.a);
01348   position.b = TO_PROG_ANG(canonEndPoint.b);
01349   position.c = TO_PROG_ANG(canonEndPoint.c);
01350 
01351   // FIXME-- back end of hot comment
01352   if (probefile != NULL) {
01353       if (last_probed_position.x != position.x ||
01354           last_probed_position.y != position.y ||
01355           last_probed_position.z != position.z) {
01356         fprintf(probefile, "%f %f %f\n", position.x, position.y, position.z);
01357         last_probed_position = position;
01358       }
01359   }
01360   
01361   return position;
01362 }

double GET_EXTERNAL_PROBE_VALUE (    ) 
 

Definition at line 1364 of file emccanon.cc.

01365 {
01366   // only for analog non-contact probe, so force a 0
01367   return 0.0;
01368 }

double GET_EXTERNAL_SPEED (    ) 
 

Definition at line 1453 of file emccanon.cc.

01454 {
01455   // speed is in RPMs everywhere
01456   return emcStatus->io.spindle.speed;
01457 }

CANON_DIRECTION GET_EXTERNAL_SPINDLE (    ) 
 

Definition at line 1459 of file emccanon.cc.

01460 {
01461   if (emcStatus->io.spindle.speed == 0)
01462     {
01463       return CANON_STOPPED;
01464     }
01465 
01466   if (emcStatus->io.spindle.speed >= 0.0)
01467     {
01468       return CANON_CLOCKWISE;
01469     }
01470 
01471   return CANON_COUNTERCLOCKWISE;
01472 }

int GET_EXTERNAL_TOOL (    ) 
 

Definition at line 1433 of file emccanon.cc.

01434 {
01435   return emcStatus->io.tool.toolInSpindle;
01436 }

int GET_EXTERNAL_TOOL_MAX (    ) 
 

Definition at line 1474 of file emccanon.cc.

01475 {
01476   return CANON_TOOL_MAX;
01477 }

CANON_TOOL_TABLE GET_EXTERNAL_TOOL_TABLE (  int    pocket ) 
 

Definition at line 1268 of file emccanon.cc.

01269 {
01270   CANON_TOOL_TABLE retval;
01271 
01272   if (pocket < 0 || pocket >= CANON_TOOL_MAX) {
01273     retval.id = 0;
01274     retval.length = 0.0;
01275     retval.diameter = 0.0;
01276   }
01277   else {
01278     retval = emcStatus->io.tool.toolTable[pocket];
01279 
01280     // convert from user to program units
01281     retval.length = TO_PROG_LEN(FROM_EXT_LEN(retval.length));
01282     retval.diameter = TO_PROG_LEN(FROM_EXT_LEN(retval.diameter));
01283   }
01284 
01285   return retval;
01286 }

double GET_EXTERNAL_TRAVERSE_RATE (    ) 
 

Definition at line 1390 of file emccanon.cc.

01391 {
01392   double traverse;
01393 
01394   // convert from external to program units
01395   traverse = TO_PROG_LEN(FROM_EXT_LEN(emcStatus->motion.traj.maxVelocity));
01396 
01397   // now convert from per-sec to per-minute
01398   traverse *= 60.0;
01399 
01400   return traverse;
01401 }

CANON_UNITS GET_LENGTH_UNITS (    ) 
 

Definition at line 1192 of file emccanon.cc.

01193 {
01194   return lengthUnits;
01195 }

CANON_MOTION_MODE GET_MOTION_CONTROL_MODE (    ) 
 

Definition at line 418 of file emccanon.cc.

00419 {
00420   return canonMotionMode;
00421 }

CANON_PLANE GET_PLANE (    ) 
 

Definition at line 1197 of file emccanon.cc.

01198 {
01199   return activePlane;
01200 }

CANON_VECTOR GET_PROGRAM_ORIGIN (    ) 
 

Definition at line 1179 of file emccanon.cc.

01180 {
01181   CANON_VECTOR origin;
01182 
01183   /* and convert from mm units to interpreter units */
01184   origin.x = TO_PROG_LEN(programOrigin.x);
01185   origin.y = TO_PROG_LEN(programOrigin.y);
01186   origin.z = TO_PROG_LEN(programOrigin.z);
01187 
01188   return origin;                /* in program units */
01189 }

double GET_TOOL_LENGTH_OFFSET (    ) 
 

Definition at line 1202 of file emccanon.cc.

01203 {
01204   return TO_EXT_LEN(currentToolLengthOffset);
01205 }

void INIT_CANON (    ) 
 

Definition at line 1212 of file emccanon.cc.

01213 {
01214   // initialize locals to original values
01215   programOrigin.x = 0.0;
01216   programOrigin.y = 0.0;
01217   programOrigin.z = 0.0;
01218   programOrigin.a = 0.0;
01219   programOrigin.b = 0.0;
01220   programOrigin.c = 0.0;
01221   activePlane = CANON_PLANE_XY;
01222   canonEndPoint.x = 0.0;
01223   canonEndPoint.y = 0.0;
01224   canonEndPoint.z = 0.0;
01225   canonEndPoint.a = 0.0;
01226   canonEndPoint.b = 0.0;
01227   canonEndPoint.c = 0.0;
01228   SET_MOTION_CONTROL_MODE(CANON_CONTINUOUS);
01229   spindleSpeed = 0.0;
01230   preppedTool = 0;
01231   currentLinearFeedRate = FROM_EXT_LEN(TRAJ_DEFAULT_VELOCITY); // from emcglb.h
01232   currentLinearTraverseRate = FROM_EXT_LEN(TRAJ_MAX_VELOCITY); // from emcglb.h
01233   currentAngularFeedRate = 0.0;
01234   currentAngularTraverseRate = 0.0;
01235   currentToolLengthOffset = 0.0;
01236   lengthUnits = CANON_UNITS_MM;
01237 }

int IS_EXTERNAL_QUEUE_EMPTY (    ) 
 

Definition at line 1370 of file emccanon.cc.

01371 {
01372   return emcStatus->motion.traj.queue == 0 ? 1 : 0;
01373 }

void MESSAGE (  char *    s ) 
 

Definition at line 1106 of file emccanon.cc.

01107 {
01108   EMC_OPERATOR_DISPLAY operator_display_msg;
01109 
01110   operator_display_msg.id = 0;
01111   strncpy(operator_display_msg.display, s, EMC_OPERATOR_DISPLAY_LEN);
01112   operator_display_msg.display[EMC_OPERATOR_DISPLAY_LEN - 1] = 0;
01113 
01114   interp_list.append(operator_display_msg);
01115 }

void MIST_OFF (    ) 
 

Definition at line 1117 of file emccanon.cc.

01118 {
01119   EMC_COOLANT_MIST_OFF mist_off_msg;
01120 
01121   interp_list.append(mist_off_msg);
01122 }

void MIST_ON (    ) 
 

Definition at line 1124 of file emccanon.cc.

01125 {
01126   EMC_COOLANT_MIST_ON mist_on_msg;
01127 
01128   interp_list.append(mist_on_msg);
01129 }

void OPTIONAL_PROGRAM_STOP (    ) 
 

Definition at line 1165 of file emccanon.cc.

01166 {
01167   // FIXME-- implemented as PROGRAM_STOP, that is, no option
01168   PROGRAM_STOP();
01169 }

void ORIENT_SPINDLE (  double    orientation,
CANON_DIRECTION    direction
) 
 

Definition at line 914 of file emccanon.cc.

00915 {
00916   // FIXME-- unimplemented
00917 }

void PALLET_SHUTTLE (    ) 
 

Definition at line 1131 of file emccanon.cc.

01132 {
01133   // FIXME-- unimplemented
01134 }

void PROGRAM_END (    ) 
 

Definition at line 1171 of file emccanon.cc.

01172 {
01173   EMC_TASK_PLAN_END endMsg;
01174 
01175   interp_list.append(endMsg);
01176 }

void PROGRAM_STOP (    ) 
 

Definition at line 1155 of file emccanon.cc.

01156 {
01157   /*
01158     implement this as a pause. A resume will cause motion to proceed.
01159    */
01160   EMC_TASK_PLAN_PAUSE pauseMsg;
01161 
01162   interp_list.append(pauseMsg);
01163 }

void SELECT_MOTION_MODE (  CANON_MOTION_MODE    mode ) 
 

Definition at line 453 of file emccanon.cc.

00454 {
00455   // nothing need be done here
00456 }

void SELECT_PLANE (  CANON_PLANE    in_plane ) 
 

Definition at line 423 of file emccanon.cc.

00424 {
00425   activePlane = in_plane;
00426 }

void SELECT_TOOL (  int    slot ) 
 

Definition at line 956 of file emccanon.cc.

00957 {
00958   EMC_TOOL_PREPARE prep_for_tool_msg;
00959 
00960   prep_for_tool_msg.tool = slot;
00961 
00962   interp_list.append(prep_for_tool_msg);
00963 }

void SET_CUTTER_RADIUS_COMPENSATION (  double    radius ) 
 

Definition at line 428 of file emccanon.cc.

00429 {
00430   // nothing need be done here
00431 }

void SET_FEED_RATE (  double    rate ) 
 

Definition at line 371 of file emccanon.cc.

00372 {
00373   double vmax;
00374 
00375   /* convert from /min to /sec */
00376   rate /= 60.0;
00377 
00378   /* convert to traj units (mm) if needed */
00379   currentLinearFeedRate = FROM_PROG_LEN(rate);
00380   currentAngularFeedRate = FROM_PROG_ANG(rate);
00381 
00382 
00383   /* make sure it's less than system max velocity */
00384   vmax = FROM_EXT_LEN(TRAJ_MAX_VELOCITY);
00385   if (currentLinearFeedRate > vmax) {
00386     currentLinearFeedRate = vmax;
00387   }
00388 }

void SET_FEED_REFERENCE (  CANON_FEED_REFERENCE    reference ) 
 

Definition at line 390 of file emccanon.cc.

00391 {
00392   // nothing need be done here
00393 }

void SET_MOTION_CONTROL_MODE (  CANON_MOTION_MODE    mode ) 
 

Definition at line 395 of file emccanon.cc.

00396 {
00397   EMC_TRAJ_SET_TERM_COND setTermCondMsg;
00398 
00399   if (mode != canonMotionMode)
00400     {
00401       canonMotionMode = mode;
00402 
00403       switch (mode)
00404         {
00405         case CANON_CONTINUOUS:
00406           setTermCondMsg.cond = EMC_TRAJ_TERM_COND_BLEND;
00407           break;
00408 
00409         default:
00410           setTermCondMsg.cond = EMC_TRAJ_TERM_COND_STOP;
00411           break;
00412         }
00413 
00414       interp_list.append(setTermCondMsg);
00415     }
00416 }

void SET_ORIGIN_OFFSETS (  double    x,
double    y,
double    z,
double    a = 0,
double    b = 0,
double    c = 0
) 
 

Definition at line 214 of file emccanon.cc.

00216 {
00217   EMC_TRAJ_SET_ORIGIN set_origin_msg;
00218 
00219   /* convert to mm units */
00220   x = FROM_PROG_LEN(x);
00221   y = FROM_PROG_LEN(y);
00222   z = FROM_PROG_LEN(z);
00223 
00224   programOrigin.x = x;
00225   programOrigin.y = y;
00226   programOrigin.z = z;
00227 
00228   /* append it to interp list so it gets updated at the right time,
00229      not at read-ahead time */
00230   set_origin_msg.origin.tran.x = TO_EXT_LEN(programOrigin.x);
00231   set_origin_msg.origin.tran.y = TO_EXT_LEN(programOrigin.y);
00232   set_origin_msg.origin.tran.z = TO_EXT_LEN(programOrigin.z);
00233   set_origin_msg.origin.a = a;
00234   set_origin_msg.origin.b = b;
00235   set_origin_msg.origin.c = c;
00236 
00237   interp_list.append(set_origin_msg);
00238 }

void SET_SPINDLE_SPEED (  double    rpm ) 
 

Definition at line 884 of file emccanon.cc.

00885 {
00886   // speed is in RPMs everywhere
00887   spindleSpeed = r;
00888 
00889   // check if we need to resend command
00890   if (spindleOn == 1)
00891     {
00892       START_SPINDLE_CLOCKWISE();
00893     }
00894   else if (spindleOn == -1)
00895     {
00896       START_SPINDLE_COUNTERCLOCKWISE();
00897     }
00898 }

void SET_TRAVERSE_RATE (  double    rate ) 
 

Definition at line 249 of file emccanon.cc.

00250 {
00251   double vmax;
00252 
00253 
00254   /* convert from /min to /sec */
00255   rate /= 60.0;
00256 
00257   /* convert to traj units (mm) if needed */
00258   currentLinearTraverseRate = FROM_PROG_LEN(rate);
00259   currentAngularTraverseRate = FROM_PROG_ANG(rate);
00260 
00261 
00262   /* make sure it's less than system max velocity */
00263   vmax  = FROM_EXT_LEN(TRAJ_MAX_VELOCITY);
00264   if (currentLinearTraverseRate > vmax) {
00265     currentLinearTraverseRate = vmax;
00266   }
00267 
00268 }

void SPINDLE_RETRACT (    ) 
 

Definition at line 909 of file emccanon.cc.

00910 {
00911   // FIXME-- unimplemented
00912 }

void SPINDLE_RETRACT_TRAVERSE (    ) 
 

Definition at line 854 of file emccanon.cc.

00855 {
00856   // FIXME-- unimplemented
00857 }

void START_CUTTER_RADIUS_COMPENSATION (  int    side ) 
 

Definition at line 433 of file emccanon.cc.

00434 {
00435   // nothing need be done here
00436 }

void START_SPEED_FEED_SYNCH (    ) 
 

Definition at line 443 of file emccanon.cc.

00444 {
00445   // FIXME-- unimplemented
00446 }

void START_SPINDLE_CLOCKWISE (    ) 
 

Definition at line 862 of file emccanon.cc.

00863 {
00864   EMC_SPINDLE_ON emc_spindle_on_msg;
00865 
00866   emc_spindle_on_msg.speed = spindleSpeed;
00867 
00868   interp_list.append(emc_spindle_on_msg);
00869 
00870   spindleOn = 1;
00871 }

void START_SPINDLE_COUNTERCLOCKWISE (    ) 
 

Definition at line 873 of file emccanon.cc.

00874 {
00875   EMC_SPINDLE_ON emc_spindle_on_msg;
00876 
00877   emc_spindle_on_msg.speed = - spindleSpeed;
00878 
00879   interp_list.append(emc_spindle_on_msg);
00880 
00881   spindleOn = -1;
00882 }

void STOP_CUTTER_RADIUS_COMPENSATION (    ) 
 

Definition at line 438 of file emccanon.cc.

00439 {
00440   // nothing need be done here
00441 }

void STOP_SPEED_FEED_SYNCH (    ) 
 

Definition at line 448 of file emccanon.cc.

00449 {
00450   // FIXME-- unimplemented
00451 }

void STOP_SPINDLE_TURNING (    ) 
 

Definition at line 900 of file emccanon.cc.

00901 {
00902   EMC_SPINDLE_OFF emc_spindle_off_msg;
00903 
00904   interp_list.append(emc_spindle_off_msg);
00905 
00906   spindleOn = 0;
00907 }

void STRAIGHT_FEED (  double    x,
double    y,
double    z,
double    a = 0,
double    b = 0,
double    c = 0
) 
 

Definition at line 683 of file emccanon.cc.

00684 {
00685   EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
00686   double dx, dy, dz, da, db, dc, dtot;
00687   double tx, ty, tz, ta, tb, tc, tmax;
00688   double vel;
00689 
00690   // convert to mm units
00691   x = FROM_PROG_LEN(x);
00692   y = FROM_PROG_LEN(y);
00693   z = FROM_PROG_LEN(z);  
00694   a = FROM_PROG_ANG(a);
00695   b = FROM_PROG_ANG(b);
00696   c = FROM_PROG_ANG(c);
00697 
00698   x += programOrigin.x;
00699   y += programOrigin.y;
00700   z += programOrigin.z; 
00701   a += programOrigin.a;
00702   b += programOrigin.b;
00703   c += programOrigin.c;
00704 
00705   z += currentToolLengthOffset;
00706 
00707   // now x, y, z, and b are in absolute mm or degree units
00708 
00709   linearMoveMsg.end.tran.x = TO_EXT_LEN(x);
00710   linearMoveMsg.end.tran.y = TO_EXT_LEN(y);
00711   linearMoveMsg.end.tran.z = TO_EXT_LEN(z);
00712 
00713   // fill in the orientation
00714   linearMoveMsg.end.a = TO_EXT_ANG(a);
00715   linearMoveMsg.end.b = TO_EXT_ANG(b);
00716   linearMoveMsg.end.c = TO_EXT_ANG(c);
00717 
00718   // compute maximal feed rate
00719 
00720   pure_angular_move = 0;
00721 
00722   dx = x - canonEndPoint.x;
00723   dy = y - canonEndPoint.y;
00724   dz = z - canonEndPoint.z;
00725   da = a - canonEndPoint.a;
00726   db = b - canonEndPoint.b;
00727   dc = c - canonEndPoint.c;
00728   dtot = sqrt(dx*dx + dy*dy + dz*dz);
00729   if (dtot <= 0.0) {
00730     dtot = sqrt(da*da + db*db + dc*dc);
00731     if (dtot > 0.0) {
00732       pure_angular_move  =1;
00733     }
00734   }
00735 
00736   if (!pure_angular_move) {
00737     tx = fabs(dx / FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]));
00738     ty = fabs(dy / FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]));
00739     tz = fabs(dz / FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]));
00740     tmax = tx > ty ? tx : ty;
00741     tmax = tz > tmax ? tz : tmax;
00742 
00743     if (tmax <= 0.0) {
00744       vel = currentLinearFeedRate;
00745     }
00746     else {
00747       vel = dtot/tmax;
00748       if (vel > currentLinearFeedRate) {
00749         vel = currentLinearFeedRate;
00750       }
00751     }
00752 
00753   }
00754   else {
00755     ta = fabs(da / FROM_EXT_ANG(AXIS_MAX_VELOCITY[3]));
00756     tb = fabs(db / FROM_EXT_ANG(AXIS_MAX_VELOCITY[4]));
00757     tc = fabs(dc / FROM_EXT_ANG(AXIS_MAX_VELOCITY[5]));
00758     tmax = ta > tb ? ta : tb;
00759     tmax = tc > tmax ? tc : tmax;
00760 
00761     if (tmax <= 0.0) {
00762       vel = currentAngularFeedRate;
00763     }
00764     else {
00765       vel = dtot/tmax;
00766       if (vel > currentAngularFeedRate) {
00767         vel = currentAngularFeedRate;
00768       }
00769     }
00770   }
00771 
00772   // ensure proper feed rate
00773   properVelocity(vel);
00774 
00775   interp_list.append(linearMoveMsg);
00776 
00777   canonUpdateEndPoint(x, y, z, a, b, c);
00778 }

void STRAIGHT_PROBE (  double    x,
double    y,
double    z,
double    a = 0,
double    b = 0,
double    c = 0
) 
 

Definition at line 784 of file emccanon.cc.

00785 {
00786   EMC_TRAJ_PROBE probeMsg;
00787   double dx, dy, dz, dtot;
00788   double tx, ty, tz, tmax;
00789   double vel;
00790 
00791   // convert to mm units
00792   x = FROM_PROG_LEN(x);
00793   y = FROM_PROG_LEN(y);
00794   z = FROM_PROG_LEN(z);
00795 
00796   x += programOrigin.x;
00797   y += programOrigin.y;
00798   z += programOrigin.z;
00799 
00800   z += currentToolLengthOffset;
00801 
00802   // now x, y, z, and b are in absolute mm or degree units
00803 
00804   probeMsg.pos.tran.x = TO_EXT_LEN(x);
00805   probeMsg.pos.tran.y = TO_EXT_LEN(y);
00806   probeMsg.pos.tran.z = TO_EXT_LEN(z);
00807 
00808   // compute maximal feed rate
00809 
00810   dx = x - canonEndPoint.x;
00811   dy = y - canonEndPoint.y;
00812   dz = z - canonEndPoint.z;
00813   dtot = sqrt(dx*dx + dy*dy + dz*dz);
00814 
00815   tx = fabs(dx / FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]));
00816   ty = fabs(dy / FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]));
00817   tz = fabs(dz / FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]));
00818   tmax = tx > ty ? tx : ty;
00819   tmax = tz > tmax ? tz : tmax;
00820 
00821   if (tmax <= 0.0) {
00822     vel = currentLinearFeedRate;
00823   }
00824   else {
00825     vel = dtot/tmax;
00826     if (vel > currentLinearFeedRate) {
00827       vel = currentLinearFeedRate;
00828     }
00829   }
00830 
00831   // ensure proper feed rate
00832   properVelocity(vel);
00833 
00834   // fill in the orientation
00835   probeMsg.pos.a = a;
00836   probeMsg.pos.b = b;
00837   probeMsg.pos.c = c;
00838 
00839   interp_list.append(probeMsg);
00840 
00841   canonUpdateEndPoint(x, y, z, a, b,c);
00842 }

void STRAIGHT_TRAVERSE (  double    x,
double    y,
double    z,
double    a = 0,
double    b = 0,
double    c = 0
) 
 

Definition at line 270 of file emccanon.cc.

00272 {
00273   EMC_TRAJ_LINEAR_MOVE linearMoveMsg;
00274   double dx, dy, dz, da, db, dc, dtot;
00275   double tx, ty, tz, ta, tb, tc, tmax;
00276   double vel;
00277 
00278   // convert to mm units
00279   x = FROM_PROG_LEN(x);
00280   y = FROM_PROG_LEN(y);
00281   z = FROM_PROG_LEN(z);
00282   a = FROM_PROG_ANG(a);
00283   b = FROM_PROG_ANG(b);
00284   c = FROM_PROG_ANG(c);
00285 
00286   x += programOrigin.x;
00287   y += programOrigin.y;
00288   z += programOrigin.z;
00289   a += programOrigin.a;
00290   b += programOrigin.b;
00291   c += programOrigin.c;
00292 
00293   z += currentToolLengthOffset;
00294 
00295   // now x, y, z, and b are in absolute mm or degree units
00296 
00297   linearMoveMsg.end.tran.x = TO_EXT_LEN(x);
00298   linearMoveMsg.end.tran.y = TO_EXT_LEN(y);
00299   linearMoveMsg.end.tran.z = TO_EXT_LEN(z);
00300 
00301   // fill in the orientation
00302   linearMoveMsg.end.a = TO_EXT_ANG(a);
00303   linearMoveMsg.end.b = TO_EXT_ANG(b);
00304   linearMoveMsg.end.c = TO_EXT_ANG(c);
00305 
00306   // compute maximal feed rate
00307 
00308   pure_angular_move = 0;
00309 
00310   dx = x - canonEndPoint.x;
00311   dy = y - canonEndPoint.y;
00312   dz = z - canonEndPoint.z;
00313   da = a - canonEndPoint.a;
00314   db = b - canonEndPoint.b;
00315   dc = c - canonEndPoint.c;
00316   dtot = sqrt(dx*dx + dy*dy + dz*dz);
00317   if (dtot <= 0.0) {
00318     dtot = sqrt(da*da + db*db + dc*dc);
00319     if (dtot > 0.0) {
00320       pure_angular_move  =1;
00321     }
00322   }
00323 
00324   if (! pure_angular_move) {
00325     tx = fabs(dx / FROM_EXT_LEN(AXIS_MAX_VELOCITY[0]));
00326     ty = fabs(dy / FROM_EXT_LEN(AXIS_MAX_VELOCITY[1]));
00327     tz = fabs(dz / FROM_EXT_LEN(AXIS_MAX_VELOCITY[2]));
00328     tmax = tx > ty ? tx : ty;
00329     tmax = tz > tmax ? tz : tmax;
00330 
00331     if (tmax <= 0.0) {
00332       vel = currentLinearTraverseRate;
00333     }
00334     else {
00335       vel = dtot/tmax;
00336       if (vel > currentLinearTraverseRate) {
00337         vel = currentLinearTraverseRate;
00338       }
00339     }
00340 
00341   }
00342   else {
00343     ta = fabs(da / FROM_EXT_ANG(AXIS_MAX_VELOCITY[3]));
00344     tb = fabs(db / FROM_EXT_ANG(AXIS_MAX_VELOCITY[4]));
00345     tc = fabs(dc / FROM_EXT_ANG(AXIS_MAX_VELOCITY[5]));
00346     tmax = ta > tb ? ta : tb;
00347     tmax = tc > tmax ? tc : tmax;
00348 
00349     if (tmax <= 0.0) {
00350       vel = currentAngularTraverseRate;
00351     }
00352     else {
00353       vel = dtot/tmax;
00354       if (vel > currentAngularTraverseRate) {
00355         vel = currentAngularTraverseRate;
00356       }
00357     }
00358 
00359   }
00360 
00361   // ensure proper feed rate
00362   properVelocity(vel);
00363 
00364   interp_list.append(linearMoveMsg);
00365 
00366   canonUpdateEndPoint(x, y, z, a, b, c);
00367 }

void TURN_PROBE_OFF (    ) 
 

Definition at line 1136 of file emccanon.cc.

01137 {
01138   // don't do anything-- this is called when the probing is done
01139 }

void TURN_PROBE_ON (    ) 
 

Definition at line 1141 of file emccanon.cc.

01142 {
01143   EMC_TRAJ_CLEAR_PROBE_TRIPPED_FLAG clearMsg;
01144 
01145   interp_list.append(clearMsg);
01146 }

void UNCLAMP_AXIS (  CANON_AXIS    axis ) 
 

Definition at line 1148 of file emccanon.cc.

01149 {
01150   // FIXME-- unimplemented
01151 }

void USE_LENGTH_UNITS (  CANON_UNITS    in_unit ) 
 

Definition at line 240 of file emccanon.cc.

00241 {
00242   lengthUnits = in_unit;
00243 
00244   emcStatus->task.programUnits = in_unit;
00245 }

void USE_NO_SPINDLE_FORCE (    ) 
 

Definition at line 919 of file emccanon.cc.

00920 {
00921   // FIXME-- unimplemented
00922 }

void USE_TOOL_LENGTH_OFFSET (  double    length ) 
 

Definition at line 930 of file emccanon.cc.

00931 {
00932   EMC_TRAJ_SET_OFFSET set_offset_msg;
00933 
00934   /* convert to mm units for internal canonical use */
00935   currentToolLengthOffset = FROM_PROG_LEN(length);
00936 
00937   /* append it to interp list so it gets updated at the right time,
00938      not at read-ahead time */
00939   set_offset_msg.offset.tran.x = 0.0;
00940   set_offset_msg.offset.tran.y = 0.0;
00941   set_offset_msg.offset.tran.z = TO_EXT_LEN(currentToolLengthOffset);
00942   set_offset_msg.offset.a = 0.0;
00943   set_offset_msg.offset.b = 0.0;
00944   set_offset_msg.offset.c = 0.0;
00945 
00946   interp_list.append(set_offset_msg);
00947 }

char __attribute__ (  (unused)    )  [static]
 

Definition at line 89 of file emccanon.cc.

00089                                                   : emccanon.cc,v 1.15 2001/07/14 17:32:54 paul_c Exp $";
00090 
00091 #include <stdio.h>
00092 #include <stdarg.h>
00093 #include <math.h>
00094 #include <string.h>             // strncpy()
00095 #include <ctype.h>              // isspace()
00096 #include "emc.hh"               // EMC NML
00097 #include "canon.hh"             // these decls
00098 #include "interpl.hh"           // interp_list
00099 #include "emcglb.h"             // TRAJ_MAX_VELOCITY
00100 #include "emcpos.h"
00101 
00102 static PM_QUATERNION quat(1, 0, 0, 0);
00103 
00104 /*
00105   Origin offsets, length units, and active plane are all maintained
00106   here in this file. Controller runs in absolute mode, and does not
00107   have plane select concept.
00108 
00109   programOrigin is stored in mm always, and converted when set or read.
00110   When it's applied to positions, convert positions to mm units first
00111   and then add programOrigin.
00112 
00113   Units are then converted from mm to external units, as reported by
00114   the GET_EXTERNAL_LENGTH_UNITS() function.
00115   */
00116 static CANON_POSITION programOrigin(0.0, 0.0, 0.0);
00117 static CANON_UNITS lengthUnits = CANON_UNITS_MM;
00118 static CANON_PLANE activePlane = CANON_PLANE_XY;
00119 
00120 /*
00121   canonEndPoint is the last programmed end point, stored in case it's
00122   needed for subsequent calculations. It's in absolute frame, mm units.
00123   */
00124 static CANON_POSITION canonEndPoint;
00125 static void canonUpdateEndPoint(double x, double y, double z, double a, double b, double c)
00126 {
00127   canonEndPoint.x = x;
00128   canonEndPoint.y = y;
00129   canonEndPoint.z = z;
00130   canonEndPoint.a = a; 
00131   canonEndPoint.b = b; 
00132   canonEndPoint.c = c; 
00133 }

char* addString (  char *    dst,
const char *    src,
int    maxlen
)  [static]
 

Definition at line 985 of file emccanon.cc.

Referenced by COMMENT(). 

00986 {
00987   int dstlen = strlen(dst);
00988   int srclen = strlen(src);
00989   int actlen;
00990 
00991   if (srclen >= maxlen - dstlen) {
00992     actlen = maxlen - dstlen - 1;
00993     dst[maxlen - 1] = 0;
00994   }
00995   else {
00996     actlen = srclen;
00997   }
00998 
00999   strncat(dst, src, actlen);
01000 
01001   return dst;
01002 }

int properVelocity (  double    vel )  [static]
 

Definition at line 193 of file emccanon.cc.

Referenced by ARC_FEED(), STRAIGHT_FEED(), STRAIGHT_PROBE(), and STRAIGHT_TRAVERSE(). 

00194 {
00195   EMC_TRAJ_SET_VELOCITY velMsg;
00196 
00197   if (! pure_angular_move) {
00198     velMsg.velocity = TO_EXT_LEN(vel);
00199   }
00200   else {
00201     velMsg.velocity = TO_EXT_ANG(vel);
00202   }      
00203 
00204   if (velMsg.velocity != lastVelSet) {
00205     lastVelSet = velMsg.velocity;
00206     interp_list.append(velMsg);
00207   }
00208 
00209   return 0;
00210 }

char* setString (  char *    dst,
const char *    src,
int    maxlen
)  [static]
 

Definition at line 977 of file emccanon.cc.

Referenced by COMMENT(). 

00978 {
00979   dst[0] = 0;
00980   strncat(dst, src, maxlen - 1);
00981   dst[maxlen - 1] = 0;
00982   return dst;
00983 }

Variable Documentation

CANON_MOTION_MODE canonMotionMode = 0 [static]
 

Definition at line 137 of file emccanon.cc.

double currentAngularFeedRate = 1.0 [static]
 

Definition at line 172 of file emccanon.cc.

double currentAngularTraverseRate = 1.0 [static]
 

Definition at line 173 of file emccanon.cc.

double currentLinearFeedRate = 1.0 [static]
 

Definition at line 170 of file emccanon.cc.

double currentLinearTraverseRate = 1.0 [static]
 

Definition at line 171 of file emccanon.cc.

double currentToolLengthOffset = 0.0 [static]
 

Definition at line 181 of file emccanon.cc.

double lastVelSet = -1 [static]
 

Definition at line 190 of file emccanon.cc.

int preppedTool = 0 [static]
 

Definition at line 163 of file emccanon.cc.

FILE* probefile = NULL [static]
 

Definition at line 1010 of file emccanon.cc.

int pure_angular_move = 0 [static]
 

Definition at line 175 of file emccanon.cc.

int spindleOn = 0 [static]
 

Definition at line 860 of file emccanon.cc.

double spindleSpeed = 0.0 [static]
 

Definition at line 158 of file emccanon.cc.

Generated on Sun Dec 2 15:27:53 2001 for EMC by doxygen1.2.11.1 written by Dimitri van Heesch, © 1997-2001