linse/sicspsi
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- Make Poldi Tensile device work

Browse Source 
- Added error resetting and a coupel of bug fixes for SLS magnets
- Implemented new table driving mode for MARS
This commit is contained in:
koennecke
2007-05-30 11:59:13 +00:00
parent 317aa90840
commit 8fbfe687aa
9 changed files with 729 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
make_gen
View File

@ -4,7 +4,10 @@
# Mark Koennecke, June 2003
#-------------------------------------------------------------------------
.SUFFIXES:
.SUFFIXES: .c .o .f
.SUFFIXES: .c .o .f .tc
.tc.c:
tjxp $*.tc $*.c
OBJ=psi.o buffer.o ruli.o dmc.o nxsans.o nextrics.o sps.o pimotor.o \
pipiezo.o sanswave.o faverage.o fowrite.o amor2t.o nxamor.o \
@ -18,7 +21,10 @@ OBJ=psi.o buffer.o ruli.o dmc.o nxsans.o nextrics.o sps.o pimotor.o \
el737hpv2driv.o swmotor2.o tricssupport.o amorcomp.o \
$(MZOBJ) amordrive.o amorset.o tcpdornier.o sinqhttp.o\
dgrambroadcast.o sinq.o tabledrive.o tcpdocho.o julcho.o \
ritastorage.o poldizug.o audinelib.o delcam.o el737hpdrivsps.o
ritastorage.o poldizug.o audinelib.o delcam.o el737hpdrivsps.o \
rebin.o sanslirebin.o
.SECONDARY.: sanslirebin.c
MZOBJ=fsm.o logger.o sugar.o pardef.o ease.o strobj.o oxinst.o logreader.o \
ipsdriv.o ilmdriv.o itcdriv.o ighdriv.o euro2kdriv.o modbus.o arrobj.o \

268
poldizug.c
View File

