API tools faq
paste
Advertisement
Guest User

servo.h

a guest
Dec 4th, 2012
110
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 10.42 KB | None | 0 0
raw download clone embed print report
  1. //---------------------------------------------------------------------------
  2. #include "sio_util.h"
  3.  
  4. //--------------------- Servo Module specific stuff ---------------------------
  5. typedef struct _GAINVECT {
  6.     short int   kp;     //gain values
  7.     short int   kd;
  8.     short int   ki;
  9.     short int   il;
  10.     byte        ol;
  11.     byte        cl;
  12.     short int   el;
  13.     byte        sr;
  14.     byte        dc;
  15. } GAINVECT;
  16.  
  17. typedef struct _SERVOMOD {
  18.     long    pos;        //current position
  19.     short int   ad;     //a/d value
  20.     short int   vel;            //current velocity
  21.     byte    aux;            //auxilliary status byte
  22.     long    home;           //home position
  23.     short int   perror;     //position error
  24.     byte    inport1;    //input port 1
  25.     byte    inport2;    //input port 2
  26.     byte    inport3;    //input port 3
  27. //The following data is stored locally for reference
  28.     long    cmdpos;     //last commanded position
  29.     long    cmdvel;     //last commanded velocity
  30.     long    cmdacc;     //last commanded acceleration
  31.     byte    cmdpwm;         //last commanded PWM value
  32.     short int cmdadc;         //last commanded Ext Pos value // 10 bits for type 90 (SERVOHYBTYPE, ver. >= 2)
  33.     GAINVECT    gain;
  34.     long    stoppos;    //motor stop position (used by stop command)
  35.     byte    outport1;   //output port val (used by I/O control)
  36.     byte    outport2;   //output port val (used by I/O control)
  37.     byte    outport3;   //output port val (used by I/O control)
  38.     byte    outport4;   //output port val (used by I/O control)
  39.     byte    stopctrl;   //stop control byte
  40.     byte    movectrl;   //load_traj control byte
  41.     byte    ioctrl;     //I/O control byte
  42.     byte    homectrl;   //homing control byte
  43.     byte    servoinit;  //set to 1 for servo on powerup, zero otherwise
  44.     byte        stp_dir_mode;   // step/direction mode
  45.     byte    ph_adv;     //phase advance (for ss-drive modules)
  46.     byte    ph_off;     //phase offset (for ss-drive modules)
  47. // LS-174
  48.     byte        advmode;        //0 = LS-173, 1 = advanced mode
  49.     byte        npoints;        //number of points in path buffer
  50.     long        last_ppoint;    //last path point specified
  51. } SERVOMOD;
  52.  
  53.  
  54. //Servo Module Command set:
  55. #define RESET_POS     0x00  //Reset encoder counter to 0 (0 bytes)
  56. #define SET_ADDR      0x01  //Set address and group address (2 bytes)
  57. #define DEF_STAT      0x02  //Define status items to return (1 byte)
  58. #define READ_STAT     0x03  //Read value of current status items
  59. #define LOAD_TRAJ     0x04  //Load trajectory date (1 - 14 bytes)
  60. #define START_MOVE    0x05  //Start pre-loaded trajectory (0 bytes)
  61. #define SET_GAIN      0x06  //Set servo gain and control parameters (13 or 14)
  62. #define STOP_MOTOR    0x07  //Stop motor (1 byte)
  63. #define IO_CTRL       0x08  //Define bit directions and set output (1 byte)
  64. #define SET_HOMING    0x09  //Define homing mode (1 byte)
  65. #define SET_BAUD      0x0A  //Set the baud rate (1 byte)
  66. #define CLEAR_BITS    0x0B  //Save current pos. in home pos. register (0 bytes)
  67. #define SAVE_AS_HOME      0x0C  //Store the input bytes and timer val (0 bytes)
  68. #define EEPROM_CTRL   0x0D  //Store or retrieve values from EEPROM
  69. // LS-174
  70. #define ADD_PATHPOINT     0x0D  //Adds path point in path mode
  71. #define NOP       0x0E  //No operation - returns prev. defined status (0 bytes)
  72. #define HARD_RESET    0x0F  //RESET - no status is returned
  73.  
  74. // LS-174
  75. //Servo Module RESET_POS control byte bit definitions:
  76. //(if no control byte is used, reset is absolute)
  77. #define REL_HOME      0x01  //Reset position relative to current home position
  78.  
  79. //Servo Module STATUSITEMS bit definitions:
  80. #define SEND_POS      0x01  //4 bytes data
  81. #define SEND_AD       0x02  //1 byte
  82. #define SEND_VEL      0x04  //2 bytes
  83. #define SEND_AUX      0x08  //1 byte
  84. #define SEND_HOME     0x10  //4 bytes
  85. #define SEND_ID       0x20  //2 bytes
  86. #define SEND_PERROR   0x40  //2 bytes
  87. #define SEND_INPORTS      0x80  //3 bytes
  88. // LS-174
  89. #define SEND_NPOINTS      0x80  //1 byte
  90.  
  91. //Servo Module LOAD_TRAJ control byte bit definitions:
  92. #define LOAD_POS      0x01  //+4 bytes
  93. #define LOAD_VEL      0x02  //+4 bytes
  94. #define LOAD_ACC      0x04  //+4 bytes
  95. #define LOAD_PWM      0x08  //+1 byte
  96. #define ENABLE_SERVO      0x10  //1 = servo mode, 0 = PWM mode
  97. #define VEL_MODE      0x20  //1 = velocity mode, 0 = trap. position mode
  98. #define REVERSE       0x40  //1 = command neg. PWM or vel, 0 = positive
  99. // LS-174
  100. #define MOVE_REL      0x40  //1 = relative move, 0 = absolute move; works in advanced mode
  101. #define START_NOW     0x80  //1 = start now, 0 = wait for START_MOVE command
  102.  
  103. //Servo Module STOP_MOTOR control byte bit definitions:
  104. #define SRV_ENABLE_AMP    0x01  //1 = raise amp enable output, 0 = lower amp enable
  105. #define MOTOR_OFF     0x02  //set to turn motor off
  106. #define STOP_ABRUPT       0x04  //set to stop motor immediately
  107. #define STOP_SMOOTH   0x08  //set to decellerate motor smoothly
  108. #define STOP_HERE     0x10  //set to stop at position (4 add'l data bytes required)
  109. // LS-174
  110. #define ADV_MODE      0x20  //Enables advanced mode
  111.  
  112. //Servo Module IO_CTRL control byte bit definitions:
  113. #define SET_OUT1      0x01  //1 = set limit 1 output, 0 = clear limit 1 output
  114. #define SET_OUT2      0x02  //1 = set limit 2 output, 0 = clear limit 2 output
  115. #define IO1_IN        0x04  //1 = limit 1 is an input, 0 = limit 1 is an output
  116. #define IO2_IN        0x08  //1 = limit 2 is an input, 0 = limit 2 is an output
  117. #define WR_OUT1       0x10  //Write to output port 1
  118. #define WR_OUT2       0x20  //Write to output port 2
  119. #define WR_OUT3       0x40  //Write to output port 3
  120. // LS-174
  121. #define FAST_PATH     0x40  //0 = 30/60 Hz, 1 = 60/120 Hz
  122. #define WR_OUT4       0x80  //Write to output port 4
  123.  
  124. //Servo Module SET_HOMING control byte bit definitions:
  125. #define ON_LIMIT1     0x01  //home on change in limit 1
  126. #define ON_LIMIT2     0x02  //home on change in limit 2
  127. #define HOME_MOTOR_OFF    0x04  //turn motor off when homed
  128. #define ON_INDEX      0x08  //home on change in index
  129. #define HOME_STOP_ABRUPT  0x10  //stop abruptly when homed
  130. #define HOME_STOP_SMOOTH  0x20  //stop smoothly when homed
  131. #define ON_POS_ERR    0x40  //home on excessive position error
  132. #define ON_CUR_ERR    0x80  //home on overcurrent error
  133.  
  134. //Servo Module EEPROM_CTRL control byte bit definitions:
  135. #define STORE_GAINS       0x01  //Store servo gains
  136. #define FETCH_GAINS   0x02  //Retrieve stored gains
  137. #define STORE_VA          0x04  //Store velocity and acceleration
  138. #define FETCH_VA      0x08  //Retrieve stored velocity and acceleration
  139. #define STORE_OUTPUTS     0x10  //Store output port values
  140. #define FETCH_OUTPUTS     0x20  //Retrieve stored output port values
  141. #define STORE_SI_BIT      0x40  //Store "servo initialize" bit
  142. #define INIT_SERVO    0x80  //Initializes servo on power-up
  143.  
  144. // LS-174
  145. //Servo Module ADD_PATHPOINT frequency definitions
  146. #define P_30HZ        30    //30 hz path resolution
  147. #define P_60HZ        60    //60 hz path resolution
  148. #define P_120HZ       120   //120 hz path resolution
  149.  
  150. //Servo Module Status byte bit definitions:
  151. #define MOVE_DONE     0x01  //set when move done (trap. pos mode), when goal
  152.                                 //vel. has been reached (vel mode) or when not servoing
  153. #define CKSUM_ERROR   0x02  //checksum error in received command
  154. #define OVERCURRENT   0x04  //set on overcurrent condition (sticky bit)
  155. #define POWER_ON      0x08  //set when motor power is on
  156. #define POS_ERR       0x10  //set on excess pos. error (sticky bit)
  157. #define LIMIT1        0x20  //value of limit 1 input
  158. #define LIMIT2        0x40  //value of limit 2 input
  159. #define HOME_IN_PROG      0x80  //set while searching for home, cleared when home found
  160.  
  161. //Servo Module Auxilliary status byte bit definitions:
  162. #define INDEX         0x01  //value of the encoder index signal
  163. #define POS_WRAP      0x02  //set when 32 bit position counter wraps around
  164.                         //  (sticky bit)
  165. #define SERVO_ON      0x04  //set when position servo is operating
  166. #define ACCEL_DONE    0x08  //set when acceleration portion of a move is done
  167. #define SLEW_DONE     0x10  //set when slew portion of a move is done
  168. #define SERVO_OVERRUN     0x20  //set if servo takes longer than the specified
  169.                     //servo period to execute
  170. // LS-174
  171. #define PATH_MODE     0x40  //path mode is enabled
  172.  
  173. //Servo module function prototypes:
  174. SERVOMOD * ServoNewMod();
  175. DLLENTRY(BOOL) ServoGetStat(byte addr);
  176. DLLENTRY(long) ServoGetPos(byte addr);
  177. DLLENTRY(byte) ServoGetAD(byte addr);
  178. DLLENTRY(short int) ServoGetAD10(byte addr);
  179. DLLENTRY(short int) ServoGetVel(byte addr);
  180. DLLENTRY(byte) ServoGetAux(byte addr);
  181. DLLENTRY(long) ServoGetHome(byte addr);
  182. DLLENTRY(short int) ServoGetPError(byte addr);
  183. DLLENTRY(byte) ServoGetInport1(byte addr);
  184. DLLENTRY(byte) ServoGetInport2(byte addr);
  185. DLLENTRY(byte) ServoGetInport3(byte addr);
  186. DLLENTRY(byte) ServoGetOutport1(byte addr);
  187. DLLENTRY(byte) ServoGetOutport2(byte addr);
  188. DLLENTRY(byte) ServoGetOutport3(byte addr);
  189. DLLENTRY(byte) ServoGetOutport4(byte addr);
  190. DLLENTRY(long) ServoGetCmdPos(byte addr);
  191. DLLENTRY(long) ServoGetCmdVel(byte addr);
  192. DLLENTRY(long) ServoGetCmdAcc(byte addr);
  193. DLLENTRY(byte) ServoGetCmdPwm (byte addr);
  194. DLLENTRY(byte) ServoGetCmdAdc (byte addr);
  195. DLLENTRY(short int) ServoGetCmdAdc10 (byte addr);
  196. DLLENTRY(byte) ServoGetMoveCtrl(byte addr);
  197. DLLENTRY(byte) ServoGetStopCtrl(byte addr);
  198. DLLENTRY(byte) ServoGetHomeCtrl(byte addr);
  199. DLLENTRY(byte) ServoGetIoCtrl(byte addr);
  200. DLLENTRY(byte) ServoGetServoInit(byte addr);
  201. DLLENTRY(byte) ServoGetSDMode(byte addr);
  202. DLLENTRY(void) ServoGetGain(byte addr, short int * kp, short int * kd, short int * ki,
  203.           short int * il, byte * ol, byte * cl, short int * el, byte * sr, byte * dc);
  204. DLLENTRY(BOOL) ServoSetGain(byte addr, short int kp, short int kd, short int ki,
  205.           short int il, byte ol, byte cl, short int el, byte sr, byte dc);
  206. DLLENTRY(BOOL) ServoResetPos(byte addr);
  207. DLLENTRY(BOOL) ServoClearBits(byte addr);
  208. DLLENTRY(BOOL) ServoStopMotor(byte addr, byte mode);
  209. DLLENTRY(BOOL) ServoLoadTraj(byte addr, byte mode, long pos, long vel, long acc, byte pwm);
  210. DLLENTRY(BOOL) ServoLoadTraj10(byte addr, byte mode, long pos, long vel, long acc, short int pwm);
  211. DLLENTRY(BOOL) ServoStartMotion(byte groupaddr);
  212. DLLENTRY(BOOL) ServoSetHoming(byte addr, byte mode);
  213. DLLENTRY(BOOL) ServoSetOutputs(byte addr, byte mode, byte out1, byte out2, byte out3, byte out4);
  214. DLLENTRY(BOOL) ServoEEPROMCtrl(byte addr, byte mode, byte out1, byte out2, byte out3, byte out4);
  215.  
  216. // LS-174
  217. DLLENTRY(BOOL) ServoResetRelHome(byte addr);
  218. DLLENTRY(BOOL) ServoSetFastPath(byte addr, bool fast);
  219. DLLENTRY(void) ServoInitPath(byte addr);
  220. DLLENTRY(BOOL) ServoAddPathPoints(byte addr, int npoints, long *path, bool high_freq);
  221. DLLENTRY(BOOL) ServoStartPathMode(byte groupaddr);
  222. DLLENTRY(byte) ServoGetNPoints(byte addr);
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!