ServoSerial.h
Go to the documentation of this file.
1 #ifndef _SERVO_SERIAL_H_
2 #define _SERVO_SERIAL_H_
3 
4 #include <termios.h>
5 #include <unistd.h>
6 #include <stdio.h>
7 #include <fcntl.h>
8 #include <errno.h>
9 #include <string.h>
10 #include <math.h>
11 #include <sys/types.h>
12 #include <sys/stat.h>
13 #include <fcntl.h>
14 #include <sys/select.h>
15 
16 //http://www.futaba.co.jp/dbps_data/_material_/localhost/robot/servo/manuals/RS301CR_RS302CD_114.pdf
17 
18 #define cfsetspeed(term, baudrate) \
19  cfsetispeed(term, baudrate); \
20  cfsetospeed(term, baudrate);
21 
22 
23 class ServoSerial {
24 public:
25  int fd;
26 
27  ServoSerial(const char *devname) {
28  fd = open(devname, O_RDWR);
29  if (fd<0) {
30  char *pmesg = strerror(errno);
31  fprintf (stderr, "[ServoSerial] failed to open %s: %s\n", devname, pmesg);
32  }
33 
34  struct termios term;
35  int res = tcgetattr(fd, &term);
36  if (res<0) {
37  char *pmesg = strerror(errno);
38  fprintf (stderr, "[ServoSerial] failed to tcgetattr(): %s\n", pmesg);
39  }
40  cfmakeraw(&term);
41  res = cfsetspeed(&term, 115200);
42  if (res<0) {
43  char *pmesg = strerror(errno);
44  fprintf (stderr, "[ServoSerial] failed to cfsetspeed(): %s\n", pmesg);
45  }
46  term.c_iflag |= IGNPAR; // Ignore characters with parity errors
47  term.c_cflag |= (CLOCAL | CREAD); // needed for QNX 6.3.2
48  term.c_cflag &= ~PARENB; // disable parity check
49  term.c_cflag |= CS8; // 8 data bit
50  term.c_cflag &= ~CSTOPB; // 1 stop bit
51  term.c_lflag = IEXTEN;
52  term.c_lflag &= ~(ECHO | ECHOCTL | ECHONL); // disable ECHO
53 
54 
55  term.c_cc[VMIN] = 1;
56  term.c_cc[VTIME] = 0;
57 #ifdef __QNX__
58  term.c_cflag &= ~(IHFLOW | OHFLOW);
59 #endif
60  res = tcsetattr(fd, TCSANOW, &term);
61  if (res<0) {
62  char *pmesg = strerror(errno);
63  fprintf (stderr, "[ServoSerial] failed to tcsetattr(): %s\n", pmesg);
64  }
65 
66  // clear existing packet
67  clear_packet();
68  }
69 
71  close(fd);
72  }
73 
74  int setReset(int id) {
75  sendPacket(0xFAAF, id, 0x20, 0xFF, 0, 0, NULL);
76  }
77 
78  int setPosition(int id, double rad) {// #30
79  signed short angle = (signed short)(180/M_PI*rad*10);
80  printf("[ServoSerial] setPosition %f, %04x\n", 180/M_PI*rad, angle);
81  unsigned char data[2] = {0xff & angle, 0xff & (angle>>8)};
82  sendPacket(0xFAAF, id, 0x00, 0x1E, 2, 1, data);
83  return 0;
84  }
85 
86  int setPositions(int len, int *id, double *rad) {// #30
87  unsigned char data[3*len];
88  for (int i = 0; i < len; i++) {
89  short angle = (int)(180/M_PI*rad[i]*10);
90  printf("[ServoSerial] setPositions %d: %f, %04x\n", id[i], 180/M_PI*rad[i], angle);
91  data[i*3 + 0] = id[i];
92  data[i*3 + 1] = 0xff & angle;
93  data[i*3 + 2] = 0xff & (angle>>8);
94  }
95  sendPacket(0xFAAF, 0x00, 0x00, 0x1E, 3, len, data);
96  return 0;
97  }
98 
99  int setPosition(int id, double rad, double sec) {// #32
100  short angle = (short)(180/M_PI*rad*10);
101  short msec = (short)(sec * 100);
102  printf("[ServoSerial] setPosition %f %f, %04x, %04x\n", 180/M_PI*rad, sec, angle, msec);
103  unsigned char data[4] = {0xff & angle,0xff & (angle>>8),
104  0xff & msec, 0xff & (msec>>8) };
105  sendPacket(0xFAAF, id, 0x00, 0x1E, 4, 1, data);
106  return 0;
107  }
108 
109  int setPositions(int len, int *id, double *rad, double *sec) {// #32
110  unsigned char data[5*len];
111  for (int i = 0; i < len; i++) {
112  short angle = (int)(180/M_PI*rad[i]*10);
113  short msec = (short)(sec[i] * 100);
114  printf("[ServoSerial] setPositions %d: %f %f, %04x, %04x\n", id[i], 180/M_PI*rad[i], sec[i], angle, msec);
115  data[i*5 + 0] = id[i];
116  data[i*5 + 1] = 0xff & angle;
117  data[i*5 + 2] = 0xff & (angle>>8);
118  data[i*5 + 3] = 0xff & msec;
119  data[i*5 + 4] = 0xff & (msec>>8);
120  }
121  sendPacket(0xFAAF, 0x00, 0x00, 0x1E, 5, len, data);
122  return 0;
123  }
124 
125  int setMaxTorque(int id, short percentage) {// #35
126  unsigned char data[1];
127  data[0] = percentage;
128  sendPacket(0xFAAF, id, 0x00, 0x23, 1, 1, data);
129  return 0;
130  }
131 
132  int setTorqueOn(int id) { // #36
133  printf("[ServoSerial] setTorqueOn(%d)\n", id);
134  unsigned char data[1] = {0x01};
135  sendPacket(0xFAAF, id, 0x00, 0x24, 1, 1, data);
136  return 0;
137  }
138  int setTorqueOff(int id) { // #36
139  printf("[ServoSerial] setTorqueOff(%d)\n", id);
140  unsigned char data[1] = {0x00};
141  sendPacket(0xFAAF, id, 0x00, 0x24, 1, 1, data);
142  return 0;
143  }
144  int setTorqueBreak(int id) { // #36
145  unsigned char data[1] = {0x02};
146  sendPacket(0xFAAF, id, 0x00, 0x24, 1, 1, data);
147  return 0;
148  }
149 
150  int getPosition(int id, double *angle) { // #42
151  if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
152  clear_packet();
153  return -1;
154  }
155  unsigned char data[0x12];
156  if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
157  clear_packet();
158  return -1;
159  }
160  *angle = ((short)(data[1]<<8|data[0]))/10.0;
161  return 0;
162  }
163 
164  int getDuration(int id, double *duration) { // #44
165  if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
166  clear_packet();
167  return -1;
168  }
169  unsigned char data[0x12];
170  if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
171  clear_packet();
172  return -1;
173  }
174  *duration = ((short)(data[3]<<8|data[2]))*10.0;
175  return 0;
176  }
177 
178  int getSpeed(int id, double *duration) { // #46
179  if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
180  clear_packet();
181  return -1;
182  }
183  unsigned char data[0x12];
184  if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
185  clear_packet();
186  return -1;
187  }
188  *duration = ((short)(data[5]<<8|data[4]));
189  return 0;
190  }
191 
192  int getMaxTorque(int id, short *percentage) {
193  if (sendPacket(0xFAAF, id, 0x0B, 0x00, 0, 1, NULL)<0) {
194  clear_packet();
195  return -1;
196  }
197  unsigned char data[0x0C];
198  if (receivePacket(id, 0x1E, 0x0C, data) < 0) {
199  clear_packet();
200  return -1;
201  }
202  *percentage = (short)(data[5]);
203  return 0;
204  }
205 
206  int getTorque(int id, double *torque) { // #48
207  if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
208  clear_packet();
209  return -1;
210  }
211  unsigned char data[0x12];
212  if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
213  clear_packet();
214  return -1;
215  }
216  *torque = ((short)(data[7]<<8|data[6]));
217  return 0;
218  }
219 
220  int getTemperature(int id, double *temperature) { // #50
221  if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
222  clear_packet();
223  return -1;
224  }
225  unsigned char data[0x12];
226  if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
227  clear_packet();
228  return -1;
229  }
230  *temperature = ((short)(data[9]<<8|data[8]));
231  return 0;
232  }
233 
234  int getVoltage(int id, double *voltage) { // #52
235  if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
236  clear_packet();
237  return -1;
238  }
239  unsigned char data[0x12];
240  if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
241  clear_packet();
242  return -1;
243  }
244  *voltage = ((short)(data[11]<<8|data[10]))/100;
245  return 0;
246  }
247 
248  int getState(int id, unsigned char *data) {
249  if (sendPacket(0xFAAF, id, 0x05, 0x00, 0, 1, NULL)<0) {
250  clear_packet();
251  return -1;
252  }
253  if ( receivePacket(id, 0x1E, 30, data) < 0 ) {
254  clear_packet();
255  return -1;
256  }
257  return 0;
258  }
259 
260  int receivePacket(int id, int address, int length, unsigned char data[]){
261  unsigned short header;
262  unsigned char ids, flags, addr, len, count, sum;
263  unsigned char s = 0;
264  int ret;
265 
266  fprintf(stderr, "[ServoSerial] received: ");
267  read(fd, &header, 2);
268  printf("%02X ", header>>8);
269  printf("%02X ", 0xff&header); fflush(stdout);
270  read(fd, &ids, 1); s ^= ids;
271  printf("%02X " , ids); fflush(stdout);
272  read(fd, &flags, 1);s ^= flags;
273  printf("%02X ", flags); fflush(stdout);
274  read(fd, &addr, 1); s ^= addr;
275  printf("%02X ", addr); fflush(stdout);
276  read(fd, &len, 1); s ^= len;
277  printf("%02X ", len); fflush(stdout);
278  read(fd, &count, 1);s ^= count;
279  printf("%02X ", count); fflush(stdout);
280  read(fd, data, length);
281  for(int i = 0; i < length; i++){
282  s ^= data[i];
283  printf("%02X ", data[i]); fflush(stdout);
284  }
285  ret = read(fd, &sum, 1);
286  printf("%02X - %02X\n", sum, s); fflush(stdout);
287 
288  if ( address != addr || length != len || sum != s ) {
289  fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d)\n", id);
290  ret = -1;
291  }
292 
293  if ( flags & 0x0002 ) { // 0b00000010
294  fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d) Fail to process received packet\n", id);
295  ret = -1;
296  }
297 
298  if ( flags & 0x0008 ) { // 0b00001000
299  fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d) fail to write Flash ROM\n", id);
300  ret = -1;
301  }
302 
303  if ( flags & 0x0020 ) { // 0b00100000
304  fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d) temperature limit warning\n", id);
305  ret = -1;
306  }
307 
308  if ( flags & 0x0080 ) { // 0b10000000
309  fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d) Temperature limit error\n", id);
310  ret = -1;
311  }
312 
313  return ret;
314  }
315 
316  int sendPacket(int header, int id,
317  int flag, int address,
318  int length, int count,
319  void *data){
320 
321  unsigned char c, sum = 0x00, packet[8+length*count];
322  c = 0xff & (header>>8); packet[0] = c;
323  c = 0xff & header; packet[1] = c;
324  c = id; packet[2] = c;
325  c = flag; packet[3] = c;
326  c = address; packet[4] = c;
327  c = length; packet[5] = c;
328  c = count; packet[6] = c;
329  if ( length * count > 0 ) {
330  memcpy((void *)(&(packet[7])), (void *)data, length*count);
331  }
332  for(int i = 2; i < 7 + length*count; i++){
333  sum ^= packet[i];
334  }
335  packet[7+length*count] = sum;
336 
337  fprintf (stderr, "[ServoSerial] sending : ");
338  for(int i = 0; i < 7 + length*count + 1; i++){
339  fprintf(stderr, "%02X ", packet[i]);
340  }
341  fprintf(stderr, " - ");
342 
343  int ret1;
344  ret1 = write(fd, packet, 8+length*count);
345 
346  fprintf(stderr, "%d\n", ret1);
347 
348  if (ret1 != 8+length*count) {
349  fprintf(stderr, "[ServoSerial] Failed to send packet to servo(id:%d)\n", id);
350  return -1;
351  }
352 
353  unsigned char echo[8 + length*count];
354  int ret2;
355 
356  // wait at most 200 msec
357  fd_set set;
358  struct timeval timeout;
359  FD_ZERO(&set); /* clear the set */
360  FD_SET(fd, &set); /* add our file descriptor to the set */
361  timeout.tv_sec = 0;
362  timeout.tv_usec = 200*1000;
363  select(fd + 1, &set, NULL, NULL, &timeout);
364  ret2 = read(fd, &echo, 8+length*count);
365 
366 
367  fprintf(stderr, "[ServoSerial] received: ");
368  for(int i = 0; i < ret2; i++){
369  fprintf(stderr, "%02X ", echo[i]);
370  }
371  fprintf(stderr, " - %d\n", ret2);
372  if (ret2 != ret1) {
373  fprintf(stderr, "[ServoSerial] Failed to receive packet from servo (id:%d)\n", id);
374  clear_packet();
375  return -1;
376  }
377 
378  for(int i = 0; i < 8 + length*count; i++){
379  if (echo[i] != packet[i]) {
380  fprintf(stderr, "[ServoSerial] Failed to confirm packet from servo(id:%d)\n", id);
381  clear_packet();
382  ret1 = -1;
383  }
384  }
385 
386  return ret1;
387  }
388 
389  void clear_packet() {
390  // clear existing packet
391  int oldf = fcntl(fd, F_GETFL, 0);
392  fcntl(fd, F_SETFL, oldf | O_NONBLOCK);
393  unsigned char c;
394  while ( read(fd, &c, 1) != EOF );
395  fcntl(fd, F_SETFL, oldf);
396  }
397 };
398 
399 #endif //_SERVO_SERIAL_H_
ret1
int sendPacket(int header, int id, int flag, int address, int length, int count, void *data)
Definition: ServoSerial.h:316
int getSpeed(int id, double *duration)
Definition: ServoSerial.h:178
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
int getVoltage(int id, double *voltage)
Definition: ServoSerial.h:234
int getPosition(int id, double *angle)
Definition: ServoSerial.h:150
int getDuration(int id, double *duration)
Definition: ServoSerial.h:164
std_msgs::Header * header(M &m)
int receivePacket(int id, int address, int length, unsigned char data[])
Definition: ServoSerial.h:260
void clear_packet()
Definition: ServoSerial.h:389
png_uint_32 i
int temperature(int s)
png_uint_32 int flags
int setTorqueOff(int id)
Definition: ServoSerial.h:138
int getTemperature(int id, double *temperature)
Definition: ServoSerial.h:220
int getState(int id, unsigned char *data)
Definition: ServoSerial.h:248
int setMaxTorque(int id, short percentage)
Definition: ServoSerial.h:125
int getTorque(int id, double *torque)
Definition: ServoSerial.h:206
ret2
int getMaxTorque(int id, short *percentage)
Definition: ServoSerial.h:192
int setTorqueBreak(int id)
Definition: ServoSerial.h:144
int setPosition(int id, double rad)
Definition: ServoSerial.h:78
int setTorqueOn(int id)
Definition: ServoSerial.h:132
#define M_PI
typedef int
int setReset(int id)
Definition: ServoSerial.h:74
int setPosition(int id, double rad, double sec)
Definition: ServoSerial.h:99
int setPositions(int len, int *id, double *rad)
Definition: ServoSerial.h:86
int setPositions(int len, int *id, double *rad, double *sec)
Definition: ServoSerial.h:109
ServoSerial(const char *devname)
Definition: ServoSerial.h:27
static int id
JSAMPIMAGE data
#define cfsetspeed(term, baudrate)
Definition: ServoSerial.h:18


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51