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

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,