Fumiya KOIKE
/
RS405CB
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
RS405CB.cpp@5:e6208d124ead, 2019-12-12 (annotated)
- Committer:
- FumiyaKoike
- Date:
- Thu Dec 12 05:00:22 2019 +0000
- Revision:
- 5:e6208d124ead
- Parent:
- 4:8289dcb78a23
Fix the issure
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| FumiyaKoike | 0:22a84e434ec8 | 1 | #include "mbed.h" |
| FumiyaKoike | 0:22a84e434ec8 | 2 | #include "RS405CB.h" |
| FumiyaKoike | 0:22a84e434ec8 | 3 | |
| FumiyaKoike | 0:22a84e434ec8 | 4 | RS405CB::RS405CB(PinName tx,PinName rx,PinName permit):Serial(tx,rx),_permit(permit) |
| FumiyaKoike | 0:22a84e434ec8 | 5 | { |
| FumiyaKoike | 0:22a84e434ec8 | 6 | baud(115200); |
| FumiyaKoike | 0:22a84e434ec8 | 7 | format(8,Serial::None,1); |
| FumiyaKoike | 0:22a84e434ec8 | 8 | } |
| FumiyaKoike | 0:22a84e434ec8 | 9 | |
| FumiyaKoike | 0:22a84e434ec8 | 10 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 0:22a84e434ec8 | 11 | Arguments: |
| FumiyaKoike | 0:22a84e434ec8 | 12 | ID ... トルクを設定したいサーボのID |
| FumiyaKoike | 0:22a84e434ec8 | 13 | data ... トルクの状態の設定 |
| FumiyaKoike | 0:22a84e434ec8 | 14 | 0x00 OFF |
| FumiyaKoike | 0:22a84e434ec8 | 15 | 0x01 ON |
| FumiyaKoike | 0:22a84e434ec8 | 16 | 0x02 BREKE |
| FumiyaKoike | 0:22a84e434ec8 | 17 | Description: |
| FumiyaKoike | 0:22a84e434ec8 | 18 | トルク設定。入力されたIDのサーボモータのトルクを制御する |
| FumiyaKoike | 0:22a84e434ec8 | 19 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 0:22a84e434ec8 | 20 | void RS405CB::Torque(unsigned char ID, unsigned char data) |
| FumiyaKoike | 0:22a84e434ec8 | 21 | { |
| FumiyaKoike | 0:22a84e434ec8 | 22 | unsigned char TxData[9]= {0xFA, 0xAF, ID, 0x00, 0x24, 0x01, 0x01, data, 0}; |
| FumiyaKoike | 0:22a84e434ec8 | 23 | unsigned char CheckSum = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 24 | for(int i = 2; i <= 7; i++) CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 0:22a84e434ec8 | 25 | TxData[8] = CheckSum; |
| FumiyaKoike | 0:22a84e434ec8 | 26 | _permit = 1; |
| FumiyaKoike | 0:22a84e434ec8 | 27 | for (int i=0; i<=8; i++) putc((char)TxData[i]); |
| FumiyaKoike | 0:22a84e434ec8 | 28 | wait_us(750); |
| FumiyaKoike | 0:22a84e434ec8 | 29 | _permit = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 30 | } |
| FumiyaKoike | 0:22a84e434ec8 | 31 | |
| FumiyaKoike | 0:22a84e434ec8 | 32 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 0:22a84e434ec8 | 33 | Arguments: |
| FumiyaKoike | 2:7e4294a350fd | 34 | ID ... トルクをオンに設定したいサーボのID |
| FumiyaKoike | 2:7e4294a350fd | 35 | |
| FumiyaKoike | 2:7e4294a350fd | 36 | Description: |
| FumiyaKoike | 2:7e4294a350fd | 37 | トルクをオンにする |
| FumiyaKoike | 2:7e4294a350fd | 38 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 2:7e4294a350fd | 39 | void RS405CB::TorqueOn(unsigned char ID) |
| FumiyaKoike | 2:7e4294a350fd | 40 | { |
| FumiyaKoike | 2:7e4294a350fd | 41 | unsigned char TxData[9]= {0xFA, 0xAF, ID, 0x00, 0x24, 0x01, 0x01, 0x01, 0}; |
| FumiyaKoike | 2:7e4294a350fd | 42 | unsigned char CheckSum = 0; |
| FumiyaKoike | 2:7e4294a350fd | 43 | for(int i = 2; i <= 7; i++) CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 2:7e4294a350fd | 44 | TxData[8] = CheckSum; |
| FumiyaKoike | 2:7e4294a350fd | 45 | _permit = 1; |
| FumiyaKoike | 2:7e4294a350fd | 46 | for (int i=0; i<=8; i++) putc(TxData[i]); |
| FumiyaKoike | 2:7e4294a350fd | 47 | wait_us(750); |
| FumiyaKoike | 2:7e4294a350fd | 48 | _permit = 0; |
| FumiyaKoike | 2:7e4294a350fd | 49 | } |
| FumiyaKoike | 2:7e4294a350fd | 50 | |
| FumiyaKoike | 2:7e4294a350fd | 51 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 2:7e4294a350fd | 52 | Arguments: |
| FumiyaKoike | 2:7e4294a350fd | 53 | ID ... トルクをオンに設定したいサーボのIDの配列 |
| FumiyaKoike | 2:7e4294a350fd | 54 | |
| FumiyaKoike | 2:7e4294a350fd | 55 | Description: |
| FumiyaKoike | 2:7e4294a350fd | 56 | トルクをオンにする |
| FumiyaKoike | 2:7e4294a350fd | 57 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 2:7e4294a350fd | 58 | void RS405CB::TorqueOn(unsigned char* ID) |
| FumiyaKoike | 2:7e4294a350fd | 59 | { |
| FumiyaKoike | 2:7e4294a350fd | 60 | const unsigned char size = sizeof(ID); |
| FumiyaKoike | 2:7e4294a350fd | 61 | unsigned char TxData[8 + 2*size]; |
| FumiyaKoike | 2:7e4294a350fd | 62 | TxData[0] = 0xFA; //header |
| FumiyaKoike | 2:7e4294a350fd | 63 | TxData[1] = 0xAF; //header |
| FumiyaKoike | 2:7e4294a350fd | 64 | TxData[2] = 0x00; // ID -> 0x00 |
| FumiyaKoike | 2:7e4294a350fd | 65 | TxData[3] = 0x00; // Flags -> 0x00 |
| FumiyaKoike | 2:7e4294a350fd | 66 | TxData[4] = 0x24; // Addr -> Torque(0x24) |
| FumiyaKoike | 2:7e4294a350fd | 67 | TxData[5] = 0x02; // Length -> ID(1Byte) + Torque(1Byte) |
| FumiyaKoike | 2:7e4294a350fd | 68 | TxData[6] = (unsigned char)size; // Count -> Number of Servos |
| FumiyaKoike | 2:7e4294a350fd | 69 | |
| FumiyaKoike | 2:7e4294a350fd | 70 | for(int i=0; i<size; i++) |
| FumiyaKoike | 2:7e4294a350fd | 71 | { |
| FumiyaKoike | 2:7e4294a350fd | 72 | TxData[7 + 2*i] = ID[i]; |
| FumiyaKoike | 2:7e4294a350fd | 73 | TxData[8 + 2*i] = 0x01; |
| FumiyaKoike | 2:7e4294a350fd | 74 | } |
| FumiyaKoike | 2:7e4294a350fd | 75 | unsigned char CheckSum = 0; |
| FumiyaKoike | 2:7e4294a350fd | 76 | for(int i=2; i<=6+2*size; i++) |
| FumiyaKoike | 2:7e4294a350fd | 77 | CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 2:7e4294a350fd | 78 | TxData[7+2*size] = CheckSum; |
| FumiyaKoike | 2:7e4294a350fd | 79 | _permit = 1; |
| FumiyaKoike | 2:7e4294a350fd | 80 | for (int i=0; i<=7+2*size; i++) putc(TxData[i]); |
| FumiyaKoike | 2:7e4294a350fd | 81 | wait_us(750); |
| FumiyaKoike | 2:7e4294a350fd | 82 | _permit = 0; |
| FumiyaKoike | 2:7e4294a350fd | 83 | } |
| FumiyaKoike | 2:7e4294a350fd | 84 | |
| FumiyaKoike | 2:7e4294a350fd | 85 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 2:7e4294a350fd | 86 | Arguments: |
| FumiyaKoike | 0:22a84e434ec8 | 87 | ID ... 動かしたいサーボのID |
| FumiyaKoike | 0:22a84e434ec8 | 88 | data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900） |
| FumiyaKoike | 0:22a84e434ec8 | 89 | |
| FumiyaKoike | 0:22a84e434ec8 | 90 | Description: |
| FumiyaKoike | 0:22a84e434ec8 | 91 | |
| FumiyaKoike | 0:22a84e434ec8 | 92 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 0:22a84e434ec8 | 93 | void RS405CB::GoalPosition(unsigned char ID, int data) |
| FumiyaKoike | 0:22a84e434ec8 | 94 | { |
| FumiyaKoike | 0:22a84e434ec8 | 95 | unsigned char TxData[10]; |
| FumiyaKoike | 0:22a84e434ec8 | 96 | unsigned char CheckSum = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 97 | |
| FumiyaKoike | 0:22a84e434ec8 | 98 | TxData[0] = 0xFA; |
| FumiyaKoike | 0:22a84e434ec8 | 99 | TxData[1] = 0xAF; |
| FumiyaKoike | 0:22a84e434ec8 | 100 | TxData[2] = ID; |
| FumiyaKoike | 0:22a84e434ec8 | 101 | TxData[3] = 0x00; |
| FumiyaKoike | 0:22a84e434ec8 | 102 | TxData[4] = 0x1E; |
| FumiyaKoike | 0:22a84e434ec8 | 103 | TxData[5] = 0x02; |
| FumiyaKoike | 0:22a84e434ec8 | 104 | TxData[6] = 0x01; |
| FumiyaKoike | 0:22a84e434ec8 | 105 | TxData[7] = (unsigned char)0x00FF & data; |
| FumiyaKoike | 0:22a84e434ec8 | 106 | TxData[8] = (unsigned char)0x00FF & (data >> 8); |
| FumiyaKoike | 0:22a84e434ec8 | 107 | |
| FumiyaKoike | 0:22a84e434ec8 | 108 | for(int i=2; i<=8; i++) |
| FumiyaKoike | 0:22a84e434ec8 | 109 | CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 3:f7eb7b062ae0 | 110 | |
| FumiyaKoike | 0:22a84e434ec8 | 111 | TxData[9] = CheckSum; |
| FumiyaKoike | 0:22a84e434ec8 | 112 | |
| FumiyaKoike | 0:22a84e434ec8 | 113 | _permit = 1; |
| FumiyaKoike | 0:22a84e434ec8 | 114 | for(int i=0; i<=9; i++) { |
| FumiyaKoike | 0:22a84e434ec8 | 115 | putc(TxData[i]); |
| FumiyaKoike | 0:22a84e434ec8 | 116 | } |
| FumiyaKoike | 0:22a84e434ec8 | 117 | wait_us(810); |
| FumiyaKoike | 0:22a84e434ec8 | 118 | _permit = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 119 | } |
| FumiyaKoike | 0:22a84e434ec8 | 120 | |
| FumiyaKoike | 0:22a84e434ec8 | 121 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 0:22a84e434ec8 | 122 | Arguments: |
| FumiyaKoike | 2:7e4294a350fd | 123 | ID ... 動かしたいサーボのIDの配列 |
| FumiyaKoike | 2:7e4294a350fd | 124 | data ... 目標角の配列．degreeの10倍の値を指定する．（ex 90° -> data = 900） |
| FumiyaKoike | 2:7e4294a350fd | 125 | speed... 移動時間の配列．10ms単位で指定(ex. 5000ms -> 500) |
| FumiyaKoike | 2:7e4294a350fd | 126 | |
| FumiyaKoike | 2:7e4294a350fd | 127 | Description: |
| FumiyaKoike | 2:7e4294a350fd | 128 | GoalPositionのロングパケットバージョン |
| FumiyaKoike | 2:7e4294a350fd | 129 | |
| FumiyaKoike | 2:7e4294a350fd | 130 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 2:7e4294a350fd | 131 | void RS405CB::GoalPosition(unsigned char* ID, int* data) |
| FumiyaKoike | 2:7e4294a350fd | 132 | { |
| FumiyaKoike | 2:7e4294a350fd | 133 | const unsigned char size = sizeof(ID); |
| FumiyaKoike | 2:7e4294a350fd | 134 | unsigned char TxData[8 + 3 * size]; |
| FumiyaKoike | 2:7e4294a350fd | 135 | unsigned char CheckSum = 0; |
| FumiyaKoike | 2:7e4294a350fd | 136 | |
| FumiyaKoike | 2:7e4294a350fd | 137 | TxData[0] = 0xFA; //header |
| FumiyaKoike | 2:7e4294a350fd | 138 | TxData[1] = 0xAF; //header |
| FumiyaKoike | 2:7e4294a350fd | 139 | TxData[2] = 0x00; // ID -> 0x00 |
| FumiyaKoike | 2:7e4294a350fd | 140 | TxData[3] = 0x00; // Flags -> 0x00 |
| FumiyaKoike | 2:7e4294a350fd | 141 | TxData[4] = 0x1E; // Addr -> GoalPosition(0x1E) |
| FumiyaKoike | 2:7e4294a350fd | 142 | TxData[5] = 0x03; // Length -> ID(1Byte) + GoalPosition_L,H(2Byte) |
| FumiyaKoike | 2:7e4294a350fd | 143 | TxData[6] = (unsigned char)size; // Count -> Number of Servos |
| FumiyaKoike | 2:7e4294a350fd | 144 | |
| FumiyaKoike | 2:7e4294a350fd | 145 | for(int i=0; i<size; i++) |
| FumiyaKoike | 2:7e4294a350fd | 146 | { |
| FumiyaKoike | 2:7e4294a350fd | 147 | TxData[7 + i*3] = ID[i]; |
| FumiyaKoike | 2:7e4294a350fd | 148 | TxData[8 + i*3] = (unsigned char)0x00FF & data[i]; |
| FumiyaKoike | 2:7e4294a350fd | 149 | TxData[9 + i*3] = (unsigned char)0x00FF & (data[i] >> 8); |
| FumiyaKoike | 2:7e4294a350fd | 150 | } |
| FumiyaKoike | 2:7e4294a350fd | 151 | |
| FumiyaKoike | 2:7e4294a350fd | 152 | for(int i=2; i<=6+3*size; i++) |
| FumiyaKoike | 2:7e4294a350fd | 153 | CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 2:7e4294a350fd | 154 | |
| FumiyaKoike | 2:7e4294a350fd | 155 | TxData[7 + 3*size] = CheckSum; |
| FumiyaKoike | 2:7e4294a350fd | 156 | |
| FumiyaKoike | 2:7e4294a350fd | 157 | _permit = 1; |
| FumiyaKoike | 3:f7eb7b062ae0 | 158 | |
| FumiyaKoike | 3:f7eb7b062ae0 | 159 | for(int i=0; i<=7+3*size; i++) |
| FumiyaKoike | 2:7e4294a350fd | 160 | putc(TxData[i]); |
| FumiyaKoike | 3:f7eb7b062ae0 | 161 | |
| FumiyaKoike | 2:7e4294a350fd | 162 | wait_us(810); |
| FumiyaKoike | 2:7e4294a350fd | 163 | _permit = 0; |
| FumiyaKoike | 2:7e4294a350fd | 164 | } |
| FumiyaKoike | 2:7e4294a350fd | 165 | |
| FumiyaKoike | 2:7e4294a350fd | 166 | |
| FumiyaKoike | 2:7e4294a350fd | 167 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 2:7e4294a350fd | 168 | Arguments: |
| FumiyaKoike | 4:8289dcb78a23 | 169 | ID ... |
| FumiyaKoike | 1:d9b19e1c1a9f | 170 | speed... 移動時間の指定．10ms単位で指定(ex. 5000ms -> 500) |
| FumiyaKoike | 1:d9b19e1c1a9f | 171 | |
| FumiyaKoike | 1:d9b19e1c1a9f | 172 | Description: |
| FumiyaKoike | 4:8289dcb78a23 | 173 | サーボの移動時間を設定する |
| FumiyaKoike | 1:d9b19e1c1a9f | 174 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 4:8289dcb78a23 | 175 | void RS405CB::SetSpeed(unsigned char ID, int speed) |
| FumiyaKoike | 1:d9b19e1c1a9f | 176 | { |
| FumiyaKoike | 4:8289dcb78a23 | 177 | unsigned char TxData[10]; |
| FumiyaKoike | 1:d9b19e1c1a9f | 178 | unsigned char CheckSum = 0; |
| FumiyaKoike | 1:d9b19e1c1a9f | 179 | |
| FumiyaKoike | 1:d9b19e1c1a9f | 180 | TxData[0] = 0xFA; |
| FumiyaKoike | 1:d9b19e1c1a9f | 181 | TxData[1] = 0xAF; |
| FumiyaKoike | 1:d9b19e1c1a9f | 182 | TxData[2] = ID; |
| FumiyaKoike | 1:d9b19e1c1a9f | 183 | TxData[3] = 0x00; |
| FumiyaKoike | 4:8289dcb78a23 | 184 | TxData[4] = 0x1E + 2; |
| FumiyaKoike | 4:8289dcb78a23 | 185 | TxData[5] = 0x02; |
| FumiyaKoike | 1:d9b19e1c1a9f | 186 | TxData[6] = 0x01; |
| FumiyaKoike | 4:8289dcb78a23 | 187 | TxData[7] = (unsigned char)0x00FF & speed; |
| FumiyaKoike | 4:8289dcb78a23 | 188 | TxData[8] = (unsigned char)0x00FF & (speed >> 8); |
| FumiyaKoike | 1:d9b19e1c1a9f | 189 | |
| FumiyaKoike | 4:8289dcb78a23 | 190 | for(int i=2; i<=8; i++) |
| FumiyaKoike | 1:d9b19e1c1a9f | 191 | { |
| FumiyaKoike | 1:d9b19e1c1a9f | 192 | CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 1:d9b19e1c1a9f | 193 | } |
| FumiyaKoike | 4:8289dcb78a23 | 194 | TxData[9] = CheckSum; |
| FumiyaKoike | 1:d9b19e1c1a9f | 195 | |
| FumiyaKoike | 1:d9b19e1c1a9f | 196 | _permit = 1; |
| FumiyaKoike | 4:8289dcb78a23 | 197 | for(int i=0; i<=9; i++) { |
| FumiyaKoike | 1:d9b19e1c1a9f | 198 | putc(TxData[i]); |
| FumiyaKoike | 1:d9b19e1c1a9f | 199 | } |
| FumiyaKoike | 1:d9b19e1c1a9f | 200 | wait_us(810); |
| FumiyaKoike | 1:d9b19e1c1a9f | 201 | _permit = 0; |
| FumiyaKoike | 1:d9b19e1c1a9f | 202 | } |
| FumiyaKoike | 1:d9b19e1c1a9f | 203 | |
| FumiyaKoike | 3:f7eb7b062ae0 | 204 | |
| FumiyaKoike | 2:7e4294a350fd | 205 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 2:7e4294a350fd | 206 | Arguments: |
| FumiyaKoike | 4:8289dcb78a23 | 207 | ID ... |
| FumiyaKoike | 2:7e4294a350fd | 208 | speed... 移動時間の配列．10ms単位で指定(ex. 5000ms -> 500) |
| FumiyaKoike | 2:7e4294a350fd | 209 | |
| FumiyaKoike | 2:7e4294a350fd | 210 | Description: |
| FumiyaKoike | 4:8289dcb78a23 | 211 | |
| FumiyaKoike | 2:7e4294a350fd | 212 | |
| FumiyaKoike | 2:7e4294a350fd | 213 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 5:e6208d124ead | 214 | void RS405CB::SetSpeed(unsigned char* ID, int* speed) |
| FumiyaKoike | 2:7e4294a350fd | 215 | { |
| FumiyaKoike | 2:7e4294a350fd | 216 | const unsigned char size = sizeof(ID); |
| FumiyaKoike | 4:8289dcb78a23 | 217 | unsigned char TxData[8 + 3 * size]; |
| FumiyaKoike | 2:7e4294a350fd | 218 | unsigned char CheckSum = 0; |
| FumiyaKoike | 2:7e4294a350fd | 219 | |
| FumiyaKoike | 2:7e4294a350fd | 220 | TxData[0] = 0xFA; //header |
| FumiyaKoike | 2:7e4294a350fd | 221 | TxData[1] = 0xAF; //header |
| FumiyaKoike | 2:7e4294a350fd | 222 | TxData[2] = 0x00; // ID -> 0x00 |
| FumiyaKoike | 2:7e4294a350fd | 223 | TxData[3] = 0x00; // Flags -> 0x00 |
| FumiyaKoike | 5:e6208d124ead | 224 | TxData[4] = 0x1E + 2; //Addr |
| FumiyaKoike | 4:8289dcb78a23 | 225 | TxData[5] = 0x03; // Length -> ID(1Byte) + GoalTime_L,H(2Byte) |
| FumiyaKoike | 2:7e4294a350fd | 226 | TxData[6] = (unsigned char)size; // Count -> Number of Servos |
| FumiyaKoike | 2:7e4294a350fd | 227 | |
| FumiyaKoike | 2:7e4294a350fd | 228 | for(int i=0; i<size; i++) |
| FumiyaKoike | 2:7e4294a350fd | 229 | { |
| FumiyaKoike | 4:8289dcb78a23 | 230 | TxData[7 + i*3] = ID[i]; |
| FumiyaKoike | 4:8289dcb78a23 | 231 | TxData[8 + i*3] = (unsigned char)0x00FF & speed[i]; |
| FumiyaKoike | 4:8289dcb78a23 | 232 | TxData[9 + i*3] = (unsigned char)0x00FF & (speed[i] >> 8); |
| FumiyaKoike | 2:7e4294a350fd | 233 | } |
| FumiyaKoike | 2:7e4294a350fd | 234 | |
| FumiyaKoike | 4:8289dcb78a23 | 235 | for(int i=2; i<=6+3*size; i++) |
| FumiyaKoike | 2:7e4294a350fd | 236 | CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 4:8289dcb78a23 | 237 | |
| FumiyaKoike | 4:8289dcb78a23 | 238 | TxData[7 + 3*size] = CheckSum; |
| FumiyaKoike | 2:7e4294a350fd | 239 | |
| FumiyaKoike | 2:7e4294a350fd | 240 | _permit = 1; |
| FumiyaKoike | 4:8289dcb78a23 | 241 | |
| FumiyaKoike | 4:8289dcb78a23 | 242 | for(int i=0; i<=7+3*size; i++) |
| FumiyaKoike | 2:7e4294a350fd | 243 | putc(TxData[i]); |
| FumiyaKoike | 4:8289dcb78a23 | 244 | |
| FumiyaKoike | 2:7e4294a350fd | 245 | wait_us(810); |
| FumiyaKoike | 2:7e4294a350fd | 246 | _permit = 0; |
| FumiyaKoike | 3:f7eb7b062ae0 | 247 | |
| FumiyaKoike | 2:7e4294a350fd | 248 | } |
| FumiyaKoike | 2:7e4294a350fd | 249 | |
| FumiyaKoike | 1:d9b19e1c1a9f | 250 | |
| FumiyaKoike | 1:d9b19e1c1a9f | 251 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 1:d9b19e1c1a9f | 252 | Arguments: |
| FumiyaKoike | 0:22a84e434ec8 | 253 | oldID ... IDを変更したいサーボの現在のID |
| FumiyaKoike | 0:22a84e434ec8 | 254 | newID ... 新しいID．最大は255 |
| FumiyaKoike | 0:22a84e434ec8 | 255 | |
| FumiyaKoike | 0:22a84e434ec8 | 256 | Description: |
| FumiyaKoike | 0:22a84e434ec8 | 257 | |
| FumiyaKoike | 0:22a84e434ec8 | 258 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 0:22a84e434ec8 | 259 | void RS405CB::Change_ID(unsigned char oldID, unsigned char newID){ |
| FumiyaKoike | 0:22a84e434ec8 | 260 | unsigned char TxData[9]; |
| FumiyaKoike | 0:22a84e434ec8 | 261 | unsigned char CheckSum = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 262 | |
| FumiyaKoike | 0:22a84e434ec8 | 263 | TxData[0] = 0xFA; |
| FumiyaKoike | 0:22a84e434ec8 | 264 | TxData[1] = 0xAF; |
| FumiyaKoike | 0:22a84e434ec8 | 265 | TxData[2] = oldID; |
| FumiyaKoike | 0:22a84e434ec8 | 266 | TxData[3] = 0x00; |
| FumiyaKoike | 0:22a84e434ec8 | 267 | TxData[4] = 0x04; |
| FumiyaKoike | 0:22a84e434ec8 | 268 | TxData[5] = 0x01; |
| FumiyaKoike | 0:22a84e434ec8 | 269 | TxData[6] = 0x01; |
| FumiyaKoike | 0:22a84e434ec8 | 270 | TxData[7] = newID; |
| FumiyaKoike | 0:22a84e434ec8 | 271 | |
| FumiyaKoike | 0:22a84e434ec8 | 272 | for(int i=2; i<=7; i++) |
| FumiyaKoike | 0:22a84e434ec8 | 273 | { |
| FumiyaKoike | 0:22a84e434ec8 | 274 | CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 0:22a84e434ec8 | 275 | } |
| FumiyaKoike | 0:22a84e434ec8 | 276 | TxData[8] = CheckSum; |
| FumiyaKoike | 0:22a84e434ec8 | 277 | |
| FumiyaKoike | 0:22a84e434ec8 | 278 | _permit = 1; |
| FumiyaKoike | 0:22a84e434ec8 | 279 | for(int i=0; i<=8; i++) { |
| FumiyaKoike | 0:22a84e434ec8 | 280 | putc(TxData[i]); |
| FumiyaKoike | 0:22a84e434ec8 | 281 | } |
| FumiyaKoike | 0:22a84e434ec8 | 282 | wait_us(810); |
| FumiyaKoike | 0:22a84e434ec8 | 283 | _permit = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 284 | |
| FumiyaKoike | 0:22a84e434ec8 | 285 | _ROM_Update(newID); |
| FumiyaKoike | 0:22a84e434ec8 | 286 | } |
| FumiyaKoike | 0:22a84e434ec8 | 287 | |
| FumiyaKoike | 0:22a84e434ec8 | 288 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 0:22a84e434ec8 | 289 | Arguments: |
| FumiyaKoike | 0:22a84e434ec8 | 290 | ID ... 角度を読みたいサーボのID |
| FumiyaKoike | 0:22a84e434ec8 | 291 | |
| FumiyaKoike | 0:22a84e434ec8 | 292 | Return: |
| FumiyaKoike | 0:22a84e434ec8 | 293 | get_deg(int) ... サーボの角度 |
| FumiyaKoike | 0:22a84e434ec8 | 294 | |
| FumiyaKoike | 0:22a84e434ec8 | 295 | Description: |
| FumiyaKoike | 0:22a84e434ec8 | 296 | |
| FumiyaKoike | 0:22a84e434ec8 | 297 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 0:22a84e434ec8 | 298 | int RS405CB::Read_Deg(unsigned char ID) |
| FumiyaKoike | 0:22a84e434ec8 | 299 | { |
| FumiyaKoike | 0:22a84e434ec8 | 300 | unsigned char CheckSum=0; |
| FumiyaKoike | 0:22a84e434ec8 | 301 | int get_deg = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 302 | unsigned char RxData[8]; |
| FumiyaKoike | 0:22a84e434ec8 | 303 | unsigned char TxData[8]; |
| FumiyaKoike | 0:22a84e434ec8 | 304 | |
| FumiyaKoike | 0:22a84e434ec8 | 305 | TxData[0] = 0xFA; |
| FumiyaKoike | 0:22a84e434ec8 | 306 | TxData[1] = 0xAF; |
| FumiyaKoike | 0:22a84e434ec8 | 307 | TxData[2] = ID; |
| FumiyaKoike | 0:22a84e434ec8 | 308 | TxData[3] = 0x0F; |
| FumiyaKoike | 0:22a84e434ec8 | 309 | TxData[4] = 0x2A; |
| FumiyaKoike | 0:22a84e434ec8 | 310 | TxData[5] = 0x02; |
| FumiyaKoike | 0:22a84e434ec8 | 311 | TxData[6] = 0x00; |
| FumiyaKoike | 0:22a84e434ec8 | 312 | |
| FumiyaKoike | 0:22a84e434ec8 | 313 | for(int j=2; j<=6; j++) { |
| FumiyaKoike | 0:22a84e434ec8 | 314 | CheckSum = CheckSum ^ TxData[j]; |
| FumiyaKoike | 0:22a84e434ec8 | 315 | } |
| FumiyaKoike | 0:22a84e434ec8 | 316 | TxData[7]=CheckSum; |
| FumiyaKoike | 0:22a84e434ec8 | 317 | |
| FumiyaKoike | 0:22a84e434ec8 | 318 | _permit = 1; |
| FumiyaKoike | 0:22a84e434ec8 | 319 | for (int j=0; j<=7; j++) |
| FumiyaKoike | 0:22a84e434ec8 | 320 | putc(TxData[j]); |
| FumiyaKoike | 0:22a84e434ec8 | 321 | wait_us(670); |
| FumiyaKoike | 0:22a84e434ec8 | 322 | _permit = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 323 | |
| FumiyaKoike | 0:22a84e434ec8 | 324 | CheckSum = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 325 | while(getc() != 0xFD); |
| FumiyaKoike | 0:22a84e434ec8 | 326 | while(getc() != 0xDF); |
| FumiyaKoike | 0:22a84e434ec8 | 327 | |
| FumiyaKoike | 0:22a84e434ec8 | 328 | for(int j=0; j<=6; j++){ |
| FumiyaKoike | 0:22a84e434ec8 | 329 | RxData[j] = getc(); |
| FumiyaKoike | 0:22a84e434ec8 | 330 | CheckSum = CheckSum ^ RxData[j]; |
| FumiyaKoike | 0:22a84e434ec8 | 331 | } |
| FumiyaKoike | 0:22a84e434ec8 | 332 | |
| FumiyaKoike | 0:22a84e434ec8 | 333 | if(CheckSum == getc()){ |
| FumiyaKoike | 0:22a84e434ec8 | 334 | get_deg = (int)RxData[6]; |
| FumiyaKoike | 0:22a84e434ec8 | 335 | get_deg = get_deg << 8; |
| FumiyaKoike | 0:22a84e434ec8 | 336 | get_deg |= (int)RxData[5]; |
| FumiyaKoike | 0:22a84e434ec8 | 337 | } |
| FumiyaKoike | 0:22a84e434ec8 | 338 | |
| FumiyaKoike | 0:22a84e434ec8 | 339 | return _s16(get_deg); |
| FumiyaKoike | 0:22a84e434ec8 | 340 | } |
| FumiyaKoike | 0:22a84e434ec8 | 341 | |
| FumiyaKoike | 0:22a84e434ec8 | 342 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 0:22a84e434ec8 | 343 | Arguments: |
| FumiyaKoike | 0:22a84e434ec8 | 344 | ID ... 電流を読みたいサーボのID |
| FumiyaKoike | 0:22a84e434ec8 | 345 | |
| FumiyaKoike | 0:22a84e434ec8 | 346 | Return: |
| FumiyaKoike | 0:22a84e434ec8 | 347 | get_mA(int) ... サーボに流れている電流量 |
| FumiyaKoike | 0:22a84e434ec8 | 348 | |
| FumiyaKoike | 0:22a84e434ec8 | 349 | Description: |
| FumiyaKoike | 0:22a84e434ec8 | 350 | メーカー曰く，参考程度の値なので注意 |
| FumiyaKoike | 0:22a84e434ec8 | 351 | |
| FumiyaKoike | 0:22a84e434ec8 | 352 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 0:22a84e434ec8 | 353 | int RS405CB::Read_mA(unsigned char ID) |
| FumiyaKoike | 0:22a84e434ec8 | 354 | { |
| FumiyaKoike | 0:22a84e434ec8 | 355 | unsigned char CheckSum=0; |
| FumiyaKoike | 0:22a84e434ec8 | 356 | int get_mA = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 357 | unsigned char RxData[8]; |
| FumiyaKoike | 0:22a84e434ec8 | 358 | unsigned char TxData[8]; |
| FumiyaKoike | 0:22a84e434ec8 | 359 | TxData[0] = 0xFA; |
| FumiyaKoike | 0:22a84e434ec8 | 360 | TxData[1] = 0xAF; |
| FumiyaKoike | 0:22a84e434ec8 | 361 | TxData[2] = ID; |
| FumiyaKoike | 0:22a84e434ec8 | 362 | TxData[3] = 0x0F; |
| FumiyaKoike | 0:22a84e434ec8 | 363 | TxData[4] = 0x30; |
| FumiyaKoike | 0:22a84e434ec8 | 364 | TxData[5] = 0x02; |
| FumiyaKoike | 0:22a84e434ec8 | 365 | TxData[6] = 0x00; |
| FumiyaKoike | 0:22a84e434ec8 | 366 | |
| FumiyaKoike | 0:22a84e434ec8 | 367 | for(int j=2; j<=6; j++) |
| FumiyaKoike | 0:22a84e434ec8 | 368 | { |
| FumiyaKoike | 0:22a84e434ec8 | 369 | CheckSum = CheckSum ^ TxData[j]; |
| FumiyaKoike | 0:22a84e434ec8 | 370 | } |
| FumiyaKoike | 0:22a84e434ec8 | 371 | TxData[7]=CheckSum; |
| FumiyaKoike | 0:22a84e434ec8 | 372 | |
| FumiyaKoike | 0:22a84e434ec8 | 373 | _permit = 1; |
| FumiyaKoike | 0:22a84e434ec8 | 374 | for (int j=0; j<=7; j++) |
| FumiyaKoike | 0:22a84e434ec8 | 375 | putc(TxData[j]); |
| FumiyaKoike | 0:22a84e434ec8 | 376 | wait_us(670); |
| FumiyaKoike | 0:22a84e434ec8 | 377 | _permit = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 378 | |
| FumiyaKoike | 0:22a84e434ec8 | 379 | CheckSum = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 380 | while(getc() != 0xFD); |
| FumiyaKoike | 0:22a84e434ec8 | 381 | while(getc() != 0xDF); |
| FumiyaKoike | 0:22a84e434ec8 | 382 | |
| FumiyaKoike | 0:22a84e434ec8 | 383 | for(int j=0; j<=6; j++){ |
| FumiyaKoike | 0:22a84e434ec8 | 384 | RxData[j] = getc(); |
| FumiyaKoike | 0:22a84e434ec8 | 385 | CheckSum = CheckSum ^ RxData[j]; |
| FumiyaKoike | 0:22a84e434ec8 | 386 | } |
| FumiyaKoike | 0:22a84e434ec8 | 387 | |
| FumiyaKoike | 0:22a84e434ec8 | 388 | if(CheckSum == getc()){ |
| FumiyaKoike | 0:22a84e434ec8 | 389 | get_mA = (int)RxData[6]; |
| FumiyaKoike | 0:22a84e434ec8 | 390 | get_mA = get_mA << 8; |
| FumiyaKoike | 0:22a84e434ec8 | 391 | get_mA |= (int)RxData[5]; |
| FumiyaKoike | 0:22a84e434ec8 | 392 | } |
| FumiyaKoike | 0:22a84e434ec8 | 393 | |
| FumiyaKoike | 0:22a84e434ec8 | 394 | return _s16(get_mA); |
| FumiyaKoike | 0:22a84e434ec8 | 395 | } |
| FumiyaKoike | 0:22a84e434ec8 | 396 | |
| FumiyaKoike | 0:22a84e434ec8 | 397 | |
| FumiyaKoike | 0:22a84e434ec8 | 398 | /* ----------------------------------------------------------------------------- |
| FumiyaKoike | 0:22a84e434ec8 | 399 | Arguments: |
| FumiyaKoike | 0:22a84e434ec8 | 400 | ID ... ROMをアップデートしたいサーボのID |
| FumiyaKoike | 0:22a84e434ec8 | 401 | |
| FumiyaKoike | 0:22a84e434ec8 | 402 | Description: |
| FumiyaKoike | 0:22a84e434ec8 | 403 | ROM領域の値を変更した時に実行する． |
| FumiyaKoike | 0:22a84e434ec8 | 404 | |
| FumiyaKoike | 0:22a84e434ec8 | 405 | ----------------------------------------------------------------------------- */ |
| FumiyaKoike | 0:22a84e434ec8 | 406 | void RS405CB::_ROM_Update(unsigned char ID){ |
| FumiyaKoike | 0:22a84e434ec8 | 407 | unsigned char TxData[8]; |
| FumiyaKoike | 0:22a84e434ec8 | 408 | unsigned char CheckSum = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 409 | |
| FumiyaKoike | 0:22a84e434ec8 | 410 | TxData[0] = 0xFA; |
| FumiyaKoike | 0:22a84e434ec8 | 411 | TxData[1] = 0xAF; |
| FumiyaKoike | 0:22a84e434ec8 | 412 | TxData[2] = ID; |
| FumiyaKoike | 0:22a84e434ec8 | 413 | TxData[3] = 0x40; |
| FumiyaKoike | 0:22a84e434ec8 | 414 | TxData[4] = 0xFF; |
| FumiyaKoike | 0:22a84e434ec8 | 415 | TxData[5] = 0x00; |
| FumiyaKoike | 0:22a84e434ec8 | 416 | TxData[6] = 0x00; |
| FumiyaKoike | 0:22a84e434ec8 | 417 | |
| FumiyaKoike | 0:22a84e434ec8 | 418 | for(int i=2; i<=6; i++) |
| FumiyaKoike | 0:22a84e434ec8 | 419 | { |
| FumiyaKoike | 0:22a84e434ec8 | 420 | CheckSum = CheckSum ^ TxData[i]; |
| FumiyaKoike | 0:22a84e434ec8 | 421 | } |
| FumiyaKoike | 0:22a84e434ec8 | 422 | TxData[7] = CheckSum; |
| FumiyaKoike | 0:22a84e434ec8 | 423 | |
| FumiyaKoike | 0:22a84e434ec8 | 424 | _permit = 1; |
| FumiyaKoike | 0:22a84e434ec8 | 425 | for(int i=0; i<=7; i++) { |
| FumiyaKoike | 0:22a84e434ec8 | 426 | putc(TxData[i]); |
| FumiyaKoike | 0:22a84e434ec8 | 427 | } |
| FumiyaKoike | 0:22a84e434ec8 | 428 | wait_us(810); |
| FumiyaKoike | 0:22a84e434ec8 | 429 | _permit = 0; |
| FumiyaKoike | 0:22a84e434ec8 | 430 | wait(1.5); |
| FumiyaKoike | 0:22a84e434ec8 | 431 | } |
| FumiyaKoike | 0:22a84e434ec8 | 432 | |
| FumiyaKoike | 0:22a84e434ec8 | 433 | int RS405CB::_s16(int value) |
| FumiyaKoike | 0:22a84e434ec8 | 434 | { |
| FumiyaKoike | 0:22a84e434ec8 | 435 | return -(value & 0b1000000000000000)|(value & 0b0111111111111111); |
| FumiyaKoike | 0:22a84e434ec8 | 436 | } |