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?
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 | 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 | } |