Pastebin launched a little side project called VERYVIRAL.com, check it out ;-) Want more features on Pastebin? Sign Up, it's FREE!
Guest

servo.h

By: a guest on Dec 4th, 2012  |  syntax: C++  |  size: 10.42 KB  |  views: 12  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  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);
clone this paste RAW Paste Data