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@1:50d0eb774b0d, 2022-07-03 (annotated)

Committer:
koki_konishi
Date:
Sun Jul 03 04:35:46 2022 +0000
Revision:
1:50d0eb774b0d
Parent:
0:7bf595a76895
Child:
2:431a802d01e2
Kondo-servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
koki_konishi 0:7bf595a76895 1 #include "KRS.h"
koki_konishi 0:7bf595a76895 2 #include "mbed.h"
koki_konishi 0:7bf595a76895 3
koki_konishi 0:7bf595a76895 4 KRS::KRS(PinName tx,PinName rx) : _serial(tx,rx){
koki_konishi 0:7bf595a76895 5 _serial.baud(115200);
koki_konishi 0:7bf595a76895 6 _serial.format(8,Serial::Even,1);
koki_konishi 0:7bf595a76895 7 }
koki_konishi 0:7bf595a76895 8
koki_konishi 1:50d0eb774b0d 9 void KRS::SetStretch(uint8_t address, uint8_t value){
koki_konishi 1:50d0eb774b0d 10 uint8_t ins = 0b11000000;
koki_konishi 1:50d0eb774b0d 11 if(address <= 0x1F){
koki_konishi 1:50d0eb774b0d 12 ins |= address;
koki_konishi 1:50d0eb774b0d 13 }else{
koki_konishi 1:50d0eb774b0d 14 ins |= 0x1F;
koki_konishi 1:50d0eb774b0d 15 }
koki_konishi 1:50d0eb774b0d 16
koki_konishi 1:50d0eb774b0d 17 if(value > 0x80){
koki_konishi 1:50d0eb774b0d 18 value = 0x80;
koki_konishi 1:50d0eb774b0d 19 }
koki_konishi 1:50d0eb774b0d 20
koki_konishi 1:50d0eb774b0d 21 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 22 _serial.putc(0x01);
koki_konishi 1:50d0eb774b0d 23 _serial.putc(value);
koki_konishi 1:50d0eb774b0d 24
koki_konishi 1:50d0eb774b0d 25 wait_ms(10);
koki_konishi 1:50d0eb774b0d 26 }
koki_konishi 1:50d0eb774b0d 27
koki_konishi 1:50d0eb774b0d 28 void KRS::SetSpeed(uint8_t address, uint8_t value){
koki_konishi 1:50d0eb774b0d 29 uint8_t ins = 0b11000000;
koki_konishi 1:50d0eb774b0d 30 if(address <= 0x1F){
koki_konishi 1:50d0eb774b0d 31 ins |= address;
koki_konishi 1:50d0eb774b0d 32 }else{
koki_konishi 1:50d0eb774b0d 33 ins |= 0x1F;
koki_konishi 1:50d0eb774b0d 34 }
koki_konishi 1:50d0eb774b0d 35
koki_konishi 1:50d0eb774b0d 36 if(value > 0x80){
koki_konishi 1:50d0eb774b0d 37 value = 0x80;
koki_konishi 1:50d0eb774b0d 38 }
koki_konishi 1:50d0eb774b0d 39
koki_konishi 1:50d0eb774b0d 40 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 41 _serial.putc(0x02);
koki_konishi 1:50d0eb774b0d 42 _serial.putc(value);
koki_konishi 1:50d0eb774b0d 43
koki_konishi 1:50d0eb774b0d 44 wait_ms(10);
koki_konishi 1:50d0eb774b0d 45 }
koki_konishi 1:50d0eb774b0d 46
koki_konishi 1:50d0eb774b0d 47 void KRS::SetCurrent(uint8_t address, ROTATE rotate, uint8_t value){
koki_konishi 1:50d0eb774b0d 48 uint8_t ins = 0b11000000;
koki_konishi 1:50d0eb774b0d 49 if(address <= 0x1F){
koki_konishi 1:50d0eb774b0d 50 ins |= address;
koki_konishi 1:50d0eb774b0d 51 }else{
koki_konishi 1:50d0eb774b0d 52 ins |= 0x1F;
koki_konishi 1:50d0eb774b0d 53 }
koki_konishi 1:50d0eb774b0d 54
koki_konishi 1:50d0eb774b0d 55 if(value > 0x40){
koki_konishi 1:50d0eb774b0d 56 value = 0x40;
koki_konishi 1:50d0eb774b0d 57 }
koki_konishi 1:50d0eb774b0d 58
koki_konishi 1:50d0eb774b0d 59 switch(rotate){
koki_konishi 1:50d0eb774b0d 60 case F:
koki_konishi 1:50d0eb774b0d 61 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 62 _serial.putc(0x03);
koki_konishi 1:50d0eb774b0d 63 _serial.putc(value);
koki_konishi 1:50d0eb774b0d 64 break;
koki_konishi 1:50d0eb774b0d 65 case R:
koki_konishi 1:50d0eb774b0d 66 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 67 _serial.putc(0x03);
koki_konishi 1:50d0eb774b0d 68 _serial.putc(value+64);
koki_konishi 1:50d0eb774b0d 69 break;
koki_konishi 1:50d0eb774b0d 70 case COM:
koki_konishi 1:50d0eb774b0d 71 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 72 _serial.putc(0x03);
koki_konishi 1:50d0eb774b0d 73 _serial.putc(value);
koki_konishi 1:50d0eb774b0d 74 wait_ms(10);
koki_konishi 1:50d0eb774b0d 75 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 76 _serial.putc(0x03);
koki_konishi 1:50d0eb774b0d 77 _serial.putc(value+64);
koki_konishi 1:50d0eb774b0d 78 break;
koki_konishi 1:50d0eb774b0d 79 }
koki_konishi 1:50d0eb774b0d 80
koki_konishi 1:50d0eb774b0d 81 wait_ms(10);
koki_konishi 1:50d0eb774b0d 82 }
koki_konishi 1:50d0eb774b0d 83
koki_konishi 0:7bf595a76895 84 void KRS::SetPosition(uint8_t address, uint16_t value){
koki_konishi 1:50d0eb774b0d 85 uint8_t ins = 0b10000000;
koki_konishi 0:7bf595a76895 86 if(address <= 0x1F){
koki_konishi 0:7bf595a76895 87 ins |= address;
koki_konishi 0:7bf595a76895 88 }else{
koki_konishi 0:7bf595a76895 89 ins |= 0x1F;
koki_konishi 0:7bf595a76895 90 }
koki_konishi 0:7bf595a76895 91
koki_konishi 0:7bf595a76895 92 if(value == 0){
koki_konishi 0:7bf595a76895 93 value = 0;
koki_konishi 0:7bf595a76895 94 }else if(value < 3500){
koki_konishi 0:7bf595a76895 95 value = 3500;
koki_konishi 0:7bf595a76895 96 }else if(value > 11500){
koki_konishi 0:7bf595a76895 97 value = 11500;
koki_konishi 0:7bf595a76895 98 }
koki_konishi 1:50d0eb774b0d 99 uint8_t byteH = value >> 7;
koki_konishi 1:50d0eb774b0d 100 uint8_t byteL = value;
koki_konishi 0:7bf595a76895 101 byteL &= (0 << 8);
koki_konishi 0:7bf595a76895 102
koki_konishi 0:7bf595a76895 103 _serial.putc(ins);
koki_konishi 0:7bf595a76895 104 _serial.putc(byteH);
koki_konishi 0:7bf595a76895 105 _serial.putc(byteL);
koki_konishi 1:50d0eb774b0d 106
koki_konishi 1:50d0eb774b0d 107 wait_ms(10);
koki_konishi 0:7bf595a76895 108 }