lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

Start

Browse Source
This commit is contained in:
Ron Sluiter
2004-06-07 19:27:05 +00:00
parent 091c4f54bd
commit 4a8a39ab7f
6 changed files with 1453 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/DeltaTauSrc/Makefile
+19
View File
@@ -0,0 +1,19 @@
# Makefile
TOP = ../..
include $(TOP)/configure/CONFIG
#----------------------------------------
# ADD MACRO DEFINITIONS AFTER THIS LINE
# The following are used for debugging messages.
USR_CXXFLAGS += -DDEBUG
DBD += devDeltaTau.dbd
LIBRARY_IOC_vxWorks = DeltaTau
SRCS += devPmac.cc drvPmac.cc
include $(TOP)/configure/RULES
#----------------------------------------
# ADD RULES AFTER THIS LINE
motorApp/DeltaTauSrc/README
+38
View File
@@ -0,0 +1,38 @@
Delta Tau PMAC
==============
Notation: "$" denotes hexadecimal number.
Configuring PMAC
----------------
- Set the "VMEbus Address Modifier" (I90):
- $39 for A24 addressing (default).
- $9 for A32 addressing.
- Enable both "supervisory" and "non-privileged" VMEbus data access by setting
the "VMEbus Address Modifier Don't Care Bits" (I91) to $4 (default).
- For A32 addressing only, set the "VME Base Address Bits A31-A24" (I92) to the
most significant hex nibble; e.g. for $A123 0000 set I92 to $A1. The
default value ($FF) is N/A.
- Leave the "VME Mailbox Address Bits A23-A16" (I93) at the default value
($7F) so it does not conflict with the DPRAM.
- Leave the "VME Mailbox Address Bits A15-A08" (I94) at the default value
($A0) so it does not conflict with the DPRAM.
- Set
- Set the "VME DPRAM Base Address Bits A23-A20" (I97); for example,
- for A32 addressing, at base address $A123 0000 set I97 to $23.
- for A24 addressing, at base address $00D0 0000 set I97 to $D0.
The default value is $0.
- Set the "VME DPRAM Enable" (I98) to $E0 to enable DPRAM access; the default
value is $60.
- Set the "VME Address Width Control" (I99) to:
- $90 for A24 addressing.
- $80 for A32 addressing.
The default value is $30.
- Enable DPRAM ASCII communication; set I58 to 1.
motorApp/DeltaTauSrc/devDeltaTau.dbd
+4
View File
@@ -0,0 +1,4 @@
# Oregon Micro Systems VME8/44 and VME58 Driver support.
device(motor,VME_IO,devPmac,"PMAC")
driver(drvPmac)
motorApp/DeltaTauSrc/devPmac.cc
+298
View File
@@ -0,0 +1,298 @@
/*
FILENAME... devPmac.cc
USAGE... Device level support for Delta Tau PMAC.
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-06-07 19:27:05 $
*/
/*
* Original Author: Ron Sluiter
* Date: 04/16/04
* Current Author: Ron Sluiter
*
* Experimental Physics and Industrial Control System (EPICS)
*
* Copyright 1991, the Regents of the University of California,
* and the University of Chicago Board of Governors.
*
* This software was produced under U.S. Government contracts:
* (W-7405-ENG-36) at the Los Alamos National Laboratory,
* and (W-31-109-ENG-38) at Argonne National Laboratory.
*
* Initial development by:
* The Controls and Automation Group (AT-8)
* Ground Test Accelerator
* Accelerator Technology Division
* Los Alamos National Laboratory
*
* Co-developed with
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
* 01 rls 04/16/04 Copied from devOms.cc
*/
#include <string.h>
#include "motorRecord.h"
#include "motor.h"
#include "motordevCom.h"
#include "drvPmac.h"
#include "epicsExport.h"
extern int Pmac_num_cards;
extern struct driver_table Pmac_access;
/* ----------------Create the dsets for devOMS----------------- */
static long Pmac_init(void *);
static long Pmac_init_record(void *);
static long Pmac_start_trans(struct motorRecord *);
static RTN_STATUS Pmac_build_trans(motor_cmnd, double *, struct motorRecord *);
static RTN_STATUS Pmac_end_trans(struct motorRecord *);
struct motor_dset devPmac =
{
{8, NULL, Pmac_init, Pmac_init_record, NULL},
motor_update_values,
Pmac_start_trans,
Pmac_build_trans,
Pmac_end_trans
};
epicsExportAddress(dset,devPmac);
/* This table is used to define the command types */
/* WARNING! this must match "motor_cmnd" in motor.h */
static msg_types Pmac_table[] = {
MOTION, /* MOVE_ABS */
MOTION, /* MOVE_REL */
MOTION, /* HOME_FOR */
MOTION, /* HOME_REV */
IMMEDIATE, /* LOAD_POS */
IMMEDIATE, /* SET_VEL_BASE */
IMMEDIATE, /* SET_VELOCITY */
IMMEDIATE, /* SET_ACCEL */
IMMEDIATE, /* GO */
IMMEDIATE, /* SET_ENC_RATIO */
INFO, /* GET_INFO */
MOVE_TERM, /* STOP_AXIS */
VELOCITY, /* JOG */
IMMEDIATE, /* SET_PGAIN */
IMMEDIATE, /* SET_IGAIN */
IMMEDIATE, /* SET_DGAIN */
IMMEDIATE, /* ENABLE_TORQUE */
IMMEDIATE, /* DISABL_TORQUE */
IMMEDIATE, /* PRIMITIVE */
IMMEDIATE, /* SET_HIGH_LIMIT */
IMMEDIATE, /* SET_LOW_LIMIT */
VELOCITY /* JOG_VELOCITY */
};
static struct board_stat **Pmac_cards;
static const char errmsg[] = {"\n\n!!!ERROR!!! - Oms driver uninitialized.\n"};
static long Pmac_init(void *arg)
{
int after = (int) arg;
if (*(Pmac_access.init_indicator) == NO)
{
errlogSevPrintf(errlogMinor, "%s", errmsg);
return(ERROR);
}
else
return(motor_init_com(after, Pmac_num_cards, &Pmac_access, &Pmac_cards));
}
static long Pmac_init_record(void *arg)
{
struct motorRecord *mr = (struct motorRecord *) arg;
return(motor_init_record_com(mr, Pmac_num_cards, &Pmac_access, Pmac_cards));
}
static long Pmac_start_trans(struct motorRecord *mr)
{
long rtnval;
rtnval = motor_start_trans_com(mr, Pmac_cards);
return(rtnval);
}
static RTN_STATUS Pmac_end_trans(struct motorRecord *mr)
{
if (*(Pmac_access.init_indicator) == NO)
{
errlogSevPrintf(errlogMinor, "%s", errmsg);
return(ERROR);
}
else
return(motor_end_trans_com(mr, &Pmac_access));
}
/* add a part to the transaction */
static RTN_STATUS Pmac_build_trans(motor_cmnd command, double *parms, struct motorRecord *mr)
{
struct motor_trans *trans = (struct motor_trans *) mr->dpvt;
struct mess_node *motor_call;
struct controller *brdptr;
char buff[110];
int axis, card, intval;
unsigned int size;
RTN_STATUS rtnval;
bool send;
msta_field msta;
send = true; /* Default to send motor command. */
rtnval = OK;
buff[0] = '\0';
/* Protect against NULL pointer with WRTITE_MSG(GO/STOP_AXIS/GET_INFO, NULL). */
intval = (parms == NULL) ? 0 : NINT(parms[0]);
msta.All = mr->msta;
motor_call = &(trans->motor_call);
card = motor_call->card;
axis = motor_call->signal + 1;
brdptr = (*trans->tabptr->card_array)[card];
if (brdptr == NULL)
return(rtnval = ERROR);
if (Pmac_table[command] > motor_call->type)
motor_call->type = Pmac_table[command];
if (trans->state != BUILD_STATE)
return(rtnval = ERROR);
if (command == PRIMITIVE && mr->init != NULL && strlen(mr->init) != 0)
{
strcat(motor_call->message, "? ");
strcat(motor_call->message, mr->init);
}
switch (command)
{
case MOVE_ABS:
case MOVE_REL:
case HOME_FOR:
case HOME_REV:
case JOG:
if (strlen(mr->prem) != 0)
{
strcat(motor_call->message, mr->prem);
strcat(motor_call->message, " ");
}
if (strlen(mr->post) != 0)
motor_call->postmsgptr = (char *) &mr->post;
break;
default:
break;
}
switch (command)
{
case MOVE_ABS:
sprintf(buff, "?MA %d", intval);
break;
case MOVE_REL:
sprintf(buff, "?MR %d", intval);
break;
case HOME_FOR:
sprintf(buff, "? F1000 0");
break;
case HOME_REV:
sprintf(buff, "? F1000 1");
break;
case LOAD_POS: /* Use "Position Bias". */
sprintf(buff, "M%.2d64=%d", axis, intval);
break;
case SET_VEL_BASE:
sprintf(buff, "?VI %d", intval);
break;
case SET_VELOCITY:
sprintf(buff, "?VM %d", intval);
break;
case SET_ACCEL:
sprintf(buff, "?A=%d", intval);
break;
case GO:
/* The MDrive starts moving immediately on move commands, GO command
* does nothing. Use it to set ACCL = DECL. */
sprintf(buff, "?A=D");
break;
case PRIMITIVE:
case GET_INFO:
/* These commands are not actually done by sending a message, but
rather they will indirectly cause the driver to read the status
of all motors */
break;
case STOP_AXIS:
sprintf(buff, "?SL 0");
break;
case JOG_VELOCITY:
sprintf(buff, "I%.2d22=%d", axis, intval);
break;
case JOG:
if (intval >= 0)
sprintf(buff, "#%.2dJ+", axis);
else
sprintf(buff, "#%.2dJ-", axis);
break;
case SET_PGAIN:
case SET_IGAIN:
case SET_DGAIN:
send = false;
break;
case ENABLE_TORQUE:
sprintf(buff, "?DE=1");
break;
case DISABL_TORQUE:
sprintf(buff, "?DE=0");
break;
case SET_HIGH_LIMIT:
case SET_LOW_LIMIT:
case SET_ENC_RATIO:
trans->state = IDLE_STATE; /* No command sent to the controller. */
send = false;
break;
default:
send = false;
rtnval = ERROR;
}
size = strlen(buff);
if (send == false)
return(rtnval);
else if (size > sizeof(buff) || (strlen(motor_call->message) + size) > MAX_MSG_SIZE)
errlogMessage("MDrive_build_trans(): buffer overflow.\n");
else
strcat(motor_call->message, buff);
return(rtnval);
}
motorApp/DeltaTauSrc/drvPmac.cc
+877
View File
@@ -0,0 +1,877 @@
/*
FILENAME... drvPmac.cc
USAGE... Driver level support for Delta Tau PMAC model.
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-06-07 19:27:05 $
*/
/*
* Experimental Physics and Industrial Control System (EPICS)
*
* Copyright 1991, the Regents of the University of California,
* and the University of Chicago Board of Governors.
*
* This software was produced under U.S. Government contracts:
* (W-7405-ENG-36) at the Los Alamos National Laboratory,
* and (W-31-109-ENG-38) at Argonne National Laboratory.
*
* Initial development by:
* The Controls and Automation Group (AT-8)
* Ground Test Accelerator
* Accelerator Technology Division
* Los Alamos National Laboratory
*
* Co-developed with
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
*
* NOTES
* -----
* Verified with firmware:
*
* Modification Log:
* -----------------
* .00 04/17/04 rls - Copied from drvOms.cc
*/
#include <vxLib.h>
#include <sysLib.h>
#include <string.h>
#include <rebootLib.h>
#include <logLib.h>
#include <drvSup.h>
#include <epicsVersion.h>
#if EPICS_MODIFICATION <= 4
extern "C" {
#include <devLib.h>
}
#else
#include <devLib.h>
#endif
#include <dbAccess.h>
#include <epicsThread.h>
#include <epicsInterrupt.h>
#include "motor.h"
#include "drvPmac.h"
#include "epicsExport.h"
#define CMD_CLEAR '\030' /* Control-X, clears command errors only */
#define ALL_INFO "QA RP RE EA" /* jps: move QA to top. */
#define AXIS_INFO "QA RP" /* jps: move QA to top. */
#define ENCODER_QUERY "EA"
#define DONE_QUERY "RA"
/* Control character responses. */
#define CMNDERR 0x03
#define ACK 0x06
#define CR 0x0D
/*----------------debugging-----------------*/
#ifdef DEBUG
volatile int drvPmacdebug = 0;
#define Debug(l, f, args...) { if(l<=drvPmacdebug) printf(f,## args); }
#else
#define Debug(l, f, args...)
#endif
/* Global data. */
int Pmac_num_cards = 0;
/* Local data required for every driver; see "motordrvComCode.h" */
#include "motordrvComCode.h"
/* --- Local data common to all Pmac drivers. --- */
static char *Pmac_addrs = 0x0; /* Base address of DPRAM. */
static char *Mbox_addrs = 0x0; /* Base address of Mailbox. */
static epicsAddressType Pmac_ADDRS_TYPE;
static volatile unsigned PmacInterruptVector = 0;
static volatile epicsUInt8 PmacInterruptLevel = Pmac_INT_LEVEL;
static char Pmac_axis[] = {'1', '2', '3', '4', '5', '6', '7', '8'};
static double quantum;
/*----------------functions-----------------*/
/* Common local function declarations. */
static long report(int level);
static long init();
static void query_done(int, int, struct mess_node *);
static int set_status(int card, int signal);
RTN_STATUS send_mess(int card, char const *com, char c);
int recv_mess(int, char *, int);
static void motorIsr(int card);
static int motor_init();
static void Pmac_reset();
static RTN_STATUS PmacPut(int card, char *pcom);
static int motorIsrEnable(int card);
static void motorIsrDisable(int card);
struct driver_table Pmac_access =
{
NULL,
motor_send,
motor_free,
motor_card_info,
motor_axis_info,
&mess_queue,
&queue_lock,
&free_list,
&freelist_lock,
&motor_sem,
&motor_state,
&total_cards,
&any_motor_in_motion,
send_mess,
recv_mess,
set_status,
query_done,
NULL,
&initialized,
Pmac_axis
};
struct
{
long number;
long (*report) (int);
long (*init) (void);
} drvPmac = {2, report, init};
epicsExportAddress(drvet, drvPmac);
static struct thread_args targs = {SCAN_RATE, &Pmac_access};
/*----------------functions-----------------*/
static long report(int level)
{
int card;
if (Pmac_num_cards <= 0)
printf(" No VME8/44 controllers configured.\n");
else
{
for (card = 0; card < Pmac_num_cards; card++)
if (motor_state[card])
printf(" Pmac VME8/44 motor card %d @ 0x%X, id: %s \n", card,
(uint_t) motor_state[card]->localaddr,
motor_state[card]->ident);
}
return (0);
}
static long init()
{
initialized = true; /* Indicate that driver is initialized. */
(void) motor_init();
return ((long) 0);
}
static void query_done(int card, int axis, struct mess_node *nodeptr)
{
char buffer[40];
send_mess(card, DONE_QUERY, Pmac_axis[axis]);
recv_mess(card, buffer, 1);
if (nodeptr->status.Bits.RA_PROBLEM)
send_mess(card, AXIS_STOP, Pmac_axis[axis]);
}
static int set_status(int card, int signal)
{
struct mess_node *nodeptr;
MOTOR_STATUS motorstat;
struct mess_info *motor_info;
/* Message parsing variables */
char buff[BUFF_SIZE], outbuf[20];
int rtn_state;
double motorData;
bool plusdir, ls_active = false, plusLS, minusLS;
msta_field status;
motor_info = &(motor_state[card]->motor_info[signal]);
nodeptr = motor_info->motor_motion;
status.All = motor_info->status.All;
send_mess(card, "?", Pmac_axis[signal]);
recv_mess(card, buff, 1);
rtn_state = sscanf(buff, "%4hx%4hx%4hx", &motorstat.word1.All, &motorstat.word2.All,
&motorstat.word3.All);
status.Bits.RA_DONE = (motorstat.word3.Bits.in_position == YES) ? 0 : 1;
status.Bits.EA_POSITION = (motorstat.word1.Bits.amp_enabled == YES) ? 1 : 0;
/*
* Parse motor position
* Position string format: 1TP5.012,2TP1.123,3TP-100.567,...
* Skip to substring for this motor, convert to double
*/
sprintf(outbuf, "M%.2d61", (signal + 1));
send_mess(card, outbuf, (char) NULL);
recv_mess(card, buff, 1);
motorData = atof(buff);
if (motorData == motor_info->position)
{
if (nodeptr != 0) /* Increment counter only if motor is moving. */
motor_info->no_motion_count++;
}
else
{
epicsInt32 newposition;
newposition = NINT(motorData);
status.Bits.RA_DIRECTION = (newposition >= motor_info->position) ? 1 : 0;
motor_info->position = newposition;
motor_info->no_motion_count = 0;
}
plusdir = (status.Bits.RA_DIRECTION) ? true : false;
plusLS = motorstat.word1.Bits.pos_limit_set;
minusLS = motorstat.word1.Bits.neg_limit_set;
/* Set limit switch error indicators. */
if (plusLS == true)
{
status.Bits.RA_PLUS_LS = 1;
if (plusdir == true)
ls_active = true;
}
else
status.Bits.RA_PLUS_LS = 0;
if (minusLS == true)
{
status.Bits.RA_MINUS_LS = 1;
if (plusdir == false)
ls_active = true;
}
else
status.Bits.RA_MINUS_LS = 0;
/* encoder status */
status.Bits.EA_SLIP = 0;
status.Bits.EA_SLIP_STALL = 0;
status.Bits.EA_HOME = 0;
send_mess(card, "P", (char) NULL);
recv_mess(card, buff, 1);
motorData = atof(buff);
motorData *= 32.0; /* "P" command units are in 1/32 of a count. */
motor_info->encoder_position = (int32_t) motorData;
status.Bits.RA_PROBLEM = 0;
/* Parse motor velocity? */
/* NEEDS WORK */
motor_info->velocity = 0;
if (!status.Bits.RA_DIRECTION)
motor_info->velocity *= -1;
rtn_state = (!motor_info->no_motion_count || ls_active == true ||
status.Bits.RA_DONE | status.Bits.RA_PROBLEM) ? 1 : 0;
/* Test for post-move string. */
if ((status.Bits.RA_DONE || ls_active == true) && nodeptr != 0 &&
nodeptr->postmsgptr != 0)
{
strcpy(buff, nodeptr->postmsgptr);
send_mess(card, buff, (char) NULL);
nodeptr->postmsgptr = NULL;
}
motor_info->status.All = status.All;
return(rtn_state);
}
/*****************************************************/
/* send a message to the Pmac board */
/* send_mess() */
/*****************************************************/
RTN_STATUS send_mess(int card, char const *com, char inchar)
{
char outbuf[MAX_MSG_SIZE];
RTN_STATUS return_code;
if (strlen(com) > MAX_MSG_SIZE)
{
logMsg((char *) "drvPmac.cc:send_mess(); message size violation.\n",
0, 0, 0, 0, 0, 0);
return (ERROR);
}
/* Check that card exists */
if (!motor_state[card])
{
logMsg((char *) "drvPmac.cc:send_mess() - invalid card #%d\n", card,
0, 0, 0, 0, 0);
return (ERROR);
}
/* Flush receive buffer */
recv_mess(card, (char *) NULL, -1);
if (inchar == NULL)
strcpy(outbuf, com);
else
{
strcpy(outbuf, "#?");
outbuf[1] = inchar;
strcat(outbuf, com);
}
Debug(9, "send_mess: ready to send message.\n");
return_code = PmacPut(card, outbuf);
if (return_code == OK)
{
Debug(4, "sent message: (%s)\n", outbuf);
}
else
{
Debug(4, "unable to send message (%s)\n", outbuf);
return (ERROR);
}
return (return_code);
}
/*
* FUNCTION... recv_mess(int card, char *com, int amount)
*
* INPUT ARGUMENTS...
* card - controller card # (0,1,...).
* *com - caller's response buffer.
* amount | -1 = flush controller's output buffer.
* | >= 1 = the # of command responses to retrieve into caller's
* response buffer.
*
* LOGIC...
* IF controller card does not exist.
* ERROR RETURN.
* ENDIF
* IF "amount" indicates buffer flush.
* WHILE characters left in input buffer.
* Call PmacGet().
* ENDWHILE
* ENDIF
*
* FOR each message requested (i.e. "amount").
* Initialize head and tail pointers.
* Initialize retry counter and state indicator.
* WHILE retry count not exhausted, AND, state indicator is NOT at END.
* IF characters left in controller's input buffer.
* Process input character.
* ELSE IF command error occured - call PmacError().
* ERROR RETURN.
* ENDIF
* ENDWHILE
* IF retry count exhausted.
* Terminate receive buffer.
* ERROR RETURN.
* ENDIF
* Terminate command response.
* ENDFOR
*
* IF commands processed.
* Terminate response buffer.
* ELSE
* Clear response buffer.
* ENDIF
* NORMAL RETURN.
*/
int recv_mess(int card, char *com, int amount)
{
volatile struct controller *pmotorState;
volatile struct pmac_dpram *pmotor;
volatile REPLY_STATUS *stptr;
int trys;
char control;
pmotorState = motor_state[card];
pmotor = (struct pmac_dpram *) pmotorState->localaddr;
stptr = &pmotor->reply_status;
/* Check that card exists */
if (card >= total_cards)
{
Debug(1, "recv_mess - invalid card #%d\n", card);
return (-1);
}
if (amount == -1)
{
bool timeout = false, flushed = false;
control = stptr->Bits.cntrl_char;
while (timeout == false && flushed == false)
{
const double flush_delay = quantum;
if (control == NULL)
{
Debug(6, "recv_mess() - flush wait on NULL\n");
epicsThreadSleep(flush_delay);
control = stptr->Bits.cntrl_char;
if (control == NULL)
flushed = true;
else
Debug(6, "recv_mess() - NULL -> %c\n", control);
}
else if (control == ACK)
{
stptr->All = 0;
Debug(6, "recv_mess() - flush wait on ACK\n");
epicsThreadSleep(flush_delay);
control = stptr->Bits.cntrl_char;
}
else if (control == CR)
{
stptr->All = 0;
Debug(6, "recv_mess() - flush wait on CR\n");
for (trys = 0; trys < 10 && stptr->Bits.cntrl_char == NULL; trys++)
{
epicsThreadSleep(quantum * trys);
Debug(6, "recv_mess() - flush wait #%d\n", trys);
}
if (trys >= 10)
timeout = true;
control = stptr->Bits.cntrl_char;
}
else
{
stptr->All = 0;
errlogPrintf("%s(%d): ERROR = 0x%X\n", __FILE__, __LINE__, (unsigned int) control);
epicsThreadSleep(flush_delay);
control = stptr->Bits.cntrl_char;
}
}
if (timeout == true)
errlogPrintf("%s(%d): flush timeout\n", __FILE__, __LINE__);
return(0);
}
for (trys = 0; trys < 10;)
{
if (stptr->All == 0)
{
trys++;
epicsThreadSleep(quantum * 2.0);
}
else
break;
}
if (trys >= 10)
{
Debug(1, "recv_mess() timeout.\n");
return(-1);
}
control = stptr->Bits.cntrl_char;
if (control == CMNDERR)
{
stptr->All = 0;
Debug(1, "recv_mess(): command error.\n");
return(-1);
}
else if (control == ACK)
{
Debug(4, "recv_mess(): control = ACK\n");
stptr->All = 0;
return(recv_mess(card, com, amount));
}
else if (control == CR)
{
strcpy(com, (char *) &pmotor->response[0]);
stptr->All = 0;
Debug(4, "recv_mess(): card %d, msg: (%s)\n", card, com);
return(0);
}
else
{
stptr->All = 0;
errlogPrintf("%s(%d): ERROR = 0x%X\n", __FILE__, __LINE__, (unsigned int) control);
return(-1);
}
}
/*****************************************************/
/* Send Message to Pmac */
/* PmacPut() */
/*****************************************************/
static RTN_STATUS PmacPut(int card, char *pmess)
{
volatile struct controller *pmotorState;
volatile struct pmac_dpram *pmotor;
volatile REPLY_STATUS *stptr;
int itera;
pmotorState = motor_state[card];
pmotor = (struct pmac_dpram *) pmotorState->localaddr;
stptr = &pmotor->reply_status;
for(itera = 0; itera < 10; itera++)
{
if(pmotor->out_cntrl_wd == 0)
break;
else
epicsThreadSleep(0.010);
}
if(itera >= 10)
return(ERROR);
else
{
strcpy((char *) &pmotor->cmndbuff[0], pmess);
pmotor->out_cntrl_wd = 1;
}
/* Wait for response. */
for (itera = 0; itera < 10 && stptr->Bits.cntrl_char == NULL; itera++)
{
epicsThreadSleep(quantum * itera);
Debug(7, "PmacPut() - response wait #%d\n", itera);
}
if (itera >= 10)
{
errlogPrintf("%s(%d): response timeout.\n", __FILE__, __LINE__);
return(ERROR);
}
return (OK);
}
/*****************************************************/
/* Interrupt service routine. */
/* motorIsr() */
/*****************************************************/
static void motorIsr(int card)
{
}
static int motorIsrEnable(int card)
{
long status;
status = devConnectInterrupt(intVME, PmacInterruptVector + card,
// Tornado 2.0.2 (void (*)()) motorIsr, (void *) card);
(devLibVOIDFUNCPTR) motorIsr, (void *) card);// Tornado 2.2
status = devEnableInterruptLevel(Pmac_INTERRUPT_TYPE,
PmacInterruptLevel);
return (OK);
}
static void motorIsrDisable(int card)
{
long status;
status = devDisconnectInterrupt(intVME, PmacInterruptVector + card,
// Tornado 2.0.2 (void (*)()) motorIsr);
(devLibVOIDFUNCPTR) motorIsr);// Tornado 2.2
if (!RTN_SUCCESS(status))
errPrintf(status, __FILE__, __LINE__, "Can't disconnect vector %d\n",
PmacInterruptVector + card);
}
/*****************************************************/
/* Configuration function for module_types data */
/* areas. PmacSetup() */
/*****************************************************/
int PmacSetup(int num_cards, /* maximum number of cards in rack */
void *mbox, /* Mailbox base address. */
void *addrs, /* DPRAM Base Address */
int addrs_type, /* VME address type; 24 - A24 or 32 - A32. */
unsigned vector, /* noninterrupting(0), valid vectors(64-255) */
int int_level, /* interrupt level (1-6) */
int scan_rate) /* polling rate - in HZ */
{
void *erraddr = 0;
long status;
if (num_cards < 1 || num_cards > Pmac_NUM_CARDS)
Pmac_num_cards = Pmac_NUM_CARDS;
else
Pmac_num_cards = num_cards;
switch(addrs_type)
{
case 24:
Pmac_ADDRS_TYPE = atVMEA24;
if ((uint32_t) mbox & 0xF)
erraddr = mbox;
else if ((uint32_t) addrs & 0xF)
erraddr = addrs;
if (erraddr != 0)
Debug(1, "PmacSetup: invalid A24 address 0x%X\n", (uint_t) mbox);
break;
case 32:
Pmac_ADDRS_TYPE = atVMEA32;
break;
default:
Debug(1, "PmacSetup: invalid Address Type %d\n", (uint_t) addrs);
break;
}
status = devNoResponseProbe(Pmac_ADDRS_TYPE, (unsigned int) mbox, 1);
if (status == 0)
{
Pmac_addrs = (char *) addrs;
Mbox_addrs = (char *) mbox;
*(Mbox_addrs + 0x121) = (char) ((unsigned long) Pmac_addrs >> 14); /* Select VME A19-A14 for DPRAM. */
}
else
{
errlogPrintf("%s(%d): Mailbox bus error - 0x%X\n", __FILE__, __LINE__,
(unsigned int) mbox);
Pmac_num_cards = 0;
}
PmacInterruptVector = vector;
if (vector < 64 || vector > 255)
{
if (vector != 0)
{
Debug(1, "PmacSetup: invalid interrupt vector %d\n", vector);
PmacInterruptVector = (unsigned) Pmac_INT_VECTOR;
}
}
if (int_level < 1 || int_level > 6)
{
Debug(1, "PmacSetup: invalid interrupt level %d\n", int_level);
PmacInterruptLevel = Pmac_INT_LEVEL;
}
else
PmacInterruptLevel = int_level;
/* Set motor polling task rate */
if (scan_rate >= 1 && scan_rate <= MAX_SCAN_RATE)
targs.motor_scan_rate = scan_rate;
else
{
targs.motor_scan_rate = SCAN_RATE;
errlogPrintf("%s(%d): invalid poll rate - %d HZ\n", __FILE__, __LINE__,
scan_rate);
}
return(0);
}
/*****************************************************/
/* initialize all software and hardware */
/* motor_init() */
/*****************************************************/
static int motor_init()
{
volatile struct controller *pmotorState;
volatile struct pmac_dpram *pmotor;
long status;
int card_index, motor_index;
char axis_pos[50], encoder_pos[50];
char *tok_save;
int total_encoders = 0, total_axis = 0;
volatile void *localaddr;
void *probeAddr;
bool errind;
tok_save = NULL;
quantum = epicsThreadSleepQuantum();
Debug(5, "motor_init: epicsThreadSleepQuantum = %f\n", quantum);
/* Check for setup */
if (Pmac_num_cards <= 0)
{
Debug(1, "motor_init: *Pmac driver disabled* \n PmacSetup() is missing from startup script.\n");
return (ERROR);
}
/* allocate space for total number of motors */
motor_state = (struct controller **) malloc(Pmac_num_cards *
sizeof(struct controller *));
/* allocate structure space for each motor present */
total_cards = Pmac_num_cards;
if (rebootHookAdd((FUNCPTR) Pmac_reset) == ERROR)
Debug(1, "vme8/44 motor_init: Pmac_reset disabled\n");
for (card_index = 0; card_index < Pmac_num_cards; card_index++)
{
int8_t *startAddr;
int8_t *endAddr;
Debug(2, "motor_init: card %d\n", card_index);
probeAddr = Pmac_addrs + (card_index * Pmac_BRD_SIZE);
startAddr = (int8_t *) probeAddr + 1;
endAddr = startAddr + Pmac_BRD_SIZE;
Debug(9, "motor_init: devNoResponseProbe() on addr 0x%x\n", (uint_t) probeAddr);
/* Scan memory space to assure card id */
do
{
status = devNoResponseProbe(Pmac_ADDRS_TYPE, (unsigned int) startAddr, 1);
startAddr += 0x100;
} while (PROBE_SUCCESS(status) && startAddr < endAddr);
if (PROBE_SUCCESS(status))
{
status = devRegisterAddress(__FILE__, Pmac_ADDRS_TYPE,
(size_t) probeAddr, Pmac_BRD_SIZE,
(volatile void **) &localaddr);
Debug(9, "motor_init: devRegisterAddress() status = %d\n",
(int) status);
if (!RTN_SUCCESS(status))
{
errPrintf(status, __FILE__, __LINE__,
"Can't register address 0x%x\n", (unsigned) probeAddr);
return (ERROR);
}
Debug(9, "motor_init: localaddr = %x\n", (int) localaddr);
Debug(9, "motor_init: malloc'ing motor_state\n");
motor_state[card_index] = (struct controller *) malloc(sizeof(struct controller));
pmotorState = motor_state[card_index];
pmotorState->localaddr = (char *) localaddr;
pmotorState->motor_in_motion = 0;
pmotorState->cmnd_response = false;
/* Initialize DPRAM communication. */
pmotor = (struct pmac_dpram *) pmotorState->localaddr;
pmotor->out_cntrl_wd = 0; /* Clear "Data ready from host" bit indicator. */
pmotor->out_cntrl_char = 0; /* Clear "Buffer Control Character. */
pmotor->reply_status.All = 0;
send_mess(card_index, "TYPE", (char) NULL);
recv_mess(card_index, (char *) pmotorState->ident, 1);
send_mess(card_index, "VERSION", (char) NULL);
recv_mess(card_index, axis_pos, 1);
strcat((char *) &pmotorState->ident, ", ");
strcat((char *) &pmotorState->ident, axis_pos);
Debug(3, "Identification = %s\n", pmotorState->ident);
for (total_axis = 0, errind = false; errind == false &&
total_axis < Pmac_MAX_AXES; total_axis++)
{
char outbuf[10];
sprintf(outbuf, "I%.2d00", (total_axis + 1));
send_mess(card_index, outbuf, (char) NULL);
recv_mess(card_index, axis_pos, 1);
if (strcmp(axis_pos, "0") == 0)
errind = true;
else if (strcmp(axis_pos, "1") == 0)
{
pmotorState->motor_info[total_axis].motor_motion = NULL;
pmotorState->motor_info[total_axis].status.All = 0;
}
else
{
Debug(1, "Invalid response = \"%s\" to msg = \"%s\"\n", axis_pos, outbuf);
}
}
pmotorState->total_axis = --total_axis;
Debug(3, "Total axis = %d\n", total_axis);
/*
* Enable interrupt-when-done if selected - driver depends on
* motor_state->total_axis being set.
*/
if (PmacInterruptVector)
{
if (motorIsrEnable(card_index) == ERROR)
errPrintf(0, __FILE__, __LINE__, "Interrupts Disabled!\n");
}
for (total_encoders = 0, motor_index = 0; motor_index < total_axis; motor_index++)
{
total_encoders++;
pmotorState->motor_info[motor_index].encoder_present = YES;
}
for (motor_index = 0; motor_index < total_axis; motor_index++)
{
pmotorState->motor_info[motor_index].status.All = 0;
pmotorState->motor_info[motor_index].no_motion_count = 0;
pmotorState->motor_info[motor_index].encoder_position = 0;
pmotorState->motor_info[motor_index].position = 0;
if (pmotorState->motor_info[motor_index].encoder_present == YES)
pmotorState->motor_info[motor_index].status.Bits.EA_PRESENT = 1;
set_status(card_index, motor_index);
}
Debug(2, "Init Address=0x%8.8x\n", (uint_t) localaddr);
Debug(3, "Total encoders = %d\n\n", (int) total_encoders);
}
else
{
Debug(3, "motor_init: Card NOT found!\n");
motor_state[card_index] = (struct controller *) NULL;
}
}
any_motor_in_motion = 0;
mess_queue.head = (struct mess_node *) NULL;
mess_queue.tail = (struct mess_node *) NULL;
free_list.head = (struct mess_node *) NULL;
free_list.tail = (struct mess_node *) NULL;
Debug(3, "Motors initialized\n");
epicsThreadCreate((const char *) "Pmac_motor", 64, 5000, (EPICSTHREADFUNC) motor_task, (void *) &targs);
Debug(3, "Started motor_task\n");
return (0);
}
/* Disables interrupts. Called on CTL X reboot. */
static void Pmac_reset()
{
}
/*---------------------------------------------------------------------*/
motorApp/DeltaTauSrc/drvPmac.h
+217
View File
@@ -0,0 +1,217 @@
/*
FILENAME... drvPmac.h
USAGE... This file contains Delta Tau PMAC driver "include" information.
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2004-06-07 19:27:05 $
*/
/*
* Experimental Physics and Industrial Control System (EPICS)
*
* Copyright 1991, the Regents of the University of California,
* and the University of Chicago Board of Governors.
*
* This software was produced under U.S. Government contracts:
* (W-7405-ENG-36) at the Los Alamos National Laboratory,
* and (W-31-109-ENG-38) at Argonne National Laboratory.
*
* Initial development by:
* The Controls and Automation Group (AT-8)
* Ground Test Accelerator
* Accelerator Technology Division
* Los Alamos National Laboratory
*
* Co-developed with
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
* 00 04/16/04 rls - Copied from drvOms.h
*/
#ifndef INCdrvPmach
#define INCdrvPmach 1
#include "motor.h"
#include "motordrvCom.h"
/* Define for return test on devNoResponseProbe() */
#define PROBE_SUCCESS(STATUS) ((STATUS)==S_dev_addressOverlap)
#define BUFF_SIZE 100 /* Maximum length of string to/from PMAC */
/* Default profile. */
#define Pmac_NUM_CARDS 1 /* Maximum number of cards. */
#define Pmac_BRD_SIZE 0x4000 /* card address boundary */
#define Pmac_MAX_AXES 32
#define Pmac_INTERRUPT_TYPE intVME
#define Pmac_INT_VECTOR 180 /* default interrupt vector (64-255) */
#define Pmac_INT_LEVEL 5 /* default interrupt level (1-6) */
/* PMAC Commands. */
#define AXIS_STOP "\\"
/* !!!!! DELETE THIS !!!PMAC specific data is stored in this structure. */
struct PMACcontroller
{
int status;
};
typedef struct
{
union
{
epicsUInt16 All;
struct
{
#ifdef MSB_First
unsigned int motor_on :1; /* Motor Activated (23). */
unsigned int neg_limit_set :1; /* Negative End Limit Set (22). */
unsigned int pos_limit_set :1; /* Positive End Limit Set (21). */
unsigned int x_servo_on :1; /* Extended Servo Algorithm Enabled (20). */
unsigned int amp_enabled :1; /* Amplifier Enabled (19). */
unsigned int open_loop :1; /* Open Loop Mode (18). */
unsigned int move_time_on :1; /* Move Timer Active (17). */
unsigned int integrate_mode :1; /* Integration Mode (16). */
unsigned int dwell :1; /* Dwell in Progress (15). */
unsigned int DataBlkErr :1; /* Data Block Error (14). */
unsigned int CmndVelZero :1; /* Desired Velocity Zero (13). */
unsigned int decel_abort :1; /* Abort Deceleration (12). */
unsigned int block_request :1; /* Block Request (11). */
unsigned int homeing :1; /* Home Search in Progress (10). */
unsigned int user_phase :1; /* User-Written Phase Enable (9). */
unsigned int user_servo :1; /* User-Written Servo Enable (8). */
#else
unsigned int follow_enable :1; /* . */
unsigned int follow_offset :1; /* . */
unsigned int phased_motor :1; /* . */
unsigned int alt_src_dest :1; /* . */
unsigned int user_enable :1; /* . */
unsigned int user_phase :1; /* . */
unsigned int homeing :1; /* . */
unsigned int block_request :1; /* . */
unsigned int decel_abort :1; /* . */
unsigned int move_time_on :1; /* . */
unsigned int open_loop :1; /* . */
unsigned int amp_enabled :1; /* . */
unsigned int x_servo_on :1; /* . */
unsigned int pos_limit_set :1; /* . */
unsigned int neg_limit_set :1; /* . */
unsigned int motor_on :1; /* . */
#endif
} Bits;
} word1;
union
{
epicsUInt16 All;
struct
{
#ifdef MSB_First
unsigned int alt_src_dest :1; /* Alternate Source/Destination (7). */
unsigned int phased_motor :1; /* Phased Motor (6). */
unsigned int follow_offset :1; /* Following Offset Mode (5). */
unsigned int follow_enable :1; /* Following Enabled (4). */
unsigned int error_trigger :1; /* . */
unsigned int soft_pos_capture :1; /* . */
unsigned int alt_cmndout_mode :1; /* . */
unsigned int maxrapid_speed :1; /* . */
unsigned int CS_assignment :4; /* Coordinate System Number. */
unsigned int CD_assignment :4; /* Coordinate Definition. */
#else
unsigned int desired_stop :1; /* . */
unsigned int fore_in_pos :1; /* Foreground In-Position. */
unsigned int na14 :1; /* . */
unsigned int assigned_CS :1; /* Assigned to C.S. */
unsigned int CD_assignment :4; /* Coordinate Definition. */
unsigned int CS_assignment :4; /* Coordinate System Number. */
unsigned int maxrapid_speed :1; /* . */
unsigned int alt_cmndout_mode :1; /* . */
unsigned int soft_pos_capture :1; /* . */
unsigned int error_trigger :1; /* . */
#endif
} Bits;
} word2;
union
{
epicsUInt16 All;
struct
{
#ifdef MSB_First
unsigned int assigned_CS :1; /* Assigned to C.S. (15).*/
unsigned int na14 :1; /* N/A (14). */
unsigned int fore_in_pos :1; /* Foreground In-Position (13). */
unsigned int desired_stop :1; /* Stopped on Desired Position Limit (12) . */
unsigned int pos_limit_stop :1; /* Stopped on Position Limit (11). */
unsigned int home_complete :1; /* Home Complete (10). */
unsigned int phase_search :1; /* Phase Search/Read Active (9). */
unsigned int phase_ref_err :1; /* Phase Reference Error (8). */
unsigned int trigger_move :1; /* Trigger Move (7). */
unsigned int i2_follow_err :1; /* Integrated Fatal Following Error (6). */
unsigned int i2t_amp_fault :1; /* I2T Aplifier Fault (5). */
unsigned int neg_backlash :1; /* Negative Backlash Direction Flag (4). */
unsigned int amp_fault :1; /* Amplifier Fault (3). */
unsigned int err_follow_err :1; /* Fatal Following Error (2). */
unsigned int warn_follow_err:1; /* Warning Following Error (1). */
unsigned int in_position :1; /* In position (0). */
#else
unsigned int in_position :1; /* In position. */
unsigned int warn_follow_err:1; /* Following error warning. */
unsigned int err_follow_err :1; /* Fatal Following error. */
unsigned int amp_fault :1; /* Amplifier Fault. */
unsigned int neg_backlash :1; /* Negative Backlash Direction Flag. */
unsigned int i2t_amp_fault :1; /* I2T Aplifier Fault. */
unsigned int i2_follow_err :1; /* Integrated Fatal Following Error. */
unsigned int trigger_move :1; /* Trigger Move. */
unsigned int phase_ref_err :1; /* Phase Reference Error. */
unsigned int home_complete :1; /* Home Complete. */
unsigned int pos_limit_stop :1; /* Stopped on Position Limit. */
#endif
} Bits;
} word3;
} MOTOR_STATUS;
typedef union
{
epicsUInt16 All;
struct
{
#ifdef MSB_First
unsigned int cntrl_char :8; /* Response control charcter. */
unsigned int type :2; /* Response type. */
unsigned int na1 :5; /* n/a bit #1-4. */
unsigned int error :1; /* Error indicator. */
#else
unsigned int error :1; /* Error indicator. */
unsigned int na1 :5; /* n/a bit #1-4. */
unsigned int type :2; /* Response type. */
unsigned int cntrl_char :8; /* Response control charcter. */
#endif
} Bits;
} REPLY_STATUS;
/* PMAC DPRAM structure. */
struct pmac_dpram
{
epicsUInt8 na0[0xE9C];
epicsUInt8 out_cntrl_wd; /* Control Word at 0x0E9C. */
epicsUInt8 na1;
epicsUInt16 out_cntrl_char; /* Control Character at 0x0E9E. */
epicsUInt8 cmndbuff[160]; /* Command Buffer at 0x0EA0. */
REPLY_STATUS reply_status; /* Response Buffer Control Characters. */
epicsUInt8 reply_count; /* Response Character count - 1. */
epicsUInt8 na2;
epicsUInt8 response[256]; /* Response Buffer at 0x0F44. */
};
#endif /* INCdrvPmach */