[ServoController] Add new functions to testServoSerial

rtc/ServoController/ServoSerial.h

@@ -75,6 +75,34 @@ class ServoSerial {
     sendPacket(0xFAAF, id, 0x20, 0xFF, 0, 0, NULL);
   }
 
+  int setID(int id, unsigned char new_id) {// #4
+    if (new_id < 1 || 127 < new_id) {
+      fprintf(stderr, "[ServoSerial] Given ID %d is out of range\n", new_id);
+      return -1;
+    }
+    printf("[ServoSerial] setID %d: %d\n", id, new_id);
+    sendPacket(0xFAAF, id, 0x00, 0x04, 1, 1, &new_id);
+    sendPacket(0xFAAF, new_id, 0x40, 0xFF, 0, 0, NULL);  // Write to Flash ROM
+    // I don't know why, but after the command above, powering off servo is required to get return packet from servo
+    // setReset instead of powering off doesn't work
+    return 0;
+  }
+
+  int setReverse(int id, int is_reverse) {// #5
+    if (is_reverse != 0 && is_reverse != 1) {
+      printf("[ServoSerial] Change is_reverse %d to 1 as is_reverse should be 0 (false) or 1 (true)\n", is_reverse);
+      is_reverse = 1;
+    }
+    printf("[ServoSerial] setReverse %d: %d\n", id, is_reverse);
+    unsigned char data[1];
+    data[0] = is_reverse;
+    sendPacket(0xFAAF, id, 0x00, 0x05, 1, 1, data);
+    sendPacket(0xFAAF, id, 0x40, 0xFF, 0, 0, NULL);  // Write to Flash ROM
+    // I don't know why, but after the command above, powering off servo is required to get return packet from servo
+    // setReset instead of powering off doesn't work
+    return 0;
+  }
+
   int setPosition(int id, double rad) {// #30
     signed short angle = (signed short)(180/M_PI*rad*10);
     printf("[ServoSerial] setPosition %f, %04x\n", 180/M_PI*rad, angle);
@@ -257,6 +285,18 @@ class ServoSerial {
     return 0;
   }
 
+  int getROMData(int id, unsigned char *data) {
+    if (sendPacket(0xFAAF, id, 0x03, 0x00, 0, 1, NULL)<0) {
+      clear_packet();
+      return -1;
+    }
+    if ( receivePacket(id, 0x00, 30, data) < 0 ) {
+      clear_packet();
+      return -1;
+    }
+    return 0;
+  }
+
   int receivePacket(int id, int address, int length, unsigned char data[]){
     unsigned short header;
     unsigned char ids, flags, addr, len, count, sum;

rtc/ServoController/testServoSerial.cpp

@@ -33,6 +33,9 @@ void usage() {
     printf("servo test program, current id = %d\n", id);
     printf(" 1-9 : change id\n");
     printf("   r : setReset\n");
+    printf("   D : setID\n");
+    printf("   e : setReverse to 1\n");
+    printf("   E : setReverse to 0\n");
     printf("   o : setTorqueOn\n");
     printf("   f : setTorqueOff\n");
     printf("   b : setTorqueBreak\n");
@@ -50,6 +53,7 @@ void usage() {
     printf("   c : getTemperature\n");
     printf("   v : getVoltage\n");
     printf("   S : getState\n");
+    printf("   R : getROMData\n");
 }
 
 int main() {
@@ -78,6 +82,17 @@ int main() {
             case 'r':
                 serial->setReset(id);
                 break;
+            case 'D':
+                {printf("please type new ID and press Enter\n");
+                 unsigned char new_id = getchar()-'0';
+                 serial->setID(id, new_id);}
+                break;
+            case 'e':
+                serial->setReverse(id, 1);
+                break;
+            case 'E':
+                serial->setReverse(id, 0);
+                break;
             case 'o':
                 serial->setTorqueOn(id);
                 break;
@@ -160,6 +175,16 @@ int main() {
                 }
 		fprintf(stderr, "\n");
                 break;
+            case 'R':
+                {unsigned char data[30];
+                 serial->getROMData(id, data);
+		 fprintf(stderr, "ROM data =");
+                 for(int i = 0; i < 30; i++) {
+                         fprintf(stderr, " %02X", data[i]);
+                         if ( i % 10 == 9 ) fprintf(stderr, " : ");
+                 }
+		 fprintf(stderr, "\n");}
+                break;
             }
             usage();
         }