//--------------------------------------------------------------------------- #include "sio_util.h" //--------------------- Servo Module specific stuff --------------------------- typedef struct _GAINVECT { short int kp; //gain values short int kd; short int ki; short int il; byte ol; byte cl; short int el; byte sr; byte dc; } GAINVECT; typedef struct _SERVOMOD { long pos; //current position short int ad; //a/d value short int vel; //current velocity byte aux; //auxilliary status byte long home; //home position short int perror; //position error byte inport1; //input port 1 byte inport2; //input port 2 byte inport3; //input port 3 //The following data is stored locally for reference long cmdpos; //last commanded position long cmdvel; //last commanded velocity long cmdacc; //last commanded acceleration byte cmdpwm; //last commanded PWM value short int cmdadc; //last commanded Ext Pos value // 10 bits for type 90 (SERVOHYBTYPE, ver. >= 2) GAINVECT gain; long stoppos; //motor stop position (used by stop command) byte outport1; //output port val (used by I/O control) byte outport2; //output port val (used by I/O control) byte outport3; //output port val (used by I/O control) byte outport4; //output port val (used by I/O control) byte stopctrl; //stop control byte byte movectrl; //load_traj control byte byte ioctrl; //I/O control byte byte homectrl; //homing control byte byte servoinit; //set to 1 for servo on powerup, zero otherwise byte stp_dir_mode; // step/direction mode byte ph_adv; //phase advance (for ss-drive modules) byte ph_off; //phase offset (for ss-drive modules) // LS-174 byte advmode; //0 = LS-173, 1 = advanced mode byte npoints; //number of points in path buffer long last_ppoint; //last path point specified } SERVOMOD; //Servo Module Command set: #define RESET_POS 0x00 //Reset encoder counter to 0 (0 bytes) #define SET_ADDR 0x01 //Set address and group address (2 bytes) #define DEF_STAT 0x02 //Define status items to return (1 byte) #define READ_STAT 0x03 //Read value of current status items #define LOAD_TRAJ 0x04 //Load trajectory date (1 - 14 bytes) #define START_MOVE 0x05 //Start pre-loaded trajectory (0 bytes) #define SET_GAIN 0x06 //Set servo gain and control parameters (13 or 14) #define STOP_MOTOR 0x07 //Stop motor (1 byte) #define IO_CTRL 0x08 //Define bit directions and set output (1 byte) #define SET_HOMING 0x09 //Define homing mode (1 byte) #define SET_BAUD 0x0A //Set the baud rate (1 byte) #define CLEAR_BITS 0x0B //Save current pos. in home pos. register (0 bytes) #define SAVE_AS_HOME 0x0C //Store the input bytes and timer val (0 bytes) #define EEPROM_CTRL 0x0D //Store or retrieve values from EEPROM // LS-174 #define ADD_PATHPOINT 0x0D //Adds path point in path mode #define NOP 0x0E //No operation - returns prev. defined status (0 bytes) #define HARD_RESET 0x0F //RESET - no status is returned // LS-174 //Servo Module RESET_POS control byte bit definitions: //(if no control byte is used, reset is absolute) #define REL_HOME 0x01 //Reset position relative to current home position //Servo Module STATUSITEMS bit definitions: #define SEND_POS 0x01 //4 bytes data #define SEND_AD 0x02 //1 byte #define SEND_VEL 0x04 //2 bytes #define SEND_AUX 0x08 //1 byte #define SEND_HOME 0x10 //4 bytes #define SEND_ID 0x20 //2 bytes #define SEND_PERROR 0x40 //2 bytes #define SEND_INPORTS 0x80 //3 bytes // LS-174 #define SEND_NPOINTS 0x80 //1 byte //Servo Module LOAD_TRAJ control byte bit definitions: #define LOAD_POS 0x01 //+4 bytes #define LOAD_VEL 0x02 //+4 bytes #define LOAD_ACC 0x04 //+4 bytes #define LOAD_PWM 0x08 //+1 byte #define ENABLE_SERVO 0x10 //1 = servo mode, 0 = PWM mode #define VEL_MODE 0x20 //1 = velocity mode, 0 = trap. position mode #define REVERSE 0x40 //1 = command neg. PWM or vel, 0 = positive // LS-174 #define MOVE_REL 0x40 //1 = relative move, 0 = absolute move; works in advanced mode #define START_NOW 0x80 //1 = start now, 0 = wait for START_MOVE command //Servo Module STOP_MOTOR control byte bit definitions: #define SRV_ENABLE_AMP 0x01 //1 = raise amp enable output, 0 = lower amp enable #define MOTOR_OFF 0x02 //set to turn motor off #define STOP_ABRUPT 0x04 //set to stop motor immediately #define STOP_SMOOTH 0x08 //set to decellerate motor smoothly #define STOP_HERE 0x10 //set to stop at position (4 add'l data bytes required) // LS-174 #define ADV_MODE 0x20 //Enables advanced mode //Servo Module IO_CTRL control byte bit definitions: #define SET_OUT1 0x01 //1 = set limit 1 output, 0 = clear limit 1 output #define SET_OUT2 0x02 //1 = set limit 2 output, 0 = clear limit 2 output #define IO1_IN 0x04 //1 = limit 1 is an input, 0 = limit 1 is an output #define IO2_IN 0x08 //1 = limit 2 is an input, 0 = limit 2 is an output #define WR_OUT1 0x10 //Write to output port 1 #define WR_OUT2 0x20 //Write to output port 2 #define WR_OUT3 0x40 //Write to output port 3 // LS-174 #define FAST_PATH 0x40 //0 = 30/60 Hz, 1 = 60/120 Hz #define WR_OUT4 0x80 //Write to output port 4 //Servo Module SET_HOMING control byte bit definitions: #define ON_LIMIT1 0x01 //home on change in limit 1 #define ON_LIMIT2 0x02 //home on change in limit 2 #define HOME_MOTOR_OFF 0x04 //turn motor off when homed #define ON_INDEX 0x08 //home on change in index #define HOME_STOP_ABRUPT 0x10 //stop abruptly when homed #define HOME_STOP_SMOOTH 0x20 //stop smoothly when homed #define ON_POS_ERR 0x40 //home on excessive position error #define ON_CUR_ERR 0x80 //home on overcurrent error //Servo Module EEPROM_CTRL control byte bit definitions: #define STORE_GAINS 0x01 //Store servo gains #define FETCH_GAINS 0x02 //Retrieve stored gains #define STORE_VA 0x04 //Store velocity and acceleration #define FETCH_VA 0x08 //Retrieve stored velocity and acceleration #define STORE_OUTPUTS 0x10 //Store output port values #define FETCH_OUTPUTS 0x20 //Retrieve stored output port values #define STORE_SI_BIT 0x40 //Store "servo initialize" bit #define INIT_SERVO 0x80 //Initializes servo on power-up // LS-174 //Servo Module ADD_PATHPOINT frequency definitions #define P_30HZ 30 //30 hz path resolution #define P_60HZ 60 //60 hz path resolution #define P_120HZ 120 //120 hz path resolution //Servo Module Status byte bit definitions: #define MOVE_DONE 0x01 //set when move done (trap. pos mode), when goal //vel. has been reached (vel mode) or when not servoing #define CKSUM_ERROR 0x02 //checksum error in received command #define OVERCURRENT 0x04 //set on overcurrent condition (sticky bit) #define POWER_ON 0x08 //set when motor power is on #define POS_ERR 0x10 //set on excess pos. error (sticky bit) #define LIMIT1 0x20 //value of limit 1 input #define LIMIT2 0x40 //value of limit 2 input #define HOME_IN_PROG 0x80 //set while searching for home, cleared when home found //Servo Module Auxilliary status byte bit definitions: #define INDEX 0x01 //value of the encoder index signal #define POS_WRAP 0x02 //set when 32 bit position counter wraps around // (sticky bit) #define SERVO_ON 0x04 //set when position servo is operating #define ACCEL_DONE 0x08 //set when acceleration portion of a move is done #define SLEW_DONE 0x10 //set when slew portion of a move is done #define SERVO_OVERRUN 0x20 //set if servo takes longer than the specified //servo period to execute // LS-174 #define PATH_MODE 0x40 //path mode is enabled //Servo module function prototypes: SERVOMOD * ServoNewMod(); DLLENTRY(BOOL) ServoGetStat(byte addr); DLLENTRY(long) ServoGetPos(byte addr); DLLENTRY(byte) ServoGetAD(byte addr); DLLENTRY(short int) ServoGetAD10(byte addr); DLLENTRY(short int) ServoGetVel(byte addr); DLLENTRY(byte) ServoGetAux(byte addr); DLLENTRY(long) ServoGetHome(byte addr); DLLENTRY(short int) ServoGetPError(byte addr); DLLENTRY(byte) ServoGetInport1(byte addr); DLLENTRY(byte) ServoGetInport2(byte addr); DLLENTRY(byte) ServoGetInport3(byte addr); DLLENTRY(byte) ServoGetOutport1(byte addr); DLLENTRY(byte) ServoGetOutport2(byte addr); DLLENTRY(byte) ServoGetOutport3(byte addr); DLLENTRY(byte) ServoGetOutport4(byte addr); DLLENTRY(long) ServoGetCmdPos(byte addr); DLLENTRY(long) ServoGetCmdVel(byte addr); DLLENTRY(long) ServoGetCmdAcc(byte addr); DLLENTRY(byte) ServoGetCmdPwm (byte addr); DLLENTRY(byte) ServoGetCmdAdc (byte addr); DLLENTRY(short int) ServoGetCmdAdc10 (byte addr); DLLENTRY(byte) ServoGetMoveCtrl(byte addr); DLLENTRY(byte) ServoGetStopCtrl(byte addr); DLLENTRY(byte) ServoGetHomeCtrl(byte addr); DLLENTRY(byte) ServoGetIoCtrl(byte addr); DLLENTRY(byte) ServoGetServoInit(byte addr); DLLENTRY(byte) ServoGetSDMode(byte addr); DLLENTRY(void) ServoGetGain(byte addr, short int * kp, short int * kd, short int * ki, short int * il, byte * ol, byte * cl, short int * el, byte * sr, byte * dc); DLLENTRY(BOOL) ServoSetGain(byte addr, short int kp, short int kd, short int ki, short int il, byte ol, byte cl, short int el, byte sr, byte dc); DLLENTRY(BOOL) ServoResetPos(byte addr); DLLENTRY(BOOL) ServoClearBits(byte addr); DLLENTRY(BOOL) ServoStopMotor(byte addr, byte mode); DLLENTRY(BOOL) ServoLoadTraj(byte addr, byte mode, long pos, long vel, long acc, byte pwm); DLLENTRY(BOOL) ServoLoadTraj10(byte addr, byte mode, long pos, long vel, long acc, short int pwm); DLLENTRY(BOOL) ServoStartMotion(byte groupaddr); DLLENTRY(BOOL) ServoSetHoming(byte addr, byte mode); DLLENTRY(BOOL) ServoSetOutputs(byte addr, byte mode, byte out1, byte out2, byte out3, byte out4); DLLENTRY(BOOL) ServoEEPROMCtrl(byte addr, byte mode, byte out1, byte out2, byte out3, byte out4); // LS-174 DLLENTRY(BOOL) ServoResetRelHome(byte addr); DLLENTRY(BOOL) ServoSetFastPath(byte addr, bool fast); DLLENTRY(void) ServoInitPath(byte addr); DLLENTRY(BOOL) ServoAddPathPoints(byte addr, int npoints, long *path, bool high_freq); DLLENTRY(BOOL) ServoStartPathMode(byte groupaddr); DLLENTRY(byte) ServoGetNPoints(byte addr);