- Adapted indenation to new agreed upon system
- Added support for second generation scriptcontext based counter
This commit is contained in:
koennecke
208
hklmot.c
208
hklmot.c
@ -17,159 +17,176 @@
|
||||
#include "hklmot.h"
|
||||
#include "singlex.h"
|
||||
/*=================== Object Descriptor Interface ===================================================*/
|
||||
static void *HKLGetInterface(void *pData, int iID){
|
||||
static void *HKLGetInterface(void *pData, int iID)
|
||||
{
|
||||
pHKLMot self = NULL;
|
||||
|
||||
self = (pHKLMot)pData;
|
||||
if(self == NULL){
|
||||
self = (pHKLMot) pData;
|
||||
if (self == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
if(iID == DRIVEID){
|
||||
if (iID == DRIVEID) {
|
||||
return self->pDriv;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------------------------------------*/
|
||||
extern void stopHKLMotors(pHKL hkl);
|
||||
/*=================== Drivable Interface ============================================================*/
|
||||
static int HKLHalt(void *pData){
|
||||
static int HKLHalt(void *pData)
|
||||
{
|
||||
pHKLMot self = NULL;
|
||||
|
||||
self = (pHKLMot)pData;
|
||||
self = (pHKLMot) pData;
|
||||
assert(self != NULL);
|
||||
stopHKLMotors(self->pHkl);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------------------------------------*/
|
||||
static int HKLCheckLimits(void *self, float fVal, char *error, int errLen){
|
||||
static int HKLCheckLimits(void *self, float fVal, char *error, int errLen)
|
||||
{
|
||||
/*
|
||||
There is no meaningful implementation here. This gets called when starting the motor.
|
||||
At that stage not all other values may be known. If the calculation fails, this will die
|
||||
at status check time.
|
||||
*/
|
||||
There is no meaningful implementation here. This gets called when starting the motor.
|
||||
At that stage not all other values may be known. If the calculation fails, this will die
|
||||
at status check time.
|
||||
*/
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------------------------------------*/
|
||||
static long HKLSetValue(void *pData, SConnection *pCon, float fVal){
|
||||
static long HKLSetValue(void *pData, SConnection * pCon, float fVal)
|
||||
{
|
||||
pHKLMot self = NULL;
|
||||
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
self = (pHKLMot)pData;
|
||||
self = (pHKLMot) pData;
|
||||
assert(self != NULL);
|
||||
|
||||
self->pHkl->targetHKL[self->index] = fVal;
|
||||
self->pHkl->targetDirty = 1;
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------------------------------*/
|
||||
static int checkMotors(pHKLMot self, SConnection *pCon){
|
||||
static int checkMotors(pHKLMot self, SConnection * pCon)
|
||||
{
|
||||
int status;
|
||||
pMotor pTheta, pOmega, pChi, pPhi, pNu;
|
||||
|
||||
|
||||
pTheta = SXGetMotor(TwoTheta);
|
||||
pOmega = SXGetMotor(Omega);
|
||||
pChi = SXGetMotor(Chi);
|
||||
pPhi = SXGetMotor(Phi);
|
||||
if(pTheta == NULL || pOmega == NULL){
|
||||
SCWrite(pCon,"ERROR: configuration problem, stt,om motors not found,",
|
||||
eError);
|
||||
return HWFault;
|
||||
if (pTheta == NULL || pOmega == NULL) {
|
||||
SCWrite(pCon, "ERROR: configuration problem, stt,om motors not found,",
|
||||
eError);
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
|
||||
|
||||
status = pTheta->pDrivInt->CheckStatus(pTheta, pCon);
|
||||
if(status != HWIdle && status != OKOK){
|
||||
if (status != HWIdle && status != OKOK) {
|
||||
return status;
|
||||
}
|
||||
status = pOmega->pDrivInt->CheckStatus(pOmega, pCon);
|
||||
if(status != HWIdle && status != OKOK){
|
||||
if (status != HWIdle && status != OKOK) {
|
||||
return status;
|
||||
}
|
||||
if(SXGetMode() == NB){
|
||||
pNu = SXGetMotor(Nu);
|
||||
if(pNu == NULL){
|
||||
SCWrite(pCon,"ERROR: configuration problem, nu motor not found,",
|
||||
eError);
|
||||
return HWFault;
|
||||
|
||||
}
|
||||
if (SXGetMode() == NB) {
|
||||
pNu = SXGetMotor(Nu);
|
||||
if (pNu == NULL) {
|
||||
SCWrite(pCon, "ERROR: configuration problem, nu motor not found,",
|
||||
eError);
|
||||
return HWFault;
|
||||
|
||||
}
|
||||
status = pNu->pDrivInt->CheckStatus(pNu, pCon);
|
||||
if(status != HWIdle && status != OKOK){
|
||||
if (status != HWIdle && status != OKOK) {
|
||||
return status;
|
||||
}
|
||||
} else {
|
||||
pChi = SXGetMotor(Chi);
|
||||
pPhi = SXGetMotor(Phi);
|
||||
if(pTheta == NULL || pOmega == NULL){
|
||||
SCWrite(pCon,"ERROR: configuration problem, chi, phi motors not found,",
|
||||
eError);
|
||||
return HWFault;
|
||||
}
|
||||
pChi = SXGetMotor(Chi);
|
||||
pPhi = SXGetMotor(Phi);
|
||||
if (pTheta == NULL || pOmega == NULL) {
|
||||
SCWrite(pCon,
|
||||
"ERROR: configuration problem, chi, phi motors not found,",
|
||||
eError);
|
||||
return HWFault;
|
||||
}
|
||||
status = pChi->pDrivInt->CheckStatus(pChi, pCon);
|
||||
if(status != HWIdle && status != OKOK){
|
||||
if (status != HWIdle && status != OKOK) {
|
||||
return status;
|
||||
}
|
||||
status = pPhi->pDrivInt->CheckStatus(pPhi, pCon);
|
||||
if(status != HWIdle && status != OKOK){
|
||||
if (status != HWIdle && status != OKOK) {
|
||||
return status;
|
||||
}
|
||||
}
|
||||
return HWIdle;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------------------------------------*/
|
||||
static int HKLCheckStatus(void *pData, SConnection *pCon){
|
||||
static int HKLCheckStatus(void *pData, SConnection * pCon)
|
||||
{
|
||||
pHKLMot self = NULL;
|
||||
int status;
|
||||
|
||||
self = (pHKLMot)pData;
|
||||
self = (pHKLMot) pData;
|
||||
assert(self != NULL);
|
||||
if(self->pHkl->targetDirty == 1){
|
||||
status = RunHKL(self->pHkl,self->pHkl->targetHKL,.0,0,pCon);
|
||||
if (self->pHkl->targetDirty == 1) {
|
||||
status = RunHKL(self->pHkl, self->pHkl->targetHKL, .0, 0, pCon);
|
||||
self->pHkl->targetDirty = 0;
|
||||
if(status != 1){
|
||||
SCSetInterrupt(pCon,eAbortOperation);
|
||||
if (status != 1) {
|
||||
SCSetInterrupt(pCon, eAbortOperation);
|
||||
return HWFault;
|
||||
}
|
||||
return HWBusy;
|
||||
} else {
|
||||
return checkMotors(self,pCon);
|
||||
return checkMotors(self, pCon);
|
||||
}
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------------------------------------------------*/
|
||||
static float HKLGetValue(void *pData, SConnection *pCon){
|
||||
static float HKLGetValue(void *pData, SConnection * pCon)
|
||||
{
|
||||
pHKLMot self = NULL;
|
||||
float fVal[3];
|
||||
int status;
|
||||
|
||||
self = (pHKLMot)pData;
|
||||
self = (pHKLMot) pData;
|
||||
assert(self != NULL);
|
||||
|
||||
|
||||
status = GetHKLFromAngles(self->pHkl,pCon,fVal);
|
||||
if(status != 1){
|
||||
SCWrite(pCon,"ERROR: failed to read positions or convert to HKL",eError);
|
||||
status = GetHKLFromAngles(self->pHkl, pCon, fVal);
|
||||
if (status != 1) {
|
||||
SCWrite(pCon, "ERROR: failed to read positions or convert to HKL",
|
||||
eError);
|
||||
return -9999.99;
|
||||
}
|
||||
return fVal[self->index];
|
||||
}
|
||||
|
||||
/*=============================== Live and Death ====================================*/
|
||||
static pHKLMot MakeHKLMot(pHKL pHkl, int index){
|
||||
static pHKLMot MakeHKLMot(pHKL pHkl, int index)
|
||||
{
|
||||
pHKLMot self = NULL;
|
||||
|
||||
assert(pHkl != NULL);
|
||||
assert(index >= 0 && index < 3);
|
||||
|
||||
self = (pHKLMot)malloc(sizeof(HKLMot));
|
||||
if(self == NULL){
|
||||
self = (pHKLMot) malloc(sizeof(HKLMot));
|
||||
if (self == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
memset(self,0,sizeof(HKLMot));
|
||||
memset(self, 0, sizeof(HKLMot));
|
||||
self->pDes = CreateDescriptor("HKLMot");
|
||||
self->pDriv = CreateDrivableInterface();
|
||||
if(self->pDes == NULL || self->pDriv == NULL){
|
||||
if (self->pDes == NULL || self->pDriv == NULL) {
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
@ -183,86 +200,95 @@ static pHKLMot MakeHKLMot(pHKL pHkl, int index){
|
||||
self->index = index;
|
||||
return self;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------------------*/
|
||||
static void KillHklMot(void *pData){
|
||||
static void KillHklMot(void *pData)
|
||||
{
|
||||
pHKLMot self = NULL;
|
||||
|
||||
self = (pHKLMot)pData;
|
||||
if(self == NULL){
|
||||
self = (pHKLMot) pData;
|
||||
if (self == NULL) {
|
||||
return;
|
||||
}
|
||||
if(self->pDes != NULL){
|
||||
if (self->pDes != NULL) {
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
if(self->pDriv){
|
||||
if (self->pDriv) {
|
||||
free(self->pDriv);
|
||||
}
|
||||
free(self);
|
||||
}
|
||||
|
||||
/*=============================== Interpreter Interface ============================*/
|
||||
int HKLMotAction(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]){
|
||||
int HKLMotAction(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pHKLMot self = NULL;
|
||||
float value;
|
||||
char pBuffer[132];
|
||||
|
||||
self = (pHKLMot)pData;
|
||||
self = (pHKLMot) pData;
|
||||
assert(self != NULL);
|
||||
|
||||
value = self->pDriv->GetValue(self,pCon);
|
||||
if(value < -9000.){
|
||||
snprintf(pBuffer,131,"ERROR: failed to read %s",argv[0]);
|
||||
SCWrite(pCon,pBuffer,eError);
|
||||
value = self->pDriv->GetValue(self, pCon);
|
||||
if (value < -9000.) {
|
||||
snprintf(pBuffer, 131, "ERROR: failed to read %s", argv[0]);
|
||||
SCWrite(pCon, pBuffer, eError);
|
||||
return 0;
|
||||
}
|
||||
snprintf(pBuffer,131,"%s = %f", argv[0], value);
|
||||
SCWrite(pCon,pBuffer,eValue);
|
||||
snprintf(pBuffer, 131, "%s = %f", argv[0], value);
|
||||
SCWrite(pCon, pBuffer, eValue);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*------------------------------------------------------------------------------------------*/
|
||||
int HKLMotInstall(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]){
|
||||
int HKLMotInstall(SConnection * pCon, SicsInterp * pSics, void *pData,
|
||||
int argc, char *argv[])
|
||||
{
|
||||
pHKL pHkl;
|
||||
pHKLMot pMot = NULL;
|
||||
char pBuffer[131];
|
||||
|
||||
if(argc < 2){
|
||||
pHkl = (pHKL)FindCommandData(pSics,"hkl","4-Circle-Calculus");
|
||||
if (argc < 2) {
|
||||
pHkl = (pHKL) FindCommandData(pSics, "hkl", "4-Circle-Calculus");
|
||||
} else {
|
||||
strtolower(argv[1]);
|
||||
pHkl = (pHKL)FindCommandData(pSics,argv[1],"4-Circle-Calculus");
|
||||
strtolower(argv[1]);
|
||||
pHkl = (pHKL) FindCommandData(pSics, argv[1], "4-Circle-Calculus");
|
||||
}
|
||||
if(pHkl == NULL){
|
||||
snprintf(pBuffer,131,"ERROR: %s is not present or no HKL object",argv[1]);
|
||||
SCWrite(pCon,pBuffer,eError);
|
||||
if (pHkl == NULL) {
|
||||
snprintf(pBuffer, 131, "ERROR: %s is not present or no HKL object",
|
||||
argv[1]);
|
||||
SCWrite(pCon, pBuffer, eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pMot = MakeHKLMot(pHkl, 0);
|
||||
if(pMot == NULL){
|
||||
SCWrite(pCon,"ERROR: out of memory creating H Motor",eError);
|
||||
if (pMot == NULL) {
|
||||
SCWrite(pCon, "ERROR: out of memory creating H Motor", eError);
|
||||
return 0;
|
||||
}
|
||||
if(!AddCommand(pSics,"H",HKLMotAction,KillHklMot,pMot)){
|
||||
SCWrite(pCon,"ERROR: duplicate command H not created",eError);
|
||||
if (!AddCommand(pSics, "H", HKLMotAction, KillHklMot, pMot)) {
|
||||
SCWrite(pCon, "ERROR: duplicate command H not created", eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pMot = MakeHKLMot(pHkl, 1);
|
||||
if(pMot == NULL){
|
||||
SCWrite(pCon,"ERROR: out of memory creating K Motor",eError);
|
||||
if (pMot == NULL) {
|
||||
SCWrite(pCon, "ERROR: out of memory creating K Motor", eError);
|
||||
return 0;
|
||||
}
|
||||
if(!AddCommand(pSics,"K",HKLMotAction,KillHklMot,pMot)){
|
||||
SCWrite(pCon,"ERROR: duplicate command K not created",eError);
|
||||
if (!AddCommand(pSics, "K", HKLMotAction, KillHklMot, pMot)) {
|
||||
SCWrite(pCon, "ERROR: duplicate command K not created", eError);
|
||||
return 0;
|
||||
}
|
||||
|
||||
pMot = MakeHKLMot(pHkl, 2);
|
||||
if(pMot == NULL){
|
||||
SCWrite(pCon,"ERROR: out of memory creating L Motor",eError);
|
||||
if (pMot == NULL) {
|
||||
SCWrite(pCon, "ERROR: out of memory creating L Motor", eError);
|
||||
return 0;
|
||||
}
|
||||
if(!AddCommand(pSics,"L",HKLMotAction,KillHklMot,pMot)){
|
||||
SCWrite(pCon,"ERROR: duplicate command L not created",eError);
|
||||
if (!AddCommand(pSics, "L", HKLMotAction, KillHklMot, pMot)) {
|
||||
SCWrite(pCon, "ERROR: duplicate command L not created", eError);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
|
||||
Reference in New Issue
Block a user