---

dcmotor.c

Go to the documentation of this file.

/*
   dcmotor.c

   DC motor simulation

   From Benjamin C. Kuo, "Automatic Control Systems," Fourth Edition,
   pp. 176-186.

   Units are SI for all quantities. Note that back EMF constant Kb is
   identically equal to Torque Constant in SI units, so Ki is omitted
   from interface and replaced with Kb in calculations.

   Modification history:

   26-Apr-2000 WPS added some code to try using a simpler simulation
   model if Jm or La are equal to zero. (At least it avoids a divide by
   zero.)
   12-Apr-2000 WPS removed redundant second include of inifile.h
   23-Sep-1999  WPS replaced stdio functions with inet_file functions
   which work under CE.
   23-Sep-1999 WPS initialized suspicious return retval from dcmotorGetParameters
   27-Jan-1999 WPS added "volatile"'s to avoid NT optimization problem.
   23-Mar-1998  FMP included <string.h>, as I should have
   17-Mar-1998  FMP added section arg to dcmotorIniLoad()
   9-Sep-1997  FMP added dcmotorResetState();
   17-Aug-1997  FMP took out etime() call in dcmotorRunCycle-- need to
   add a function to get real time-- etime() in rcs library does this,
   but we don't want to include all of that here
   2-May-1997  FMP fixed bug in dcmotorRunCycle() where I left deltaT
   unset if cycleTime was not <= 0.0. This bug, of course, cost me a week.
   25-Apr-1997  FMP added lastTime and etime() for real-time sim
   24-Apr-1997  FMP added dcmotorIniLoad()
   17-Apr-1997  FMP created from C part of original dcmotor.c
*/

#ifndef NO_STDIO_SUPPORT
#ifdef UNDER_CE
#include "rcs_ce.h"             /* RCS_CE_ATOF() */
#else
#include <stdio.h>
#endif
#include "inetfile.hh"          /* inet_file_open() */
#include "inifile.h"
#include "rcs_prnt.hh"          /* rcs_print() */
#include <string.h>
#endif
#include "dcmotor.h"            /* these decls */

/* ident tag */
#ifndef __GNUC__
#ifndef __attribute__
#define __attribute__(x)
#endif
#endif

static char __attribute__((unused)) ident[] = "$Id: dcmotor.c,v 1.3 2000/10/27 20:34:42 terrylr Exp $";

#define PARAMETERS_SET 0x01
#define CYCLE_TIME_SET 0x02
#define OFFSET_SET 0x04
#define REVS_PER_UNIT_SET 0x08
#define ALL_SET (PARAMETERS_SET | CYCLE_TIME_SET | OFFSET_SET | REVS_PER_UNIT_SET)

/* Conversion factors */
/* oz-in * 0.00708 = newton-meters */
/* foot-pounds * 1.359 = newton-meters */
#define OZ_IN_TO_N_M 0.00708
#define LB_FT_TO_N_M 1.359

int dcmotorInit(DC_MOTOR_STRUCT * dcmotor)
{
  if (0 == dcmotor)
  {
    return -1;
  }

  /* parameters */
  dcmotor->Ra = 0.0;
  dcmotor->La = 0.0;
  dcmotor->Kb = 0.0;
  dcmotor->Jm = 0.0;
  dcmotor->Bm = 0.0;

  /* internal vars */
  dcmotor->configured = 0;
  dcmotor->cycleTime = 0.0;
  dcmotor->offset = 0.0;
  dcmotor->revsPerUnit = 0.0;
  dcmotor->lastTime = -1.0;

  /* state vars */
  return dcmotorResetState(dcmotor);
}

int dcmotorResetState(DC_MOTOR_STRUCT * dcmotor)
{
  /* state vars */
  dcmotor->ia = 0.0;
  dcmotor->dia = 0.0;
  dcmotor->wm = 0.0;
  dcmotor->dwm = 0.0;
  dcmotor->thm = 0.0;
  dcmotor->dthm = 0.0;

  return 0;
}

