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@3:f7eb7b062ae0, 2019-12-11 (annotated)

Committer:
FumiyaKoike
Date:
Wed Dec 11 06:23:57 2019 +0000
Revision:
3:f7eb7b062ae0
Parent:
2:7e4294a350fd
Child:
4:8289dcb78a23
Minor Update

Who changed what in which revision?

UserRevisionLine numberNew 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 1:d9b19e1c1a9f 169 ID ... 動かしたいサーボのID
FumiyaKoike 1:d9b19e1c1a9f 170 data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）
FumiyaKoike 1:d9b19e1c1a9f 171 speed... 移動時間の指定．10ms単位で指定(ex. 5000ms -> 500)
FumiyaKoike 1:d9b19e1c1a9f 172
FumiyaKoike 1:d9b19e1c1a9f 173 Description:
FumiyaKoike 1:d9b19e1c1a9f 174
FumiyaKoike 1:d9b19e1c1a9f 175 ----------------------------------------------------------------------------- */
FumiyaKoike 3:f7eb7b062ae0 176 void RS405CB::GoalTime(unsigned char ID, int data, int speed)
FumiyaKoike 1:d9b19e1c1a9f 177 {
FumiyaKoike 1:d9b19e1c1a9f 178 unsigned char TxData[12];
FumiyaKoike 1:d9b19e1c1a9f 179 unsigned char CheckSum = 0;
FumiyaKoike 1:d9b19e1c1a9f 180
FumiyaKoike 1:d9b19e1c1a9f 181 TxData[0] = 0xFA;
FumiyaKoike 1:d9b19e1c1a9f 182 TxData[1] = 0xAF;
FumiyaKoike 1:d9b19e1c1a9f 183 TxData[2] = ID;
FumiyaKoike 1:d9b19e1c1a9f 184 TxData[3] = 0x00;
FumiyaKoike 1:d9b19e1c1a9f 185 TxData[4] = 0x1E;
FumiyaKoike 1:d9b19e1c1a9f 186 TxData[5] = 0x04;
FumiyaKoike 1:d9b19e1c1a9f 187 TxData[6] = 0x01;
FumiyaKoike 3:f7eb7b062ae0 188 TxData[7] = (unsigned char)0x00FF & data;
FumiyaKoike 3:f7eb7b062ae0 189 TxData[8] = (unsigned char)0x00FF & (data >> 8);
FumiyaKoike 3:f7eb7b062ae0 190 TxData[9] = (unsigned char)0x00FF & speed;
FumiyaKoike 1:d9b19e1c1a9f 191 TxData[10] = (unsigned char)0x00FF & (speed >> 8);
FumiyaKoike 1:d9b19e1c1a9f 192
FumiyaKoike 1:d9b19e1c1a9f 193 for(int i=2; i<=10; i++)
FumiyaKoike 1:d9b19e1c1a9f 194 {
FumiyaKoike 1:d9b19e1c1a9f 195 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 1:d9b19e1c1a9f 196 }
FumiyaKoike 1:d9b19e1c1a9f 197 TxData[11] = CheckSum;
FumiyaKoike 1:d9b19e1c1a9f 198
FumiyaKoike 1:d9b19e1c1a9f 199 _permit = 1;
FumiyaKoike 1:d9b19e1c1a9f 200 for(int i=0; i<=11; i++) {
FumiyaKoike 1:d9b19e1c1a9f 201 putc(TxData[i]);
FumiyaKoike 1:d9b19e1c1a9f 202 }
FumiyaKoike 1:d9b19e1c1a9f 203 wait_us(810);
FumiyaKoike 1:d9b19e1c1a9f 204 _permit = 0;
FumiyaKoike 1:d9b19e1c1a9f 205 }
FumiyaKoike 1:d9b19e1c1a9f 206
FumiyaKoike 3:f7eb7b062ae0 207
FumiyaKoike 2:7e4294a350fd 208 /* -----------------------------------------------------------------------------
FumiyaKoike 2:7e4294a350fd 209 Arguments:
FumiyaKoike 2:7e4294a350fd 210 ID ... 動かしたいサーボのIDの配列
FumiyaKoike 2:7e4294a350fd 211 data ... 目標角の配列．degreeの10倍の値を指定する．（ex 90° -> data = 900）
FumiyaKoike 2:7e4294a350fd 212 speed... 移動時間の配列．10ms単位で指定(ex. 5000ms -> 500)
FumiyaKoike 2:7e4294a350fd 213
FumiyaKoike 2:7e4294a350fd 214 Description:
FumiyaKoike 2:7e4294a350fd 215 GoalPositionのロングパケットバージョン
FumiyaKoike 2:7e4294a350fd 216
FumiyaKoike 2:7e4294a350fd 217 ----------------------------------------------------------------------------- */
FumiyaKoike 3:f7eb7b062ae0 218 void RS405CB::GoalTime(unsigned char* ID, int* data, int* speed)
FumiyaKoike 2:7e4294a350fd 219 {
FumiyaKoike 2:7e4294a350fd 220 const unsigned char size = sizeof(ID);
FumiyaKoike 2:7e4294a350fd 221 unsigned char TxData[8 + 5 * size];
FumiyaKoike 2:7e4294a350fd 222 unsigned char CheckSum = 0;
FumiyaKoike 3:f7eb7b062ae0 223 int max_speed;
FumiyaKoike 3:f7eb7b062ae0 224 for(int i=0; i<sizeof(speed); i++)
FumiyaKoike 3:f7eb7b062ae0 225 max_speed = max(speed[i], max_speed);
FumiyaKoike 2:7e4294a350fd 226
FumiyaKoike 2:7e4294a350fd 227 TxData[0] = 0xFA; //header
FumiyaKoike 2:7e4294a350fd 228 TxData[1] = 0xAF; //header
FumiyaKoike 2:7e4294a350fd 229 TxData[2] = 0x00; // ID -> 0x00
FumiyaKoike 2:7e4294a350fd 230 TxData[3] = 0x00; // Flags -> 0x00
FumiyaKoike 2:7e4294a350fd 231 TxData[4] = 0x1E; // Addr -> GoalPosition(0x1E)
FumiyaKoike 2:7e4294a350fd 232 TxData[5] = 0x05; // Length -> ID(1Byte) + GoalPosition_L,H(2Byte) + GoalSpeed_L,H(2Byte)
FumiyaKoike 2:7e4294a350fd 233 TxData[6] = (unsigned char)size; // Count -> Number of Servos
FumiyaKoike 2:7e4294a350fd 234
FumiyaKoike 2:7e4294a350fd 235 for(int i=0; i<size; i++)
FumiyaKoike 2:7e4294a350fd 236 {
FumiyaKoike 2:7e4294a350fd 237 TxData[7 + i*5] = ID[i];
FumiyaKoike 2:7e4294a350fd 238 TxData[8 + i*5] = (unsigned char)0x00FF & data[i];
FumiyaKoike 2:7e4294a350fd 239 TxData[9 + i*5] = (unsigned char)0x00FF & (data[i] >> 8);
FumiyaKoike 2:7e4294a350fd 240 TxData[10 + i*5] = (unsigned char)0x00FF & speed[i];
FumiyaKoike 2:7e4294a350fd 241 TxData[11 + i*5] = (unsigned char)0x00FF & (speed[i] >> 8);
FumiyaKoike 2:7e4294a350fd 242 }
FumiyaKoike 2:7e4294a350fd 243
FumiyaKoike 2:7e4294a350fd 244 for(int i=2; i<=6+5*size; i++)
FumiyaKoike 2:7e4294a350fd 245 {
FumiyaKoike 2:7e4294a350fd 246 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 2:7e4294a350fd 247 }
FumiyaKoike 2:7e4294a350fd 248 TxData[7 + 5*size] = CheckSum;
FumiyaKoike 2:7e4294a350fd 249
FumiyaKoike 2:7e4294a350fd 250 _permit = 1;
FumiyaKoike 2:7e4294a350fd 251 for(int i=0; i<=7+5*size; i++) {
FumiyaKoike 2:7e4294a350fd 252 putc(TxData[i]);
FumiyaKoike 2:7e4294a350fd 253 }
FumiyaKoike 2:7e4294a350fd 254 wait_us(810);
FumiyaKoike 2:7e4294a350fd 255 _permit = 0;
FumiyaKoike 3:f7eb7b062ae0 256
FumiyaKoike 3:f7eb7b062ae0 257 wait_ms(max_speed * 10);
FumiyaKoike 3:f7eb7b062ae0 258
FumiyaKoike 2:7e4294a350fd 259 }
FumiyaKoike 2:7e4294a350fd 260
FumiyaKoike 1:d9b19e1c1a9f 261
FumiyaKoike 1:d9b19e1c1a9f 262 /* -----------------------------------------------------------------------------
FumiyaKoike 1:d9b19e1c1a9f 263 Arguments:
FumiyaKoike 0:22a84e434ec8 264 oldID ... IDを変更したいサーボの現在のID
FumiyaKoike 0:22a84e434ec8 265 newID ... 新しいID．最大は255
FumiyaKoike 0:22a84e434ec8 266
FumiyaKoike 0:22a84e434ec8 267 Description:
FumiyaKoike 0:22a84e434ec8 268
FumiyaKoike 0:22a84e434ec8 269 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 270 void RS405CB::Change_ID(unsigned char oldID, unsigned char newID){
FumiyaKoike 0:22a84e434ec8 271 unsigned char TxData[9];
FumiyaKoike 0:22a84e434ec8 272 unsigned char CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 273
FumiyaKoike 0:22a84e434ec8 274 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 275 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 276 TxData[2] = oldID;
FumiyaKoike 0:22a84e434ec8 277 TxData[3] = 0x00;
FumiyaKoike 0:22a84e434ec8 278 TxData[4] = 0x04;
FumiyaKoike 0:22a84e434ec8 279 TxData[5] = 0x01;
FumiyaKoike 0:22a84e434ec8 280 TxData[6] = 0x01;
FumiyaKoike 0:22a84e434ec8 281 TxData[7] = newID;
FumiyaKoike 0:22a84e434ec8 282
FumiyaKoike 0:22a84e434ec8 283 for(int i=2; i<=7; i++)
FumiyaKoike 0:22a84e434ec8 284 {
FumiyaKoike 0:22a84e434ec8 285 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 0:22a84e434ec8 286 }
FumiyaKoike 0:22a84e434ec8 287 TxData[8] = CheckSum;
FumiyaKoike 0:22a84e434ec8 288
FumiyaKoike 0:22a84e434ec8 289 _permit = 1;
FumiyaKoike 0:22a84e434ec8 290 for(int i=0; i<=8; i++) {
FumiyaKoike 0:22a84e434ec8 291 putc(TxData[i]);
FumiyaKoike 0:22a84e434ec8 292 }
FumiyaKoike 0:22a84e434ec8 293 wait_us(810);
FumiyaKoike 0:22a84e434ec8 294 _permit = 0;
FumiyaKoike 0:22a84e434ec8 295
FumiyaKoike 0:22a84e434ec8 296 _ROM_Update(newID);
FumiyaKoike 0:22a84e434ec8 297 }
FumiyaKoike 0:22a84e434ec8 298
FumiyaKoike 0:22a84e434ec8 299 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 300 Arguments:
FumiyaKoike 0:22a84e434ec8 301 ID ... 角度を読みたいサーボのID
FumiyaKoike 0:22a84e434ec8 302
FumiyaKoike 0:22a84e434ec8 303 Return:
FumiyaKoike 0:22a84e434ec8 304 get_deg(int) ... サーボの角度
FumiyaKoike 0:22a84e434ec8 305
FumiyaKoike 0:22a84e434ec8 306 Description:
FumiyaKoike 0:22a84e434ec8 307
FumiyaKoike 0:22a84e434ec8 308 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 309 int RS405CB::Read_Deg(unsigned char ID)
FumiyaKoike 0:22a84e434ec8 310 {
FumiyaKoike 0:22a84e434ec8 311 unsigned char CheckSum=0;
FumiyaKoike 0:22a84e434ec8 312 int get_deg = 0;
FumiyaKoike 0:22a84e434ec8 313 unsigned char RxData[8];
FumiyaKoike 0:22a84e434ec8 314 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 315
FumiyaKoike 0:22a84e434ec8 316 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 317 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 318 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 319 TxData[3] = 0x0F;
FumiyaKoike 0:22a84e434ec8 320 TxData[4] = 0x2A;
FumiyaKoike 0:22a84e434ec8 321 TxData[5] = 0x02;
FumiyaKoike 0:22a84e434ec8 322 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 323
FumiyaKoike 0:22a84e434ec8 324 for(int j=2; j<=6; j++) {
FumiyaKoike 0:22a84e434ec8 325 CheckSum = CheckSum ^ TxData[j];
FumiyaKoike 0:22a84e434ec8 326 }
FumiyaKoike 0:22a84e434ec8 327 TxData[7]=CheckSum;
FumiyaKoike 0:22a84e434ec8 328
FumiyaKoike 0:22a84e434ec8 329 _permit = 1;
FumiyaKoike 0:22a84e434ec8 330 for (int j=0; j<=7; j++)
FumiyaKoike 0:22a84e434ec8 331 putc(TxData[j]);
FumiyaKoike 0:22a84e434ec8 332 wait_us(670);
FumiyaKoike 0:22a84e434ec8 333 _permit = 0;
FumiyaKoike 0:22a84e434ec8 334
FumiyaKoike 0:22a84e434ec8 335 CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 336 while(getc() != 0xFD);
FumiyaKoike 0:22a84e434ec8 337 while(getc() != 0xDF);
FumiyaKoike 0:22a84e434ec8 338
FumiyaKoike 0:22a84e434ec8 339 for(int j=0; j<=6; j++){
FumiyaKoike 0:22a84e434ec8 340 RxData[j] = getc();
FumiyaKoike 0:22a84e434ec8 341 CheckSum = CheckSum ^ RxData[j];
FumiyaKoike 0:22a84e434ec8 342 }
FumiyaKoike 0:22a84e434ec8 343
FumiyaKoike 0:22a84e434ec8 344 if(CheckSum == getc()){
FumiyaKoike 0:22a84e434ec8 345 get_deg = (int)RxData[6];
FumiyaKoike 0:22a84e434ec8 346 get_deg = get_deg << 8;
FumiyaKoike 0:22a84e434ec8 347 get_deg |= (int)RxData[5];
FumiyaKoike 0:22a84e434ec8 348 }
FumiyaKoike 0:22a84e434ec8 349
FumiyaKoike 0:22a84e434ec8 350 return _s16(get_deg);
FumiyaKoike 0:22a84e434ec8 351 }
FumiyaKoike 0:22a84e434ec8 352
FumiyaKoike 0:22a84e434ec8 353 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 354 Arguments:
FumiyaKoike 0:22a84e434ec8 355 ID ... 電流を読みたいサーボのID
FumiyaKoike 0:22a84e434ec8 356
FumiyaKoike 0:22a84e434ec8 357 Return:
FumiyaKoike 0:22a84e434ec8 358 get_mA(int) ... サーボに流れている電流量
FumiyaKoike 0:22a84e434ec8 359
FumiyaKoike 0:22a84e434ec8 360 Description:
FumiyaKoike 0:22a84e434ec8 361 メーカー曰く，参考程度の値なので注意
FumiyaKoike 0:22a84e434ec8 362
FumiyaKoike 0:22a84e434ec8 363 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 364 int RS405CB::Read_mA(unsigned char ID)
FumiyaKoike 0:22a84e434ec8 365 {
FumiyaKoike 0:22a84e434ec8 366 unsigned char CheckSum=0;
FumiyaKoike 0:22a84e434ec8 367 int get_mA = 0;
FumiyaKoike 0:22a84e434ec8 368 unsigned char RxData[8];
FumiyaKoike 0:22a84e434ec8 369 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 370 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 371 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 372 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 373 TxData[3] = 0x0F;
FumiyaKoike 0:22a84e434ec8 374 TxData[4] = 0x30;
FumiyaKoike 0:22a84e434ec8 375 TxData[5] = 0x02;
FumiyaKoike 0:22a84e434ec8 376 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 377
FumiyaKoike 0:22a84e434ec8 378 for(int j=2; j<=6; j++)
FumiyaKoike 0:22a84e434ec8 379 {
FumiyaKoike 0:22a84e434ec8 380 CheckSum = CheckSum ^ TxData[j];
FumiyaKoike 0:22a84e434ec8 381 }
FumiyaKoike 0:22a84e434ec8 382 TxData[7]=CheckSum;
FumiyaKoike 0:22a84e434ec8 383
FumiyaKoike 0:22a84e434ec8 384 _permit = 1;
FumiyaKoike 0:22a84e434ec8 385 for (int j=0; j<=7; j++)
FumiyaKoike 0:22a84e434ec8 386 putc(TxData[j]);
FumiyaKoike 0:22a84e434ec8 387 wait_us(670);
FumiyaKoike 0:22a84e434ec8 388 _permit = 0;
FumiyaKoike 0:22a84e434ec8 389
FumiyaKoike 0:22a84e434ec8 390 CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 391 while(getc() != 0xFD);
FumiyaKoike 0:22a84e434ec8 392 while(getc() != 0xDF);
FumiyaKoike 0:22a84e434ec8 393
FumiyaKoike 0:22a84e434ec8 394 for(int j=0; j<=6; j++){
FumiyaKoike 0:22a84e434ec8 395 RxData[j] = getc();
FumiyaKoike 0:22a84e434ec8 396 CheckSum = CheckSum ^ RxData[j];
FumiyaKoike 0:22a84e434ec8 397 }
FumiyaKoike 0:22a84e434ec8 398
FumiyaKoike 0:22a84e434ec8 399 if(CheckSum == getc()){
FumiyaKoike 0:22a84e434ec8 400 get_mA = (int)RxData[6];
FumiyaKoike 0:22a84e434ec8 401 get_mA = get_mA << 8;
FumiyaKoike 0:22a84e434ec8 402 get_mA |= (int)RxData[5];
FumiyaKoike 0:22a84e434ec8 403 }
FumiyaKoike 0:22a84e434ec8 404
FumiyaKoike 0:22a84e434ec8 405 return _s16(get_mA);
FumiyaKoike 0:22a84e434ec8 406 }
FumiyaKoike 0:22a84e434ec8 407
FumiyaKoike 0:22a84e434ec8 408
FumiyaKoike 0:22a84e434ec8 409 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 410 Arguments:
FumiyaKoike 0:22a84e434ec8 411 ID ... ROMをアップデートしたいサーボのID
FumiyaKoike 0:22a84e434ec8 412
FumiyaKoike 0:22a84e434ec8 413 Description:
FumiyaKoike 0:22a84e434ec8 414 ROM領域の値を変更した時に実行する．
FumiyaKoike 0:22a84e434ec8 415
FumiyaKoike 0:22a84e434ec8 416 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 417 void RS405CB::_ROM_Update(unsigned char ID){
FumiyaKoike 0:22a84e434ec8 418 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 419 unsigned char CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 420
FumiyaKoike 0:22a84e434ec8 421 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 422 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 423 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 424 TxData[3] = 0x40;
FumiyaKoike 0:22a84e434ec8 425 TxData[4] = 0xFF;
FumiyaKoike 0:22a84e434ec8 426 TxData[5] = 0x00;
FumiyaKoike 0:22a84e434ec8 427 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 428
FumiyaKoike 0:22a84e434ec8 429 for(int i=2; i<=6; i++)
FumiyaKoike 0:22a84e434ec8 430 {
FumiyaKoike 0:22a84e434ec8 431 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 0:22a84e434ec8 432 }
FumiyaKoike 0:22a84e434ec8 433 TxData[7] = CheckSum;
FumiyaKoike 0:22a84e434ec8 434
FumiyaKoike 0:22a84e434ec8 435 _permit = 1;
FumiyaKoike 0:22a84e434ec8 436 for(int i=0; i<=7; i++) {
FumiyaKoike 0:22a84e434ec8 437 putc(TxData[i]);
FumiyaKoike 0:22a84e434ec8 438 }
FumiyaKoike 0:22a84e434ec8 439 wait_us(810);
FumiyaKoike 0:22a84e434ec8 440 _permit = 0;
FumiyaKoike 0:22a84e434ec8 441 wait(1.5);
FumiyaKoike 0:22a84e434ec8 442 }
FumiyaKoike 0:22a84e434ec8 443
FumiyaKoike 0:22a84e434ec8 444 int RS405CB::_s16(int value)
FumiyaKoike 0:22a84e434ec8 445 {
FumiyaKoike 0:22a84e434ec8 446 return -(value & 0b1000000000000000)|(value & 0b0111111111111111);
FumiyaKoike 0:22a84e434ec8 447 }