ampfile.cpp

美国COPLEY驱动器,程序开发工具之一.

/************************************************************/
/*                                                          */
/*  Copley Motion Libraries                                 */
/*                                                          */
/*  Author: Stephen Glow                                    */
/*                                                          */
/*  Copyright (c) 2002-2005 Copley Controls Corp.           */
/*                          http://www.copleycontrols.com   */
/*                                                          */
/************************************************************/

/***************************************************************************/
/** \file
  This file contains code used to read a CME-2 .ccx amplifier file.
  */
/***************************************************************************/

#include "CML.h"

#ifdef CML_FILE_ACCESS_OK
#include <stdio.h>
#include <ctype.h>
#include <stdlib.h>
#include <string.h>
#endif


CML_NAMESPACE_USE();

CML_NEW_ERROR( AmpFileError, format,        "Amplifier file format error" );
CML_NEW_ERROR( AmpFileError, tooOld,        "Amplifier file format is too old, use CME version 3.1 or later" );
CML_NEW_ERROR( AmpFileError, noFileAccess,  "File access was not enabled at compile time.  See CML_Settings.h" );
CML_NEW_ERROR( AmpFileError, fileOpen,      "Error opening amplifier file" );
CML_NEW_ERROR( AmpFileError, range,         "A parameter in the amplifier file is out of range" );
CML_NEW_ERROR( AmpFileError, axis,          "Amplifier file is for multi axis, not supported" );

// Local functions
#ifdef CML_FILE_ACCESS_OK
static const Error *ReadLine( FILE *fp, char *buff, int max );
static int SplitLine( char *buff, char **seg, int max, char delim=',' );
static const Error *StrToInt32( char *str, int32 &i, int base=0 );
static const Error *StrToUInt32( char *str, uint32 &i, int base=0 );
static const Error *StrToInt16( char *str, int16 &i, int base=0 );
static const Error *StrToUInt16( char *str, uint16 &i, int base=0 );
static const Error *StrToOutCfg( char *str, OUTPUT_PIN_CONFIG &cfg, uint32 &mask1, uint32 &mask2 );
static const Error *StrToFilter( char *str, Filter &flt );
static COPLEY_HOME_METHOD HomeMethodConvert( uint16 x );
#endif

// Handy macros
#define StrToLoadPos( str, pos )  { int32 i32; err=StrToInt32( str, i32 ); pos = PosLoad2User(i32); }
#define StrToLoadVel( str, vel )  { int32 i32; err=StrToInt32( str, i32 ); vel = VelLoad2User(i32); }
#define StrToLoadAcc( str, acc )  { int32 i32; err=StrToInt32( str, i32 ); acc = AccLoad2User(i32); }
#define StrToLoadJrk( str, jrk )  { int32 i32; err=StrToInt32( str, i32 ); jrk = JrkLoad2User(i32); }
#define StrToMtrPos( str, pos )  { int32 i32; err=StrToInt32( str, i32 ); pos = PosMtr2User(i32); }
#define StrToMtrVel( str, vel )  { int32 i32; err=StrToInt32( str, i32 ); vel = VelMtr2User(i32); }
#define StrToMtrAcc( str, acc )  { int32 i32; err=StrToInt32( str, i32 ); acc = AccMtr2User(i32); }

