lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

Removed MicosSrc; Added motorMicos submodule

Browse Source
This commit is contained in:
kpetersn
2019-04-09 15:14:04 -05:00
parent 27fca6f1cd
commit ecbb008597
18 changed files with 5 additions and 1693 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitmodules
+3
View File
@@ -52,3 +52,6 @@
[submodule "modules/motorAcs"]
path = modules/motorAcs
url = https://github.com/epics-motor/motorAcs.git
[submodule "modules/motorMicos"]
path = modules/motorMicos
url = https://github.com/epics-motor/motorMicos.git
iocBoot/iocWithAsyn/motor.cmd.SMChydra
-44
View File
@@ -1,44 +0,0 @@
### motor.cmd.SMChydra
#
# This file is designed to be sourced:
# * after the communication port has been configured
# * before iocInit
#
# Example ethernet port configuration:
# drvAsynIPPortConfigure("hydraEth","192.168.1.17:400", 0, 0, 0)
#
# Ethernet
HydraPort = "hydraEth"
# Serial
#!HydraPort = "serial5"
# Show communication
#asynSetTraceMask(HydraPort, 0, 3)
# Only show errors
asynSetTraceMask(HydraPort, 0, 1)
# Leave ascii selected so traces can be turned on with a single click
asynSetTraceIOMask(HydraPort, 0, 1)
# Set end-of-string terminators
asynOctetSetInputEos(HydraPort,0,"\r\n")
asynOctetSetOutputEos(HydraPort,0,"\r\n")
dbLoadTemplate("motor.substitutions.SMChydra")
# SMChydraCreateController(
# port name,
# asyn port name,
# num axes,
# moving poll period (ms),
# idle poll period (ms) )
SMChydraCreateController("Hydra1", HydraPort, 2, 100, 500)
# Use the following line to disable idle polling
#!SMChydraCreateController("Hydra1", HydraPort, 2, 100, 0)
# This sleep is useful when asyn traces are enabled
#!epicsThreadSleep(3.0)
# Include an asyn record for each motor to make it easy to enable diagnostic messages
dbLoadRecords("$(ASYN)/db/asynRecord.db","P=IOC:,R=hydraAsyn1,PORT=Hydra1,ADDR=0,OMAX=256,IMAX=256")
dbLoadRecords("$(ASYN)/db/asynRecord.db","P=IOC:,R=hydraAsyn2,PORT=Hydra1,ADDR=1,OMAX=256,IMAX=256")
iocBoot/iocWithAsyn/motor.substitutions.SMChydra
-8
View File
@@ -1,8 +0,0 @@
file "$(TOP)/db/basic_asyn_motor.db"
{
pattern
{P, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VMAX, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT}
{IOC:, m1, "asynMotor", Hydra1, 0, "m1", degrees, Pos, 15., 3., .05, .5, 0, 1.0, 2, 0.01, 2, 175, -175, ""}
{IOC:, m2, "asynMotor", Hydra1, 1, "m2", degrees, Pos, 15., 3., .05, .5, 0, 1.0, 2, 0.01, 2, 175, -175, ""}
}
iocBoot/iocWithAsyn/st.cmd.Vx
-21
View File
@@ -57,27 +57,6 @@ dbLoadRecords("$(MOTOR)/db/motorUtil.db", "P=IOC:")
#!drvPMNC87xxdebug=4
# Micos MoCo driver setup parameters:
# Load MicosSetup once.
# (1) max # of controller groups. Controller groups are per serial
# port.
# (2) max # axes per controller group. Maximum 16. (addr 0-15)
# (3) motor task polling rate (min=1Hz, max=60Hz, 10Hz works well)
# Example:
# MicosSetup(1, 2, 10) 1 group. 2 axes (controllers) in the group.
# 10Hz poll.
#!MicosSetup(1, 2, 10)
# Micos MoCo driver configuration parameters:
# Load one MicosConfig for each group of Micos drivers.
# (1) "Controller group" number
# (2) ASYN port name
#!MicosConfig(0, "a-Serial[0]")
#!drvMicosDebug = 4
# Micos SMC hydra
#!< motor.cmd.SMChydra
# IMS MDrive driver setup parameters:
# (1) maximum number of controllers in system
# (2) motor task polling rate (min=1Hz,max=60Hz)
iocBoot/iocWithAsyn/st.cmd.unix
-18
View File
@@ -37,24 +37,6 @@ asynSetOption("L0", -1, "crtscts", "N")
#!IM483PLConfig(0, "L0")
#!var drvIM483PLdebug 4
# Micos MoCo driver setup parameters:
# Load MicosSetup once.
# (1) max # of controller groups. Controller groups are per serial
# port.
# (2) max # axes per controller group. Maximum 16. (addr 0-15)
# (3) motor task polling rate (min=1Hz, max=60Hz, 10Hz works well)
# Example:
# MicosSetup(1, 2, 10) 1 group. 2 axes (controllers) in the group.
# 10Hz poll.
#!MicosSetup(1, 2, 10)
# Micos MoCo driver configuration parameters:
# Load one MicosConfig for each group of Micos drivers.
# (1) "Controller group" number
# (2) ASYN port name
#!MicosConfig(0, "L0")
#!var drvMicosDebug 4
# IMS MDrive driver setup parameters:
# (1) maximum number of controllers in system
# (2) motor task polling rate (min=1Hz,max=60Hz)
iocBoot/iocWithAsyn/st.cmd.win32
-18
View File
@@ -26,24 +26,6 @@ dbLoadRecords("$(MOTOR)/db/motorUtil.db", "P=IOC:")
#!IM483PLConfig(0, "L0")
#!var drvIM483PLdebug 4
# Micos MoCo driver setup parameters:
# Load MicosSetup once.
# (1) max # of controller groups. Controller groups are per serial
# port.
# (2) max # axes per controller group. Maximum 16. (addr 0-15)
# (3) motor task polling rate (min=1Hz, max=60Hz, 10Hz works well)
# Example:
# MicosSetup(1, 2, 10) 1 group. 2 axes (controllers) in the group.
# 10Hz poll.
#!MicosSetup(1, 2, 10)
# Micos MoCo driver configuration parameters:
# Load one MicosConfig for each group of Micos drivers.
# (1) "Controller group" number
# (2) ASYN port name
#!MicosConfig(0, "L0")
#!var drvMicosDebug 4
# IMS MDrive driver setup parameters:
# (1) maximum number of controllers in system
# (2) motor task polling rate (min=1Hz,max=60Hz)
modules/Makefile
+1
View File
@@ -15,6 +15,7 @@ SUBMODULES += motorHytec
endif
SUBMODULES += motorKohzu
SUBMODULES += motorMclennan
SUBMODULES += motorMicos
SUBMODULES += motorMicroMo
SUBMODULES += motorMicronix
SUBMODULES += motorNPoint
modules/motorMicos
Submodule
+1
Submodule modules/motorMicos added at 132c52a645
motorApp/Makefile
-3
View File
@@ -27,9 +27,6 @@ ImsSrc_DEPEND_DIRS = MotorSrc
DIRS += PIGCS2Src
PIGCS2Src_DEPEND_DIRS = MotorSrc
DIRS += MicosSrc
MicosSrc_DEPEND_DIRS = MotorSrc
DIRS += PC6KSrc
PC6KSrc_DEPEND_DIRS = MotorSrc
motorApp/MicosSrc/Makefile
-24
View File
@@ -1,24 +0,0 @@
# Makefile
TOP = ../..
include $(TOP)/configure/CONFIG
# Both the following line, and a line in the *.dbd file,
# must be uncommented to use diagnostic debugging messages.
#!USR_CXXFLAGS += -DDEBUG
DBD += devMicos.dbd
LIBRARY_IOC = Micos
# Intelligent Motion Systems driver support.
SRCS += MicosRegister.cc
SRCS += devMicos.cc drvMicos.cc
# SMC hydra support
SRCS += SMChydraDriver.cpp
Micos_LIBS += motor asyn
Micos_LIBS += $(EPICS_BASE_IOC_LIBS)
include $(TOP)/configure/RULES
motorApp/MicosSrc/MicosRegister.cc
-64
View File
@@ -1,64 +0,0 @@
/*
FILENAME... MicosRegister.cc
USAGE... Register Micos MoCo dc motor controller device driver shell commands.
*/
/*****************************************************************
COPYRIGHT NOTIFICATION
*****************************************************************
(C) COPYRIGHT 1993 UNIVERSITY OF CHICAGO
This software was developed under a United States Government license
described on the COPYRIGHT_UniversityOfChicago file included as part
of this distribution.
**********************************************************************/
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>
#include <iocsh.h>
#include "motor.h"
#include "drvMicos.h"
#include "epicsExport.h"
extern "C"
{
// Micos Setup arguments
static const iocshArg setupArg0 = {"Max. controller count", iocshArgInt};
static const iocshArg setupArg1 = {"Max. motor count", iocshArgInt};
static const iocshArg setupArg2 = {"Polling rate", iocshArgInt};
// Micos Config arguments
static const iocshArg configArg0 = {"Card being configured", iocshArgInt};
static const iocshArg configArg1 = {"asyn port name", iocshArgString};
static const iocshArg * const MicosSetupArgs[3] = {&setupArg0, &setupArg1,
&setupArg2};
static const iocshArg * const MicosConfigArgs[2] = {&configArg0, &configArg1};
static const iocshFuncDef setupMicos = {"MicosSetup", 3, MicosSetupArgs};
static const iocshFuncDef configMicos = {"MicosConfig", 2, MicosConfigArgs};
static void setupMicosCallFunc(const iocshArgBuf *args)
{
MicosSetup(args[0].ival, args[1].ival, args[2].ival);
}
static void configMicosCallFunc(const iocshArgBuf *args)
{
MicosConfig(args[0].ival, args[1].sval);
}
static void MicosmotorRegister(void)
{
iocshRegister(&setupMicos, setupMicosCallFunc);
}
epicsExportRegistrar(MicosmotorRegister);
} // extern "C"
motorApp/MicosSrc/SMChydraDriver.cpp
-496
View File
@@ -1,496 +0,0 @@
/*
FILENAME... SMChydraDriver.cpp
USAGE... Motor driver support for the Micos SMC hydra controller.
Note: This driver was tested with the Micos SMC hydra CM and
motor forms 0 (stepper) and 1 (linear).
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <iocsh.h>
#include <epicsThread.h>
#include <asynOctetSyncIO.h>
#include "asynMotorController.h"
#include "asynMotorAxis.h"
#include <epicsExport.h>
#include "SMChydraDriver.h"
#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5)
/** Creates a new SMChydraController object.
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] SMChydraPortName The name of the drvAsynSerialPort that was created previously to connect to the SMC hydra controller
* \param[in] numAxes The number of axes that this controller supports
* \param[in] movingPollPeriod The time between polls when any axis is moving
* \param[in] idlePollPeriod The time between polls when no axis is moving
*/
SMChydraController::SMChydraController(const char *portName, const char *SMChydraPortName, int numAxes,
double movingPollPeriod, double idlePollPeriod)
: asynMotorController(portName, numAxes, NUM_SMCHYDRA_PARAMS,
0, // No additional interfaces beyond those in base class
0, // No additional callback interfaces beyond those in base class
ASYN_CANBLOCK | ASYN_MULTIDEVICE,
1, // autoconnect
0, 0) // Default priority and stack size
{
int axis;
asynStatus status;
SMChydraAxis *pAxis;
static const char *functionName = "SMChydraController::SMChydraController";
/* Connect to SMC hydra controller */
status = pasynOctetSyncIO->connect(SMChydraPortName, 0, &pasynUserController_, NULL);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: cannot connect to SMC hydra controller\n",
functionName);
}
for (axis=0; axis<numAxes; axis++) {
pAxis = new SMChydraAxis(this, axis);
}
startPoller(movingPollPeriod, idlePollPeriod, 2);
}
/** Creates a new SMChydraController object.
* Configuration command, called directly or from iocsh
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] SMChydraPortName The name of the drvAsynIPPPort that was created previously to connect to the SMC hydra controller
* \param[in] numAxes The number of axes that this controller supports
* \param[in] movingPollPeriod The time in ms between polls when any axis is moving
* \param[in] idlePollPeriod The time in ms between polls when no axis is moving
*/
extern "C" int SMChydraCreateController(const char *portName, const char *SMChydraPortName, int numAxes,
int movingPollPeriod, int idlePollPeriod)
{
SMChydraController *pSMChydraController
= new SMChydraController(portName, SMChydraPortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
pSMChydraController = NULL;
return(asynSuccess);
}
/** Reports on status of the driver
* \param[in] fp The file pointer on which report information will be written
* \param[in] level The level of report detail desired
*
* If details > 0 then information is printed about each axis.
* After printing controller-specific information it calls asynMotorController::report()
*/
void SMChydraController::report(FILE *fp, int level)
{
fprintf(fp, "SMC hydra motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n",
this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);
// Call the base class method
asynMotorController::report(fp, level);
}
/** Returns a pointer to an SMChydraAxis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
SMChydraAxis* SMChydraController::getAxis(asynUser *pasynUser)
{
return static_cast<SMChydraAxis*>(asynMotorController::getAxis(pasynUser));
}
/** Returns a pointer to an SMChydraAxis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] axisNo Axis index number. */
SMChydraAxis* SMChydraController::getAxis(int axisNo)
{
return static_cast<SMChydraAxis*>(asynMotorController::getAxis(axisNo));
}
// These are the SMChydraAxis methods
/** Creates a new SMChydraAxis object.
* \param[in] pC Pointer to the SMChydraController to which this axis belongs.
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
*
* Initializes register numbers, etc.
*/
SMChydraAxis::SMChydraAxis(SMChydraController *pC, int axisNo)
: asynMotorAxis(pC, axisNo),
pC_(pC)
{
sprintf(pC_->outString_, "%i getmotor", (axisNo + 1));
pC_->writeReadController();
motorForm_ = atoi( (char *) &pC_->inString_ );
sprintf(pC_->outString_, "%i getpitch", (axisNo + 1));
pC_->writeReadController();
pitch_ = atof( (char *) &pC_->inString_ );
sprintf(pC_->outString_, "%i getpolepairs", (axisNo + 1));
pC_->writeReadController();
polePairs_ = atoi( (char *) &pC_->inString_ );
sprintf(pC_->outString_, "%i getclperiod", (axisNo + 1));
pC_->writeReadController();
clPeriod_ = atof( (char *) &pC_->inString_ );
switch (motorForm_)
{
case 0:
// Stepper motor
axisRes_ = pitch_ / ( 4.0 * polePairs_);
break;
case 1:
// Linear or torque motor
axisRes_ = clPeriod_;
break;
default:
// For now assume clPeriod_ works for other motor forms
axisRes_ = clPeriod_;
break;
}
/* Enable gain support so that the CNEN field can be used to send
the init command to clear a motor fault for stepper motors, even
though they lack closed-loop support. */
setIntegerParam(pC_->motorStatusGainSupport_, 1);
// Determine the travel limits (will change after homing)
sprintf(pC_->outString_, "%i getnlimit", (axisNo + 1));
pC_->writeReadController();
sscanf(pC_->inString_, "%lf %lf", &negTravelLimit_, &posTravelLimit_);
}
/** Reports on status of the axis
* \param[in] fp The file pointer on which report information will be written
* \param[in] level The level of report detail desired
*
* After printing device-specific information calls asynMotorAxis::report()
*/
void SMChydraAxis::report(FILE *fp, int level)
{
if (level > 0) {
fprintf(fp, " axis %d\n", axisNo_);
fprintf(fp, " motorForm %d\n", motorForm_);
fprintf(fp, " pitch %f\n", pitch_);
fprintf(fp, " polePairs %d\n", polePairs_);
fprintf(fp, " clPeriod %f\n", clPeriod_);
fprintf(fp, " axisRes %f\n", axisRes_);
fprintf(fp, " lowLimitConfig %d\n", lowLimitConfig_);
fprintf(fp, " highLimitConfig %d\n", highLimitConfig_);
fprintf(fp, " posTravelLimit %f\n", posTravelLimit_);
fprintf(fp, " negTravelLimit %f\n", negTravelLimit_);
}
// Call the base class method
asynMotorAxis::report(fp, level);
}
asynStatus SMChydraAxis::sendAccelAndVelocity(double acceleration, double velocity)
{
asynStatus status;
// static const char *functionName = "SMChydraAxis::sendAccelAndVelocity";
// Send the velocity
sprintf(pC_->outString_, "%f %i snv", (velocity * axisRes_), (axisNo_ + 1));
status = pC_->writeController();
// Send the acceleration
// acceleration is in units/sec/sec
sprintf(pC_->outString_, "%f %i sna", (acceleration * axisRes_), (axisNo_ + 1));
status = pC_->writeController();
return status;
}
asynStatus SMChydraAxis::move(double position, int relative, double baseVelocity, double slewVelocity, double acceleration)
{
asynStatus status;
// static const char *functionName = "SMChydraAxis::move";
status = sendAccelAndVelocity(acceleration, slewVelocity);
if (relative) {
sprintf(pC_->outString_, "%f %i nr", (position * axisRes_), (axisNo_ + 1));
} else {
sprintf(pC_->outString_, "%f %i nm", (position * axisRes_), (axisNo_ + 1));
}
status = pC_->writeController();
return status;
}
asynStatus SMChydraAxis::home(double baseVelocity, double slewVelocity, double acceleration, int forwards)
{
asynStatus status;
// static const char *functionName = "SMChydraAxis::home";
status = sendAccelAndVelocity(acceleration, slewVelocity);
if (forwards) {
sprintf(pC_->outString_, "%i nrm", (axisNo_ + 1));
} else {
sprintf(pC_->outString_, "%i ncal", (axisNo_ + 1));
}
status = pC_->writeController();
return status;
}
asynStatus SMChydraAxis::moveVelocity(double baseVelocity, double slewVelocity, double acceleration)
{
asynStatus status;
static const char *functionName = "SMChydraAxis::moveVelocity";
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s: baseVelocity=%f, slewVelocity=%f, acceleration=%f\n",
functionName, baseVelocity, slewVelocity, acceleration);
/* SMC hydra does not have jog command. Move to a limit*/
if (slewVelocity > 0.) {
status = sendAccelAndVelocity(acceleration, slewVelocity);
sprintf(pC_->outString_, "%f %i nm", posTravelLimit_, (axisNo_ + 1));
} else {
status = sendAccelAndVelocity(acceleration, (slewVelocity * -1.0));
sprintf(pC_->outString_, "%f %i nm", negTravelLimit_, (axisNo_ + 1));
}
status = pC_->writeController();
return status;
}
asynStatus SMChydraAxis::stop(double acceleration )
{
asynStatus status;
//static const char *functionName = "SMChydraAxis::stop";
// Set stop deceleration (will be overridden by accel if accel is higher)
sprintf(pC_->outString_, "%f %i ssd", (acceleration * axisRes_), (axisNo_ + 1));
status = pC_->writeController();
sprintf(pC_->outString_, "%i nabort", (axisNo_ + 1));
status = pC_->writeController();
return status;
}
asynStatus SMChydraAxis::setPosition(double position)
{
asynStatus status;
//static const char *functionName = "SMChydraAxis::setPosition";
// The argument to the setnpos command is the distance from the current position of the
// desired origin, which is why the position needs to be multiplied by -1.0
sprintf(pC_->outString_, "%f %i setnpos", (position * axisRes_ * -1.0), (axisNo_ + 1));
status = pC_->writeController();
return status;
}
asynStatus SMChydraAxis::setClosedLoop(bool closedLoop)
{
asynStatus status = asynSuccess;
//static const char *functionName = "SMChydraAxis::setClosedLoop";
switch (motorForm_)
{
case 0:
// Stepper motor
// reinit to clear a motor fault when CNEN=1
if (closedLoop)
{
sprintf(pC_->outString_, "%i init", (axisNo_ + 1));
status = pC_->writeController();
// a delay is required after the init command is sent
epicsThreadSleep(0.2);
}
break;
case 1:
// Linear or torque motor
// enable closed-loop control
sprintf(pC_->outString_, "%i %i setcloop", closedLoop ? 1:0, (axisNo_ + 1));
status = pC_->writeController();
// reinit so the closed-loop setting takes effect
sprintf(pC_->outString_, "%i init", (axisNo_ + 1));
status = pC_->writeController();
// a delay is required after the init command is sent
epicsThreadSleep(0.2);
break;
default:
// Do nothing
break;
}
return status;
}
/** Polls the axis.
* This function reads the motor position, the limit status, the home status, the moving status,
* and the drive power-on status.
* It calls setIntegerParam() and setDoubleParam() for each item that it polls,
* and then calls callParamCallbacks() at the end.
* \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
asynStatus SMChydraAxis::poll(bool *moving)
{
int done;
int driveOn;
int lowLimit;
int highLimit;
int ignoreLowLimit;
int ignoreHighLimit;
int axisStatus=-1;
double position=0.0;
asynStatus comStatus;
static const char *functionName = "SMChydraAxis::poll";
// Read the current motor position
sprintf(pC_->outString_, "%i np", (axisNo_ + 1));
comStatus = pC_->writeReadController();
if (comStatus) goto skip;
// The response string is a double
position = atof( (char *) &pC_->inString_);
setDoubleParam(pC_->motorPosition_, (position / axisRes_) );
setDoubleParam(pC_->motorEncoderPosition_, (position / axisRes_) );
// Read the status of this motor
sprintf(pC_->outString_, "%i nst", (axisNo_ + 1));
comStatus = pC_->writeReadController();
if (comStatus) goto skip;
// The response string is an int
axisStatus = atoi( (char *) &pC_->inString_);
// Check the moving bit
done = !(axisStatus & 0x1);
setIntegerParam(pC_->motorStatusDone_, done);
setIntegerParam(pC_->motorStatusMoving_, !done);
*moving = done ? false:true;
// Read the commanded velocity and acceleration
sprintf(pC_->outString_, "%i gnv", (axisNo_ + 1));
comStatus = pC_->writeReadController();
sprintf(pC_->outString_, "%i gna", (axisNo_ + 1));
comStatus = pC_->writeReadController();
// Check the limit bit (0x40)
if (axisStatus & 0x40)
{
asynPrint(this->pasynUser_, ASYN_TRACEIO_DRIVER,
"%s: axis %i limit indicator active.\n",
functionName, (axisNo_ + 1));
// query limits?
}
// Check the e-stop bit (0x80)
if (axisStatus & 0x80)
{
asynPrint(this->pasynUser_, ASYN_TRACEIO_DRIVER,
"%s: axis %i emergency stopped.\n",
functionName, (axisNo_ + 1));
}
// Check the e-stop switch active bit (0x200)
if (axisStatus & 0x200)
{
asynPrint(this->pasynUser_, ASYN_TRACEIO_DRIVER,
"%s: axis %i emergency stop switch active.\n",
functionName, (axisNo_ + 1));
setIntegerParam(pC_->motorStatusProblem_, 1);
}
else{
setIntegerParam(pC_->motorStatusProblem_, 0);
}
// Check the device busy bit (0x400)
if (axisStatus & 0x400)
{
asynPrint(this->pasynUser_, ASYN_TRACE_ERROR,
"%s: axis %i device is busy - move commands discarded.\n",
functionName, (axisNo_ + 1));
}
// Read the limit status
// Note: calibration switch = low limit; range measure switch = high limit
// also need to read the switch confiruation to see if limits are ignored"
// Read switch confiruation
// Bit 0: polarity (0 = NO, 1 = NC)
// Bit 1: mask (0 = enabled, 1 = disabled)
sprintf(pC_->outString_, "%i getsw", (axisNo_ + 1));
comStatus = pC_->writeReadController();
if (comStatus) goto skip;
sscanf(pC_->inString_, "%i %i", &lowLimitConfig_, &highLimitConfig_);
ignoreLowLimit = lowLimitConfig_ & 0x2;
ignoreHighLimit = highLimitConfig_ & 0x2;
// Read status of switches 0=inactive 1=active
sprintf(pC_->outString_, "%i getswst", (axisNo_ + 1));
comStatus = pC_->writeReadController();
if (comStatus) goto skip;
// The response string is of the form "0 0"
sscanf(pC_->inString_, "%i %i", &lowLimit, &highLimit);
//
if (ignoreLowLimit)
setIntegerParam(pC_->motorStatusLowLimit_, 0);
else
setIntegerParam(pC_->motorStatusLowLimit_, lowLimit);
if (ignoreHighLimit)
setIntegerParam(pC_->motorStatusHighLimit_, 0);
else
setIntegerParam(pC_->motorStatusHighLimit_, highLimit);
/*setIntegerParam(pC_->motorStatusAtHome_, limit);*/
// Check the drive power bit (0x100)
driveOn = (axisStatus & 0x100) ? 0 : 1;
setIntegerParam(pC_->motorStatusPowerOn_, driveOn);
setIntegerParam(pC_->motorStatusProblem_, 0);
skip:
setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
callParamCallbacks();
return comStatus ? asynError : asynSuccess;
}
/** Code for iocsh registration */
static const iocshArg SMChydraCreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg SMChydraCreateControllerArg1 = {"SMC hydra port name", iocshArgString};
static const iocshArg SMChydraCreateControllerArg2 = {"Number of axes", iocshArgInt};
static const iocshArg SMChydraCreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt};
static const iocshArg SMChydraCreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt};
static const iocshArg * const SMChydraCreateControllerArgs[] = {&SMChydraCreateControllerArg0,
&SMChydraCreateControllerArg1,
&SMChydraCreateControllerArg2,
&SMChydraCreateControllerArg3,
&SMChydraCreateControllerArg4};
static const iocshFuncDef SMChydraCreateControllerDef = {"SMChydraCreateController", 5, SMChydraCreateControllerArgs};
static void SMChydraCreateControllerCallFunc(const iocshArgBuf *args)
{
SMChydraCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
}
static void SMChydraRegister(void)
{
iocshRegister(&SMChydraCreateControllerDef, SMChydraCreateControllerCallFunc);
}
extern "C" {
epicsExportRegistrar(SMChydraRegister);
}
motorApp/MicosSrc/SMChydraDriver.h
-56
View File
@@ -1,56 +0,0 @@
/*
FILENAME... SMChydraDriver.h
USAGE... Motor driver support for the Micos SMC hydra controller.
*/
#include "asynMotorController.h"
#include "asynMotorAxis.h"
#define MAX_SMCHYDRA_AXES 2
// No controller-specific parameters yet
#define NUM_SMCHYDRA_PARAMS 0
class epicsShareClass SMChydraAxis : public asynMotorAxis
{
public:
/* These are the methods we override from the base class */
SMChydraAxis(class SMChydraController *pC, int axis);
void report(FILE *fp, int level);
asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
asynStatus stop(double acceleration);
asynStatus poll(bool *moving);
asynStatus setPosition(double position);
asynStatus setClosedLoop(bool closedLoop);
private:
SMChydraController *pC_; /* Pointer to the asynMotorController to which this axis belongs.
Abbreviated because it is used very frequently */
asynStatus sendAccelAndVelocity(double accel, double velocity);
int motorForm_;
double pitch_;
int polePairs_;
double clPeriod_;
double axisRes_;
double mres_;
int lowLimitConfig_;
int highLimitConfig_;
double posTravelLimit_;
double negTravelLimit_;
friend class SMChydraController;
};
class epicsShareClass SMChydraController : public asynMotorController {
public:
SMChydraController(const char *portName, const char *SMChydraPortName, int numAxes, double movingPollPeriod, double idlePollPeriod);
void report(FILE *fp, int level);
SMChydraAxis* getAxis(asynUser *pasynUser);
SMChydraAxis* getAxis(int axisNo);
friend class SMChydraAxis;
};
motorApp/MicosSrc/devMicos.cc
-317
View File
@@ -1,317 +0,0 @@
/* File: devMicos.cc */
/* Device Support Routines for Micos MoCo dc motor controller. */
/*
* Original Author: Kurt Goetze
* Date: 11-24-2003
*
* Modification Log:
* -----------------
* .00 11-24-2003 kag initialized from drvMCB4B.c
* .01 02-06-2004 rls Eliminate erroneous "Motor motion timeout ERROR".
* .02 02-12-2004 rls copied from devMicos.c; ported to R3.14.x
*/
#define VERSION 2.00
#include <string.h>
#include "motorRecord.h"
#include "motor.h"
#include "motordevCom.h"
#include "drvMicos.h"
#include "epicsExport.h"
/*----------------debugging-----------------*/
volatile int devMicosDebug = 0;
extern "C" {epicsExportAddress(int, devMicosDebug);}
static inline void Debug(int level, const char *format, ...) {
#ifdef DEBUG
if (level < devMicosDebug) {
va_list pVar;
va_start(pVar, format);
vprintf(format, pVar);
va_end(pVar);
}
#endif
}
/* Debugging levels:
* devMicosDebug >= 3 Print new part of command and command string so far
* at the end of Micos_build_trans
*/
extern struct driver_table Micos_access;
/* ----------------Create the dsets for devMicos----------------- */
static struct driver_table *drvtabptr;
static long Micos_init(int);
static long Micos_init_record(void *);
static long Micos_start_trans(struct motorRecord *);
static RTN_STATUS Micos_build_trans(motor_cmnd, double *, struct motorRecord *);
static RTN_STATUS Micos_end_trans(struct motorRecord *);
struct motor_dset devMicos =
{
{8, NULL, (DEVSUPFUN) Micos_init, (DEVSUPFUN) Micos_init_record, NULL},
motor_update_values,
Micos_start_trans,
Micos_build_trans,
Micos_end_trans
};
extern "C" {epicsExportAddress(dset,devMicos);}
/* --------------------------- program data --------------------- */
/* This table is used to define the command types */
static msg_types Micos_table[] = {
MOTION, /* MOVE_ABS */
MOTION, /* MOVE_REL */
MOTION, /* HOME_FOR */
MOTION, /* HOME_REV */
IMMEDIATE, /* LOAD_POS */
IMMEDIATE, /* SET_VEL_BASE */
IMMEDIATE, /* SET_VELOCITY */
IMMEDIATE, /* SET_ACCEL */
IMMEDIATE, /* GO */
IMMEDIATE, /* SET_ENC_RATIO */
INFO, /* GET_INFO */
MOVE_TERM, /* STOP_AXIS */
VELOCITY, /* JOG */
IMMEDIATE, /* SET_PGAIN */
IMMEDIATE, /* SET_IGAIN */
IMMEDIATE, /* SET_DGAIN */
IMMEDIATE, /* ENABLE_TORQUE */
IMMEDIATE, /* DISABL_TORQUE */
IMMEDIATE, /* PRIMITIVE */
IMMEDIATE, /* SET_HIGH_LIMIT */
IMMEDIATE /* SET_LOW_LIMIT */
};
static struct board_stat **Micos_cards;
/* --------------------------- program data --------------------- */
/* initialize device support for Micos DC motor */
static long Micos_init(int after)
{
long rtnval;
Debug(5, "Micos_init: entry\n");
if (!after)
{
drvtabptr = &Micos_access;
(drvtabptr->init)();
}
rtnval = motor_init_com(after, *drvtabptr->cardcnt_ptr, drvtabptr, &Micos_cards);
Debug(5, "Micos_init: exit\n");
return(rtnval);
}
/* initialize a record instance */
static long Micos_init_record(void *arg)
{
struct motorRecord *mr = (struct motorRecord *) arg;
long rtnval;
Debug(5, "Micos_init_record: entry\n");
rtnval = motor_init_record_com(mr, *drvtabptr->cardcnt_ptr,
drvtabptr, Micos_cards);
Debug(5, "Micos_init_record: exit\n");
return(rtnval);
}
/* start building a transaction */
static long Micos_start_trans(struct motorRecord *mr)
{
return(OK);
}
/* end building a transaction */
static RTN_STATUS Micos_end_trans(struct motorRecord *mr)
{
return(OK);
}
/* add a part to the transaction */
static RTN_STATUS Micos_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr)
{
struct motor_trans *trans = (struct motor_trans *) mr->dpvt;
struct mess_node *motor_call;
struct controller *brdptr;
struct MicosController *cntrl;
char buff[30];
int axis, card;
RTN_STATUS rtnval;
double dval; /* placeholder for double values passed from motor record */
long ival; /* placeholder for ints passed from motor record */
rtnval = OK;
buff[0] = '\0';
/* Protect against NULL pointer with WRTITE_MSG(GO/STOP_AXIS/GET_INFO, NULL). */
dval = (parms == NULL) ? 0.0 : *parms;
ival = NINT(parms[0]);
rtnval = (RTN_STATUS) motor_start_trans_com(mr, Micos_cards);
Debug(5, "Micos_build_trans: entry, motor_start_trans_com=%ld\n", rtnval);
motor_call = &(trans->motor_call);
motor_call->type = Micos_table[command];
card = motor_call->card; /* card is the group of drivers per unique serial port */
axis = motor_call->signal; /* axis is Micos address, up to 16 (0-15) per serial port */
/* Note: Each Micos driver drives _1_ motor */
brdptr = (*trans->tabptr->card_array)[card];
Debug(5, "Micos_build_trans: axis=%d, command=%d\n", axis, command);
if (brdptr == NULL)
return(rtnval = ERROR);
cntrl = (struct MicosController *) brdptr->DevicePrivate;
if (trans->state != BUILD_STATE)
return(rtnval = ERROR);
if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0)
{
strcpy(motor_call->message, mr->init);
rtnval = motor_end_trans_com(mr, drvtabptr);
rtnval = (RTN_STATUS) motor_start_trans_com(mr, Micos_cards);
motor_call->type = Micos_table[command];
}
switch (command)
{
case MOVE_ABS:
case MOVE_REL:
case HOME_FOR:
case HOME_REV:
case JOG:
if (strlen(mr->prem) != 0)
{
strcpy(motor_call->message, mr->prem);
rtnval = (RTN_STATUS) motor_start_trans_com(mr, Micos_cards);
motor_call->type = Micos_table[command];
}
if (strlen(mr->post) != 0)
motor_call->postmsgptr = (char *) &mr->post;
break;
default:
break;
}
switch (command)
{
case MOVE_ABS:
sprintf(motor_call->message, "%c%dma%ld", CTLA, axis, ival);
break;
case MOVE_REL:
sprintf(motor_call->message, "%c%dmr%ld", CTLA, axis, ival);
break;
case HOME_FOR:
sprintf(motor_call->message, "%c%dgh", CTLA, axis);
break;
case HOME_REV:
sprintf(motor_call->message, "%c%dgh", CTLA, axis);
break;
case LOAD_POS: /* Micos allows you to define the zero position only */
if (dval == 0.0)
sprintf(motor_call->message, "%c%ddh,ud", CTLA, axis);
else
rtnval = ERROR;
break;
case SET_VEL_BASE:
trans->state = IDLE_STATE;
break; /* Micos does not use base velocity */
case SET_VELOCITY:
if (ival < 0) ival = 0;
if (ival > 1000000) ival = 1000000;
sprintf(motor_call->message, "%c%ddv%ld,ud", CTLA, axis, ival);
break;
case SET_ACCEL:
/* dval is acceleration in steps/sec/sec */
if (ival < 0) ival = 0;
if (ival > 1000000) ival = 1000000;
sprintf(motor_call->message, "%c%dda%ld,ud", CTLA, axis, ival);
break;
case GO:
/*
* The Micos starts moving immediately on move commands, GO command
* does nothing
*/
trans->state = IDLE_STATE;
break;
case SET_ENC_RATIO:
/*
* The Micos does not have the concept of encoder ratio, ignore this
* command
*/
trans->state = IDLE_STATE;
break;
case GET_INFO:
/* These commands are not actually done by sending a message, but
rather they will indirectly cause the driver to read the status
of all motors */
break;
case STOP_AXIS: /* (decelerate to a) stop */
sprintf(motor_call->message, "%c%dab1", CTLA, axis);
break;
case JOG:
/*
* Micos does not have a jog command. Simulate with move absolute
* to the appropriate software limit. The record will prevent JOG motion
* beyond its soft limits
*/
if (dval > 0.)
sprintf(motor_call->message, "%c%dma%ld", CTLA, axis, (long)(mr->dhlm / mr->mres));
else
sprintf(motor_call->message, "%c%dma%ld", CTLA, axis, (long)(mr->dllm / mr->mres));
break;
case SET_PGAIN:
if (ival < 0) ival = 0;
if (ival > 32767) ival = 32767;
sprintf(motor_call->message, "%c%ddp%ld,ud", CTLA, axis, ival);
break;
case SET_IGAIN:
if (ival < 0) ival = 0;
if (ival > 32767) ival = 32767;
sprintf(motor_call->message, "%c%ddi%ld,ud", CTLA, axis, ival);
break;
case SET_DGAIN:
if (ival < 0) ival = 0;
if (ival > 32767) ival = 32767;
sprintf(motor_call->message, "%c%ddd%ld,ud", CTLA, axis, ival);
break;
case ENABLE_TORQUE:
sprintf(motor_call->message, "%c%dmn", CTLA, axis);
break;
case DISABL_TORQUE:
sprintf(motor_call->message, "%c%dmf", CTLA, axis);
break;
case SET_HIGH_LIMIT:
case SET_LOW_LIMIT:
trans->state = IDLE_STATE;
break;
default:
rtnval = ERROR;
}
rtnval = motor_end_trans_com(mr, drvtabptr);
Debug(5, "Micos_send_msg: motor_end_trans_com status=%ld, exit\n", rtnval);
return (rtnval);
}
motorApp/MicosSrc/devMicos.dbd
-6
View File
@@ -1,6 +0,0 @@
# Micos MoCo dc series support.
device(motor,VME_IO,devMicos,"Micos MoCo")
driver(drvMicos)
#variable(drvMicosDebug)
registrar(SMChydraRegister)
motorApp/MicosSrc/drvMicos.cc
-576
View File
@@ -1,576 +0,0 @@
/* File: drvMicos.cc */
/* Device Driver Support routines for Micos MoCo dc motor controller. */
/*
* Original Author: Kurt Goetze
* Date: 11/24/2003
*
* Modification Log:
* -----------------
* .00 11-24-2003 kag initialized from drvMCB4B.c
* .01 02-06-2004 rls Eliminate erroneous "Motor motion timeout ERROR".
* .02 02-12-2004 rls Copied from drvMicos.c. Upgraded from R3.14.x
* .03 02-17-2004 rls Removed Debug calls to tickGet().
* .04 07-12-2004 rls Converted from MPF to asyn.
* .05 09-20-2004 rls support for 32axes/controller.
* .08 12-16-2004 rls - asyn R4.0 support.
* - make debug variables always available.
* - MS Visual C compatibility; make all epicsExportAddress
* extern "C" linkage.
*/
#include <string.h>
#include <epicsThread.h>
#include <drvSup.h>
#include <stdlib.h>
#include <errlog.h>
#include "motor.h"
#include "drvMicos.h"
#include "epicsExport.h"
#define WAIT 1
#define COMM_TIMEOUT 2 /* Command timeout in seconds. */
#define BUFF_SIZE 100 /* Maximum length of string to/from Micos */
struct mess_queue
{
struct mess_node *head;
struct mess_node *tail;
};
/*----------------debugging-----------------*/
volatile int drvMicosDebug = 0;
extern "C" {epicsExportAddress(int, drvMicosDebug);}
static inline void Debug(int level, const char *format, ...) {
#ifdef DEBUG
if (level < drvMicosDebug) {
va_list pVar;
va_start(pVar, format);
vprintf(format, pVar);
va_end(pVar);
}
#endif
}
/* Debugging notes:
* drvMicosDebug == 0 No debugging information is printed
* drvMicosDebug >= 1 Warning information is printed
* drvMicosDebug >= 2 Time-stamped messages are printed for each string
* sent to and received from the controller
* drvMicosDebug >= 3 Additional debugging messages
*/
volatile int Micos_num_cards = 0;
volatile int Micos_num_axis = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
/*----------------functions-----------------*/
static int recv_mess(int, char *, int);
static RTN_STATUS send_mess(int, const char *, char *);
static void start_status(int);
static int set_status(int, int);
static long report(int);
static long init();
static int motor_init();
static void query_done(int, int, struct mess_node *);
/*----------------functions-----------------*/
struct driver_table Micos_access =
{
motor_init,
motor_send,
motor_free,
motor_card_info,
motor_axis_info,
&mess_queue,
&queue_lock,
&free_list,
&freelist_lock,
&motor_sem,
&motor_state,
&total_cards,
&any_motor_in_motion,
send_mess,
recv_mess,
set_status,
query_done,
start_status,
&initialized,
NULL
};
struct
{
long number;
long (*report) (int);
long (*init) (void);
} drvMicos = {2, report, init};
extern "C" {epicsExportAddress(drvet, drvMicos);}
static struct thread_args targs = {SCAN_RATE, &Micos_access, 0.0};
/*********************************************************
* Print out driver status report
*********************************************************/
static long report(int level)
{
int card;
if (Micos_num_cards <=0)
printf(" NO Micos controllers found\n");
else
{
for (card = 0; card < Micos_num_cards; card++)
if (motor_state[card])
printf(" Micos controller group %d, id: %s \n",
card,
motor_state[card]->ident);
}
return (0);
}
static long init()
{
/*
* We cannot call motor_init() here, because that function can do GPIB I/O,
* and hence requires that the drvGPIB have already been initialized.
* That cannot be guaranteed, so we need to call motor_init from device
* support
*/
/* Check for setup */
if (Micos_num_cards <= 0)
{
Debug(1, "init: *Micos driver disabled*\n");
Debug(1, "MicosSetup() is missing from startup script.\n");
return (ERROR);
}
return ((long) 0);
}
static void query_done(int card, int axis, struct mess_node *nodeptr)
{
}
/*********************************************************
* Read the status and position of all motors on a card
* start_status(int card)
* if card == -1 then start all cards
*********************************************************/
static void start_status(int card)
{
/* The Micos cannot query status or positions of all axes with a
* single command. This needs to be done on an axis-by-axis basis,
* so this function does nothing
*/
}
/**************************************************************
* Query position and status for an axis
* set_status()
************************************************************/
static int set_status(int card, int signal)
{
register struct mess_info *motor_info;
char command[BUFF_SIZE];
char response[BUFF_SIZE];
struct mess_node *nodeptr;
int rtn_state, i, j;
long motorData;
long bytes[7];
char temp[5];
char buff[BUFF_SIZE];
bool ls_active = false;
msta_field status;
motor_info = &(motor_state[card]->motor_info[signal]);
nodeptr = motor_info->motor_motion;
status.All = motor_info->status.All;
/* Request the moving status of this motor */
/* MM4000 has a delay in case the motor is not done due to servo PID-related settling.. do we need this delay? */
/* ...after some testing, it doesn't look like we need the delay */
/* Get the motor status (ts) */
sprintf(command, "%c%dts", CTLA, signal);
send_mess(card, command, 0);
recv_mess(card, response, WAIT);
/* The response string is of the form "byte0 byte1 ... byte6" */
/* Convert ASCII characters to hex */
temp[0]='0'; temp[1]='x'; temp[2]='0', temp[3]='0', temp[4]='\0';
if (signal > 9) j = 5;
else j = 4;
for (i = 0; i < 7; i++) {
temp[2] = response[j];
temp[3] = response[j+1];
bytes[i] = strtol(temp, (char **)NULL, 0);
j += 3;
}
/* check to see if motor is moving */
status.Bits.RA_DONE = (bytes[0] & 0x04) ? 1 : 0;
/* check limits */
status.Bits.RA_PLUS_LS = status.Bits.RA_MINUS_LS = 0;
if ((bytes[5] & 0x04) & (bytes[3] & 0x04)) { /* if +lim AND pos move */
status.Bits.RA_PLUS_LS = 1;
ls_active = true;
}
if ((bytes[5] & 0x01) & !(bytes[3] & 0x04)) { /* if -lim AND neg move */
status.Bits.RA_MINUS_LS = 1;
ls_active = true;
}
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_POSITION = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
if ((bytes[3] & 0x08) | (bytes[3] & 0x40)) {
printf("drvMicos: set_status: EA_SLIP_STALL = 1, %ld\n", bytes[3]);
status.Bits.EA_SLIP_STALL = 1;
}
/* Request the position of this motor */
sprintf(command, "%c%dtp", CTLA, signal);
send_mess(card, command, 0);
recv_mess(card, response, WAIT);
/* The response string is of the form "0P0:+00001000" */
if (signal > 9)
motorData = atoi(&response[5]);
else
motorData = atoi(&response[4]);
/* derive direction information */
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
status.Bits.RA_DIRECTION = (bytes[3] & 0x04) ? 1 : 0;
motor_info->position = motorData;
motor_info->encoder_position = motorData;
motor_info->no_motion_count = 0;
}
/* Parse motor velocity? */
/* NEEDS WORK */
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
/* Test for post-move string. */
if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 &&
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char*) NULL);
/* The Micos will not send back a response for a 'set' command, don't need next line */
/* recv_mess(card, buff, WAIT); */
nodeptr->postmsgptr = NULL;
}
motor_info->status.All = status.All;
return (rtn_state);
}
/*****************************************************/
/* send a message to the Micos board */
/* send_mess() */
/*****************************************************/
static RTN_STATUS send_mess(int card, const char *com, char *name)
{
struct MicosController *cntrl;
int size;
size_t nwrite;
size = strlen(com);
/* Check that card exists */
if (!motor_state[card])
{
errlogPrintf("send_mess - invalid card #%d\n", card);
return(ERROR);
}
/* If the string is NULL just return */
if (size == 0) return(OK);
cntrl = (struct MicosController *) motor_state[card]->DevicePrivate;
Debug(2, "send_mess: sending message to card %d, message=%s\n", card, com);
cntrl = (struct MicosController *) motor_state[card]->DevicePrivate;
pasynOctetSyncIO->write(cntrl->pasynUser, com, size, COMM_TIMEOUT, &nwrite);
return (OK);
}
/*****************************************************/
/* Read a response string from the Micos board */
/* recv_mess() */
/*****************************************************/
static int recv_mess(int card, char *com, int flag)
{
struct MicosController *cntrl;
size_t nread = 0;
asynStatus status = asynError;
int eomReason;
/* Check that card exists */
if (!motor_state[card])
{
errlogPrintf("recv_mess - invalid card #%d\n", card);
return (-1);
}
cntrl = (struct MicosController *) motor_state[card]->DevicePrivate;
Debug(3, "recv_mess entry: card %d, flag=%d\n", card, flag);
if (flag == FLUSH)
pasynOctetSyncIO->flush(cntrl->pasynUser);
else
status = pasynOctetSyncIO->read(cntrl->pasynUser, com, BUFF_SIZE,
COMM_TIMEOUT, &nread, &eomReason);
if ((status != asynSuccess) || (nread <= 0))
{
com[0] = '\0';
nread = 0;
if (flag != FLUSH)
{
Debug(1, "recv_mess: card %d ERROR: no response\n", card);
}
}
else
{
Debug(2, "recv_mess: card %d, message = \"%s\"\n", card, com);
}
return (nread);
}
/*****************************************************/
/* Setup system configuration */
/* MicosSetup() */
/*****************************************************/
RTN_STATUS
MicosSetup(int num_cards, /* maximum number of "controllers" in system */
int num_channels, /* max number of drivers */
int scan_rate) /* polling rate - 1/60 sec units */
{
int itera;
if (num_cards < 1 || num_cards > MICOS_NUM_CARDS)
Micos_num_cards = MICOS_NUM_CARDS;
else
Micos_num_cards = num_cards;
if (num_channels < 1 || num_channels > MICOS_NUM_AXIS)
Micos_num_axis = MICOS_NUM_AXIS;
else
Micos_num_axis = num_channels;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= 60)
targs.motor_scan_rate = scan_rate;
else
targs.motor_scan_rate = SCAN_RATE;
/*
* Allocate space for motor_state structure pointers. Note this must be done
* before MicosConfig is called, so it cannot be done in motor_init()
* This means that we must allocate space for a card without knowing
* if it really exists, which is not a serious problem since this is just
* an array of pointers.
*/
motor_state = (struct controller **) malloc(Micos_num_cards *
sizeof(struct controller *));
for (itera = 0; itera < Micos_num_cards; itera++)
motor_state[itera] = (struct controller *) NULL;
return(OK);
}
/*****************************************************/
/* Configure a controller */
/* MicosConfig() */
/*****************************************************/
RTN_STATUS
MicosConfig(int card, /* "controller" being configured */
const char *name) /* asyn server task name */
{
struct MicosController *cntrl;
if (card < 0 || card >= Micos_num_cards)
return (ERROR);
motor_state[card] = (struct controller *) malloc(sizeof(struct controller));
motor_state[card]->DevicePrivate = malloc(sizeof(struct MicosController));
cntrl = (struct MicosController *) motor_state[card]->DevicePrivate;
strcpy(cntrl->asyn_port, name);
return(OK);
}
/*****************************************************/
/* initialize all software and hardware */
/* This is called from the initialization routine in */
/* device support. */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
struct controller *brdptr;
struct MicosController *cntrl;
int card_index, motor_index;
char cmd[BUFF_SIZE];
char buff[BUFF_SIZE];
int total_axis = 0;
int status = 0;
bool errind;
asynStatus success_rtn;
static const char output_terminator[] = "\r";
/* The response from the Micos is terminated with <CR><LF><ETX>. */
static const char input_terminator[] = "\r\n\3";
initialized = true; /* Indicate that driver is initialized. */
/* Check for setup */
if (Micos_num_cards <= 0)
{
Debug(1, "motor_init: *Micos driver disabled*\n");
Debug(1, "MicosSetup() is missing from startup script.\n");
return (ERROR);
}
for (card_index = 0; card_index < Micos_num_cards; card_index++)
{
if (!motor_state[card_index])
continue;
brdptr = motor_state[card_index];
total_cards = card_index + 1;
cntrl = (struct MicosController *) brdptr->DevicePrivate;
/* Initialize communications channel */
errind = false;
success_rtn = pasynOctetSyncIO->connect(cntrl->asyn_port, 0,
&cntrl->pasynUser, NULL);
if (success_rtn == asynSuccess)
{
int retry = 0;
pasynOctetSyncIO->setOutputEos(cntrl->pasynUser, output_terminator,
strlen(output_terminator));
pasynOctetSyncIO->setInputEos(cntrl->pasynUser, input_terminator,
strlen(input_terminator));
/* Each "controller" can have max 16 axes. */
total_axis = Micos_num_axis;
brdptr->total_axis = total_axis;
/* flush any junk at input port - should not be any data available */
do {
recv_mess(card_index, buff, FLUSH);
} while (strlen(buff) != 0);
/* Send a message to each Micos driver. See if it responds */
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
do
{
sprintf(cmd, "%c%dts", CTLA, motor_index);
send_mess(card_index, cmd, 0);
status = recv_mess(card_index, buff, WAIT);
retry++;
/* Return value is length of response string */
} while(status == 0 && retry < 3);
if (status == 0) break;
}
}
if (success_rtn == asynSuccess && status > 0)
{
brdptr->localaddr = (char *) NULL;
brdptr->motor_in_motion = 0;
brdptr->cmnd_response = false;
start_status(card_index);
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
struct mess_info *motor_info = &brdptr->motor_info[motor_index];
brdptr->motor_info[motor_index].motor_motion = NULL;
/* turn off echo */
sprintf(buff, "%c%def", CTLA, motor_index);
send_mess(card_index, buff, 0);
/* Don't turn on motor power, too dangerous */
/*sprintf(buff,"#%02dW=1", motor_index); */
/* send_mess(card_index, buff, 0); */
/* Stop motor */
sprintf(buff,"%c%dab1", CTLA, motor_index);
send_mess(card_index, buff, 0);
/* recv_mess(card_index, buff, WAIT); Throw away response */
strcpy(brdptr->ident, "MICOS");
motor_info->status.All = 0;
motor_info->no_motion_count = 0;
motor_info->encoder_position = 0;
motor_info->position = 0;
motor_info->encoder_present = YES;
motor_info->status.Bits.EA_PRESENT = 1;
motor_info->pid_present = YES;
motor_info->status.Bits.GAIN_SUPPORT = 1;
set_status(card_index, motor_index); /* Read status of each motor */
}
}
else
motor_state[card_index] = (struct controller *) NULL;
}
Debug(3, "motor_init: spawning motor task\n");
any_motor_in_motion = 0;
mess_queue.head = (struct mess_node *) NULL;
mess_queue.tail = (struct mess_node *) NULL;
free_list.head = (struct mess_node *) NULL;
free_list.tail = (struct mess_node *) NULL;
epicsThreadCreate((char *) "tMicos", epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) motor_task, (void *) &targs);
return (0);
}
motorApp/MicosSrc/drvMicos.h
-40
View File
@@ -1,40 +0,0 @@
/* File: drvMicos.h */
/* Device Driver Support definitions for Micos MoCo dc motor controller. */
/*
* Original Author: Kurt Goetze
* Current Author: Kurt Goetze
* Date: 11/24/2003
*
* Modification Log:
* -----------------
* .00 11/24/2003 kag initialized from drvMCB4B.h
* .01 07/12/2004 rls Converted from MPF to asyn.
*/
#ifndef INCdrvMicosh
#define INCdrvMicosh 1
#include "motor.h"
#include "motordrvCom.h"
#include "asynDriver.h"
#include "asynOctetSyncIO.h"
/* Micos default profile. */
#define MICOS_NUM_CARDS 16
#define MICOS_NUM_AXIS 16
#define CTLA 1
struct MicosController
{
asynUser *pasynUser; /* For RS-232 */
char asyn_port[80]; /* asyn port name */
};
/* Function prototypes. */
extern RTN_STATUS MicosSetup(int, int, int);
extern RTN_STATUS MicosConfig(int, const char *);
#endif /* INCdrvMicosh */
motorExApp/WithAsyn/Makefile
-2
View File
@@ -13,7 +13,6 @@ PROD_IOC_vxWorks = WithAsynVx
COMMONDBDS = base.dbd
COMMONDBDS += motorSupport.dbd
COMMONDBDS += devImsMotor.dbd
COMMONDBDS += devMicos.dbd
ifdef SNCSEQ
COMMONDBDS += devAerotechSeq.dbd
endif
@@ -61,7 +60,6 @@ WithAsynVx_SRCS += WithAsynVx_registerRecordDeviceDriver.cpp
WithAsyn_OBJS_vxWorks += $(EPICS_BASE_BIN)/vxComLibrary
COMMONLIBS += Aerotech
COMMONLIBS += Micos
COMMONLIBS += PI
COMMONLIBS += Ims
COMMONLIBS += softMotor