forked from epics_driver_modules/motorBase
Added SVN keywords to file headers and set svn:eol-style to "native".
This commit is contained in:
Ron Sluiter
+36
-36
@@ -1,36 +1,36 @@
|
||||
TOP=../..
|
||||
|
||||
include $(TOP)/configure/CONFIG
|
||||
#----------------------------------------
|
||||
# ADD MACRO DEFINITIONS AFTER THIS LINE
|
||||
#=============================
|
||||
|
||||
#==================================================
|
||||
# Build an IOC support library
|
||||
|
||||
LIBRARY_IOC += PI_GCS2Support
|
||||
|
||||
# motorRecord.h will be created from motorRecord.dbd
|
||||
# install devMotorSoft.dbd into <top>/dbd
|
||||
DBD += PI_GCS2Support.dbd
|
||||
|
||||
# The following are compiled and added to the Support library
|
||||
PI_GCS2Support_SRCS += PIasynController.cpp
|
||||
PI_GCS2Support_SRCS += PIasynAxis.cpp
|
||||
PI_GCS2Support_SRCS += PIGCSController.cpp
|
||||
PI_GCS2Support_SRCS += PIGCSMotorController.cpp
|
||||
PI_GCS2Support_SRCS += PIGCSPiezoController.cpp
|
||||
PI_GCS2Support_SRCS += PIE517Controller.cpp
|
||||
PI_GCS2Support_SRCS += PIE755Controller.cpp
|
||||
PI_GCS2Support_SRCS += PIHexapodController.cpp
|
||||
PI_GCS2Support_SRCS += PIC702Controller.cpp
|
||||
PI_GCS2Support_SRCS += translateerror.c
|
||||
|
||||
PI_GCS2Support_LIBS += motor
|
||||
PI_GCS2Support_LIBS += asyn
|
||||
PI_GCS2Support_LIBS += $(EPICS_BASE_IOC_LIBS)
|
||||
|
||||
include $(TOP)/configure/RULES
|
||||
#----------------------------------------
|
||||
# ADD RULES AFTER THIS LINE
|
||||
|
||||
TOP=../..
|
||||
|
||||
include $(TOP)/configure/CONFIG
|
||||
#----------------------------------------
|
||||
# ADD MACRO DEFINITIONS AFTER THIS LINE
|
||||
#=============================
|
||||
|
||||
#==================================================
|
||||
# Build an IOC support library
|
||||
|
||||
LIBRARY_IOC += PI_GCS2Support
|
||||
|
||||
# motorRecord.h will be created from motorRecord.dbd
|
||||
# install devMotorSoft.dbd into <top>/dbd
|
||||
DBD += PI_GCS2Support.dbd
|
||||
|
||||
# The following are compiled and added to the Support library
|
||||
PI_GCS2Support_SRCS += PIasynController.cpp
|
||||
PI_GCS2Support_SRCS += PIasynAxis.cpp
|
||||
PI_GCS2Support_SRCS += PIGCSController.cpp
|
||||
PI_GCS2Support_SRCS += PIGCSMotorController.cpp
|
||||
PI_GCS2Support_SRCS += PIGCSPiezoController.cpp
|
||||
PI_GCS2Support_SRCS += PIE517Controller.cpp
|
||||
PI_GCS2Support_SRCS += PIE755Controller.cpp
|
||||
PI_GCS2Support_SRCS += PIHexapodController.cpp
|
||||
PI_GCS2Support_SRCS += PIC702Controller.cpp
|
||||
PI_GCS2Support_SRCS += translateerror.c
|
||||
|
||||
PI_GCS2Support_LIBS += motor
|
||||
PI_GCS2Support_LIBS += asyn
|
||||
PI_GCS2Support_LIBS += $(EPICS_BASE_IOC_LIBS)
|
||||
|
||||
include $(TOP)/configure/RULES
|
||||
#----------------------------------------
|
||||
# ADD RULES AFTER THIS LINE
|
||||
|
||||
|
||||
@@ -1,187 +1,194 @@
|
||||
/*
|
||||
* PIC702Controller
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#include "PIC702Controller.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
asynStatus PIC702Controller::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
int busy;
|
||||
|
||||
epicsTimeStamp now;
|
||||
epicsTimeGetCurrent(&now);
|
||||
if(epicsTimeDiffInSeconds(&now,&m_timeREFstarted) < 1.0)
|
||||
{
|
||||
homing = 1;
|
||||
moving = 1;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
asynStatus status = getBusy(pAxis, busy);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
negLimit = 0;
|
||||
posLimit = 0;
|
||||
homing = busy;
|
||||
if (busy)
|
||||
{
|
||||
moving = busy;
|
||||
return status;
|
||||
}
|
||||
|
||||
status = getMoving(pAxis, moving);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::findConnectedAxes()
|
||||
{
|
||||
// GCS! - axes are not separated by LineFeeds
|
||||
// axis identifier is only one single char, all axes returned as single "word"
|
||||
m_nrFoundAxes = 0;
|
||||
for (size_t i=0; i<MAX_NR_AXES; i++)
|
||||
{
|
||||
m_axesIDs[i] = NULL;
|
||||
}
|
||||
char allAxesIDs[127];
|
||||
asynStatus status = PIGCSController::sendAndReceive("SAI?", allAxesIDs, 127);
|
||||
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
m_nrFoundAxes = strlen(allAxesIDs);
|
||||
if (MAX_NR_AXES <= m_nrFoundAxes)
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
char* p = m_allAxesIDs;
|
||||
for (size_t i=0; i<m_nrFoundAxes; i++)
|
||||
{
|
||||
*p = allAxesIDs[m_nrFoundAxes - 1 - i];
|
||||
m_axesIDs[i] = p;
|
||||
p++;
|
||||
*p = '\0';
|
||||
p++;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::getMaxAcceleration(PIasynAxis* pAxis)
|
||||
{
|
||||
double maxAcc, maxDec;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_MOT_CURR_ACCEL, maxAcc);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
status = getGCSParameter(pAxis, PI_PARA_MOT_CURR_DECEL, maxDec);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
|
||||
if (maxAcc < maxDec)
|
||||
{
|
||||
pAxis->m_maxAcceleration = maxAcc;
|
||||
}
|
||||
else
|
||||
{
|
||||
pAxis->m_maxAcceleration = maxDec;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::hasReferenceSensor(PIasynAxis* pAxis)
|
||||
{
|
||||
double hasref;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_MOT_HAT_REF, hasref);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
pAxis->m_bHasReference = hasref > 0.1;
|
||||
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIC702Controller::hasReferenceSwitch() axis has %sreference sensor\n",
|
||||
pAxis->m_bHasReference?"":"no ");
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::getReferencedState(PIasynAxis* pAxis)
|
||||
{
|
||||
double isref;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_C702_REFERENCED, isref);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
pAxis->m_homed = (isref > 0.1)?1:0;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
|
||||
{
|
||||
asynStatus status = setServo(pAxis, 1);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
|
||||
status = setVelocityCts(pAxis, velocity);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
|
||||
char cmd[100];
|
||||
if (pAxis->m_bHasReference)
|
||||
{
|
||||
// call REF - find reference
|
||||
sprintf(cmd,"REF %s", pAxis->m_szAxisName);
|
||||
}
|
||||
else if (pAxis->m_bHasLimitSwitches)
|
||||
{
|
||||
if (forwards)
|
||||
{
|
||||
// call MPL - find positive limit switch
|
||||
sprintf(cmd,"MPL %s", pAxis->m_szAxisName);
|
||||
}
|
||||
else
|
||||
{
|
||||
// call MNL - find negative limit switch
|
||||
sprintf(cmd,"MNL %s", pAxis->m_szAxisName);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIC702Controller::referenceVelCts() failed - axis has no reference/limit switch\n");
|
||||
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
|
||||
"PIC702Controller::referenceVelCts() failed - axis has no reference/limit switch\n");
|
||||
return asynError;
|
||||
}
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIC702Controller::referenceVelCts() failed\n");
|
||||
return status;
|
||||
}
|
||||
pAxis->m_isHoming = 1;
|
||||
epicsTimeGetCurrent(&m_timeREFstarted);
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
* PIC702Controller
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#include "PIC702Controller.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
asynStatus PIC702Controller::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
int busy;
|
||||
|
||||
epicsTimeStamp now;
|
||||
epicsTimeGetCurrent(&now);
|
||||
if(epicsTimeDiffInSeconds(&now,&m_timeREFstarted) < 1.0)
|
||||
{
|
||||
homing = 1;
|
||||
moving = 1;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
asynStatus status = getBusy(pAxis, busy);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
negLimit = 0;
|
||||
posLimit = 0;
|
||||
homing = busy;
|
||||
if (busy)
|
||||
{
|
||||
moving = busy;
|
||||
return status;
|
||||
}
|
||||
|
||||
status = getMoving(pAxis, moving);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::findConnectedAxes()
|
||||
{
|
||||
// GCS! - axes are not separated by LineFeeds
|
||||
// axis identifier is only one single char, all axes returned as single "word"
|
||||
m_nrFoundAxes = 0;
|
||||
for (size_t i=0; i<MAX_NR_AXES; i++)
|
||||
{
|
||||
m_axesIDs[i] = NULL;
|
||||
}
|
||||
char allAxesIDs[127];
|
||||
asynStatus status = PIGCSController::sendAndReceive("SAI?", allAxesIDs, 127);
|
||||
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
m_nrFoundAxes = strlen(allAxesIDs);
|
||||
if (MAX_NR_AXES <= m_nrFoundAxes)
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
char* p = m_allAxesIDs;
|
||||
for (size_t i=0; i<m_nrFoundAxes; i++)
|
||||
{
|
||||
*p = allAxesIDs[m_nrFoundAxes - 1 - i];
|
||||
m_axesIDs[i] = p;
|
||||
p++;
|
||||
*p = '\0';
|
||||
p++;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::getMaxAcceleration(PIasynAxis* pAxis)
|
||||
{
|
||||
double maxAcc, maxDec;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_MOT_CURR_ACCEL, maxAcc);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
status = getGCSParameter(pAxis, PI_PARA_MOT_CURR_DECEL, maxDec);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
|
||||
if (maxAcc < maxDec)
|
||||
{
|
||||
pAxis->m_maxAcceleration = maxAcc;
|
||||
}
|
||||
else
|
||||
{
|
||||
pAxis->m_maxAcceleration = maxDec;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::hasReferenceSensor(PIasynAxis* pAxis)
|
||||
{
|
||||
double hasref;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_MOT_HAT_REF, hasref);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
pAxis->m_bHasReference = hasref > 0.1;
|
||||
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIC702Controller::hasReferenceSwitch() axis has %sreference sensor\n",
|
||||
pAxis->m_bHasReference?"":"no ");
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::getReferencedState(PIasynAxis* pAxis)
|
||||
{
|
||||
double isref;
|
||||
asynStatus status = getGCSParameter(pAxis, PI_PARA_C702_REFERENCED, isref);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
pAxis->m_homed = (isref > 0.1)?1:0;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIC702Controller::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
|
||||
{
|
||||
asynStatus status = setServo(pAxis, 1);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
|
||||
status = setVelocityCts(pAxis, velocity);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
|
||||
char cmd[100];
|
||||
if (pAxis->m_bHasReference)
|
||||
{
|
||||
// call REF - find reference
|
||||
sprintf(cmd,"REF %s", pAxis->m_szAxisName);
|
||||
}
|
||||
else if (pAxis->m_bHasLimitSwitches)
|
||||
{
|
||||
if (forwards)
|
||||
{
|
||||
// call MPL - find positive limit switch
|
||||
sprintf(cmd,"MPL %s", pAxis->m_szAxisName);
|
||||
}
|
||||
else
|
||||
{
|
||||
// call MNL - find negative limit switch
|
||||
sprintf(cmd,"MNL %s", pAxis->m_szAxisName);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIC702Controller::referenceVelCts() failed - axis has no reference/limit switch\n");
|
||||
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
|
||||
"PIC702Controller::referenceVelCts() failed - axis has no reference/limit switch\n");
|
||||
return asynError;
|
||||
}
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIC702Controller::referenceVelCts() failed\n");
|
||||
return status;
|
||||
}
|
||||
pAxis->m_isHoming = 1;
|
||||
epicsTimeGetCurrent(&m_timeREFstarted);
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,45 +1,45 @@
|
||||
/*
|
||||
* PIC702Controller.h
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#ifndef PIC702CONTROLLER_H_
|
||||
#define PIC702CONTROLLER_H_
|
||||
|
||||
#include "PIGCSMotorController.h"
|
||||
#include <epicsTime.h>
|
||||
|
||||
/**
|
||||
* class representing PI C-702.
|
||||
*
|
||||
*/
|
||||
class PIC702Controller : public PIGCSMotorController
|
||||
{
|
||||
public:
|
||||
PIC702Controller(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSMotorController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIC702Controller() {}
|
||||
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
virtual asynStatus getMaxAcceleration( PIasynAxis* pAxis );
|
||||
virtual asynStatus hasReferenceSensor(PIasynAxis* pAxis);
|
||||
virtual asynStatus getReferencedState(PIasynAxis* axis);
|
||||
virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards);
|
||||
|
||||
virtual bool IsGCS2() { return false; }
|
||||
|
||||
protected:
|
||||
virtual asynStatus findConnectedAxes();
|
||||
enum
|
||||
{
|
||||
PI_PARA_C702_REFERENCED = 0x000001CUL
|
||||
};
|
||||
|
||||
private:
|
||||
epicsTimeStamp m_timeREFstarted;
|
||||
};
|
||||
|
||||
#endif /* PIC702CONTROLLER_H_ */
|
||||
/*
|
||||
* PIC702Controller.h
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#ifndef PIC702CONTROLLER_H_
|
||||
#define PIC702CONTROLLER_H_
|
||||
|
||||
#include "PIGCSMotorController.h"
|
||||
#include <epicsTime.h>
|
||||
|
||||
/**
|
||||
* class representing PI C-702.
|
||||
*
|
||||
*/
|
||||
class PIC702Controller : public PIGCSMotorController
|
||||
{
|
||||
public:
|
||||
PIC702Controller(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSMotorController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIC702Controller() {}
|
||||
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
virtual asynStatus getMaxAcceleration( PIasynAxis* pAxis );
|
||||
virtual asynStatus hasReferenceSensor(PIasynAxis* pAxis);
|
||||
virtual asynStatus getReferencedState(PIasynAxis* axis);
|
||||
virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards);
|
||||
|
||||
virtual bool IsGCS2() { return false; }
|
||||
|
||||
protected:
|
||||
virtual asynStatus findConnectedAxes();
|
||||
enum
|
||||
{
|
||||
PI_PARA_C702_REFERENCED = 0x000001CUL
|
||||
};
|
||||
|
||||
private:
|
||||
epicsTimeStamp m_timeREFstarted;
|
||||
};
|
||||
|
||||
#endif /* PIC702CONTROLLER_H_ */
|
||||
|
||||
@@ -1,73 +1,80 @@
|
||||
/*
|
||||
* PIGCSPiezoController
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#include "PIE517Controller.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#undef asynPrint
|
||||
#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
asynStatus PIE517Controller::init()
|
||||
{
|
||||
asynStatus status;
|
||||
status = PIGCSPiezoController::init();
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
// all output channels need to be set "online" before
|
||||
// any commands are accepted over the interface.
|
||||
status = getNrOutputChannels();
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
for (int ch=1; ch<=m_nrOutputChannels; ch++)
|
||||
{
|
||||
status = setOnline(ch, 1);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIE517Controller::setOnline(int channel, int onlineState)
|
||||
{
|
||||
char cmd[100];
|
||||
sprintf(cmd, "ONL %d %d", channel, onlineState);
|
||||
asynStatus status = sendOnly(cmd);
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIE517Controller::getNrOutputChannels()
|
||||
{
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive("TPC?", buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
m_nrOutputChannels = atoi(buf);
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIE517Controller::initAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
pAxis->m_movingStateMask = pow(2.0, pAxis->getAxisNo());
|
||||
|
||||
return setServo(pAxis, 1);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* PIGCSPiezoController
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#include "PIE517Controller.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#undef asynPrint
|
||||
#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
asynStatus PIE517Controller::init()
|
||||
{
|
||||
asynStatus status;
|
||||
status = PIGCSPiezoController::init();
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
// all output channels need to be set "online" before
|
||||
// any commands are accepted over the interface.
|
||||
status = getNrOutputChannels();
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
for (int ch=1; ch<=m_nrOutputChannels; ch++)
|
||||
{
|
||||
status = setOnline(ch, 1);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIE517Controller::setOnline(int channel, int onlineState)
|
||||
{
|
||||
char cmd[100];
|
||||
sprintf(cmd, "ONL %d %d", channel, onlineState);
|
||||
asynStatus status = sendOnly(cmd);
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIE517Controller::getNrOutputChannels()
|
||||
{
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive("TPC?", buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
m_nrOutputChannels = atoi(buf);
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIE517Controller::initAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
pAxis->m_movingStateMask = pow(2.0, pAxis->getAxisNo());
|
||||
|
||||
return setServo(pAxis, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,42 +1,49 @@
|
||||
/*
|
||||
* PIGCScontroller.h
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#ifndef PIE517CONTROLLER_H_
|
||||
#define PIE517CONTROLLER_H_
|
||||
|
||||
#include "PIGCSPiezoController.h"
|
||||
|
||||
/**
|
||||
* class representing PI E-517 and E-545 controllers.
|
||||
*
|
||||
* Basically these controllers have the same functionality as a
|
||||
* digital piezo controller. Specialties are "velocity control" mode
|
||||
* and the "online" state. Output channels must be set to "online" mode
|
||||
* before commands are accepted over the interface. This is done in
|
||||
* init()
|
||||
*/
|
||||
class PIE517Controller : public PIGCSPiezoController
|
||||
{
|
||||
public:
|
||||
PIE517Controller(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSPiezoController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIE517Controller() {}
|
||||
|
||||
virtual asynStatus init(void);
|
||||
virtual asynStatus initAxis(PIasynAxis* pAxis);
|
||||
|
||||
private:
|
||||
asynStatus setOnline(int outputChannel, int onlineState);
|
||||
asynStatus getNrOutputChannels();
|
||||
|
||||
int m_nrOutputChannels;
|
||||
|
||||
};
|
||||
|
||||
#endif /* PIE517CONTROLLER_H_ */
|
||||
/*
|
||||
* PIGCScontroller.h
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#ifndef PIE517CONTROLLER_H_
|
||||
#define PIE517CONTROLLER_H_
|
||||
|
||||
#include "PIGCSPiezoController.h"
|
||||
|
||||
/**
|
||||
* class representing PI E-517 and E-545 controllers.
|
||||
*
|
||||
* Basically these controllers have the same functionality as a
|
||||
* digital piezo controller. Specialties are "velocity control" mode
|
||||
* and the "online" state. Output channels must be set to "online" mode
|
||||
* before commands are accepted over the interface. This is done in
|
||||
* init()
|
||||
*/
|
||||
class PIE517Controller : public PIGCSPiezoController
|
||||
{
|
||||
public:
|
||||
PIE517Controller(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSPiezoController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIE517Controller() {}
|
||||
|
||||
virtual asynStatus init(void);
|
||||
virtual asynStatus initAxis(PIasynAxis* pAxis);
|
||||
|
||||
private:
|
||||
asynStatus setOnline(int outputChannel, int onlineState);
|
||||
asynStatus getNrOutputChannels();
|
||||
|
||||
int m_nrOutputChannels;
|
||||
|
||||
};
|
||||
|
||||
#endif /* PIE517CONTROLLER_H_ */
|
||||
|
||||
@@ -1,40 +1,47 @@
|
||||
/*
|
||||
* PIE755Controller
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#include "PIE755Controller.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <stdlib.h>
|
||||
|
||||
#undef asynPrint
|
||||
#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
asynStatus PIE755Controller::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
int busy;
|
||||
asynStatus status = getBusy(pAxis, busy);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
negLimit = 0;
|
||||
posLimit = 0;
|
||||
homing = busy;
|
||||
if (busy)
|
||||
{
|
||||
moving = busy;
|
||||
return status;
|
||||
}
|
||||
|
||||
status = getMoving(pAxis, moving);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
/*
|
||||
* PIE755Controller
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#include "PIE755Controller.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <stdlib.h>
|
||||
|
||||
#undef asynPrint
|
||||
#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
asynStatus PIE755Controller::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
int busy;
|
||||
asynStatus status = getBusy(pAxis, busy);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
negLimit = 0;
|
||||
posLimit = 0;
|
||||
homing = busy;
|
||||
if (busy)
|
||||
{
|
||||
moving = busy;
|
||||
return status;
|
||||
}
|
||||
|
||||
status = getMoving(pAxis, moving);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
@@ -1,41 +1,48 @@
|
||||
/*
|
||||
* PIE755Controller.h
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#ifndef PIE755CONTROLLER_H_
|
||||
#define PIE755CONTROLLER_H_
|
||||
|
||||
#include "PIGCSMotorController.h"
|
||||
|
||||
/**
|
||||
* class representing PI E-755.
|
||||
*
|
||||
* These controllers share most of the parameters with digital piezo controllers.
|
||||
* Encoders for Nexline stages are often only incremental, so they need to be homed.
|
||||
* Homing works with "hard stops" and "Limit switches".
|
||||
* From an EPICS point of view this behaves more like a motor than a piezo controller.
|
||||
*/
|
||||
class PIE755Controller : public PIGCSMotorController
|
||||
{
|
||||
public:
|
||||
PIE755Controller(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSMotorController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIE755Controller() {}
|
||||
|
||||
virtual asynStatus getResolution(PIasynAxis* pAxis, double& resolution )
|
||||
{
|
||||
return PIGCSController::getResolution(pAxis, resolution);
|
||||
}
|
||||
virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
#endif /* PIE755CONTROLLER_H_ */
|
||||
/*
|
||||
* PIE755Controller.h
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#ifndef PIE755CONTROLLER_H_
|
||||
#define PIE755CONTROLLER_H_
|
||||
|
||||
#include "PIGCSMotorController.h"
|
||||
|
||||
/**
|
||||
* class representing PI E-755.
|
||||
*
|
||||
* These controllers share most of the parameters with digital piezo controllers.
|
||||
* Encoders for Nexline stages are often only incremental, so they need to be homed.
|
||||
* Homing works with "hard stops" and "Limit switches".
|
||||
* From an EPICS point of view this behaves more like a motor than a piezo controller.
|
||||
*/
|
||||
class PIE755Controller : public PIGCSMotorController
|
||||
{
|
||||
public:
|
||||
PIE755Controller(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSMotorController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIE755Controller() {}
|
||||
|
||||
virtual asynStatus getResolution(PIasynAxis* pAxis, double& resolution )
|
||||
{
|
||||
return PIGCSController::getResolution(pAxis, resolution);
|
||||
}
|
||||
virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
#endif /* PIE755CONTROLLER_H_ */
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,114 +1,121 @@
|
||||
/*
|
||||
* PIGCScontroller.h
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#ifndef PIGCSCONTROLLER_H_
|
||||
#define PIGCSCONTROLLER_H_
|
||||
|
||||
#include <asynDriver.h>
|
||||
#include <string.h>
|
||||
#include <epicsMutex.h>
|
||||
#include "picontrollererrors.h"
|
||||
|
||||
|
||||
class PIasynAxis;
|
||||
class asynMotorAxis;
|
||||
|
||||
/**
|
||||
* Base class for all PI GCS(2) controllers.
|
||||
*
|
||||
* Most functions will be implemented here, since for basic functionality
|
||||
* GCS commands are always the same.
|
||||
*/
|
||||
class PIGCSController
|
||||
{
|
||||
public:
|
||||
PIGCSController(asynUser* pCom, const char* szIDN);
|
||||
virtual ~PIGCSController();
|
||||
|
||||
static PIGCSController* CreateGCSController(asynUser* pCom, const char* szIDN);
|
||||
|
||||
virtual asynStatus init(void);
|
||||
virtual asynStatus initAxis(PIasynAxis* pAxis);
|
||||
|
||||
static asynStatus sendOnly(asynUser* pInterface, char c, asynUser* logSink);
|
||||
static asynStatus sendOnly(asynUser* pInterface, const char *outputBuff, asynUser* logSink);
|
||||
static asynStatus sendAndReceive(asynUser* pInterface, const char *outputBuff, char *inputBuff, int inputSize, asynUser* logSink);
|
||||
static asynStatus sendAndReceive(asynUser* pInterface, char c, char *inputBuff, int inputSize, asynUser* logSink);
|
||||
|
||||
bool getValue(const char* szMsg, double& value);
|
||||
bool getValue(const char* szMsg, int& value);
|
||||
bool getValue(const char* szMsg, bool& value);
|
||||
|
||||
virtual asynStatus setVelocityCts( PIasynAxis* pAxis, double velocity );
|
||||
virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus move( PIasynAxis* pAxis, double target);
|
||||
virtual asynStatus moveCts( PIasynAxis* pAxis, int target);
|
||||
virtual asynStatus moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes);
|
||||
virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards) { return asynSuccess; }
|
||||
virtual asynStatus haltAxis(PIasynAxis* pAxis);
|
||||
|
||||
virtual asynStatus getAxisPosition(PIasynAxis* pAxis, double& position);
|
||||
virtual asynStatus getAxisVelocity(PIasynAxis* pAxis);
|
||||
virtual asynStatus getAxisPositionCts(PIasynAxis* pAxis);
|
||||
virtual asynStatus setServo(PIasynAxis* pAxis, int servoState);
|
||||
virtual asynStatus getResolution(PIasynAxis* pAxis, double& resolution );
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl) = 0;
|
||||
virtual asynStatus getGlobalState( asynMotorAxis** Axes, int numAxes ) { return asynSuccess; }
|
||||
virtual asynStatus getMoving(PIasynAxis* pAxis, int& homing);
|
||||
virtual asynStatus getBusy(PIasynAxis* pAxis, int& busy);
|
||||
virtual asynStatus getTravelLimits(PIasynAxis* pAxis, double& negLimit, double& posLimit);
|
||||
virtual asynStatus hasLimitSwitches(PIasynAxis* pAxis);
|
||||
virtual asynStatus hasReferenceSensor(PIasynAxis* pAxis);
|
||||
virtual asynStatus getReferencedState(PIasynAxis* axis);
|
||||
|
||||
virtual asynStatus SetPivotX(double value);
|
||||
virtual asynStatus SetPivotY(double value);
|
||||
virtual asynStatus SetPivotZ(double value);
|
||||
|
||||
virtual double GetPivotX() { return 0.0; }
|
||||
virtual double GetPivotY() { return 0.0; }
|
||||
virtual double GetPivotZ() { return 0.0; }
|
||||
|
||||
virtual bool AcceptsNewTarget() { return true; }
|
||||
virtual bool CanCommunicateWhileHoming() { return true; }
|
||||
|
||||
asynStatus sendOnly(const char *outputBuff);
|
||||
asynStatus sendOnly(char c);
|
||||
asynStatus sendAndReceive(const char *outputBuff, char *inputBuff, int inputSize);
|
||||
asynStatus sendAndReceive(char c, char *inputBuff, int inputSize);
|
||||
|
||||
const char* getAxesID(size_t axisIdx) { return m_axesIDs[axisIdx]; }
|
||||
size_t getNrFoundAxes() { return m_nrFoundAxes; }
|
||||
|
||||
virtual bool IsGCS2() { return true; }
|
||||
|
||||
int getGCSError();
|
||||
|
||||
int GetLastError() { return m_LastError; }
|
||||
|
||||
asynUser* m_pCurrentLogSink;
|
||||
static const size_t MAX_NR_AXES = 64;
|
||||
bool m_bAnyAxisMoving;
|
||||
protected:
|
||||
asynStatus setGCSParameter(PIasynAxis* pAxis, unsigned int paramID, double value);
|
||||
asynStatus getGCSParameter(PIasynAxis* pAxis, unsigned int paramID, double& value);
|
||||
|
||||
virtual asynStatus findConnectedAxes();
|
||||
|
||||
asynUser* m_pInterface;
|
||||
epicsMutex m_interfaceMutex;
|
||||
static double TIMEOUT;
|
||||
char szIdentification[200];
|
||||
int m_nrAxesOnController;
|
||||
char* m_axesIDs[MAX_NR_AXES];
|
||||
size_t m_nrFoundAxes;
|
||||
char m_allAxesIDs[255];
|
||||
int m_LastError;
|
||||
};
|
||||
|
||||
#endif /* PIGCSCONTROLLER_H_ */
|
||||
/*
|
||||
* PIGCScontroller.h
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#ifndef PIGCSCONTROLLER_H_
|
||||
#define PIGCSCONTROLLER_H_
|
||||
|
||||
#include <asynDriver.h>
|
||||
#include <string.h>
|
||||
#include <epicsMutex.h>
|
||||
#include "picontrollererrors.h"
|
||||
|
||||
|
||||
class PIasynAxis;
|
||||
class asynMotorAxis;
|
||||
|
||||
/**
|
||||
* Base class for all PI GCS(2) controllers.
|
||||
*
|
||||
* Most functions will be implemented here, since for basic functionality
|
||||
* GCS commands are always the same.
|
||||
*/
|
||||
class PIGCSController
|
||||
{
|
||||
public:
|
||||
PIGCSController(asynUser* pCom, const char* szIDN);
|
||||
virtual ~PIGCSController();
|
||||
|
||||
static PIGCSController* CreateGCSController(asynUser* pCom, const char* szIDN);
|
||||
|
||||
virtual asynStatus init(void);
|
||||
virtual asynStatus initAxis(PIasynAxis* pAxis);
|
||||
|
||||
static asynStatus sendOnly(asynUser* pInterface, char c, asynUser* logSink);
|
||||
static asynStatus sendOnly(asynUser* pInterface, const char *outputBuff, asynUser* logSink);
|
||||
static asynStatus sendAndReceive(asynUser* pInterface, const char *outputBuff, char *inputBuff, int inputSize, asynUser* logSink);
|
||||
static asynStatus sendAndReceive(asynUser* pInterface, char c, char *inputBuff, int inputSize, asynUser* logSink);
|
||||
|
||||
bool getValue(const char* szMsg, double& value);
|
||||
bool getValue(const char* szMsg, int& value);
|
||||
bool getValue(const char* szMsg, bool& value);
|
||||
|
||||
virtual asynStatus setVelocityCts( PIasynAxis* pAxis, double velocity );
|
||||
virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus move( PIasynAxis* pAxis, double target);
|
||||
virtual asynStatus moveCts( PIasynAxis* pAxis, int target);
|
||||
virtual asynStatus moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes);
|
||||
virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards) { return asynSuccess; }
|
||||
virtual asynStatus haltAxis(PIasynAxis* pAxis);
|
||||
|
||||
virtual asynStatus getAxisPosition(PIasynAxis* pAxis, double& position);
|
||||
virtual asynStatus getAxisVelocity(PIasynAxis* pAxis);
|
||||
virtual asynStatus getAxisPositionCts(PIasynAxis* pAxis);
|
||||
virtual asynStatus setServo(PIasynAxis* pAxis, int servoState);
|
||||
virtual asynStatus getResolution(PIasynAxis* pAxis, double& resolution );
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl) = 0;
|
||||
virtual asynStatus getGlobalState( asynMotorAxis** Axes, int numAxes ) { return asynSuccess; }
|
||||
virtual asynStatus getMoving(PIasynAxis* pAxis, int& homing);
|
||||
virtual asynStatus getBusy(PIasynAxis* pAxis, int& busy);
|
||||
virtual asynStatus getTravelLimits(PIasynAxis* pAxis, double& negLimit, double& posLimit);
|
||||
virtual asynStatus hasLimitSwitches(PIasynAxis* pAxis);
|
||||
virtual asynStatus hasReferenceSensor(PIasynAxis* pAxis);
|
||||
virtual asynStatus getReferencedState(PIasynAxis* axis);
|
||||
|
||||
virtual asynStatus SetPivotX(double value);
|
||||
virtual asynStatus SetPivotY(double value);
|
||||
virtual asynStatus SetPivotZ(double value);
|
||||
|
||||
virtual double GetPivotX() { return 0.0; }
|
||||
virtual double GetPivotY() { return 0.0; }
|
||||
virtual double GetPivotZ() { return 0.0; }
|
||||
|
||||
virtual bool AcceptsNewTarget() { return true; }
|
||||
virtual bool CanCommunicateWhileHoming() { return true; }
|
||||
|
||||
asynStatus sendOnly(const char *outputBuff);
|
||||
asynStatus sendOnly(char c);
|
||||
asynStatus sendAndReceive(const char *outputBuff, char *inputBuff, int inputSize);
|
||||
asynStatus sendAndReceive(char c, char *inputBuff, int inputSize);
|
||||
|
||||
const char* getAxesID(size_t axisIdx) { return m_axesIDs[axisIdx]; }
|
||||
size_t getNrFoundAxes() { return m_nrFoundAxes; }
|
||||
|
||||
virtual bool IsGCS2() { return true; }
|
||||
|
||||
int getGCSError();
|
||||
|
||||
int GetLastError() { return m_LastError; }
|
||||
|
||||
asynUser* m_pCurrentLogSink;
|
||||
static const size_t MAX_NR_AXES = 64;
|
||||
bool m_bAnyAxisMoving;
|
||||
protected:
|
||||
asynStatus setGCSParameter(PIasynAxis* pAxis, unsigned int paramID, double value);
|
||||
asynStatus getGCSParameter(PIasynAxis* pAxis, unsigned int paramID, double& value);
|
||||
|
||||
virtual asynStatus findConnectedAxes();
|
||||
|
||||
asynUser* m_pInterface;
|
||||
epicsMutex m_interfaceMutex;
|
||||
static double TIMEOUT;
|
||||
char szIdentification[200];
|
||||
int m_nrAxesOnController;
|
||||
char* m_axesIDs[MAX_NR_AXES];
|
||||
size_t m_nrFoundAxes;
|
||||
char m_allAxesIDs[255];
|
||||
int m_LastError;
|
||||
};
|
||||
|
||||
#endif /* PIGCSCONTROLLER_H_ */
|
||||
|
||||
@@ -1,189 +1,196 @@
|
||||
/*
|
||||
* PIGCSMotorController.cpp
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#include "PIGCSMotorController.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
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 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;
|
||||
|
||||
return setAcceleration(pAxis, acceleration);
|
||||
}
|
||||
|
||||
asynStatus PIGCSMotorController::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
|
||||
{
|
||||
asynStatus status = setServo(pAxis, 1);
|
||||
if (asynSuccess != 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);
|
||||
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_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 = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode == 0)
|
||||
{
|
||||
return asynSuccess;
|
||||
}
|
||||
asynPrint(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;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Get encoder resolution from counts-per-unit (CPU) fraction of the axis.
|
||||
*/
|
||||
asynStatus PIGCSMotorController::getResolution(PIasynAxis* pAxis, double& resolution )
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
status = getGCSParameter(pAxis, PI_PARA_MOT_CPU_N, denom);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
pAxis->m_CPUnumerator = num;
|
||||
pAxis->m_CPUdenominator = denom;
|
||||
resolution = double(pAxis->m_CPUdenominator) / double(pAxis->m_CPUnumerator) ;
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIGCSMotorController::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(4), buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
// TODO this is for a single axis C-863/867 controller!!!!
|
||||
// TODO a) change it to multi-axis code.
|
||||
// TODO b) support other controllers which do not understand #4 or have different bit masks
|
||||
|
||||
int idx = 2 + pAxis->getAxisNo()*4;
|
||||
buf[idx+4] = '\0';
|
||||
char* szMask = buf+idx;
|
||||
long mask = strtol(szMask, NULL, 16);
|
||||
moving = (mask & 0x2000) ? 1 : 0;
|
||||
homing = (mask & 0x4000) ? 1 : 0;
|
||||
negLimit = (mask & 0x0001) ? 1 : 0;
|
||||
posLimit = (mask & 0x0004) ? 1 : 0;
|
||||
servoControl = (mask & 0x1000) ? 1 : 0;
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSMotorController::getStatus() buf:%s moving %d, svo: %d\n",
|
||||
buf, moving, servoControl);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
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 (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;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* PIGCSMotorController.cpp
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#include "PIGCSMotorController.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
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 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;
|
||||
|
||||
return setAcceleration(pAxis, acceleration);
|
||||
}
|
||||
|
||||
asynStatus PIGCSMotorController::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
|
||||
{
|
||||
asynStatus status = setServo(pAxis, 1);
|
||||
if (asynSuccess != 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);
|
||||
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_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 = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
return status;
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode == 0)
|
||||
{
|
||||
return asynSuccess;
|
||||
}
|
||||
asynPrint(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;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Get encoder resolution from counts-per-unit (CPU) fraction of the axis.
|
||||
*/
|
||||
asynStatus PIGCSMotorController::getResolution(PIasynAxis* pAxis, double& resolution )
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
status = getGCSParameter(pAxis, PI_PARA_MOT_CPU_N, denom);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
pAxis->m_CPUnumerator = num;
|
||||
pAxis->m_CPUdenominator = denom;
|
||||
resolution = double(pAxis->m_CPUdenominator) / double(pAxis->m_CPUnumerator) ;
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIGCSMotorController::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(4), buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
// TODO this is for a single axis C-863/867 controller!!!!
|
||||
// TODO a) change it to multi-axis code.
|
||||
// TODO b) support other controllers which do not understand #4 or have different bit masks
|
||||
|
||||
int idx = 2 + pAxis->getAxisNo()*4;
|
||||
buf[idx+4] = '\0';
|
||||
char* szMask = buf+idx;
|
||||
long mask = strtol(szMask, NULL, 16);
|
||||
moving = (mask & 0x2000) ? 1 : 0;
|
||||
homing = (mask & 0x4000) ? 1 : 0;
|
||||
negLimit = (mask & 0x0001) ? 1 : 0;
|
||||
posLimit = (mask & 0x0004) ? 1 : 0;
|
||||
servoControl = (mask & 0x1000) ? 1 : 0;
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIGCSMotorController::getStatus() buf:%s moving %d, svo: %d\n",
|
||||
buf, moving, servoControl);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
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 (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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,53 +1,60 @@
|
||||
/*
|
||||
* PIGCScontroller.h
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#ifndef PIGCSMOTORCONTROLLER_H_
|
||||
#define PIGCSMOTORCONTROLLER_H_
|
||||
|
||||
#include "PIGCSController.h"
|
||||
#include <asynDriver.h>
|
||||
|
||||
/**
|
||||
* class representing PI GCS2 motor controllers.
|
||||
*
|
||||
* "motor" does not strictly restrict the driving principle to DC stages or
|
||||
* stepper but includes other controller families (e.g. for piezo motors) also.
|
||||
*/
|
||||
class PIGCSMotorController : public PIGCSController
|
||||
{
|
||||
public:
|
||||
PIGCSMotorController(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIGCSMotorController() {}
|
||||
virtual asynStatus initAxis(PIasynAxis* pAxis);
|
||||
|
||||
virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration);
|
||||
virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration);
|
||||
virtual asynStatus getMaxAcceleration( PIasynAxis* pAxis );
|
||||
virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards);
|
||||
virtual asynStatus getResolution(PIasynAxis* pAxis, double& resolution );
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
|
||||
protected:
|
||||
enum
|
||||
{
|
||||
PI_PARA_MOT_CURR_ACCEL = 0x000000BUL,
|
||||
PI_PARA_MOT_CURR_DECEL = 0x000000CUL,
|
||||
PI_PARA_MOT_CPU_Z = 0x000000EUL,
|
||||
PI_PARA_MOT_CPU_N = 0x000000FUL,
|
||||
PI_PARA_MOT_HAT_REF = 0x0000014UL,
|
||||
PI_PARA_MOT_MAX_ACCEL = 0x000004AUL,
|
||||
PI_PARA_MOT_MAX_DECEL = 0x000004BUL
|
||||
};
|
||||
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
#endif /* PIGCSMOTORCONTROLLER_H_ */
|
||||
/*
|
||||
* PIGCScontroller.h
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#ifndef PIGCSMOTORCONTROLLER_H_
|
||||
#define PIGCSMOTORCONTROLLER_H_
|
||||
|
||||
#include "PIGCSController.h"
|
||||
#include <asynDriver.h>
|
||||
|
||||
/**
|
||||
* class representing PI GCS2 motor controllers.
|
||||
*
|
||||
* "motor" does not strictly restrict the driving principle to DC stages or
|
||||
* stepper but includes other controller families (e.g. for piezo motors) also.
|
||||
*/
|
||||
class PIGCSMotorController : public PIGCSController
|
||||
{
|
||||
public:
|
||||
PIGCSMotorController(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIGCSMotorController() {}
|
||||
virtual asynStatus initAxis(PIasynAxis* pAxis);
|
||||
|
||||
virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration);
|
||||
virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration);
|
||||
virtual asynStatus getMaxAcceleration( PIasynAxis* pAxis );
|
||||
virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards);
|
||||
virtual asynStatus getResolution(PIasynAxis* pAxis, double& resolution );
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
|
||||
protected:
|
||||
enum
|
||||
{
|
||||
PI_PARA_MOT_CURR_ACCEL = 0x000000BUL,
|
||||
PI_PARA_MOT_CURR_DECEL = 0x000000CUL,
|
||||
PI_PARA_MOT_CPU_Z = 0x000000EUL,
|
||||
PI_PARA_MOT_CPU_N = 0x000000FUL,
|
||||
PI_PARA_MOT_HAT_REF = 0x0000014UL,
|
||||
PI_PARA_MOT_MAX_ACCEL = 0x000004AUL,
|
||||
PI_PARA_MOT_MAX_DECEL = 0x000004BUL
|
||||
};
|
||||
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
#endif /* PIGCSMOTORCONTROLLER_H_ */
|
||||
|
||||
@@ -1,37 +1,44 @@
|
||||
/*
|
||||
* PIGCSPiezoController
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#include "PIGCSPiezoController.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#undef asynPrint
|
||||
#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
asynStatus PIGCSPiezoController::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
asynStatus status = getMoving(pAxis, moving);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
homing = 0;
|
||||
negLimit = 0;
|
||||
posLimit = 0;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSPiezoController::getReferencedState(PIasynAxis* pAxis)
|
||||
{
|
||||
pAxis->m_homed = 1;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/*
|
||||
* PIGCSPiezoController
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#include "PIGCSPiezoController.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#undef asynPrint
|
||||
#define asynPrint(user,reason,format...) 0
|
||||
|
||||
|
||||
asynStatus PIGCSPiezoController::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
asynStatus status = getMoving(pAxis, moving);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
homing = 0;
|
||||
negLimit = 0;
|
||||
posLimit = 0;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIGCSPiezoController::getReferencedState(PIasynAxis* pAxis)
|
||||
{
|
||||
pAxis->m_homed = 1;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,38 +1,45 @@
|
||||
/*
|
||||
* PIGCScontroller.h
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#ifndef PIGCSPIEZOCONTROLLER_H_
|
||||
#define PIGCSPIEZOCONTROLLER_H_
|
||||
|
||||
#include "PIGCSController.h"
|
||||
#include <asynDriver.h>
|
||||
|
||||
/**
|
||||
* Class representing PI's GCS2 digital piezo controllers.
|
||||
*
|
||||
* Main difference to motor controllers is the usage of absolute sensors.
|
||||
*/
|
||||
class PIGCSPiezoController : public PIGCSController
|
||||
{
|
||||
public:
|
||||
PIGCSPiezoController(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIGCSPiezoController() {}
|
||||
|
||||
virtual asynStatus init(void) { return PIGCSController::init(); }
|
||||
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
virtual asynStatus getReferencedState(PIasynAxis* pAxis);
|
||||
|
||||
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
#endif /* PIGCSPIEZOCONTROLLER_H_ */
|
||||
/*
|
||||
* PIGCScontroller.h
|
||||
*
|
||||
* Created on: 15.12.2010
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#ifndef PIGCSPIEZOCONTROLLER_H_
|
||||
#define PIGCSPIEZOCONTROLLER_H_
|
||||
|
||||
#include "PIGCSController.h"
|
||||
#include <asynDriver.h>
|
||||
|
||||
/**
|
||||
* Class representing PI's GCS2 digital piezo controllers.
|
||||
*
|
||||
* Main difference to motor controllers is the usage of absolute sensors.
|
||||
*/
|
||||
class PIGCSPiezoController : public PIGCSController
|
||||
{
|
||||
public:
|
||||
PIGCSPiezoController(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIGCSPiezoController() {}
|
||||
|
||||
virtual asynStatus init(void) { return PIGCSController::init(); }
|
||||
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
virtual asynStatus getReferencedState(PIasynAxis* pAxis);
|
||||
|
||||
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
#endif /* PIGCSPIEZOCONTROLLER_H_ */
|
||||
|
||||
@@ -1,444 +1,451 @@
|
||||
/*
|
||||
* PIE755Controller
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#include "PIHexapodController.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <stdlib.h>
|
||||
#include <epicsThread.h>
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
asynStatus PIHexapodController::init(void)
|
||||
{
|
||||
asynStatus status = PIGCSController::init();
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
// Try to find out if #4 is supported
|
||||
char buf[200];
|
||||
status = sendAndReceive(char(4), buf, 199);
|
||||
if (status == asynSuccess)
|
||||
{
|
||||
m_bCanReadStatusWithChar4 = true;
|
||||
}
|
||||
else if (status == asynTimeout)
|
||||
{
|
||||
m_bCanReadStatusWithChar4 = false;
|
||||
getGCSError(); // clear error UNKNOWN COMMAND
|
||||
}
|
||||
// status = sendAndReceive(char(3), buf, 199);
|
||||
// if (status == asynSuccess)
|
||||
// {
|
||||
// m_bCanReadPosWithChar3 = true;
|
||||
// }
|
||||
// else if (status == asynTimeout)
|
||||
{
|
||||
m_bCanReadPosWithChar3 = false;
|
||||
getGCSError(); // clear error UNKNOWN COMMAND
|
||||
}
|
||||
|
||||
m_bHoming = false;
|
||||
|
||||
ReadPivotSettings();
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
asynStatus PIHexapodController::getGlobalState(asynMotorAxis** pAxes, int numAxes)
|
||||
{
|
||||
// do not call moving here, at least simulation software does return
|
||||
// values != 0 with bit 1 not set (e.g. "2")
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(5), buf, 99);;
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
printf("PIGCSController::getGlobalState() failed, status %d", status);
|
||||
return status;
|
||||
}
|
||||
|
||||
char* pStr;
|
||||
long moving = strtol(buf, &pStr, 16);
|
||||
m_bAnyAxisMoving = (moving != 0);
|
||||
if (m_bHoming && (moving == 0))
|
||||
{
|
||||
m_bHoming = false;
|
||||
}
|
||||
for(int i=0; i<numAxes; i++)
|
||||
{
|
||||
((PIasynAxis*)pAxes[i])->m_bMoving = (moving != 0);
|
||||
}
|
||||
return asynSuccess;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
negLimit = 0;
|
||||
posLimit = 0;
|
||||
homing = m_bHoming ? 1:0;
|
||||
|
||||
moving = pAxis->m_bMoving || pAxis->deferred_move;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::findConnectedAxes()
|
||||
{
|
||||
for (size_t i=0; i<MAX_NR_AXES; i++)
|
||||
{
|
||||
m_axesIDs[i] = NULL;
|
||||
}
|
||||
sprintf(m_allAxesIDs, "X\nY\nZ\nU\nV\nW");
|
||||
char* szAxis = strtok(m_allAxesIDs, "\n");
|
||||
while (szAxis != NULL)
|
||||
{
|
||||
int i=strlen(szAxis)-1;
|
||||
while (szAxis[i] == ' ')
|
||||
{
|
||||
szAxis[i] = '\0';
|
||||
i--;
|
||||
}
|
||||
if (MAX_NR_AXES <= m_nrFoundAxes)
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
m_axesIDs[m_nrFoundAxes] = szAxis;
|
||||
m_nrFoundAxes++;
|
||||
szAxis = strtok(NULL, "\n");
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::initAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
pAxis->m_movingStateMask = 1;
|
||||
return setServo(pAxis, 1);
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::getReferencedState(PIasynAxis* pAxis)
|
||||
{
|
||||
if (!m_bCanReadStatusWithChar4)
|
||||
{
|
||||
pAxis->m_homed = 1;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(4), buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
long mask = strtol(buf, NULL, 10);
|
||||
pAxis->m_homed = (mask & 0x10000) ? 1 : 0;
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::moveCts( PIasynAxis* pAxis, int targetCts )
|
||||
{
|
||||
// printf("PIHexapodController::moveCts(,%d)...\n",targetCts);
|
||||
asynStatus status;
|
||||
char cmd[100];
|
||||
double target = double(targetCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
sprintf(cmd,"MOV %s %f", pAxis->m_szAxisName, target);
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
epicsThreadSleep( 0.2 ); // TODO test if we do need this
|
||||
asynMotorAxis* pAsynAxis = (asynMotorAxis*)pAxis;
|
||||
status = getGlobalState(&pAsynAxis, 1);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
// printf("PIHexapodController::moveCts(,%d) - getGlobalState() failed, status=%d\n", targetCts, status);
|
||||
return status;
|
||||
}
|
||||
if (!pAxis->m_bMoving)
|
||||
{
|
||||
int errorCode = getGCSError();
|
||||
// printf("PIHexapodController::moveCts(,%d) - not moving after MOV() gcserror=%d\n",targetCts, errorCode);
|
||||
if (errorCode != 0)
|
||||
{
|
||||
asynPrint(m_pInterface, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIHexapodController::moveCts() failed, GCS error %d\n", errorCode);
|
||||
return asynError;
|
||||
}
|
||||
}
|
||||
m_bAnyAxisMoving = true;
|
||||
pAxis->m_lastDirection = (targetCts > pAxis->m_positionCts) ? 1 : 0;
|
||||
printf("PIHexapodController::moveCts(,%d) - OK!\n",targetCts);
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes)
|
||||
{
|
||||
asynStatus status;
|
||||
char cmd[1000] = "MOV";
|
||||
char subCmd[100];
|
||||
for (int axis = 0; axis <numAxes; axis++)
|
||||
{
|
||||
PIasynAxis* pAxis = pAxesArray[axis];
|
||||
double target = double(pTargetCtsArray[axis]) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
sprintf(subCmd," %s %f", pAxis->m_szAxisName, target);
|
||||
strcat(cmd, subCmd);
|
||||
pAxis->m_lastDirection = (pTargetCtsArray[axis] > pAxis->m_positionCts) ? 1 : 0;
|
||||
}
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
epicsThreadSleep(0.2);
|
||||
status = getGlobalState((asynMotorAxis**)pAxesArray, numAxes);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
if (!pAxesArray[0]->m_bMoving)
|
||||
{
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode != 0)
|
||||
{
|
||||
asynPrint(m_pInterface, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIHexapodController::moveCts() failed, GCS error %d\n", errorCode);
|
||||
return asynError;
|
||||
}
|
||||
}
|
||||
m_bAnyAxisMoving = true;
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
asynStatus PIHexapodController::getAxisPosition(PIasynAxis* pAxis, double& position)
|
||||
{
|
||||
if (!m_bAnyAxisMoving)
|
||||
{
|
||||
return PIGCSController::getAxisPosition(pAxis, position);
|
||||
}
|
||||
if (m_bCanReadPosWithChar3)
|
||||
{
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(3), buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
char *pStart = buf;
|
||||
for(;;)
|
||||
{
|
||||
bool bEnd = false;
|
||||
while(*pStart == ' ') pStart++;
|
||||
char *pLF = strstr(pStart, "\n");
|
||||
if (pLF != NULL)
|
||||
{
|
||||
*pLF = '\0';
|
||||
}
|
||||
else
|
||||
{
|
||||
bEnd = true;
|
||||
}
|
||||
// single line will look like "X = 1.0 \n"
|
||||
if (pStart[0] == pAxis->m_szAxisName[0] )
|
||||
{
|
||||
double value;
|
||||
if (!getValue(pStart, value))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
position = value;
|
||||
return status;
|
||||
}
|
||||
if (bEnd)
|
||||
{
|
||||
break;
|
||||
}
|
||||
pStart = pLF + 1;
|
||||
if (*pStart == '\0')
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
// should not come here!
|
||||
return asynError;
|
||||
}
|
||||
// return last position
|
||||
position = double(pAxis->m_positionCts) * double(pAxis->m_CPUdenominator) / double(pAxis->m_CPUnumerator);
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::SetPivotX(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIHexapodController::SetPivotX() value %f", value);
|
||||
}
|
||||
asynStatus status = SetPivot('R', value);
|
||||
if (status== asynSuccess)
|
||||
{
|
||||
m_PivotX = value;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::SetPivotY(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIHexapodController::SetPivotY() value %f", value);
|
||||
}
|
||||
asynStatus status = SetPivot('S', value);
|
||||
if (status== asynSuccess)
|
||||
{
|
||||
m_PivotY = value;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::SetPivotZ(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIHexapodController::SetPivotZ() value %f", value);
|
||||
}
|
||||
asynStatus status = SetPivot('T', value);
|
||||
if (status== asynSuccess)
|
||||
{
|
||||
m_PivotZ = value;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::SetPivot(char cAxis, double value)
|
||||
{
|
||||
if (m_bAnyAxisMoving)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIHexapodController::SetPivot() cannot change pivot point while platform is moving");
|
||||
}
|
||||
return asynError;
|
||||
}
|
||||
|
||||
asynStatus status;
|
||||
char cmd[100];
|
||||
sprintf(cmd,"SPI %c %f", cAxis, value);
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode != 0)
|
||||
{
|
||||
asynPrint(m_pInterface, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIHexapodController::SetPivot() failed, GCS error %d\n", errorCode);
|
||||
return asynError;
|
||||
}
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::ReadPivotSettings()
|
||||
{
|
||||
char buf[100];
|
||||
char cmd[] = "SPI? RST";
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
char* pStart = buf;
|
||||
bool bEnd = false;
|
||||
double px = 0.0;
|
||||
double py = 0.0;
|
||||
double pz = 0.0;
|
||||
for(;;)
|
||||
{
|
||||
while(*pStart == ' ') pStart++;
|
||||
char *pLF = strstr(pStart, "\n");
|
||||
if (pLF != NULL)
|
||||
{
|
||||
*pLF = '\0';
|
||||
}
|
||||
else
|
||||
{
|
||||
bEnd = true;
|
||||
}
|
||||
// single line will look like "R = 1.0 \n"
|
||||
double value;
|
||||
if (!getValue(pStart, value))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
switch(pStart[0])
|
||||
{
|
||||
case'R': px = value; break;
|
||||
case'S': py = value; break;
|
||||
case'T': pz = value; break;
|
||||
}
|
||||
if (bEnd)
|
||||
{
|
||||
break;
|
||||
}
|
||||
pStart = pLF + 1;
|
||||
if (*pStart == '\0')
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_PivotX = px;
|
||||
m_PivotY = py;
|
||||
m_PivotZ = pz;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
|
||||
{
|
||||
asynStatus status = sendOnly("INI X");
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
int errorCode = getGCSError();
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIHexapodController::referenceVelCts() failed\n");
|
||||
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
|
||||
"PIHexapodController::referenceVelCts() failed - GCS Error %d\n",errorCode);
|
||||
return status;
|
||||
}
|
||||
m_bHoming = true;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::haltAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
asynStatus status = sendOnly(char(24));
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
//epicsThreadSleep(0.1); // TODO test value
|
||||
int err = getGCSError();
|
||||
// controller will set error code to PI_CNTR_STOP (10)
|
||||
if (err != PI_CNTR_STOP)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIGCSController::haltAxis() failed, GCS error %d", err);
|
||||
return asynError;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* PIE755Controller
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#include "PIHexapodController.h"
|
||||
#include "PIasynAxis.h"
|
||||
#include <stdlib.h>
|
||||
#include <epicsThread.h>
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
asynStatus PIHexapodController::init(void)
|
||||
{
|
||||
asynStatus status = PIGCSController::init();
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
// Try to find out if #4 is supported
|
||||
char buf[200];
|
||||
status = sendAndReceive(char(4), buf, 199);
|
||||
if (status == asynSuccess)
|
||||
{
|
||||
m_bCanReadStatusWithChar4 = true;
|
||||
}
|
||||
else if (status == asynTimeout)
|
||||
{
|
||||
m_bCanReadStatusWithChar4 = false;
|
||||
getGCSError(); // clear error UNKNOWN COMMAND
|
||||
}
|
||||
// status = sendAndReceive(char(3), buf, 199);
|
||||
// if (status == asynSuccess)
|
||||
// {
|
||||
// m_bCanReadPosWithChar3 = true;
|
||||
// }
|
||||
// else if (status == asynTimeout)
|
||||
{
|
||||
m_bCanReadPosWithChar3 = false;
|
||||
getGCSError(); // clear error UNKNOWN COMMAND
|
||||
}
|
||||
|
||||
m_bHoming = false;
|
||||
|
||||
ReadPivotSettings();
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
asynStatus PIHexapodController::getGlobalState(asynMotorAxis** pAxes, int numAxes)
|
||||
{
|
||||
// do not call moving here, at least simulation software does return
|
||||
// values != 0 with bit 1 not set (e.g. "2")
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(5), buf, 99);;
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
printf("PIGCSController::getGlobalState() failed, status %d", status);
|
||||
return status;
|
||||
}
|
||||
|
||||
char* pStr;
|
||||
long moving = strtol(buf, &pStr, 16);
|
||||
m_bAnyAxisMoving = (moving != 0);
|
||||
if (m_bHoming && (moving == 0))
|
||||
{
|
||||
m_bHoming = false;
|
||||
}
|
||||
for(int i=0; i<numAxes; i++)
|
||||
{
|
||||
((PIasynAxis*)pAxes[i])->m_bMoving = (moving != 0);
|
||||
}
|
||||
return asynSuccess;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl)
|
||||
{
|
||||
negLimit = 0;
|
||||
posLimit = 0;
|
||||
homing = m_bHoming ? 1:0;
|
||||
|
||||
moving = pAxis->m_bMoving || pAxis->deferred_move;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::findConnectedAxes()
|
||||
{
|
||||
for (size_t i=0; i<MAX_NR_AXES; i++)
|
||||
{
|
||||
m_axesIDs[i] = NULL;
|
||||
}
|
||||
sprintf(m_allAxesIDs, "X\nY\nZ\nU\nV\nW");
|
||||
char* szAxis = strtok(m_allAxesIDs, "\n");
|
||||
while (szAxis != NULL)
|
||||
{
|
||||
int i=strlen(szAxis)-1;
|
||||
while (szAxis[i] == ' ')
|
||||
{
|
||||
szAxis[i] = '\0';
|
||||
i--;
|
||||
}
|
||||
if (MAX_NR_AXES <= m_nrFoundAxes)
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
m_axesIDs[m_nrFoundAxes] = szAxis;
|
||||
m_nrFoundAxes++;
|
||||
szAxis = strtok(NULL, "\n");
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::initAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
pAxis->m_movingStateMask = 1;
|
||||
return setServo(pAxis, 1);
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::getReferencedState(PIasynAxis* pAxis)
|
||||
{
|
||||
if (!m_bCanReadStatusWithChar4)
|
||||
{
|
||||
pAxis->m_homed = 1;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(4), buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
long mask = strtol(buf, NULL, 10);
|
||||
pAxis->m_homed = (mask & 0x10000) ? 1 : 0;
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::moveCts( PIasynAxis* pAxis, int targetCts )
|
||||
{
|
||||
// printf("PIHexapodController::moveCts(,%d)...\n",targetCts);
|
||||
asynStatus status;
|
||||
char cmd[100];
|
||||
double target = double(targetCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
sprintf(cmd,"MOV %s %f", pAxis->m_szAxisName, target);
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
epicsThreadSleep( 0.2 ); // TODO test if we do need this
|
||||
asynMotorAxis* pAsynAxis = (asynMotorAxis*)pAxis;
|
||||
status = getGlobalState(&pAsynAxis, 1);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
// printf("PIHexapodController::moveCts(,%d) - getGlobalState() failed, status=%d\n", targetCts, status);
|
||||
return status;
|
||||
}
|
||||
if (!pAxis->m_bMoving)
|
||||
{
|
||||
int errorCode = getGCSError();
|
||||
// printf("PIHexapodController::moveCts(,%d) - not moving after MOV() gcserror=%d\n",targetCts, errorCode);
|
||||
if (errorCode != 0)
|
||||
{
|
||||
asynPrint(m_pInterface, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIHexapodController::moveCts() failed, GCS error %d\n", errorCode);
|
||||
return asynError;
|
||||
}
|
||||
}
|
||||
m_bAnyAxisMoving = true;
|
||||
pAxis->m_lastDirection = (targetCts > pAxis->m_positionCts) ? 1 : 0;
|
||||
printf("PIHexapodController::moveCts(,%d) - OK!\n",targetCts);
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes)
|
||||
{
|
||||
asynStatus status;
|
||||
char cmd[1000] = "MOV";
|
||||
char subCmd[100];
|
||||
for (int axis = 0; axis <numAxes; axis++)
|
||||
{
|
||||
PIasynAxis* pAxis = pAxesArray[axis];
|
||||
double target = double(pTargetCtsArray[axis]) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
|
||||
sprintf(subCmd," %s %f", pAxis->m_szAxisName, target);
|
||||
strcat(cmd, subCmd);
|
||||
pAxis->m_lastDirection = (pTargetCtsArray[axis] > pAxis->m_positionCts) ? 1 : 0;
|
||||
}
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
epicsThreadSleep(0.2);
|
||||
status = getGlobalState((asynMotorAxis**)pAxesArray, numAxes);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
if (!pAxesArray[0]->m_bMoving)
|
||||
{
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode != 0)
|
||||
{
|
||||
asynPrint(m_pInterface, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIHexapodController::moveCts() failed, GCS error %d\n", errorCode);
|
||||
return asynError;
|
||||
}
|
||||
}
|
||||
m_bAnyAxisMoving = true;
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
asynStatus PIHexapodController::getAxisPosition(PIasynAxis* pAxis, double& position)
|
||||
{
|
||||
if (!m_bAnyAxisMoving)
|
||||
{
|
||||
return PIGCSController::getAxisPosition(pAxis, position);
|
||||
}
|
||||
if (m_bCanReadPosWithChar3)
|
||||
{
|
||||
char buf[255];
|
||||
asynStatus status = sendAndReceive(char(3), buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
char *pStart = buf;
|
||||
for(;;)
|
||||
{
|
||||
bool bEnd = false;
|
||||
while(*pStart == ' ') pStart++;
|
||||
char *pLF = strstr(pStart, "\n");
|
||||
if (pLF != NULL)
|
||||
{
|
||||
*pLF = '\0';
|
||||
}
|
||||
else
|
||||
{
|
||||
bEnd = true;
|
||||
}
|
||||
// single line will look like "X = 1.0 \n"
|
||||
if (pStart[0] == pAxis->m_szAxisName[0] )
|
||||
{
|
||||
double value;
|
||||
if (!getValue(pStart, value))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
position = value;
|
||||
return status;
|
||||
}
|
||||
if (bEnd)
|
||||
{
|
||||
break;
|
||||
}
|
||||
pStart = pLF + 1;
|
||||
if (*pStart == '\0')
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
// should not come here!
|
||||
return asynError;
|
||||
}
|
||||
// return last position
|
||||
position = double(pAxis->m_positionCts) * double(pAxis->m_CPUdenominator) / double(pAxis->m_CPUnumerator);
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::SetPivotX(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIHexapodController::SetPivotX() value %f", value);
|
||||
}
|
||||
asynStatus status = SetPivot('R', value);
|
||||
if (status== asynSuccess)
|
||||
{
|
||||
m_PivotX = value;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::SetPivotY(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIHexapodController::SetPivotY() value %f", value);
|
||||
}
|
||||
asynStatus status = SetPivot('S', value);
|
||||
if (status== asynSuccess)
|
||||
{
|
||||
m_PivotY = value;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::SetPivotZ(double value)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIHexapodController::SetPivotZ() value %f", value);
|
||||
}
|
||||
asynStatus status = SetPivot('T', value);
|
||||
if (status== asynSuccess)
|
||||
{
|
||||
m_PivotZ = value;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::SetPivot(char cAxis, double value)
|
||||
{
|
||||
if (m_bAnyAxisMoving)
|
||||
{
|
||||
if (NULL != m_pCurrentLogSink)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW,
|
||||
"PIHexapodController::SetPivot() cannot change pivot point while platform is moving");
|
||||
}
|
||||
return asynError;
|
||||
}
|
||||
|
||||
asynStatus status;
|
||||
char cmd[100];
|
||||
sprintf(cmd,"SPI %c %f", cAxis, value);
|
||||
status = sendOnly(cmd);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
|
||||
int errorCode = getGCSError();
|
||||
if (errorCode != 0)
|
||||
{
|
||||
asynPrint(m_pInterface, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIHexapodController::SetPivot() failed, GCS error %d\n", errorCode);
|
||||
return asynError;
|
||||
}
|
||||
return status;
|
||||
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::ReadPivotSettings()
|
||||
{
|
||||
char buf[100];
|
||||
char cmd[] = "SPI? RST";
|
||||
asynStatus status = sendAndReceive(cmd, buf, 99);
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
char* pStart = buf;
|
||||
bool bEnd = false;
|
||||
double px = 0.0;
|
||||
double py = 0.0;
|
||||
double pz = 0.0;
|
||||
for(;;)
|
||||
{
|
||||
while(*pStart == ' ') pStart++;
|
||||
char *pLF = strstr(pStart, "\n");
|
||||
if (pLF != NULL)
|
||||
{
|
||||
*pLF = '\0';
|
||||
}
|
||||
else
|
||||
{
|
||||
bEnd = true;
|
||||
}
|
||||
// single line will look like "R = 1.0 \n"
|
||||
double value;
|
||||
if (!getValue(pStart, value))
|
||||
{
|
||||
return asynError;
|
||||
}
|
||||
switch(pStart[0])
|
||||
{
|
||||
case'R': px = value; break;
|
||||
case'S': py = value; break;
|
||||
case'T': pz = value; break;
|
||||
}
|
||||
if (bEnd)
|
||||
{
|
||||
break;
|
||||
}
|
||||
pStart = pLF + 1;
|
||||
if (*pStart == '\0')
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_PivotX = px;
|
||||
m_PivotY = py;
|
||||
m_PivotZ = pz;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards)
|
||||
{
|
||||
asynStatus status = sendOnly("INI X");
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
int errorCode = getGCSError();
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_ERROR,
|
||||
"PIHexapodController::referenceVelCts() failed\n");
|
||||
epicsSnprintf(pAxis->m_pasynUser->errorMessage,pAxis->m_pasynUser->errorMessageSize,
|
||||
"PIHexapodController::referenceVelCts() failed - GCS Error %d\n",errorCode);
|
||||
return status;
|
||||
}
|
||||
m_bHoming = true;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIHexapodController::haltAxis(PIasynAxis* pAxis)
|
||||
{
|
||||
asynStatus status = sendOnly(char(24));
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
//epicsThreadSleep(0.1); // TODO test value
|
||||
int err = getGCSError();
|
||||
// controller will set error code to PI_CNTR_STOP (10)
|
||||
if (err != PI_CNTR_STOP)
|
||||
{
|
||||
asynPrint(m_pCurrentLogSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIGCSController::haltAxis() failed, GCS error %d", err);
|
||||
return asynError;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,74 +1,81 @@
|
||||
/*
|
||||
* PIE755Controller.h
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
#ifndef PIHEXAPODCONTROLLER_H_
|
||||
#define PIHEXAPODCONTROLLER_H_
|
||||
|
||||
#include "PIGCSController.h"
|
||||
|
||||
/**
|
||||
* class representing PI Hexapods.
|
||||
*/
|
||||
class PIHexapodController : public PIGCSController
|
||||
{
|
||||
public:
|
||||
PIHexapodController(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIHexapodController() {}
|
||||
|
||||
virtual asynStatus init(void);
|
||||
virtual asynStatus initAxis(PIasynAxis* pAxis);
|
||||
|
||||
virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
virtual asynStatus getGlobalState(asynMotorAxis** Axes, int numAxes);
|
||||
virtual asynStatus getTravelLimits(PIasynAxis* pAxis, double& negLimit, double& posLimit)
|
||||
{
|
||||
negLimit = -100;
|
||||
posLimit = 100;
|
||||
return asynSuccess;
|
||||
}
|
||||
virtual asynStatus getReferencedState(PIasynAxis* pAxis);
|
||||
|
||||
virtual asynStatus haltAxis(PIasynAxis* pAxis);
|
||||
|
||||
virtual bool AcceptsNewTarget() { return !m_bAnyAxisMoving; }
|
||||
virtual bool CanCommunicateWhileHoming() { return false; }
|
||||
|
||||
virtual asynStatus moveCts( PIasynAxis* pAxis, int target);
|
||||
virtual asynStatus moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes);
|
||||
virtual asynStatus getAxisPosition(PIasynAxis* pAxis, double& position);
|
||||
|
||||
virtual asynStatus SetPivotX(double value);
|
||||
virtual asynStatus SetPivotY(double value);
|
||||
virtual asynStatus SetPivotZ(double value);
|
||||
|
||||
virtual double GetPivotX() { return m_PivotX; }
|
||||
virtual double GetPivotY() { return m_PivotY; }
|
||||
virtual double GetPivotZ() { return m_PivotZ; }
|
||||
|
||||
virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards);
|
||||
|
||||
protected:
|
||||
virtual asynStatus findConnectedAxes();
|
||||
asynStatus ReadPivotSettings();
|
||||
asynStatus SetPivot(char cAxis, double value);
|
||||
|
||||
double m_PivotX;
|
||||
double m_PivotY;
|
||||
double m_PivotZ;
|
||||
|
||||
bool m_bHoming;
|
||||
|
||||
private:
|
||||
bool m_bCanReadStatusWithChar4;
|
||||
bool m_bCanReadPosWithChar3;
|
||||
};
|
||||
|
||||
#endif /* PIHEXAPODCONTROLLER_H_ */
|
||||
/*
|
||||
* PIE755Controller.h
|
||||
*
|
||||
* Author: sra
|
||||
*/
|
||||
|
||||
/*
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
*/
|
||||
|
||||
#ifndef PIHEXAPODCONTROLLER_H_
|
||||
#define PIHEXAPODCONTROLLER_H_
|
||||
|
||||
#include "PIGCSController.h"
|
||||
|
||||
/**
|
||||
* class representing PI Hexapods.
|
||||
*/
|
||||
class PIHexapodController : public PIGCSController
|
||||
{
|
||||
public:
|
||||
PIHexapodController(asynUser* pCom, const char* szIDN)
|
||||
: PIGCSController(pCom, szIDN)
|
||||
{
|
||||
}
|
||||
~PIHexapodController() {}
|
||||
|
||||
virtual asynStatus init(void);
|
||||
virtual asynStatus initAxis(PIasynAxis* pAxis);
|
||||
|
||||
virtual asynStatus setAccelerationCts( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus setAcceleration( PIasynAxis* pAxis, double acceleration) { return asynSuccess; }
|
||||
virtual asynStatus getStatus(PIasynAxis* pAxis, int& homing, int& moving, int& negLimit, int& posLimit, int& servoControl);
|
||||
virtual asynStatus getGlobalState(asynMotorAxis** Axes, int numAxes);
|
||||
virtual asynStatus getTravelLimits(PIasynAxis* pAxis, double& negLimit, double& posLimit)
|
||||
{
|
||||
negLimit = -100;
|
||||
posLimit = 100;
|
||||
return asynSuccess;
|
||||
}
|
||||
virtual asynStatus getReferencedState(PIasynAxis* pAxis);
|
||||
|
||||
virtual asynStatus haltAxis(PIasynAxis* pAxis);
|
||||
|
||||
virtual bool AcceptsNewTarget() { return !m_bAnyAxisMoving; }
|
||||
virtual bool CanCommunicateWhileHoming() { return false; }
|
||||
|
||||
virtual asynStatus moveCts( PIasynAxis* pAxis, int target);
|
||||
virtual asynStatus moveCts( PIasynAxis** pAxesArray, int* pTargetCtsArray, int numAxes);
|
||||
virtual asynStatus getAxisPosition(PIasynAxis* pAxis, double& position);
|
||||
|
||||
virtual asynStatus SetPivotX(double value);
|
||||
virtual asynStatus SetPivotY(double value);
|
||||
virtual asynStatus SetPivotZ(double value);
|
||||
|
||||
virtual double GetPivotX() { return m_PivotX; }
|
||||
virtual double GetPivotY() { return m_PivotY; }
|
||||
virtual double GetPivotZ() { return m_PivotZ; }
|
||||
|
||||
virtual asynStatus referenceVelCts( PIasynAxis* pAxis, double velocity, int forwards);
|
||||
|
||||
protected:
|
||||
virtual asynStatus findConnectedAxes();
|
||||
asynStatus ReadPivotSettings();
|
||||
asynStatus SetPivot(char cAxis, double value);
|
||||
|
||||
double m_PivotX;
|
||||
double m_PivotY;
|
||||
double m_PivotZ;
|
||||
|
||||
bool m_bHoming;
|
||||
|
||||
private:
|
||||
bool m_bCanReadStatusWithChar4;
|
||||
bool m_bCanReadPosWithChar3;
|
||||
};
|
||||
|
||||
#endif /* PIHEXAPODCONTROLLER_H_ */
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
include "asyn.dbd"
|
||||
include "motorRecord.dbd"
|
||||
##device(motor,VME_IO,devPIasyn,"PI Motor GCS2")
|
||||
##driver(PIasyn)
|
||||
registrar(PIasynDriverRegister)
|
||||
##registrar(PIasynRegister)
|
||||
include "asyn.dbd"
|
||||
include "motorRecord.dbd"
|
||||
##device(motor,VME_IO,devPIasyn,"PI Motor GCS2")
|
||||
##driver(PIasyn)
|
||||
registrar(PIasynDriverRegister)
|
||||
##registrar(PIasynRegister)
|
||||
|
||||
+311
-306
@@ -1,306 +1,311 @@
|
||||
/*
|
||||
FILENAME... PIasynController.cpp
|
||||
USAGE... Simulated Motor Support.
|
||||
|
||||
Based on drvMotorSim.c
|
||||
|
||||
Mark Rivers
|
||||
December 13, 2009
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <epicsTime.h>
|
||||
#include <epicsThread.h>
|
||||
#include <epicsString.h>
|
||||
#include <epicsMutex.h>
|
||||
#include <ellLib.h>
|
||||
#include <iocsh.h>
|
||||
#include <epicsExport.h>
|
||||
#include <motor_interface.h>
|
||||
|
||||
#include "PIasynAxis.h"
|
||||
#include "PIasynController.h"
|
||||
#include "PIGCSController.h"
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
static const char *driverName = "PIasynAxis";
|
||||
|
||||
PIasynAxis::PIasynAxis(PIasynController *pController, PIGCSController* pGCSController, int axis, const char* szName )
|
||||
: asynMotorAxis((asynMotorController*)pController, axis)
|
||||
, pController_(pController)
|
||||
, m_szAxisName(NULL)
|
||||
, m_isHoming(0)
|
||||
, m_homed(0)
|
||||
, m_acceleration(0.0)
|
||||
, m_maxAcceleration(-1.0)
|
||||
, m_lastDirection(0)
|
||||
, m_CPUnumerator(1000)
|
||||
, m_CPUdenominator(1)
|
||||
, m_pasynUser(NULL)
|
||||
, m_bHasLimitSwitches(false)
|
||||
, m_bHasReference(false)
|
||||
, m_bProblem(false)
|
||||
, m_bServoControl(false)
|
||||
, m_bMoving(false)
|
||||
, m_pGCSController(pGCSController)
|
||||
{
|
||||
if (szName != NULL)
|
||||
{
|
||||
m_szAxisName = new char[strlen(szName)+1];
|
||||
strcpy(m_szAxisName, szName);
|
||||
}
|
||||
|
||||
printf("PIasynAxis::PIasynAxis() %d: %s\n",
|
||||
axis, m_szAxisName);
|
||||
}
|
||||
|
||||
|
||||
void PIasynAxis::Init(const char *portName)
|
||||
{
|
||||
asynUser* logSink = pasynManager->createAsynUser(0,0);
|
||||
asynStatus status = pasynManager->connectDevice(logSink, portName, getAxisNo());
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
asynPrint(logSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIasynController::configAxis() - connectDevice() failed\n");
|
||||
return;
|
||||
}
|
||||
m_pGCSController->m_pCurrentLogSink = logSink;
|
||||
|
||||
setIntegerParam(motorAxisHasClosedLoop, 1);
|
||||
|
||||
m_pGCSController->initAxis(this);
|
||||
double resolution;
|
||||
m_pGCSController->getResolution(this, resolution);
|
||||
m_pGCSController->getAxisVelocity(this);
|
||||
m_pGCSController->getAxisPositionCts(this);
|
||||
setDoubleParam(pController_->motorPosition_, m_positionCts);
|
||||
setDoubleParam(pController_->motorMoveAbs_, m_positionCts);
|
||||
m_pGCSController->getTravelLimits(this, negLimit_, posLimit_);
|
||||
setDoubleParam(pController_->motorLowLimit_, negLimit_);
|
||||
setDoubleParam(pController_->motorHighLimit_, posLimit_);
|
||||
m_pGCSController->getReferencedState(this);
|
||||
setIntegerParam( pController_->motorStatusHomed_, m_homed );
|
||||
callParamCallbacks();
|
||||
|
||||
pasynManager->freeAsynUser(logSink);
|
||||
|
||||
}
|
||||
|
||||
PIasynAxis::~PIasynAxis()
|
||||
{
|
||||
if (m_szAxisName != NULL)
|
||||
{
|
||||
delete [] m_szAxisName;
|
||||
}
|
||||
}
|
||||
|
||||
asynStatus PIasynAxis::poll(bool *returnMoving)
|
||||
{
|
||||
int done = 0;
|
||||
|
||||
int moving, negLimit, posLimit, servoControl;
|
||||
int oldHoming = m_isHoming;
|
||||
m_pGCSController->getStatus(this, m_isHoming, moving, negLimit, posLimit, servoControl);
|
||||
if (moving == 0 && m_isHoming == 0)
|
||||
done = 1;
|
||||
|
||||
m_bMoving = (done!=1);
|
||||
if (!m_isHoming || m_pGCSController->CanCommunicateWhileHoming())
|
||||
{
|
||||
if (oldHoming && oldHoming != m_isHoming)
|
||||
{
|
||||
m_pGCSController->getReferencedState(this);
|
||||
asynPrint(pasynUser_, ASYN_TRACE_ERROR, //FIXME: ASYN_TRACE_FLOW,
|
||||
"PIasynAxis::poll() axis %d referencing state changed, homed = %d\n",
|
||||
axisNo_, m_homed );
|
||||
}
|
||||
if (m_bServoControl && servoControl == 0) // servo changed without user interaction!
|
||||
{
|
||||
m_bProblem = true;
|
||||
}
|
||||
if (!m_isHoming || m_pGCSController->IsGCS2())
|
||||
{
|
||||
m_bServoControl = (servoControl == 1);
|
||||
m_pGCSController->getAxisPositionCts(this);
|
||||
double realPosition;
|
||||
m_pGCSController->getAxisPosition(this, realPosition);
|
||||
setDoubleParam(pController_->PI_SUP_POSITION, realPosition );
|
||||
}
|
||||
}
|
||||
if (m_isHoming)
|
||||
{
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"PIasynAxis::poll() axis %d referencing ...\n", axisNo_ );
|
||||
}
|
||||
setDoubleParam(pController_->motorPosition_, m_positionCts );
|
||||
setDoubleParam(pController_->motorEncoderPosition_, m_positionCts);
|
||||
setIntegerParam(pController_->motorStatusDirection_, m_lastDirection);
|
||||
setIntegerParam(pController_->motorStatusDone_, done );
|
||||
setIntegerParam(pController_->motorStatusHighLimit_, posLimit);
|
||||
setIntegerParam(pController_->motorStatusHomed_, m_homed );
|
||||
setIntegerParam(pController_->motorStatusMoving_, !done );
|
||||
setIntegerParam(pController_->motorStatusLowLimit_, negLimit);
|
||||
setIntegerParam(pController_->motorStatusGainSupport_, true);
|
||||
setIntegerParam(pController_->motorStatusProblem_, m_bProblem);
|
||||
setIntegerParam(pController_->motorStatusPowerOn_, m_bServoControl);
|
||||
setIntegerParam(pController_->PI_SUP_SERVO, m_bServoControl );
|
||||
|
||||
callParamCallbacks();
|
||||
|
||||
*returnMoving = m_bMoving;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIasynAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
||||
{
|
||||
m_pGCSController->m_pCurrentLogSink = pasynUser_;
|
||||
asynStatus status = asynError;
|
||||
static const char *functionName = "moveAxis";
|
||||
|
||||
if (!m_pGCSController->AcceptsNewTarget())
|
||||
{
|
||||
asynPrint(pasynUser_, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set port %s, axis %d - controller does not accept new target (busy?)\n",
|
||||
driverName, functionName, pC_->portName, axisNo_ );
|
||||
printf("%s:%s: Set port %s, axis %d - controller does not accept new target (busy?)\n",
|
||||
driverName, functionName, pC_->portName, axisNo_ );
|
||||
return status;
|
||||
}
|
||||
|
||||
if (relative)
|
||||
{
|
||||
//TODO: MVR oder letztes target!
|
||||
//TODO: when is this used?
|
||||
}
|
||||
|
||||
if (pController_->movesDeferred != 0)
|
||||
{ /*Deferred moves.*/
|
||||
deferred_position = position;
|
||||
deferred_move = 1;
|
||||
deferred_relative = relative;
|
||||
setIntegerParam(pController_->motorStatusDone_, 0);
|
||||
callParamCallbacks();
|
||||
return asynSuccess;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (maxVelocity != 0)
|
||||
{
|
||||
status = m_pGCSController->setVelocityCts(this, maxVelocity);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if (acceleration != 0)
|
||||
{
|
||||
status = m_pGCSController->setAccelerationCts(this, acceleration);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
setIntegerParam(pController_->motorStatusDone_, 0);
|
||||
callParamCallbacks();
|
||||
|
||||
status = m_pGCSController->moveCts(this, position);
|
||||
}
|
||||
epicsEventSignal(pController_->pollEventId_);
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set driver %s, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f, deffered=%d - status=%d\n",
|
||||
driverName, functionName, pC_->portName, axisNo_, position, minVelocity, maxVelocity, acceleration, pController_->movesDeferred, int(status) );
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIasynAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
|
||||
{
|
||||
m_pGCSController->m_pCurrentLogSink = pasynUser_;
|
||||
asynStatus status = asynError;
|
||||
static const char *functionName = "moveVelocityAxis";
|
||||
|
||||
if (!m_pGCSController->AcceptsNewTarget())
|
||||
{
|
||||
asynPrint(pasynUser_, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set port %s, axis %d - controller does not accept new target (busy?)",
|
||||
driverName, functionName, pController_->portName, axisNo_ );
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
setIntegerParam(pController_->motorStatusDone_, 0);
|
||||
callParamCallbacks();
|
||||
|
||||
|
||||
double target = maxVelocity > 0 ? posLimit_ : negLimit_;
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set port %s, axis %d move with velocity of %f, accel=%f / target %f - BEFORE MOV\n",
|
||||
driverName, functionName, pController_->portName, axisNo_, maxVelocity, acceleration, target );
|
||||
|
||||
m_pGCSController->setVelocityCts(this, maxVelocity);
|
||||
m_pGCSController->move(this, target);
|
||||
|
||||
epicsEventSignal(pController_->pollEventId_);
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set port %s, axis %d move with velocity of %f, accel=%f / target %f - AFTER MOV\n",
|
||||
driverName, functionName, pController_->portName, axisNo_, maxVelocity, acceleration, target );
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIasynAxis::stop(double acceleration)
|
||||
{
|
||||
m_pGCSController->m_pCurrentLogSink = pasynUser_;
|
||||
static const char *functionName = "stopAxis";
|
||||
|
||||
deferred_move = 0;
|
||||
|
||||
m_pGCSController->haltAxis(this);
|
||||
|
||||
epicsEventSignal(pController_->pollEventId_);
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set axis %d to stop with accel=%f",
|
||||
driverName, functionName, axisNo_, acceleration );
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
|
||||
asynStatus PIasynAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
|
||||
{
|
||||
m_pGCSController->m_pCurrentLogSink = pasynUser_;
|
||||
asynStatus status = asynError;
|
||||
static const char *functionName = "homeAxis";
|
||||
|
||||
m_isHoming = 1;
|
||||
setIntegerParam(pController_->motorStatusDone_, 0 );
|
||||
callParamCallbacks();
|
||||
|
||||
status = m_pGCSController->referenceVelCts(this, maxVelocity, forwards);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
setIntegerParam(pController_->motorStatusHomed_, m_homed );
|
||||
epicsEventSignal(pController_->pollEventId_);
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set driver %s, axis %d to home %s, min vel=%f, max_vel=%f, accel=%f",
|
||||
driverName, functionName, pController_->portName, axisNo_, (forwards?"FORWARDS":"REVERSE"), minVelocity, maxVelocity, acceleration );
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
FILENAME... PIasynController.cpp
|
||||
USAGE... Simulated Motor Support.
|
||||
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
|
||||
Based on drvMotorSim.c
|
||||
|
||||
Mark Rivers
|
||||
December 13, 2009
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <epicsTime.h>
|
||||
#include <epicsThread.h>
|
||||
#include <epicsString.h>
|
||||
#include <epicsMutex.h>
|
||||
#include <ellLib.h>
|
||||
#include <iocsh.h>
|
||||
#include <epicsExport.h>
|
||||
#include <motor_interface.h>
|
||||
|
||||
#include "PIasynAxis.h"
|
||||
#include "PIasynController.h"
|
||||
#include "PIGCSController.h"
|
||||
|
||||
//#undef asynPrint
|
||||
//#define asynPrint(user,reason,format...) 0
|
||||
|
||||
static const char *driverName = "PIasynAxis";
|
||||
|
||||
PIasynAxis::PIasynAxis(PIasynController *pController, PIGCSController* pGCSController, int axis, const char* szName )
|
||||
: asynMotorAxis((asynMotorController*)pController, axis)
|
||||
, pController_(pController)
|
||||
, m_szAxisName(NULL)
|
||||
, m_isHoming(0)
|
||||
, m_homed(0)
|
||||
, m_acceleration(0.0)
|
||||
, m_maxAcceleration(-1.0)
|
||||
, m_lastDirection(0)
|
||||
, m_CPUnumerator(1000)
|
||||
, m_CPUdenominator(1)
|
||||
, m_pasynUser(NULL)
|
||||
, m_bHasLimitSwitches(false)
|
||||
, m_bHasReference(false)
|
||||
, m_bProblem(false)
|
||||
, m_bServoControl(false)
|
||||
, m_bMoving(false)
|
||||
, m_pGCSController(pGCSController)
|
||||
{
|
||||
if (szName != NULL)
|
||||
{
|
||||
m_szAxisName = new char[strlen(szName)+1];
|
||||
strcpy(m_szAxisName, szName);
|
||||
}
|
||||
|
||||
printf("PIasynAxis::PIasynAxis() %d: %s\n",
|
||||
axis, m_szAxisName);
|
||||
}
|
||||
|
||||
|
||||
void PIasynAxis::Init(const char *portName)
|
||||
{
|
||||
asynUser* logSink = pasynManager->createAsynUser(0,0);
|
||||
asynStatus status = pasynManager->connectDevice(logSink, portName, getAxisNo());
|
||||
if (status != asynSuccess)
|
||||
{
|
||||
asynPrint(logSink, ASYN_TRACE_FLOW|ASYN_TRACE_ERROR,
|
||||
"PIasynController::configAxis() - connectDevice() failed\n");
|
||||
return;
|
||||
}
|
||||
m_pGCSController->m_pCurrentLogSink = logSink;
|
||||
|
||||
setIntegerParam(motorAxisHasClosedLoop, 1);
|
||||
|
||||
m_pGCSController->initAxis(this);
|
||||
double resolution;
|
||||
m_pGCSController->getResolution(this, resolution);
|
||||
m_pGCSController->getAxisVelocity(this);
|
||||
m_pGCSController->getAxisPositionCts(this);
|
||||
setDoubleParam(pController_->motorPosition_, m_positionCts);
|
||||
setDoubleParam(pController_->motorMoveAbs_, m_positionCts);
|
||||
m_pGCSController->getTravelLimits(this, negLimit_, posLimit_);
|
||||
setDoubleParam(pController_->motorLowLimit_, negLimit_);
|
||||
setDoubleParam(pController_->motorHighLimit_, posLimit_);
|
||||
m_pGCSController->getReferencedState(this);
|
||||
setIntegerParam( pController_->motorStatusHomed_, m_homed );
|
||||
callParamCallbacks();
|
||||
|
||||
pasynManager->freeAsynUser(logSink);
|
||||
|
||||
}
|
||||
|
||||
PIasynAxis::~PIasynAxis()
|
||||
{
|
||||
if (m_szAxisName != NULL)
|
||||
{
|
||||
delete [] m_szAxisName;
|
||||
}
|
||||
}
|
||||
|
||||
asynStatus PIasynAxis::poll(bool *returnMoving)
|
||||
{
|
||||
int done = 0;
|
||||
|
||||
int moving, negLimit, posLimit, servoControl;
|
||||
int oldHoming = m_isHoming;
|
||||
m_pGCSController->getStatus(this, m_isHoming, moving, negLimit, posLimit, servoControl);
|
||||
if (moving == 0 && m_isHoming == 0)
|
||||
done = 1;
|
||||
|
||||
m_bMoving = (done!=1);
|
||||
if (!m_isHoming || m_pGCSController->CanCommunicateWhileHoming())
|
||||
{
|
||||
if (oldHoming && oldHoming != m_isHoming)
|
||||
{
|
||||
m_pGCSController->getReferencedState(this);
|
||||
asynPrint(pasynUser_, ASYN_TRACE_ERROR, //FIXME: ASYN_TRACE_FLOW,
|
||||
"PIasynAxis::poll() axis %d referencing state changed, homed = %d\n",
|
||||
axisNo_, m_homed );
|
||||
}
|
||||
if (m_bServoControl && servoControl == 0) // servo changed without user interaction!
|
||||
{
|
||||
m_bProblem = true;
|
||||
}
|
||||
if (!m_isHoming || m_pGCSController->IsGCS2())
|
||||
{
|
||||
m_bServoControl = (servoControl == 1);
|
||||
m_pGCSController->getAxisPositionCts(this);
|
||||
double realPosition;
|
||||
m_pGCSController->getAxisPosition(this, realPosition);
|
||||
setDoubleParam(pController_->PI_SUP_POSITION, realPosition );
|
||||
}
|
||||
}
|
||||
if (m_isHoming)
|
||||
{
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"PIasynAxis::poll() axis %d referencing ...\n", axisNo_ );
|
||||
}
|
||||
setDoubleParam(pController_->motorPosition_, m_positionCts );
|
||||
setDoubleParam(pController_->motorEncoderPosition_, m_positionCts);
|
||||
setIntegerParam(pController_->motorStatusDirection_, m_lastDirection);
|
||||
setIntegerParam(pController_->motorStatusDone_, done );
|
||||
setIntegerParam(pController_->motorStatusHighLimit_, posLimit);
|
||||
setIntegerParam(pController_->motorStatusHomed_, m_homed );
|
||||
setIntegerParam(pController_->motorStatusMoving_, !done );
|
||||
setIntegerParam(pController_->motorStatusLowLimit_, negLimit);
|
||||
setIntegerParam(pController_->motorStatusGainSupport_, true);
|
||||
setIntegerParam(pController_->motorStatusProblem_, m_bProblem);
|
||||
setIntegerParam(pController_->motorStatusPowerOn_, m_bServoControl);
|
||||
setIntegerParam(pController_->PI_SUP_SERVO, m_bServoControl );
|
||||
|
||||
callParamCallbacks();
|
||||
|
||||
*returnMoving = m_bMoving;
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
asynStatus PIasynAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
|
||||
{
|
||||
m_pGCSController->m_pCurrentLogSink = pasynUser_;
|
||||
asynStatus status = asynError;
|
||||
static const char *functionName = "moveAxis";
|
||||
|
||||
if (!m_pGCSController->AcceptsNewTarget())
|
||||
{
|
||||
asynPrint(pasynUser_, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set port %s, axis %d - controller does not accept new target (busy?)\n",
|
||||
driverName, functionName, pC_->portName, axisNo_ );
|
||||
printf("%s:%s: Set port %s, axis %d - controller does not accept new target (busy?)\n",
|
||||
driverName, functionName, pC_->portName, axisNo_ );
|
||||
return status;
|
||||
}
|
||||
|
||||
if (relative)
|
||||
{
|
||||
//TODO: MVR oder letztes target!
|
||||
//TODO: when is this used?
|
||||
}
|
||||
|
||||
if (pController_->movesDeferred != 0)
|
||||
{ /*Deferred moves.*/
|
||||
deferred_position = position;
|
||||
deferred_move = 1;
|
||||
deferred_relative = relative;
|
||||
setIntegerParam(pController_->motorStatusDone_, 0);
|
||||
callParamCallbacks();
|
||||
return asynSuccess;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (maxVelocity != 0)
|
||||
{
|
||||
status = m_pGCSController->setVelocityCts(this, maxVelocity);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
}
|
||||
if (acceleration != 0)
|
||||
{
|
||||
status = m_pGCSController->setAccelerationCts(this, acceleration);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
}
|
||||
|
||||
setIntegerParam(pController_->motorStatusDone_, 0);
|
||||
callParamCallbacks();
|
||||
|
||||
status = m_pGCSController->moveCts(this, position);
|
||||
}
|
||||
epicsEventSignal(pController_->pollEventId_);
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set driver %s, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f, deffered=%d - status=%d\n",
|
||||
driverName, functionName, pC_->portName, axisNo_, position, minVelocity, maxVelocity, acceleration, pController_->movesDeferred, int(status) );
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIasynAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
|
||||
{
|
||||
m_pGCSController->m_pCurrentLogSink = pasynUser_;
|
||||
asynStatus status = asynError;
|
||||
static const char *functionName = "moveVelocityAxis";
|
||||
|
||||
if (!m_pGCSController->AcceptsNewTarget())
|
||||
{
|
||||
asynPrint(pasynUser_, ASYN_TRACE_ERROR|ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set port %s, axis %d - controller does not accept new target (busy?)",
|
||||
driverName, functionName, pController_->portName, axisNo_ );
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
setIntegerParam(pController_->motorStatusDone_, 0);
|
||||
callParamCallbacks();
|
||||
|
||||
|
||||
double target = maxVelocity > 0 ? posLimit_ : negLimit_;
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set port %s, axis %d move with velocity of %f, accel=%f / target %f - BEFORE MOV\n",
|
||||
driverName, functionName, pController_->portName, axisNo_, maxVelocity, acceleration, target );
|
||||
|
||||
m_pGCSController->setVelocityCts(this, maxVelocity);
|
||||
m_pGCSController->move(this, target);
|
||||
|
||||
epicsEventSignal(pController_->pollEventId_);
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set port %s, axis %d move with velocity of %f, accel=%f / target %f - AFTER MOV\n",
|
||||
driverName, functionName, pController_->portName, axisNo_, maxVelocity, acceleration, target );
|
||||
return status;
|
||||
}
|
||||
|
||||
asynStatus PIasynAxis::stop(double acceleration)
|
||||
{
|
||||
m_pGCSController->m_pCurrentLogSink = pasynUser_;
|
||||
static const char *functionName = "stopAxis";
|
||||
|
||||
deferred_move = 0;
|
||||
|
||||
m_pGCSController->haltAxis(this);
|
||||
|
||||
epicsEventSignal(pController_->pollEventId_);
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set axis %d to stop with accel=%f",
|
||||
driverName, functionName, axisNo_, acceleration );
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
|
||||
|
||||
asynStatus PIasynAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
|
||||
{
|
||||
m_pGCSController->m_pCurrentLogSink = pasynUser_;
|
||||
asynStatus status = asynError;
|
||||
static const char *functionName = "homeAxis";
|
||||
|
||||
m_isHoming = 1;
|
||||
setIntegerParam(pController_->motorStatusDone_, 0 );
|
||||
callParamCallbacks();
|
||||
|
||||
status = m_pGCSController->referenceVelCts(this, maxVelocity, forwards);
|
||||
if (asynSuccess != status)
|
||||
{
|
||||
return status;
|
||||
}
|
||||
setIntegerParam(pController_->motorStatusHomed_, m_homed );
|
||||
epicsEventSignal(pController_->pollEventId_);
|
||||
|
||||
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
||||
"%s:%s: Set driver %s, axis %d to home %s, min vel=%f, max_vel=%f, accel=%f",
|
||||
driverName, functionName, pController_->portName, axisNo_, (forwards?"FORWARDS":"REVERSE"), minVelocity, maxVelocity, acceleration );
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,74 +1,79 @@
|
||||
/*
|
||||
FILENAME... PIasynController.cpp
|
||||
USAGE... PI GCS Motor Support.
|
||||
|
||||
Based on drvMotorSim.c
|
||||
|
||||
Mark Rivers
|
||||
December 13, 2009
|
||||
|
||||
Steffen Rau,
|
||||
January 2011
|
||||
*/
|
||||
|
||||
|
||||
#include <asynDriver.h> // for asynStatus
|
||||
#include <asynMotorAxis.h>
|
||||
|
||||
class PIasynController;
|
||||
class PIGCSController;
|
||||
|
||||
class PIasynAxis : public asynMotorAxis
|
||||
{
|
||||
public:
|
||||
PIasynAxis(class PIasynController *pController, PIGCSController* pGCSController, int axis, const char* szName);
|
||||
virtual~PIasynAxis();
|
||||
|
||||
void Init(const char *portName);
|
||||
|
||||
class PIasynController *pController_;
|
||||
int getAxisNo() { return axisNo_; }
|
||||
|
||||
virtual asynStatus poll(bool *moving);
|
||||
virtual asynStatus move(double position, int relative, double minVelocity, double maxVelocity, double acceleration);
|
||||
virtual asynStatus moveVelocity(double minVelocity, double maxVelocity, double acceleration);
|
||||
virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards);
|
||||
virtual asynStatus stop(double acceleration);
|
||||
|
||||
|
||||
char* m_szAxisName; ///< GCS name
|
||||
|
||||
int m_isHoming; ///< if \b TRUE indicating that axis is currently homing/referencing
|
||||
double deferred_position; ///< currently not used
|
||||
int deferred_move; ///< currently not used
|
||||
int deferred_relative; ///< currently not used
|
||||
int m_homed; ///< if \b TRUE axis was homed and absolute positions are correct
|
||||
|
||||
double m_velocity;
|
||||
double m_acceleration;
|
||||
double m_maxAcceleration;
|
||||
int m_positionCts;
|
||||
double m_position;
|
||||
int m_lastDirection;
|
||||
|
||||
int m_CPUnumerator;
|
||||
int m_CPUdenominator;
|
||||
|
||||
asynUser* m_pasynUser;
|
||||
|
||||
bool m_bHasLimitSwitches;
|
||||
bool m_bHasReference;
|
||||
bool m_bProblem;
|
||||
bool m_bServoControl;
|
||||
bool m_bMoving;
|
||||
int m_movingStateMask;
|
||||
|
||||
friend class PIasynController;
|
||||
private:
|
||||
|
||||
double negLimit_;
|
||||
double posLimit_;
|
||||
PIGCSController* m_pGCSController;
|
||||
|
||||
};
|
||||
|
||||
/*
|
||||
FILENAME... PIasynController.cpp
|
||||
USAGE... PI GCS Motor Support.
|
||||
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
|
||||
Based on drvMotorSim.c
|
||||
|
||||
Mark Rivers
|
||||
December 13, 2009
|
||||
|
||||
Steffen Rau,
|
||||
January 2011
|
||||
*/
|
||||
|
||||
|
||||
#include <asynDriver.h> // for asynStatus
|
||||
#include <asynMotorAxis.h>
|
||||
|
||||
class PIasynController;
|
||||
class PIGCSController;
|
||||
|
||||
class PIasynAxis : public asynMotorAxis
|
||||
{
|
||||
public:
|
||||
PIasynAxis(class PIasynController *pController, PIGCSController* pGCSController, int axis, const char* szName);
|
||||
virtual~PIasynAxis();
|
||||
|
||||
void Init(const char *portName);
|
||||
|
||||
class PIasynController *pController_;
|
||||
int getAxisNo() { return axisNo_; }
|
||||
|
||||
virtual asynStatus poll(bool *moving);
|
||||
virtual asynStatus move(double position, int relative, double minVelocity, double maxVelocity, double acceleration);
|
||||
virtual asynStatus moveVelocity(double minVelocity, double maxVelocity, double acceleration);
|
||||
virtual asynStatus home(double minVelocity, double maxVelocity, double acceleration, int forwards);
|
||||
virtual asynStatus stop(double acceleration);
|
||||
|
||||
|
||||
char* m_szAxisName; ///< GCS name
|
||||
|
||||
int m_isHoming; ///< if \b TRUE indicating that axis is currently homing/referencing
|
||||
double deferred_position; ///< currently not used
|
||||
int deferred_move; ///< currently not used
|
||||
int deferred_relative; ///< currently not used
|
||||
int m_homed; ///< if \b TRUE axis was homed and absolute positions are correct
|
||||
|
||||
double m_velocity;
|
||||
double m_acceleration;
|
||||
double m_maxAcceleration;
|
||||
int m_positionCts;
|
||||
double m_position;
|
||||
int m_lastDirection;
|
||||
|
||||
int m_CPUnumerator;
|
||||
int m_CPUdenominator;
|
||||
|
||||
asynUser* m_pasynUser;
|
||||
|
||||
bool m_bHasLimitSwitches;
|
||||
bool m_bHasReference;
|
||||
bool m_bProblem;
|
||||
bool m_bServoControl;
|
||||
bool m_bMoving;
|
||||
int m_movingStateMask;
|
||||
|
||||
friend class PIasynController;
|
||||
private:
|
||||
|
||||
double negLimit_;
|
||||
double posLimit_;
|
||||
PIGCSController* m_pGCSController;
|
||||
|
||||
};
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,80 +1,85 @@
|
||||
/*
|
||||
FILENAME... PIasynController.cpp
|
||||
USAGE... Simulated Motor Support.
|
||||
|
||||
Based on drvMotorSim.c
|
||||
|
||||
Mark Rivers
|
||||
December 13, 2009
|
||||
|
||||
*/
|
||||
|
||||
#ifndef PI_ASYN_DRIVER_INCLUDED_
|
||||
#define PI_ASYN_DRIVER_INCLUDED_
|
||||
|
||||
#include "asynMotorController.h"
|
||||
#include "asynMotorAxis.h"
|
||||
|
||||
class PIasynAxis;
|
||||
class PIGCSController;
|
||||
|
||||
class PIasynController : asynMotorController {
|
||||
public:
|
||||
PIasynController(const char *portName, const char* asynPort, int numAxes, int priority, int stackSize, int movingPollPeriod, int idlePollPeriod);
|
||||
asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
|
||||
asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value);
|
||||
void report(FILE *fp, int level);
|
||||
asynStatus profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger);
|
||||
asynStatus triggerProfile(asynUser *pasynUser);
|
||||
asynStatus configAxis(PIasynAxis *pAxis);
|
||||
|
||||
PIasynAxis* getPIAxis(asynUser *pasynUser) { return (PIasynAxis*)asynMotorController::getAxis(pasynUser); }
|
||||
PIasynAxis* getPIAxis(int axisNo) { return (PIasynAxis*)asynMotorController::getAxis(axisNo); }
|
||||
|
||||
virtual asynStatus poll();
|
||||
|
||||
friend class PIasynAxis;
|
||||
|
||||
private:
|
||||
// void process(PIasynAxis *pAxis);
|
||||
epicsThreadId motorThread;
|
||||
int movesDeferred;
|
||||
//int numAxes;
|
||||
asynStatus processDeferredMoves();
|
||||
//PIasynAxis** m_pAxes;
|
||||
|
||||
PIGCSController* m_pGCSController;
|
||||
|
||||
int PI_SUP_POSITION;
|
||||
int PI_SUP_TARGET;
|
||||
int PI_SUP_SERVO;
|
||||
int PI_SUP_LAST_ERR;
|
||||
int PI_SUP_PIVOT_X;
|
||||
int PI_SUP_PIVOT_Y;
|
||||
int PI_SUP_PIVOT_Z;
|
||||
int PI_SUP_RBPIVOT_X;
|
||||
int PI_SUP_RBPIVOT_Y;
|
||||
int PI_SUP_RBPIVOT_Z;
|
||||
|
||||
};
|
||||
|
||||
|
||||
#define PI_SUP_POSITION_String "PI_SUP_POSITION"
|
||||
#define PI_SUP_TARGET_String "PI_SUP_TARGET"
|
||||
#define PI_SUP_SERVO_String "PI_SUP_SERVO"
|
||||
#define PI_SUP_LAST_ERR_String "PI_SUP_LAST_ERR"
|
||||
#define PI_SUP_PIVOT_X_String "PI_SUP_PIVOT_X"
|
||||
#define PI_SUP_PIVOT_Y_String "PI_SUP_PIVOT_Y"
|
||||
#define PI_SUP_PIVOT_Z_String "PI_SUP_PIVOT_Z"
|
||||
#define PI_SUP_RBPIVOT_X_String "PI_SUP_RBPIVOT_X"
|
||||
#define PI_SUP_RBPIVOT_Y_String "PI_SUP_RBPIVOT_Y"
|
||||
#define PI_SUP_RBPIVOT_Z_String "PI_SUP_RBPIVOT_Z"
|
||||
|
||||
|
||||
typedef struct PIasynControllerNode {
|
||||
ELLNODE node;
|
||||
const char *portName;
|
||||
PIasynController *pController;
|
||||
} PIasynControllerNode;
|
||||
|
||||
#endif // PI_ASYN_DRIVER_INCLUDED_
|
||||
/*
|
||||
FILENAME... PIasynController.cpp
|
||||
USAGE... Simulated Motor Support.
|
||||
|
||||
Version: $Revision$
|
||||
Modified By: $Author$
|
||||
Last Modified: $Date$
|
||||
HeadURL: $URL$
|
||||
|
||||
Based on drvMotorSim.c
|
||||
|
||||
Mark Rivers
|
||||
December 13, 2009
|
||||
|
||||
*/
|
||||
|
||||
#ifndef PI_ASYN_DRIVER_INCLUDED_
|
||||
#define PI_ASYN_DRIVER_INCLUDED_
|
||||
|
||||
#include "asynMotorController.h"
|
||||
#include "asynMotorAxis.h"
|
||||
|
||||
class PIasynAxis;
|
||||
class PIGCSController;
|
||||
|
||||
class PIasynController : asynMotorController {
|
||||
public:
|
||||
PIasynController(const char *portName, const char* asynPort, int numAxes, int priority, int stackSize, int movingPollPeriod, int idlePollPeriod);
|
||||
asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
|
||||
asynStatus writeFloat64(asynUser *pasynUser, epicsFloat64 value);
|
||||
void report(FILE *fp, int level);
|
||||
asynStatus profileMove(asynUser *pasynUser, int npoints, double positions[], double times[], int relative, int trigger);
|
||||
asynStatus triggerProfile(asynUser *pasynUser);
|
||||
asynStatus configAxis(PIasynAxis *pAxis);
|
||||
|
||||
PIasynAxis* getPIAxis(asynUser *pasynUser) { return (PIasynAxis*)asynMotorController::getAxis(pasynUser); }
|
||||
PIasynAxis* getPIAxis(int axisNo) { return (PIasynAxis*)asynMotorController::getAxis(axisNo); }
|
||||
|
||||
virtual asynStatus poll();
|
||||
|
||||
friend class PIasynAxis;
|
||||
|
||||
private:
|
||||
// void process(PIasynAxis *pAxis);
|
||||
epicsThreadId motorThread;
|
||||
int movesDeferred;
|
||||
//int numAxes;
|
||||
asynStatus processDeferredMoves();
|
||||
//PIasynAxis** m_pAxes;
|
||||
|
||||
PIGCSController* m_pGCSController;
|
||||
|
||||
int PI_SUP_POSITION;
|
||||
int PI_SUP_TARGET;
|
||||
int PI_SUP_SERVO;
|
||||
int PI_SUP_LAST_ERR;
|
||||
int PI_SUP_PIVOT_X;
|
||||
int PI_SUP_PIVOT_Y;
|
||||
int PI_SUP_PIVOT_Z;
|
||||
int PI_SUP_RBPIVOT_X;
|
||||
int PI_SUP_RBPIVOT_Y;
|
||||
int PI_SUP_RBPIVOT_Z;
|
||||
|
||||
};
|
||||
|
||||
|
||||
#define PI_SUP_POSITION_String "PI_SUP_POSITION"
|
||||
#define PI_SUP_TARGET_String "PI_SUP_TARGET"
|
||||
#define PI_SUP_SERVO_String "PI_SUP_SERVO"
|
||||
#define PI_SUP_LAST_ERR_String "PI_SUP_LAST_ERR"
|
||||
#define PI_SUP_PIVOT_X_String "PI_SUP_PIVOT_X"
|
||||
#define PI_SUP_PIVOT_Y_String "PI_SUP_PIVOT_Y"
|
||||
#define PI_SUP_PIVOT_Z_String "PI_SUP_PIVOT_Z"
|
||||
#define PI_SUP_RBPIVOT_X_String "PI_SUP_RBPIVOT_X"
|
||||
#define PI_SUP_RBPIVOT_Y_String "PI_SUP_RBPIVOT_Y"
|
||||
#define PI_SUP_RBPIVOT_Z_String "PI_SUP_RBPIVOT_Z"
|
||||
|
||||
|
||||
typedef struct PIasynControllerNode {
|
||||
ELLNODE node;
|
||||
const char *portName;
|
||||
PIasynController *pController;
|
||||
} PIasynControllerNode;
|
||||
|
||||
#endif // PI_ASYN_DRIVER_INCLUDED_
|
||||
|
||||
@@ -1,61 +1,61 @@
|
||||
Readme for PIasyn - EPICS support for PI GCS2 stages
|
||||
========================================================
|
||||
|
||||
The PIasyn driver is written to support PI motion controllers which
|
||||
support GCS2 (General Command Set) as commanding language.
|
||||
Currently this is implemented with different C++ classes where the
|
||||
differences and specialties are handled. This can be implemented
|
||||
by querying the controller what features are supported. So in future
|
||||
developments these classes may disappear.
|
||||
|
||||
Homing
|
||||
========
|
||||
|
||||
a) stages with absolute sensors
|
||||
Some PI stages, mostly piezo stages, use an absolute position sensor.
|
||||
The correct absolute position is known to the controller immediately after power up.
|
||||
For these stages homing is not necessary and thus not supported.
|
||||
These stages will ignore HOMF and HOMR.
|
||||
The HOME flag of the MSTA field is meaningless for these stages
|
||||
and HOMED is always true,
|
||||
|
||||
b) stages with incremental sensors
|
||||
Some PIStages needs to be referenced after controller power up. The state
|
||||
is reflected by the HOMED flag of MSTA. There are different ways to home
|
||||
a stage. The default is by using the reference switch. If a stage has
|
||||
a reference switch HOMF and HOMR will move to this switch. The direction is
|
||||
determined by the controller. The reference signal is edge based, so the controller
|
||||
can find out on which side of the reference the stage is and can move using
|
||||
the shortest path. HOMF and HOMR will trigger the same move. If the stage has
|
||||
no reference switch the built in limit switches can be used to determine the absolute
|
||||
position. In this case HOMF will send the stage to its positive limit switch and
|
||||
HOMR to its negative limit switch. As the reference is edge based or not present the
|
||||
HOME bit of the MSTA field is meaningless for these stages.
|
||||
|
||||
c) setting the homing velocity
|
||||
Reducing the velocity used for homing is usually done to improve the accuracy.
|
||||
PI controllers reach the reference every time from the same "side". So only this
|
||||
last approach is performed with the homing velocity set with HVEL. The coarse move
|
||||
to the proximity of the reference is done with the a velocity limited by the current
|
||||
velocity (set with VELO) and the maximum possible velocity to decelerate in case of
|
||||
hitting a limit switch.
|
||||
|
||||
"Motion Errors"
|
||||
=================
|
||||
|
||||
If the controller detects a "motion error", i.e. the difference between target and
|
||||
current position is greater than some given limit (also known as "following error"),
|
||||
the driver will set the PROBLEM bit in MSTA. Servo control is also switched off.
|
||||
The EA_POSITION bit of MSTA will reflect the "servo control" state. To re-enable
|
||||
servo control use CNEN.
|
||||
|
||||
|
||||
C-702
|
||||
=======
|
||||
|
||||
The C-702 controller uses an older "dialect" of GCS1.
|
||||
That's why at some points different parameter IDs are used and why there is no communication
|
||||
during homing. So the position cannot be updated during homing and will "jump" to the final
|
||||
position after homing is finished.
|
||||
|
||||
|
||||
Readme for PIasyn - EPICS support for PI GCS2 stages
|
||||
========================================================
|
||||
|
||||
The PIasyn driver is written to support PI motion controllers which
|
||||
support GCS2 (General Command Set) as commanding language.
|
||||
Currently this is implemented with different C++ classes where the
|
||||
differences and specialties are handled. This can be implemented
|
||||
by querying the controller what features are supported. So in future
|
||||
developments these classes may disappear.
|
||||
|
||||
Homing
|
||||
========
|
||||
|
||||
a) stages with absolute sensors
|
||||
Some PI stages, mostly piezo stages, use an absolute position sensor.
|
||||
The correct absolute position is known to the controller immediately after power up.
|
||||
For these stages homing is not necessary and thus not supported.
|
||||
These stages will ignore HOMF and HOMR.
|
||||
The HOME flag of the MSTA field is meaningless for these stages
|
||||
and HOMED is always true,
|
||||
|
||||
b) stages with incremental sensors
|
||||
Some PIStages needs to be referenced after controller power up. The state
|
||||
is reflected by the HOMED flag of MSTA. There are different ways to home
|
||||
a stage. The default is by using the reference switch. If a stage has
|
||||
a reference switch HOMF and HOMR will move to this switch. The direction is
|
||||
determined by the controller. The reference signal is edge based, so the controller
|
||||
can find out on which side of the reference the stage is and can move using
|
||||
the shortest path. HOMF and HOMR will trigger the same move. If the stage has
|
||||
no reference switch the built in limit switches can be used to determine the absolute
|
||||
position. In this case HOMF will send the stage to its positive limit switch and
|
||||
HOMR to its negative limit switch. As the reference is edge based or not present the
|
||||
HOME bit of the MSTA field is meaningless for these stages.
|
||||
|
||||
c) setting the homing velocity
|
||||
Reducing the velocity used for homing is usually done to improve the accuracy.
|
||||
PI controllers reach the reference every time from the same "side". So only this
|
||||
last approach is performed with the homing velocity set with HVEL. The coarse move
|
||||
to the proximity of the reference is done with the a velocity limited by the current
|
||||
velocity (set with VELO) and the maximum possible velocity to decelerate in case of
|
||||
hitting a limit switch.
|
||||
|
||||
"Motion Errors"
|
||||
=================
|
||||
|
||||
If the controller detects a "motion error", i.e. the difference between target and
|
||||
current position is greater than some given limit (also known as "following error"),
|
||||
the driver will set the PROBLEM bit in MSTA. Servo control is also switched off.
|
||||
The EA_POSITION bit of MSTA will reflect the "servo control" state. To re-enable
|
||||
servo control use CNEN.
|
||||
|
||||
|
||||
C-702
|
||||
=======
|
||||
|
||||
The C-702 controller uses an older "dialect" of GCS1.
|
||||
That's why at some points different parameter IDs are used and why there is no communication
|
||||
during homing. So the position cannot be updated during homing and will "jump" to the final
|
||||
position after homing is finished.
|
||||
|
||||
|
||||
|
||||
+1075
-1075
File diff suppressed because it is too large
Load Diff
+3139
-3132
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user