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

dcmotor.c File Reference

#include <stdio.h>
#include "inetfile.hh"
#include "inifile.h"
#include "rcs_prnt.hh"
#include <string.h>
#include "dcmotor.h"

Include dependency graph for dcmotor.c:

Include dependency graph

Go to the source code of this file.

Defines

#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)
#define OZ_IN_TO_N_M   0.00708
#define LB_FT_TO_N_M   1.359

Functions

char __attribute__ ((unused)) ident[]="$Id
int dcmotorResetState (DC_MOTOR_STRUCT *dcmotor)
int dcmotorSetCycleTime (DC_MOTOR_STRUCT *dcmotor, double seconds)
double dcmotorGetCycleTime (DC_MOTOR_STRUCT *dcmotor)
int dcmotorSetParameters (DC_MOTOR_STRUCT *dcmotor, DC_MOTOR_STRUCT params)
DC_MOTOR_STRUCT dcmotorGetParameters (DC_MOTOR_STRUCT *dcmotor)
int dcmotorSetOffset (DC_MOTOR_STRUCT *dcmotor, double _offset)
double dcmotorGetOffset (DC_MOTOR_STRUCT *dcmotor)
int dcmotorSetRevsPerUnit (DC_MOTOR_STRUCT *dcmotor, double _revsPerUnit)
double dcmotorGetRevsPerUnit (DC_MOTOR_STRUCT *dcmotor)
double dcmotorGetOutput (DC_MOTOR_STRUCT *dcmotor)
double dcmotorRunCycle (volatile DC_MOTOR_STRUCT *dcm, volatile double ea)
int dcmotorIniLoad (DC_MOTOR_STRUCT *dcmotor, const char *filename, const char *section)

Define Documentation

#define ALL_SET   (PARAMETERS_SET | CYCLE_TIME_SET | OFFSET_SET | REVS_PER_UNIT_SET)
 

#define CYCLE_TIME_SET   0x02
 

#define LB_FT_TO_N_M   1.359
 

#define OFFSET_SET   0x04
 

#define OZ_IN_TO_N_M   0.00708
 

#define PARAMETERS_SET   0x01
 

#define REVS_PER_UNIT_SET   0x08
 

Function Documentation

char __attribute__ (  (unused)    )  [static]
 

Definition at line 56 of file dcmotor.c.

00056                                                   : dcmotor.c,v 1.3 2000/10/27 20:34:42 terrylr Exp $";
00057 
00058 #define PARAMETERS_SET 0x01
00059 #define CYCLE_TIME_SET 0x02
00060 #define OFFSET_SET 0x04
00061 #define REVS_PER_UNIT_SET 0x08
00062 #define ALL_SET (PARAMETERS_SET | CYCLE_TIME_SET | OFFSET_SET | REVS_PER_UNIT_SET)
00063 
00064 /* Conversion factors */
00065 /* oz-in * 0.00708 = newton-meters */
00066 /* foot-pounds * 1.359 = newton-meters */
00067 #define OZ_IN_TO_N_M 0.00708
00068 #define LB_FT_TO_N_M 1.359
00069 
00070 int dcmotorInit(DC_MOTOR_STRUCT * dcmotor)
00071 {
00072   if (0 == dcmotor)
00073   {
00074     return -1;
00075   }
00076 
00077   /* parameters */
00078   dcmotor->Ra = 0.0;
00079   dcmotor->La = 0.0;
00080   dcmotor->Kb = 0.0;
00081   dcmotor->Jm = 0.0;
00082   dcmotor->Bm = 0.0;
00083 
00084   /* internal vars */
00085   dcmotor->configured = 0;
00086   dcmotor->cycleTime = 0.0;
00087   dcmotor->offset = 0.0;
00088   dcmotor->revsPerUnit = 0.0;
00089   dcmotor->lastTime = -1.0;
00090 
00091   /* state vars */
00092   return dcmotorResetState(dcmotor);
00093 }

double dcmotorGetCycleTime (  DC_MOTOR_STRUCT *    dcmotor ) 
 

Definition at line 122 of file dcmotor.c.

00123 {
00124   if (0 == dcmotor ||
00125       ! (dcmotor->configured & CYCLE_TIME_SET))
00126   {
00127     return 0.0;
00128   }
00129 
00130   return dcmotor->cycleTime;
00131 }

double dcmotorGetOffset (  DC_MOTOR_STRUCT *    dcmotor ) 
 

