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:
- 2022-07-03
- 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); }