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@2:431a802d01e2, 2022-07-03 (annotated)

Committer:
koki_konishi
Date:
Sun Jul 03 05:11:26 2022 +0000
Revision:
2:431a802d01e2
Parent:
1:50d0eb774b0d 
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 2:431a802d01e2 9 void KRS::WriteAddress(uint8_t address){
koki_konishi 2:431a802d01e2 10 uint8_t ins = 0b11100000;
koki_konishi 2:431a802d01e2 11 if(address <= 0x1F){
koki_konishi 2:431a802d01e2 12 ins |= address;
koki_konishi 2:431a802d01e2 13 }else{
koki_konishi 2:431a802d01e2 14 ins |= 0x1F;
koki_konishi 2:431a802d01e2 15 }
koki_konishi 2:431a802d01e2 16
koki_konishi 2:431a802d01e2 17 _serial.putc(ins);
koki_konishi 2:431a802d01e2 18 _serial.putc(0x01);
koki_konishi 2:431a802d01e2 19 _serial.putc(0x01);
koki_konishi 2:431a802d01e2 20 _serial.putc(0x01);
koki_konishi 2:431a802d01e2 21
koki_konishi 2:431a802d01e2 22 wait_ms(10);
koki_konishi 2:431a802d01e2 23 }
koki_konishi 2:431a802d01e2 24
koki_konishi 1:50d0eb774b0d 25 void KRS::SetStretch(uint8_t address, uint8_t value){
koki_konishi 1:50d0eb774b0d 26 uint8_t ins = 0b11000000;
koki_konishi 1:50d0eb774b0d 27 if(address <= 0x1F){
koki_konishi 1:50d0eb774b0d 28 ins |= address;
koki_konishi 1:50d0eb774b0d 29 }else{
koki_konishi 1:50d0eb774b0d 30 ins |= 0x1F;
koki_konishi 1:50d0eb774b0d 31 }
koki_konishi 1:50d0eb774b0d 32
koki_konishi 1:50d0eb774b0d 33 if(value > 0x80){
koki_konishi 1:50d0eb774b0d 34 value = 0x80;
koki_konishi 1:50d0eb774b0d 35 }
koki_konishi 1:50d0eb774b0d 36
koki_konishi 1:50d0eb774b0d 37 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 38 _serial.putc(0x01);
koki_konishi 1:50d0eb774b0d 39 _serial.putc(value);
koki_konishi 1:50d0eb774b0d 40
koki_konishi 1:50d0eb774b0d 41 wait_ms(10);
koki_konishi 1:50d0eb774b0d 42 }
koki_konishi 1:50d0eb774b0d 43
koki_konishi 1:50d0eb774b0d 44 void KRS::SetSpeed(uint8_t address, uint8_t value){
koki_konishi 1:50d0eb774b0d 45 uint8_t ins = 0b11000000;
koki_konishi 1:50d0eb774b0d 46 if(address <= 0x1F){
koki_konishi 1:50d0eb774b0d 47 ins |= address;
koki_konishi 1:50d0eb774b0d 48 }else{
koki_konishi 1:50d0eb774b0d 49 ins |= 0x1F;
koki_konishi 1:50d0eb774b0d 50 }
koki_konishi 1:50d0eb774b0d 51
koki_konishi 1:50d0eb774b0d 52 if(value > 0x80){
koki_konishi 1:50d0eb774b0d 53 value = 0x80;
koki_konishi 1:50d0eb774b0d 54 }
koki_konishi 1:50d0eb774b0d 55
koki_konishi 1:50d0eb774b0d 56 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 57 _serial.putc(0x02);
koki_konishi 1:50d0eb774b0d 58 _serial.putc(value);
koki_konishi 1:50d0eb774b0d 59
koki_konishi 1:50d0eb774b0d 60 wait_ms(10);
koki_konishi 1:50d0eb774b0d 61 }
koki_konishi 1:50d0eb774b0d 62
koki_konishi 1:50d0eb774b0d 63 void KRS::SetCurrent(uint8_t address, ROTATE rotate, uint8_t value){
koki_konishi 1:50d0eb774b0d 64 uint8_t ins = 0b11000000;
koki_konishi 1:50d0eb774b0d 65 if(address <= 0x1F){
koki_konishi 1:50d0eb774b0d 66 ins |= address;
koki_konishi 1:50d0eb774b0d 67 }else{
koki_konishi 1:50d0eb774b0d 68 ins |= 0x1F;
koki_konishi 1:50d0eb774b0d 69 }
koki_konishi 1:50d0eb774b0d 70
koki_konishi 1:50d0eb774b0d 71 if(value > 0x40){
koki_konishi 1:50d0eb774b0d 72 value = 0x40;
koki_konishi 1:50d0eb774b0d 73 }
koki_konishi 1:50d0eb774b0d 74
koki_konishi 1:50d0eb774b0d 75 switch(rotate){
koki_konishi 1:50d0eb774b0d 76 case F:
koki_konishi 1:50d0eb774b0d 77 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 78 _serial.putc(0x03);
koki_konishi 1:50d0eb774b0d 79 _serial.putc(value);
koki_konishi 1:50d0eb774b0d 80 break;
koki_konishi 1:50d0eb774b0d 81 case R:
koki_konishi 1:50d0eb774b0d 82 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 83 _serial.putc(0x03);
koki_konishi 1:50d0eb774b0d 84 _serial.putc(value+64);
koki_konishi 1:50d0eb774b0d 85 break;
koki_konishi 1:50d0eb774b0d 86 case COM:
koki_konishi 1:50d0eb774b0d 87 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 88 _serial.putc(0x03);
koki_konishi 1:50d0eb774b0d 89 _serial.putc(value);
koki_konishi 1:50d0eb774b0d 90 wait_ms(10);
koki_konishi 1:50d0eb774b0d 91 _serial.putc(ins);
koki_konishi 1:50d0eb774b0d 92 _serial.putc(0x03);
koki_konishi 1:50d0eb774b0d 93 _serial.putc(value+64);
koki_konishi 1:50d0eb774b0d 94 break;
koki_konishi 1:50d0eb774b0d 95 }
koki_konishi 1:50d0eb774b0d 96
koki_konishi 1:50d0eb774b0d 97 wait_ms(10);
koki_konishi 1:50d0eb774b0d 98 }
koki_konishi 1:50d0eb774b0d 99
koki_konishi 0:7bf595a76895 100 void KRS::SetPosition(uint8_t address, uint16_t value){
koki_konishi 1:50d0eb774b0d 101 uint8_t ins = 0b10000000;
koki_konishi 0:7bf595a76895 102 if(address <= 0x1F){
koki_konishi 0:7bf595a76895 103 ins |= address;
koki_konishi 0:7bf595a76895 104 }else{
koki_konishi 0:7bf595a76895 105 ins |= 0x1F;
koki_konishi 0:7bf595a76895 106 }
koki_konishi 0:7bf595a76895 107
koki_konishi 0:7bf595a76895 108 if(value == 0){
koki_konishi 0:7bf595a76895 109 value = 0;
koki_konishi 0:7bf595a76895 110 }else if(value < 3500){
koki_konishi 0:7bf595a76895 111 value = 3500;
koki_konishi 0:7bf595a76895 112 }else if(value > 11500){
koki_konishi 0:7bf595a76895 113 value = 11500;
koki_konishi 0:7bf595a76895 114 }
koki_konishi 1:50d0eb774b0d 115 uint8_t byteH = value >> 7;
koki_konishi 1:50d0eb774b0d 116 uint8_t byteL = value;
koki_konishi 0:7bf595a76895 117 byteL &= (0 << 8);
koki_konishi 0:7bf595a76895 118
koki_konishi 0:7bf595a76895 119 _serial.putc(ins);
koki_konishi 0:7bf595a76895 120 _serial.putc(byteH);
koki_konishi 0:7bf595a76895 121 _serial.putc(byteL);
koki_konishi 1:50d0eb774b0d 122
koki_konishi 1:50d0eb774b0d 123 wait_ms(10);
koki_konishi 0:7bf595a76895 124 }