Definition at line 197 of file dcmotor.c.

00198 {
00199   if (0 == dcmotor ||
00200       ! (dcmotor->configured & OFFSET_SET))
00201   {
00202     return 0.0;
00203   }
00204 
00205   return dcmotor->offset;
00206 }

double dcmotorGetOutput (  DC_MOTOR_STRUCT *    dcmotor ) 
 

Definition at line 233 of file dcmotor.c.

00234 {
00235   if (0 == dcmotor ||
00236       dcmotor->configured != ALL_SET)
00237   {
00238     return 0.0;
00239   }
00240 
00241   return dcmotor->thm + dcmotor->offset;
00242 }

DC_MOTOR_STRUCT dcmotorGetParameters (  DC_MOTOR_STRUCT *    dcmotor ) 
 

Definition at line 151 of file dcmotor.c.

00152 {
00153   DC_MOTOR_STRUCT retval;
00154 
00155   if (0 == dcmotor)
00156   {
00157      /* parameters */
00158     retval.Ra=0;                        /* armature resistance, ohms */
00159     retval.La=0;                        /* armature inductance, henries */
00160     retval.Kb=0;                        /* back emf constant, V/rad/s, N-m/amp */
00161     retval.Jm=0;                        /* motor rotor inertia, N-m-s^2 */
00162     retval.Bm=0;                        /* viscous friction coefficient, N-m/rad/sec */
00163 
00164   /* state vars */
00165     retval.ia=0;                        /* armature current */
00166     retval.dia=0;                       /* derivative of above */
00167     retval.wm=0;                        /* angular velocity of motor */
00168     retval.dwm=0;                       /* derivative of above */
00169     retval.thm=0;                       /* angular position of motor */
00170     retval.dthm=0;                      /* derivative of above */
00171     retval.lastTime=0;          /* timestamp of last cycle */
00172 
00173   /* internal vars */
00174     retval.configured=0;
00175     retval.cycleTime=0;         /* copy of arg to setCycleTime() */
00176     retval.offset=0;
00177     retval.revsPerUnit=0;
00178     return retval;
00179   }
00180 
00181   return *dcmotor;
00182 }

double dcmotorGetRevsPerUnit (  DC_MOTOR_STRUCT *    dcmotor ) 
 

Definition at line 222 of file dcmotor.c.

00223 {
00224   if (0 == dcmotor ||
00225       ! (dcmotor->configured & REVS_PER_UNIT_SET))
00226   {
00227     return 0.0;
00228   }
00229 
00230   return dcmotor->revsPerUnit;
00231 }

int dcmotorIniLoad (  DC_MOTOR_STRUCT *    dcmotor,
const char *    filename,
const char *    section
) 
 

The angular velocity(w) is controlled by the equation.

(14) w = A*e^(sa_real*t)*cos(sa_img*t)+ B*e^(sa_real*t)*sin(sa_img*t)+ea/c

sa_real, sa_img and c were calculated in dcmotorSetParameters() (e is ofcourse 2.7182818 ... )

A and B still need to be calculated from the initial conditions.

At time t = 0, e^(sa_real)*t = 1, cos(sa_img*t)=1, sin(sa_img*t)=0, and w = wo, so

(15) wo = A*(1)*(1) + B*(1)*(0) + ea/c

The derivative of both sides of (14)

(16) w' = A*sa_real*e^(sa_real*t)*cos(sa_real*t)+ -A*sa_img*e^(sa_real*t)*sin(sa_real*t)+ B*sa_real*e^(sa_real*t)*sin(sa_real*t)+ A*sa_img*e^(sa_real*t)*cos(sa_real*t)

At time t = 0, e^(sa_real)*t = 1, cos(sa_img*t)=1, sin(sa_img*t)=0, and w' = dwo, so

(17) dwo = A*sa_real*(1)*(1) + -A*sa_real*(1)*(0) + B*sa_real*(1)*(0) + B*sa_img*(1)*(1)

Once A and B are calculated from (15) and (17) it is straight forward to calculate the current values of w and w' by substiting t=cycleTime in to equations (14) and (16).

e^(sa_real*cycleTime)*cos(sa_img*cycleTime) and e^(sa_real*cycleTime)*sin(sa_img*cycleTime) were previously calculated in dcmotorSetParameters and stored in sa_time_factor and sb_time_factor.

To get the change in position or delta_thm, integrate (14) from 0 to time t=cycleTime.

Definition at line 333 of file dcmotor.c.

