Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //---------------------------------------------------------------------------
- #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);
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement