forked from epics_driver_modules/motorBase
kpetersn
326 lines
10 KiB
C++
326 lines
10 KiB
C++
/*
|
|
FILENAME... PIasynController.cpp
|
|
USAGE...
|
|
|
|
*************************************************************************
|
|
* Copyright (c) 2011-2013 Physik Instrumente (PI) GmbH & Co. KG
|
|
* This file is distributed subject to the EPICS Open License Agreement
|
|
* found in the file LICENSE that is included with this distribution.
|
|
*************************************************************************
|
|
|
|
|
|
Original Author: Steffen Rau
|
|
|
|
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 <motor_interface.h>
|
|
|
|
#include "PIasynAxis.h"
|
|
#include "PIasynController.h"
|
|
#include "PIGCSController.h"
|
|
#include "PIInterface.h"
|
|
|
|
#include <epicsExport.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_pInterface->m_pCurrentLogSink = logSink;
|
|
|
|
setIntegerParam(pController_->motorStatusGainSupport_, 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_pInterface->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_pInterface->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_pInterface->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_pInterface->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;
|
|
}
|
|
|
|
asynStatus PIasynAxis::setPosition(double position)
|
|
{
|
|
m_pGCSController->m_pInterface->m_pCurrentLogSink = pasynUser_;
|
|
asynStatus status = asynError;
|
|
status = m_pGCSController->setAxisPositionCts(this, position);
|
|
epicsEventSignal(pController_->pollEventId_);
|
|
|
|
asynPrint(pasynUser_, ASYN_TRACE_FLOW,
|
|
"%s:%s: Set driver %s, axis %d set position to %f - status=%d\n",
|
|
driverName, "setPositionAxis", pC_->portName, axisNo_, position, int(status) );
|
|
return status;
|
|
}
|
|
|
|
|