00334 {
00335 #ifndef NO_STDIO_SUPPORT
00336 
00337   DC_MOTOR_STRUCT dcm;
00338   double cycleTime;
00339   double offset;
00340   double revsPerUnit;
00341   int retval = 0;
00342   const char *inistring;
00343   enum {TORQUE_UNITS_INVALID = 0,
00344         N_M,
00345         OZ_IN,
00346         LB_FT
00347         } torque_units;
00348 
00349 #ifndef UNDER_CE
00350   FILE *fp;
00351 
00352   if (NULL == (fp = fopen(filename, "r")))
00353   {
00354     rcs_print_error( "can't open ini file %s\n", filename);
00355     return -1;
00356   }
00357 #else
00358   INET_FILE *fp;
00359 
00360   if (NULL == (fp = inet_file_open(filename, "r")))
00361   {
00362     rcs_print_error( "can't open ini file %s\n", filename);
00363     return -1;
00364   }
00365 #endif
00366 
00367   /* look up torque units */
00368   if (NULL != (inistring = iniFind(fp, "TORQUE_UNITS", section)))
00369   {
00370     /* found the entry in the ini file */
00371     if (!strcmp(inistring, "N_M"))
00372     {
00373       torque_units = N_M;
00374     }
00375     else if (!strcmp(inistring, "LB_FT"))
00376     {
00377       torque_units = LB_FT;
00378     }
00379     else if (!strcmp(inistring, "OZ_IN"))
00380     {
00381       torque_units = OZ_IN;
00382     }
00383     else
00384     {
00385       torque_units = TORQUE_UNITS_INVALID;
00386       rcs_print_error( "bad torque units specified in ini file: %s\n",
00387               inistring);
00388       retval = -1;
00389     }
00390   }
00391   else
00392   {
00393     /* not in ini file-- use default */
00394     torque_units = N_M;
00395   }
00396 
00397   if (NULL != (inistring = iniFind(fp, "ARMATURE_RESISTANCE", section)))
00398   {
00399 #ifndef UNDER_CE
00400     if (1 != sscanf((char *) inistring, "%lf", &dcm.Ra))
00401     {
00402       /* found, but invalid */
00403       rcs_print_error( "invalid ARMATURE_RESISTANCE: %s\n", inistring);
00404       retval = -1;
00405     }
00406 #else
00407     dcm.Ra = RCS_CE_ATOF(inistring);
00408 #endif
00409   }
00410   if (NULL != (inistring = iniFind(fp, "ARMATURE_INDUCTANCE", section)))
00411   {
00412 #ifndef UNDER_CE
00413     if (1 != sscanf((char *) inistring, "%lf", &dcm.La))
00414     {
00415       /* found, but invalid */
00416       rcs_print_error( "invalid ARMATURE_INDUCTANCE: %s\n", inistring);
00417       retval = -1;
00418     }
00419 #else
00420     dcm.La = RCS_CE_ATOF(inistring);
00421 #endif
00422   }
00423   if (NULL != (inistring = iniFind(fp, "BACK_EMF_CONSTANT", section)))
00424   {
00425 #ifndef UNDER_CE
00426     if (1 != sscanf((char *) inistring, "%lf", &dcm.Kb))
00427     {
00428       /* found, but invalid */
00429       rcs_print_error( "invalid BACK_EMF_CONSTANT: %s\n", inistring);
00430       retval = -1;
00431     }
00432 #else
00433     dcm.Kb = RCS_CE_ATOF(inistring);
00434 #endif
00435   }
00436   if (NULL != (inistring = iniFind(fp, "ROTOR_INERTIA", section)))
00437   {
00438 #ifndef UNDER_CE
00439     if (1 != sscanf((char *) inistring, "%lf", &dcm.Jm))
00440     {
00441       /* found, but invalid */
00442       rcs_print_error( "invalid ROTOR_INERTIA: %s\n", inistring);
00443       retval = -1;
00444     }
00445 #else
00446     dcm.Jm = RCS_CE_ATOF(inistring);
00447 #endif
00448     switch (torque_units)
00449     {
00450     case OZ_IN:
00451       dcm.Jm *= OZ_IN_TO_N_M;
00452       break;
00453     case LB_FT:
00454       dcm.Jm *= LB_FT_TO_N_M;
00455       break;
00456     default:
00457       break;
00458     }
00459   }
00460   if (NULL != (inistring = iniFind(fp, "DAMPING_FRICTION_COEFFICIENT", section)))
00461   {
00462 #ifndef UNDER_CE
00463     if (1 != sscanf((char *) inistring, "%lf", &dcm.Bm))
00464     {
00465       /* found, but invalid */
00466       rcs_print_error( "invalid DAMPING_FRICTION_COEFFICIENT: %s\n", inistring);
00467       retval = -1;
00468     }
00469 #else
00470     dcm.Bm = RCS_CE_ATOF(inistring);
00471 #endif
00472     switch (torque_units)
00473     {
00474     case OZ_IN:
00475       dcm.Bm *= OZ_IN_TO_N_M;
00476       break;
00477     case LB_FT:
00478       dcm.Bm *= LB_FT_TO_N_M;
00479       break;
00480     default:
00481       break;
00482     }
00483   }
00484 
00485   if (retval == 0)
00486   {
00487     dcmotorSetParameters(dcmotor, dcm);
00488   }
00489 
00490   if (NULL != (inistring = iniFind(fp, "CYCLE_TIME", section)))
00491   {
00492 #ifndef UNDER_CE
00493     if (1 != sscanf((char *) inistring, "%lf", &cycleTime))
00494     {
00495       /* found, but invalid */
00496       rcs_print_error( "invalid CYCLE_TIME: %s\n", inistring);
00497       retval = -1;
00498     }
00499     else
00500     {
00501       dcmotorSetCycleTime(dcmotor, cycleTime);
00502     }
00503 #else
00504     cycleTime = RCS_CE_ATOF(inistring);
00505     dcmotorSetCycleTime(dcmotor, cycleTime);
00506 #endif
00507   }
00508 
00509   if (NULL != (inistring = iniFind(fp, "SHAFT_OFFSET", section)))
00510   {
00511 #ifndef UNDER_CE
00512     if (1 != sscanf((char *) inistring, "%lf", &offset))
00513     {
00514       /* found, but invalid */
00515       rcs_print_error( "invalid OFFSET: %s\n", inistring);
00516       retval = -1;
00517     }
00518     else
00519     {
00520       dcmotorSetOffset(dcmotor, offset);
00521     }
00522 #else
00523     offset = RCS_CE_ATOF(inistring);
00524     dcmotorSetOffset(dcmotor, offset);
00525 #endif
00526   }
00527 
00528   if (NULL != (inistring = iniFind(fp, "REVS_PER_UNIT", section)))
00529   {
00530 #ifndef UNDER_CE
00531     if (1 != sscanf((char *) inistring, "%lf", &revsPerUnit))
00532     {
00533       /* found, but invalid */
00534       rcs_print_error( "invalid REVS_PER_UNIT: %s\n", inistring);
00535       retval = -1;
00536     }
00537     else
00538     {
00539       dcmotorSetRevsPerUnit(dcmotor, revsPerUnit);
00540     }
00541 #else
00542     revsPerUnit = RCS_CE_ATOF(inistring);
00543     dcmotorSetRevsPerUnit(dcmotor, revsPerUnit);
00544 #endif
00545   }
00546 
00547   /* close inifile */
00548 #ifndef UNDER_CE
00549   fclose(fp);
00550 #else
00551   inet_file_close(fp);
00552 #endif
00553 
00554   return retval;
00555 
00556 #else
00557 
00558   return -1;
00559 
00560 #endif /* not STDIO_SUPPORT */
00561 }

