lin-epics-modules/motorHuber
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
9b0f02e1e8701f14408eadd76737501aff40fe8a
motorHuber/SMC9300Driver.cpp

451 lines
16 KiB
C++
Raw Blame History

/*
FILENAME... SMC9300Driver.cpp
USAGE... Motor driver support for the HUBER SMC9300 controller.
Yang Dongliang
March 1, 2020
Michele Brambilla
17 March, 2022
Trying to fix communication problems: Mark Koennecke
August, 2023
*/
#include "SMC9300Driver.h"
#include <asynOctetSyncIO.h>
#include <epicsExport.h>
#include <epicsThread.h>
#include <iocsh.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <bitset>
#include <errlog.h>
#define STEPS_PER_EGU 10000.
#define NINT(f) (int)((f) > 0 ? (f) + 0.5 : (f)-0.5)
#define CHECK_BIT(var,pos) ((var) & (1 << pos))
#
/** Creates a new SMC9300Controller object.
* \param[in] portName The name of the asyn port that will be created
* for this driver \param[in] SMC9300PortName The name of the
* drvAsynSerialPort that was created previously to connect to the SMC9300
* 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.
*/
/* Michele Brambilla
* Add \param[in] extraParams to the constructor. The default = 1 accounts for
* the motor message
*/
SMC9300Controller::SMC9300Controller(const char *portName,
const char *SMC9300PortName, int numAxes,
double movingPollPeriod,
double idlePollPeriod,
const int &extraParams = 1)
: asynMotorController(
portName, numAxes, NUM_SMC9300_PARAMS + extraParams,
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;
SMC9300Axis *pAxis;
static const char *functionName = "SMC9300Controller::SMC9300Controller";
/* Connect to SMC9300 controller */
status = pasynOctetSyncIO->connect(SMC9300PortName, 0, &pasynUserController_,
NULL);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: cannot connect to Huber_SMC9300 controller\n", functionName);
}
for (axis = 1; axis < numAxes; axis++) {
pAxis = new SMC9300Axis(this, axis);
}
startPoller(movingPollPeriod, idlePollPeriod, 2);
createParam(motorMessageTextString, asynParamOctet, &motorMessageText_);
}
asynStatus SMC9300Controller::transact(void)
{
size_t nread, in;
int reason;
float timeout = 5;
/* read with a short timeout in order to remove stray messages
* from the line. This also serves to slow down communication
*/
pasynOctetSyncIO->read(pasynUserController_, inString_, sizeof(inString_), .01, &in, &reason);
return writeReadController(outString_, inString_, sizeof(inString_), &nread, timeout);
}
/** Creates a new SMC9300Controller 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] SMC9300PortName The name of the
* drvAsynIPPPort that was created previously to connect to the SMC9300
* 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 SMC9300CreateController(const char *portName,
const char *SMC9300PortName, int numAxes,
int movingPollPeriod,
int idlePollPeriod) {
SMC9300Controller *pSMC9300Controller =
new SMC9300Controller(portName, SMC9300PortName, numAxes,
movingPollPeriod / 1000., idlePollPeriod / 1000.);
pSMC9300Controller = 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 SMC9300Controller::report(FILE *fp, int level) {
fprintf(fp,
"Huber_SMC9300 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 SMC9300Axis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number.
*/
SMC9300Axis *SMC9300Controller::getAxis(asynUser *pasynUser) {
return static_cast<SMC9300Axis *>(asynMotorController::getAxis(pasynUser));
}
/** Returns a pointer to an SMC9300Axis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] axisNo Axis index number. */
SMC9300Axis *SMC9300Controller::getAxis(int axisNo) {
return static_cast<SMC9300Axis *>(asynMotorController::getAxis(axisNo));
}
// These are the SMC9300Axis methods
/** Creates a new SMC9300Axis object.
* \param[in] pC Pointer to the SMC9300Controller to which this axis belongs.
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
*
* Initializes register numbers, etc.
*/
SMC9300Axis::SMC9300Axis(SMC9300Controller *pC, int axisNo)
: asynMotorAxis(pC, axisNo), pC_(pC) {
static const char *functionName = "SMC9300Axis::SMC9300Axis";
bool moving;
// Do an initial poll to get some values from the axis
if (poll(&moving) != asynSuccess) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: getAxisInitialStatus failed to return asynSuccess. "
"Controller: %s, Axis: %d.\n",
functionName, pC_->portName, axisNo);
}
}
/** 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 SMC9300Axis::report(FILE *fp, int level) {
if (level > 0) {
fprintf(fp, " axis %d\n", axisNo_);
}
// Call the base class method
asynMotorAxis::report(fp, level);
}
// asynStatus SMC9300Axis::sendAccelAndVelocity(double acceleration, double
// velocity)
// {
// asynStatus status;
// int ival;
// // static const char *functionName = "SMC9300::sendAccelAndVelocity";
// // Send the velocity
// // ival = NINT(fabs(115200./velocity));
// // if (ival < 2) ival=5000;
// // if (ival > 2) ival=5000;
// ival = 5000;
// if (axisNo_ == 1) ival=5000;
// if (axisNo_ == 2) ival=30000;
// if (axisNo_ == 3) ival=30000;
// if (axisNo_ == 4) ival=30000;
// sprintf(pC_->outString_, "ffast%d:%d", axisNo_, ival);
// status = pC_->writeController();
// // Send the acceleration
// // acceleration is in steps/sec/sec
// // SMC is programmed with Ramp Index (R) where:
// // dval (steps/sec/sec) = 720,000/(256-R) */
// // or R=256-(720,000/dval) */
// // ival = NINT(256-(720000./acceleration));
// // if (ival < 1) ival=20;
// // if (ival > 1) ival=20;
// ival = 20;
// if (axisNo_ == 1) ival=20;
// if (axisNo_ == 2) ival=80;
// if (axisNo_ == 3) ival=80;
// if (axisNo_ == 4) ival=80;
// sprintf(pC_->outString_, "acc%d:%d", axisNo_, ival);
// status = pC_->writeController();
// return status;
// }
asynStatus SMC9300Axis::move(double position, int relative, double minVelocity,
double maxVelocity, double acceleration) {
asynStatus status;
// static const char *functionName = "SMC9300Axis::move";
// status = sendAccelAndVelocity(acceleration, maxVelocity);
if (relative) {
sprintf(pC_->outString_, "move%d:%f", axisNo_, position / STEPS_PER_EGU);
} else {
sprintf(pC_->outString_, "goto%d:%f", axisNo_, position / STEPS_PER_EGU);
}
status = pC_->writeController();
// The controller needs a while before it can accept more commands
usleep(300);
return status;
}
asynStatus SMC9300Axis::home(double minVelocity, double maxVelocity,
double acceleration, int forwards) {
asynStatus status;
// static const char *functionName = "SMC9300Axis::home";
// status = sendAccelAndVelocity(acceleration, maxVelocity);
if (forwards) {
sprintf(pC_->outString_, "home%d:hs", axisNo_);
} else {
sprintf(pC_->outString_, "home%d:hs", axisNo_);
}
status = pC_->writeController();
// The controller needs a while before it can accept more commands
usleep(300);
return status;
}
asynStatus SMC9300Axis::moveVelocity(double minVelocity, double maxVelocity,
double acceleration) {
asynStatus status;
static const char *functionName = "SMC9300Axis::moveVelocity";
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
functionName, minVelocity, maxVelocity, acceleration);
// status = sendAccelAndVelocity(acceleration, maxVelocity);
/* SMC-9300 does not have jog command. Move 1 million steps */
if (maxVelocity > 0.) {
/* This is a positive move in SMC9300 coordinates */
sprintf(pC_->outString_, "move%d:0.1", axisNo_);
} else {
/* This is a negative move in SMC9300 coordinates */
sprintf(pC_->outString_, "move%d:-0.1", axisNo_);
}
status = pC_->writeController();
// The controller needs a while before it can accept more commands
usleep(300);
return status;
}
asynStatus SMC9300Axis::stop(double acceleration) {
asynStatus status;
// static const char *functionName = "SMC9300Axis::stop";
sprintf(pC_->outString_, "q%d", axisNo_);
status = pC_->writeController();
// The controller needs a while before it can accept more commands
usleep(300);
return status;
}
asynStatus SMC9300Axis::setPosition(double position) {
asynStatus status;
// static const char *functionName = "SMC9300Axis::setPosition";
sprintf(pC_->outString_, "pos%d:%f", axisNo_, position / STEPS_PER_EGU);
status = pC_->writeController();
// The controller needs a while before it can accept more commands
usleep(300);
return status;
}
asynStatus SMC9300Axis::setClosedLoop(bool closedLoop) {
asynStatus status;
// static const char *functionName = "SMC9300Axis::setClosedLoop";
sprintf(pC_->outString_, "ecl%d:%d", axisNo_, closedLoop ? 0 : 1);
status = pC_->writeController();
// The controller needs a while before it can accept more commands
usleep(300);
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 SMC9300Axis::poll(bool *moving) {
int done;
int axisNo;
int nvals;
int value;
double position;
asynStatus comStatus;
static const char *functionName = "SMC9300Controller::poll";
char errString[1024];
updateMsgTxtFromDriver("");
// Read the current motor position
sprintf(pC_->outString_, "?p%d", axisNo_);
comStatus = pC_->transact();
nvals = sscanf(pC_->inString_, "%d:%lf", &axisNo, &position);
if (comStatus || nvals != 2) {
asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvSMC9300AxisGetStatus: Failed to read position\nStatus: %d"
"\nCommand :%s\nResponse:%s\n",
comStatus, pC_->outString_, pC_->inString_);
}
if (axisNo == axisNo_) {
setDoubleParam(pC_->motorPosition_, position * STEPS_PER_EGU);
}
// Read the current motor encoder position
sprintf(pC_->outString_, "?e%d", axisNo_);
comStatus = pC_->transact();
nvals = sscanf(pC_->inString_, "%d:%lf", &axisNo, &position);
if (comStatus || nvals != 2) {
asynPrint(
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvSMC9300AxisGetStatus: Failed to read encoder position\nStatus: %d"
"\nCommand :%s\nResponse:%s\n",
comStatus, pC_->outString_, pC_->inString_);
}
if (axisNo == axisNo_) {
setDoubleParam(pC_->motorEncoderPosition_, position * STEPS_PER_EGU);
}
// Read the moving status of this motor
sprintf(pC_->outString_, "?s%d", axisNo_);
comStatus = pC_->transact();
nvals = sscanf(pC_->inString_, "%d:%d", &axisNo, &value);
if (comStatus || nvals != 2) {
asynPrint(
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvSMC9300AxisGetStatus: Failed to read axis moving status\nStatus: %d"
"\nCommand :%s\nResponse:%s\n",
comStatus, pC_->outString_, pC_->inString_);
}
if (axisNo == axisNo_) {
done = value & 0x1;
setIntegerParam(pC_->motorStatusDone_, done);
*moving = done ? false : true;
setIntegerParam(pC_->motorStatusHighLimit_, value >> 2 & 0x1);
setIntegerParam(pC_->motorStatusLowLimit_, value >> 3 & 0x1);
setIntegerParam(pC_->motorStatusAtHome_, 0);
setIntegerParam(pC_->motorStatusPowerOn_, value >> 7 & 0x1);
setIntegerParam(pC_->motorStatusProblem_, 0);
// setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1 : 0);
if(CHECK_BIT(value, 15)) {
// Read errors from controller
sprintf(pC_->outString_, "?err%d", axisNo_);
comStatus = pC_->transact();
nvals = sscanf(pC_->inString_, "%d:%d %[^NULL]", &axisNo, &value, errString);
if (comStatus || (nvals != 3 && nvals != 2)) {
asynPrint(
pC_->pasynUserSelf, ASYN_TRACE_ERROR,
"drvSMC9300AxisGetStatus: Failed to read error message\nStatus: %d"
"\nCommand :%s\nResponse:%s\n",
comStatus, pC_->outString_, pC_->inString_);
}
if (axisNo == axisNo_) {
updateMsgTxtFromDriver(errString);
setIntegerParam(pC_->motorStatusProblem_, 1);
errlogPrintf("SMC9300 motor %d reported %s\n", axisNo_, errString);
return asynError;
}
}
}
comStatus = callParamCallbacks();
return comStatus ? asynError : asynSuccess;
}
asynStatus SMC9300Axis::updateMsgTxtFromDriver(const char *value) {
return asynSuccess;
// if (value && value[0]) {
// return setStringParam(pC_->motorMessageText_, value);
//}
//return setStringParam(pC_->motorMessageText_, "");
}
/** Code for iocsh registration */
static const iocshArg SMC9300CreateControllerArg0 = {"Port name",
iocshArgString};
static const iocshArg SMC9300CreateControllerArg1 = {"SMC9300 port name",
iocshArgString};
static const iocshArg SMC9300CreateControllerArg2 = {"Number of axes",
iocshArgInt};
static const iocshArg SMC9300CreateControllerArg3 = {"Moving poll period (ms)",
iocshArgInt};
static const iocshArg SMC9300CreateControllerArg4 = {"Idle poll period (ms)",
iocshArgInt};
static const iocshArg *const SMC9300CreateControllerArgs[] = {
&SMC9300CreateControllerArg0, &SMC9300CreateControllerArg1,
&SMC9300CreateControllerArg2, &SMC9300CreateControllerArg3,
&SMC9300CreateControllerArg4};
static const iocshFuncDef SMC9300CreateControllerDef = {
"SMC9300CreateController", 5, SMC9300CreateControllerArgs};
static void SMC9300CreateContollerCallFunc(const iocshArgBuf *args) {
SMC9300CreateController(args[0].sval, args[1].sval, args[2].ival,
args[3].ival, args[4].ival);
}
static void SMC9300Register(void) {
iocshRegister(&SMC9300CreateControllerDef, SMC9300CreateContollerCallFunc);
}
extern "C" {
epicsExportRegistrar(SMC9300Register);
}
Reference in New Issue View Git Blame Copy Permalink