ServoSerial.h
Go to the documentation of this file.
00001 #ifndef _SERVO_SERIAL_H_
00002 #define _SERVO_SERIAL_H_
00003 
00004 #include <termios.h>
00005 #include <unistd.h>
00006 #include <stdio.h>
00007 #include <fcntl.h>
00008 #include <errno.h>
00009 #include <string.h>
00010 #include <math.h>
00011 #include <sys/types.h>
00012 #include <sys/stat.h>
00013 #include <fcntl.h>
00014 #include <sys/select.h>
00015 
00016 //http://www.futaba.co.jp/dbps_data/_material_/localhost/robot/servo/manuals/RS301CR_RS302CD_114.pdf
00017 
00018 #define cfsetspeed(term, baudrate)              \
00019   cfsetispeed(term, baudrate);                  \
00020   cfsetospeed(term, baudrate);
00021 
00022 
00023 class ServoSerial {
00024 public:
00025   int fd;
00026 
00027   ServoSerial(const char *devname)  {
00028     fd = open(devname, O_RDWR);
00029     if (fd<0) {
00030       char *pmesg = strerror(errno);
00031       fprintf (stderr, "[ServoSerial] failed to open %s: %s\n", devname, pmesg);
00032     }
00033 
00034     struct termios term;
00035     int res = tcgetattr(fd, &term);
00036     if (res<0) {
00037       char *pmesg = strerror(errno);
00038       fprintf (stderr, "[ServoSerial] failed to tcgetattr(): %s\n", pmesg);
00039     }
00040     cfmakeraw(&term);
00041     res = cfsetspeed(&term, 115200);
00042     if (res<0) {
00043       char *pmesg = strerror(errno);
00044       fprintf (stderr, "[ServoSerial] failed to cfsetspeed(): %s\n", pmesg);
00045     }
00046     term.c_iflag |= IGNPAR;            // Ignore characters with parity errors
00047     term.c_cflag |= (CLOCAL | CREAD);  // needed for QNX 6.3.2
00048     term.c_cflag &= ~PARENB;           // disable parity check
00049     term.c_cflag |= CS8;               // 8 data bit
00050     term.c_cflag &= ~CSTOPB;           // 1 stop bit
00051     term.c_lflag = IEXTEN;
00052     term.c_lflag &= ~(ECHO | ECHOCTL | ECHONL);  // disable ECHO 
00053 
00054 
00055     term.c_cc[VMIN] = 1;
00056     term.c_cc[VTIME] = 0;
00057 #ifdef __QNX__
00058     term.c_cflag &= ~(IHFLOW | OHFLOW);
00059 #endif
00060     res = tcsetattr(fd, TCSANOW, &term);
00061     if (res<0) {
00062       char *pmesg = strerror(errno);
00063       fprintf (stderr, "[ServoSerial] failed to tcsetattr(): %s\n", pmesg);
00064     }
00065 
00066     // clear existing packet
00067     clear_packet();
00068   }
00069 
00070   ~ServoSerial()  {
00071       close(fd);
00072   }
00073 
00074   int setReset(int id) {
00075     sendPacket(0xFAAF, id, 0x20, 0xFF, 0, 0, NULL);
00076   }
00077 
00078   int setPosition(int id, double rad) {// #30
00079     signed short angle = (signed short)(180/M_PI*rad*10);
00080     printf("[ServoSerial] setPosition %f, %04x\n", 180/M_PI*rad, angle);
00081     unsigned char data[2] = {0xff & angle, 0xff & (angle>>8)};
00082     sendPacket(0xFAAF, id, 0x00, 0x1E, 2, 1, data);
00083     return 0;
00084   }
00085 
00086   int setPositions(int len, int *id, double *rad) {// #30
00087     unsigned char data[3*len];
00088     for (int i = 0; i < len; i++) {
00089       short angle = (int)(180/M_PI*rad[i]*10);
00090       printf("[ServoSerial] setPositions %d: %f, %04x\n", id[i], 180/M_PI*rad[i], angle);
00091       data[i*3 + 0] = id[i];
00092       data[i*3 + 1] = 0xff & angle;
00093       data[i*3 + 2] = 0xff & (angle>>8);
00094     }
00095     sendPacket(0xFAAF, 0x00, 0x00, 0x1E, 3, len, data);
00096     return 0;
00097   }
00098 
00099   int setPosition(int id, double rad, double sec) {// #32
00100     short angle = (short)(180/M_PI*rad*10);
00101     short msec = (short)(sec * 100);
00102     printf("[ServoSerial] setPosition %f %f, %04x, %04x\n", 180/M_PI*rad, sec, angle, msec);
00103     unsigned char data[4] = {0xff & angle,0xff & (angle>>8),
00104                              0xff & msec, 0xff & (msec>>8) };
00105     sendPacket(0xFAAF, id, 0x00, 0x1E, 4, 1, data);
00106     return 0;
00107   }
00108 
00109   int setPositions(int len, int *id, double *rad, double *sec) {// #32
00110     unsigned char data[5*len];
00111     for (int i = 0; i < len; i++) {
00112       short angle = (int)(180/M_PI*rad[i]*10);
00113       short msec = (short)(sec[i] * 100);
00114       printf("[ServoSerial] setPositions %d: %f %f, %04x, %04x\n", id[i], 180/M_PI*rad[i], sec[i], angle, msec);
00115       data[i*5 + 0] = id[i];
00116       data[i*5 + 1] = 0xff & angle;
00117       data[i*5 + 2] = 0xff & (angle>>8);
00118       data[i*5 + 3] = 0xff & msec;
00119       data[i*5 + 4] = 0xff & (msec>>8);
00120     }
00121     sendPacket(0xFAAF, 0x00, 0x00, 0x1E, 5, len, data);
00122     return 0;
00123   }
00124 
00125   int setMaxTorque(int id, short percentage) {// #35
00126     unsigned char data[1];
00127     data[0] = percentage;
00128     sendPacket(0xFAAF, id, 0x00, 0x23, 1, 1, data);
00129     return 0;
00130   }
00131 
00132   int setTorqueOn(int id) { // #36
00133     printf("[ServoSerial] setTorqueOn(%d)\n", id);
00134     unsigned char data[1] = {0x01};
00135     sendPacket(0xFAAF, id, 0x00, 0x24, 1, 1, data);
00136     return 0;
00137   }
00138   int setTorqueOff(int id) { // #36
00139     printf("[ServoSerial] setTorqueOff(%d)\n", id);
00140     unsigned char data[1] = {0x00};
00141     sendPacket(0xFAAF, id, 0x00, 0x24, 1, 1, data);
00142     return 0;
00143   }
00144   int setTorqueBreak(int id) { // #36
00145     unsigned char data[1] = {0x02};
00146     sendPacket(0xFAAF, id, 0x00, 0x24, 1, 1, data);
00147     return 0;
00148   }
00149 
00150   int getPosition(int id, double *angle) { // #42
00151     if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
00152       clear_packet();
00153       return -1;
00154     }
00155     unsigned char data[0x12];
00156     if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
00157       clear_packet();
00158       return -1;
00159     }
00160     *angle = ((short)(data[1]<<8|data[0]))/10.0;
00161     return 0;
00162   }
00163 
00164   int getDuration(int id, double *duration) { // #44
00165     if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
00166       clear_packet();
00167       return -1;
00168     }
00169     unsigned char data[0x12];
00170     if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
00171       clear_packet();
00172       return -1;
00173     }
00174     *duration = ((short)(data[3]<<8|data[2]))*10.0;
00175     return 0;
00176   }
00177 
00178   int getSpeed(int id, double *duration) { // #46
00179     if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
00180       clear_packet();
00181       return -1;
00182     }
00183     unsigned char data[0x12];
00184     if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
00185       clear_packet();
00186       return -1;
00187     }
00188     *duration = ((short)(data[5]<<8|data[4]));
00189     return 0;
00190   }
00191 
00192   int getMaxTorque(int id, short *percentage) {
00193     if (sendPacket(0xFAAF, id, 0x0B, 0x00, 0, 1, NULL)<0) {
00194       clear_packet();
00195       return -1;
00196     }
00197     unsigned char data[0x0C];
00198     if (receivePacket(id, 0x1E, 0x0C, data) < 0) {
00199       clear_packet();
00200       return -1;
00201     }
00202     *percentage = (short)(data[5]);
00203     return 0;
00204   }
00205 
00206   int getTorque(int id, double *torque) { // #48
00207     if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
00208       clear_packet();
00209       return -1;
00210     }
00211     unsigned char data[0x12];
00212     if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
00213       clear_packet();
00214       return -1;
00215     }
00216     *torque = ((short)(data[7]<<8|data[6]));
00217     return 0;
00218   }
00219 
00220   int getTemperature(int id, double *temperature) { // #50
00221     if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
00222       clear_packet();
00223       return -1;
00224     }
00225     unsigned char data[0x12];
00226     if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
00227       clear_packet();
00228       return -1;
00229     }
00230     *temperature = ((short)(data[9]<<8|data[8]));
00231     return 0;
00232   }
00233 
00234   int getVoltage(int id, double *voltage) { // #52
00235     if (sendPacket(0xFAAF, id, 0x09, 0x00, 0, 1, NULL)<0) {
00236       clear_packet();
00237       return -1;
00238     }
00239     unsigned char data[0x12];
00240     if ( receivePacket(id, 0x2A, 0x12, data) < 0 ) {
00241       clear_packet();
00242       return -1;
00243     }
00244     *voltage = ((short)(data[11]<<8|data[10]))/100;
00245     return 0;
00246   }
00247 
00248   int getState(int id, unsigned char *data) {
00249     if (sendPacket(0xFAAF, id, 0x05, 0x00, 0, 1, NULL)<0) {
00250       clear_packet();
00251       return -1;
00252     }
00253     if ( receivePacket(id, 0x1E, 30, data) < 0 ) {
00254       clear_packet();
00255       return -1;
00256     }
00257     return 0;
00258   }
00259 
00260   int receivePacket(int id, int address, int length, unsigned char data[]){
00261     unsigned short header;
00262     unsigned char ids, flags, addr, len, count, sum;
00263     unsigned char s = 0;
00264     int ret;
00265 
00266     fprintf(stderr, "[ServoSerial] received: ");
00267     read(fd, &header, 2);
00268     printf("%02X ", header>>8); 
00269     printf("%02X ", 0xff&header); fflush(stdout);
00270     read(fd, &ids, 1);  s ^= ids;
00271     printf("%02X " , ids); fflush(stdout);
00272     read(fd, &flags, 1);s ^= flags;
00273     printf("%02X ", flags); fflush(stdout);
00274     read(fd, &addr, 1); s ^= addr;
00275     printf("%02X ", addr); fflush(stdout);
00276     read(fd, &len, 1);  s ^= len;
00277     printf("%02X ", len); fflush(stdout);
00278     read(fd, &count, 1);s ^= count;
00279     printf("%02X ", count); fflush(stdout);
00280     read(fd, data, length);
00281     for(int i = 0; i < length; i++){
00282       s ^= data[i];
00283       printf("%02X ", data[i]); fflush(stdout);
00284     }
00285     ret = read(fd, &sum, 1);
00286     printf("%02X - %02X\n", sum, s); fflush(stdout);
00287 
00288     if ( address != addr || length != len || sum != s ) {
00289       fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d)\n", id);
00290       ret = -1;
00291     }
00292 
00293     if ( flags & 0x0002 ) { // 0b00000010
00294       fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d) Fail to process received packet\n", id);
00295       ret = -1;
00296     }
00297 
00298     if ( flags & 0x0008 ) { // 0b00001000
00299       fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d) fail to write Flash ROM\n", id);
00300       ret = -1;
00301     }
00302 
00303     if ( flags & 0x0020 ) { // 0b00100000
00304       fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d) temperature limit warning\n", id);
00305       ret = -1;
00306     }
00307 
00308     if ( flags & 0x0080 ) { // 0b10000000
00309       fprintf(stderr, "[ServoSerial] Failed to receive packet from servo(id:%d) Temperature limit error\n", id);
00310       ret = -1;
00311     }
00312 
00313     return ret;
00314   }
00315 
00316   int sendPacket(int header, int id,
00317                  int flag,   int address,
00318                  int length, int count,
00319                  void *data){
00320 
00321     unsigned char c, sum = 0x00, packet[8+length*count];
00322     c = 0xff & (header>>8); packet[0] = c;
00323     c = 0xff & header;      packet[1] = c;
00324     c = id;      packet[2] = c;
00325     c = flag;    packet[3] = c;
00326     c = address; packet[4] = c;
00327     c = length;  packet[5] = c;
00328     c = count;   packet[6] = c;
00329     if ( length * count > 0 ) {
00330       memcpy((void *)(&(packet[7])), (void *)data, length*count);
00331     }
00332     for(int i = 2; i < 7 + length*count; i++){
00333       sum ^= packet[i];
00334     }
00335     packet[7+length*count] = sum;
00336 
00337     fprintf (stderr, "[ServoSerial] sending : ");
00338     for(int i = 0; i < 7 + length*count + 1; i++){
00339       fprintf(stderr, "%02X ", packet[i]);
00340     }
00341     fprintf(stderr, " - ");
00342 
00343     int ret1;
00344     ret1 = write(fd, packet, 8+length*count);
00345 
00346     fprintf(stderr, "%d\n", ret1);
00347 
00348     if (ret1 != 8+length*count) {
00349         fprintf(stderr, "[ServoSerial] Failed to send packet to servo(id:%d)\n", id);
00350         return -1;
00351     }
00352 
00353     unsigned char echo[8 + length*count];
00354     int ret2;
00355 
00356     // wait at most 200 msec
00357     fd_set set;
00358     struct timeval timeout;
00359     FD_ZERO(&set); /* clear the set */
00360     FD_SET(fd, &set); /* add our file descriptor to the set */
00361     timeout.tv_sec = 0;
00362     timeout.tv_usec = 200*1000;
00363     select(fd + 1, &set, NULL, NULL, &timeout);
00364     ret2 = read(fd, &echo, 8+length*count);
00365 
00366     
00367     fprintf(stderr, "[ServoSerial] received: ");
00368     for(int i = 0; i < ret2; i++){
00369       fprintf(stderr, "%02X ", echo[i]);
00370     }
00371     fprintf(stderr, " - %d\n", ret2);
00372     if (ret2 != ret1) {
00373       fprintf(stderr, "[ServoSerial] Failed to receive packet from servo (id:%d)\n", id);
00374       clear_packet();
00375       return -1;
00376     }
00377     
00378     for(int i = 0; i < 8 + length*count; i++){
00379       if (echo[i] != packet[i]) {
00380         fprintf(stderr, "[ServoSerial] Failed to confirm packet from servo(id:%d)\n", id);
00381         clear_packet();
00382         ret1 = -1;
00383       }
00384     }
00385 
00386     return ret1;
00387   }
00388 
00389   void clear_packet() {
00390     // clear existing packet
00391     int oldf = fcntl(fd, F_GETFL, 0);
00392     fcntl(fd, F_SETFL, oldf | O_NONBLOCK);
00393     unsigned char c;
00394     while ( read(fd, &c, 1) != EOF );
00395     fcntl(fd, F_SETFL, oldf);
00396   }
00397 };
00398 
00399 #endif //_SERVO_SERIAL_H_


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56