int dcmotorResetState (  DC_MOTOR_STRUCT *    dcmotor ) 
 

Definition at line 95 of file dcmotor.c.

00096 {
00097   /* state vars */
00098   dcmotor->ia = 0.0;
00099   dcmotor->dia = 0.0;
00100   dcmotor->wm = 0.0;
00101   dcmotor->dwm = 0.0;
00102   dcmotor->thm = 0.0;
00103   dcmotor->dthm = 0.0;
00104 
00105   return 0;
00106 }

double dcmotorRunCycle (  volatile DC_MOTOR_STRUCT *    dcmotor,
volatile double    ea
) 
 

Definition at line 244 of file dcmotor.c.

00245 {
00246   volatile double deltaT;
00247   volatile double now;
00248 
00249   if (0 == dcm ||
00250       dcm->configured != ALL_SET)
00251     {
00252       return 0.0;
00253     }
00254 
00255   /* calculate deltaT */
00256   if (dcm->cycleTime <= 0.0)
00257     {
00258       /* if cycleTime <= 0.0, we want to use real time stamps */
00259       if (dcm->lastTime <= 0.0)
00260         {
00261           /* first time through */
00262           dcm->lastTime = 0.0;
00263           deltaT = 0.0;         /* results will all be zero */
00264         }
00265       else
00266         {
00267           /* FIXME-- get current time in 'now', instead of 0 */
00268           now = 0.0;
00269           deltaT = now - dcm->lastTime;
00270           dcm->lastTime = now;
00271         }
00272     }
00273   else
00274     {
00275       deltaT = dcm->cycleTime;
00276     }
00277 
00278   /* calculate differentials */
00279 
00280   if(dcm->La != 0.0)
00281     {
00282       dcm->dia = deltaT *
00283         (-dcm->Ra * dcm->ia -
00284          dcm->Kb * dcm->wm +
00285          ea) / dcm->La;
00286     }
00287   else if (dcm->Ra != 0.0)
00288     {
00289       dcm->dia = (ea - dcm->Kb * dcm->wm)/dcm->Ra - dcm->ia;
00290     }
00291   else
00292     {
00293       dcm->dia = -dcm->ia;
00294       if(dcm->Kb != 0.0)
00295         {
00296           dcm->wm = ea/dcm->Kb;
00297         }
00298       else
00299         {
00300           dcm->wm = 0.0;
00301         }
00302     }
00303 
00304   if(dcm->Jm != 0.0)
00305     {
00306       dcm->dwm = deltaT *
00307         (dcm->Kb * dcm->ia -
00308          dcm->Bm * dcm->wm) / dcm->Jm; /* Ki == Kb, Tl = 0 */
00309     }
00310   else if(dcm->Bm != 0.0)
00311     {
00312       dcm->dwm = (dcm->Kb * dcm->ia)/dcm->Bm - dcm->wm;
00313     }
00314   else
00315     {
00316       dcm->dwm = -dcm->wm;
00317       dcm->ia = 0.0;
00318     }
00319 
00320   dcm->dthm = deltaT * dcm->wm;
00321 
00322   /* update state vars with differentials */
00323 
00324   dcm->ia += dcm->dia;
00325   dcm->wm += dcm->dwm;
00326   dcm->thm += dcm->dthm;
00327 
00328   /* return motor position */
00329 
00330   return dcm->thm + dcm->offset;
00331 }

