![](/media/cache/profiles/d741d4ae35aff8fd3674206068e40684.jpg.50x50_q85.jpg)
// 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
main.cpp
- Committer:
- mfurukawa
- Date:
- 2017-04-25
- Revision:
- 1:859610213d81
- Parent:
- 0:19618cb94138
File content as of revision 1:859610213d81:
// 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 // // Serial FIFO (16bytes) // http://qiita.com/matsujirushi/items/b6801887f751a8f5e6cd // // Servo ICS Command Reference // http://kondo-robot.com/faq/serial-servo-method-tech-2 // // Logic Level Converter // https://www.switch-science.com/catalog/1193/ #define PI 3.14159 #include "mbed.h" #include "math.h" #include "sequence.h" Serial pc(USBTX,USBRX); // tx, rx Serial master1(p9,p10); // tx, rx Serial master2(p13,p14); // tx, rx Serial master3(p28,p27); // tx, rx int ICS_set_pos1(const char id, const int pos) { // out of range error // http://kondo-robot.com/faq/serial-servo-method-tech-2 if (pos > 11500 || pos < 3500) return -1; b[0] = 0x80 | id; // Set Pos Command "0b100 = 0x04" b[1] = (pos & 0x3F80)>>7; // Position (High 7 Bytes) b[2] = pos & 0x7f; // Position (Low 7 Bytes) 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]); return 0; } int ICS_set_pos2(const char id, const int pos) { // out of range error // http://kondo-robot.com/faq/serial-servo-method-tech-2 if (pos > 11500 || pos < 3500) return -1; b[0] = 0x80 | id; // Set Pos Command "0b100 = 0x04" b[1] = (pos & 0x3F80)>>7; // Position (High 7 Bytes) b[2] = pos & 0x7f; // Position (Low 7 Bytes) master2.putc(b[0]); master2.putc(b[1]); master2.putc(b[2]); wait_us(800); //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]); return 0; } int ICS_set_pos3(const char id, const int pos) { // out of range error // http://kondo-robot.com/faq/serial-servo-method-tech-2 if (pos > 11500 || pos < 3500) return -1; b[0] = 0x80 | id; // Set Pos Command "0b100 = 0x04" b[1] = (pos & 0x3F80)>>7; // Position (High 7 Bytes) b[2] = pos & 0x7f; // Position (Low 7 Bytes) master3.putc(b[0]); master3.putc(b[1]); master3.putc(b[2]); wait_us(1800); //pc.printf("\n\rid %d pos %d %x %x %x", id, pos, b[0], b[1], b[2]); return 0; } int main() { pc.baud(115200); pc.printf("\n\r%s\n\r", TITLE); master1.baud(115200); master2.baud(115200); master3.baud(115200); 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; while(1){ t=t+1; 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); pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos2(6,pos); pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos2(7,pos); pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos2(8,pos); pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos2(9,pos); pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos2(10,pos); pos = (int)(4000.0 * sin( t/180.0*PI) + 7500.0); ICS_set_pos3(0,pos); pos = (int)(4000.0 * sin(0.3*t/180.0*PI) + 7500.0); ICS_set_pos3(2,pos); pos = (int)(4000.0 * sin(0.7*t/180.0*PI) + 7500.0); ICS_set_pos3(6,pos); pos = (int)(4000.0 * sin(1.3*t/180.0*PI) + 7500.0); ICS_set_pos3(7,pos); pos = (int)(4000.0 * sin(1.4*t/180.0*PI) + 7500.0); ICS_set_pos3(8,pos); pos = (int)(4000.0 * sin(1.7*t/180.0*PI) + 7500.0); ICS_set_pos3(9,pos); pos = (int)(4000.0 * sin(0.4*t/180.0*PI) + 7500.0); ICS_set_pos3(10,pos); //wait_ms(5); } } // SerialHalfDuplex (revision 43 @mbed library) // http://d.hatena.ne.jp/rinie/20121125/1353770901 //