forked from epics_driver_modules/motorBase
Keenan Lang
+1
-1
@@ -14,4 +14,4 @@ envPaths
|
||||
dllPath.bat
|
||||
auto_settings.sav*
|
||||
auto_positions.sav*
|
||||
|
||||
.ccfxprepdir/
|
||||
|
||||
@@ -5,5 +5,10 @@ For more information, see:
|
||||
* [main page](http://htmlpreview.github.io/?https://github.com/epics-modules/motor/blob/master/documentation/motor.html)
|
||||
* [motor wiki](https://github.com/epics-modules/motor/wiki)
|
||||
* http://www.aps.anl.gov/bcda/synApps
|
||||
* [HTML documentation](https://github.com/epics-modules/motor/blob/master/documentation/README.md)
|
||||
|
||||
[Report an issue with Motor](https://github.com/epics-modules/motor/issues/new?title=%20ISSUE%20NAME%20HERE&body=**Describe%20the%20issue**%0A%0A**Steps%20to%20reproduce**%0A1.%20Step%20one%0A2.%20Step%20two%0A3.%20Step%20three%0A%0A**Expected%20behaivour**%0A%0A**Actual%20behaviour**%0A%0A**Build%20Environment**%0AArchitecture:%0AEpics%20Base%20Version:%0ADependent%20Module%20Versions:&labels=bug)
|
||||
[Request a feature](https://github.com/epics-modules/motor/issues/new?title=%20FEATURE%20SHORT%20DESCRIPTION&body=**Feature%20Long%20Description**%0A%0A**Why%20should%20this%20be%20added?**%0A&labels=enhancement)
|
||||
|
||||
|
||||
converted from APS SVN repository: Fri Oct 16 12:31:41 CDT 2015
|
||||
|
||||
@@ -37,9 +37,6 @@ EPICS_BASE=
|
||||
# Recommended IPAC release: R2-12
|
||||
IPAC=$(SUPPORT)/ipac/R2-12
|
||||
|
||||
# Script module needed to build ScriptMotorController
|
||||
#!SCRIPT=$(SUPPORT)/script
|
||||
|
||||
# The following is only needed for the motor examples in iocBoot.
|
||||
#!MOTOR=$(TOP)
|
||||
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
# HTML documentation
|
||||
|
||||
* [motor.html](http://htmlpreview.github.com/?https://github.com/epics-modules/motor/blob/master/documentation/motor.html)
|
||||
* [motorDeviceDriver.html](http://htmlpreview.github.com/?https://github.com/epics-modules/motor/blob/master/documentation/motorDeviceDriver.html)
|
||||
* [motorRecord.html](http://htmlpreview.github.com/?https://github.com/epics-modules/motor/blob/master/documentation/motorRecord.html)
|
||||
* [motor_files.html](http://htmlpreview.github.com/?https://github.com/epics-modules/motor/blob/master/documentation/motor_files.html)
|
||||
* [motor_release.html](http://htmlpreview.github.com/?https://github.com/epics-modules/motor/blob/master/documentation/motor_release.html)
|
||||
* [trajectoryScan.html](http://htmlpreview.github.com/?https://github.com/epics-modules/motor/blob/master/documentation/trajectoryScan.html)
|
||||
@@ -39,7 +39,7 @@
|
||||
record, and related EPICS software required to build and use it. Version
|
||||
R6-9 of the motor record is compatible with EPICS base R3.14.12.2 and above.
|
||||
<p>
|
||||
The motor record is intended to support motors of all kinds, but currently
|
||||
The motor record is intended to support positioning motors of all kinds, but currently
|
||||
supports only the following variety of motor controllers (in addition to Soft
|
||||
Channel support):
|
||||
</p>
|
||||
|
||||
@@ -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}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
file "$(TOP)/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}
|
||||
# Newport NSA12 stage, 6.35 microns/full step, 128 micro-steps/s
|
||||
{IOC:, 1, "m$(N)", "asynMotor", CONEX1, 0, "X", mm, Pos, 1., 0.1, .25, 0, 1, .2, 4.9609375e-5, 4, 28, -1, ""}
|
||||
{IOC:, 2, "m$(N)", "asynMotor", CONEX2, 0, "Y", mm, Pos, 1., 0.1, .25, 0, 1, .2, 4.9609375e-5, 4, 28, -1, ""}
|
||||
{IOC:, 3, "m$(N)", "asynMotor", CONEX3, 0, "Z", mm, Pos, 1., 0.1, .25, 0, 1, .2, 4.9609375e-5, 4, 28, -1, ""}
|
||||
}
|
||||
@@ -1,15 +0,0 @@
|
||||
file "$(TOP)/db/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", VMC_MOTOR, 0, "VMCMotor", mm, Pos, 15., 3., .05, .5, 0, 1.0, 2, .025, 5, 0, 0, ""}
|
||||
{IOC:, m2, "asynMotor", SOFT_MOTOR, 0, "SoftMotor", mm, Pos, 15., 3., .05, .5, 0, 1.0, 2, .025, 5, 0, 0, ""}
|
||||
}
|
||||
|
||||
file "$(TOP)/db/ScriptMotorReload.db"
|
||||
{
|
||||
pattern
|
||||
{P, PORT}
|
||||
{IOC:, SOFT_MOTOR}
|
||||
{IOC:, VMC_MOTOR}
|
||||
}
|
||||
@@ -1,67 +0,0 @@
|
||||
IdlePollPeriod = 1.00
|
||||
MovingPollPeriod = 0.25
|
||||
|
||||
lastPos = 0
|
||||
targetPos = 0
|
||||
|
||||
function move(position, relative, minVel, maxVel, accel)
|
||||
local MRES = asyn.getDoubleParam( DRIVER, AXIS, "MOTOR_REC_RESOLUTION")
|
||||
|
||||
if (relative) then
|
||||
local prev = asyn.getDoubleParam( DRIVER, AXIS, "MOTOR_POSITION")
|
||||
targetPos = prev + (position * MRES)
|
||||
else
|
||||
targetPos = (position * MRES)
|
||||
end
|
||||
|
||||
epics.put(DRIVE_PV, targetPos)
|
||||
|
||||
if (position > 0) then
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DIRECTION", 1)
|
||||
else
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DIRECTION", 0)
|
||||
end
|
||||
|
||||
asyn.callParamCallbacks(DRIVER, AXIS)
|
||||
end
|
||||
|
||||
|
||||
function poll()
|
||||
local MRES = asyn.getDoubleParam( DRIVER, AXIS, "MOTOR_REC_RESOLUTION")
|
||||
|
||||
if (MRES == 0.0) then
|
||||
return true
|
||||
end
|
||||
|
||||
local curr = epics.get(READBACK_PV)
|
||||
|
||||
asyn.setDoubleParam( DRIVER, AXIS, "MOTOR_POSITION", curr / MRES)
|
||||
|
||||
local done = 0
|
||||
local moving = 1
|
||||
|
||||
if (curr == targetPos) then
|
||||
done = 1
|
||||
moving = 0
|
||||
elseif (math.abs(curr - targetPos) <= MRES and lastPos == curr) then
|
||||
done = 1
|
||||
moving = 0
|
||||
end
|
||||
|
||||
lastPos = curr
|
||||
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DONE", done)
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_MOVING", moving)
|
||||
|
||||
asyn.callParamCallbacks(DRIVER, AXIS)
|
||||
|
||||
return (done ~= 1)
|
||||
end
|
||||
|
||||
|
||||
function stop(acceleration)
|
||||
local curr = asyn.getDoubleParam( DRIVER, AXIS, "MOTOR_POSITION")
|
||||
|
||||
epics.put(DRIVE_PV, curr)
|
||||
targetPos = curr
|
||||
end
|
||||
@@ -1,66 +0,0 @@
|
||||
IdlePollPeriod = 1.0
|
||||
MovingPollPeriod = 0.25
|
||||
ForcedFastPolls = 2
|
||||
|
||||
InTerminator = "\r\n"
|
||||
OutTerminator = "\r\n"
|
||||
|
||||
function sendAccelAndVelocity(minVel, maxVel, accel)
|
||||
asyn.writeread( string.format( "%d BAS %f", AXIS + 1, minVel) , PORT);
|
||||
asyn.writeread( string.format( "%d VEL %f", AXIS + 1, maxVel) , PORT);
|
||||
asyn.writeread( string.format( "%d ACC %f", AXIS + 1, accel ) , PORT);
|
||||
end
|
||||
|
||||
|
||||
function move(position, relative, minVel, maxVel, accel)
|
||||
sendAccelAndVelocity( minVel, maxVel, accel)
|
||||
|
||||
if (relative) then
|
||||
asyn.writeread( string.format( "%d MR %d", AXIS + 1, math.floor(position) ) , PORT)
|
||||
else
|
||||
asyn.writeread( string.format( "%d MV %d", AXIS + 1, math.floor(position) ) , PORT)
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
function moveVelocity(minVel, maxVel, accel)
|
||||
sendAccelAndVelocity( minVel, maxVel, accel)
|
||||
|
||||
asyn.writeread( string.format( "%d JOG %f", AXIS + 1, maxVel) , PORT);
|
||||
end
|
||||
|
||||
|
||||
function poll()
|
||||
asyn.write( string.format( "%d POS?", AXIS + 1) , PORT)
|
||||
|
||||
asyn.setDoubleParam( DRIVER, AXIS, "MOTOR_POSITION", tonumber( asyn.read(PORT) ) )
|
||||
|
||||
asyn.write( string.format( "%d ST?", AXIS + 1) , PORT)
|
||||
|
||||
local status = tonumber( asyn.read(PORT) )
|
||||
|
||||
local direction = (status & 1)
|
||||
local done = (status & 2) >> 1
|
||||
local limit_high = (status & 8) >> 3
|
||||
local limit_low = (status & 16) >> 4
|
||||
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DIRECTION", direction)
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DONE", done)
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_MOVING", done ~ 1)
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_HIGH_LIMIT", limit_high)
|
||||
asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_LOW_LIMIT", limit_low)
|
||||
|
||||
asyn.callParamCallbacks(DRIVER, AXIS)
|
||||
|
||||
return (done ~= 1)
|
||||
end
|
||||
|
||||
|
||||
function stop(acceleration)
|
||||
asyn.writeread( string.format( "%d AB", AXIS + 1) , PORT);
|
||||
end
|
||||
|
||||
|
||||
function setPosition(position)
|
||||
asyn.writeread( string.format( "%d POS %d", AXIS + 1, math.floor(position) ) , PORT);
|
||||
end
|
||||
@@ -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)
|
||||
@@ -0,0 +1,45 @@
|
||||
#errlogInit(5000)
|
||||
< envPaths
|
||||
# Tell EPICS all about the record types, device-support modules, drivers,
|
||||
# etc.
|
||||
dbLoadDatabase("../../dbd/WithAsyn.dbd")
|
||||
WithAsyn_registerRecordDeviceDriver(pdbbase)
|
||||
|
||||
### Motors
|
||||
dbLoadTemplate "motor.substitutions.CONEX-PP"
|
||||
|
||||
# For Windows
|
||||
drvAsynSerialPortConfigure("serial1", "COM8", 0, 0, 0)
|
||||
# For Linux
|
||||
#drvAsynSerialPortConfigure("serial1", "/dev/ttyUSB0", 0, 0, 0)
|
||||
asynOctetSetInputEos("serial1",0,"\r\n")
|
||||
asynOctetSetOutputEos("serial1",0,"\r\n")
|
||||
asynSetOption("serial1",0,"baud","115200")
|
||||
asynSetOption("serial1",0,"bits","8")
|
||||
asynSetOption("serial1",0,"stop","1")
|
||||
asynSetOption("serial1",0,"parity","none")
|
||||
asynSetOption("serial1",0,"clocal","Y")
|
||||
asynSetOption("serial1",0,"crtscts","N")
|
||||
|
||||
asynSetTraceIOMask("serial1", 0, 2)
|
||||
#asynSetTraceMask("serial1", 0, 9)
|
||||
|
||||
# Load asyn record
|
||||
dbLoadRecords("$(ASYN)/db/asynRecord.db", "P=IOC:,R=serial1,PORT=serial1, ADDR=0,OMAX=256,IMAX=256")
|
||||
|
||||
# AG_CONEXCreateController(asyn port, serial port, controllerID,
|
||||
# active poll period (ms), idle poll period (ms))
|
||||
AG_CONEXCreateController("CONEX1", "serial1", 1, 50, 500)
|
||||
asynSetTraceIOMask("CONEX1", 0, 2)
|
||||
#asynSetTraceMask("CONEX1", 0, 255)
|
||||
AG_CONEXCreateController("CONEX2", "serial1", 2, 50, 500)
|
||||
asynSetTraceIOMask("CONEX2", 0, 2)
|
||||
#asynSetTraceMask("CONEX2", 0, 255)
|
||||
AG_CONEXCreateController("CONEX3", "serial1", 3, 50, 500)
|
||||
asynSetTraceIOMask("CONEX3", 0, 2)
|
||||
#asynSetTraceMask("CONEX3", 0, 255)
|
||||
|
||||
iocInit
|
||||
|
||||
dbpf IOC:m1.RTRY 0
|
||||
dbpf IOC:m1.NTM 0
|
||||
@@ -1,18 +0,0 @@
|
||||
< envPaths
|
||||
|
||||
dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd")
|
||||
WithAsyn_registerRecordDeviceDriver(pdbbase)
|
||||
|
||||
epicsEnvSet("LUA_SCRIPT_PATH", "./scripts")
|
||||
|
||||
# Connect to virtual motor controller server
|
||||
drvAsynIPPortConfigure("VMC","127.0.0.1:31337", 0, 0, 0)
|
||||
|
||||
#ScriptControllerConfig( "PORT_NAME", num_axes, "lua_script", "PARAMS=")
|
||||
ScriptControllerConfig("VMC_MOTOR", 1, "vmc.lua", "PORT=VMC")
|
||||
|
||||
ScriptControllerConfig("SOFT_MOTOR", 1, "softMotor.lua", "DRIVE_PV='IOC:m1.VAL', READBACK_PV='IOC:m1.RBV'")
|
||||
|
||||
dbLoadTemplate("motor.substitutions.script")
|
||||
|
||||
iocInit
|
||||
@@ -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"
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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;
|
||||
};
|
||||
@@ -0,0 +1,2 @@
|
||||
#registrar(ANG1MotorRegister)
|
||||
registrar(ANG1Register)
|
||||
@@ -1,4 +1,3 @@
|
||||
# Makefile
|
||||
TOP=../..
|
||||
|
||||
include $(TOP)/configure/CONFIG
|
||||
@@ -8,21 +7,21 @@ include $(TOP)/configure/CONFIG
|
||||
|
||||
#==================================================
|
||||
# Build an IOC support library
|
||||
LIBRARY_IOC = ScriptMotor
|
||||
|
||||
LIBRARY_IOC += AMCI
|
||||
|
||||
SRCS += AMCIRegister.cc
|
||||
|
||||
# motorRecord.h will be created from motorRecord.dbd
|
||||
# install devMotorSoft.dbd into <top>/dbd
|
||||
DBD += ScriptMotorDriver.dbd
|
||||
|
||||
INC += ScriptMotorDriver.h
|
||||
DBD += ANG1Support.dbd
|
||||
|
||||
# The following are compiled and added to the Support library
|
||||
ScriptMotor_SRCS += ScriptMotorDriver.cpp
|
||||
AMCI_SRCS += ANG1Driver.cpp
|
||||
|
||||
ScriptMotor_LIBS += motor
|
||||
ScriptMotor_LIBS += asyn
|
||||
ScriptMotor_LIBS += script
|
||||
ScriptMotor_LIBS += $(EPICS_BASE_IOC_LIBS)
|
||||
AMCI_LIBS += motor
|
||||
AMCI_LIBS += asyn
|
||||
AMCI_LIBS += $(EPICS_BASE_IOC_LIBS)
|
||||
|
||||
include $(TOP)/configure/RULES
|
||||
#----------------------------------------
|
||||
@@ -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.
|
||||
|
||||
@@ -46,7 +46,6 @@ DB += profileMoveAxisXPS.template
|
||||
DB += PI_Support.db PI_SupportCtrl.db
|
||||
DB += Phytron_motor.db Phytron_I1AM01.db Phytron_MCM01.db
|
||||
DB += asyn_auto_power.db
|
||||
DB += ScriptMotorReload.db
|
||||
|
||||
#----------------------------------------------------
|
||||
# Declare template files which do not show up in DB
|
||||
|
||||
@@ -1,9 +0,0 @@
|
||||
record(bo, "$(P)$(PORT):ScriptReload")
|
||||
{
|
||||
field(DESC, "$(PORT) Script Reload")
|
||||
field(DTYP, "asynInt32")
|
||||
field(OUT, "@asyn($(PORT), 0, 0)RELOAD_SCRIPT")
|
||||
|
||||
field(ZNAM, "Loaded")
|
||||
field(ONAM, "Reload")
|
||||
}
|
||||
+2
-5
@@ -105,11 +105,8 @@ MicronixSrc_DEPEND_DIRS = MotorSrc
|
||||
DIRS += PhytronSrc
|
||||
PhytronSrc_DEPEND_DIRS = MotorSrc
|
||||
|
||||
# The Script motor support requires the lua scripting module
|
||||
ifdef SCRIPT
|
||||
DIRS += ScriptMotorSrc
|
||||
ScriptMotorSrc_DEPEND_DIRS = MotorSrc
|
||||
endif
|
||||
DIRS += AMCISrc
|
||||
AMCISrc_DEPEND_DIRS = MotorSrc
|
||||
|
||||
endif
|
||||
|
||||
|
||||
@@ -1,31 +1,32 @@
|
||||
Here's my best guess at what all this stuff is. TMM
|
||||
|
||||
Model 3 (asyn, C++) driver base classes
|
||||
--------------------------------
|
||||
---------------------------------------
|
||||
asynMotorAxis.cpp
|
||||
asynMotorAxis.h
|
||||
asynMotorController.cpp
|
||||
asynMotorController.h
|
||||
|
||||
Model 2 and Model 3 device support
|
||||
--------------------------------
|
||||
----------------------------------
|
||||
devMotorAsyn.c
|
||||
motor_interface.h
|
||||
|
||||
Model 2 (asyn, C)
|
||||
--------------
|
||||
-----------------
|
||||
drvMotorAsyn.c
|
||||
motor_interface.h
|
||||
paramLib.c
|
||||
paramLib.h
|
||||
|
||||
Definitions used in record, device support, and utilities
|
||||
---------------------------------------------------------
|
||||
motor.h
|
||||
|
||||
motor record
|
||||
------------
|
||||
motor.h
|
||||
motorRecord.cc
|
||||
motorRecord.dbd
|
||||
|
||||
Model 1 (non-asyn) device/driver support
|
||||
------------------------------
|
||||
----------------------------------------
|
||||
motordevCom.cc
|
||||
motordevCom.h
|
||||
motordrvCom.cc
|
||||
|
||||
@@ -183,6 +183,9 @@ USAGE... Motor Record Support.
|
||||
* .72 03-13-15 rls - Changed RDBL to set RRBV rather than DRBV.
|
||||
* .73 02-15-16 rls - JOGF/R soft limit error check was using the wrong coordinate sytem limits.
|
||||
* Changed error checks from dial to user limits.
|
||||
* .74 09-28-16 rls - Reverted .71 FLNK change. Except for the condition that DMOV == FALSE, FLNK
|
||||
* processing was standard. If processing is needed on a DMOV false to true
|
||||
* transition, a new motor record field should be added.
|
||||
*/
|
||||
|
||||
#define VERSION 6.10
|
||||
@@ -1163,10 +1166,10 @@ Exit:
|
||||
Update record timestamp, call recGblGetTimeStamp().
|
||||
Process alarms, call alarm_sub().
|
||||
Monitor changes to record fields, call monitor().
|
||||
IF Done Moving field (DMOV) is TRUE, AND, Last Done Moving (LDMV) was False.
|
||||
|
||||
IF Done Moving field (DMOV) is TRUE
|
||||
Process the forward-scan-link record, call recGblFwdLink().
|
||||
ENDIF
|
||||
Update Last Done Moving (LDMV).
|
||||
Set Processing Active indicator field (PACT) false.
|
||||
Exit.
|
||||
|
||||
@@ -1375,7 +1378,7 @@ enter_do_work:
|
||||
if (pmr->lvio != old_lvio)
|
||||
{
|
||||
MARK(M_LVIO);
|
||||
if (pmr->lvio && !pmr->set)
|
||||
if (pmr->lvio && (!pmr->set && !pmr->igset))
|
||||
{
|
||||
pmr->stop = 1;
|
||||
MARK(M_STOP);
|
||||
@@ -1406,9 +1409,8 @@ process_exit:
|
||||
alarm_sub(pmr); /* If we've violated alarm limits, yell. */
|
||||
monitor(pmr); /* If values have changed, broadcast them. */
|
||||
|
||||
if (pmr->dmov != 0 && pmr->ldmv == 0) /* Test for False to True transition. */
|
||||
if (pmr->dmov != 0)
|
||||
recGblFwdLink(pmr); /* Process the forward-scan-link record. */
|
||||
pmr->ldmv = pmr->dmov;
|
||||
|
||||
pmr->pact = 0;
|
||||
Debug(4, "process:---------------------- end; motor \"%s\"\n", pmr->name);
|
||||
@@ -1866,7 +1868,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
|
||||
WRITE_MSG(SET_ENC_RATIO, ep_mp);
|
||||
SEND_MSG();
|
||||
}
|
||||
if (pmr->set)
|
||||
if (pmr->set && !pmr->igset)
|
||||
{
|
||||
pmr->pp = TRUE;
|
||||
INIT_MSG();
|
||||
@@ -2080,7 +2082,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
|
||||
if (pmr->val != pmr->lval)
|
||||
{
|
||||
MARK(M_VAL);
|
||||
if (set && !pmr->foff)
|
||||
if ((set && !pmr->igset) && !pmr->foff)
|
||||
{
|
||||
/*
|
||||
* Act directly on .val. and return. User wants to redefine .val
|
||||
@@ -2130,7 +2132,7 @@ static RTN_STATUS do_work(motorRecord * pmr, CALLBACK_VALUE proc_ind)
|
||||
|
||||
pmr->rdif = NINT(pmr->diff / pmr->mres);
|
||||
MARK(M_RDIF);
|
||||
if (set)
|
||||
if (set && !pmr->igset)
|
||||
{
|
||||
if ((pmr->mip & MIP_LOAD_P) == 0) /* Test for LOAD_POS completion. */
|
||||
load_pos(pmr);
|
||||
@@ -2450,7 +2452,6 @@ static long special(DBADDR *paddr, int after)
|
||||
if (pmr->dmov == TRUE)
|
||||
{
|
||||
pmr->dmov = FALSE;
|
||||
pmr->ldmv = pmr->dmov;
|
||||
db_post_events(pmr, &pmr->dmov, DBE_VAL_LOG);
|
||||
}
|
||||
return(OK);
|
||||
|
||||
@@ -586,11 +586,6 @@ recordtype(motor) {
|
||||
special(SPC_NOMOD)
|
||||
initial("1")
|
||||
}
|
||||
field(LDMV,DBF_SHORT) {
|
||||
prompt("Last Done moving value")
|
||||
special(SPC_NOMOD)
|
||||
initial("1")
|
||||
}
|
||||
field(MOVN,DBF_SHORT) {
|
||||
prompt("Motor is moving")
|
||||
special(SPC_NOMOD)
|
||||
@@ -793,4 +788,8 @@ recordtype(motor) {
|
||||
pp(TRUE)
|
||||
interest(1)
|
||||
}
|
||||
field(IGSET,DBF_SHORT) {
|
||||
prompt("Ignore SET field")
|
||||
interest(2)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/*
|
||||
FILENAME... AG_CONEX.cpp
|
||||
USAGE... Motor driver support for the Newport CONEX-AGP and CONEX-CC series controllers.
|
||||
USAGE... Motor driver support for the Newport CONEX-AGP, CONEX-CC and CONEX-PP series controllers.
|
||||
|
||||
Mark Rivers
|
||||
April 11, 2013
|
||||
@@ -29,6 +29,9 @@ April 11, 2013
|
||||
#define CONEX_TIMEOUT 2.0
|
||||
#define LINUX_WRITE_DELAY 0.1
|
||||
|
||||
// We force minus end-of-run home type for Conex-PP for now
|
||||
#define HOME_TYPE_MINUS_EOR 4
|
||||
|
||||
/** Creates a new AG_CONEXController object.
|
||||
* \param[in] portName The name of the asyn port that will be created for this driver
|
||||
* \param[in] serialPortName The name of the drvAsynSerialPort that was created previously to connect to the CONEX controller
|
||||
@@ -186,6 +189,9 @@ AG_CONEXAxis::AG_CONEXAxis(AG_CONEXController *pC)
|
||||
KIMax_ = 1.e6;
|
||||
KDMax_ = 1.e6;
|
||||
}
|
||||
else if (strstr(pC->controllerVersion_, "Conex PP")) {
|
||||
conexModel_ = ModelConexPP;
|
||||
}
|
||||
else {
|
||||
asynPrint(pC->pasynUserSelf, ASYN_TRACE_ERROR,
|
||||
"%s: unknown model, firmware string=%s\n",
|
||||
@@ -196,12 +202,16 @@ AG_CONEXAxis::AG_CONEXAxis(AG_CONEXController *pC)
|
||||
// Read the stage ID
|
||||
sprintf(pC_->outString_, "%dID?", pC->controllerID_);
|
||||
pC_->writeReadController();
|
||||
strcpy(stageID_, &pC_->inString_[4]);
|
||||
strcpy(stageID_, &pC_->inString_[3]);
|
||||
|
||||
// Read the encoder increment
|
||||
sprintf(pC_->outString_, "%dSU?", pC->controllerID_);
|
||||
pC_->writeReadController();
|
||||
encoderIncrement_ = atof(&pC_->inString_[3]);
|
||||
// Read the encoder increment (CC and AGP only)
|
||||
if ((conexModel_ == ModelConexAGP) || (conexModel_ == ModelConexCC)) {
|
||||
sprintf(pC_->outString_, "%dSU?", pC->controllerID_);
|
||||
pC_->writeReadController();
|
||||
encoderIncrement_ = atof(&pC_->inString_[3]);
|
||||
} else {
|
||||
encoderIncrement_ = 1.;
|
||||
}
|
||||
|
||||
// Read the interpolation factor (AGP only)
|
||||
if (conexModel_ == ModelConexAGP) {
|
||||
@@ -212,8 +222,21 @@ AG_CONEXAxis::AG_CONEXAxis(AG_CONEXController *pC)
|
||||
interpolationFactor_ = 1.;
|
||||
}
|
||||
|
||||
if (conexModel_ == ModelConexPP) {
|
||||
sprintf(pC_->outString_, "%dFRM?", pC->controllerID_);
|
||||
pC_->writeReadController();
|
||||
microStepsPerFullStep_ = atoi(&pC_->inString_[4]);
|
||||
sprintf(pC_->outString_, "%dFRS?", pC->controllerID_);
|
||||
pC_->writeReadController();
|
||||
fullStepSize_ = atof(&pC_->inString_[4]);
|
||||
}
|
||||
|
||||
// Compute the minimum step size
|
||||
stepSize_ = encoderIncrement_ / interpolationFactor_;
|
||||
if ((conexModel_ == ModelConexAGP) || (conexModel_ == ModelConexCC)) {
|
||||
stepSize_ = encoderIncrement_ / interpolationFactor_;
|
||||
} else {
|
||||
stepSize_ = fullStepSize_ / microStepsPerFullStep_ / 1000.;
|
||||
}
|
||||
|
||||
// Read the low and high software limits
|
||||
sprintf(pC_->outString_, "%dSL?", pC->controllerID_);
|
||||
@@ -223,8 +246,10 @@ AG_CONEXAxis::AG_CONEXAxis(AG_CONEXController *pC)
|
||||
pC_->writeReadController();
|
||||
highLimit_ = atof(&pC_->inString_[3]);
|
||||
|
||||
// Tell the motor record that we have an gain supprt
|
||||
setIntegerParam(pC_->motorStatusGainSupport_, 1);
|
||||
// Tell the motor record that we have an gain support (CC and AGP only)
|
||||
if ((conexModel_ == ModelConexAGP) || (conexModel_ == ModelConexCC)) {
|
||||
setIntegerParam(pC_->motorStatusGainSupport_, 1);
|
||||
}
|
||||
}
|
||||
|
||||
/** Reports on status of the axis
|
||||
@@ -236,32 +261,36 @@ AG_CONEXAxis::AG_CONEXAxis(AG_CONEXController *pC)
|
||||
void AG_CONEXAxis::report(FILE *fp, int level)
|
||||
{
|
||||
if (level > 0) {
|
||||
// Read KOP, KI, LF
|
||||
sprintf(pC_->outString_, "%dKP?", pC_->controllerID_);
|
||||
pC_->writeReadController();
|
||||
KP_ = atof(&pC_->inString_[3]);
|
||||
sprintf(pC_->outString_, "%dKI?", pC_->controllerID_);
|
||||
pC_->writeReadController();
|
||||
KI_ = atof(&pC_->inString_[3]);
|
||||
if (conexModel_ == ModelConexAGP) {
|
||||
sprintf(pC_->outString_, "%dLF?", pC_->controllerID_);
|
||||
// Read KOP, KI, LF (CC and AGP only)
|
||||
if ((conexModel_ == ModelConexAGP) || (conexModel_ == ModelConexCC)) {
|
||||
sprintf(pC_->outString_, "%dKP?", pC_->controllerID_);
|
||||
pC_->writeReadController();
|
||||
LF_ = atof(&pC_->inString_[3]);
|
||||
} else if (conexModel_ == ModelConexCC) {
|
||||
sprintf(pC_->outString_, "%dKD?", pC_->controllerID_);
|
||||
KP_ = atof(&pC_->inString_[3]);
|
||||
sprintf(pC_->outString_, "%dKI?", pC_->controllerID_);
|
||||
pC_->writeReadController();
|
||||
KD_ = atof(&pC_->inString_[3]);
|
||||
LF_ = KD_; // For printout below
|
||||
KI_ = atof(&pC_->inString_[3]);
|
||||
if (conexModel_ == ModelConexAGP) {
|
||||
sprintf(pC_->outString_, "%dLF?", pC_->controllerID_);
|
||||
pC_->writeReadController();
|
||||
LF_ = atof(&pC_->inString_[3]);
|
||||
} else if (conexModel_ == ModelConexCC) {
|
||||
sprintf(pC_->outString_, "%dKD?", pC_->controllerID_);
|
||||
pC_->writeReadController();
|
||||
KD_ = atof(&pC_->inString_[3]);
|
||||
LF_ = KD_; // For printout below
|
||||
}
|
||||
}
|
||||
|
||||
fprintf(fp, " stageID=%s\n"
|
||||
" currentPosition=%f, encoderIncrement=%f\n"
|
||||
" interpolationFactor=%f, stepSize=%f, lowLimit=%f, highLimit=%f\n"
|
||||
" KP=%f, KI=%f, KD/LF=%f\n",
|
||||
" KP=%f, KI=%f, KD/LF=%f\n"
|
||||
" fullStepSize=%f, microStepsPerFullStep=%d\n",
|
||||
stageID_,
|
||||
currentPosition_, encoderIncrement_,
|
||||
interpolationFactor_, stepSize_, lowLimit_, highLimit_,
|
||||
KP_, KI_, LF_);
|
||||
KP_, KI_, LF_,
|
||||
fullStepSize_, microStepsPerFullStep_);
|
||||
}
|
||||
|
||||
// Call the base class method
|
||||
@@ -273,8 +302,8 @@ asynStatus AG_CONEXAxis::move(double position, int relative, double minVelocity,
|
||||
asynStatus status;
|
||||
// static const char *functionName = "AG_CONEXAxis::move";
|
||||
|
||||
// The CONEX-CC supports velocity and acceleration, the CONEX-AGP does not
|
||||
if (conexModel_ == ModelConexCC) {
|
||||
// The CONEX-CC and CONEX-PP support velocity and acceleration, the CONEX-AGP does not
|
||||
if ((conexModel_ == ModelConexCC) || (conexModel_ == ModelConexPP)) {
|
||||
sprintf(pC_->outString_, "%dAC%f", pC_->controllerID_, acceleration*stepSize_);
|
||||
status = pC_->writeCONEX();
|
||||
sprintf(pC_->outString_, "%dVA%f", pC_->controllerID_, maxVelocity*stepSize_);
|
||||
@@ -304,6 +333,14 @@ asynStatus AG_CONEXAxis::home(double minVelocity, double maxVelocity, double acc
|
||||
// and writing to non-volatile memory with the OH command.
|
||||
// This is time-consuming and can only be done a limited number of times so we don't do it here.
|
||||
|
||||
// The CONEX-PP supports home velocity and home type. We force negative limit switch home type.
|
||||
if (conexModel_ == ModelConexPP) {
|
||||
sprintf(pC_->outString_, "%dOH%f", pC_->controllerID_, maxVelocity);
|
||||
status = pC_->writeCONEX();
|
||||
sprintf(pC_->outString_, "%dHT%d", pC_->controllerID_, HOME_TYPE_MINUS_EOR);
|
||||
status = pC_->writeCONEX();
|
||||
}
|
||||
|
||||
sprintf(pC_->outString_, "%dOR", pC_->controllerID_);
|
||||
status = pC_->writeCONEX();
|
||||
return status;
|
||||
@@ -367,52 +404,58 @@ asynStatus AG_CONEXAxis::getClosedLoop(bool *closedLoop)
|
||||
|
||||
asynStatus AG_CONEXAxis::setPGain(double pGain)
|
||||
{
|
||||
asynStatus status;
|
||||
asynStatus status = asynSuccess;
|
||||
bool closedLoop;
|
||||
//static const char *functionName = "AG_CONEXAxis::setPGain";
|
||||
|
||||
getClosedLoop(&closedLoop);
|
||||
setClosedLoop(false);
|
||||
// The pGain value from the motor record is between 0 and 1.
|
||||
sprintf(pC_->outString_, "%dKP%f", pC_->controllerID_, pGain*KPMax_);
|
||||
status = pC_->writeCONEX();
|
||||
if (closedLoop) setClosedLoop(true);
|
||||
if ((conexModel_ == ModelConexAGP) || (conexModel_ == ModelConexCC)) {
|
||||
getClosedLoop(&closedLoop);
|
||||
setClosedLoop(false);
|
||||
// The pGain value from the motor record is between 0 and 1.
|
||||
sprintf(pC_->outString_, "%dKP%f", pC_->controllerID_, pGain*KPMax_);
|
||||
status = pC_->writeCONEX();
|
||||
if (closedLoop) setClosedLoop(true);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus AG_CONEXAxis::setIGain(double iGain)
|
||||
{
|
||||
asynStatus status;
|
||||
asynStatus status = asynSuccess;
|
||||
bool closedLoop;
|
||||
//static const char *functionName = "AG_CONEXAxis::setIGain";
|
||||
|
||||
getClosedLoop(&closedLoop);
|
||||
setClosedLoop(false);
|
||||
// The iGain value from the motor record is between 0 and 1.
|
||||
sprintf(pC_->outString_, "%dKI%f", pC_->controllerID_, iGain*KIMax_);
|
||||
status = pC_->writeCONEX();
|
||||
if (closedLoop) setClosedLoop(true);
|
||||
if ((conexModel_ == ModelConexAGP) || (conexModel_ == ModelConexCC)) {
|
||||
getClosedLoop(&closedLoop);
|
||||
setClosedLoop(false);
|
||||
// The iGain value from the motor record is between 0 and 1.
|
||||
sprintf(pC_->outString_, "%dKI%f", pC_->controllerID_, iGain*KIMax_);
|
||||
status = pC_->writeCONEX();
|
||||
if (closedLoop) setClosedLoop(true);
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus AG_CONEXAxis::setDGain(double dGain)
|
||||
{
|
||||
asynStatus status;
|
||||
asynStatus status = asynSuccess;
|
||||
bool closedLoop;
|
||||
//static const char *functionName = "AG_CONEXAxis::setPGain";
|
||||
|
||||
getClosedLoop(&closedLoop);
|
||||
setClosedLoop(false);
|
||||
if (conexModel_ == ModelConexCC) {
|
||||
// The dGain value from the motor record is between 0 and 1.
|
||||
sprintf(pC_->outString_, "%dKI%f", pC_->controllerID_, dGain*KDMax_);
|
||||
} else if (conexModel_ == ModelConexAGP) {
|
||||
// We are using the DGain for the Low pass filter frequency.
|
||||
// DGain value is between 0 and 1
|
||||
sprintf(pC_->outString_, "%dLF%f", pC_->controllerID_, dGain*LFMax_);
|
||||
if ((conexModel_ == ModelConexAGP) || (conexModel_ == ModelConexCC)) {
|
||||
getClosedLoop(&closedLoop);
|
||||
setClosedLoop(false);
|
||||
if (conexModel_ == ModelConexCC) {
|
||||
// The dGain value from the motor record is between 0 and 1.
|
||||
sprintf(pC_->outString_, "%dKI%f", pC_->controllerID_, dGain*KDMax_);
|
||||
} else if (conexModel_ == ModelConexAGP) {
|
||||
// We are using the DGain for the Low pass filter frequency.
|
||||
// DGain value is between 0 and 1
|
||||
sprintf(pC_->outString_, "%dLF%f", pC_->controllerID_, dGain*LFMax_);
|
||||
}
|
||||
status = pC_->writeCONEX();
|
||||
if (closedLoop) setClosedLoop(true);
|
||||
}
|
||||
status = pC_->writeCONEX();
|
||||
if (closedLoop) setClosedLoop(true);
|
||||
return status;
|
||||
}
|
||||
|
||||
@@ -455,10 +498,10 @@ asynStatus AG_CONEXAxis::poll(bool *moving)
|
||||
setIntegerParam(pC_->motorStatusDone_, done);
|
||||
*moving = done ? false:true;
|
||||
|
||||
// The meaning of the error bits is different for the CC and AGP
|
||||
if (conexModel_ == ModelConexCC) {
|
||||
if (status & 0x100) lowLimit = 1;
|
||||
if (status & 0x200) highLimit = 1;
|
||||
// The meaning of the error bits is different for the CC, AGP, and PP
|
||||
if ((conexModel_ == ModelConexCC) || (conexModel_ == ModelConexPP)) {
|
||||
if (status & 0x100) lowLimit = 1;
|
||||
if (status & 0x200) highLimit = 1;
|
||||
}
|
||||
|
||||
setIntegerParam(pC_->motorStatusLowLimit_, lowLimit);
|
||||
|
||||
@@ -12,7 +12,8 @@ April 11, 2013
|
||||
|
||||
typedef enum {
|
||||
ModelConexAGP,
|
||||
ModelConexCC
|
||||
ModelConexCC,
|
||||
ModelConexPP
|
||||
} ConexModel_t;
|
||||
|
||||
// No controller-specific parameters yet
|
||||
@@ -43,6 +44,8 @@ private:
|
||||
double encoderIncrement_;
|
||||
double interpolationFactor_;
|
||||
double stepSize_;
|
||||
double fullStepSize_;
|
||||
int microStepsPerFullStep_;
|
||||
double highLimit_;
|
||||
double lowLimit_;
|
||||
double KP_;
|
||||
|
||||
@@ -5,6 +5,7 @@ driver(drvOms)
|
||||
# Oregon Micro Systems VME58 driver support.
|
||||
device(motor,VME_IO,devOms58,"OMS VME58")
|
||||
driver(drvOms58)
|
||||
registrar(oms58Registrar)
|
||||
|
||||
# Oregon Micro Systems MAXv driver support.
|
||||
device(motor,VME_IO,devMAXv,"OMS MAXv")
|
||||
|
||||
+25
-20
@@ -93,7 +93,8 @@ USAGE... Motor record driver level support for OMS model MAXv.
|
||||
* command into two commands.
|
||||
* - Fix for intermittent wrong command displayed from Command Error message. motorIsr() saves the
|
||||
* message in a separate static buffer.
|
||||
* 26 11-05-13 rls - Valid IRQ levels are 2 thru 6.
|
||||
* 26 02-08-16 rls - Valid IRQ levels are 2 thru 6.
|
||||
* 27 04-04-17 rls - Added error check for new failure mode where board reboots after 1st command with response.
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -167,7 +168,7 @@ static epicsAddressType MAXv_ADDRS_TYPE;
|
||||
static volatile unsigned MAXvInterruptVector = 0;
|
||||
static volatile epicsUInt8 omsInterruptLevel = OMS_INT_LEVEL;
|
||||
static volatile int motionTO = 10;
|
||||
static char *MAXv_axis[] = {"X", "Y", "Z", "T", "U", "V", "R", "S"};
|
||||
static const char *MAXv_axis[] = {"X", "Y", "Z", "T", "U", "V", "R", "S"};
|
||||
static double quantum;
|
||||
static char **initstring = 0;
|
||||
static epicsUInt32 MAXv_brd_size; /* card address boundary */
|
||||
@@ -254,7 +255,7 @@ struct driver_table MAXv_access =
|
||||
query_done,
|
||||
NULL,
|
||||
&initialized,
|
||||
MAXv_axis
|
||||
(char **) MAXv_axis
|
||||
};
|
||||
|
||||
struct drvMAXv_drvet
|
||||
@@ -308,10 +309,10 @@ static void query_done(int card, int axis, struct mess_node *nodeptr)
|
||||
{
|
||||
char buffer[MAX_IDENT_LEN];
|
||||
|
||||
send_recv_mess(card, DONE_QUERY, MAXv_axis[axis], buffer, 1);
|
||||
send_recv_mess(card, DONE_QUERY, (char *) MAXv_axis[axis], buffer, 1);
|
||||
|
||||
if (nodeptr->status.Bits.RA_PROBLEM)
|
||||
send_mess(card, AXIS_STOP, MAXv_axis[axis]);
|
||||
send_mess(card, AXIS_STOP, (char *) MAXv_axis[axis]);
|
||||
}
|
||||
|
||||
|
||||
@@ -372,7 +373,6 @@ static int set_status(int card, int signal)
|
||||
char q_buf[MAX_IDENT_LEN], outbuf[50];
|
||||
int index;
|
||||
bool ls_active = false;
|
||||
bool got_encoder;
|
||||
msta_field status;
|
||||
|
||||
int absoluteAxis = (configurationFlags[card] & (1 << signal));
|
||||
@@ -406,16 +406,14 @@ static int set_status(int card, int signal)
|
||||
if (motor_info->encoder_present == YES)
|
||||
{
|
||||
/* get 4 pieces of info from axis */
|
||||
send_recv_mess(card, "QA", MAXv_axis[signal], &q_buf[0], 1);
|
||||
send_recv_mess(card, "QA", (char *) MAXv_axis[signal], &q_buf[0], 1);
|
||||
q_buf[4] = ',';
|
||||
send_recv_mess(card, "EA", MAXv_axis[signal], &q_buf[5], 1);
|
||||
got_encoder = true;
|
||||
send_recv_mess(card, "EA", (char *) MAXv_axis[signal], &q_buf[5], 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* get 2 pieces of info from axis */
|
||||
send_recv_mess(card, AXIS_INFO, MAXv_axis[signal], q_buf, 1);
|
||||
got_encoder = false;
|
||||
send_recv_mess(card, AXIS_INFO, (char *) MAXv_axis[signal], q_buf, 1);
|
||||
}
|
||||
|
||||
for (index = 0, p = epicsStrtok_r(q_buf, ",", &tok_save); p;
|
||||
@@ -477,7 +475,7 @@ static int set_status(int card, int signal)
|
||||
if (motor_info->no_motion_count > motionTO)
|
||||
{
|
||||
status.Bits.RA_PROBLEM = 1;
|
||||
send_mess(card, AXIS_STOP, MAXv_axis[signal]);
|
||||
send_mess(card, AXIS_STOP, (char *) MAXv_axis[signal]);
|
||||
motor_info->no_motion_count = 0;
|
||||
errlogSevPrintf(errlogMinor, "Motor motion timeout ERROR on card: %d, signal: %d\n",
|
||||
card, signal);
|
||||
@@ -491,7 +489,7 @@ static int set_status(int card, int signal)
|
||||
status.Bits.RA_PROBLEM = 0;
|
||||
|
||||
/* get command velocity */
|
||||
send_recv_mess(card, "RV", MAXv_axis[signal], q_buf, 1);
|
||||
send_recv_mess(card, "RV", (char *) MAXv_axis[signal], q_buf, 1);
|
||||
motor_info->velocity = atoi(q_buf);
|
||||
|
||||
/* Get encoder position */
|
||||
@@ -584,7 +582,7 @@ errorexit: errMessage(-1, "Invalid device directive");
|
||||
strcpy(buffer, nodeptr->postmsgptr);
|
||||
|
||||
strcpy(outbuf, buffer);
|
||||
send_mess(card, outbuf, MAXv_axis[signal]);
|
||||
send_mess(card, outbuf, (char *) MAXv_axis[signal]);
|
||||
nodeptr->postmsgptr = NULL;
|
||||
}
|
||||
|
||||
@@ -1193,6 +1191,7 @@ static int motor_init()
|
||||
epicsInt8 *startAddr;
|
||||
epicsInt8 *endAddr;
|
||||
bool wdtrip;
|
||||
int rtn_code;
|
||||
|
||||
Debug(2, "motor_init: card %d\n", card_index);
|
||||
|
||||
@@ -1265,7 +1264,13 @@ static int motor_init()
|
||||
send_mess(card_index, ERROR_CLEAR, (char) NULL);
|
||||
send_mess(card_index, STOP_ALL, (char) NULL);
|
||||
|
||||
send_recv_mess(card_index, GET_IDENT, (char) NULL, (char *) pmotorState->ident, 1);
|
||||
rtn_code = send_recv_mess(card_index, GET_IDENT, (char) NULL, (char *) pmotorState->ident, 1);
|
||||
if (rtn_code != 0)
|
||||
{
|
||||
errlogPrintf("\n***MAXv card #%d Disabled*** not responding to commands!\n\n", card_index);
|
||||
motor_state[card_index] = (struct controller *) NULL;
|
||||
goto loopend;
|
||||
}
|
||||
Debug(3, "Identification = %s\n", pmotorState->ident);
|
||||
|
||||
/* Save firmware version. */
|
||||
@@ -1308,7 +1313,7 @@ static int motor_init()
|
||||
STATUS1 flag1;
|
||||
|
||||
/* Test if motor has an encoder. */
|
||||
send_mess(card_index, ENCODER_QUERY, MAXv_axis[motor_index]);
|
||||
send_mess(card_index, ENCODER_QUERY, (char *) MAXv_axis[motor_index]);
|
||||
while (!pmotor->status1_flag.Bits.done) /* Wait for command to complete. */
|
||||
epicsThreadSleep(quantum);
|
||||
|
||||
@@ -1327,7 +1332,7 @@ static int motor_init()
|
||||
}
|
||||
|
||||
/* Test if motor has PID parameters. */
|
||||
send_mess(card_index, PID_QUERY, MAXv_axis[motor_index]);
|
||||
send_mess(card_index, PID_QUERY, (char *) MAXv_axis[motor_index]);
|
||||
while (!pmotor->status1_flag.Bits.done) /* Wait for command to complete. */
|
||||
epicsThreadSleep(quantum);
|
||||
if (pmotor->status1_flag.Bits.cmndError)
|
||||
@@ -1354,9 +1359,9 @@ static int motor_init()
|
||||
|
||||
if (pvtdata->fwver >= 1.30)
|
||||
{
|
||||
send_recv_mess(card_index, "LM?", MAXv_axis[motor_index], axis_pos, 1);
|
||||
send_recv_mess(card_index, "LM?", (char *) MAXv_axis[motor_index], axis_pos, 1);
|
||||
if (strcmp(axis_pos, "=f") == 0) /* If limit mode is set to "Off". */
|
||||
send_mess(card_index, "LMH", MAXv_axis[motor_index]); /* Set limit mode to "Hard". */
|
||||
send_mess(card_index, "LMH", (char *) MAXv_axis[motor_index]); /* Set limit mode to "Hard". */
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1383,7 +1388,7 @@ static int motor_init()
|
||||
|
||||
set_status(card_index, motor_index);
|
||||
/* Is this needed??? */
|
||||
send_recv_mess(card_index, DONE_QUERY, MAXv_axis[motor_index], axis_pos, 1);
|
||||
send_recv_mess(card_index, DONE_QUERY, (char *) MAXv_axis[motor_index], axis_pos, 1);
|
||||
}
|
||||
|
||||
Debug(2, "motor_init: Init Address=%p\n", localaddr);
|
||||
|
||||
@@ -111,6 +111,13 @@ USAGE... Motor record driver level support for OMS model VME58.
|
||||
* .41 10-20-11 rls - Added counter in send_mess() to prevent endless loop
|
||||
* after VME58 reboot.
|
||||
* .42 07-26-12 rls - Added reboot test to send_mess().
|
||||
* .43 02-21-16 rls - Added code to send_mess() that test for a VME58 reboot
|
||||
* after a motion related (ST, MA, MR) command. Found new
|
||||
* VME58 failure mode where board reboots after the 1st
|
||||
* motion related command. Since delay between motion
|
||||
* command and reboot test must be long enough to avoid a
|
||||
* VMEbus error, delay is excessive for normal operation.
|
||||
* Hence, the oms58_reboot_test external variable.
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -134,6 +141,7 @@ USAGE... Motor record driver level support for OMS model VME58.
|
||||
#include "drvOms58.h"
|
||||
|
||||
#include "epicsExport.h"
|
||||
#include "iocsh.h"
|
||||
|
||||
#define PRIVATE_FUNCTIONS 1 /* normal:1, debug:0 */
|
||||
|
||||
@@ -167,6 +175,7 @@ static inline void Debug(int level, const char *format, ...) {
|
||||
|
||||
/* Global data. */
|
||||
int oms58_num_cards = 0;
|
||||
int oms58_reboot_test = 0;
|
||||
|
||||
/* Local data required for every driver; see "motordrvComCode.h" */
|
||||
#include "motordrvComCode.h"
|
||||
@@ -720,7 +729,20 @@ static RTN_STATUS send_mess(int card, char const *com, char *name)
|
||||
Debug(4, "send_mess: sent card %d message:", card);
|
||||
Debug(4, "%s\n", outbuf);
|
||||
|
||||
pmotor->outPutIndex = putIndex; /* Message Sent */
|
||||
pmotor->outPutIndex = putIndex; /* Send message. */
|
||||
|
||||
if (oms58_reboot_test != 0) /* Test if board has rebooted after sending message. */
|
||||
{
|
||||
epicsThreadSleep(0.300);
|
||||
if (pmotor->rebootind != 0x4321)
|
||||
{
|
||||
errlogPrintf(rebootmsg, card);
|
||||
/* Disable board. */
|
||||
motor_state[card] = (struct controller *)NULL;
|
||||
epicsThreadSleep(1.0);
|
||||
return (ERROR);
|
||||
}
|
||||
}
|
||||
|
||||
for (count = 0; (putIndex != pmotor->outGetIndex) && (count < 1000); count++)
|
||||
{
|
||||
@@ -1336,4 +1358,28 @@ static void oms_reset(void *arg)
|
||||
}
|
||||
|
||||
|
||||
/* Epics iocsh bindings */
|
||||
|
||||
static const iocshArg oms58Arg0 = {"num_card", iocshArgInt};
|
||||
static const iocshArg oms58Arg1 = {"addrs", iocshArgInt};
|
||||
static const iocshArg oms58Arg2 = {"vector", iocshArgInt};
|
||||
static const iocshArg oms58Arg3 = {"int_level", iocshArgInt};
|
||||
static const iocshArg oms58Arg4 = {"scan_rate", iocshArgInt};
|
||||
|
||||
static const iocshArg* const oms58Args[5] = {&oms58Arg0, &oms58Arg1, &oms58Arg2, &oms58Arg3, &oms58Arg4};
|
||||
|
||||
static const iocshFuncDef oms58FuncDef = {"oms58Setup", 5, oms58Args};
|
||||
|
||||
static void oms58CallFunc(const iocshArgBuf* args)
|
||||
{
|
||||
oms58Setup(args[0].ival, (void*) args[1].ival, (unsigned) args[2].ival, args[3].ival, args[4].ival);
|
||||
}
|
||||
|
||||
void oms58Registrar(void)
|
||||
{
|
||||
iocshRegister(&oms58FuncDef, &oms58CallFunc);
|
||||
}
|
||||
|
||||
epicsExportRegistrar(oms58Registrar);
|
||||
|
||||
/*---------------------------------------------------------------------*/
|
||||
|
||||
@@ -46,6 +46,8 @@ PIGCSController* PIGCSController::CreateGCSController(PIInterface* pInterface, c
|
||||
|| strstr(szIDN, "C-867") != NULL
|
||||
|| strstr(szIDN, "C-884") != NULL
|
||||
|| strstr(szIDN, "E-861") != NULL
|
||||
|| strstr(szIDN, "E-871") != NULL
|
||||
|| strstr(szIDN, "E-873") != NULL
|
||||
)
|
||||
{
|
||||
return new PIGCSMotorController(pInterface, szIDN);
|
||||
@@ -111,6 +113,10 @@ bool PIGCSController::IsGCS2(PIInterface* pInterface)
|
||||
|
||||
asynStatus PIGCSController::setVelocityCts( PIasynAxis* pAxis, double velocity )
|
||||
{
|
||||
if (!m_KnowsVELcommand)
|
||||
{
|
||||
return asynSuccess;
|
||||
}
|
||||
char cmd[100];
|
||||
velocity = fabs(velocity) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
sprintf(cmd,"VEL %s %f", pAxis->m_szAxisName, velocity);
|
||||
@@ -247,17 +253,21 @@ int PIGCSController::getGCSError()
|
||||
if (0 != errorCode)
|
||||
{
|
||||
m_LastError = errorCode;
|
||||
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"PIGCSController::getGCSError() GCS error code = %d\n",
|
||||
errorCode);
|
||||
char szErrorMsg[1024];
|
||||
if (TranslatePIError(errorCode, szErrorMsg, 1024))
|
||||
{
|
||||
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"PIGCSController::getGCSError() GCS error, %s\n",
|
||||
szErrorMsg);
|
||||
if (m_pInterface->m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"PIGCSController::getGCSError() GCS error code = %d\n",
|
||||
errorCode);
|
||||
|
||||
char szErrorMsg[1024];
|
||||
if (TranslatePIError(errorCode, szErrorMsg, 1024))
|
||||
{
|
||||
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"PIGCSController::getGCSError() GCS error, %s\n",
|
||||
szErrorMsg);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return errorCode;
|
||||
}
|
||||
@@ -308,6 +318,10 @@ asynStatus PIGCSController::getAxisPosition(PIasynAxis* pAxis, double& position)
|
||||
*/
|
||||
asynStatus PIGCSController::getAxisVelocity(PIasynAxis* pAxis)
|
||||
{
|
||||
if (!m_KnowsVELcommand)
|
||||
{
|
||||
return asynSuccess;
|
||||
}
|
||||
char cmd[100];
|
||||
char buf[255];
|
||||
sprintf(cmd, "VEL? %s", pAxis->m_szAxisName);
|
||||
@@ -443,7 +457,7 @@ asynStatus PIGCSController::getAxisPositionCts(PIasynAxis* pAxis)
|
||||
pAxis->m_position = pos;
|
||||
if (pAxis->m_CPUdenominator==0 || pAxis->m_CPUnumerator==0)
|
||||
{
|
||||
pAxis->m_positionCts = pos;
|
||||
pAxis->m_positionCts = int(pos);
|
||||
return status;
|
||||
}
|
||||
|
||||
@@ -577,7 +591,7 @@ asynStatus PIGCSController::initAxis(PIasynAxis* pAxis)
|
||||
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSController::initAxis() stage configuration: %s\n", buf);
|
||||
}
|
||||
pAxis->m_movingStateMask = pow(2.0, pAxis->getAxisNo());
|
||||
pAxis->m_movingStateMask = int (pow(2.0, pAxis->getAxisNo()) );
|
||||
|
||||
return setServo(pAxis, 1);
|
||||
}
|
||||
@@ -585,6 +599,15 @@ asynStatus PIGCSController::initAxis(PIasynAxis* pAxis)
|
||||
asynStatus PIGCSController::init(void)
|
||||
{
|
||||
asynStatus status;
|
||||
char buffer [1024];
|
||||
status = m_pInterface->sendAndReceive("VEL?", buffer, 1023);
|
||||
m_KnowsVELcommand = ( asynSuccess == status);
|
||||
if (!m_KnowsVELcommand)
|
||||
{
|
||||
(void) getGCSError ();
|
||||
}
|
||||
|
||||
|
||||
status = findConnectedAxes();
|
||||
return status;
|
||||
}
|
||||
|
||||
@@ -109,6 +109,8 @@ protected:
|
||||
size_t m_nrFoundAxes;
|
||||
char m_allAxesIDs[255];
|
||||
int m_LastError;
|
||||
|
||||
bool m_KnowsVELcommand;
|
||||
};
|
||||
|
||||
#endif /* PIGCSCONTROLLER_H_ */
|
||||
|
||||
@@ -24,87 +24,87 @@ Created: 15.12.2010
|
||||
|
||||
asynStatus PIGCSMotorController::initAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
asynStatus status = hasLimitSwitches(pAxis);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
status = hasReferenceSensor(pAxis);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
return PIGCSController::initAxis(pAxis);
|
||||
asynStatus status = hasLimitSwitches(pAxis);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
status = hasReferenceSensor(pAxis);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
return PIGCSController::initAxis(pAxis);
|
||||
}
|
||||
|
||||
asynStatus PIGCSMotorController::setAccelerationCts( PIasynAxis* pAxis, double accelerationCts)
|
||||
{
|
||||
double acceleration = fabs(accelerationCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
if (acceleration == pAxis->m_acceleration)
|
||||
return asynSuccess;
|
||||
if (pAxis->m_maxAcceleration < 0)
|
||||
{
|
||||
getMaxAcceleration(pAxis);
|
||||
}
|
||||
if (acceleration > pAxis->m_maxAcceleration)
|
||||
acceleration = pAxis->m_maxAcceleration;
|
||||
double acceleration = fabs(accelerationCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
if (acceleration == pAxis->m_acceleration)
|
||||
return asynSuccess;
|
||||
if (pAxis->m_maxAcceleration < 0)
|
||||
{
|
||||
getMaxAcceleration(pAxis);
|
||||
}
|
||||
if (acceleration > pAxis->m_maxAcceleration)
|
||||
acceleration = pAxis->m_maxAcceleration;
|
||||
|
||||
return setAcceleration(pAxis, acceleration);
|
||||
return setAcceleration(pAxis, acceleration);
|
||||
}
|
||||
|
||||
asynStatus PIGCSMotorController::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
|
||||
{
|
||||
asynStatus status = setServo(pAxis, 1);
|
||||
asynStatus status = setServo(pAxis, 1);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
return status;
|
||||
|
||||
char cmd[100];
|
||||
if (velocity != 0)
|
||||
{
|
||||
velocity = fabs(velocity) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
sprintf(cmd,"SPA %s 0x50 %f", pAxis->m_szAxisName, velocity);
|
||||
m_pInterface->sendOnly(cmd);
|
||||
}
|
||||
char cmd[100];
|
||||
if (velocity != 0)
|
||||
{
|
||||
velocity = fabs(velocity) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
sprintf(cmd,"SPA %s 0x50 %f", pAxis->m_szAxisName, velocity);
|
||||
m_pInterface->sendOnly(cmd);
|
||||
}
|
||||
|
||||
if (pAxis->m_bHasReference)
|
||||
{
|
||||
// call FRF - find reference
|
||||
sprintf(cmd,"FRF %s", pAxis->m_szAxisName);
|
||||
}
|
||||
else if (pAxis->m_bHasLimitSwitches)
|
||||
{
|
||||
if (forwards)
|
||||
{
|
||||
// call FPL - find positive limit switch
|
||||
sprintf(cmd,"FPL %s", pAxis->m_szAxisName);
|
||||
}
|
||||
else
|
||||
{
|
||||
// call FNL - find negative limit switch
|
||||
sprintf(cmd,"FNL %s", pAxis->m_szAxisName);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIGCSMotorController::referenceVelCts() failed - axis has no reference/limit switch\n");
|
||||
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
|
||||
"PIGCSMotorController::referenceVelCts() failed - axis has no reference/limit switch\n");
|
||||
return asynError;
|
||||
}
|
||||
status = m_pInterface->sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode == 0)
|
||||
{
|
||||
return asynSuccess;
|
||||
}
|
||||
if (pAxis->m_bHasReference)
|
||||
{
|
||||
// call FRF - find reference
|
||||
sprintf(cmd,"FRF %s", pAxis->m_szAxisName);
|
||||
}
|
||||
else if (pAxis->m_bHasLimitSwitches)
|
||||
{
|
||||
if (forwards)
|
||||
{
|
||||
// call FPL - find positive limit switch
|
||||
sprintf(cmd,"FPL %s", pAxis->m_szAxisName);
|
||||
}
|
||||
else
|
||||
{
|
||||
// call FNL - find negative limit switch
|
||||
sprintf(cmd,"FNL %s", pAxis->m_szAxisName);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIGCSMotorController::referenceVelCts() failed - axis has no reference/limit switch\n");
|
||||
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
|
||||
"PIGCSMotorController::referenceVelCts() failed - axis has no reference/limit switch\n");
|
||||
return asynError;
|
||||
}
|
||||
status = m_pInterface->sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode == 0)
|
||||
{
|
||||
return asynSuccess;
|
||||
}
|
||||
asynPrint(m_pInterface->m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIGCSMotorController::referenceVelCts() failed\n");
|
||||
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
|
||||
"PIGCSMotorController::referenceVelCts() failed - GCS Error %d\n",errorCode);
|
||||
return asynError;
|
||||
"PIGCSMotorController::referenceVelCts() failed\n");
|
||||
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
|
||||
"PIGCSMotorController::referenceVelCts() failed - GCS Error %d\n",errorCode);
|
||||
return asynError;
|
||||
|
||||
}
|
||||
|
||||
@@ -113,18 +113,18 @@ asynStatus PIGCSMotorController::referenceVelCts( PIasynAxis* pAxis, double velo
|
||||
*/
|
||||
asynStatus PIGCSMotorController::getResolution(PIasynAxis* pAxis, double& resolution )
|
||||
{
|
||||
// CPU is "Counts Per Unit"
|
||||
// this is stored as two integers in the controller
|
||||
// CPU is "Counts Per Unit"
|
||||
// this is stored as two integers in the controller
|
||||
double num, denom;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_MOT_CPU_Z, num);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
return status;
|
||||
}
|
||||
status = getGCSParameter(pAxis, PI_PARA_MOT_CPU_N, denom);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
return status;
|
||||
}
|
||||
pAxis->m_CPUnumerator = num;
|
||||
pAxis->m_CPUdenominator = denom;
|
||||
@@ -135,11 +135,11 @@ asynStatus PIGCSMotorController::getResolution(PIasynAxis* pAxis, double& resolu
|
||||
|
||||
asynStatus PIGCSMotorController::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
char buf[255];
|
||||
char buf[255];
|
||||
asynStatus status = m_pInterface->sendAndReceive(char(4), buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
return status;
|
||||
}
|
||||
// TODO this is for a single axis C-863/867 controller!!!!
|
||||
// TODO a) change it to multi-axis code.
|
||||
@@ -163,35 +163,43 @@ asynStatus PIGCSMotorController::getStatus(PIasynAxis* pAxis, int& homing, int&
|
||||
|
||||
asynStatus PIGCSMotorController::getMaxAcceleration(PIasynAxis* pAxis)
|
||||
{
|
||||
double maxAcc, maxDec;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_MOT_MAX_ACCEL, maxAcc);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
status = getGCSParameter(pAxis, PI_PARA_MOT_MAX_DECEL, maxDec);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
if (!m_KnowsVELcommand)
|
||||
{
|
||||
return asynSuccess;
|
||||
}
|
||||
double maxAcc, maxDec;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_MOT_MAX_ACCEL, maxAcc);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
status = getGCSParameter(pAxis, PI_PARA_MOT_MAX_DECEL, maxDec);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
|
||||
if (maxAcc < maxDec)
|
||||
{
|
||||
pAxis->m_maxAcceleration = maxAcc;
|
||||
}
|
||||
else
|
||||
{
|
||||
pAxis->m_maxAcceleration = maxDec;
|
||||
}
|
||||
return status;
|
||||
if (maxAcc < maxDec)
|
||||
{
|
||||
pAxis->m_maxAcceleration = maxAcc;
|
||||
}
|
||||
else
|
||||
{
|
||||
pAxis->m_maxAcceleration = maxDec;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSMotorController::setAcceleration( PIasynAxis* pAxis, double acceleration)
|
||||
{
|
||||
asynStatus status = setGCSParameter(pAxis, PI_PARA_MOT_CURR_ACCEL, acceleration);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
status = setGCSParameter(pAxis, PI_PARA_MOT_CURR_DECEL, acceleration);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
pAxis->m_acceleration = acceleration;
|
||||
return status;
|
||||
if (!m_KnowsVELcommand)
|
||||
{
|
||||
return asynSuccess;
|
||||
}
|
||||
asynStatus status = setGCSParameter(pAxis, PI_PARA_MOT_CURR_ACCEL, acceleration);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
status = setGCSParameter(pAxis, PI_PARA_MOT_CURR_DECEL, acceleration);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
pAxis->m_acceleration = acceleration;
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,859 +0,0 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <string>
|
||||
|
||||
#include <iocsh.h>
|
||||
#include <epicsThread.h>
|
||||
|
||||
#include <asynOctetSyncIO.h>
|
||||
|
||||
#include "asynMotorController.h"
|
||||
#include "asynMotorAxis.h"
|
||||
|
||||
#include <epicsExport.h>
|
||||
#include "ScriptMotorDriver.h"
|
||||
|
||||
#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5)
|
||||
|
||||
|
||||
/************************************************
|
||||
* These are the ScriptMotorController methods *
|
||||
************************************************/
|
||||
|
||||
|
||||
/** Creates a new ScriptMotorController object.
|
||||
* \param[in] asyn_port The name of the asyn port that will be created for this driver
|
||||
* \param[in] serial_port The name of the drvAsynSerialPort that was created previously to connect to the VirtualMotor controller
|
||||
* \param[in] max_axes The number of axes that this controller supports
|
||||
* \param[in] script_file
|
||||
* \param[in] params
|
||||
*/
|
||||
ScriptMotorController::ScriptMotorController(const char* asyn_port,
|
||||
int max_axes,
|
||||
const char* script_file,
|
||||
const char* params)
|
||||
: asynMotorController(asyn_port,
|
||||
max_axes,
|
||||
1, // No. ScriptMotorController asyn parameters
|
||||
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, // Default priority
|
||||
0) // Default stack size
|
||||
{
|
||||
this->script = std::string(script_file);
|
||||
|
||||
if (params) { this->init_params = std::string(params); }
|
||||
else { this->init_params = std::string(""); }
|
||||
|
||||
this->createParam("RELOAD_SCRIPT", asynParamInt32, &this->ScriptMotorReload);
|
||||
|
||||
for (int axis = 0; axis < max_axes; axis += 1)
|
||||
{
|
||||
new ScriptMotorAxis(this, axis, script_file, params);
|
||||
}
|
||||
|
||||
this->startPoller(this->movingPollPeriod_, this->idlePollPeriod_, this->forcedFastPolls_);
|
||||
}
|
||||
|
||||
void ScriptMotorController::reload()
|
||||
{
|
||||
this->lock();
|
||||
for (int index = 0; index < this->numAxes_; index += 1)
|
||||
{
|
||||
ScriptMotorAxis* axis = this->getAxis(index);
|
||||
axis->reload(this->script.c_str(), this->init_params.c_str());
|
||||
}
|
||||
this->unlock();
|
||||
|
||||
printf("Controller %s reloaded %s.\n", this->portName, this->script.c_str());
|
||||
}
|
||||
|
||||
/** Creates a new ScriptMotorController object.
|
||||
* Configuration command, called directly or from iocsh
|
||||
* \param[in] asyn_port The name of the asyn port that will be created for this driver
|
||||
* \param[in] max_axes The number of axes that this controller supports
|
||||
* \param[in] script_file
|
||||
* \param[in] params
|
||||
*/
|
||||
extern "C" int ScriptControllerConfig(const char* asyn_port,
|
||||
int max_axes,
|
||||
const char* script_file,
|
||||
const char* params)
|
||||
{
|
||||
new ScriptMotorController(asyn_port, max_axes, script_file, params);
|
||||
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 ScriptMotorController::report(FILE *fp, int level)
|
||||
{
|
||||
fprintf(fp, "Script Motor Controller driver %s\n", this->portName);
|
||||
fprintf(fp, " numAxes=%d\n", numAxes_);
|
||||
fprintf(fp, " moving poll period=%f\n", movingPollPeriod_);
|
||||
fprintf(fp, " idle poll period=%f\n", idlePollPeriod_);
|
||||
|
||||
// Call the base class method
|
||||
asynMotorController::report(fp, level);
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value)
|
||||
{
|
||||
int function = pasynUser->reason;
|
||||
|
||||
if (function == this->ScriptMotorReload)
|
||||
{
|
||||
if (value == 1) { this->reload(); }
|
||||
return asynSuccess;
|
||||
}
|
||||
else
|
||||
{
|
||||
return asynMotorController::writeInt32(pasynUser, value);
|
||||
}
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorController::setIntegerParam(int list, int function, int value)
|
||||
{
|
||||
if (function >= this->motorStatusDirection_ && function <= this->motorStatusHomed_)
|
||||
{
|
||||
ScriptMotorAxis* axis = (ScriptMotorAxis*) this->getAxis(list);
|
||||
epicsUInt32 status = axis->setStatusParam(function, value);
|
||||
asynMotorController::setIntegerParam(list, this->motorStatus_, status);
|
||||
}
|
||||
|
||||
return asynMotorController::setIntegerParam(list, function, value);
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorController::setDoubleParam(int list, int function, double value)
|
||||
{
|
||||
if (function == this->motorPosition_ || function == this->motorEncoderPosition_)
|
||||
{
|
||||
ScriptMotorAxis* axis = (ScriptMotorAxis*) this->getAxis(list);
|
||||
axis->setPositionParam(function, value);
|
||||
}
|
||||
|
||||
return asynMotorController::setDoubleParam(list, function, value);
|
||||
}
|
||||
|
||||
void ScriptMotorController::configAxis(int axisNo, const char* params)
|
||||
{
|
||||
ScriptMotorAxis* axis = this->getAxis(axisNo);
|
||||
|
||||
if (params) { axis->params = std::string(params); }
|
||||
else { axis->params = std::string(""); }
|
||||
|
||||
axis->config(params);
|
||||
}
|
||||
|
||||
/** Returns a pointer to an ScriptMotorAxis object.
|
||||
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
||||
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
|
||||
ScriptMotorAxis* ScriptMotorController::getAxis(asynUser *pasynUser)
|
||||
{
|
||||
return static_cast<ScriptMotorAxis*>(asynMotorController::getAxis(pasynUser));
|
||||
}
|
||||
|
||||
|
||||
/** Returns a pointer to an ScriptMotorAxis object.
|
||||
* Returns NULL if the axis number encoded in pasynUser is invalid.
|
||||
* \param[in] axisNo Axis index number. */
|
||||
ScriptMotorAxis* ScriptMotorController::getAxis(int axisNo)
|
||||
{
|
||||
return static_cast<ScriptMotorAxis*>(asynMotorController::getAxis(axisNo));
|
||||
}
|
||||
|
||||
|
||||
/******************************************
|
||||
* These are the ScriptMotorAxis methods *
|
||||
******************************************/
|
||||
|
||||
|
||||
/** Creates a new ScriptMotorAxis object.
|
||||
* \param[in] pC Pointer to the ScriptMotorController to which this axis belongs.
|
||||
* \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
|
||||
*
|
||||
* Initializes register numbers, etc.
|
||||
*/
|
||||
ScriptMotorAxis::ScriptMotorAxis(ScriptMotorController *pC, int axisNo, const char* script_file, const char* params)
|
||||
: asynMotorAxis(pC, axisNo),
|
||||
pC_(pC)
|
||||
{
|
||||
this->initState(script_file);
|
||||
this->config(params);
|
||||
|
||||
int isnum;
|
||||
|
||||
lua_getglobal(this->state, "MovingPollPeriod");
|
||||
double MovingPollPeriod = lua_tonumberx(this->state, -1, &isnum);
|
||||
if (isnum) { pC->movingPollPeriod_ = MovingPollPeriod; }
|
||||
lua_remove(this->state, -1);
|
||||
|
||||
lua_getglobal(this->state, "IdlePollPeriod");
|
||||
double IdlePollPeriod = lua_tonumberx(this->state, -1, &isnum);
|
||||
if (isnum) { pC->idlePollPeriod_ = IdlePollPeriod; }
|
||||
lua_remove(this->state, -1);
|
||||
|
||||
lua_getglobal(this->state, "ForcedFastPolls");
|
||||
double ForcedFastPolls = lua_tonumberx(this->state, -1, &isnum);
|
||||
if (isnum) { pC->forcedFastPolls_ = ForcedFastPolls; }
|
||||
lua_remove(this->state, -1);
|
||||
|
||||
// Zero the encoder position (this only appears to be a problem on windows)
|
||||
setDoubleParam(pC_->motorEncoderPosition_, 0.0);
|
||||
|
||||
// Make the changed parameters take effect
|
||||
callParamCallbacks();
|
||||
}
|
||||
|
||||
void ScriptMotorAxis::initState(const char* script_file)
|
||||
{
|
||||
this->state = luaL_newstate();
|
||||
int status = luaLoadScript(this->state, script_file);
|
||||
|
||||
if (status) { printf("Error compiling script file: %s\n", script_file); }
|
||||
|
||||
lua_pushstring(this->state, (const char*) this->pC_->portName);
|
||||
lua_setglobal(this->state, "DRIVER");
|
||||
|
||||
lua_pushnumber(this->state, axisNo_);
|
||||
lua_setglobal(this->state, "AXIS");
|
||||
}
|
||||
|
||||
void ScriptMotorAxis::reload(const char* script_file, const char* controller_params)
|
||||
{
|
||||
this->initState(script_file);
|
||||
this->config(controller_params);
|
||||
this->config(this->params.c_str());
|
||||
}
|
||||
|
||||
epicsUInt32 ScriptMotorAxis::setStatusParam(int index, int value)
|
||||
{
|
||||
if (index >= pC_->motorStatusDirection_ && index <= pC_->motorStatusHomed_)
|
||||
{
|
||||
epicsUInt32 status = status_.status;
|
||||
int mask = 1 << (index - pC_->motorStatusDirection_);
|
||||
|
||||
if (value) { status |= mask; }
|
||||
else { status &= ~mask; }
|
||||
|
||||
if (status != status_.status)
|
||||
{
|
||||
status_.status = status;
|
||||
statusChanged_ = 1;
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ScriptMotorAxis::setPositionParam(int index, double value)
|
||||
{
|
||||
if (index == pC_->motorPosition_)
|
||||
{
|
||||
if (value != status_.position)
|
||||
{
|
||||
statusChanged_ = 1;
|
||||
status_.position = value;
|
||||
}
|
||||
}
|
||||
else if (index == pC_->motorEncoderPosition_)
|
||||
{
|
||||
if (value != status_.encoderPosition)
|
||||
{
|
||||
statusChanged_ = 1;
|
||||
status_.encoderPosition = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
extern "C" int ScriptAxisConfig(const char* ScriptMotorName, int axisNo, const char* params)
|
||||
{
|
||||
static const char *functionName = "VirtualMotorCreateAxis";
|
||||
|
||||
ScriptMotorController *pC = (ScriptMotorController*) findAsynPortDriver(ScriptMotorName);
|
||||
if (!pC)
|
||||
{
|
||||
printf("Error port %s not found\n", ScriptMotorName);
|
||||
return asynError;
|
||||
}
|
||||
|
||||
pC->lock();
|
||||
pC->configAxis(axisNo, params);
|
||||
pC->unlock();
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
/** 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 ScriptMotorAxis::report(FILE *fp, int level)
|
||||
{
|
||||
if (level > 0) {
|
||||
fprintf(fp, " Axis #%d\n", axisNo_);
|
||||
fprintf(fp, " axisIndex_=%d\n", axisIndex_);
|
||||
}
|
||||
|
||||
// Call the base class method
|
||||
asynMotorAxis::report(fp, level);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* move() is called by asynMotor device support when an absolute or a relative move is requested.
|
||||
* It can be called multiple times if BDST > 0 or RTRY > 0.
|
||||
*
|
||||
* Arguments in terms of motor record fields:
|
||||
* position (steps) = RVAL = DVAL / MRES
|
||||
* baseVelocity (steps/s) = VBAS / abs(MRES)
|
||||
* velocity (step/s) = VELO / abs(MRES)
|
||||
* acceleration (step/s/s) = (velocity - baseVelocity) / ACCL
|
||||
*/
|
||||
asynStatus ScriptMotorAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
||||
{
|
||||
asynStatus status;
|
||||
// static const char *functionName = "ScriptMotorAxis::move";
|
||||
|
||||
int result = lua_getglobal(this->state, "move");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, position);
|
||||
lua_pushboolean(this->state, relative);
|
||||
lua_pushnumber(this->state, minVelocity);
|
||||
lua_pushnumber(this->state, maxVelocity);
|
||||
lua_pushnumber(this->state, acceleration);
|
||||
|
||||
if (lua_pcall(this->state, 5, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* home() is called by asynMotor device support when a home is requested.
|
||||
* Note: forwards is set by device support, NOT by the motor record.
|
||||
*
|
||||
* Arguments in terms of motor record fields:
|
||||
* minVelocity (steps/s) = VBAS / abs(MRES)
|
||||
* maxVelocity (step/s) = HVEL / abs(MRES)
|
||||
* acceleration (step/s/s) = (maxVelocity - minVelocity) / ACCL
|
||||
* forwards = 1 if HOMF was pressed, 0 if HOMR was pressed
|
||||
*/
|
||||
|
||||
asynStatus ScriptMotorAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "home");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, minVelocity);
|
||||
lua_pushnumber(this->state, maxVelocity);
|
||||
lua_pushnumber(this->state, acceleration);
|
||||
lua_pushboolean(this->state, forwards);
|
||||
|
||||
if (lua_pcall(this->state, 4, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* moveVelocity() is called by asynMotor device support when a jog is requested.
|
||||
* If a controller doesn't have a jog command (or jog commands), this a jog can be simulated here.
|
||||
*
|
||||
* Arguments in terms of motor record fields:
|
||||
* minVelocity (steps/s) = VBAS / abs(MRES)
|
||||
* maxVelocity (step/s) = (jog_direction == forward) ? JVEL * DIR / MRES : -1 * JVEL * DIR / MRES
|
||||
* acceleration (step/s/s) = JAR / abs(EGU)
|
||||
*/
|
||||
asynStatus ScriptMotorAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "moveVelocity");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, minVelocity);
|
||||
lua_pushnumber(this->state, maxVelocity);
|
||||
lua_pushnumber(this->state, acceleration);
|
||||
|
||||
if (lua_pcall(this->state, 3, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* stop() is called by asynMotor device support whenever a user presses the stop button.
|
||||
* It is also called when the jog button is released.
|
||||
*
|
||||
* Arguments in terms of motor record fields:
|
||||
* acceleration = ???
|
||||
*/
|
||||
asynStatus ScriptMotorAxis::stop(double acceleration)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "stop");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, acceleration);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* setPosition() is called by asynMotor device support when a position is redefined.
|
||||
* It is also required for autosave to restore a position to the controller at iocInit.
|
||||
*
|
||||
* Arguments in terms of motor record fields:
|
||||
* position (steps) = DVAL / MRES = RVAL
|
||||
*/
|
||||
asynStatus ScriptMotorAxis::setPosition(double position)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setPosition");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, position);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorAxis::setEncoderPosition(double position)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setEncoderPosition");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, position);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorAxis::setHighLimit(double highLimit)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setHighLimit");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, highLimit);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorAxis::setLowLimit(double lowLimit)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setLowLimit");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, lowLimit);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
asynStatus ScriptMotorAxis::setPGain(double PGain)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setPGain");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, PGain);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorAxis::setIGain(double IGain)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setIGain");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, IGain);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorAxis::setDGain(double DGain)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setDGain");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, DGain);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
* setClosedLoop() is called by asynMotor device support when a user enables or disables torque,
|
||||
* usually from the motorx_all.adl, but only for drivers that set the following params to 1:
|
||||
* pC->motorStatusGainSupport_
|
||||
* pC->motorStatusHasEncoder_
|
||||
* What is actually implemented here varies greatly based on the specfics of the controller.
|
||||
*
|
||||
* Arguments in terms of motor record fields:
|
||||
* closedLoop = CNEN
|
||||
*/
|
||||
|
||||
asynStatus ScriptMotorAxis::setClosedLoop(bool closedLoop)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setClosedLoop");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushboolean(this->state, (int) closedLoop);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus ScriptMotorAxis::setEncoderRatio(double EncoderRatio)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "setEncoderRatio");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
lua_pushnumber(this->state, EncoderRatio);
|
||||
|
||||
if (lua_pcall(this->state, 1, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
// Do something with returned value
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/** 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 ScriptMotorAxis::poll(bool *moving)
|
||||
{
|
||||
int result = lua_getglobal(this->state, "poll");
|
||||
if (result != LUA_TFUNCTION)
|
||||
{
|
||||
// No function in script
|
||||
}
|
||||
else
|
||||
{
|
||||
if (lua_pcall(this->state, 0, 1, 0))
|
||||
{
|
||||
this->logError();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
int rettype = lua_type(this->state, -1);
|
||||
|
||||
if (rettype == LUA_TBOOLEAN)
|
||||
{
|
||||
if (lua_toboolean(this->state, -1)) { *moving = true; }
|
||||
else { *moving = false; }
|
||||
}
|
||||
|
||||
lua_pop(this->state, 1);
|
||||
}
|
||||
|
||||
this->callParamCallbacks();
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
void ScriptMotorAxis::config(const char* params)
|
||||
{
|
||||
luaLoadMacros(this->state, params);
|
||||
}
|
||||
|
||||
void ScriptMotorAxis::logError()
|
||||
{
|
||||
std::string err(lua_tostring(this->state, -1));
|
||||
lua_pop(this->state, 1);
|
||||
|
||||
printf("%s\n", err.c_str());
|
||||
}
|
||||
|
||||
void ScriptMotorReload(const char* port)
|
||||
{
|
||||
ScriptMotorController* controller = (ScriptMotorController*) findAsynPortDriver(port);
|
||||
|
||||
if (controller != NULL) { controller->reload(); }
|
||||
}
|
||||
|
||||
|
||||
/** Code for iocsh registration */
|
||||
static const iocshArg ScriptMotorReloadArg0 = {"Motor Port name", iocshArgString};
|
||||
|
||||
static const iocshArg* const ScriptMotorReloadArgs[] = {&ScriptMotorReloadArg0};
|
||||
|
||||
static const iocshFuncDef ScriptMotorReloadDef = {"ScriptMotorReload", 1, ScriptMotorReloadArgs};
|
||||
|
||||
static void ScriptMotorReloadCallFunc(const iocshArgBuf *args)
|
||||
{
|
||||
ScriptMotorReload(args[0].sval);
|
||||
}
|
||||
|
||||
|
||||
static const iocshArg ScriptMotorCreateControllerArg0 = {"Motor Port name", iocshArgString};
|
||||
static const iocshArg ScriptMotorCreateControllerArg1 = {"Number of axes", iocshArgInt};
|
||||
static const iocshArg ScriptMotorCreateControllerArg2 = {"Control Script", iocshArgString};
|
||||
static const iocshArg ScriptMotorCreateControllerArg3 = {"Parameters", iocshArgString};
|
||||
static const iocshArg * const ScriptMotorCreateControllerArgs[] = {&ScriptMotorCreateControllerArg0,
|
||||
&ScriptMotorCreateControllerArg1,
|
||||
&ScriptMotorCreateControllerArg2,
|
||||
&ScriptMotorCreateControllerArg3};
|
||||
static const iocshFuncDef ScriptMotorCreateControllerDef = {"ScriptControllerConfig", 4, ScriptMotorCreateControllerArgs};
|
||||
static void ScriptMotorCreateContollerCallFunc(const iocshArgBuf *args)
|
||||
{
|
||||
ScriptControllerConfig(args[0].sval, args[1].ival, args[2].sval, args[3].sval);
|
||||
}
|
||||
|
||||
|
||||
static const iocshArg ScriptMotorCreateAxisArg0 = {"Controller port name", iocshArgString};
|
||||
static const iocshArg ScriptMotorCreateAxisArg1 = {"Axis number", iocshArgInt};
|
||||
static const iocshArg ScriptMotorCreateAxisArg2 = {"Parameters", iocshArgString};
|
||||
|
||||
static const iocshArg * const ScriptMotorCreateAxisArgs[] = {&ScriptMotorCreateAxisArg0,
|
||||
&ScriptMotorCreateAxisArg1,
|
||||
&ScriptMotorCreateAxisArg2};
|
||||
|
||||
static const iocshFuncDef ScriptMotorCreateAxisDef = {"ScriptAxisConfig", 3, ScriptMotorCreateAxisArgs};
|
||||
static void ScriptMotorCreateAxisCallFunc(const iocshArgBuf *args)
|
||||
{
|
||||
ScriptAxisConfig(args[0].sval, args[1].ival, args[2].sval);
|
||||
}
|
||||
|
||||
|
||||
|
||||
static void ScriptMotorRegister(void)
|
||||
{
|
||||
iocshRegister(&ScriptMotorReloadDef, ScriptMotorReloadCallFunc);
|
||||
iocshRegister(&ScriptMotorCreateControllerDef, ScriptMotorCreateContollerCallFunc);
|
||||
iocshRegister(&ScriptMotorCreateAxisDef, ScriptMotorCreateAxisCallFunc);
|
||||
}
|
||||
|
||||
|
||||
extern "C" {
|
||||
epicsExportRegistrar(ScriptMotorRegister);
|
||||
}
|
||||
@@ -1 +0,0 @@
|
||||
registrar(ScriptMotorRegister)
|
||||
@@ -1,76 +0,0 @@
|
||||
#include "asynMotorController.h"
|
||||
#include "asynMotorAxis.h"
|
||||
|
||||
#include <string>
|
||||
#include "epicsScript.h"
|
||||
|
||||
class epicsShareClass ScriptMotorAxis : public asynMotorAxis
|
||||
{
|
||||
public:
|
||||
/* These are the methods we override from the base class */
|
||||
ScriptMotorAxis(class ScriptMotorController *pC, int axisNo, const char* script_file, const char* params);
|
||||
|
||||
void reload(const char* script, const char* params);
|
||||
|
||||
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 setEncoderPosition(double position);
|
||||
asynStatus setHighLimit(double highLimit);
|
||||
asynStatus setLowLimit(double lowLimit);
|
||||
asynStatus setPGain(double pGain);
|
||||
asynStatus setIGain(double iGain);
|
||||
asynStatus setDGain(double dGain);
|
||||
asynStatus setClosedLoop(bool closedLoop);
|
||||
asynStatus setEncoderRatio(double ratio);
|
||||
|
||||
virtual epicsUInt32 setStatusParam(int index, int value);
|
||||
virtual void setPositionParam(int index, double value);
|
||||
|
||||
private:
|
||||
ScriptMotorController *pC_; /**< Pointer to the asynMotorController to which this axis belongs.
|
||||
* Abbreviated because it is used very frequently */
|
||||
int axisIndex_;
|
||||
|
||||
std::string params;
|
||||
|
||||
lua_State* state;
|
||||
|
||||
void initState(const char* script_file);
|
||||
void config(const char* params);
|
||||
void logError();
|
||||
|
||||
friend class ScriptMotorController;
|
||||
};
|
||||
|
||||
class epicsShareClass ScriptMotorController : public asynMotorController {
|
||||
public:
|
||||
ScriptMotorController(const char *asyn_port, int max_axes, const char* script_file, const char* params);
|
||||
|
||||
virtual asynStatus setIntegerParam(int list, int function, int value);
|
||||
virtual asynStatus setDoubleParam(int list, int function, double value);
|
||||
|
||||
virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
|
||||
|
||||
void report(FILE *fp, int level);
|
||||
ScriptMotorAxis* getAxis(asynUser *pasynUser);
|
||||
ScriptMotorAxis* getAxis(int axisNo);
|
||||
|
||||
void configAxis(int axisNo, const char* params);
|
||||
|
||||
void reload();
|
||||
|
||||
protected:
|
||||
int ScriptMotorReload;
|
||||
|
||||
private:
|
||||
std::string script;
|
||||
std::string init_params;
|
||||
|
||||
friend class ScriptMotorAxis;
|
||||
};
|
||||
@@ -1,140 +0,0 @@
|
||||
|
||||
file {
|
||||
name="/home/oxygen40/KLANG/Documents/Projects/Repository/git/motor/motorApp/op/adl/ScriptMotorReload.adl"
|
||||
version=030107
|
||||
}
|
||||
display {
|
||||
object {
|
||||
x=663
|
||||
y=227
|
||||
width=220
|
||||
height=75
|
||||
}
|
||||
clr=14
|
||||
bclr=4
|
||||
cmap=""
|
||||
gridSpacing=5
|
||||
gridOn=0
|
||||
snapToGrid=0
|
||||
}
|
||||
"color map" {
|
||||
ncolors=65
|
||||
colors {
|
||||
ffffff,
|
||||
ececec,
|
||||
dadada,
|
||||
c8c8c8,
|
||||
bbbbbb,
|
||||
aeaeae,
|
||||
9e9e9e,
|
||||
919191,
|
||||
858585,
|
||||
787878,
|
||||
696969,
|
||||
5a5a5a,
|
||||
464646,
|
||||
2d2d2d,
|
||||
000000,
|
||||
00d800,
|
||||
1ebb00,
|
||||
339900,
|
||||
2d7f00,
|
||||
216c00,
|
||||
fd0000,
|
||||
de1309,
|
||||
be190b,
|
||||
a01207,
|
||||
820400,
|
||||
5893ff,
|
||||
597ee1,
|
||||
4b6ec7,
|
||||
3a5eab,
|
||||
27548d,
|
||||
fbf34a,
|
||||
f9da3c,
|
||||
eeb62b,
|
||||
e19015,
|
||||
cd6100,
|
||||
ffb0ff,
|
||||
d67fe2,
|
||||
ae4ebc,
|
||||
8b1a96,
|
||||
610a75,
|
||||
a4aaff,
|
||||
8793e2,
|
||||
6a73c1,
|
||||
4d52a4,
|
||||
343386,
|
||||
c7bb6d,
|
||||
b79d5c,
|
||||
a47e3c,
|
||||
7d5627,
|
||||
58340f,
|
||||
99ffff,
|
||||
73dfff,
|
||||
4ea5f9,
|
||||
2a63e4,
|
||||
0a00b8,
|
||||
ebf1b5,
|
||||
d4db9d,
|
||||
bbc187,
|
||||
a6a462,
|
||||
8b8239,
|
||||
73ff6b,
|
||||
52da3b,
|
||||
3cb420,
|
||||
289315,
|
||||
1a7309,
|
||||
}
|
||||
}
|
||||
rectangle {
|
||||
object {
|
||||
x=0
|
||||
y=0
|
||||
width=250
|
||||
height=25
|
||||
}
|
||||
"basic attribute" {
|
||||
clr=29
|
||||
}
|
||||
}
|
||||
"message button" {
|
||||
object {
|
||||
x=10
|
||||
y=35
|
||||
width=200
|
||||
height=30
|
||||
}
|
||||
control {
|
||||
chan="$(P)$(PORT):ScriptReload"
|
||||
clr=14
|
||||
bclr=62
|
||||
}
|
||||
label="Reload $(PORT)"
|
||||
press_msg="1"
|
||||
}
|
||||
rectangle {
|
||||
object {
|
||||
x=-7
|
||||
y=-7
|
||||
width=257
|
||||
height=32
|
||||
}
|
||||
"basic attribute" {
|
||||
clr=14
|
||||
fill="outline"
|
||||
width=3
|
||||
}
|
||||
}
|
||||
text {
|
||||
object {
|
||||
x=5
|
||||
y=3
|
||||
width=200
|
||||
height=20
|
||||
}
|
||||
"basic attribute" {
|
||||
clr=0
|
||||
}
|
||||
textix="Script Motor Reload"
|
||||
}
|
||||
@@ -39,9 +39,6 @@ ifdef BUSY
|
||||
endif
|
||||
COMMONDBDS += PI_GCS2Support.dbd
|
||||
COMMONDBDS += phytron.dbd
|
||||
ifdef SCRIPT
|
||||
COMMONDBDS += ScriptMotorDriver.dbd
|
||||
endif
|
||||
|
||||
DBD += WithAsyn.dbd
|
||||
WithAsyn_DBD += $(COMMONDBDS)
|
||||
@@ -86,10 +83,6 @@ COMMONLIBS += ACRMotor
|
||||
COMMONLIBS += PI_GCS2Support
|
||||
COMMONLIBS += phytronAxisMotor
|
||||
COMMONLIBS += motor
|
||||
ifdef SCRIPT
|
||||
COMMONLIBS += ScriptMotor
|
||||
COMMONLIBS += script
|
||||
endif
|
||||
|
||||
# Needed for Newport SNL programs
|
||||
WithAsyn_LIBS += $(COMMONLIBS)
|
||||
|
||||
Reference in New Issue
Block a user