- Adapted indenation to new agreed upon system
- Fixed bad status in poldi zug driver
This commit is contained in:
koennecke
852
poldizug.c
852
poldizug.c
@@ -11,7 +11,7 @@
|
||||
* - May 2007(The device was actually available)
|
||||
*/
|
||||
#include <sics.h>
|
||||
#include <rs232controller.h>
|
||||
#include <rs232controller.h>
|
||||
/*===============================================================*/
|
||||
#define FORCE 0
|
||||
#define ZUGPOS 1
|
||||
@@ -19,113 +19,117 @@
|
||||
#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;
|
||||
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,
|
||||
char *command,
|
||||
char *reply, int replyLen){
|
||||
int status, i;
|
||||
char buffer[512], error[132];
|
||||
|
||||
/*
|
||||
* try three times to get our message through...
|
||||
*/
|
||||
for(i = 0; i < 3; i++){
|
||||
status = transactRS232(controller,command, strlen(command),
|
||||
reply, replyLen);
|
||||
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;
|
||||
static int PZRCommand(prs232 controller, SConnection * pCon,
|
||||
char *command, char *reply, int replyLen)
|
||||
{
|
||||
int status, i;
|
||||
char buffer[512], error[132];
|
||||
|
||||
/*
|
||||
* try three times to get our message through...
|
||||
*/
|
||||
for (i = 0; i < 3; i++) {
|
||||
status = transactRS232(controller, command, strlen(command),
|
||||
reply, replyLen);
|
||||
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;
|
||||
}
|
||||
/*
|
||||
* error processing
|
||||
*/
|
||||
memset(error,0,132);
|
||||
getRS232Error(status,error,131);
|
||||
status = fixRS232Error(controller,status);
|
||||
if(pCon != NULL){
|
||||
if(status == 1){
|
||||
snprintf(buffer,511,"WARNING: trying to fix: %s",
|
||||
error);
|
||||
SCWrite(pCon,buffer,eWarning);
|
||||
} else {
|
||||
snprintf(buffer,511,"ERROR: %s",
|
||||
error);
|
||||
SCWrite(pCon,buffer,eError);
|
||||
}
|
||||
}
|
||||
if(status == 0){
|
||||
return HWFault;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
return HWFault;
|
||||
/*
|
||||
* error processing
|
||||
*/
|
||||
memset(error, 0, 132);
|
||||
getRS232Error(status, error, 131);
|
||||
status = fixRS232Error(controller, status);
|
||||
if (pCon != NULL) {
|
||||
if (status == 1) {
|
||||
snprintf(buffer, 511, "WARNING: trying to fix: %s", error);
|
||||
SCWrite(pCon, buffer, eWarning);
|
||||
} else {
|
||||
snprintf(buffer, 511, "ERROR: %s", error);
|
||||
SCWrite(pCon, buffer, eError);
|
||||
}
|
||||
}
|
||||
if (status == 0) {
|
||||
return HWFault;
|
||||
}
|
||||
}
|
||||
return HWFault;
|
||||
}
|
||||
|
||||
/*===============================================================*/
|
||||
static void *PZRGetInterface(void *data, int iD){
|
||||
pPZR self = NULL;
|
||||
|
||||
self = (pPZR)data;
|
||||
if(self != NULL && iD == DRIVEID){
|
||||
return self->pDriv;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
return NULL;
|
||||
static void *PZRGetInterface(void *data, int iD)
|
||||
{
|
||||
pPZR self = NULL;
|
||||
|
||||
self = (pPZR) data;
|
||||
if (self != NULL && iD == DRIVEID) {
|
||||
return self->pDriv;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return either OKOK or HWFault when thing
|
||||
go wrong. However, the return value of Halt is usually ignored!
|
||||
------------------------------------------------------------------*/
|
||||
static int PZRHalt(void *data) {
|
||||
pPZR self = NULL;
|
||||
char buffer[80];
|
||||
|
||||
self = (pPZR)data;
|
||||
static int PZRHalt(void *data)
|
||||
{
|
||||
pPZR self = NULL;
|
||||
char buffer[80];
|
||||
|
||||
PZRCommand(self->controller,NULL,"stop\r\n",buffer,79);
|
||||
self = (pPZR) data;
|
||||
|
||||
return OKOK;
|
||||
PZRCommand(self->controller, NULL, "stop\r\n", buffer, 79);
|
||||
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return either 1 or 0. 1 means the position can
|
||||
be reached, 0 NOT
|
||||
If 0, error shall contain up to errlen characters of information
|
||||
about which limit was violated
|
||||
------------------------------------------------------------------*/
|
||||
static int PZRCheckLimits(void *data, float val,
|
||||
char *error, int errlen){
|
||||
pPZR self = NULL;
|
||||
|
||||
self = (pPZR)data;
|
||||
if(self->iMode == FORCE){
|
||||
if(val < self->minForce || val > self->maxForce){
|
||||
snprintf(error,errlen,"%f is not within limits %f to %f",
|
||||
val,self->minForce, self->maxForce);
|
||||
return 0;
|
||||
}
|
||||
} else {
|
||||
if(val < self->minPos || val > self->maxPos){
|
||||
snprintf(error,errlen,"%f is not within limits %f to %f",
|
||||
val,self->minPos, self->maxPos);
|
||||
return 0;
|
||||
}
|
||||
static int PZRCheckLimits(void *data, float val, char *error, int errlen)
|
||||
{
|
||||
pPZR self = NULL;
|
||||
|
||||
self = (pPZR) data;
|
||||
if (self->iMode == FORCE) {
|
||||
if (val < self->minForce || val > self->maxForce) {
|
||||
snprintf(error, errlen, "%f is not within limits %f to %f",
|
||||
val, self->minForce, self->maxForce);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
} else {
|
||||
if (val < self->minPos || val > self->maxPos) {
|
||||
snprintf(error, errlen, "%f is not within limits %f to %f",
|
||||
val, self->minPos, self->maxPos);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
This routine can return 0 when a limit problem occurred
|
||||
OKOK when the motor was successfully started
|
||||
@@ -135,62 +139,66 @@ static int PZRCheckLimits(void *data, float val,
|
||||
to start the motor in question
|
||||
val is the value to drive the motor too
|
||||
------------------------------------------------------------------*/
|
||||
static long PZRSetValue(void *data, SConnection *pCon, float val){
|
||||
pPZR self = NULL;
|
||||
char command[80], reply[80];
|
||||
int status;
|
||||
|
||||
self = (pPZR)data;
|
||||
static long PZRSetValue(void *data, SConnection * pCon, float val)
|
||||
{
|
||||
pPZR self = NULL;
|
||||
char command[80], reply[80];
|
||||
int status;
|
||||
|
||||
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;
|
||||
break;
|
||||
self = (pPZR) data;
|
||||
|
||||
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;
|
||||
}
|
||||
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;
|
||||
break;
|
||||
}
|
||||
return OKOK;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
Checks the status of a running motor. Possible return values
|
||||
HWBusy The motor is still running
|
||||
@@ -201,302 +209,320 @@ 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 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, i;
|
||||
char command[80], reply[80], bits;
|
||||
|
||||
self = (pPZR)data;
|
||||
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 = (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,4)){
|
||||
SCWrite(pCon,"ERROR: hit positive limit switch",eError);
|
||||
return HWFault;
|
||||
}
|
||||
if(BitSet(bits,5)){
|
||||
SCWrite(pCon,"ERROR: hit negative limit switch",eError);
|
||||
return HWFault;
|
||||
}
|
||||
if(BitSet(bits,6)){
|
||||
SCWrite(pCon,"ERROR: max force exceed, manual reset REQUIRED!",eError);
|
||||
return HWFault;
|
||||
}
|
||||
if(BitSet(bits,7)){
|
||||
SCWrite(pCon,"ERROR: bit 7 error, ask Marcel what this means",eError);
|
||||
return HWFault;
|
||||
}
|
||||
static int PZRCheckStatus(void *data, SConnection * pCon)
|
||||
{
|
||||
pPZR self = NULL;
|
||||
int status, code, i;
|
||||
char command[80], reply[80], bits;
|
||||
|
||||
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;
|
||||
}
|
||||
self = (pPZR) data;
|
||||
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 = (char) code;
|
||||
|
||||
return HWFault;
|
||||
/*
|
||||
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, 4)) {
|
||||
SCWrite(pCon, "ERROR: hit positive limit switch", eError);
|
||||
return HWFault;
|
||||
}
|
||||
if (BitSet(bits, 5)) {
|
||||
SCWrite(pCon, "ERROR: hit negative limit switch", eError);
|
||||
return HWFault;
|
||||
}
|
||||
if (BitSet(bits, 6)) {
|
||||
SCWrite(pCon, "ERROR: max force exceed, manual reset REQUIRED!",
|
||||
eError);
|
||||
return HWFault;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------
|
||||
GetValue is supposed to read a motor position
|
||||
On errors, -99999999.99 is returned and messages printed to pCon
|
||||
------------------------------------------------------------------*/
|
||||
static float PZRGetValue(void *data, SConnection *pCon){
|
||||
pPZR self = NULL;
|
||||
float val = -9999.99;
|
||||
int status;
|
||||
char command[80], reply[80];
|
||||
|
||||
self = (pPZR)data;
|
||||
if(self->iMode == FORCE){
|
||||
snprintf(command,79,"rd force\r\n");
|
||||
} else {
|
||||
snprintf(command,79,"rd pos\r\n");
|
||||
}
|
||||
status = PZRCommand(self->controller, pCon, command, reply, 79);
|
||||
if(status != 1){
|
||||
return HWFault;
|
||||
}
|
||||
sscanf(reply,"%f",&val);
|
||||
return val;
|
||||
}
|
||||
/*---------------------------------------------------------------*/
|
||||
static int PZRConnect(pPZR self, SConnection *pCon){
|
||||
char error[132], buffer[356];
|
||||
int status;
|
||||
static float PZRGetValue(void *data, SConnection * pCon)
|
||||
{
|
||||
pPZR self = NULL;
|
||||
float val = -9999.99;
|
||||
int status;
|
||||
char command[80], reply[80];
|
||||
|
||||
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;
|
||||
self = (pPZR) data;
|
||||
if (self->iMode == FORCE) {
|
||||
snprintf(command, 79, "rd force\r\n");
|
||||
} else {
|
||||
snprintf(command, 79, "rd pos\r\n");
|
||||
}
|
||||
status = PZRCommand(self->controller, pCon, command, reply, 79);
|
||||
if (status != 1) {
|
||||
return HWFault;
|
||||
}
|
||||
sscanf(reply, "%f", &val);
|
||||
return val;
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------*/
|
||||
static int PZRConnect(pPZR self, SConnection * pCon)
|
||||
{
|
||||
char error[132], buffer[356];
|
||||
int status;
|
||||
|
||||
closeRS232(self->controller);
|
||||
self->state = OFF;
|
||||
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
|
||||
------------------------------------------------------------------*/
|
||||
static pPZR PZRMakeObject(char *host, int port){
|
||||
pPZR self = NULL;
|
||||
static pPZR PZRMakeObject(char *host, int port)
|
||||
{
|
||||
pPZR self = NULL;
|
||||
|
||||
self = malloc(sizeof(PZR));
|
||||
if(self == NULL){
|
||||
return NULL;
|
||||
}
|
||||
self->pDes = CreateDescriptor("PoldiReiss");
|
||||
self->pDriv = CreateDrivableInterface();
|
||||
self->controller = createRS232(host,port);
|
||||
if(self->pDes == NULL || self->pDriv == NULL || self->controller == NULL){
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
initRS232(self->controller);
|
||||
setRS232SendTerminator(self->controller,"\n");
|
||||
setRS232ReplyTerminator(self->controller,"\n");
|
||||
setRS232Timeout(self->controller,10000);
|
||||
setRS232Debug(self->controller,1);
|
||||
|
||||
|
||||
self->pDes->GetInterface = PZRGetInterface;
|
||||
self->pDriv->Halt = PZRHalt;
|
||||
self->pDriv->CheckLimits = PZRCheckLimits;
|
||||
self->pDriv->SetValue = PZRSetValue;
|
||||
self->pDriv->CheckStatus = PZRCheckStatus;
|
||||
self->pDriv->GetValue = PZRGetValue;
|
||||
|
||||
return self;
|
||||
}
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static void killPoldiZug(void *data){
|
||||
pPZR self = (pPZR)data;
|
||||
|
||||
if(self == NULL){
|
||||
return;
|
||||
}
|
||||
if(self->pDes != NULL){
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
if(self->controller != NULL){
|
||||
closeRS232(self->controller);
|
||||
KillRS232(self->controller);
|
||||
}
|
||||
self = malloc(sizeof(PZR));
|
||||
if (self == NULL) {
|
||||
return NULL;
|
||||
}
|
||||
self->pDes = CreateDescriptor("PoldiReiss");
|
||||
self->pDriv = CreateDrivableInterface();
|
||||
self->controller = createRS232(host, port);
|
||||
if (self->pDes == NULL || self->pDriv == NULL
|
||||
|| self->controller == NULL) {
|
||||
free(self);
|
||||
return NULL;
|
||||
}
|
||||
initRS232(self->controller);
|
||||
setRS232SendTerminator(self->controller, "\n");
|
||||
setRS232ReplyTerminator(self->controller, "\n");
|
||||
setRS232Timeout(self->controller, 10000);
|
||||
setRS232Debug(self->controller, 1);
|
||||
|
||||
|
||||
self->pDes->GetInterface = PZRGetInterface;
|
||||
self->pDriv->Halt = PZRHalt;
|
||||
self->pDriv->CheckLimits = PZRCheckLimits;
|
||||
self->pDriv->SetValue = PZRSetValue;
|
||||
self->pDriv->CheckStatus = PZRCheckStatus;
|
||||
self->pDriv->GetValue = PZRGetValue;
|
||||
|
||||
return self;
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------------------------*/
|
||||
static void killPoldiZug(void *data)
|
||||
{
|
||||
pPZR self = (pPZR) data;
|
||||
|
||||
if (self == NULL) {
|
||||
return;
|
||||
}
|
||||
if (self->pDes != NULL) {
|
||||
DeleteDescriptor(self->pDes);
|
||||
}
|
||||
if (self->controller != NULL) {
|
||||
closeRS232(self->controller);
|
||||
KillRS232(self->controller);
|
||||
}
|
||||
free(self);
|
||||
}
|
||||
|
||||
/*============================================================================*/
|
||||
static int PoldiReissAction(SConnection *pCon, SicsInterp *pSics,
|
||||
void *pData, int argc, char *argv[]){
|
||||
pPZR self = NULL;
|
||||
char command[80], reply[80];
|
||||
float val;
|
||||
int status;
|
||||
|
||||
self = (pPZR)pData;
|
||||
assert(self != NULL);
|
||||
|
||||
if(argc > 1) {
|
||||
strtolower(argv[1]);
|
||||
if(strcmp(argv[1],"mode") == 0) {
|
||||
if(argc > 2){
|
||||
if(!SCMatchRights(pCon,usUser)){
|
||||
return 0;
|
||||
}
|
||||
strtolower(argv[2]);
|
||||
if(strcmp(argv[2],"force") == 0) {
|
||||
self->iMode = FORCE;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
} else if(strcmp(argv[2],"pos") == 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]);
|
||||
}
|
||||
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;
|
||||
}
|
||||
static int PoldiReissAction(SConnection * pCon, SicsInterp * pSics,
|
||||
void *pData, int argc, char *argv[])
|
||||
{
|
||||
pPZR self = NULL;
|
||||
char command[80], reply[80];
|
||||
float val;
|
||||
int status;
|
||||
|
||||
self = (pPZR) pData;
|
||||
assert(self != NULL);
|
||||
|
||||
if (argc > 1) {
|
||||
strtolower(argv[1]);
|
||||
if (strcmp(argv[1], "mode") == 0) {
|
||||
if (argc > 2) {
|
||||
if (!SCMatchRights(pCon, usUser)) {
|
||||
return 0;
|
||||
}
|
||||
strtolower(argv[2]);
|
||||
if (strcmp(argv[2], "force") == 0) {
|
||||
self->iMode = FORCE;
|
||||
SCSendOK(pCon);
|
||||
return 1;
|
||||
} else if (strcmp(argv[2], "pos") == 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]);
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
val = self->pDriv->GetValue(self,pCon);
|
||||
snprintf(command,79,"%s = %f", argv[0], val);
|
||||
SCWrite(pCon,command,eValue);
|
||||
return 1;
|
||||
}
|
||||
val = self->pDriv->GetValue(self, pCon);
|
||||
snprintf(command, 79, "%s = %f", argv[0], val);
|
||||
SCWrite(pCon, command, eValue);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------------
|
||||
* MakePoldiReiss name host port minForce maxForce minPos maxPos
|
||||
* ---------------------------------------------------------------------------*/
|
||||
int MakePoldiReiss(SConnection *pCon, SicsInterp *pSics,
|
||||
void *pData, int argc, char *argv[]){
|
||||
pPZR self = NULL;
|
||||
char reply[80];
|
||||
|
||||
if(argc < 8) {
|
||||
SCWrite(pCon,"ERROR: not enough arguments to MakePoldiReiss",eError);
|
||||
return 0;
|
||||
}
|
||||
self = PZRMakeObject(argv[2],atoi(argv[3]));
|
||||
if(self == NULL){
|
||||
SCWrite(pCon,"ERROR: out of memory making PoldiReiss",eError);
|
||||
return 0;
|
||||
}
|
||||
self->minForce = atof(argv[4]);
|
||||
self->maxForce = atof(argv[5]);
|
||||
self->minPos = atof(argv[6]);
|
||||
self->maxPos = atof(argv[7]);
|
||||
self->iMode = FORCE;
|
||||
self->state = OFF;
|
||||
self->velocity = 10;
|
||||
PZRCommand(self->controller, pCon,
|
||||
"wr mode 0\r\n", reply,79);
|
||||
int MakePoldiReiss(SConnection * pCon, SicsInterp * pSics,
|
||||
void *pData, int argc, char *argv[])
|
||||
{
|
||||
pPZR self = NULL;
|
||||
char reply[80];
|
||||
|
||||
AddCommand(pSics,argv[1],
|
||||
PoldiReissAction,
|
||||
killPoldiZug,
|
||||
self);
|
||||
return 1;
|
||||
if (argc < 8) {
|
||||
SCWrite(pCon, "ERROR: not enough arguments to MakePoldiReiss", eError);
|
||||
return 0;
|
||||
}
|
||||
self = PZRMakeObject(argv[2], atoi(argv[3]));
|
||||
if (self == NULL) {
|
||||
SCWrite(pCon, "ERROR: out of memory making PoldiReiss", eError);
|
||||
return 0;
|
||||
}
|
||||
self->minForce = atof(argv[4]);
|
||||
self->maxForce = atof(argv[5]);
|
||||
self->minPos = atof(argv[6]);
|
||||
self->maxPos = atof(argv[7]);
|
||||
self->iMode = FORCE;
|
||||
self->state = OFF;
|
||||
self->velocity = 10;
|
||||
PZRCommand(self->controller, pCon, "wr mode 0\r\n", reply, 79);
|
||||
|
||||
AddCommand(pSics, argv[1], PoldiReissAction, killPoldiZug, self);
|
||||
return 1;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user