File content as of revision 0:f1e50cadca05:

//近藤科学製サーボKHR-6003HV用プログラム
//ICS3.0対応
//半2重通信(HalfDuplex)使用
//※mbedライブラリのリビジョンを43に戻してコンパイルしてください
//RX,TXを2.2kΩでプルアップ、RXに2.2kΩの電流制限をいれる
//回路図は近藤科学の「シリアルサーボ制御方法（１） 回路編」
//http://kondo-robot.com/faq/serial-servo-method-tech 参照のこと
#include "mbed.h"
#include "number.h"

#define READ_MODE
//#define PRINT
#define ID 2

Num n(USBTX,USBRX,3);
//Serial pc(USBTX,USBRX);
SerialHalfDuplex master(p13,p14);
//SerialHalfDuplex master(p9,p10);

DigitalOut leds[4]={LED1,LED2,LED3,LED4};
void set_degree(char id, float degree);
void setID(char id);
void readID(void);


int main() {
#ifdef READ_MODE
    char id=0;
    for(int i = 0;i<4;i++){
        leds[i]=0;
    }
    leds[0]=1;

    master.baud(115200);
    master.format(8,Serial::Even,1);
    setID((char)id);
    readID();

    for(int j = 1;j<4;j++){
        leds[j]=1;
        wait(0.25);
    }
#endif
#ifndef READ_MODE
    double deg = 0;
    printf("\r\nRESET\t");
    
    for(int i = 0;i<4;i++){
        leds[i]=0;
    }
    for(int j = 0;j<4;j++){
        leds[j]=1;
        wait(0.25);
    }
    
    master.baud(115200);
    master.format(8,Serial::Even,1);
    
    printf("deg:0\r\ndeg:");
    set_degree(ID,0);
    while(1){
        deg=n.get_number();
        printf("\r\ndeg:");
        set_degree(ID,deg);
        wait(0.1);
    }
#endif    
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void set_degree(char id, float degree){
    char pos_h,pos_l;
    long pos = 8000 * degree / 270 + 3500; // (11500-3500)÷270 = 29.62 ≒30
    pos_h = pos / 128;              //上位7bit
    pos_l = pos % 128;              //下位7bit
    // set position     
    master.putc(0x80+id);           // ID
    master.putc(pos_h);             //position(3500-11500 | 0-270deg)
    master.putc(pos_l);
    int ret1 = master.getc() - 128;
    int ret2 = master.getc();
    int ret3 = master.getc();
#ifdef PRINT
    printf("\tret : id=%d, H=%x, L=%x, deg=%d\r\n", ret1, ret2, ret3, ((ret2*128+ret3)-3500)*270/8000);
#endif
}
/* <setID readID> */
/* setID、readID使用時はサーボと1対1で接続すること */
void setID(char id){
    master.putc(0xE0+id);
    master.putc(0x01);
    master.putc(0x01);
    master.putc(0x01);
}

void readID(void){
    printf("reading...\t");
    master.putc(0xFF);
    master.putc(0x00);
    master.putc(0x00);
    master.putc(0x00);
    char id = master.getc() - 224;
    printf("ret : %d\r\n", id);
}