From b70df71684373e2ca7b11c7f4a3ea6928e679119 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 10 Nov 2020 12:39:26 +0900 Subject: [PATCH 1/3] [ServoController] Add getROMData with R command on testServoSerial --- rtc/ServoController/ServoSerial.h | 12 ++++++++++++ rtc/ServoController/testServoSerial.cpp | 11 +++++++++++ 2 files changed, 23 insertions(+) diff --git a/rtc/ServoController/ServoSerial.h b/rtc/ServoController/ServoSerial.h index 98cef34190a..0eb08bd2fa0 100644 --- a/rtc/ServoController/ServoSerial.h +++ b/rtc/ServoController/ServoSerial.h @@ -257,6 +257,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; diff --git a/rtc/ServoController/testServoSerial.cpp b/rtc/ServoController/testServoSerial.cpp index fa1fc2b0d3e..5bf01128a4d 100644 --- a/rtc/ServoController/testServoSerial.cpp +++ b/rtc/ServoController/testServoSerial.cpp @@ -50,6 +50,7 @@ void usage() { printf(" c : getTemperature\n"); printf(" v : getVoltage\n"); printf(" S : getState\n"); + printf(" R : getROMData\n"); } int main() { @@ -160,6 +161,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(); } From ebec17a4e0b4e0d27e74c0491f98432821c5ac04 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 10 Nov 2020 14:31:04 +0900 Subject: [PATCH 2/3] [ServoController] Add setID with D command on testServoSerial --- rtc/ServoController/ServoSerial.h | 13 +++++++++++++ rtc/ServoController/testServoSerial.cpp | 6 ++++++ 2 files changed, 19 insertions(+) diff --git a/rtc/ServoController/ServoSerial.h b/rtc/ServoController/ServoSerial.h index 0eb08bd2fa0..047caaba962 100644 --- a/rtc/ServoController/ServoSerial.h +++ b/rtc/ServoController/ServoSerial.h @@ -75,6 +75,19 @@ 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 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); diff --git a/rtc/ServoController/testServoSerial.cpp b/rtc/ServoController/testServoSerial.cpp index 5bf01128a4d..b778c40eddf 100644 --- a/rtc/ServoController/testServoSerial.cpp +++ b/rtc/ServoController/testServoSerial.cpp @@ -33,6 +33,7 @@ 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(" o : setTorqueOn\n"); printf(" f : setTorqueOff\n"); printf(" b : setTorqueBreak\n"); @@ -79,6 +80,11 @@ 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 'o': serial->setTorqueOn(id); break; From 14fc304dc32405756183e22914e0d01d1ef5498c Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Tue, 10 Nov 2020 15:19:57 +0900 Subject: [PATCH 3/3] [ServoController] Add setReverse with e, E commands on testServoSerial --- rtc/ServoController/ServoSerial.h | 15 +++++++++++++++ rtc/ServoController/testServoSerial.cpp | 8 ++++++++ 2 files changed, 23 insertions(+) diff --git a/rtc/ServoController/ServoSerial.h b/rtc/ServoController/ServoSerial.h index 047caaba962..6481dec72f3 100644 --- a/rtc/ServoController/ServoSerial.h +++ b/rtc/ServoController/ServoSerial.h @@ -88,6 +88,21 @@ class ServoSerial { 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); diff --git a/rtc/ServoController/testServoSerial.cpp b/rtc/ServoController/testServoSerial.cpp index b778c40eddf..6ef5cd24644 100644 --- a/rtc/ServoController/testServoSerial.cpp +++ b/rtc/ServoController/testServoSerial.cpp @@ -34,6 +34,8 @@ void usage() { 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"); @@ -85,6 +87,12 @@ int main() { 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;