Fumiya KOIKE / RS405CB_light

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@2:8ff319643fc3, 2019-12-11 (annotated)

Committer:
FumiyaKoike
Date:
Wed Dec 11 07:26:19 2019 +0000
Revision:
2:8ff319643fc3
Parent:
1:d9b19e1c1a9f
Child:
3:9311ea940978
Add SetSpeed Function

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 0:22a84e434ec8 34 ID ... 動かしたいサーボのID
FumiyaKoike 0:22a84e434ec8 35 data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）
FumiyaKoike 0:22a84e434ec8 36
FumiyaKoike 0:22a84e434ec8 37 Description:
FumiyaKoike 0:22a84e434ec8 38
FumiyaKoike 0:22a84e434ec8 39 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 40 void RS405CB::GoalPosition(unsigned char ID, int data)
FumiyaKoike 0:22a84e434ec8 41 {
FumiyaKoike 0:22a84e434ec8 42 unsigned char TxData[10];
FumiyaKoike 0:22a84e434ec8 43 unsigned char CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 44
FumiyaKoike 0:22a84e434ec8 45 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 46 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 47 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 48 TxData[3] = 0x00;
FumiyaKoike 0:22a84e434ec8 49 TxData[4] = 0x1E;
FumiyaKoike 0:22a84e434ec8 50 TxData[5] = 0x02;
FumiyaKoike 0:22a84e434ec8 51 TxData[6] = 0x01;
FumiyaKoike 0:22a84e434ec8 52 TxData[7] = (unsigned char)0x00FF & data;
FumiyaKoike 0:22a84e434ec8 53 TxData[8] = (unsigned char)0x00FF & (data >> 8);
FumiyaKoike 0:22a84e434ec8 54
FumiyaKoike 0:22a84e434ec8 55 for(int i=2; i<=8; i++)
FumiyaKoike 0:22a84e434ec8 56 {
FumiyaKoike 0:22a84e434ec8 57 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 0:22a84e434ec8 58 }
FumiyaKoike 0:22a84e434ec8 59 TxData[9] = CheckSum;
FumiyaKoike 0:22a84e434ec8 60
FumiyaKoike 0:22a84e434ec8 61 _permit = 1;
FumiyaKoike 0:22a84e434ec8 62 for(int i=0; i<=9; i++) {
FumiyaKoike 0:22a84e434ec8 63 putc(TxData[i]);
FumiyaKoike 0:22a84e434ec8 64 }
FumiyaKoike 0:22a84e434ec8 65 wait_us(810);
FumiyaKoike 0:22a84e434ec8 66 _permit = 0;
FumiyaKoike 0:22a84e434ec8 67 }
FumiyaKoike 0:22a84e434ec8 68
FumiyaKoike 0:22a84e434ec8 69 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 70 Arguments:
FumiyaKoike 1:d9b19e1c1a9f 71 ID ... 動かしたいサーボのID
FumiyaKoike 1:d9b19e1c1a9f 72 data ... 目標角．degreeの10倍の値を指定する．（ex 90° -> data = 900）
FumiyaKoike 1:d9b19e1c1a9f 73 speed... 移動時間の指定．10ms単位で指定(ex. 5000ms -> 500)
FumiyaKoike 1:d9b19e1c1a9f 74
FumiyaKoike 1:d9b19e1c1a9f 75 Description:
FumiyaKoike 1:d9b19e1c1a9f 76
FumiyaKoike 1:d9b19e1c1a9f 77 ----------------------------------------------------------------------------- */
FumiyaKoike 1:d9b19e1c1a9f 78 void RS405CB::GoalPosition(unsigned char ID, int data, int speed)
FumiyaKoike 1:d9b19e1c1a9f 79 {
FumiyaKoike 1:d9b19e1c1a9f 80 unsigned char TxData[12];
FumiyaKoike 1:d9b19e1c1a9f 81 unsigned char CheckSum = 0;
FumiyaKoike 1:d9b19e1c1a9f 82
FumiyaKoike 1:d9b19e1c1a9f 83 TxData[0] = 0xFA;
FumiyaKoike 1:d9b19e1c1a9f 84 TxData[1] = 0xAF;
FumiyaKoike 1:d9b19e1c1a9f 85 TxData[2] = ID;
FumiyaKoike 1:d9b19e1c1a9f 86 TxData[3] = 0x00;
FumiyaKoike 1:d9b19e1c1a9f 87 TxData[4] = 0x1E;
FumiyaKoike 1:d9b19e1c1a9f 88 TxData[5] = 0x04;
FumiyaKoike 1:d9b19e1c1a9f 89 TxData[6] = 0x01;
FumiyaKoike 1:d9b19e1c1a9f 90 TxData[7] = (unsigned char)0x00FF & data;
FumiyaKoike 1:d9b19e1c1a9f 91 TxData[8] = (unsigned char)0x00FF & (data >> 8);
FumiyaKoike 1:d9b19e1c1a9f 92 TxData[9] = (unsigned char)0x00FF & speed;
FumiyaKoike 1:d9b19e1c1a9f 93 TxData[10] = (unsigned char)0x00FF & (speed >> 8);
FumiyaKoike 1:d9b19e1c1a9f 94
FumiyaKoike 1:d9b19e1c1a9f 95 for(int i=2; i<=10; i++)
FumiyaKoike 1:d9b19e1c1a9f 96 {
FumiyaKoike 1:d9b19e1c1a9f 97 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 1:d9b19e1c1a9f 98 }
FumiyaKoike 1:d9b19e1c1a9f 99 TxData[11] = CheckSum;
FumiyaKoike 1:d9b19e1c1a9f 100
FumiyaKoike 1:d9b19e1c1a9f 101 _permit = 1;
FumiyaKoike 1:d9b19e1c1a9f 102 for(int i=0; i<=11; i++) {
FumiyaKoike 1:d9b19e1c1a9f 103 putc(TxData[i]);
FumiyaKoike 1:d9b19e1c1a9f 104 }
FumiyaKoike 1:d9b19e1c1a9f 105 wait_us(810);
FumiyaKoike 1:d9b19e1c1a9f 106 _permit = 0;
FumiyaKoike 1:d9b19e1c1a9f 107 }
FumiyaKoike 1:d9b19e1c1a9f 108
FumiyaKoike 2:8ff319643fc3 109 /* -----------------------------------------------------------------------------
FumiyaKoike 2:8ff319643fc3 110 Arguments:
FumiyaKoike 2:8ff319643fc3 111 ID ...
FumiyaKoike 2:8ff319643fc3 112 speed ... 移動時間を10ms単位で指定
FumiyaKoike 2:8ff319643fc3 113
FumiyaKoike 2:8ff319643fc3 114 Description:
FumiyaKoike 2:8ff319643fc3 115
FumiyaKoike 2:8ff319643fc3 116 ----------------------------------------------------------------------------- */
FumiyaKoike 2:8ff319643fc3 117
FumiyaKoike 2:8ff319643fc3 118 void RS405CB::SetSpeed(unsigned char ID, int speed)
FumiyaKoike 2:8ff319643fc3 119 {
FumiyaKoike 2:8ff319643fc3 120 unsigned char TxData[10];
FumiyaKoike 2:8ff319643fc3 121 unsigned char CheckSum = 0;
FumiyaKoike 2:8ff319643fc3 122
FumiyaKoike 2:8ff319643fc3 123 TxData[0] = 0xFA;
FumiyaKoike 2:8ff319643fc3 124 TxData[1] = 0xAF;
FumiyaKoike 2:8ff319643fc3 125 TxData[2] = ID;
FumiyaKoike 2:8ff319643fc3 126 TxData[3] = 0x00;
FumiyaKoike 2:8ff319643fc3 127 TxData[4] = 0x1E + 2;
FumiyaKoike 2:8ff319643fc3 128 TxData[5] = 0x02;
FumiyaKoike 2:8ff319643fc3 129 TxData[6] = 0x01;
FumiyaKoike 2:8ff319643fc3 130 TxData[7] = (unsigned char)0x00FF & speed;
FumiyaKoike 2:8ff319643fc3 131 TxData[8] = (unsigned char)0x00FF & (speed >> 8);
FumiyaKoike 2:8ff319643fc3 132
FumiyaKoike 2:8ff319643fc3 133 for(int i=2; i<=8; i++)
FumiyaKoike 2:8ff319643fc3 134 {
FumiyaKoike 2:8ff319643fc3 135 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 2:8ff319643fc3 136 }
FumiyaKoike 2:8ff319643fc3 137 TxData[9] = CheckSum;
FumiyaKoike 2:8ff319643fc3 138
FumiyaKoike 2:8ff319643fc3 139 _permit = 1;
FumiyaKoike 2:8ff319643fc3 140 for(int i=0; i<=9; i++) {
FumiyaKoike 2:8ff319643fc3 141 putc(TxData[i]);
FumiyaKoike 2:8ff319643fc3 142 }
FumiyaKoike 2:8ff319643fc3 143 wait_us(810);
FumiyaKoike 2:8ff319643fc3 144 _permit = 0;
FumiyaKoike 2:8ff319643fc3 145 }
FumiyaKoike 1:d9b19e1c1a9f 146
FumiyaKoike 1:d9b19e1c1a9f 147 /* -----------------------------------------------------------------------------
FumiyaKoike 1:d9b19e1c1a9f 148 Arguments:
FumiyaKoike 0:22a84e434ec8 149 oldID ... IDを変更したいサーボの現在のID
FumiyaKoike 0:22a84e434ec8 150 newID ... 新しいID．最大は255
FumiyaKoike 0:22a84e434ec8 151
FumiyaKoike 0:22a84e434ec8 152 Description:
FumiyaKoike 0:22a84e434ec8 153
FumiyaKoike 0:22a84e434ec8 154 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 155 void RS405CB::Change_ID(unsigned char oldID, unsigned char newID){
FumiyaKoike 0:22a84e434ec8 156 unsigned char TxData[9];
FumiyaKoike 0:22a84e434ec8 157 unsigned char CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 158
FumiyaKoike 0:22a84e434ec8 159 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 160 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 161 TxData[2] = oldID;
FumiyaKoike 0:22a84e434ec8 162 TxData[3] = 0x00;
FumiyaKoike 0:22a84e434ec8 163 TxData[4] = 0x04;
FumiyaKoike 0:22a84e434ec8 164 TxData[5] = 0x01;
FumiyaKoike 0:22a84e434ec8 165 TxData[6] = 0x01;
FumiyaKoike 0:22a84e434ec8 166 TxData[7] = newID;
FumiyaKoike 0:22a84e434ec8 167
FumiyaKoike 0:22a84e434ec8 168 for(int i=2; i<=7; i++)
FumiyaKoike 0:22a84e434ec8 169 {
FumiyaKoike 0:22a84e434ec8 170 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 0:22a84e434ec8 171 }
FumiyaKoike 0:22a84e434ec8 172 TxData[8] = CheckSum;
FumiyaKoike 0:22a84e434ec8 173
FumiyaKoike 0:22a84e434ec8 174 _permit = 1;
FumiyaKoike 0:22a84e434ec8 175 for(int i=0; i<=8; i++) {
FumiyaKoike 0:22a84e434ec8 176 putc(TxData[i]);
FumiyaKoike 0:22a84e434ec8 177 }
FumiyaKoike 0:22a84e434ec8 178 wait_us(810);
FumiyaKoike 0:22a84e434ec8 179 _permit = 0;
FumiyaKoike 0:22a84e434ec8 180
FumiyaKoike 0:22a84e434ec8 181 _ROM_Update(newID);
FumiyaKoike 0:22a84e434ec8 182 }
FumiyaKoike 0:22a84e434ec8 183
FumiyaKoike 0:22a84e434ec8 184 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 185 Arguments:
FumiyaKoike 0:22a84e434ec8 186 ID ... 角度を読みたいサーボのID
FumiyaKoike 0:22a84e434ec8 187
FumiyaKoike 0:22a84e434ec8 188 Return:
FumiyaKoike 0:22a84e434ec8 189 get_deg(int) ... サーボの角度
FumiyaKoike 0:22a84e434ec8 190
FumiyaKoike 0:22a84e434ec8 191 Description:
FumiyaKoike 0:22a84e434ec8 192
FumiyaKoike 0:22a84e434ec8 193 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 194 int RS405CB::Read_Deg(unsigned char ID)
FumiyaKoike 0:22a84e434ec8 195 {
FumiyaKoike 0:22a84e434ec8 196 unsigned char CheckSum=0;
FumiyaKoike 0:22a84e434ec8 197 int get_deg = 0;
FumiyaKoike 0:22a84e434ec8 198 unsigned char RxData[8];
FumiyaKoike 0:22a84e434ec8 199 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 200
FumiyaKoike 0:22a84e434ec8 201 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 202 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 203 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 204 TxData[3] = 0x0F;
FumiyaKoike 0:22a84e434ec8 205 TxData[4] = 0x2A;
FumiyaKoike 0:22a84e434ec8 206 TxData[5] = 0x02;
FumiyaKoike 0:22a84e434ec8 207 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 208
FumiyaKoike 0:22a84e434ec8 209 for(int j=2; j<=6; j++) {
FumiyaKoike 0:22a84e434ec8 210 CheckSum = CheckSum ^ TxData[j];
FumiyaKoike 0:22a84e434ec8 211 }
FumiyaKoike 0:22a84e434ec8 212 TxData[7]=CheckSum;
FumiyaKoike 0:22a84e434ec8 213
FumiyaKoike 0:22a84e434ec8 214 _permit = 1;
FumiyaKoike 0:22a84e434ec8 215 for (int j=0; j<=7; j++)
FumiyaKoike 0:22a84e434ec8 216 putc(TxData[j]);
FumiyaKoike 0:22a84e434ec8 217 wait_us(670);
FumiyaKoike 0:22a84e434ec8 218 _permit = 0;
FumiyaKoike 0:22a84e434ec8 219
FumiyaKoike 0:22a84e434ec8 220 CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 221 while(getc() != 0xFD);
FumiyaKoike 0:22a84e434ec8 222 while(getc() != 0xDF);
FumiyaKoike 0:22a84e434ec8 223
FumiyaKoike 0:22a84e434ec8 224 for(int j=0; j<=6; j++){
FumiyaKoike 0:22a84e434ec8 225 RxData[j] = getc();
FumiyaKoike 0:22a84e434ec8 226 CheckSum = CheckSum ^ RxData[j];
FumiyaKoike 0:22a84e434ec8 227 }
FumiyaKoike 0:22a84e434ec8 228
FumiyaKoike 0:22a84e434ec8 229 if(CheckSum == getc()){
FumiyaKoike 0:22a84e434ec8 230 get_deg = (int)RxData[6];
FumiyaKoike 0:22a84e434ec8 231 get_deg = get_deg << 8;
FumiyaKoike 0:22a84e434ec8 232 get_deg |= (int)RxData[5];
FumiyaKoike 0:22a84e434ec8 233 }
FumiyaKoike 0:22a84e434ec8 234
FumiyaKoike 0:22a84e434ec8 235 return _s16(get_deg);
FumiyaKoike 0:22a84e434ec8 236 }
FumiyaKoike 0:22a84e434ec8 237
FumiyaKoike 0:22a84e434ec8 238 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 239 Arguments:
FumiyaKoike 0:22a84e434ec8 240 ID ... 電流を読みたいサーボのID
FumiyaKoike 0:22a84e434ec8 241
FumiyaKoike 0:22a84e434ec8 242 Return:
FumiyaKoike 0:22a84e434ec8 243 get_mA(int) ... サーボに流れている電流量
FumiyaKoike 0:22a84e434ec8 244
FumiyaKoike 0:22a84e434ec8 245 Description:
FumiyaKoike 0:22a84e434ec8 246 メーカー曰く，参考程度の値なので注意
FumiyaKoike 0:22a84e434ec8 247
FumiyaKoike 0:22a84e434ec8 248 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 249 int RS405CB::Read_mA(unsigned char ID)
FumiyaKoike 0:22a84e434ec8 250 {
FumiyaKoike 0:22a84e434ec8 251 unsigned char CheckSum=0;
FumiyaKoike 0:22a84e434ec8 252 int get_mA = 0;
FumiyaKoike 0:22a84e434ec8 253 unsigned char RxData[8];
FumiyaKoike 0:22a84e434ec8 254 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 255 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 256 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 257 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 258 TxData[3] = 0x0F;
FumiyaKoike 0:22a84e434ec8 259 TxData[4] = 0x30;
FumiyaKoike 0:22a84e434ec8 260 TxData[5] = 0x02;
FumiyaKoike 0:22a84e434ec8 261 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 262
FumiyaKoike 0:22a84e434ec8 263 for(int j=2; j<=6; j++)
FumiyaKoike 0:22a84e434ec8 264 {
FumiyaKoike 0:22a84e434ec8 265 CheckSum = CheckSum ^ TxData[j];
FumiyaKoike 0:22a84e434ec8 266 }
FumiyaKoike 0:22a84e434ec8 267 TxData[7]=CheckSum;
FumiyaKoike 0:22a84e434ec8 268
FumiyaKoike 0:22a84e434ec8 269 _permit = 1;
FumiyaKoike 0:22a84e434ec8 270 for (int j=0; j<=7; j++)
FumiyaKoike 0:22a84e434ec8 271 putc(TxData[j]);
FumiyaKoike 0:22a84e434ec8 272 wait_us(670);
FumiyaKoike 0:22a84e434ec8 273 _permit = 0;
FumiyaKoike 0:22a84e434ec8 274
FumiyaKoike 0:22a84e434ec8 275 CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 276 while(getc() != 0xFD);
FumiyaKoike 0:22a84e434ec8 277 while(getc() != 0xDF);
FumiyaKoike 0:22a84e434ec8 278
FumiyaKoike 0:22a84e434ec8 279 for(int j=0; j<=6; j++){
FumiyaKoike 0:22a84e434ec8 280 RxData[j] = getc();
FumiyaKoike 0:22a84e434ec8 281 CheckSum = CheckSum ^ RxData[j];
FumiyaKoike 0:22a84e434ec8 282 }
FumiyaKoike 0:22a84e434ec8 283
FumiyaKoike 0:22a84e434ec8 284 if(CheckSum == getc()){
FumiyaKoike 0:22a84e434ec8 285 get_mA = (int)RxData[6];
FumiyaKoike 0:22a84e434ec8 286 get_mA = get_mA << 8;
FumiyaKoike 0:22a84e434ec8 287 get_mA |= (int)RxData[5];
FumiyaKoike 0:22a84e434ec8 288 }
FumiyaKoike 0:22a84e434ec8 289
FumiyaKoike 0:22a84e434ec8 290 return _s16(get_mA);
FumiyaKoike 0:22a84e434ec8 291 }
FumiyaKoike 0:22a84e434ec8 292
FumiyaKoike 0:22a84e434ec8 293
FumiyaKoike 0:22a84e434ec8 294 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 295 Arguments:
FumiyaKoike 0:22a84e434ec8 296 ID ... ROMをアップデートしたいサーボのID
FumiyaKoike 0:22a84e434ec8 297
FumiyaKoike 0:22a84e434ec8 298 Description:
FumiyaKoike 0:22a84e434ec8 299 ROM領域の値を変更した時に実行する．
FumiyaKoike 0:22a84e434ec8 300
FumiyaKoike 0:22a84e434ec8 301 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 302 void RS405CB::_ROM_Update(unsigned char ID){
FumiyaKoike 0:22a84e434ec8 303 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 304 unsigned char CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 305
FumiyaKoike 0:22a84e434ec8 306 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 307 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 308 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 309 TxData[3] = 0x40;
FumiyaKoike 0:22a84e434ec8 310 TxData[4] = 0xFF;
FumiyaKoike 0:22a84e434ec8 311 TxData[5] = 0x00;
FumiyaKoike 0:22a84e434ec8 312 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 313
FumiyaKoike 0:22a84e434ec8 314 for(int i=2; i<=6; i++)
FumiyaKoike 0:22a84e434ec8 315 {
FumiyaKoike 0:22a84e434ec8 316 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 0:22a84e434ec8 317 }
FumiyaKoike 0:22a84e434ec8 318 TxData[7] = CheckSum;
FumiyaKoike 0:22a84e434ec8 319
FumiyaKoike 0:22a84e434ec8 320 _permit = 1;
FumiyaKoike 0:22a84e434ec8 321 for(int i=0; i<=7; i++) {
FumiyaKoike 0:22a84e434ec8 322 putc(TxData[i]);
FumiyaKoike 0:22a84e434ec8 323 }
FumiyaKoike 0:22a84e434ec8 324 wait_us(810);
FumiyaKoike 0:22a84e434ec8 325 _permit = 0;
FumiyaKoike 0:22a84e434ec8 326 wait(1.5);
FumiyaKoike 0:22a84e434ec8 327 }
FumiyaKoike 0:22a84e434ec8 328
FumiyaKoike 0:22a84e434ec8 329 int RS405CB::_s16(int value)
FumiyaKoike 0:22a84e434ec8 330 {
FumiyaKoike 0:22a84e434ec8 331 return -(value & 0b1000000000000000)|(value & 0b0111111111111111);
FumiyaKoike 0:22a84e434ec8 332 }