/***************************************************************************/
/**
  Load the specified amplifier data file.  This function presently supports
  loading *.ccx files created by the CME-2 program, version 3.1 and later.
  @param name The name (and optionally path) of the file to load
  @param line If not NULL, the last line number read from the file is returned
  here.  This is useful for finding file format errors.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::LoadFromFile( char *name, int &line )
{
#define MAX_LINE_LEN  200
#define MAX_LINE_SEGS 4

   line = 0;
#ifndef CML_FILE_ACCESS_OK
   return &AmpFileError::noFileAccess;
#else
   AmpConfig cfg;

   // Load the configuration structure with the current amplifier
   // configuration data.  This ensures that any parameters not 
   // specified in the file will remain unchanged.
   const Error *err = GetAmpConfig( cfg );
   if( err ) return err;

   // Open the file and read each parameter into my configuration
   // structure.
   FILE *fp;

   fp = fopen( name, "rt" );
   if( !fp )
      return &AmpFileError::fileOpen;

   char buff[MAX_LINE_LEN];
   char *seg[MAX_LINE_SEGS];
   int ct;
   int numOfSegs = 3;
   int segIndex = 2;

   int16 fileRev, mtrFamily;
   int16 i16;
   uint16 u16;
   int32 i32;

   // Read file version number
   line++;
   ReadLine( fp, buff, MAX_LINE_LEN );
   SplitLine( buff, seg, MAX_LINE_SEGS );
   err = StrToInt16( seg[0], fileRev, 10 );
   if( err ) return err;

   if( fileRev < 9 ) 
      return &AmpFileError::tooOld;

   line++;
   ReadLine( fp, buff, MAX_LINE_LEN );
   SplitLine( buff, seg, MAX_LINE_SEGS );

   if(fileRev < 12){
      // Read the motor family info
      err = StrToInt16( seg[0], mtrFamily, 10 );
      if( err ) return err;
   }

   if(fileRev == 12){
      // Read the motor family info
      SplitLine( seg[2], seg, 4, ':' );
      err = StrToInt16( seg[0], mtrFamily, 16 );
      if( err ) return err;
   }

   if (fileRev >= 13)
   {
      numOfSegs = 4;
      segIndex = 3;

      // read the number of axis
      err = StrToInt16( seg[0], i16, 10 );
      if( err ) return err;

      // only capable of 1 axis
      if( i16 > 1 ) 
	 return &AmpFileError::axis;

      // Read the motor family info
      line++;
      ReadLine( fp, buff, MAX_LINE_LEN );
      SplitLine( buff, seg, MAX_LINE_SEGS );
      SplitLine( seg[3], seg, 4, ':' );
      err = StrToInt16( seg[0], mtrFamily, 16 );
      if( err ) return err;
   }

   if( mtrFamily < 0 || mtrFamily > 2 ) 
      return &AmpFileError::format;

   cfg.CME_Config[4] = (char)(mtrFamily>>8);
   cfg.CME_Config[5] = (char)(mtrFamily);

   // Read all parameters
   while( !feof(fp) && !err )
   {
      int16 param;

      line++;
      ReadLine( fp, buff, MAX_LINE_LEN );
      ct = SplitLine( buff, seg, MAX_LINE_SEGS );
      if( ct == 0 )
	 continue;

      if( ct != numOfSegs )
	 err = &AmpFileError::format;
      else
	 err = StrToInt16( seg[0], param, 16 );

      if( err ) break;

      switch( param )
      {
	 case 0x000: err = StrToInt16( seg[segIndex], cfg.cLoop.kp ); break;
	 case 0x001: err = StrToInt16( seg[segIndex], cfg.cLoop.ki ); break;
	 case 0x002: err = StrToInt16( seg[segIndex], cfg.progCrnt ); break;

	 case 0x019: err = StrToInt32( seg[segIndex], cfg.ref.scale  ); break;
	 case 0x01a: err = StrToInt16( seg[segIndex], cfg.ref.offset ); break;

	 case 0x021: err = StrToInt16( seg[segIndex], cfg.cLoop.peakLim ); break;
	 case 0x022: err = StrToInt16( seg[segIndex], cfg.cLoop.contLim ); break;
	 case 0x023: err = StrToInt16( seg[segIndex], cfg.cLoop.peakTime ); break;

	 case 0x024:
	    err = StrToInt16( seg[segIndex], i16 );
	    cfg.controlMode = (AMP_MODE)(i16<<8);

	    // The ccx file only holds the amplifier control method.
	    // Build a proper amp mode out of this if it's one of the
	    // CANopen control methods.
	    if( (cfg.controlMode==AMPMODE_CAN_SERVO) || 
		  (cfg.controlMode==AMPMODE_CAN_USTEP) )
	       cfg.controlMode = (AMP_MODE)(cfg.controlMode | AMPMODE_CAN_PROFILE);
	    break;

	 case 0x026: err = StrToInt16( seg[segIndex], cfg.ref.deadband ); break;
	 case 0x027: err = StrToInt16( seg[segIndex], cfg.vLoop.kp ); break;
	 case 0x028: err = StrToInt16( seg[segIndex], cfg.vLoop.ki ); break;
	 case 0x02e: err = StrToInt16( seg[segIndex], cfg.vLoop.kaff ); break;
	 case 0x02f: StrToMtrVel( seg[segIndex], cfg.progVel ); break;
	 case 0x030: err = StrToInt16( seg[segIndex], cfg.pLoop.kp    ); break;
	 case 0x031: err = StrToInt16( seg[segIndex], cfg.vLoop.shift ); break;
	 case 0x033: err = StrToInt16( seg[segIndex], cfg.pLoop.kvff  ); break;
	 case 0x034: err = StrToInt16( seg[segIndex], cfg.pLoop.kaff  ); break;

	 // These three parameters use non-standard units in the amplifier
	 // thus the multiplication after converting them to user units.
	 case 0x036:
	    StrToMtrAcc( seg[segIndex], cfg.vLoop.maxAcc ); 
	    cfg.vLoop.maxAcc *= 100;
	    break;

	 case 0x037: 
	    StrToMtrAcc( seg[segIndex], cfg.vLoop.maxDec ); 
	    cfg.vLoop.maxDec *= 100;
	    break;

	 case 0x039: 
	    StrToMtrAcc( seg[segIndex], cfg.vLoop.estopDec ); 
	    cfg.vLoop.estopDec *= 100;
	    break;

	 case 0x03a: StrToMtrVel( seg[segIndex], cfg.vLoop.maxVel ); break;
	 case 0x03e: StrToMtrVel( seg[segIndex], cfg.window.velWarnWin ); break;
	 case 0x03f: err = StrToUInt16( seg[segIndex], cfg.window.velWarnTime ); break;

	 case 0x040: err = StrToUInt16( seg[segIndex], cfg.motor.type ); break;
	 case 0x041: strncpy( cfg.motor.mfgName, seg[segIndex], COPLEY_MAX_STRING ); break;
	 case 0x042: strncpy( cfg.motor.model,   seg[segIndex], COPLEY_MAX_STRING ); break;
	 case 0x043:
	    if( !strcmp( seg[segIndex], "Metric"  ) ) 
	       cfg.motor.mtrUnits = 0;
	    else if( !strcmp( seg[segIndex], "English"  ) ) 
	       cfg.motor.mtrUnits = 1;
	    else
	       err = StrToInt16( seg[segIndex], cfg.motor.mtrUnits  );
	    break;

	 case 0x044: 
	    err = StrToUInt32( seg[segIndex], cfg.motor.inertia ); 
	    break;

	 case 0x045: 
	    err = StrToInt16( seg[segIndex], cfg.motor.poles ); 
	    break;

	 case 0x046:
	    if( !strcmp( seg[segIndex], "No" ) ) 
	       cfg.motor.hasBrake = false;

	    else if( !strcmp( seg[segIndex], "Yes" ) ) 
	       cfg.motor.hasBrake = true;

	    else
	    {
	       err = StrToInt16( seg[segIndex], i16 );
	       cfg.motor.hasBrake = (i16==0);
	    }
	    break;

	 case 0x048: err = StrToUInt32( seg[segIndex], cfg.motor.trqConst ); break;
	 case 0x049: err = StrToUInt16( seg[segIndex], cfg.motor.resistance ); break;
	 case 0x04a: err = StrToUInt16( seg[segIndex], cfg.motor.inductance ); break; 
	 case 0x04b: err = StrToUInt32( seg[segIndex], cfg.motor.trqPeak ); break;
	 case 0x04c: err = StrToUInt32( seg[segIndex], cfg.motor.trqCont ); break;

	 case 0x04d:
	    StrToMtrVel( seg[segIndex], cfg.motor.velMax );
	    break;

	 case 0x04e: err = StrToInt16( seg[segIndex], i16 ); cfg.motor.mtrReverse = (i16!=0); break; 
	 case 0x04f: err = StrToInt16( seg[segIndex], cfg.motor.hallOffset ); break;

	 case 0x050: 
	    if( !strcmp( seg[segIndex], "None"  ) ) cfg.motor.hallType = 0;
	    else if( !strcmp( seg[segIndex], "Digital"  ) ) cfg.motor.hallType = 1;
	    else if( !strcmp( seg[segIndex], "Analog"   ) ) cfg.motor.hallType = 2;
	    else
	       err = StrToInt16( seg[segIndex], cfg.motor.hallType   );
	    break;

	 case 0x052: 
	    err = StrToInt16( seg[segIndex], cfg.motor.hallWiring ); 
	    break;

	 case 0x053: 
	    err = StrToInt16( seg[segIndex], cfg.motor.stopTime ); 
	    break;

	 case 0x054: 
	    err = StrToInt16( seg[segIndex], cfg.motor.brakeDelay ); 
	    break;

	 case 0x055: 
	    StrToMtrVel( seg[segIndex], cfg.motor.brakeVel );
	    break;

	 case 0x056: 
	    err = StrToUInt32( seg[segIndex], cfg.motor.backEMF ); 
	    break;

	 case 0x057:
	    err = StrToInt32( seg[segIndex], cfg.motor.stepsPerRev );
	    break;

	 case 0x058:
	    err = StrToInt32( seg[segIndex], cfg.motor.gearRatio );
	    break;

	 case 0x059:
	    err = StrToInt16( seg[segIndex], cfg.motor.hallVelShift );
	    break;

	 case 0x5A:
	    err = StrToUInt16( seg[segIndex], cfg.encoderOutCfg );
	    break;