int dcmotorSetCycleTime(DC_MOTOR_STRUCT * dcmotor, double seconds)
{
  if (0 == dcmotor)
  {
    return -1;
  }

  /* note that seconds <= 0.0 means use real time */
  dcmotor->cycleTime = seconds;
  dcmotor->configured |= CYCLE_TIME_SET;

  return 0;
}

double dcmotorGetCycleTime(DC_MOTOR_STRUCT * dcmotor)
{
  if (0 == dcmotor ||
      ! (dcmotor->configured & CYCLE_TIME_SET))
  {
    return 0.0;
  }

  return dcmotor->cycleTime;
}

int dcmotorSetParameters(DC_MOTOR_STRUCT * dcmotor, DC_MOTOR_STRUCT params)
{
  if (0 == dcmotor)
  {
    return -1;
  }

  dcmotor->Ra = params.Ra;
  dcmotor->La = params.La;
  dcmotor->Kb = params.Kb;
  dcmotor->Jm = params.Jm;
  dcmotor->Bm = params.Bm;

  dcmotor->configured |= PARAMETERS_SET;

  return 0;
}

DC_MOTOR_STRUCT dcmotorGetParameters(DC_MOTOR_STRUCT * dcmotor)
{
  DC_MOTOR_STRUCT retval;

  if (0 == dcmotor)
  {
     /* parameters */
    retval.Ra=0;                        /* armature resistance, ohms */
    retval.La=0;                        /* armature inductance, henries */
    retval.Kb=0;                        /* back emf constant, V/rad/s, N-m/amp */
    retval.Jm=0;                        /* motor rotor inertia, N-m-s^2 */
    retval.Bm=0;                        /* viscous friction coefficient, N-m/rad/sec */

  /* state vars */
    retval.ia=0;                        /* armature current */
    retval.dia=0;                       /* derivative of above */
    retval.wm=0;                        /* angular velocity of motor */
    retval.dwm=0;                       /* derivative of above */
    retval.thm=0;                       /* angular position of motor */
    retval.dthm=0;                      /* derivative of above */
    retval.lastTime=0;          /* timestamp of last cycle */

  /* internal vars */
    retval.configured=0;
    retval.cycleTime=0;         /* copy of arg to setCycleTime() */
    retval.offset=0;
    retval.revsPerUnit=0;
    return retval;
  }

  return *dcmotor;
}

int dcmotorSetOffset(DC_MOTOR_STRUCT * dcmotor, double _offset)
{
  if (0 == dcmotor)
  {
    return -1;
  }

  dcmotor->offset = _offset;
  dcmotor->configured |= OFFSET_SET;

  return 0;
}

double dcmotorGetOffset(DC_MOTOR_STRUCT * dcmotor)
{
  if (0 == dcmotor ||
      ! (dcmotor->configured & OFFSET_SET))
  {
    return 0.0;
  }

  return dcmotor->offset;
}

int dcmotorSetRevsPerUnit(DC_MOTOR_STRUCT * dcmotor, double _revsPerUnit)
{
  if (0 == dcmotor ||
      _revsPerUnit <= 0.0)
  {
    return -1;
  }

  dcmotor->revsPerUnit = _revsPerUnit;
  dcmotor->configured |= REVS_PER_UNIT_SET;

  return 0;
}

double dcmotorGetRevsPerUnit(DC_MOTOR_STRUCT * dcmotor)
{
  if (0 == dcmotor ||
      ! (dcmotor->configured & REVS_PER_UNIT_SET))
  {
    return 0.0;
  }

  return dcmotor->revsPerUnit;
}

double dcmotorGetOutput(DC_MOTOR_STRUCT * dcmotor)
{
  if (0 == dcmotor ||
      dcmotor->configured != ALL_SET)
  {
    return 0.0;
  }

  return dcmotor->thm + dcmotor->offset;
}

