File content as of revision 0:d2be68a55537:

// 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
//