koki konishi / KRS

KRS ICSサーボモータ用ライブラリ

初期化 KRS name(tx,rx);

位置指定 void SetPosition(id, position); [position : 0 3500 ~ 11500]

速度設定 void SetSpeed(id, speed); [speed : 0 ~ 127]

ストレッチ設定 void SetStretch(id, stretch); [stretch : 0 ~ 127]

電流制限設定 void SetCurrent(id, rotate, value : 0 ~ 127); [rotate : KRS::F KRS::R KRS::COM, current : 0 ~ 64]

ID書き込み void WriteAddress(id);

id : 0x00 ~ 0x1F

KRS.cpp

Committer:
koki_konishi
Date:
23 months ago
Revision:
2:431a802d01e2
Parent:
1:50d0eb774b0d

File content as of revision 2:431a802d01e2:

#include "KRS.h"
#include "mbed.h"

KRS::KRS(PinName tx,PinName rx) : _serial(tx,rx){
    _serial.baud(115200);
    _serial.format(8,Serial::Even,1);
}

void KRS::WriteAddress(uint8_t address){
    uint8_t ins = 0b11100000;
    if(address <= 0x1F){
        ins |= address;
    }else{
        ins |= 0x1F;
    }
    
    _serial.putc(ins);
    _serial.putc(0x01);
    _serial.putc(0x01);
    _serial.putc(0x01);
    
    wait_ms(10);
}

void KRS::SetStretch(uint8_t address, uint8_t value){
    uint8_t ins = 0b11000000;
    if(address <= 0x1F){
        ins |= address;
    }else{
        ins |= 0x1F;
    }
    
    if(value > 0x80){
        value = 0x80;
    }
    
    _serial.putc(ins);
    _serial.putc(0x01);
    _serial.putc(value);
    
    wait_ms(10);
}

void KRS::SetSpeed(uint8_t address, uint8_t value){
    uint8_t ins = 0b11000000;
    if(address <= 0x1F){
        ins |= address;
    }else{
        ins |= 0x1F;
    }
    
    if(value > 0x80){
        value = 0x80;
    }
    
    _serial.putc(ins);
    _serial.putc(0x02);
    _serial.putc(value);
    
    wait_ms(10);
}

void KRS::SetCurrent(uint8_t address, ROTATE rotate, uint8_t value){
    uint8_t ins = 0b11000000;
    if(address <= 0x1F){
        ins |= address;
    }else{
        ins |= 0x1F;
    }
    
    if(value > 0x40){
        value = 0x40;
    }
    
    switch(rotate){
        case F:
            _serial.putc(ins);
            _serial.putc(0x03);
            _serial.putc(value);
            break;
        case R:
            _serial.putc(ins);
            _serial.putc(0x03);
            _serial.putc(value+64);
            break;
        case COM:
            _serial.putc(ins);
            _serial.putc(0x03);
            _serial.putc(value);
            wait_ms(10);
            _serial.putc(ins);
            _serial.putc(0x03);
            _serial.putc(value+64);
            break;
    }
    
    wait_ms(10);
}

void KRS::SetPosition(uint8_t address, uint16_t value){
    uint8_t ins = 0b10000000;
    if(address <= 0x1F){
        ins |= address;
    }else{
        ins |= 0x1F;
    }
    
    if(value == 0){
        value = 0;
    }else if(value < 3500){
        value = 3500;
    }else if(value > 11500){
        value = 11500;
    }
    uint8_t byteH = value >> 7;
    uint8_t byteL = value;
    byteL &= (0 << 8);
    
    _serial.putc(ins);
    _serial.putc(byteH);
    _serial.putc(byteL);
    
    wait_ms(10);
}