double dcmotorRunCycle(volatile DC_MOTOR_STRUCT * dcm, volatile double ea)
{
  volatile double deltaT;
  volatile double now;

  if (0 == dcm ||
      dcm->configured != ALL_SET)
    {
      return 0.0;
    }

  /* calculate deltaT */
  if (dcm->cycleTime <= 0.0)
    {
      /* if cycleTime <= 0.0, we want to use real time stamps */
      if (dcm->lastTime <= 0.0)
        {
          /* first time through */
          dcm->lastTime = 0.0;
          deltaT = 0.0;         /* results will all be zero */
        }
      else
        {
          /* FIXME-- get current time in 'now', instead of 0 */
          now = 0.0;
          deltaT = now - dcm->lastTime;
          dcm->lastTime = now;
        }
    }
  else
    {
      deltaT = dcm->cycleTime;
    }

  /* calculate differentials */

  if(dcm->La != 0.0)
    {
      dcm->dia = deltaT *
        (-dcm->Ra * dcm->ia -
         dcm->Kb * dcm->wm +
         ea) / dcm->La;
    }
  else if (dcm->Ra != 0.0)
    {
      dcm->dia = (ea - dcm->Kb * dcm->wm)/dcm->Ra - dcm->ia;
    }
  else
    {
      dcm->dia = -dcm->ia;
      if(dcm->Kb != 0.0)
        {
          dcm->wm = ea/dcm->Kb;
        }
      else
        {
          dcm->wm = 0.0;
        }
    }

  if(dcm->Jm != 0.0)
    {
      dcm->dwm = deltaT *
        (dcm->Kb * dcm->ia -
         dcm->Bm * dcm->wm) / dcm->Jm; /* Ki == Kb, Tl = 0 */
    }
  else if(dcm->Bm != 0.0)
    {
      dcm->dwm = (dcm->Kb * dcm->ia)/dcm->Bm - dcm->wm;
    }
  else
    {
      dcm->dwm = -dcm->wm;
      dcm->ia = 0.0;
    }

  dcm->dthm = deltaT * dcm->wm;

  /* update state vars with differentials */

  dcm->ia += dcm->dia;
  dcm->wm += dcm->dwm;
  dcm->thm += dcm->dthm;

  /* return motor position */

  return dcm->thm + dcm->offset;
}

