1  /********************************************************************
  2  * Description: iniaxis.cc
  3  *   INI file initialization routines for axis NML
  4  *
  5  *   Derived from a work by Fred Proctor & Will Shackleford
  6  *
  7  * Author:
  8  * License: GPL Version 2
  9  * System: Linux
 10  *    
 11  * Copyright (c) 2004 All rights reserved.
 12  *
 13  * Last change:
 14  ********************************************************************/
 15  
 16  #include <unistd.h>
 17  #include <stdio.h>		// NULL
 18  #include <stdlib.h>		// atol(), _itoa()
 19  #include <string.h>		// strcmp()
 20  #include <ctype.h>		// isdigit()
 21  #include <sys/types.h>
 22  #include <sys/stat.h>
 23  
 24  #include "emc.hh"
 25  #include "rcs_print.hh"
 26  #include "emcIniFile.hh"
 27  #include "iniaxis.hh"		// these decls
 28  #include "emcglb.h"		// EMC_DEBUG
 29  #include "emccfg.h"		// default values for globals
 30  
 31  #include "inihal.hh"
 32  
 33  extern value_inihal_data old_inihal_data;
 34  double ext_offset_a_or_v_ratio[EMCMOT_MAX_AXIS]; // all zero
 35  
 36  // default ratio or external offset velocity,acceleration
 37  #define DEFAULT_A_OR_V_RATIO 0
 38  
 39  /*
 40    loadAxis(int axis)
 41  
 42    Loads ini file params for axis, axis = X, Y, Z, A, B, C, U, V, W 
 43  
 44    TYPE <LINEAR ANGULAR>        type of axis (hardcoded: X,Y,Z,U,V,W: LINEAR, A,B,C: ANGULAR)
 45    MAX_VELOCITY <float>         max vel for axis
 46    MAX_ACCELERATION <float>     max accel for axis
 47    MIN_LIMIT <float>            minimum soft position limit
 48    MAX_LIMIT <float>            maximum soft position limit
 49  
 50    calls:
 51  
 52    emcAxisSetMinPositionLimit(int axis, double limit);
 53    emcAxisSetMaxPositionLimit(int axis, double limit);
 54    emcAxisSetMaxVelocity(int axis, double vel, double ext_offset_vel);
 55    emcAxisSetMaxAcceleration(int axis, double acc, double ext_offset_acc);
 56    */
 57  
 58  static int loadAxis(int axis, EmcIniFile *axisIniFile)
 59  {
 60      char axisString[16];
 61      double limit;
 62      double maxVelocity;
 63      double maxAcceleration;
 64      int    lockingjnum = -1; // -1 ==> locking joint not used
 65  
 66      // compose string to match, axis = 0 -> AXIS_X etc.
 67      switch (axis) {
 68  	case 0: sprintf(axisString, "AXIS_X"); break;
 69  	case 1: sprintf(axisString, "AXIS_Y"); break;
 70  	case 2: sprintf(axisString, "AXIS_Z"); break;
 71  	case 3: sprintf(axisString, "AXIS_A"); break;
 72  	case 4: sprintf(axisString, "AXIS_B"); break;
 73  	case 5: sprintf(axisString, "AXIS_C"); break;
 74  	case 6: sprintf(axisString, "AXIS_U"); break;
 75  	case 7: sprintf(axisString, "AXIS_V"); break;
 76  	case 8: sprintf(axisString, "AXIS_W"); break;
 77      }
 78  
 79      axisIniFile->EnableExceptions(EmcIniFile::ERR_CONVERSION);
 80      
 81      try {
 82          // set min position limit
 83          limit = -1e99;	                // default
 84          axisIniFile->Find(&limit, "MIN_LIMIT", axisString);
 85          if (0 != emcAxisSetMinPositionLimit(axis, limit)) {
 86              if (emc_debug & EMC_DEBUG_CONFIG) {
 87                  rcs_print_error("bad return from emcAxisSetMinPositionLimit\n");
 88              }
 89              return -1;
 90          }
 91          old_inihal_data.axis_min_limit[axis] = limit;
 92  
 93          // set max position limit
 94          limit = 1e99;	                // default
 95          axisIniFile->Find(&limit, "MAX_LIMIT", axisString);
 96          if (0 != emcAxisSetMaxPositionLimit(axis, limit)) {
 97              if (emc_debug & EMC_DEBUG_CONFIG) {
 98                  rcs_print_error("bad return from emcAxisSetMaxPositionLimit\n");
 99              }
100              return -1;
101          }
102          old_inihal_data.axis_max_limit[axis] = limit;
103  
104          ext_offset_a_or_v_ratio[axis] = DEFAULT_A_OR_V_RATIO;
105          axisIniFile->Find(&ext_offset_a_or_v_ratio[axis], "OFFSET_AV_RATIO", axisString);
106  
107  #define REPLACE_AV_RATIO 0.1
108  #define MAX_AV_RATIO     0.9
109          if (   (ext_offset_a_or_v_ratio[axis] < 0)
110              || (ext_offset_a_or_v_ratio[axis] > MAX_AV_RATIO)
111             ) {
112             rcs_print_error("\n   Invalid:[%s]OFFSET_AV_RATIO= %8.5f\n"
113                               "   Using:  [%s]OFFSET_AV_RATIO= %8.5f\n",
114                             axisString,ext_offset_a_or_v_ratio[axis],
115                             axisString,REPLACE_AV_RATIO);
116             ext_offset_a_or_v_ratio[axis] = REPLACE_AV_RATIO;
117          }
118  
119          // set maximum velocities for axis: vel,ext_offset_vel
120          maxVelocity = DEFAULT_AXIS_MAX_VELOCITY;
121          axisIniFile->Find(&maxVelocity, "MAX_VELOCITY", axisString);
122          if (0 != emcAxisSetMaxVelocity(axis,
123                     (1 - ext_offset_a_or_v_ratio[axis]) * maxVelocity,
124                     (    ext_offset_a_or_v_ratio[axis]) * maxVelocity)) {
125              if (emc_debug & EMC_DEBUG_CONFIG) {
126                  rcs_print_error("bad return from emcAxisSetMaxVelocity\n");
127              }
128              return -1;
129          }
130          old_inihal_data.axis_max_velocity[axis] = maxVelocity;
131  
132          // set maximum accels for axis: acc,ext_offset_acc
133          maxAcceleration = DEFAULT_AXIS_MAX_ACCELERATION;
134          axisIniFile->Find(&maxAcceleration, "MAX_ACCELERATION", axisString);
135          if (0 != emcAxisSetMaxAcceleration(axis,
136                      (1 - ext_offset_a_or_v_ratio[axis]) * maxAcceleration,
137                      (    ext_offset_a_or_v_ratio[axis]) * maxAcceleration)) {
138              if (emc_debug & EMC_DEBUG_CONFIG) {
139                  rcs_print_error("bad return from emcAxisSetMaxAcceleration\n");
140              }
141              return -1;
142          }
143          old_inihal_data.axis_max_acceleration[axis] = maxAcceleration;
144  
145          axisIniFile->Find(&lockingjnum, "LOCKING_INDEXER_JOINT", axisString);
146          if (0 != emcAxisSetLockingJoint(axis, lockingjnum)) {
147              if (emc_debug & EMC_DEBUG_CONFIG) {
148                  rcs_print_error("bad return from emcAxisSetLockingJoint\n");
149              }
150              return -1;
151          }
152      }
153  
154  
155      catch(EmcIniFile::Exception &e){
156          e.Print();
157          return -1;
158      }
159  
160      return 0;
161  }
162  
163  /*
164    iniAxis(int axis, const char *filename)
165  
166    Loads ini file parameters for specified axis, [0 .. AXES - 1]
167  
168   */
169  int iniAxis(int axis, const char *filename)
170  {
171      EmcIniFile axisIniFile(EmcIniFile::ERR_TAG_NOT_FOUND |
172                             EmcIniFile::ERR_SECTION_NOT_FOUND |
173                             EmcIniFile::ERR_CONVERSION);
174  
175      if (axisIniFile.Open(filename) == false) {
176  	return -1;
177      }
178  
179      // load its values
180      if (0 != loadAxis(axis, &axisIniFile)) {
181          return -1;
182      }
183      return 0;
184  }