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?
User | Revision | Line number | New 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 | } |