int dcmotorIniLoad(DC_MOTOR_STRUCT * dcmotor, const char * filename, const char *section)
{
#ifndef NO_STDIO_SUPPORT

  DC_MOTOR_STRUCT dcm;
  double cycleTime;
  double offset;
  double revsPerUnit;
  int retval = 0;
  const char *inistring;
  enum {TORQUE_UNITS_INVALID = 0,
        N_M,
        OZ_IN,
        LB_FT
        } torque_units;

#ifndef UNDER_CE
  FILE *fp;

  if (NULL == (fp = fopen(filename, "r")))
  {
    rcs_print_error( "can't open ini file %s\n", filename);
    return -1;
  }
#else
  INET_FILE *fp;

  if (NULL == (fp = inet_file_open(filename, "r")))
  {
    rcs_print_error( "can't open ini file %s\n", filename);
    return -1;
  }
#endif

  /* look up torque units */
  if (NULL != (inistring = iniFind(fp, "TORQUE_UNITS", section)))
  {
    /* found the entry in the ini file */
    if (!strcmp(inistring, "N_M"))
    {
      torque_units = N_M;
    }
    else if (!strcmp(inistring, "LB_FT"))
    {
      torque_units = LB_FT;
    }
    else if (!strcmp(inistring, "OZ_IN"))
    {
      torque_units = OZ_IN;
    }
    else
    {
      torque_units = TORQUE_UNITS_INVALID;
      rcs_print_error( "bad torque units specified in ini file: %s\n",
              inistring);
      retval = -1;
    }
  }
  else
  {
    /* not in ini file-- use default */
    torque_units = N_M;
  }

  if (NULL != (inistring = iniFind(fp, "ARMATURE_RESISTANCE", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%lf", &dcm.Ra))
    {
      /* found, but invalid */
      rcs_print_error( "invalid ARMATURE_RESISTANCE: %s\n", inistring);
      retval = -1;
    }
#else
    dcm.Ra = RCS_CE_ATOF(inistring);
#endif
  }
  if (NULL != (inistring = iniFind(fp, "ARMATURE_INDUCTANCE", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%lf", &dcm.La))
    {
      /* found, but invalid */
      rcs_print_error( "invalid ARMATURE_INDUCTANCE: %s\n", inistring);
      retval = -1;
    }
#else
    dcm.La = RCS_CE_ATOF(inistring);
#endif
  }
  if (NULL != (inistring = iniFind(fp, "BACK_EMF_CONSTANT", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%lf", &dcm.Kb))
    {
      /* found, but invalid */
      rcs_print_error( "invalid BACK_EMF_CONSTANT: %s\n", inistring);
      retval = -1;
    }
#else
    dcm.Kb = RCS_CE_ATOF(inistring);
#endif
  }
  if (NULL != (inistring = iniFind(fp, "ROTOR_INERTIA", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%lf", &dcm.Jm))
    {
      /* found, but invalid */
      rcs_print_error( "invalid ROTOR_INERTIA: %s\n", inistring);
      retval = -1;
    }
#else
    dcm.Jm = RCS_CE_ATOF(inistring);
#endif
    switch (torque_units)
    {
    case OZ_IN:
      dcm.Jm *= OZ_IN_TO_N_M;
      break;
    case LB_FT:
      dcm.Jm *= LB_FT_TO_N_M;
      break;
    default:
      break;
    }
  }
  if (NULL != (inistring = iniFind(fp, "DAMPING_FRICTION_COEFFICIENT", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%lf", &dcm.Bm))
    {
      /* found, but invalid */
      rcs_print_error( "invalid DAMPING_FRICTION_COEFFICIENT: %s\n", inistring);
      retval = -1;
    }
#else
    dcm.Bm = RCS_CE_ATOF(inistring);
#endif
    switch (torque_units)
    {
    case OZ_IN:
      dcm.Bm *= OZ_IN_TO_N_M;
      break;
    case LB_FT:
      dcm.Bm *= LB_FT_TO_N_M;
      break;
    default:
      break;
    }
  }

  if (retval == 0)
  {
    dcmotorSetParameters(dcmotor, dcm);
  }

  if (NULL != (inistring = iniFind(fp, "CYCLE_TIME", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%lf", &cycleTime))
    {
      /* found, but invalid */
      rcs_print_error( "invalid CYCLE_TIME: %s\n", inistring);
      retval = -1;
    }
    else
    {
      dcmotorSetCycleTime(dcmotor, cycleTime);
    }
#else
    cycleTime = RCS_CE_ATOF(inistring);
    dcmotorSetCycleTime(dcmotor, cycleTime);
#endif
  }

  if (NULL != (inistring = iniFind(fp, "SHAFT_OFFSET", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%lf", &offset))
    {
      /* found, but invalid */
      rcs_print_error( "invalid OFFSET: %s\n", inistring);
      retval = -1;
    }
    else
    {
      dcmotorSetOffset(dcmotor, offset);
    }
#else
    offset = RCS_CE_ATOF(inistring);
    dcmotorSetOffset(dcmotor, offset);
#endif
  }

  if (NULL != (inistring = iniFind(fp, "REVS_PER_UNIT", section)))
  {
#ifndef UNDER_CE
    if (1 != sscanf((char *) inistring, "%lf", &revsPerUnit))
    {
      /* found, but invalid */
      rcs_print_error( "invalid REVS_PER_UNIT: %s\n", inistring);
      retval = -1;
    }
    else
    {
      dcmotorSetRevsPerUnit(dcmotor, revsPerUnit);
    }
#else
    revsPerUnit = RCS_CE_ATOF(inistring);
    dcmotorSetRevsPerUnit(dcmotor, revsPerUnit);
#endif
  }

  /* close inifile */
#ifndef UNDER_CE
  fclose(fp);
#else
  inet_file_close(fp);
#endif

  return retval;

#else

  return -1;

#endif /* not STDIO_SUPPORT */
}

---