mirror of
https://github.com/Binary-Coalescence/motorDSM.git
synced 2025-08-04 21:55:40 -05:00
New file, Model 3 driver
This commit is contained in:
MarkRivers
334
acsApp/src/MCB4BDriver.cpp
Normal file
334
acsApp/src/MCB4BDriver.cpp
Normal file
@@ -0,0 +1,334 @@
|
|||||||
|
/*
|
||||||
|
FILENAME... MCB4BDriver.cpp
|
||||||
|
USAGE... Motor driver support for the ACS MCB-4B controller.
|
||||||
|
|
||||||
|
Mark Rivers
|
||||||
|
March 1, 2012
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include <iocsh.h>
|
||||||
|
#include <epicsThread.h>
|
||||||
|
|
||||||
|
#include <asynOctetSyncIO.h>
|
||||||
|
|
||||||
|
#include "MCB4BDriver.h"
|
||||||
|
#include <epicsExport.h>
|
||||||
|
|
||||||
|
#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5)
|
||||||
|
|
||||||
|
/** Creates a new MCB4BController object.
|
||||||
|
* \param[in] portName The name of the asyn port that will be created for this driver
|
||||||
|
* \param[in] MCB4BPortName The name of the drvAsynSerialPort that was created previously to connect to the MCB4B 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
|
||||||
|
*/
|
||||||
|
MCB4BController::MCB4BController(const char *portName, const char *MCB4BPortName, int numAxes,
|
||||||
|
double movingPollPeriod, double idlePollPeriod)
|
||||||
|
: asynMotorController(portName, numAxes, NUM_MCB4B_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;
|
||||||
|
MCB4BAxis *pAxis;
|
||||||
|
static const char *functionName = "MCB4BController::MCB4BController";
|
||||||
|
|
||||||
|
/* Connect to MCB4B controller */
|
||||||
|
status = pasynOctetSyncIO->connect(MCB4BPortName, 0, &pasynUserController_, NULL);
|
||||||
|
if (status) {
|
||||||
|
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||||
|
"%s: cannot connect to MCB-4B controller\n",
|
||||||
|
functionName);
|
||||||
|
}
|
||||||
|
for (axis=0; axis<numAxes; axis++) {
|
||||||
|
pAxis = new MCB4BAxis(this, axis);
|
||||||
|
}
|
||||||
|
|
||||||
|
startPoller(movingPollPeriod, idlePollPeriod, 2);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/** Creates a new MCB4BController 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] MCB4BPortName The name of the drvAsynIPPPort that was created previously to connect to the MCB4B 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 MCB4BCreateController(const char *portName, const char *MCB4BPortName, int numAxes,
|
||||||
|
int movingPollPeriod, int idlePollPeriod)
|
||||||
|
{
|
||||||
|
MCB4BController *pMCB4BController
|
||||||
|
= new MCB4BController(portName, MCB4BPortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
|
||||||
|
pMCB4BController = 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 MCB4BController::report(FILE *fp, int level)
|
||||||
|
{
|
||||||
|
fprintf(fp, "MCB-4B 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 MCB4BAxis object.
|
||||||
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
||||||
|
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
|
||||||
|
MCB4BAxis* MCB4BController::getAxis(asynUser *pasynUser)
|
||||||
|
{
|
||||||
|
return static_cast<MCB4BAxis*>(asynMotorController::getAxis(pasynUser));
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Returns a pointer to an MCB4BAxis object.
|
||||||
|
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
||||||
|
* \param[in] axisNo Axis index number. */
|
||||||
|
MCB4BAxis* MCB4BController::getAxis(int axisNo)
|
||||||
|
{
|
||||||
|
return static_cast<MCB4BAxis*>(asynMotorController::getAxis(axisNo));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// These are the MCB4BAxis methods
|
||||||
|
|
||||||
|
/** Creates a new MCB4BAxis object.
|
||||||
|
* \param[in] pC Pointer to the MCB4BController to which this axis belongs.
|
||||||
|
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
|
||||||
|
*
|
||||||
|
* Initializes register numbers, etc.
|
||||||
|
*/
|
||||||
|
MCB4BAxis::MCB4BAxis(MCB4BController *pC, int axisNo)
|
||||||
|
: asynMotorAxis(pC, axisNo),
|
||||||
|
pC_(pC)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/** 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 calls asynMotorController::report()
|
||||||
|
*/
|
||||||
|
void MCB4BAxis::report(FILE *fp, int level)
|
||||||
|
{
|
||||||
|
if (level > 0) {
|
||||||
|
fprintf(fp, " axis %d\n",
|
||||||
|
axisNo_);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Call the base class method
|
||||||
|
asynMotorAxis::report(fp, level);
|
||||||
|
}
|
||||||
|
|
||||||
|
asynStatus MCB4BAxis::sendAccelAndVelocity(double acceleration, double velocity)
|
||||||
|
{
|
||||||
|
asynStatus status;
|
||||||
|
int ival;
|
||||||
|
// static const char *functionName = "MCB4B::sendAccelAndVelocity";
|
||||||
|
|
||||||
|
// Send the velocity
|
||||||
|
ival = NINT(fabs(115200./velocity));
|
||||||
|
if (ival < 2) ival=2;
|
||||||
|
if (ival > 255) ival = 255;
|
||||||
|
sprintf(pC_->outString_, "#%02dV=%d", axisNo_, ival);
|
||||||
|
status = pC_->writeController();
|
||||||
|
|
||||||
|
// Send the acceleration
|
||||||
|
// acceleration is in steps/sec/sec
|
||||||
|
// MCB 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=1;
|
||||||
|
if (ival > 255) ival=255;
|
||||||
|
sprintf(pC_->outString_, "#%02dR=%d", axisNo_, ival);
|
||||||
|
status = pC_->writeController();
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
asynStatus MCB4BAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
||||||
|
{
|
||||||
|
asynStatus status;
|
||||||
|
// static const char *functionName = "MCB4BAxis::move";
|
||||||
|
|
||||||
|
status = sendAccelAndVelocity(acceleration, maxVelocity);
|
||||||
|
|
||||||
|
if (relative) {
|
||||||
|
sprintf(pC_->outString_, "#%02dI%+d", axisNo_, NINT(position));
|
||||||
|
} else {
|
||||||
|
sprintf(pC_->outString_, "#%02dG%+d", axisNo_, NINT(position));
|
||||||
|
}
|
||||||
|
status = pC_->writeController();
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
asynStatus MCB4BAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
|
||||||
|
{
|
||||||
|
asynStatus status;
|
||||||
|
// static const char *functionName = "MCB4BAxis::home";
|
||||||
|
|
||||||
|
status = sendAccelAndVelocity(acceleration, maxVelocity);
|
||||||
|
|
||||||
|
if (forwards) {
|
||||||
|
sprintf(pC_->outString_, "#%02dH+", axisNo_);
|
||||||
|
} else {
|
||||||
|
sprintf(pC_->outString_, "#%02dH-", axisNo_);
|
||||||
|
}
|
||||||
|
status = pC_->writeController();
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
asynStatus MCB4BAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
|
||||||
|
{
|
||||||
|
asynStatus status;
|
||||||
|
// static const char *functionName = "MCB4BAxis::moveVelocity";
|
||||||
|
|
||||||
|
status = sendAccelAndVelocity(acceleration, maxVelocity);
|
||||||
|
|
||||||
|
/* MCB-4B does not have jog command. Move 1 million steps */
|
||||||
|
if (maxVelocity > 0.) {
|
||||||
|
/* This is a positive move in MCB4B coordinates */
|
||||||
|
sprintf(pC_->outString_, "#%02dM+1000000", axisNo_);
|
||||||
|
} else {
|
||||||
|
/* This is a negative move in MCB4B coordinates */
|
||||||
|
sprintf(pC_->outString_, "#%02dM-1000000", axisNo_);
|
||||||
|
}
|
||||||
|
status = pC_->writeController();
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
asynStatus MCB4BAxis::stop(double acceleration )
|
||||||
|
{
|
||||||
|
asynStatus status;
|
||||||
|
//static const char *functionName = "MCB4BAxis::stop";
|
||||||
|
|
||||||
|
sprintf(pC_->outString_, "#%02dQ", axisNo_);
|
||||||
|
status = pC_->writeController();
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
asynStatus MCB4BAxis::setPosition(double position)
|
||||||
|
{
|
||||||
|
asynStatus status;
|
||||||
|
//static const char *functionName = "MCB4BAxis::setPosition";
|
||||||
|
|
||||||
|
sprintf(pC_->outString_, "#%02dP=%+d", axisNo_, NINT(position));
|
||||||
|
status = pC_->writeController();
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
asynStatus MCB4BAxis::setClosedLoop(bool closedLoop)
|
||||||
|
{
|
||||||
|
asynStatus status;
|
||||||
|
//static const char *functionName = "MCB4BAxis::setClosedLoop";
|
||||||
|
|
||||||
|
sprintf(pC_->outString_, "#%02dW=%d", axisNo_, closedLoop ? 1:0);
|
||||||
|
status = pC_->writeController();
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
/** Polls the axis.
|
||||||
|
* This function reads the controller position, encoder position, the limit status, the moving status,
|
||||||
|
* and the drive power-on status. It does not current detect following error, etc. but this could be
|
||||||
|
* added.
|
||||||
|
* 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 (1) or done (0). */
|
||||||
|
asynStatus MCB4BAxis::poll(bool *moving)
|
||||||
|
{
|
||||||
|
int done;
|
||||||
|
int driveOn;
|
||||||
|
int limit;
|
||||||
|
double position;
|
||||||
|
asynStatus comStatus;
|
||||||
|
|
||||||
|
// Read the current motor position
|
||||||
|
sprintf(pC_->outString_, "#%02dP", axisNo_);
|
||||||
|
comStatus = pC_->writeReadController();
|
||||||
|
if (comStatus) goto skip;
|
||||||
|
// The response string is of the form "#01P=+1000"
|
||||||
|
position = atof(&pC_->inString_[5]);
|
||||||
|
setDoubleParam(pC_->motorPosition_, position);
|
||||||
|
|
||||||
|
// Read the moving status of this motor
|
||||||
|
sprintf(pC_->outString_, "#%02dX", axisNo_);
|
||||||
|
comStatus = pC_->writeReadController();
|
||||||
|
if (comStatus) goto skip;
|
||||||
|
// The response string is of the form "#01X=1"
|
||||||
|
done = (pC_->inString_[5] == '0') ? 1:0;
|
||||||
|
setIntegerParam(pC_->motorStatusDone_, done);
|
||||||
|
*moving = done ? false:true;
|
||||||
|
|
||||||
|
// Read the limit status
|
||||||
|
sprintf(pC_->outString_, "#%02dE", axisNo_);
|
||||||
|
comStatus = pC_->writeReadController();
|
||||||
|
if (comStatus) goto skip;
|
||||||
|
// The response string is of the form "#01E=1"
|
||||||
|
limit = (pC_->inString_[5] == '1') ? 1:0;
|
||||||
|
setIntegerParam(pC_->motorStatusHighLimit_, limit);
|
||||||
|
limit = (pC_->inString_[6] == '1') ? 1:0;
|
||||||
|
setIntegerParam(pC_->motorStatusLowLimit_, limit);
|
||||||
|
limit = (pC_->inString_[7] == '1') ? 1:0;
|
||||||
|
setIntegerParam(pC_->motorStatusAtHome_, limit);
|
||||||
|
|
||||||
|
// Read the drive power on status
|
||||||
|
sprintf(pC_->outString_, "#%02dE", axisNo_);
|
||||||
|
comStatus = pC_->writeReadController();
|
||||||
|
if (comStatus) goto skip;
|
||||||
|
driveOn = (pC_->inString_[5] == '1') ? 1:0;
|
||||||
|
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 MCB4BCreateControllerArg0 = {"Port name", iocshArgString};
|
||||||
|
static const iocshArg MCB4BCreateControllerArg1 = {"MCB-4B port name", iocshArgString};
|
||||||
|
static const iocshArg MCB4BCreateControllerArg2 = {"Number of axes", iocshArgInt};
|
||||||
|
static const iocshArg MCB4BCreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt};
|
||||||
|
static const iocshArg MCB4BCreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt};
|
||||||
|
static const iocshArg * const MCB4BCreateControllerArgs[] = {&MCB4BCreateControllerArg0,
|
||||||
|
&MCB4BCreateControllerArg1,
|
||||||
|
&MCB4BCreateControllerArg2,
|
||||||
|
&MCB4BCreateControllerArg3,
|
||||||
|
&MCB4BCreateControllerArg4};
|
||||||
|
static const iocshFuncDef MCB4BCreateControllerDef = {"MCB4BCreateController", 5, MCB4BCreateControllerArgs};
|
||||||
|
static void MCB4BCreateContollerCallFunc(const iocshArgBuf *args)
|
||||||
|
{
|
||||||
|
MCB4BCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void MCB4BRegister(void)
|
||||||
|
{
|
||||||
|
iocshRegister(&MCB4BCreateControllerDef, MCB4BCreateContollerCallFunc);
|
||||||
|
}
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
epicsExportRegistrar(MCB4BRegister);
|
||||||
|
}
|
||||||
49
acsApp/src/MCB4BDriver.h
Normal file
49
acsApp/src/MCB4BDriver.h
Normal file
@@ -0,0 +1,49 @@
|
|||||||
|
/*
|
||||||
|
FILENAME... MCB4BDriver.h
|
||||||
|
USAGE... Motor driver support for the ACS MCB-4B controller.
|
||||||
|
|
||||||
|
Mark Rivers
|
||||||
|
March 1, 2012
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "asynMotorController.h"
|
||||||
|
#include "asynMotorAxis.h"
|
||||||
|
|
||||||
|
#define MAX_MCB4B_AXES 4
|
||||||
|
|
||||||
|
// No controller-specific parameters yet
|
||||||
|
#define NUM_MCB4B_PARAMS 0
|
||||||
|
|
||||||
|
class MCB4BAxis : public asynMotorAxis
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/* These are the methods we override from the base class */
|
||||||
|
MCB4BAxis(class MCB4BController *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:
|
||||||
|
MCB4BController *pC_; /**< Pointer to the asynMotorController to which this axis belongs.
|
||||||
|
* Abbreviated because it is used very frequently */
|
||||||
|
asynStatus sendAccelAndVelocity(double accel, double velocity);
|
||||||
|
|
||||||
|
friend class MCB4BController;
|
||||||
|
};
|
||||||
|
|
||||||
|
class MCB4BController : public asynMotorController {
|
||||||
|
public:
|
||||||
|
MCB4BController(const char *portName, const char *MCB4BPortName, int numAxes, double movingPollPeriod, double idlePollPeriod);
|
||||||
|
|
||||||
|
void report(FILE *fp, int level);
|
||||||
|
MCB4BAxis* getAxis(asynUser *pasynUser);
|
||||||
|
MCB4BAxis* getAxis(int axisNo);
|
||||||
|
|
||||||
|
friend class MCB4BAxis;
|
||||||
|
};
|
||||||
Reference in New Issue
Block a user