@ -7,22 +7,26 @@
*
* copyright: see file COPYRIGHT
*
* Mark Koennecke, October 2006
* Mark Koennecke, October 2006(started)
* - May 2007(The device was actually available)
*/
#include <sics.h>
#include <rs232controller.h>
#include <bit.h>
/*===============================================================*/
#define FORCE 0
#define ZUGPOS 1
#define REGEL 10
#define OFF 20
/*---------------------------------------------------------------*/
typedef struct {
pObjectDescriptor pDes;
pIDrivable pDriv;
prs232 controller;
int iMode;
int state;
float maxForce, minForce;
float maxPos, minPos;
int velocity;
}PZR, *pPZR;
/*--------------------------------------------------------------*/
static int PZRCommand(prs232 controller, SConnection *pCon,
@ -37,7 +41,14 @@ static int PZRCommand(prs232 controller, SConnection *pCon,
for(i = 0; i < 3; i++){
status = transactRS232(controller,command, strlen(command),
reply, replyLen);
if(status == 1){
if(status > 0 ){
if(strstr(reply,"ERR") != NULL){
if(pCon != NULL){
snprintf(buffer,511,"ERROR: device reported %s", reply);
SCWrite(pCon,buffer,eError);
return HWFault;
}
}
return 1;
}
/*
@ -85,7 +96,7 @@ static int PZRHalt(void *data) {
self = (pPZR)data;
PZRCommand(self->controller,NULL,"stop\n",buffer,79);
PZRCommand(self->controller,NULL,"stop\r\n",buffer,79);
return OKOK;
}
@ -131,18 +142,54 @@ static long PZRSetValue(void *data, SConnection *pCon, float val){
self = (pPZR)data;
if(self->iMode == FORCE){
snprintf(command,79,"wr force %f\n", val);
} else {
snprintf(command,79,"wr vel %f\n", val);
}
status = PZRCommand(self->controller, pCon, command, reply, 79);
if(status == 1){
switch(self->iMode){
case FORCE:
if(self->state == OFF){
snprintf(command,79,"wr vel %d\r\n", self->velocity);
status = PZRCommand(self->controller, pCon, command, reply, 79);
if(status != 1){
return HWFault;
}
snprintf(command,79,"wr force %f\r\n", val);
status = PZRCommand(self->controller, pCon, command, reply, 79);
if(status != 1){
return HWFault;
}
status = PZRCommand(self->controller, pCon, "wr mode 1\r\n", reply, 79);
if(status != 1){
return HWFault;
}
self->state = REGEL;
return OKOK;
} else {
snprintf(command,79,"wr force %f\r\n", val);
status = PZRCommand(self->controller, pCon, command, reply, 79);
if(status != 1){
return HWFault;
}
return OKOK;
}
break;
case ZUGPOS:
self->state = OFF;
snprintf(command,79,"wr vel %d\r\n", self->velocity);
status = PZRCommand(self->controller, pCon, command, reply, 79);
if(status != 1){
return HWFault;
}
snprintf(command,79,"wr pos %f\r\n", val);
status = PZRCommand(self->controller, pCon, command, reply, 79);
if(status != 1){
return HWFault;
}
status = PZRCommand(self->controller, pCon, "wr mode 2\r\n", reply, 79);
if(status != 1){
return HWFault;
}
return OKOK;
} else {
return HWFault;
break;
}
return HWFault;
return OKOK;
}
/*----------------------------------------------------------------
Checks the status of a running motor. Possible return values
@ -154,42 +201,80 @@ static long PZRSetValue(void *data, SConnection *pCon, float val){
For real motors CheckStatus again shall try hard to fix any
issues with the motor
------------------------------------------------------------------*/
static int BitSet(char d, int n){
unsigned char e;
e = d & (1 << n);
if(e == 0){
return 0;
} else {
return 1;
}
}
/*---------------------------------------------------------------*/
static int PZRCheckStatus(void *data, SConnection *pCon){
pPZR self = NULL;
int status, code;
char command[80], reply[80], bits[1];
int status, code, i;
char command[80], reply[80], bits;
self = (pPZR)data;
snprintf(command,79,"rd stat\n");
snprintf(command,79,"rd stat\r\n");
status = PZRCommand(self->controller, pCon, command, reply, 79);
if(status != 1){
return HWFault;
}
sscanf(reply,"%x", &code);
bits[0] = (char)code;
bits = (char)code;
/*
memset(command,0,80);
for(i = 0; i < 8; i++){
if(BitSet(bits,i)){
command[i] = '1';
} else {
command[i] = '0';
}
}
snprintf(reply,79,"code = %d",code);
strcat(command,reply);
SCWrite(pCon,command,eValue);
*/
/*
* check for errors
*/
if(BITSET(bits,3)){
SCWrite(pCon,"ERROR: BANG! Sample broken!",eError);
return HWFault;
}
if(BITSET(bits,4)){
if(BitSet(bits,4)){
SCWrite(pCon,"ERROR: hit positive limit switch",eError);
return HWFault;
}
if(BITSET(bits,5)){
if(BitSet(bits,5)){
SCWrite(pCon,"ERROR: hit negative limit switch",eError);
return HWFault;
}
if(BITSET(bits,7)){
SCWrite(pCon,"ERROR: general and overall error",eError);
if(BitSet(bits,6)){
SCWrite(pCon,"ERROR: max force exceed, manual reset REQUIRED!",eError);
return HWFault;
}
if(BITSET(bits,2)){
return HWBusy;
} else {
return HWIdle;
if(BitSet(bits,7)){
SCWrite(pCon,"ERROR: bit 7 error, ask Marcel what this means",eError);
return HWFault;
}
switch(self->iMode){
case FORCE:
if(BitSet(bits,2)){
return HWIdle;
} else {
return HWBusy;
}
break;
case ZUGPOS:
if(BitSet(bits,3)){
return HWIdle;
} else {
return HWBusy;
}
break;
}
return HWFault;
@ -200,15 +285,15 @@ static int PZRCheckStatus(void *data, SConnection *pCon){
------------------------------------------------------------------*/
static float PZRGetValue(void *data, SConnection *pCon){
pPZR self = NULL;
float val = -99999999.99;
float val = -9999.99;
int status;
char command[80], reply[80];
self = (pPZR)data;
if(self->iMode == FORCE){
snprintf(command,79,"rd force\n");
snprintf(command,79,"rd force\r\n");
} else {
snprintf(command,79,"rd vel\n");
snprintf(command,79,"rd pos\r\n");
}
status = PZRCommand(self->controller, pCon, command, reply, 79);
if(status != 1){
@ -217,6 +302,22 @@ static float PZRGetValue(void *data, SConnection *pCon){
sscanf(reply,"%f",&val);
return val;
}
/*---------------------------------------------------------------*/
static int PZRConnect(pPZR self, SConnection *pCon){
char error[132], buffer[356];
int status;
closeRS232(self->controller);
status = initRS232(self->controller);
if(status < 0){
getRS232Error(status, error, 131);
snprintf(buffer,255,"ERROR: failed to reconnect with: %s", error);
SCWrite(pCon,buffer,eError);
return 0;
}
SCSendOK(pCon);
return 1;
}
/*----------------------------------------------------------------
returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
@ -237,7 +338,7 @@ static pPZR PZRMakeObject(char *host, int port){
initRS232(self->controller);
setRS232SendTerminator(self->controller,"\n");
setRS232ReplyTerminator(self->controller,"\n");
setRS232Timeout(self->controller,1000);
setRS232Timeout(self->controller,10000);
setRS232Debug(self->controller,1);
@ -286,41 +387,81 @@ static int PoldiReissAction(SConnection *pCon, SicsInterp *pSics,
}
strtolower(argv[2]);
if(strcmp(argv[2],"force") == 0) {
status = PZRCommand(self->controller, pCon,
"wr mode 1\n", reply,79);
if(status == 1){
self->iMode == FORCE;
SCSendOK(pCon);
return 1;
} else {
return 0;
}
self->iMode = FORCE;
SCSendOK(pCon);
return 1;
} else if(strcmp(argv[2],"pos") == 0) {
status = PZRCommand(self->controller, pCon,
"wr mode 0\n", reply,79);
if(status == 1){
self->iMode == ZUGPOS;
SCSendOK(pCon);
return 1;
} else {
return 0;
}
self->iMode = ZUGPOS;
SCSendOK(pCon);
return 1;
} else {
SCWrite(pCon,"ERROR: zug mode unknown, allowed force, pos",
eError);
return 0;
}
}
} else {
if(self->iMode == FORCE){
snprintf(command,79,"%s.mode = force",argv[0]);
} else {
snprintf(command,79,"%s.mode = pos",argv[0]);
if(self->iMode == FORCE){
snprintf(command,79,"%s.mode = force",argv[0]);
} else {
snprintf(command,79,"%s.mode = pos",argv[0]);
}
SCWrite(pCon,command,eValue);
return 1;
}
} else if(strcmp(argv[1],"velocity") == 0) {
if(argc > 2){
if(!SCMatchRights(pCon,usUser)){
return 0;
}
self->velocity = atoi(argv[2]);
SCSendOK(pCon);
return 1;
} else {
snprintf(command,70,"%s.velocity = %d", argv[0], self->velocity);
SCWrite(pCon,command,eValue);
return 1;
}
} else if(strcmp(argv [1],"connect") == 0){
return PZRConnect(self,pCon);
} else if(strcmp(argv[1],"off") == 0) {
status = PZRCommand(self->controller, pCon, "wr mode 0\r\n", reply, 79);
self->state = OFF;
if(status == 1) {
SCSendOK(pCon);
return 1;
} else {
return 0;
}
} else if(strcmp(argv[1],"poslimits") == 0){
if(argc > 3){
if(!SCMatchRights(pCon,usMugger)){
return 0;
}
self->minPos = atof(argv[2]);
self->maxPos = atof(argv[3]);
SCSendOK(pCon);
return 1;
} else {
snprintf(command,80,"pzr.poslimits = %f,%f", self->minPos, self->maxPos);
SCWrite(pCon,command,eValue);
return 1;
}
} else if(strcmp(argv[1],"forcelimits") == 0){
if(argc > 3){
if(!SCMatchRights(pCon,usMugger)){
return 0;
}
self->minForce = atof(argv[2]);
self->maxForce = atof(argv[3]);
SCSendOK(pCon);
return 1;
} else {
snprintf(command,80,"pzr.forcelimits = %f,%f", self->minForce, self->maxForce);
SCWrite(pCon,command,eValue);
return 1;
}
SCWrite(pCon,command,eValue);
return 1;
}
}
}
val = self->pDriv->GetValue(self,pCon);
snprintf(command,79,"%s = %f", argv[0], val);
SCWrite(pCon,command,eValue);
@ -346,9 +487,12 @@ int MakePoldiReiss(SConnection *pCon, SicsInterp *pSics,
self->minForce = atof(argv[4]);
self->maxForce = atof(argv[5]);
self->minPos = atof(argv[6]);
self->maxPos = atof(argv[7]);
self->maxPos = atof(argv[7]);
self->iMode = FORCE;
self->state = OFF;
self->velocity = 10;
PZRCommand(self->controller, pCon,
"wr mode 1\n", reply,79);
"wr mode 0\r\n", reply,79);
AddCommand(pSics,argv[1],
PoldiReissAction,

7
psi.c
View File

@ -6,7 +6,7 @@
copyright: see file COPYRIGHT
Mark Koennecke, June 2003
Mark Koennecke, June 2003 - May 2007
-----------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
@ -69,6 +69,9 @@ extern int JulChoFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
/* from ritastorage.c */
extern int MakeRitaFix(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[]);
/* from sanslirebin.c */
extern int MakeSansliRebin(SConnection *pCon, SicsInterp *pSics, void *pData,
int argc, char *argv[]);
/*--------------------------------------------------------------------------*/
void SiteInit(void) {
@ -124,6 +127,7 @@ static void AddPsiCommands(SicsInterp *pInter){
AddCommand(pInter,"MakeJulCho",JulChoFactory,NULL,NULL);
AddCommand(pInter,"MakeRitaFix",MakeRitaFix,NULL,NULL);
AddCommand(pInter,"MakePoldiReiss",MakePoldiReiss,NULL,NULL);
AddCommand(pInter,"MakeSansliRebin",MakeSansliRebin,NULL,NULL);
/*
AddCommand(pInter,"MakeDifrac",MakeDifrac,NULL,NULL);
*/
@ -147,6 +151,7 @@ static void RemovePsiCommands(SicsInterp *pSics){
RemoveCommand(pSics,"MakeJulCho");
RemoveCommand(pSics,"MakeRitaFix");
RemoveCommand(pSics,"MakePoldiReiss");
RemoveCommand(pSics,"MakeSansliRebin");
/*
RemoveCommand(pSics,"MakeDifrac");
*/

142
rebin.c Normal file
View File

@ -0,0 +1,142 @@
/**
* This is a couple of functions to do rebinning. This is neeeded when
* you have a number of data points with random coordinates and you wish to
* get this back onto a regular grid for better handling.
*
* copyright: see file COPYRIGHT
*
* Mark Koennecke, May 2007
*/
#include <math.h>
#include "rebin.h"
/*---------------------------------------------------------------------------*/
static int checkPoint(pNXDS target, double x, double y){
int maxx, maxy;
maxx = getNXDatasetDim(target,0);
maxy = getNXDatasetDim(target,1);
if(x < .0 || y < .0 || x > maxx -1 || y > maxy -1) {
return 0;
}
return 1;
}
/*--------------------------------------------------------------------------*/
static void addNXDatasetValue(pNXDS target, int iPos[], double value){
double val;
val = value + getNXDatasetValue(target,iPos);
putNXDatasetValue(target,iPos,val);
}
/*---------------------------------------------------------------------------*/
static void roundPoint(pNXDS target, double x, double y, double value){
int iDim[2];
iDim[0] = (int)floor(x + .5);
iDim[1] = (int)floor(y + 0.5);
addNXDatasetValue(target,iDim,value);
}
/*----------------------------------------------------------------------------
* I am using the squares here, no need to do the sqrt which would give the
* actual distance.
* ---------------------------------------------------------------------------*/
static double dist(double x1, double y1, double x2, double y2){
double dx, dy;
dx = x1 - x2;
dy = y1 -y2;
return dx*dx + dy*dy;
}
/*-----------------------------------------------------------------------------
* This tries to distribute value over all of the four neighbouring points
* weighted with the distance to them.
-------------------------------------------------------------------------------*/
static void distribute(pNXDS target, double x, double y, double value){
double lldist, lrdist, uldist, urdist, totalDist, ix, iy, frac;
int iPos[2];
ix = floor(x);
iy = floor(y);
lldist = dist(x,y,ix,iy);
if(lldist < .01){
iPos[0] = ix;
iPos[1] = iy;
addNXDatasetValue(target,iPos,value);
return;
}
ix += 1;
lrdist = dist(x,y,ix,iy);
if(lrdist < .01){
iPos[0] = ix;
iPos[1] = iy;
addNXDatasetValue(target,iPos,value);
return;
}
iy += 1;
urdist = dist(x,y,ix,iy);
if(urdist < .01){
iPos[0] = ix;
iPos[1] = iy;
addNXDatasetValue(target,iPos,value);
return;
}
ix = floor(x);
uldist = dist(x,y,ix,iy);
if(uldist < .01){
iPos[0] = ix;
iPos[1] = iy;
addNXDatasetValue(target,iPos,value);
return;
}
lldist = 1./lldist;
lrdist = 1./lrdist;
urdist = 1./urdist;
uldist = 1./uldist;
totalDist = lldist + lrdist + urdist + uldist;
if(totalDist < .01){
iPos[0] = (int)floor(x);
iPos[1] = (int)floor(y);
addNXDatasetValue(target,iPos,value);
return;
}
iPos[0] = (int)floor(x);
iPos[1] = (int)floor(y);
frac = (lldist/totalDist)*value;
addNXDatasetValue(target,iPos,frac);
iPos[0] += 1;
frac = (lrdist/totalDist)*value;
addNXDatasetValue(target,iPos,frac);
iPos[1] += 1;
frac = (urdist/totalDist)*value;
addNXDatasetValue(target,iPos,frac);
iPos[0] = (int)floor(x);
frac = (uldist/totalDist)*value;
addNXDatasetValue(target,iPos,frac);
}
/*---------------------------------------------------------------------------*/
int rebinPoint2D(pNXDS target, double x, double y, double value, int method){
if(!checkPoint(target,x,y)){
return 0;
}
switch(method){
case TRILINEAR:
distribute(target,x,y,value);
break;
case ROUND:
roundPoint(target,x,y,value);
break;
default:
return 0;
}
return 1;
}

27
rebin.h Normal file
View File

@ -0,0 +1,27 @@
/**
* This is a couple of functions to do rebinning. This is neeeded when
* you have a number of data points with random coordinates and you wish to
* get this back onto a regular grid for better handling.
*
* copyright: see file COPYRIGHT
*
* Mark Koennecke, May 2007
*/
#ifndef REBIN_H_
#define REBIN_H_
#include <nxdataset.h>
#define TRILINEAR 1
#define ROUND 2
/**
* rebin a point
* @param target The target dataset to rebin too
* @param x The x coordiante of the point
* @param y The y coordinate of the point
* @param value The value of the point
* @param method The method to use for interpolation
* @return 1 on success, 0 else
*/
int rebinPoint2D(pNXDS target, double x, double y, double value, int method);
#endif /*REBIN_H_*/

71
sanslirebin.tc Normal file
View File

@ -0,0 +1,71 @@
/**
* A special module for doing SANSLI rebinning. This is a fudge to make
* the data from the new detector electronics look like the old one.
*
* copyright: see file COPYRIGHT
*
* Mark Koennecke, May 2007
*/
<%! source ../sicstemplates.tcl %>
<% stdIncludes %>
#include <math.h>
#include <sicsdata.h>
#include "rebin.h"
/*---------------------------------------------------------------------------*/
<% makeSicsFunc SansliRebin%>{
pSICSData target = NULL, hmData = NULL;
pNXDS dataset = NULL;
int iDim[2], ix, iy, pos, ival;
double x, y, val;
<% testNoPar 2 4 %>
hmData = (pSICSData)FindCommandData(pSics,argv[1],"SICSData");
if(hmData == NULL){
SCWrite(pCon,"ERROR: histogram memory data not found",eError);
return 0;
}
target = (pSICSData)FindCommandData(pSics,argv[2],"SICSData");
if(target == NULL){
SCWrite(pCon,"ERROR: target sicsdata not found",eError);
return 0;
}
iDim[0] = 128;
iDim[1] = 128;
dataset = createNXDataset(2,NX_FLOAT64, iDim);
if(dataset == NULL){
SCWrite(pCon,"ERROR: out of memory allocating temporary data",eError);
return 0;
}
for(ix = 113; ix < 415; ix++){
for(iy = 153; iy < 363; iy++){
x = ((double)ix - 113.)/2.359;
y = ((double)iy - 153.)/1.640;
getSICSDataInt(hmData,512*iy + ix,&ival);
val = (double)ival;
rebinPoint2D(dataset,x,y,val,TRILINEAR);
}
}
pos = 0;
clearSICSData(target);
if(argc > 3) {
setSICSDataInt(target,0,128);
setSICSDataInt(target,1,128);
pos = 2;
}
for(ix = 0; ix < 128*128; ix++, pos++){
setSICSDataInt(target,pos,(int)floor(dataset->u.dPtr[ix] +.5));
}
dropNXDataset(dataset);
SCSendOK(pCon);
return 1;
}
/*---------------------------------------------------------------------------*/
<% makeSicsFunc MakeSansliRebin%>{
AddCommand(pSics,"sanslirebin", SansliRebin,NULL,NULL);
SCSendOK(pCon);
return 1;
}

3
sinqhmdriv.c
View File

@ -839,11 +839,12 @@ pCon);
}
/* stop DAQ, if still active */
/*
if(iDaq == 1)
{
SINQHMStopDAQ(pInternal->pMaster);
}
*/
/* do only counter, histograms are read on demand */
if(pInternal->pCounter != NULL)

48
slsmagnet.c
View File

@ -14,6 +14,19 @@
Adaptiert auf neue DSP Software version, Mark Koennecke, August 2005
Fixed some bugs and added resetting of error conditions:
Mark Koennecke, May 2007
There is another scheme for resetting controller erros which might need
to be implemented:
1) switch the controller off
2) Write 0 to 0x21
3) switch on
4) drive to target
This reboots the controller. Lukas Tanner does not recommend to do this
automatically. This is why it is not implemented yet. This reboot anyway
is not as hard as a full power cycle.
Copyright: see copyright.h
----------------------------------------------------------------------------*/
#include <string.h>
@ -209,7 +222,7 @@ static int communicateSLS(mkChannel *pSock, char msg[6], char reply[6])
err = (int)reply[5];
if(err != 0){
pMe->iError = -7000 - err;
return pMe->iError;
return 0;
}
return 1;
@ -847,7 +860,7 @@ static void slsdspCodeToText(int code, char *text, int textlen){
{
if(msg[i] != reply[i])
{
pMe->iError = BADECHO;
pMe->iError = BADECHO;
return BADECHO;
}
}
@ -880,15 +893,42 @@ static void slsdspCodeToText(int code, char *text, int textlen){
static int SLSFix(pEVDriver self, int iError)
{
pSLSDriv pMe = NULL;
int iRet, i;
int iRet, i, ival;
char buffer[80];
char msg[6], reply[6];
float fVal = .0;
assert(self);
pMe = (pSLSDriv )self->pPrivate;
assert(pMe);
if(iError < -7000){
return DEVFAULT;
/*
* This logic tries to reset the error code. Then it reads the
* error back. Serious errors persist and must be fixed by magnet
* technicians. But some can be cleared....
*/
msg[0] = DSPWRITE;
msg[1] = 0x29;
ival = double2DSPfloat((double)fVal);
memcpy(msg+2, &ival,4);
communicateSLS(pMe->pSock,msg,reply);
/*
* read again: if persists: this is really, really bad
*/
msg[0] = DSPREAD;
msg[1] = 0x29;
iRet = communicateSLS(pMe->pSock,msg,reply);
if(iRet < 0)
{
return DEVFAULT;
}
ival = (int)reply[5];
if(ival != 0){
return DEVFAULT;
} else {
return DEVREDO;
}
}
switch(iError)

261
tabledrive.c
View File

@ -104,9 +104,9 @@ static int findOrientingMotor(pTableDrive self, ptdMotor moti){
}
/*-------------------------------------------------------------------------*/
static float locatePosition(tdMotor moti, SConnection *pCon){
int status;
int status, begin = 1;
float value;
double diff;
double diff;
tdEntry lower, upper;
status = MotorGetSoftPosition(moti.pMot,pCon,&value);
@ -116,36 +116,43 @@ static float locatePosition(tdMotor moti, SConnection *pCon){
status = LLDnodePtr2First(moti.table);
while(status != 0){
LLDnodeDataTo(moti.table,&lower);
status = LLDnodePtr2Next(moti.table);
if(status != 0) {
LLDnodeDataTo(moti.table,&upper);
} else {
/*
* end of table
*/
return lower.tablePos;
}
/**
* we have to deal with ascending and descending tables. This is
* why we have to have some funny logic here
*/
if(upper.position > lower.position){
/*
* ascending table
*/
if(value >= lower.position && value < upper.position){
diff = upper.position - lower.position;
return lower.tablePos + (value - lower.position)/diff;
}
} else {
/*
* descending table
*/
if(value <= lower.position && value > upper.position){
diff = lower.position - upper.position;
return lower.tablePos + ABS(value - lower.position)/diff;
}
}
status = LLDnodePtr2Next(moti.table);
if(status != 0) {
LLDnodeDataTo(moti.table,&upper);
} else {
/*
* end of table
*/
return lower.tablePos;
}
/**
* we have to deal with ascending and descending tables. This is
* why we have to have some funny logic here
*/
if(upper.position > lower.position){
/*
* ascending table
*/
if(value >= lower.position && value < upper.position){
diff = upper.position - lower.position;
return lower.tablePos + (value - lower.position)/diff;
}
if(begin == 1 && value < lower.position) {
return lower.tablePos;
}
} else {
/*
* descending table
*/
if(value <= lower.position && value > upper.position){
diff = lower.position - upper.position;
return lower.tablePos + ABS(value - lower.position)/diff;
}
if(begin == 1 && value > lower.position){
return lower.tablePos;
}
}
begin = 0;
}
return -999.99;
}
@ -339,10 +346,10 @@ static int runToNextStep(pTableDrive self, SConnection *pCon,
while(status != 0){
LLDnodeDataTo(self->motorTable,&moti);
target = findTarget(moti,value);
test = MotorRun(moti.pMot,pCon,target);
if(test != OKOK){
return test;
}
test = MotorRun(moti.pMot,pCon,target);
if(test != OKOK){
return test;
}
status = LLDnodePtr2Next(self->motorTable);
}
return 1;
@ -384,8 +391,13 @@ static void showPositions(pTableDrive self, SConnection *pCon,
status = LLDnodePtr2Next(self->motorTable);
}
}
/*-------------------------------------------------------------------------*/
static int TableDriveCheckStatus(void *pData, SConnection *pCon){
/*-------------------------------------------------------------------------
* This is the old scheme: drive table positions tablestep by tablestep.
* I leave this code here: may be it is useful in another scenario then
* MARS. Then some means must be found to select the table drive
* algorithm.
* -----------------------------------------------------------------------*/
static int TableDriveCheckStatusOld(void *pData, SConnection *pCon){
pTableDrive self = (pTableDrive)pData;
int status;
float step;
@ -464,6 +476,179 @@ static int TableDriveCheckStatus(void *pData, SConnection *pCon){
}
return HWFault;
}
/*===========================================================================
* This is code for the new scheme of driving MARS tables:
* 1) drive ds and as to 0
* 2) drive all other motors to targets directly
* 3) drive ds and as to desired positions.
* =========================================================================*/
#define WAITDSAS 110
#define WAITOTHER 111
#define ASDSTO0 200
#define ASDSTOTARGET 201
/*-------------------------------------------------------------------------*/
static int startASDS(pTableDrive self, SConnection *pCon, int target){
tdMotor moti;
int status;
char name[132];
float targetValue;
if(self == NULL || self->motorTable < 0){
return 0;
}
status = LLDnodePtr2First(self->motorTable);
while(status != 0){
LLDnodeDataTo(self->motorTable,&moti);
strncpy(name,moti.motorName,131);
strtolower(name);
if(strstr(name,"ds") != NULL || strstr(name,"as") != NULL){
if(target == ASDSTO0){
targetValue = 0.0;
} else {
targetValue = findTarget(moti, self->targetPosition);
}
status = MotorRun(moti.pMot,pCon,targetValue);
if(status != OKOK){
return status;
}
}
status = LLDnodePtr2Next(self->motorTable);
}
return OKOK;
}
/*-------------------------------------------------------------------------*/
static int startOthers(pTableDrive self, SConnection *pCon){
tdMotor moti;
int status;
char name[132];
float targetValue;
if(self == NULL || self->motorTable < 0){
return 0;
}
status = LLDnodePtr2First(self->motorTable);
while(status != 0){
LLDnodeDataTo(self->motorTable,&moti);
strncpy(name,moti.motorName,131);
strtolower(name);
if(strstr(name,"ds") == NULL && strstr(name,"as") == NULL){
targetValue = findTarget(moti, self->targetPosition);
status = MotorRun(moti.pMot,pCon,targetValue);
if(status != OKOK){
return status;
}
}
status = LLDnodePtr2Next(self->motorTable);
}
return OKOK;
}
/*-------------------------------------------------------------------------*/
static int TableDriveCheckStatus(void *pData, SConnection *pCon){
pTableDrive self = (pTableDrive)pData;
int status;
float step;
char pBueffel[1024];
switch(self->state){
case STARTING:
/*
* make sure that our current position is up-to-date
*/
TableDriveGetValue(self,pCon);
liberateMotors(self,pCon);
status = startASDS(self,pCon,ASDSTO0);
if(status != OKOK){
TableDriveHalt(self);
return HWFault;
}
self->state = WAITDSAS;
return HWBusy;
break;
case WAITDSAS:
status = checkRunning(self,pCon);
switch(status){
case HWIdle:
case OKOK:
if(self->debug > 0){
showPositions(self,pCon,pBueffel,1023);
SCWrite(pCon,pBueffel,eValue);
}
status = startOthers(self,pCon);
if(status != OKOK){
TableDriveHalt(self);
return HWFault;
}
self->state = WAITOTHER;
return HWBusy;
break;
case HWBusy:
return HWBusy;
break;
default:
TableDriveHalt(self);
self->state = WAITFINISH;
return HWBusy;
break;
}
break;
case WAITOTHER:
status = checkRunning(self,pCon);
switch(status){
case HWIdle:
case OKOK:
if(self->debug > 0){
showPositions(self,pCon,pBueffel,1023);
SCWrite(pCon,pBueffel,eValue);
}
status = startASDS(self,pCon,ASDSTOTARGET);
if(status != OKOK){
TableDriveHalt(self);
return HWFault;
}
self->state = WAITFINISH;
return HWBusy;
break;
case HWBusy:
return HWBusy;
break;
default:
TableDriveHalt(self);
self->state = WAITFINISH;
return HWBusy;
break;
}
break;
case WAITFINISH:
status = checkRunning(self,pCon);
if(status != HWBusy){
if(self->debug > 0){
showPositions(self,pCon,pBueffel,1023);
SCWrite(pCon,pBueffel,eValue);
}
TableDriveGetValue(self,pCon);
closeMotors(self,pCon);
self->state = IDLE;
return status;
} else {
return HWBusy;
}
break;
case OUTOFSYNC:
SCWrite(pCon,"WARNING: tabledrive out of sync",eWarning);
self->state = IDLE;
return HWFault;
break;
default:
SCWrite(pCon,
"ERROR: programming error in tabledrive, invalid state",
eError);
return HWFault;
break;
}
return HWFault;
}
/*================== live and death ========================================*/
static void *TableDriveInterface(void *pData, int ID){
pTableDrive self = (pTableDrive)pData;