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

Merge branch 'master' of github.com:epics-modules/motor

Browse Source
This commit is contained in:
Mark Rivers
2017-07-19 12:02:40 -05:00
parent 7324d917dc 8966aeabf2
commit dc8e0f0da0
10 changed files with 998 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
iocBoot/iocWithAsyn/motor.substitutions.ANG1
+17
View File
@@ -0,0 +1,17 @@
file "$(MOTOR)/motorApp/Db/basic_asyn_motor.db"
{
pattern
{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT, RTRY}
{IOC:, 1, "m$(N)", "asynMotor", "ANG1_1_1", 0, "AMCI ANG1 1", steps, Pos, 100, 0, .2, 0, 50, .2, 1, 5, 1e9, -1e9, ""}
{IOC:, 2, "m$(N)", "asynMotor", "ANG1_1_2", 0, "AMCI ANG1 2", steps, Pos, 100, 0, .2, 0, 50, .2, 1, 5, 1e9, -1e9, ""}
}
file "$(MOTOR)/motorApp/Db/ANG1Aux.template"
{
pattern
{P, R, PORT, ADDR}
{IOC:, m1:, ANG1_1_1, 0}
{IOC:, m2:, ANG1_1_2, 0}
}
iocBoot/iocWithAsyn/st.cmd.ANG1
+74
View File
@@ -0,0 +1,74 @@
# ANG1 motors Command file Example
#
### Note: Modbus support (the EPICS modbus module) is required to be included in the
### EPICS application where the ANG1 support will be loaded. This file is an
### example of how to load the ANG1 support, in an ioc that is built with the
### EPICS modbus module.
# Use the following commands for TCP/IP
#drvAsynIPPortConfigure(const char *portName,
# const char *hostInfo,
# unsigned int priority,
# int noAutoConnect,
# int noProcessEos);
# One per controller. One controller can support up to six drivers.
drvAsynIPPortConfigure("ang1_1","164.54.53.107:502",0,0,1)
#drvAsynIPPortConfigure("ang1_2","164.54.53.23:502",0,0,1)
#drvAsynIPPortConfigure("ang1_3","164.54.xxx.xxx:502",0,0,1)
#modbusInterposeConfig(const char *portName,
# modbusLinkType linkType,
# int timeoutMsec,
# int writeDelayMsec)
# One per controller. One controller can support up to six drivers.
modbusInterposeConfig("ang1_1",0,2000,0)
#modbusInterposeConfig("ang1_2",0,2000,0)
# Word access at Modbus address 0
# Access 1 words as inputs.
# Function code=3
# default data type unsigned integer.
# drvModbusAsynConfigure("portName", "tcpPortName", slaveAddress, modbusFunction, modbusStartAddress, modbusLength, dataType, pollMsec, "plcType")
# One per axis. Note: "ANG1" is the AMCI model. "ANG1_1" is the controller. "ANG1_1_1" is the axis.
drvModbusAsynConfigure("ANG1_1_1_In_Word", "ang1_1", 0, 4, 0, 10, 0, 100, "ANG1_stepper")
drvModbusAsynConfigure("ANG1_1_2_In_Word", "ang1_1", 0, 4, 10, 10, 0, 100, "ANG1_stepper")
#drvModbusAsynConfigure("ANG1_1_3_In_Word", "ang1_1", 0, 3, 20, 10, 0, 100, "ANG1_stepper")
#drvModbusAsynConfigure("ANG1_1_4_In_Word", "ang1_1", 0, 3, 30, 10, 0, 100, "ANG1_stepper")
#drvModbusAsynConfigure("ANG1_1_5_In_Word", "ang1_1", 0, 3, 40, 10, 0, 100, "ANG1_stepper")
#drvModbusAsynConfigure("ANG1_1_6_In_Word", "ang1_1", 0, 3, 50, 10, 0, 100, "ANG1_stepper")
# Access 1 words as outputs.
# Either function code=6 (single register) or 16 (multiple registers) can be used, but 16
# is better because it is "atomic" when writing values longer than 16-bits.
# Default data type unsigned integer.
# drvModbusAsynConfigure("portName", "tcpPortName", slaveAddress, modbusFunction, modbusStartAddress, modbusLength, dataType, pollMsec, "plcType")
# Not sure why the outputs can't be configured for Modbus data type 4?
drvModbusAsynConfigure("ANG1_1_1_Out_Word", "ang1_1", 0, 6, 1024, 10, 0, 1, "ANG1_stepper")
drvModbusAsynConfigure("ANG1_1_2_Out_Word", "ang1_1", 0, 6, 1034, 10, 0, 1, "ANG1_stepper")
#drvModbusAsynConfigure("ANG1_1_3_Out_Word", "ang1_1", 0, 6, 1044, 10, 0, 1, "ANG1_stepper")
#drvModbusAsynConfigure("ANG1_1_4_Out_Word", "ang1_1", 0, 6, 1054, 10, 0, 1, "ANG1_stepper")
#drvModbusAsynConfigure("ANG1_1_5_Out_Word", "ang1_1", 0, 6, 1064, 10, 0, 1, "ANG1_stepper")
#drvModbusAsynConfigure("ANG1_1_6_Out_Word", "ang1_1", 0, 6, 1074, 10, 0, 1, "ANG1_stepper")
# Second ANG1 controller...
#drvModbusAsynConfigure("ANG1_2_1_Out_Word_0", "ang1_2", 0, 6, 1024, 1, 0, 1, "ANG1_stepper")
dbLoadTemplate("ANG1_motors.substitutions")
# AMCI ANG1 stepper controller/driver support
# portName The name of the asyn port that will be created for this driver
# ANG1InPortName The name of the In drvAsynIPPPort that was created previously to connect to the ANG1 controller
# ANG1OutPortName The name of the Out drvAsynIPPPort that was created previously to connect to the ANG1 controller
# numAxes The number of axes that this controller supports
# movingPollPeriod The time in ms between polls when any axis is moving
# idlePollPeriod The time in ms between polls when no axis is moving
#
# One per axis. Note: "ANG1" is the AMCI model. "ANG1_1" is the controller. "ANG1_1_1" is the axis.
# Also, ANG1_1_1 is the port name needed for motor.substitutions.
ANG1CreateController(ANG1_1_1, ANG1_1_1_In_Word, ANG1_1_1_Out_Word, 1, 100, 0)
ANG1CreateController(ANG1_1_2, ANG1_1_2_In_Word, ANG1_1_2_Out_Word, 1, 100, 0)
#ANG1CreateController(ANG1_1, ANG1_1_3_In_Word, ANG1_1_3_Out_Word, 1, 100, 0)
#ANG1CreateController(ANG1_1, ANG1_1_4_In_Word, ANG1_1_4_Out_Word, 1, 100, 0)
#ANG1CreateController(ANG1_1, ANG1_1_5_In_Word, ANG1_1_5_Out_Word, 1, 100, 0)
#ANG1CreateController(ANG1_1, ANG1_1_6_In_Word, ANG1_1_6_Out_Word, 1, 100, 0)
motorApp/AMCISrc/AMCIRegister.cc
+52
View File
@@ -0,0 +1,52 @@
/*
FILENAME... AMCIRegister.cc
USAGE... Register AMCI motor device driver shell commands.
Version: $Revision: 1.4 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-07-16 19:06:58 $
*/
/*****************************************************************
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 <iocsh.h>
#include "AMCIRegister.h"
#include "epicsExport.h"
extern "C"
{
// AMCI ANG1 Setup arguments
static const iocshArg setupArg0 = {"asyn port name", iocshArgString};
static const iocshArg setupArg1 = {"Input port name", iocshArgString};
static const iocshArg setupArg2 = {"Output port name", iocshArgString};
static const iocshArg setupArg3 = {"Number of axes", iocshArgInt};
static const iocshArg setupArg4 = {"Polling rate moving", iocshArgInt};
static const iocshArg setupArg5 = {"Polling rate idle", iocshArgInt};
static const iocshArg * const ANG1SetupArgs[6] = {&setupArg0, &setupArg1, &setupArg2, &setupArg3, &setupArg4, &setupArg5};
static const iocshFuncDef setupANG1 = {"ANG1Setup", 6, ANG1SetupArgs};
static void setupANG1CallFunc(const iocshArgBuf *args)
{
ANG1Setup(args[0].sval, args[1].sval, args[2].sval, args[3].ival, args[4].ival, args[5].ival);
}
static void AMCIRegister(void)
{
iocshRegister(&setupANG1, setupANG1CallFunc);
}
epicsExportRegistrar(AMCIRegister);
} // extern "C"
motorApp/AMCISrc/AMCIRegister.h
+44
View File
@@ -0,0 +1,44 @@
/*
FILENAME... AMCIRegister.h
USAGE... This file contains function prototypes for AMCI IOC shell commands.
Version: $Revision: 1.3 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-07-16 19:06:58 $
*/
/*
* Original Author: Ron Sluiter
* Date: 05/19/03
*
* Experimental Physics and Industrial Control System (EPICS)
*
* Copyright 1991, the Regents of the University of California,
* and the University of Chicago Board of Governors.
*
* This software was produced under U.S. Government contracts:
* (W-7405-ENG-36) at the Los Alamos National Laboratory,
* and (W-31-109-ENG-38) at Argonne National Laboratory.
*
* Initial development by:
* The Controls and Automation Group (AT-8)
* Ground Test Accelerator
* Accelerator Technology Division
* Los Alamos National Laboratory
*
* Co-developed with
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
*/
#include "motor.h"
#include "motordrvCom.h"
/* Function prototypes. */
extern RTN_STATUS ANG1Setup(const char *, const char *, const char *, int, int, int);
motorApp/AMCISrc/ANG1Driver.cpp
+640
View File
@@ -0,0 +1,640 @@
/*
FILENAME... ANG1Driver.cpp
USAGE... Motor record driver support for the AMCI ANG1 stepper motor controller over Modbus/TCP.
Kurt Goetze
Based on the ACS MCB-4B Model 3 device driver written by Mark Rivers
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <errno.h>
#include <iocsh.h>
#include <epicsThread.h>
#include <epicsString.h>
#include <asynInt32SyncIO.h>
#include "ANG1Driver.h"
#include <epicsExport.h>
#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5)
static const char *driverName = "ANG1MotorDriver";
/*** Input Registers ***/
#define STATUS_1 0
#define STATUS_2 1
#define POS_RD_UPR 2
#define POS_RD_LWR 3
#define EN_POS_UPR 4
#define EN_POS_LWR 5
#define EN_CAP_UPR 6
#define EN_CAP_LWR 7
#define MOT_CUR 8 // programmed motor current (x10)
#define JERK_RD 9
/*** Output Registers ***/
#define CMD_MSW 0 // module 0 starts at register address 1024. This is set in drvModbusAsynConfigure.
#define CMD_LSW 1
#define POS_WR_UPR 2
#define POS_WR_LWR 3
#define SPD_UPR 4
#define SPD_LWR 5
#define ACCEL 6
#define DECEL 7
// Not used must equal zero #define RESERVED 8
#define JERK 9
/** Constructor, Creates a new ANG1Controller object.
* \param[in] portName The name of the asyn port that will be created for this driver
* \param[in] ANG1InPortName The name of the drvAsynSerialPort that was created previously to connect to the ANG1 controller
* \param[in] ANG1OutPortName The name of the drvAsynSerialPort that was created previously to connect to the ANG1 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
*/
ANG1Controller::ANG1Controller(const char *portName, const char *ANG1InPortName, const char *ANG1OutPortName, int numAxes,
double movingPollPeriod, double idlePollPeriod)
: asynMotorController(portName, numAxes, NUM_ANG1_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, i;
asynStatus status;
ANG1Axis *pAxis;
static const char *functionName = "ANG1Controller::ANG1Controller";
inputDriver_ = epicsStrDup(ANG1InPortName); // Set this before calls to create Axis objects
// Create controller-specific parameters
createParam(ANG1JerkString, asynParamInt32, &ANG1Jerk_);
/* Connect to ANG1 controller */
for (i=0; i<MAX_INPUT_REGS; i++) {
status = pasynInt32SyncIO->connect(ANG1InPortName, i, &pasynUserInReg_[i], NULL);
}
for (i=0; i<MAX_OUTPUT_REGS; i++) {
status = pasynInt32SyncIO->connect(ANG1OutPortName, i, &pasynUserOutReg_[i], NULL);
}
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
"%s: cannot connect to ANG1 controller\n",
functionName);
}
for (axis=0; axis<numAxes; axis++) {
pAxis = new ANG1Axis(this, axis);
}
startPoller(movingPollPeriod, idlePollPeriod, 2);
}
/** Creates a new ANG1Controller 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] ANG1InPortName The name of the drvAsynIPPPort that was created previously to connect to the ANG1 controller
* \param[in] ANG1OutPortName The name of the drvAsynIPPPort that was created previously to connect to the ANG1 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 ANG1CreateController(const char *portName, const char *ANG1InPortName, const char *ANG1OutPortName, int numAxes,
int movingPollPeriod, int idlePollPeriod)
{
ANG1Controller *pANG1Controller
= new ANG1Controller(portName, ANG1InPortName, ANG1OutPortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
pANG1Controller = 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 ANG1Controller::report(FILE *fp, int level)
{
fprintf(fp, "ANG1 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 ANG1Axis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
ANG1Axis* ANG1Controller::getAxis(asynUser *pasynUser)
{
// ? return static_cast<ANG1Axis*>(asynMotorController::getAxis(pANG1Axis methodsasynUser));
return static_cast<ANG1Axis*>(asynMotorController::getAxis(pasynUser));
}
/** Returns a pointer to an ANG1Axis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] No Axis index number. */
ANG1Axis* ANG1Controller::getAxis(int axisNo)
{
return static_cast<ANG1Axis*>(asynMotorController::getAxis(axisNo));
}
/** Called when asyn clients call pasynInt32->write().
* Extracts the function and axis number from pasynUser.
* Sets the value in the parameter library (?)
* If the function is ANG1Jerk_ it sets the jerk value in the controller.
* Calls any registered callbacks for this pasynUser->reason and address.
* For all other functions it calls asynMotorController::writeInt32.
* \param[in] pasynUser asynUser structure that encodes the reason and address.
* \param[in] value Value to write. */
asynStatus ANG1Controller::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
int function = pasynUser->reason;
asynStatus status = asynSuccess;
ANG1Axis *pAxis = getAxis(pasynUser);
static const char *functionName = "writeInt32";
/* Set the parameter and readback in the parameter library. */
status = setIntegerParam(pAxis->axisNo_, function, value);
if (function == ANG1Jerk_)
{
// Jerk in units steps/sec/sec/sec (0 - 5000)
printf("Jerk = %d\n", value);
status = writeReg16(JERK, value, DEFAULT_CONTROLLER_TIMEOUT);
// sprintf(outString_, "%s JOG JRK %f", pAxis->axisName_, value);
// status = writeController();
} else {
/* Call base class method */
status = asynMotorController::writeInt32(pasynUser, value);
}
/* Do callbacks so higher layers see any changes */
pAxis->callParamCallbacks();
if (status)
asynPrint(pasynUser, ASYN_TRACE_ERROR,
"%s:%s: error, status=%d function=%d, value=%d\n",
driverName, functionName, status, function, value);
else
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
"%s:%s: function=%d, value=%d\n",
driverName, functionName, function, value);
return status;
}
asynStatus ANG1Controller::writeReg16(int reg, int output, double timeout)
{
asynStatus status;
//printf("writeReg16: writing %d to register %d\n", output, reg);
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,"writeReg16: writing %d to register %d\n", output, reg);
status = pasynInt32SyncIO->write(pasynUserOutReg_[reg], output, timeout);
epicsThreadSleep(0.01);
return status ;
}
asynStatus ANG1Controller::writeReg32(int reg, int output, double timeout)
{
//.. break 32-bit integer into 2 pieces
//.. write the pieces into ANG1 registers
asynStatus status;
float fnum;
int lower,upper;
fnum = (output / 1000.0);
upper = (int)fnum;
fnum = fnum - upper;
fnum = NINT(fnum * 1000);
lower = (int)fnum;
//could write the words this way
// status = pasynInt32SyncIO->write(pasynUserOutReg_[reg], upper, timeout);
// status = pasynInt32SyncIO->write(pasynUserOutReg_[reg+1], lower, timeout);
//or this way
// writeReg16(piece1 ie MSW ...
status = writeReg16(reg, upper, DEFAULT_CONTROLLER_TIMEOUT);
// writeReg16(piece2 ie LSW ...
reg++;
status = writeReg16(reg, lower, DEFAULT_CONTROLLER_TIMEOUT);
return status ;
}
asynStatus ANG1Controller::readReg16(int reg, epicsInt32 *input, double timeout)
{
asynStatus status;
//printf("reg = %d\n", reg);
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,"readReg16 reg = %d\n", reg);
status = pasynInt32SyncIO->read(pasynUserInReg_[reg], input, timeout);
return status ;
}
asynStatus ANG1Controller::readReg32(int reg, epicsInt32 *combo, double timeout)
{
//.. read 2 16-bit words from ANG1 registers
//.. assemble 2 16-bit pieces into 1 32-bit integer
asynStatus status;
// float fnum;
epicsInt32 lowerWord32, upperWord32; // only have pasynInt32SyncIO, not pasynInt16SyncIO ,
epicsInt16 lowerWord16, upperWord16; // so we need to get 32-bits and cast to 16-bit integer
//printf("calling readReg16\n");
status = readReg16(reg, &upperWord32, timeout); //get Upper Word
upperWord16 = (epicsInt16)upperWord32;
//printf("upperWord16: %d\n", upperWord16);
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,"readReg32 upperWord16: %d\n", upperWord16);
// if status != 1 :
reg++;
status = readReg16(reg, &lowerWord32, timeout); //get Lower Word
lowerWord16 = (epicsInt16)lowerWord32;
//printf("lowerWord16: %d\n", lowerWord16);
asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER,"readReg32 lowerWord16: %d\n", lowerWord16);
*combo = NINT((upperWord16 * 1000) + lowerWord16);
return status ;
}
// ANG1Axis methods Here
// These are the ANG1Axis methods
/** Creates a new ANG1Axis object.
* \param[in] pC Pointer to the ANG1Controller to which this axis belongs.
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
*
* Initializes register numbers, etc.
*/
ANG1Axis::ANG1Axis(ANG1Controller *pC, int axisNo)
: asynMotorAxis(pC, axisNo),
pC_(pC)
{
int status;
//status = pasynInt32SyncIO->connect(myModbusInputDriver, 0, &pasynUserForceRead_, "MODBUS_READ");
status = pasynInt32SyncIO->connect(pC_->inputDriver_, 0, &pasynUserForceRead_, "MODBUS_READ");
if (status) {
//printf("%s:%s: Error, unable to connect pasynUserForceRead_ to Modbus input driver %s\n", pC_->inputDriver_, pC_->functionName, myModbusInputDriver);
printf("%s: Error, unable to connect pasynUserForceRead_ to Modbus input driver\n", pC_->inputDriver_);
}
printf("ANG1Axis::ANG1Axis : pasynUserForceRead_->reason=%d\n", pasynUserForceRead_->reason);
// set position to 0
setPosition(0);
}
/** 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 ANG1Axis::report(FILE *fp, int level)
{
if (level > 0) {
fprintf(fp, " axis %d\n",
axisNo_);
}
// Call the base class method
asynMotorAxis::report(fp, level);
}
// SET VEL & ACCEL
asynStatus ANG1Axis::sendAccelAndVelocity(double acceleration, double velocity)
{
asynStatus status;
// static const char *functionName = "ANG1::sendAccelAndVelocity";
// Send the velocity in egus
//sprintf(pC_->outString_, "%1dVA%f", axisNo_ + 1, (velocity*stepSize_));
//status = pC_->writeController();
status = pC_->writeReg32(SPD_UPR, NINT(velocity), DEFAULT_CONTROLLER_TIMEOUT);
// Send the acceleration in egus/sec/sec
//printf(" velocity: %f\n", velocity);
//printf(" acceleration: %f\n", acceleration);
// ANG1 acceleration range 1 to 5000 steps/ms/sec
// Therefore need to limit range received by motor record from 1000 to 5e6 steps/sec/sec
if (acceleration < 1000) {
// print message noting that accel has been capped low
acceleration = 1000;
}
if (acceleration > 5000000) {
// print message noting that accel has been capped high
acceleration = 5000000;
}
// ANG1 acceleration units are steps/millisecond/second, so we divide by 1000 here
status = pC_->writeReg16(ACCEL, NINT(acceleration/1000.0), DEFAULT_CONTROLLER_TIMEOUT);
status = pC_->writeReg16(DECEL, NINT(acceleration/1000.0), DEFAULT_CONTROLLER_TIMEOUT);
return status;
}
// MOVE
asynStatus ANG1Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
{
asynStatus status;
int velo, distance, move_bit;
printf(" ** ANG1Axis::move called, relative = %d\n", relative);
status = sendAccelAndVelocity(acceleration, maxVelocity);
//velo = maxVelocity * SOME_SCALE_FACTOR
velo = NINT(maxVelocity);
if (relative) {
printf(" ** relative move called\n");
//status = pC_->writeReg32(SPD_UPR, velo, DEFAULT_CONTROLLER_TIMEOUT);
//distance = position * SOM_OTHER_SCALE_FACTOR;
distance = NINT(position);
status = pC_->writeReg32(POS_WR_UPR, distance, DEFAULT_CONTROLLER_TIMEOUT);
move_bit = 0x0;
status = pC_->writeReg16(CMD_MSW, move_bit, DEFAULT_CONTROLLER_TIMEOUT);
move_bit = 0x2;
status = pC_->writeReg16(CMD_MSW, move_bit, DEFAULT_CONTROLLER_TIMEOUT);
} else {
// absolute
printf(" ** absolute move called\n");
//status = pC_->writeReg32(SPD_UPR, velo, DEFAULT_CONTROLLER_TIMEOUT);
//distance = position * SOM_OTHER_SCALE_FACTOR;
distance = NINT(position);
printf(" ** distance = %d\n", distance);
status = pC_->writeReg32(POS_WR_UPR, distance, DEFAULT_CONTROLLER_TIMEOUT);
move_bit = 0x0;
status = pC_->writeReg16(CMD_MSW, move_bit, DEFAULT_CONTROLLER_TIMEOUT);
move_bit = 0x1;
status = pC_->writeReg16(CMD_MSW, move_bit, DEFAULT_CONTROLLER_TIMEOUT);
}
// Delay the first status read, give the controller some time to return moving status
epicsThreadSleep(0.05);
return status;
}
// HOME (needs work)
asynStatus ANG1Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
{
asynStatus status;
int home_bit;
// static const char *functionName = "ANG1Axis::home";
//status = sendAccelAndVelocity(acceleration, maxVelocity);
if (forwards) {
printf(" ** HOMING FORWARDS **\n");
home_bit = 0x20;
status = pC_->writeReg16(CMD_MSW, home_bit, DEFAULT_CONTROLLER_TIMEOUT);
} else {
home_bit = 0x40;
status = pC_->writeReg16(CMD_MSW, home_bit, DEFAULT_CONTROLLER_TIMEOUT);
}
return status;
}
// JOG
asynStatus ANG1Axis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
{
asynStatus status;
int velo, distance, move_bit;
static const char *functionName = "ANG1Axis::moveVelocity";
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
"%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
functionName, minVelocity, maxVelocity, acceleration);
velo = NINT(fabs(maxVelocity));
status = sendAccelAndVelocity(acceleration, velo);
/* ANG1 does not have jog command. Move 1 million steps */
if (maxVelocity > 0.) {
/* This is a positive move in ANG1 coordinates */
//printf(" ** relative move (JOG pos) called\n");
distance = 1000000;
status = pC_->writeReg32(POS_WR_UPR, distance, DEFAULT_CONTROLLER_TIMEOUT);
move_bit = 0x0;
status = pC_->writeReg16(CMD_MSW, move_bit, DEFAULT_CONTROLLER_TIMEOUT);
move_bit = 0x2;
status = pC_->writeReg16(CMD_MSW, move_bit, DEFAULT_CONTROLLER_TIMEOUT);
} else {
/* This is a negative move in ANG1 coordinates */
//printf(" ** relative move (JOG neg) called\n");
distance = -1000000;
status = pC_->writeReg32(POS_WR_UPR, distance, DEFAULT_CONTROLLER_TIMEOUT);
move_bit = 0x0;
status = pC_->writeReg16(CMD_MSW, move_bit, DEFAULT_CONTROLLER_TIMEOUT);
move_bit = 0x2;
status = pC_->writeReg16(CMD_MSW, move_bit, DEFAULT_CONTROLLER_TIMEOUT);
}
// Delay the first status read, give the controller some time to return moving status
epicsThreadSleep(0.05);
return status;
}
// STOP
asynStatus ANG1Axis::stop(double acceleration)
{
asynStatus status;
int stop_bit;
//static const char *functionName = "ANG1Axis::stop";
printf("\n STOP \n\n");
stop_bit = 0x0;
status = pC_->writeReg16(CMD_MSW, stop_bit, DEFAULT_CONTROLLER_TIMEOUT);
// stop_bit = 0x10; Immediate stop
stop_bit = 0x4; // Hold move
status = pC_->writeReg16(CMD_MSW, stop_bit, DEFAULT_CONTROLLER_TIMEOUT);
return status;
}
// SET
asynStatus ANG1Axis::setPosition(double position)
{
asynStatus status;
int set_position, set_bit;
//static const char *functionName = "ANG1Axis::setPosition";
//status = writeReg32(SPD_UPR, velo, DEFAULT_CONTROLLER_TIMEOUT);
//distance = position * SOM_OTHER_SCALE_FACTOR;
set_position = NINT(position);
status = pC_->writeReg32(POS_WR_UPR, set_position, DEFAULT_CONTROLLER_TIMEOUT);
set_bit = 0x200;
status = pC_->writeReg16(CMD_MSW, set_bit, DEFAULT_CONTROLLER_TIMEOUT);
set_bit = 0x0;
status = pC_->writeReg16(CMD_MSW, set_bit, DEFAULT_CONTROLLER_TIMEOUT);
return status;
}
// ENABLE TORQUE
asynStatus ANG1Axis::setClosedLoop(bool closedLoop)
{
asynStatus status;
int enable = 0x8000;
int disable = 0x0000;
int cmd;
printf(" ** setClosedLoop called \n");
if (closedLoop) {
printf("setting enable %X\n", enable);
// Let's reset errors first
cmd = 0x0;
status = pC_->writeReg16(CMD_MSW, cmd, DEFAULT_CONTROLLER_TIMEOUT);
cmd = 0x400;
status = pC_->writeReg16(CMD_MSW, cmd, DEFAULT_CONTROLLER_TIMEOUT);
cmd = 0x0;
status = pC_->writeReg16(CMD_MSW, cmd, DEFAULT_CONTROLLER_TIMEOUT);
status = pC_->writeReg16(CMD_LSW, enable, DEFAULT_CONTROLLER_TIMEOUT);
setIntegerParam(pC_->motorStatusPowerOn_, 1);
} else {
printf("setting disable %X\n", disable);
status = pC_->writeReg16(CMD_LSW, disable, DEFAULT_CONTROLLER_TIMEOUT);
setIntegerParam(pC_->motorStatusPowerOn_, 0);
}
return status;
}
// POLL
/** Polls the axis.
* This function reads motor position, limit status, home status, and moving 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 ANG1Axis::poll(bool *moving)
{
int done;
int limit;
int enabled;
double position;
asynStatus status;
epicsInt32 read_val; // don't use a pointer here. The _address_ of read_val should be passed to the read function.
// Force a read operation
//printf(" . . . . . Calling pasynInt32SyncIO->write\n");
//printf("Calling pasynInt32SyncIO->write(pasynUserForceRead_, 1, TIMEOUT), pasynUserForceRead_->reason=%d\n", pasynUserForceRead_->reason);
status = pasynInt32SyncIO->write(pasynUserForceRead_, 1, DEFAULT_CONTROLLER_TIMEOUT);
//printf(" . . . . . status = %d\n", status);
// if status goto end
// Read the current motor position
//
//readReg32(int reg, epicsInt32 *combo, double timeout)
status = pC_->readReg32(POS_RD_UPR, &read_val, DEFAULT_CONTROLLER_TIMEOUT);
printf("ANG1Axis::poll: Motor position raw: %d\n", read_val);
position = (double) read_val;
setDoubleParam(pC_->motorPosition_, position);
printf("ANG1Axis::poll: Motor position: %f\n", position);
// Read the moving status of this motor
//
status = pC_->readReg16(STATUS_1, &read_val, DEFAULT_CONTROLLER_TIMEOUT);
//printf("status 1 is 0x%X\n", read_val);
// Done logic
done = ((read_val & 0x8) >> 3); // status word 1 bit 3 set to 1 when the motor is not in motion.
setIntegerParam(pC_->motorStatusDone_, done);
*moving = done ? false:true;
printf("done is %d\n", done);
// Read the limit status
//
status = pC_->readReg16(STATUS_2, &read_val, DEFAULT_CONTROLLER_TIMEOUT);
printf("status 2 is 0x%X\n", read_val);
limit = (read_val & 0x1); // a cw limit has been reached
setIntegerParam(pC_->motorStatusHighLimit_, limit);
//printf("+limit %d\n", limit);
if (limit) { // reset error and set position so we can move off of the limit
// Reset error
setClosedLoop(1);
// Reset position
//printf(" Reset Position\n");
setPosition(position);
}
limit = (read_val & 0x2); // a ccw limit has been reached
setIntegerParam(pC_->motorStatusLowLimit_, limit);
//printf("-limit %d\n", limit);
if (limit) { // reset error and set position so we can move off of the limit
// Reset error
setClosedLoop(1);
// Reset position
setPosition(position);
}
// test for home
// Should be in init routine? Allows CNEN to be used.
setIntegerParam(pC_->motorStatusGainSupport_, 1);
// Check for the torque status and set accordingly.
enabled = (read_val & 0x8000);
if (enabled)
setIntegerParam(pC_->motorStatusPowerOn_, 1);
else
setIntegerParam(pC_->motorStatusPowerOn_, 0);
// Notify asynMotorController polling routine that we're ready
callParamCallbacks();
return status;
}
/** Code for iocsh registration */
static const iocshArg ANG1CreateControllerArg0 = {"Port name", iocshArgString};
static const iocshArg ANG1CreateControllerArg1 = {"ANG1 In port name", iocshArgString};
static const iocshArg ANG1CreateControllerArg2 = {"ANG1 Out port name", iocshArgString};
static const iocshArg ANG1CreateControllerArg3 = {"Number of axes", iocshArgInt};
static const iocshArg ANG1CreateControllerArg4 = {"Moving poll period (ms)", iocshArgInt};
static const iocshArg ANG1CreateControllerArg5 = {"Idle poll period (ms)", iocshArgInt};
static const iocshArg * const ANG1CreateControllerArgs[] = {&ANG1CreateControllerArg0,
&ANG1CreateControllerArg1,
&ANG1CreateControllerArg2,
&ANG1CreateControllerArg3,
&ANG1CreateControllerArg4,
&ANG1CreateControllerArg5,};
static const iocshFuncDef ANG1CreateControllerDef = {"ANG1CreateController", 6, ANG1CreateControllerArgs};
static void ANG1CreateContollerCallFunc(const iocshArgBuf *args)
{
ANG1CreateController(args[0].sval, args[1].sval, args[2].sval, args[3].ival, args[4].ival, args[5].ival);
}
static void ANG1Register(void)
{
iocshRegister(&ANG1CreateControllerDef, ANG1CreateContollerCallFunc);
}
extern "C" {
epicsExportRegistrar(ANG1Register);
}
motorApp/AMCISrc/ANG1Driver.h
+88
View File
@@ -0,0 +1,88 @@
/*
FILENAME... ANG1Driver.h
USAGE... Motor driver support for the AMCI ANG1 controller.
Based on MCB-4B driver written by
Mark Rivers
March 1, 2012
K. Goetze 2014-03-24
*/
#include "asynMotorController.h"
#include "asynMotorAxis.h"
#define MAX_ANG1_AXES 1
#define MAX_INPUT_REGS 10
#define MAX_OUTPUT_REGS 10
// No. of controller-specific parameters
#define NUM_ANG1_PARAMS 1
/** drvInfo strings for extra parameters that the ACR controller supports */
#define ANG1JerkString "ANG1_JERK"
//#define ACRReadBinaryIOString "ACR_READ_BINARY_IO"
//#define ACRBinaryInString "ACR_BINARY_IN"
//#define ACRBinaryOutString "ACR_BINARY_OUT"
//#define ACRBinaryOutRBVString "ACR_BINARY_OUT_RBV"
class ANG1Axis : public asynMotorAxis
{
public:
/* These are the methods we override from the base class */
ANG1Axis(class ANG1Controller *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:
ANG1Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs.
* Abbreviated because it is used very frequently */
asynStatus sendAccelAndVelocity(double accel, double velocity);
asynUser *pasynUserForceRead_;
friend class ANG1Controller;
};
class ANG1Controller : public asynMotorController {
public:
ANG1Controller(const char *portName, const char *ANG1InPortName, const char *ANG1OutPortName, int numAxes, double movingPollPeriod, double idlePollPeriod);
void report(FILE *fp, int level);
ANG1Axis* getAxis(asynUser *pasynUser);
ANG1Axis* getAxis(int axisNo);
asynUser *pasynUserInReg_[MAX_INPUT_REGS];
asynUser *pasynUserOutReg_[MAX_OUTPUT_REGS];
// asynUser *pasynUserForceRead_;
/* These are the methods that we override from asynMotorDriver */
asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
//asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value);
//void report(FILE *fp, int level);
//ACRAxis* getAxis(asynUser *pasynUser);
//ACRAxis* getAxis(int axisNo);
/* These are the methods that are new to this class */
protected:
int ANG1Jerk_; /**< Jerk time parameter index */
private:
asynStatus writeReg16(int, int, double);
asynStatus writeReg32(int, int, double);
asynStatus readReg16(int, epicsInt32*, double);
asynStatus readReg32(int, epicsInt32*, double);
char *inputDriver_;
friend class ANG1Axis;
};
motorApp/AMCISrc/ANG1Support.dbd
+2
View File
@@ -0,0 +1,2 @@
#registrar(ANG1MotorRegister)
registrar(ANG1Register)
motorApp/AMCISrc/Makefile
+29
View File
@@ -0,0 +1,29 @@
TOP=../..
include $(TOP)/configure/CONFIG
#----------------------------------------
# ADD MACRO DEFINITIONS AFTER THIS LINE
#=============================
#==================================================
# Build an IOC support library
LIBRARY_IOC += AMCI
SRCS += AMCIRegister.cc
# motorRecord.h will be created from motorRecord.dbd
# install devMotorSoft.dbd into <top>/dbd
DBD += ANG1Support.dbd
# The following are compiled and added to the Support library
AMCI_SRCS += ANG1Driver.cpp
AMCI_LIBS += motor
AMCI_LIBS += asyn
AMCI_LIBS += $(EPICS_BASE_IOC_LIBS)
include $(TOP)/configure/RULES
#----------------------------------------
# ADD RULES AFTER THIS LINE
motorApp/AMCISrc/README
+49
View File
@@ -0,0 +1,49 @@
AMCI ANG1
=========
Asyn model 3 driver support for the AMCI ANG1 stepper motor
controller/driver.
Modbus/TCP communication, using Mark Rivers' modbus module
Currently some basic parameters, such as IP address, motor current,
etc., will need to be set up on the controller using AMCI's PC/Win software.
Limits and Home also need to be configured with this software: *
+ Limit -> ANG1 Input 1
- Limit -> ANG1 Input 2
Home -> ANG1 Input 3
ANG1 configuration software is available from AMCI's website:
www.amci.com/product-software.asp
Eventually I would like to implement the "Configuration Mode" options
down in the driver, making the AMCI PC software necessary only for
initial comms (IP) setup.
The ANG1 controller/driver is available with or without an ethernet
port. A group of controller/drivers can contain up to 6 modules, one of
which needs to have the ethernet option. A single-channel
implementation would need the module with ethernet. This software is
intended to support configurations of multiple groups of 1 to 6 modules.
Again, each group will need 1 ethernet enabled module (ANG1-E).
asyn model 3 driver files:
--------------------------
ANG1Driver.cpp
ANG1Driver.h
ANG1Support.dbd
* Note: 1.) At some point I would like to add the capability to set these
parameters at boot-time, using the ANG1 "configuration mode".
2.) VBAS can only be set in configuration mode (which is not
currently supported) or by using the PC configuration software.
To use VBAS, set the desired value for the axis using the AMCI
PC configuration interface, then set the VBAS field in the
motor record accordingly. That way this value will be used by
the record to correctly calculate acceleration.
motorApp/Makefile
+3
View File
@@ -105,6 +105,9 @@ MicronixSrc_DEPEND_DIRS = MotorSrc
DIRS += PhytronSrc
PhytronSrc_DEPEND_DIRS = MotorSrc
DIRS += AMCISrc
AMCISrc_DEPEND_DIRS = MotorSrc
endif
# Install the edl files