int dcmotorSetCycleTime (  DC_MOTOR_STRUCT *    dcmotor,
double    seconds
) 
 

Definition at line 108 of file dcmotor.c.

00109 {
00110   if (0 == dcmotor)
00111   {
00112     return -1;
00113   }
00114 
00115   /* note that seconds <= 0.0 means use real time */
00116   dcmotor->cycleTime = seconds;
00117   dcmotor->configured |= CYCLE_TIME_SET;
00118 
00119   return 0;
00120 }

int dcmotorSetOffset (  DC_MOTOR_STRUCT *    dcmotor,
double    _offset
) 
 

Definition at line 184 of file dcmotor.c.

00185 {
00186   if (0 == dcmotor)
00187   {
00188     return -1;
00189   }
00190 
00191   dcmotor->offset = _offset;
00192   dcmotor->configured |= OFFSET_SET;
00193 
00194   return 0;
00195 }

int dcmotorSetParameters (  DC_MOTOR_STRUCT *    dcmotor,
DC_MOTOR_STRUCT    params
) 
 

Definition at line 133 of file dcmotor.c.

00134 {
00135   if (0 == dcmotor)
00136   {
00137     return -1;
00138   }
00139 
00140   dcmotor->Ra = params.Ra;
00141   dcmotor->La = params.La;
00142   dcmotor->Kb = params.Kb;
00143   dcmotor->Jm = params.Jm;
00144   dcmotor->Bm = params.Bm;
00145 
00146   dcmotor->configured |= PARAMETERS_SET;
00147 
00148   return 0;
00149 }

int dcmotorSetRevsPerUnit (  DC_MOTOR_STRUCT *    dcmotor,
double    _revsPerUnit
) 
 

Definition at line 208 of file dcmotor.c.

00209 {
00210   if (0 == dcmotor ||
00211       _revsPerUnit <= 0.0)
00212   {
00213     return -1;
00214   }
00215 
00216   dcmotor->revsPerUnit = _revsPerUnit;
00217   dcmotor->configured |= REVS_PER_UNIT_SET;
00218 
00219   return 0;
00220 }

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