diff --git a/motorApp/PhytronSrc/Makefile b/motorApp/PhytronSrc/Makefile new file mode 100644 index 00000000..02b2589e --- /dev/null +++ b/motorApp/PhytronSrc/Makefile @@ -0,0 +1,23 @@ +TOP=../.. + +include $(TOP)/configure/CONFIG +#---------------------------------------- +# Build the IOC application + +LIBRARY_IOC += phytronAxisMotor + +DBD += phytron.dbd + +# The following are compiled and added to the support library +phytronAxisMotor_SRCS += phytronAxisMotor.cpp + +INC += phytronAxisMotor.h + +phytronAxisMotor_LIBS += motor +phytronAxisMotor_LIBS += asyn +phytronAxisMotor_LIBS += $(EPICS_BASE_IOC_LIBS) + +#=========================== + +include $(TOP)/configure/RULES + diff --git a/motorApp/PhytronSrc/README.txt b/motorApp/PhytronSrc/README.txt new file mode 100644 index 00000000..9a06d717 --- /dev/null +++ b/motorApp/PhytronSrc/README.txt @@ -0,0 +1,447 @@ +******************************************************************************** +Phytron I1AM01 Stepper Motor Controller Asyn Driver Documentation + +Authors: Tom Slejko, Bor Marolt, Cosylab d.d. + tom.slejko@cosylab.com + bor.marolt@cosylab.com + +******************************************************************************** +Table of contents: +- Controller and axis configuration + - Example Application + - New Applicaion +- Database + - Supported I1AM01 Features + - Initialization records + - Status + - Homing + - Reset + - I1AM01 parameters + - Supported MCM01 Features + - Motor record + - List of remaining I1AM01 parameters handled by the motor record, + internally by controller, or not applicable +- GUI + + +Controller and axis configuration +================================= +Example application: +-------------------- +Motor record includes an example Phytron application. Before using it do: + +cd $(MOTOR_ROOT)/iocBoot/iocWithAsyn/ + +where $(MOTOR_ROOT) is the location of the motor record. Open st.cmd.phytron and +modify controller's IP address. If using serial port instead of ethernet, read +"New application" section, on how to configure the serial port. Remove or add +calls to phytronCreateAxis according to the number of axes that are to be used +(read "New application" section on how to use phytronCreateAxis). +Once st.cmd is configured, run: + +../../bin/linux-x86_64/WithAsyn st.cmd.phytron + +New application: +---------------- +This section describes how to properly configure epics application in order to +use phytronAxisMotor driver. + +The following dbd files and libraries must be added to the $(APP)/src/Makefile: +test_DBD += asyn.dbd +test_DBD += motorSupport.dbd +test_DBD += drvAsynIPPort.dbd +test_DBD += phytron.dbd + +test_LIBS += asyn +test_LIBS += motor +test_LIBS += phytronAxisMotor + +An example motor.substitutions.phytron and st.cmd.phytron files are located in +the $(MOTOR_RECORD)/iocBoot/iocWithAsyn/ directory. + +******************************************************************************** +WARNING: For the controller to work properly all three database files ( +Phytron_I1AM01.db, Phytron_MCM01.db and Phytron_motor.db) must be used. +******************************************************************************** + +Start up script must perform the following: +Before configuring the controller the user must create an asyn port by running +drvAsynIPPortConfigure or drvAsynSerialPortConfigure in order to create a +communication interface to Phytron's MCM Unit which controlls the I1AM01 +modules. + +If serial port is used for communication with the controller, baud rate, number +of data bits, parity, number of stop bits and End Of Line character must be set, +e.g.: + +drvAsynSerialPortConfigure ("testRemote","/dev/ttyUSB0") +asynOctetSetInputEos("testRemote",0,"\3") +asynSetOption ("testRemote", -1, "baud", "115200") +asynSetOption ("testRemote", -1, "bits", "8") +asynSetOption ("testRemote", -1, "parity", "none") +asynSetOption ("testRemote", -1, "stop", "1") + +If ethernet is used, the IP and port must be set, e.g.: +drvAsynIPPortConfigure("testRemote","10.5.1.181:22222",0,0,1) + +Phytron (MCM) controller port is configured (and connected to previously created +asyn port) by running the following iocsh function: + +phytronCreateController(const char *phytronPortName, const char *asynPortName, + int movingPollPeriod, int idlePollPeriod, double timeout) +- phytronPortName: Name of the particular MCM unit. +- asynPortName: Name of the previously configured asyn port - interface to MCM +- movingPollPeriod: The time between polls when any axis is moving in ms +- idlePolPeriod: The time between polls when no axis is moving in ms +- Timeout: Milliseconds before timeout for I/O requests + +where poll reads the basic axis status, e.g. position of the motor and of the +encoder, checks if axis is in movement, checks if motor is at the limit +switch, ... + +Once the phytron controller is configured, user can initialize axes by running + +phytronCreateAxis(const char* phytronPortName, int module, int axis) +- phytronPortName: Previously defined name of the MCM unit +- module: index of the I1AM01 module connected to the MCM +- axis: index of the axis on the I1AM01 module + +Module index and axis index compose the axis asyn ADDR (ADDR macro) used in the +motor.substitutions file. + +PhytronCreateAxis must be called for each axis intended to be used. + +******************************************************************************** +WARNING: For every axis, the user must specify it's address (ADDR macro) in the +motor.substitutions file for Phytron_motor.db and PhytronI1AM01.db files. +The address is composed of the I1AM01 module index and the axis index. If, for +example, the startup script configures the following axis: + +drvAsynIPPortConfigure("testRemote","10.5.1.181:22222",0,0,1) +phytronCreateController ("phyMotionPort", "testRemote", 100, 100, 1000) +phytronCreateAxis("phyMotionPort", 2, 1) + +It's asyn address that must be specified in the substitutions file is 21, + +PORT macro in the substitution file must match the phytronPortName, which is in +the example above "phyMotionPort" +******************************************************************************** + +Example st.cmd: +--------------- +#!../../bin/linux-x86_64/test + +## You may have to change test to something else +## everywhere it appears in this file + +< envPaths + +## Register all support components +dbLoadDatabase("../../dbd/test.dbd",0,0) +test_registerRecordDeviceDriver(pdbbase) + +drvAsynIPPortConfigure("testRemote","10.5.1.181:22222",0,0,1) + +phytronCreateController ("phyMotionPort", "testRemote", 100, 100, 1000) + +#phytronCreateAxis(phytronPort, module, axis) +phytronCreateAxis("phyMotionPort", 1, 1) +phytronCreateAxis("phyMotionPort", 2, 1) + + +cd $(MOTOR)/iocBoot/iocWithAsyn/ +dbLoadTemplate "motor.substitutions.phytron" + +iocInit() + +Database: +========= +All three database files (Phytron_I1AM01.db, Phytron_MCM01.db, Phytron_motor.db) +in $(MOTOR_ROOT)/motorApp/Db are expanded by motor.substitutions.phytron file in +$(MOTOR_ROOT)/iocBoot/iocWithAsyn + +******************************************************************************** +WARNING: For the controller to work properly all three database files ( +Phytron_I1AM01.db, Phytron_MCM01.db and Phytron_motor.db) must be used. +******************************************************************************** + +=============================================== +I1AM01 Controller Features - Phytron_I1AM01.db: +=============================================== +Initialization: +--------------- +Database contains 2 initialization records $(P)$(M)-INIT-AXIS-1/2 which are used +to initalize output (_SET) records. Initialization values are defined in the +motor.substitutions file. + +Status: +------- +Database contains several status records. Ai record $(P)$(M)-STATUS_ reads the +status value from the I1AM01 module, 2 mbbiDirect records ($(P)$(M)-STATUS-1/2) +are provided to set the status bits - 23 bi records are provided to parse these +status bits. An additional calc record is provided ($(P)$(M)-AXIS-ERR) which +sets it's value to 1 if any of the error bits are set (if only notification +bits are set, e.g. axis-initialized, the value of $(P)$(M)-AXIS-ERR is 0). + +Homing: +------- +RECORDS mbbo/mbbi: $(P)$(M)-HOMING_SET/_GET are used to set and readback the +type of homing to be used. Homing is executed with the use of motor record's +HOMF, HOMR fields. The following options are available for $(P)$(M)-HOMING_SET, +please keep in mind that offsets defined by I1AM01 parameters P11 and P12 (see +phylogic-en.pdf and see below for records corresponding to P11 and P12) affect +the final position after the homing procedure: +<- recordValue (Homing description); Phytron commands for HOMF and HOMR> + +- Limit (Home on limit switches); HOMF - m.aR+, HOMR - m.aR- + +- Center (Home on center switch); HOMF - m.aR+C, HOMR - m.aR-C +COMMENT: If limit switch is reached, the motor will stop and start moving in the +opposite direction until center switch is reached + +- Encoder (Home on encoder zero pulse); HOMF - m.aR+I, HOMR - m.aR-I + +- Limit-Encoder (Got to limit switch, than encoder zero pulse); + HOMF - m.aR+^I, HOMR - m.aR-^I + +- Center-Encoder (Go to the center switch than to encoder zero pulse) + HOMF - m.a.R+C^I, HOMR - m.aR-C^I +COMMENT: If limit switch is reached before center switch, the motor will stop +and start moving in the opposite direction until center switch is reached + +- Reference-Center (Driving on a reference signal to center) + HOMF - m.aRC+, HOMR - m.aRC- +COMMEN: If limit switch is reached, controller goes to axis error state + +- Ref-Center-Encoder (Driving on a reference signal to center and then to +encoder zero pulse) + HOMF - m.aRC+^I, HOMR - m.aRC-^I + +Reset: +------ +Two records are provided. Record $(P)$(M)-RESET executs Phytron command: m.aC, +where m is controller index and a is the axis index. + +$(P)$(M)-RESET-STATUS resets the axis status by executing the Phytron command: +SECm.a, where m is controller index and a is the axis index. + +If user resets the MCM unit, $(P)$(M)-REINIT_ record is triggered. This record +process the initialization records $(P)$(M)-INIT-AXIS-1/2 after a DELAY time +defined by the macro DLY in the substitutions file. + + +The following I1AM01 parameters are exposed as additional EPICS records: +------------------------------------------------------------------------ +Param. index: feature; +Record name(s): +Comment: +------------------------------------- +P01: Type of movement; +RECORDS mbbo/mbbi: $(P)$(M)-MOVE-TYP_SET/_GET; +COMMENT: Software limits are always monitored by the motor record, so only 2 +types of movements are possible: + - Rotational: HW limit/center switches are ignored + - HW switches are monitored +------------------------------------- +P11: M0P offset for limit switch in direction towards LIMIT-; +RECORDS ao/ai: $(P)$(M)-POS-OFFSET_SET/_GET +------------------------------------- +P12: M0P offset for limit switch in direction towards LIMIT+; +RECORDS ao/ai: $(P)$(M)-NEG-OFFSET_SET/_GET +------------------------------------- +P13: Time lapse during initialization +RECORDS ao/ai: $(P)$(M)-INIT-TIMEOUT_SET/_GET +------------------------------------- +P16: Time lapse after positioning +RECORDS ao/ai: $(P)$(M)-POS-TIMEOUT_SET/_GET +------------------------------------- +P17: Defines when to use boost current +RECORDS mbbo/mbbi: $(P)$(M)-BOOST_SET/_GET +------------------------------------- +P26: Encoder data transfer rate (ONLY FOR SSI) +RECORDS mbbo/mbbi: $(P)$(M)-ENC-RATE_SET/_GET +------------------------------------- +P27: Limit switch type +RECORDS mbbo/mbbi: $(P)$(M)-SWITCH-TYP_SET/_GET +------------------------------------- +P28: Power stage off/on +RECORDS bo/bi: $(P)$(M)-PWR-STAGE-MODE_SET/_GET +------------------------------------- +P34: Encoder type +RECORDS mbbo/mbbi: $(P)$(M)-ENC-TYP_SET/_GET +------------------------------------- +P35: Encoder resolution - for SSI and EnDat encoders +RECORDS ao/ai: $(P)$(M)-ENC-RES_SET/_GET +------------------------------------- +P36: Encoder function: If 1, Encoder position is continuously compared to the +motor position +RECORDS: bo/bi: $(P)$(M)-ENC-FUNC_SET/_GET +COMMENT: _SET is processed when $(P)$(M)-ENC-SFI_SET corresponding to parameter +P37 is processed. If P37 > 0, then P36 is set to 1, else it is 0. +------------------------------------- +P37: If P36 is set to 1, controller will stop the motion when the difference +between motor and encoder position is > P37. +RECORDS: bo/bi: $(P)$(M)-ENC-SFI_SET/_GET +COMMENT: If value is set to > 0, then P36 is set to 1. If P37=0, then P36=0 +------------------------------------- +P38: Encoder direction of rotation: Positive/Negative +RECORDS: bo/bi: $(P)$(M)-ENC-DIR_SET/_GET +------------------------------------- +P39: Encoder direction of rotation +RECORDS: bo/bi: $(P)$(M)-ENC-DIR_SET/_GET +------------------------------------- +P40: Stop current in mA. Can be set in 10 mA increments +RECORDS: ao/ai: $(P)$(M)-STOP-CURRENT_SET/_GET +------------------------------------- +P41: Run current in mA. Can be set in 10 mA increments +RECORDS: ao/ai: $(P)$(M)-RUN-CURRENT_SET/_GET +------------------------------------- +P42: Boost current in mA. Can be set in 10 mA increments +RECORDS: ao/ai: $(P)$(M)-BOOST-CURRENT_SET/_GET +------------------------------------- +P43: Current hold time in ms. +RECORDS: ao/ai: $(P)$(M)-CURRENT-DELAY_SET/_GET +------------------------------------- +P49: Power stage temperature +RECORDS: ai: $(P)$(M)-PS-TEMPERATURE +------------------------------------- +P53: Power stage monitoring +RECORDS: bo/bi: $(P)$(M)-PS-MONITOR/_GET +------------------------------------- +P54: Motor temperature +RECORDS: ai: $(P)$(M)-MOTOR-TEMP + + + +============================================ +MC01 Controller Features - Phytron_MCM01.db: +============================================ +This database file contains records for reading MCM01 status and to reset the +MCM01 module. + +Ai record $(P)-STATUS_ reads the status value from the MCM01 module, a +mbbiDirect record is provided to set the status bits - 16 bi records are +provided to parse these bits. An additional calc record is provided +($(P)-CON-ERR) which sets it's value to 1 if any of the error bits are set (if +only noticiation bits are set, e.g. terminal-activated, the value of +$(P)-CON-ERR is 0) + +Bo record $(P)-RESET resets the MCM01 controller, every time it is processed. +$(P)-RESET_ alternates between 0 and 1, so the monitor is posted on every reset +and the axis REINIT_ record can trigger the initialization procedures. + +================================ +Motor Record - Phytron_motor.db: +================================ +This database file is similar to basic_asyn_motor.db, the only difference is, +that 2 additional fields are defined: + - field(ERES,"$(ERES)") - encoder resolution + - field(VMAX,"$(VMAX)") - maximum velocity + + +=============================================================================== +List of remaining I1AM01 parameters handled by the motor record, internally by +controller, or not applicable +=============================================================================== +P02 (Units of movemnt) - Always set to step - unit conversion is done within the +motor record. + +P03 (Conversion factor for the thread) - Always set to 1 - unit conversion is +done within the motor record. + +P04 (Start/stop frequency) - Set by phytronAxis::move, before move is executed. + +P07 (Emergency stop ramp) - Set by phytronAxis::stop, before stop is executed. + +P08 (Initialization run frequency) - Set by phytronAxis::home, before homing is +executed. + +P09 (Ramp M0P) - Set by phytronAxis::home, before homing is executed. + +P10 (Run frequency for leaving the limit switch) - Set by phytronAxis::home, +before homing is executed + +P14 (Run frequency during program operation) - Set by phytronAxis::move, before +move is executed + +P15 (Ramp for run frequency) - Set by phytronAxis::move, before move is executed + +P18 (Used internally by controller) + +P19 (Encoder deviation M0P counter) + +P20 (Mechanical 0 counter) - read by phytronAxis::poll to determine motor +position + +P21 (Absolute counter) + +P22 (Encoder counter) - read by phytronAxis::poll to determine encoder position + +P23 and P24 - Software limit switches - these parameters are ignored, because +motor record handles software limits + +P25 (Compensation for play) - Ignored, because motor records handles backlash +corrections + +P29 (Not used (by controller)) + +P30 and P31 (For I4XM01 only) + +P32 and P33 (Not used (by controller)) + +P44 (For I4XM01 only) + +P46, P47, P48 (Not used (by controller)) + +P50 and P51 (For I4XM01 only) + +P52 (Internally used for trigger position) + + +GUI: +==== +The follwoing CSS BOY GUI screens are located in motorApp/op/opi: + +PhytronMain.opi +--------------- +Motor record name is usually defined with 2 macros: +record(motor, "$(P)$(M)") + +The value of the macro $(IOC) of the PhytronMain.opi screen must match the value +of the macro $(P) which is defined in motor.substitutions. + +PhytronMain.opi has 2 action buttons to open screen PhytronI1AM01.opi for motor +$(IOC):$(M), where the default values for $(M) are "m1" for the left button +and "m2" for the right button. User must modify the values of macros $(IOC) and +$(M) according to the macro values in motor.subsititutions. + +PhytronMain.opi also contains a CONTROLLER STATUS line. The LED displays the +value of record $(P)-CON-ERR (see section MC01 Controller Features). Action +button "MORE" opens the screen PhytronMCM01Status.opi, action button "RESET" +resets the MCM01 controller and action button "RESET CONTROLLER" resets +controller status. + +PhytronI1AM01.opi +----------------- +Status line contains a LED which displays the value of the record +$(P)$(M)-AXIS-ERR, and an action button which displays the PhytronI1AM01.opi +screen. Action buttons "RESET AXIS" and "RESET STATUS" are provided. + +The remaining widgets are used to set and read back the values of supported +I1AM01 parameters (see section I1AM01 Controller Features). + +PhytronI1AM01Status.opi +----------------------- +Contains an LED for each of the 23 bi records parsing the $(P)$(M)-STATUS-1/2 +mbbiDirect records. + +PhytronMCM01Status.opi +---------------------- +Contains an LED for each of the 16 bi records parsing the $(P)-STATUS-BITS_ +mbbiDirect record + + + + + + diff --git a/motorApp/PhytronSrc/phytron.dbd b/motorApp/PhytronSrc/phytron.dbd new file mode 100644 index 00000000..6cd997ea --- /dev/null +++ b/motorApp/PhytronSrc/phytron.dbd @@ -0,0 +1 @@ +registrar(phytronRegister) diff --git a/motorApp/PhytronSrc/phytronAxisMotor.cpp b/motorApp/PhytronSrc/phytronAxisMotor.cpp new file mode 100644 index 00000000..77149537 --- /dev/null +++ b/motorApp/PhytronSrc/phytronAxisMotor.cpp @@ -0,0 +1,1036 @@ +/* +FILENAME... phytronAxisMotor.cpp +USAGE... Motor driver support for Phytron Axis controller. + +Tom Slejko & Bor Marolt +Cosylab d.d. 2014 + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "phytronAxisMotor.h" +#include + +using namespace std; + +#ifndef ASYN_TRACE_WARNING +#define ASYN_TRACE_WARNING ASYN_TRACE_ERROR +#endif + +//Used for casting position doubles to integers +#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5) + +/* + * Contains phytronController instances, phytronCreateAxis uses it to find and + * bind axis object to the correct controller object. + */ +static vector controllers; + +/** Creates a new phytronController object. + * \param[in] portName The name of the asyn port that will be created for this driver + * \param[in] phytronPortName The name of the drvAsynIPPort that was created previously to connect to the phytron controller + * \param[in] movingPollPeriod The time between polls when any axis is moving + * \param[in] idlePollPeriod The time between polls when no axis is moving + */ +phytronController::phytronController(const char *phytronPortName, const char *asynPortName, + double movingPollPeriod, double idlePollPeriod, double timeout) + : asynMotorController(phytronPortName, + 0xFF, + NUM_PHYTRON_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 +{ + asynStatus status; + size_t response_len; + phytronStatus phyStatus; + static const char *functionName = "phytronController::phytronController"; + + //Timeout is defined in milliseconds, but sendPhytronCommand expects seconds + timeout_ = timeout/1000; + + //pyhtronCreateAxis uses portName to identify the controller + this->controllerName_ = (char *) mallocMustSucceed(sizeof(char)*(strlen(portName)+1), + "phytronController::phytronController: Controller name memory allocation failed.\n"); + + strcpy(this->controllerName_, portName); + + //Create Controller parameters + createParam(controllerStatusString, asynParamInt32, &this->controllerStatus_); + createParam(controllerStatusResetString,asynParamInt32, &this->controllerStatusReset_); + createParam(resetControllerString, asynParamInt32, &this->resetController_); + + //Create Axis parameters + createParam(axisStatusResetString, asynParamInt32, &this->axisStatusReset_); + createParam(axisResetString, asynParamInt32, &this->axisReset_); + createParam(axisStatusString, asynParamInt32, &this->axisStatus_); + createParam(homingProcedureString, asynParamInt32, &this->homingProcedure_); + createParam(axisModeString, asynParamInt32, &this->axisMode_); + createParam(mopOffsetPosString, asynParamInt32, &this->mopOffsetPos_); + createParam(mopOffsetNegString, asynParamInt32, &this->mopOffsetNeg_); + createParam(stepResolutionString, asynParamInt32, &this->stepResolution_); + createParam(stopCurrentString, asynParamInt32, &this->stopCurrent_); + createParam(runCurrentString, asynParamInt32, &this->runCurrent_); + createParam(boostCurrentString, asynParamInt32, &this->boostCurrent_); + createParam(encoderTypeString, asynParamInt32, &this->encoderType_); + createParam(initRecoveryTimeString, asynParamInt32, &this->initRecoveryTime_); + createParam(positionRecoveryTimeString, asynParamInt32, &this->positionRecoveryTime_); + createParam(boostConditionString, asynParamInt32, &this->boost_); + createParam(encoderRateString, asynParamInt32, &this->encoderRate_); + createParam(switchTypString, asynParamInt32, &this->switchTyp_); + createParam(pwrStageModeString, asynParamInt32, &this->pwrStageMode_); + createParam(encoderResolutionString, asynParamInt32, &this->encoderRes_); + createParam(encoderFunctionString, asynParamInt32, &this->encoderFunc_); + createParam(encoderSFIWidthString, asynParamInt32, &this->encoderSFIWidth_); + createParam(encoderDirectionString, asynParamInt32, &this->encoderDirection_); + createParam(powerStagetMonitorString, asynParamInt32, &this->powerStageMonitor_); + createParam(currentDelayTimeString, asynParamInt32, &this->currentDelayTime_); + createParam(powerStageTempString, asynParamFloat64, &this->powerStageTemp_); + createParam(motorTempString, asynParamFloat64, &this->motorTemp_); + + + /* Connect to phytron controller */ + status = pasynOctetSyncIO->connect(asynPortName, 0, &pasynUserController_, NULL); + if (status) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: cannot connect to phytron controller\n", + functionName); + } else { + //phytronCreateAxis will search for the controller for axis registration + controllers.push_back(this); + + //RESET THE CONTROLLER + sprintf(this->outString_, "CR"); + phyStatus = sendPhytronCommand(this->outString_, this->inString_, MAX_CONTROLLER_STRING_SIZE, &response_len); + if(phyStatus){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_WARNING, + "phytronController::phytronController: Could not reset controller %s\n", this->controllerName_); + } + + //Wait for reset to finish + epicsThreadSleep(10.0); + + startPoller(movingPollPeriod, idlePollPeriod, 5); + } + +} + +/** Creates a new phytronController 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] phytronPortName The name of the drvAsynIPPPort that was created previously to connect to the phytron controller + * \param[in] numController number of axes that this controller supports is numController*AXES_PER_CONTROLLER + * \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 phytronCreateController(const char *phytronPortName, const char *asynPortName, + int movingPollPeriod, int idlePollPeriod, double timeout) +{ + phytronController *pphytronController = new phytronController(phytronPortName, asynPortName, movingPollPeriod/1000., idlePollPeriod/1000., timeout); + pphytronController = NULL; + return asynSuccess; +} + +/** asynUsers use this to read integer parameters + * \param[in] pasynUser asynUser structure containing the reason + * \param[out] value Parameter value + */ +asynStatus phytronController::readInt32(asynUser *pasynUser, epicsInt32 *value) +{ + phytronAxis *pAxis; + phytronStatus phyStatus; + + //Call base implementation first + asynPortDriver::readInt32(pasynUser, value); + + //Check if this is a call to read a controller parameter + if(pasynUser->reason == resetController_ || pasynUser->reason == controllerStatusReset_){ + //Called only on initialization of bo records RESET and RESET-STATUS + return asynSuccess; + } else if (pasynUser->reason == controllerStatus_){ + size_t response_len; + sprintf(this->outString_, "ST"); + phyStatus = sendPhytronCommand(this->outString_, this->inString_, MAX_CONTROLLER_STRING_SIZE, &response_len); + if(phyStatus){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::readInt32: Reading controller %s status failed with error " + "code: %d\n", this->controllerName_, phyStatus); + return phyToAsyn(phyStatus); + } + + *value = atoi(this->inString_); + return asynSuccess; + } + + //This is an axis request, find the axis + pAxis = getAxis(pasynUser); + if(!pAxis){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::readInt32: Axis not found on the controller %s\n", this->controllerName_); + return asynError; + } + + if(pasynUser->reason == homingProcedure_){ + getIntegerParam(pAxis->axisNo_, homingProcedure_, value); + return asynSuccess; + } else if (pasynUser->reason == axisReset_ || pasynUser->reason == axisStatusReset_){ + //Called only on initialization of AXIS-RESET and AXIS-STATUS-RESET bo records + return asynSuccess; + } else if (pasynUser->reason == axisMode_){ + sprintf(this->outString_, "M%.1fP01R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == mopOffsetPos_){ + sprintf(this->outString_, "M%.1fP11R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == mopOffsetNeg_){ + sprintf(this->outString_, "M%.1fP12R", pAxis->axisModuleNo_); + } else if (pasynUser->reason == stepResolution_){ + sprintf(this->outString_, "M%.1fP45R", pAxis->axisModuleNo_); + } else if (pasynUser->reason == stopCurrent_){ + sprintf(this->outString_, "M%.1fP40R", pAxis->axisModuleNo_); + } else if (pasynUser->reason == runCurrent_){ + sprintf(this->outString_, "M%.1fP41R", pAxis->axisModuleNo_); + } else if (pasynUser->reason == boostCurrent_){ + sprintf(this->outString_, "M%.1fP42R", pAxis->axisModuleNo_); + } else if (pasynUser->reason == encoderType_){ + sprintf(this->outString_, "M%.1fP34R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == initRecoveryTime_){ + sprintf(this->outString_, "M%.1fP13R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == positionRecoveryTime_){ + sprintf(this->outString_, "M%.1fP16R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == boost_){ + sprintf(this->outString_, "M%.1fP17R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == encoderRate_){ + sprintf(this->outString_, "M%.1fP26R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == switchTyp_){ + sprintf(this->outString_, "M%.1fP27R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == pwrStageMode_){ + sprintf(this->outString_, "M%.1fP28R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == encoderRes_){ + sprintf(this->outString_, "M%.1fP35R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == encoderFunc_){ + sprintf(this->outString_, "M%.1fP36R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == encoderSFIWidth_){ + sprintf(this->outString_, "M%.1fP37R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == encoderDirection_){ + sprintf(this->outString_, "M%.1fP38R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == currentDelayTime_){ + sprintf(this->outString_, "M%.1fP43R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == powerStageMonitor_){ + sprintf(this->outString_, "M%.1fP53R", pAxis->axisModuleNo_); + } + + + phyStatus = sendPhytronCommand(this->outString_, this->inString_, MAX_CONTROLLER_STRING_SIZE, &pAxis->response_len); + if(phyStatus){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::readInt32: Failed with status %d for reason %d\n", phyStatus, pasynUser->reason); + return phyToAsyn(phyStatus); + } + + *value = atoi(this->inString_); + + //{STOP,RUN,BOOST} current records have EGU set to mA, but device returns 10mA + if(pasynUser->reason == stopCurrent_ || pasynUser->reason == runCurrent_ || + pasynUser->reason == boostCurrent_) + { + *value *= 10; + } // else if + + + return asynSuccess; +} + +/** asynUsers use this to write integer parameters + * \param[in] pasynUser asynUser structure containing the reason + * \param[in] value Parameter value to be written + */ +asynStatus phytronController::writeInt32(asynUser *pasynUser, epicsInt32 value) +{ + phytronAxis *pAxis; + phytronStatus phyStatus; + + //Call base implementation first + asynMotorController::writeInt32(pasynUser, value); + + /* + * Check if this is a call to reset the controller, else it is an axis request + */ + if(pasynUser->reason == resetController_){ + size_t response_len; + sprintf(this->outString_, "CR"); + phyStatus = sendPhytronCommand(this->outString_, this->inString_, MAX_CONTROLLER_STRING_SIZE, &response_len); + if(phyStatus){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::writeInt32: Reseting controller %s failed with error code: %d\n", this->controllerName_, phyStatus); + } + resetAxisEncoderRatio(); + return phyToAsyn(phyStatus); + } else if(pasynUser->reason == controllerStatusReset_){ + size_t response_len; + sprintf(this->outString_, "STC"); + phyStatus = sendPhytronCommand(this->outString_, this->inString_, MAX_CONTROLLER_STRING_SIZE, &response_len); + if(phyStatus){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::writeInt32: Reseting controller %s failed with error code: %d\n", this->controllerName_, phyStatus); + } + return phyToAsyn(phyStatus); + } + /* + * This is an axis request, find the axis + */ + pAxis = getAxis(pasynUser); + if(!pAxis){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::writeInt32: Axis not found on the controller %s\n", this->controllerName_); + return asynError; + } + + if(pasynUser->reason == homingProcedure_){ + setIntegerParam(pAxis->axisNo_, pasynUser->reason, value); + callParamCallbacks(); + return asynSuccess; + } else if(pasynUser->reason == axisReset_){ + sprintf(this->outString_, "M%.1fC", pAxis->axisModuleNo_); + } else if(pasynUser->reason == axisStatusReset_){ + sprintf(this->outString_, "SEC%.1f", pAxis->axisModuleNo_); + } else if(pasynUser->reason == axisMode_){ + sprintf(this->outString_, "M%.1fP01=%d", pAxis->axisModuleNo_,value); + } else if(pasynUser->reason == mopOffsetPos_){ + sprintf(this->outString_, "M%.1fP11=%d", pAxis->axisModuleNo_,value); + } else if(pasynUser->reason == mopOffsetNeg_){ + sprintf(this->outString_, "M%.1fP12=%d", pAxis->axisModuleNo_,value); + } else if (pasynUser->reason == stepResolution_){ + sprintf(this->outString_, "M%.1fP45=%d", pAxis->axisModuleNo_,value); + } else if (pasynUser->reason == stopCurrent_){ + value /= 10; //STOP_CURRENT record has EGU mA, device expects 10mA + sprintf(this->outString_, "M%.1fP40=%d", pAxis->axisModuleNo_,value); + } else if (pasynUser->reason == runCurrent_){ + value /= 10; //RUN_CURRENT record has EGU mA, device expects 10mA + sprintf(this->outString_, "M%.1fP41=%d", pAxis->axisModuleNo_,value); + } else if (pasynUser->reason == boostCurrent_){ + value /= 10; //BOOST_CURRENT record has EGU mA, device expects 10mA + sprintf(this->outString_, "M%.1fP42=%d", pAxis->axisModuleNo_,value); + } else if (pasynUser->reason == encoderType_){ + sprintf(this->outString_, "M%.1fP34=%d", pAxis->axisModuleNo_, value); + } else if (pasynUser->reason == initRecoveryTime_){ + sprintf(this->outString_, "M%.1fP13=%d", pAxis->axisModuleNo_, value); + } else if (pasynUser->reason == positionRecoveryTime_){ + sprintf(this->outString_, "M%.1fP16=%d", pAxis->axisModuleNo_, value); + } else if (pasynUser->reason == boost_){ + sprintf(this->outString_, "M%.1fP17=%d", pAxis->axisModuleNo_, value); + } else if (pasynUser->reason == encoderRate_){ + sprintf(this->outString_, "M%.1fP26=%d", pAxis->axisModuleNo_, value); + } else if (pasynUser->reason == switchTyp_){ + sprintf(this->outString_, "M%.1fP27=%d", pAxis->axisModuleNo_, value); + } else if (pasynUser->reason == pwrStageMode_){ + sprintf(this->outString_, "M%.1fP28=%d", pAxis->axisModuleNo_, value); + } else if (pasynUser->reason == encoderRes_){ + sprintf(this->outString_, "M%.1fP35=%d", pAxis->axisModuleNo_, value); + } else if (pasynUser->reason == encoderFunc_){ + //Value is VAL field of parameter P37 record. If P37 is positive P36 is set to 1, else 0 + sprintf(this->outString_, "M%.1fP36=%d", pAxis->axisModuleNo_, value > 0 ? 1 : 0); + } else if(pasynUser->reason == encoderSFIWidth_){ + sprintf(this->outString_, "M%.1fP37=%d", pAxis->axisModuleNo_, value); + } else if(pasynUser->reason == encoderSFIWidth_){ + sprintf(this->outString_, "M%.1fP38=%d", pAxis->axisModuleNo_, value); + } else if(pasynUser->reason == powerStageMonitor_){ + sprintf(this->outString_, "M%.1fP53=%d", pAxis->axisModuleNo_, value); + } else if(pasynUser->reason == currentDelayTime_){ + sprintf(this->outString_, "M%.1fP43=%d", pAxis->axisModuleNo_, value); + } else if(pasynUser->reason == encoderDirection_){ + sprintf(this->outString_, "M%.1fP38=%d", pAxis->axisModuleNo_, value); + } + + phyStatus = sendPhytronCommand(this->outString_, this->inString_, MAX_CONTROLLER_STRING_SIZE, &pAxis->response_len); + if(phyStatus){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::writeInt32: Failed with status %d for reason %d\n", phyStatus, pasynUser->reason); + return phyToAsyn(phyStatus); + } + + return asynSuccess; +} + +/** asynUsers use this to read float parameters + * \param[in] pasynUser asynUser structure containing the reason + * \param[out] value Parameter value + */ +asynStatus phytronController::readFloat64(asynUser *pasynUser, epicsFloat64 *value){ + phytronAxis *pAxis; + phytronStatus phyStatus; + + pAxis = getAxis(pasynUser); + if(!pAxis){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::readFloat64: Axis not found on the controller %s\n", this->controllerName_); + return asynError; + } + + //Call base implementation first + asynPortDriver::readFloat64(pasynUser, value); + + if(pasynUser->reason == powerStageTemp_){ + sprintf(this->outString_, "M%.1fP49R", pAxis->axisModuleNo_); + } else if(pasynUser->reason == motorTemp_){ + sprintf(this->outString_, "M%.1fP54R", pAxis->axisModuleNo_); + } + + phyStatus = sendPhytronCommand(this->outString_, this->inString_, MAX_CONTROLLER_STRING_SIZE, &pAxis->response_len); + if(phyStatus){ + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::readFloat64: Failed with status %d for reason %d\n", phyStatus, pasynUser->reason); + return phyToAsyn(phyStatus); + } + + *value = atof(this->inString_); + + //Power stage and motor temperature records have EGU °C, but device returns 0.1 °C + *value /= 10; + + return phyToAsyn(phyStatus); + +} +/* + * Reset the motorEncoderRatio to 1 after the reset of MCM unit + */ +void phytronController::resetAxisEncoderRatio(){ + + for(uint32_t i = 0; i < axes.size(); i++){ + setDoubleParam(axes[i]->axisNo_, motorEncoderRatio_, 1); + } +} + + +/** 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 phytronController::report(FILE *fp, int level) +{ + fprintf(fp, "MCB-4B motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", + this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_); + + // Call the base class method + asynMotorController::report(fp, level); +} + +/** Returns a pointer to an phytronAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. + */ +phytronAxis* phytronController::getAxis(asynUser *pasynUser) +{ + return static_cast(asynMotorController::getAxis(pasynUser)); +} + +/** Returns a pointer to an phytronAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. + */ +phytronAxis* phytronController::getAxis(int axisNo) +{ + return static_cast(asynMotorController::getAxis(axisNo)); +} + +/** + * @brief implements phytron specific data fromat + * @param output + * @param input + * @param maxChars + * @param nread + * @param timeout + * @return + */ +phytronStatus phytronController::sendPhytronCommand(const char *command, char *response_buffer, size_t response_max_len, size_t *nread) +{ + char buffer[255]; + char* buffer_end=buffer; + static const char *functionName = "phytronController::sendPhytronCommand"; + + *(buffer_end++)=0x02; //STX + *(buffer_end++)='0'; //Module address TODO: add class member + buffer_end += sprintf(buffer_end,"%s",command); //Append command + *(buffer_end++)=0x3a; //Append separator + + buffer_end += sprintf(buffer_end,"%c%c",'X','X'); //XX disables checksum + *(buffer_end++)=0x03; //Append ETX + *(buffer_end)=0x0; //Null terminate message for saftey + + phytronStatus status = (phytronStatus) writeReadController(buffer,buffer,255,nread, timeout_); + if(status){ + return status; + } + + char* nack_ack = strchr(buffer,0x02); //Find STX + if(!nack_ack){ + nread=0; + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Communication failed\n", + functionName); + return phytronInvalidReturn; + } + nack_ack++; //NACK/ACK is one + //ACK, extract response + if(*nack_ack==0x06){ + char* separator = strchr(nack_ack,0x3a); //find separator + + /* Copy data from nack_ack to + * separator into buffer */ + uint32_t len = separator-nack_ack-1; //calculate length of message + if(len > response_max_len) len=response_max_len; + + memcpy(response_buffer,nack_ack+1,len); //copy payload to destination + response_buffer[separator-nack_ack-1]=0; //Add NULL terminator + + *nread=strlen(response_buffer); + } + //NAK return error + else if(*nack_ack==0x15){ + nread=0; + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: Nack sent by the controller\n", + functionName); + return phytronInvalidCommand; + } + + return status; + +} + +/** Castst phytronStatus to asynStatus enumeration + * \param[in] phyStatus + */ +asynStatus phytronController::phyToAsyn(phytronStatus phyStatus){ + if(phyStatus == phytronInvalidReturn || phyStatus == phytronInvalidCommand) return asynError; + return (asynStatus) phyStatus; +} + + +//****************************************************************************** +// PHYTRON AXIS IMPLEMENTATION +//****************************************************************************** + +/** Creates a new phytronAxis object. + * Configuration command, called directly or from iocsh + * \param[in] controllerName Name of the asyn port created by calling phytronCreateController from st.cmd + * \param[in] module Index of the I1AM01 module controlling this axis + * \param[in] axis Axis index + */ +extern "C" int phytronCreateAxis(const char* controllerName, int module, int axis){ + + phytronAxis *pAxis; + + //Find the controller + uint32_t i; + for(i = 0; i < controllers.size(); i++){ + if(!strcmp(controllers[i]->controllerName_, controllerName)) { + pAxis = new phytronAxis(controllers[i], module*10 + axis); + controllers[i]->axes.push_back(pAxis); + break; + } + } + + //If controller is not found, report error + if(i == controllers.size()){ + printf("ERROR: phytronCreateAxis: Controller %s is not registered\n", controllerName); + return asynError; + } + + return asynSuccess; +} + +/** Creates a new phytronAxis object. + * \param[in] pC Pointer to the phytronController to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ +phytronAxis::phytronAxis(phytronController *pC, int axisNo) + : asynMotorAxis(pC, axisNo), + axisModuleNo_((float)axisNo/10), + pC_(pC), + response_len(0) +{ + + //Controller always supports encoder. Encoder enable/disable is set through UEIP + setIntegerParam(pC_->motorStatusHasEncoder_, 1); + + setDoubleParam(pC_->motorEncoderRatio_, 1); + +} + + +/** 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 phytronAxis::report(FILE *fp, int level) +{ + if (level > 0) { + fprintf(fp, " axis %d\n", + axisNo_); + } + + // Call the base class method + asynMotorAxis::report(fp, level); +} + +/** Sets velocity parameters before the move is executed. Controller produces a + * trapezoidal speed profile defined by these parmeters. + * \param[in] minVelocity Start velocity + * \param[in] maxVelocity Maximum velocity + * \param[in] moveType Type of movement determines which controller speed parameters are set + */ +phytronStatus phytronAxis::setVelocity(double minVelocity, double maxVelocity, int moveType) +{ + + phytronStatus maxStatus = phytronSuccess; + phytronStatus minStatus = phytronSuccess; + maxVelocity = fabs(maxVelocity); + minVelocity = fabs(minVelocity); + + if(maxVelocity > MAX_VELOCITY){ + maxVelocity = MAX_VELOCITY; + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "phytronAxis::setVelocity: Failed for axis %d - Velocity %f is to high, setting to" + "maximum velocity: %d!\n", axisNo_, maxVelocity, MAX_VELOCITY); + } else if (maxVelocity < MIN_VELOCITY){ + maxVelocity = MIN_VELOCITY; + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "phytronAxis::setVelocity: Failed for axis %d - Velocity %f is to low, setting to" + "minimum velocity: %d!\n", axisNo_, maxVelocity, MIN_VELOCITY); + } + + if(minVelocity > MAX_VELOCITY){ + minVelocity = MAX_VELOCITY; + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "phytronAxis::setVelocity: Failed for axis %d - Velocity %f is to high, setting to" + "maximum velocity: %d!\n", axisNo_, maxVelocity, MAX_VELOCITY); + } else if (minVelocity < MIN_VELOCITY){ + minVelocity = MIN_VELOCITY; + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "phytronAxis::setVelocity: Failed for axis %d - Velocity %f is to low, setting to" + "minimum velocity: %d!\n", axisNo_, minVelocity, MIN_VELOCITY); + } + + + if(moveType == stdMove){ + //Set maximum velocity (P14) + sprintf(pC_->outString_, "M%.1fP14=%f", axisModuleNo_, maxVelocity); + maxStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + + //Set minimum velocity (P04) + sprintf(pC_->outString_, "M%.1fP04=%f", axisModuleNo_, minVelocity); + minStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + } else if (moveType == homeMove){ + //Set maximum velocity (P08) + sprintf(pC_->outString_, "M%.1fP08=%f", axisModuleNo_, maxVelocity); + maxStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + + //Set minimum velocity (P10) + sprintf(pC_->outString_, "M%.1fP10=%f", axisModuleNo_, minVelocity); + minStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + } + + return (maxStatus > minStatus) ? maxStatus : minStatus; +} + +/** Sets acceleration parameters before the move is executed. + * \param[in] acceleration Acceleration to be used in the move + * \param[in] moveType Type of movement determines which controller acceleration parameters is set + */ +phytronStatus phytronAxis::setAcceleration(double acceleration, int moveType) +{ + if(acceleration > MAX_ACCELERATION){ + acceleration = MAX_ACCELERATION; + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "phytronAxis::setAcceleration: Failed for axis %d - Acceleration %f is to high, " + "setting to maximum acceleration: %d!\n", axisNo_, acceleration, MAX_ACCELERATION); + } else if(acceleration < MIN_ACCELERATION){ + acceleration = MIN_ACCELERATION; + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_WARNING, + "phytronAxis::setAcceleration: Failed for axis %d - Acceleration %f is to low, " + "setting to minimum acceleration: %d!\n", axisNo_, acceleration, MIN_ACCELERATION); + } + + if (moveType == stdMove){ + sprintf(pC_->outString_, "M%.1fP15=%f", axisModuleNo_, acceleration); + } else if(moveType == homeMove){ + sprintf(pC_->outString_, "M%.1fP09=%f", axisModuleNo_, acceleration); + } else if (moveType == stopMove){ + sprintf(pC_->outString_, "M%.1fP07=%f", axisModuleNo_, acceleration); + } + + return pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); +} + +/** Execute the move. + * \param[in] position Target position (relative or absolute). + * \param[in] relative Is the move absolute or relative + * \param[in] minVelocity Lowest velocity of the trapezoidal speed profile. + * \param[in] maxVelocity Highest velocity of the trapezoidal speed profile + * \param[in] acceleration Acceleration to be used + */ +asynStatus phytronAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) +{ + phytronStatus phyStatus; + + //NOTE: Check if velocity is different, before setting it. + phyStatus = setVelocity(minVelocity, maxVelocity, stdMove); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::move: Setting the velocity for axis %d to %f failed with error " + "code: %d!\n", axisNo_, maxVelocity, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + //NOTE: Check if velocity is different, before setting it. + phyStatus = setAcceleration(acceleration, stdMove); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::move: Setting the acceleration for axis %d to %f failed with " + "error code: %d!\n", axisNo_, acceleration, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + if (relative) { + sprintf(pC_->outString_, "M%.1f%c%d", axisModuleNo_, position>0 ? '+':'-', abs(NINT(position))); + } else { + sprintf(pC_->outString_, "M%.1fA%d", axisModuleNo_, NINT(position)); + } + + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::move: Moving axis %d failed with error code: %d!\n", axisNo_, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + return asynSuccess; +} + +/** Execute the homing procedure + * \param[in] minVelocity Lowest velocity of the trapezoidal speed profile. + * \param[in] maxVelocity Highest velocity of the trapezoidal speed profile + * \param[in] acceleration Acceleration to be used + * \param[in] forwards Direction of homing move + */ +asynStatus phytronAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) +{ + phytronStatus phyStatus; + int homingType; + + pC_->getIntegerParam(axisNo_, pC_->homingProcedure_, &homingType); + + phyStatus = setVelocity(minVelocity, maxVelocity, homeMove); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::home: Setting the velocity for axis %d to %f failed with error " + "code: %d!\n", axisNo_, maxVelocity, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + phyStatus = setAcceleration(acceleration, homeMove); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::home: Setting the acceleration for axis %d to %f failed with " + "error code: %d!\n", axisNo_, acceleration, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + if(forwards){ + if(homingType == limit) sprintf(pC_->outString_, "M%.1fR+", axisModuleNo_); + else if(homingType == center) sprintf(pC_->outString_, "M%.1fR+C", axisModuleNo_); + else if(homingType == encoder) sprintf(pC_->outString_, "M%.1fR+I", axisModuleNo_); + else if(homingType == limitEncoder) sprintf(pC_->outString_, "M%.1fR+^I", axisModuleNo_); + else if(homingType == centerEncoder) sprintf(pC_->outString_, "M%.1fR+C^I", axisModuleNo_); + //Homing procedures for rotational movements (no hardware limit switches) + else if(homingType == referenceCenter) sprintf(pC_->outString_, "M%.1fRC+", axisModuleNo_); + else if(homingType == referenceCenterEncoder) sprintf(pC_->outString_, "M%.1fRC+^I", axisModuleNo_); + } else { + if(homingType == limit) sprintf(pC_->outString_, "M%.1fR-", axisModuleNo_); + else if(homingType == center) sprintf(pC_->outString_, "M%.1fR-C", axisModuleNo_); + else if(homingType == encoder) sprintf(pC_->outString_, "M%.1fR-I", axisModuleNo_); + else if(homingType == limitEncoder) sprintf(pC_->outString_, "M%.1fR-^I", axisModuleNo_); + else if(homingType == centerEncoder) sprintf(pC_->outString_, "M%.1fR-C^I", axisModuleNo_); + //Homing procedures for rotational movements (no hardware limit switches) + else if(homingType == referenceCenter) sprintf(pC_->outString_, "M%.1fRC-", axisModuleNo_); + else if(homingType == referenceCenterEncoder) sprintf(pC_->outString_, "M%.1fRC-^I", axisModuleNo_); + } + + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::home: Moving axis %d failed with error code: %d!\n", axisNo_, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + return asynSuccess; +} + +/** Jog the motor. Direction is determined by sign of the maxVelocity profile + * \param[in] minVelocity Lowest velocity of the trapezoidal speed profile. + * \param[in] maxVelocity Highest velocity of the trapezoidal speed profile + * \param[in] acceleration Acceleration to be used + */ +asynStatus phytronAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) +{ + phytronStatus phyStatus; + + phyStatus = setVelocity(minVelocity, maxVelocity, stdMove); + if(phyStatus){ + + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::moveVelocity: Setting the velocity for axis %d to %f failed with error " + "code: %d!\n", axisNo_, maxVelocity, phyStatus); + } + + phyStatus = setAcceleration(acceleration, stdMove); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::moveVelocity: Setting the acceleration for axis %d to %f failed with " + "error code: %d!\n", axisNo_, acceleration, phyStatus); + } + + if(maxVelocity < 0) { + sprintf(pC_->outString_, "M%.1fL-", axisModuleNo_); + } else { + sprintf(pC_->outString_, "M%.1fL+", axisModuleNo_); + } + + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::moveVelocity: Moving axis %d failed with error code: %d!\n", axisNo_, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + return asynSuccess; +} + +/** Stop the motor + * \param[in] acceleration Deceleration to be used + */ +asynStatus phytronAxis::stop(double acceleration) +{ + phytronStatus phyStatus; + + phyStatus = setAcceleration(acceleration, stopMove); + + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::stop: Setting the acceleration for axis %d to %f failed with " + "error code: %d!\n", axisNo_, acceleration, phyStatus); + } + + sprintf(pC_->outString_, "M%.1fS", axisModuleNo_); + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::stop: Stopping axis %d failed with error code: %d!\n", axisNo_, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + return asynSuccess; +} + +//NOTE: Use this for step-slip check? +asynStatus phytronAxis::setEncoderRatio(double ratio){ + + phytronStatus phyStatus; + + sprintf(pC_->outString_, "M%.1fP39=%f", axisModuleNo_, 1/ratio); + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::setEncoderRatio: Failed for axis %d with status %d!\n", axisNo_, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + + return asynSuccess; +} + +//NOTE: Keep this for step-slip check? +asynStatus phytronAxis::setEncoderPosition(double position){ + + + return asynError; +} + +/** Set the new position of the motor on the controller + * \param[in] position New absolute motor position + */ +asynStatus phytronAxis::setPosition(double position) +{ + phytronStatus phyStatus = phytronSuccess; + + sprintf(pC_->outString_, "M%.1fP20=%f", axisModuleNo_, position); + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::setPosition: Setting position %f on axis %d failed with error code: %d!\n", position, axisNo_, phyStatus); + return pC_->phyToAsyn(phyStatus); + } + + 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 phytronAxis::poll(bool *moving) +{ + int axisStatus; + double position; + double encoderPosition; + double encoderRatio; + phytronStatus phyStatus; + + // Read the current motor position + sprintf(pC_->outString_, "M%.1fP20R", axisModuleNo_); + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + setIntegerParam(pC_->motorStatusProblem_, 1); + callParamCallbacks(); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::poll: Reading axis position failed for axis: %d!\n", axisNo_); + return pC_->phyToAsyn(phyStatus); + } + position = atof(pC_->inString_); + setDoubleParam(pC_->motorPosition_, position); + + // Read the current encoder value + sprintf(pC_->outString_, "M%.1fP22R", axisModuleNo_); + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + setIntegerParam(pC_->motorStatusProblem_, 1); + callParamCallbacks(); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::poll: Reading encoder value failed for axis: %d!\n", axisNo_); + return pC_->phyToAsyn(phyStatus); + } + encoderPosition = atof(pC_->inString_); + + /* + * The encoder position returned by the controller is weighted by the controller + * resolutio. To get absolute encoder position, the received position must be + * multiplied by the encoder resolution. + */ + pC_->getDoubleParam(axisNo_, pC_->motorEncoderRatio_, &encoderRatio); + setDoubleParam(pC_->motorEncoderPosition_, encoderPosition*encoderRatio); + + // Read the moving status of this motor + sprintf(pC_->outString_, "M%.1f==H", axisModuleNo_); + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + setIntegerParam(pC_->motorStatusProblem_, 1); + callParamCallbacks(); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::poll: Reading axis moving status failed for axis: %d!\n", axisNo_); + return pC_->phyToAsyn(phyStatus); + } + *moving = (pC_->inString_[0] == 'E') ? 0:1; + setIntegerParam(pC_->motorStatusDone_, !*moving); + + sprintf(pC_->outString_, "M%.1fSE", axisModuleNo_); + phyStatus = pC_->sendPhytronCommand(pC_->outString_, pC_->inString_, MAX_CONTROLLER_STRING_SIZE, &this->response_len); + if(phyStatus){ + setIntegerParam(pC_->motorStatusProblem_, 1); + callParamCallbacks(); + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "phytronAxis::poll: Reading axis status failed for axis: %d!\n", axisNo_); + return pC_->phyToAsyn(phyStatus); + } + axisStatus = atoi(pC_->inString_); + setIntegerParam(pC_->motorStatusHighLimit_, (axisStatus & 0x10)/0x10); + setIntegerParam(pC_->motorStatusLowLimit_, (axisStatus & 0x20)/0x20); + setIntegerParam(pC_->motorStatusAtHome_, (axisStatus & 0x40)/0x40); + + setIntegerParam(pC_->motorStatusHomed_, (axisStatus & 0x08)/0x08); + setIntegerParam(pC_->motorStatusHome_, (axisStatus & 0x08)/0x08); + + setIntegerParam(pC_->motorStatusSlip_, (axisStatus & 0x4000)/0x4000); + + //Update the axis status record ($(P)$(M)_STATUS) + setIntegerParam(pC_->axisStatus_, axisStatus); + + //No problem occurred + setIntegerParam(pC_->motorStatusProblem_, 0); + + callParamCallbacks(); + return asynSuccess; +} + +/** Parameters for iocsh phytron axis registration*/ +static const iocshArg phytronCreateAxisArg0 = {"Controller Name", iocshArgString}; +static const iocshArg phytronCreateAxisArg1 = {"Module index", iocshArgInt}; +static const iocshArg phytronCreateAxisArg2 = {"Axis index", iocshArgInt}; +static const iocshArg* const phytronCreateAxisArgs[] = {&phytronCreateAxisArg0, + &phytronCreateAxisArg1, + &phytronCreateAxisArg2}; + +/** Parameters for iocsh phytron controller registration */ +static const iocshArg phytronCreateControllerArg0 = {"Port name", iocshArgString}; +static const iocshArg phytronCreateControllerArg1 = {"PhytronAxis port name", iocshArgString}; +static const iocshArg phytronCreateControllerArg2 = {"Moving poll period (ms)", iocshArgInt}; +static const iocshArg phytronCreateControllerArg3 = {"Idle poll period (ms)", iocshArgInt}; +static const iocshArg phytronCreateControllerArg4 = {"Idle poll period (ms)", iocshArgDouble}; +static const iocshArg * const phytronCreateControllerArgs[] = {&phytronCreateControllerArg0, + &phytronCreateControllerArg1, + &phytronCreateControllerArg2, + &phytronCreateControllerArg3, + &phytronCreateControllerArg4}; + +static const iocshFuncDef phytronCreateAxisDef = {"phytronCreateAxis", 3, phytronCreateAxisArgs}; +static const iocshFuncDef phytronCreateControllerDef = {"phytronCreateController", 5, phytronCreateControllerArgs}; + +static void phytronCreateControllerCallFunc(const iocshArgBuf *args) +{ + phytronCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].dval); +} + +static void phytronCreateAxisCallFunc(const iocshArgBuf *args) +{ + phytronCreateAxis(args[0].sval, args[1].ival, args[2].ival); +} + +static void phytronRegister(void) +{ + iocshRegister(&phytronCreateControllerDef, phytronCreateControllerCallFunc); + iocshRegister(&phytronCreateAxisDef, phytronCreateAxisCallFunc); +} + +extern "C" { +epicsExportRegistrar(phytronRegister); +} diff --git a/motorApp/PhytronSrc/phytronAxisMotor.h b/motorApp/PhytronSrc/phytronAxisMotor.h new file mode 100644 index 00000000..4bf1816d --- /dev/null +++ b/motorApp/PhytronSrc/phytronAxisMotor.h @@ -0,0 +1,176 @@ +/* +FILENAME... phytronAxisMotor.h +USAGE... Motor record support for Phytron Axis controller. + +Tom Slejko & Bor Marolt +Cosylab d.d. 2014 + +Version: $Revision$ +Modified By: $Author$ +Last Modified: $Date$ +HeadURL: $URL$ +*/ + +#include "asynMotorController.h" +#include "asynMotorAxis.h" + + +//Number of controller specific parameters +#define NUM_PHYTRON_PARAMS 29 + +#define MAX_VELOCITY 40000 //steps/s +#define MIN_VELOCITY 1 //steps/s + +#define MAX_ACCELERATION 500000 // steps/s^2 +#define MIN_ACCELERATION 4000 // steps/s^2 + +//Controller parameters +#define controllerStatusString "CONTROLLER_STATUS" +#define controllerStatusResetString "CONTROLLER_STATUS_RESET" +#define resetControllerString "CONTROLLER_RESET" + +//Axis parameters +#define axisStatusString "AXIS_STATUS" +#define homingProcedureString "HOMING_PROCEDURE" +#define axisModeString "AXIS_MODE" +#define mopOffsetPosString "MOP_POS" +#define mopOffsetNegString "MOP_NEG" +#define stepResolutionString "STEP_RES" +#define stopCurrentString "STOP_CURRENT" +#define runCurrentString "RUN_CURRENT" +#define boostCurrentString "BOOST_CURRENT" +#define encoderTypeString "ENCODER_TYP" +#define initRecoveryTimeString "INIT_TIME" +#define positionRecoveryTimeString "POSITION_TIME" +#define boostConditionString "BOOST" +#define encoderRateString "ENC_RATE" +#define switchTypString "SWITCH_TYP" +#define pwrStageModeString "PWR_STAGE_MODE" +#define encoderResolutionString "ENC_RESOLUTION" +#define encoderFunctionString "ENC_FUNCTION" +#define encoderSFIWidthString "ENC_SFI_WIDTH" +#define encoderDirectionString "ENC_DIRECTION" +#define powerStageTempString "PS_TEMPERATURE" +#define powerStagetMonitorString "PS_MONITOR" +#define motorTempString "MOTOR_TEMP" +#define currentDelayTimeString "CURRENT_DELAY_TIME" +#define axisResetString "AXIS_RESET" +#define axisStatusResetString "AXIS_STATUS_RESET" + +typedef enum { + phytronSuccess, + phytronTimeout, + phytronOverflow, + phytronError, + phytronDisconnected, + phytronDisabled, + phytronInvalidReturn, + phytronInvalidCommand +} phytronStatus; + +enum movementType{ + stdMove, + homeMove, + stopMove +}; + +enum homingType{ + limit, + center, + encoder, + limitEncoder, + centerEncoder, + referenceCenter, + referenceCenterEncoder, +}; + + +class phytronAxis : public asynMotorAxis +{ +public: + /* These are the methods we override from the base class */ + phytronAxis(class phytronController *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 setEncoderRatio(double ratio); + asynStatus setEncoderPosition(double position); + + float axisModuleNo_; //Used by sprintf to form commands + +private: + phytronController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ + + phytronStatus setVelocity(double minVelocity, double maxVelocity, int moveType); + phytronStatus setAcceleration(double acceleration, int movementType); + + size_t response_len; + +friend class phytronController; +}; + +class phytronController : public asynMotorController { +public: + phytronController(const char *portName, const char *phytronPortName, double movingPollPeriod, double idlePollPeriod, double timeout); + asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value); + asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + asynStatus readFloat64(asynUser *pasynUser, epicsFloat64 *value); + + void report(FILE *fp, int level); + phytronAxis* getAxis(asynUser *pasynUser); + phytronAxis* getAxis(int axisNo); + + phytronStatus sendPhytronCommand(const char *command, char *response_buffer, size_t response_max_len, size_t *nread); + + void resetAxisEncoderRatio(); + + //casts phytronStatus to asynStatus + asynStatus phyToAsyn(phytronStatus phyStatus); + + char * controllerName_; + std::vector axes; + +protected: + //Additional parameters used by additional records + int axisStatus_; + int controllerStatus_; + int homingProcedure_; + int axisMode_; + int mopOffsetPos_; + int mopOffsetNeg_; + int stepResolution_; + int stopCurrent_; + int runCurrent_; + int boostCurrent_; + int encoderType_; + int initRecoveryTime_; + int positionRecoveryTime_; + int boost_; + int encoderRate_; + int switchTyp_; + int pwrStageMode_; + int encoderRes_; + int encoderFunc_; + int encoderSFIWidth_; + int encoderDirection_; + int powerStageTemp_; + int powerStageMonitor_; + int motorTemp_; + int currentDelayTime_; + int resetController_; + int axisReset_; + int axisStatusReset_; + int controllerStatusReset_; + +private: + double timeout_; + + +friend class phytronAxis; +};