// Masahiro Furukawa - m.furukawa@ist.osaka-u.ac.jp // Apr 25, 2017 // #define TITLE "KONDO Kagaku ICS Servo Controller rev0.1" // // // reference : // Serial Control // http://d.hatena.ne.jp/rinie/20110408/1302265191 // https://developer.mbed.org/users/okini3939/notebook/Serial_jp/ // // half dublex communication // http://kondo-robot.com/faq/serial-servo-method-tech // // Servo ICS Command Reference // http://kondo-robot.com/faq/serial-servo-method-tech-2 // // Logic Level Converter // https://www.switch-science.com/catalog/1193/
Masahiro Furukawa - m.furukawa@ist.osaka-u.ac.jp Apr 25, 2017
reference :
- Serial Control
half dublex communication http://kondo-robot.com/faq/serial-servo-method-tech
- Servo ICS Command Reference
- ogic Level Converter
Diff: main.cpp
- Revision:
- 1:859610213d81
- Parent:
- 0:19618cb94138
--- a/main.cpp Tue Apr 25 05:41:38 2017 +0000 +++ b/main.cpp Tue Apr 25 05:58:33 2017 +0000 @@ -28,11 +28,11 @@ #include "sequence.h" Serial pc(USBTX,USBRX); // tx, rx -Serial master(p9,p10); // tx, rx +Serial master1(p9,p10); // tx, rx Serial master2(p13,p14); // tx, rx Serial master3(p28,p27); // tx, rx -int ICS_set_pos(const char id, const int pos) +int ICS_set_pos1(const char id, const int pos) { // out of range error // http://kondo-robot.com/faq/serial-servo-method-tech-2 @@ -42,9 +42,9 @@ b[1] = (pos & 0x3F80)>>7; // Position (High 7 Bytes) b[2] = pos & 0x7f; // Position (Low 7 Bytes) - master.putc(b[0]); - master.putc(b[1]); - master.putc(b[2]); + master1.putc(b[0]); + master1.putc(b[1]); + master1.putc(b[2]); wait_us(800); //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]); @@ -85,7 +85,7 @@ master3.putc(b[0]); master3.putc(b[1]); master3.putc(b[2]); - wait_us(800); + wait_us(1800); //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]); @@ -97,10 +97,12 @@ pc.baud(115200); pc.printf("\n\r%s\n\r", TITLE); - master.baud(115200); + master1.baud(115200); master2.baud(115200); master3.baud(115200); - master.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit + master1.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit + master2.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit + master3.format(8, Serial::Even, 1); // data length = 8bit, parity = even, stop bit = 1bit double t=0; int pos=0; @@ -108,13 +110,13 @@ while(1){ t=t+1; - pos = (int)(4000.0 * sin( t/180.0*PI) + 7500.0); ICS_set_pos(0,pos); - pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos(2,pos); - pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos(6,pos); - pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos(7,pos); - pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos(8,pos); - pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos(9,pos); - pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos(10,pos); + pos = (int)(4000.0 * sin( t/180.0*PI) + 7500.0); ICS_set_pos1(0,pos); + pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos1(2,pos); + pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos1(6,pos); + pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos1(7,pos); + pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos1(8,pos); + pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos1(9,pos); + pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos1(10,pos); pos = (int)(4000.0 * sin( t/180.0*PI) + 7500.0); ICS_set_pos2(0,pos); pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos2(2,pos);