File content as of revision 13:678870d8f851:

#include "KondoServo.h"

/*KondoServo::KondoServo():master(tx,rx)
{
}*/
KondoServo::~KondoServo()
{
}


KondoServo::KondoServo(PinName txPin,PinName rxPin, int quantity, unsigned int baud):master(txPin,rxPin)
{
    baudrate = baud;
    master.baud(baudrate);
    master.format(8,Serial::Even,1);
    mode = (quantity==1) ? single : multi;
}

void KondoServo::init(int baud)
{
    baudrate = baud;
    master.baud(baudrate);
    master.format(8,Serial::Even,1);
}
void KondoServo::init()
{
    master.baud(baudrate);
    master.format(8,Serial::Even,1);
}
void KondoServo::set_degree(int id,float degree)
{
    u8 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
    master.putc(0x80+id);// ID
    master.putc(pos_h);
    master.putc(pos_l);

    /*int ret1 = master.getc() - 128;
    int ret2 = master.getc();
    int ret3 = master.getc();*/

    //return ((ret2*128+ret3)-3500)*270/8000;
}
void KondoServo::setID(u8 id)
{
    if(mode == single) {
        master.putc(0xE0+id);
        master.putc(0x01);
        master.putc(0x01);
        master.putc(0x01);
    }
}
u8 KondoServo::readID()
{
    if(mode == single) {
        master.putc(0xFF);
        master.putc(0x00);
        master.putc(0x00);
        master.putc(0x00);
        u8 readID = master.getc() - 224;
        return readID;
    } else {
        return 0xff;
    }
}

void KondoServo::setSpeed(int id, u8 speed)
{
    master.putc(0xC0+id);
    master.putc(0x02);
    master.putc(speed);
}

float KondoServo::readSpeed(int id)
{
    master.putc(0xA0);
    master.putc(0x02);

    /*int ret1 = master.getc();
    int ret2 = master.getc();
    int ret3 = master.getc();
    int ret4 = master.getc();
    int ret5 = master.getc();
    int ret6 = master.getc();

    printf("%d\n%d\n%d\n%d\n%d\n%d\n",ret1,ret2,ret3,ret4,ret5,ret6);
    */

    wait_ms(10);

    for(int i=0; i<10; i++) {
        //wait_ms(10);
        int ret = master.getc();
        printf("%d\n",ret);
    }

    /*int ret=0;
    while(ret != 0x20+id){
        ret = master.getc();
        printf("%d\n",ret);
    }*/
    //return (float)(((ret5*128+ret6)-3500)*270/8000);
    return 0;
}