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@0:22a84e434ec8, 2019-12-08 (annotated)

Committer:
FumiyaKoike
Date:
Sun Dec 08 06:20:03 2019 +0000
Revision:
0:22a84e434ec8
Child:
1:d9b19e1c1a9f
This library is for RS405CB that is controlled by RS-485

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 0:22a84e434ec8 71 oldID ... IDを変更したいサーボの現在のID
FumiyaKoike 0:22a84e434ec8 72 newID ... 新しいID．最大は255
FumiyaKoike 0:22a84e434ec8 73
FumiyaKoike 0:22a84e434ec8 74 Description:
FumiyaKoike 0:22a84e434ec8 75
FumiyaKoike 0:22a84e434ec8 76 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 77 void RS405CB::Change_ID(unsigned char oldID, unsigned char newID){
FumiyaKoike 0:22a84e434ec8 78 unsigned char TxData[9];
FumiyaKoike 0:22a84e434ec8 79 unsigned char CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 80
FumiyaKoike 0:22a84e434ec8 81 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 82 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 83 TxData[2] = oldID;
FumiyaKoike 0:22a84e434ec8 84 TxData[3] = 0x00;
FumiyaKoike 0:22a84e434ec8 85 TxData[4] = 0x04;
FumiyaKoike 0:22a84e434ec8 86 TxData[5] = 0x01;
FumiyaKoike 0:22a84e434ec8 87 TxData[6] = 0x01;
FumiyaKoike 0:22a84e434ec8 88 TxData[7] = newID;
FumiyaKoike 0:22a84e434ec8 89
FumiyaKoike 0:22a84e434ec8 90 for(int i=2; i<=7; i++)
FumiyaKoike 0:22a84e434ec8 91 {
FumiyaKoike 0:22a84e434ec8 92 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 0:22a84e434ec8 93 }
FumiyaKoike 0:22a84e434ec8 94 TxData[8] = CheckSum;
FumiyaKoike 0:22a84e434ec8 95
FumiyaKoike 0:22a84e434ec8 96 _permit = 1;
FumiyaKoike 0:22a84e434ec8 97 for(int i=0; i<=8; i++) {
FumiyaKoike 0:22a84e434ec8 98 putc(TxData[i]);
FumiyaKoike 0:22a84e434ec8 99 }
FumiyaKoike 0:22a84e434ec8 100 wait_us(810);
FumiyaKoike 0:22a84e434ec8 101 _permit = 0;
FumiyaKoike 0:22a84e434ec8 102
FumiyaKoike 0:22a84e434ec8 103 _ROM_Update(newID);
FumiyaKoike 0:22a84e434ec8 104 }
FumiyaKoike 0:22a84e434ec8 105
FumiyaKoike 0:22a84e434ec8 106 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 107 Arguments:
FumiyaKoike 0:22a84e434ec8 108 ID ... 角度を読みたいサーボのID
FumiyaKoike 0:22a84e434ec8 109
FumiyaKoike 0:22a84e434ec8 110 Return:
FumiyaKoike 0:22a84e434ec8 111 get_deg(int) ... サーボの角度
FumiyaKoike 0:22a84e434ec8 112
FumiyaKoike 0:22a84e434ec8 113 Description:
FumiyaKoike 0:22a84e434ec8 114
FumiyaKoike 0:22a84e434ec8 115 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 116 int RS405CB::Read_Deg(unsigned char ID)
FumiyaKoike 0:22a84e434ec8 117 {
FumiyaKoike 0:22a84e434ec8 118 unsigned char CheckSum=0;
FumiyaKoike 0:22a84e434ec8 119 int get_deg = 0;
FumiyaKoike 0:22a84e434ec8 120 unsigned char RxData[8];
FumiyaKoike 0:22a84e434ec8 121 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 122
FumiyaKoike 0:22a84e434ec8 123 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 124 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 125 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 126 TxData[3] = 0x0F;
FumiyaKoike 0:22a84e434ec8 127 TxData[4] = 0x2A;
FumiyaKoike 0:22a84e434ec8 128 TxData[5] = 0x02;
FumiyaKoike 0:22a84e434ec8 129 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 130
FumiyaKoike 0:22a84e434ec8 131 for(int j=2; j<=6; j++) {
FumiyaKoike 0:22a84e434ec8 132 CheckSum = CheckSum ^ TxData[j];
FumiyaKoike 0:22a84e434ec8 133 }
FumiyaKoike 0:22a84e434ec8 134 TxData[7]=CheckSum;
FumiyaKoike 0:22a84e434ec8 135
FumiyaKoike 0:22a84e434ec8 136 _permit = 1;
FumiyaKoike 0:22a84e434ec8 137 for (int j=0; j<=7; j++)
FumiyaKoike 0:22a84e434ec8 138 putc(TxData[j]);
FumiyaKoike 0:22a84e434ec8 139 wait_us(670);
FumiyaKoike 0:22a84e434ec8 140 _permit = 0;
FumiyaKoike 0:22a84e434ec8 141
FumiyaKoike 0:22a84e434ec8 142 CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 143 while(getc() != 0xFD);
FumiyaKoike 0:22a84e434ec8 144 while(getc() != 0xDF);
FumiyaKoike 0:22a84e434ec8 145
FumiyaKoike 0:22a84e434ec8 146 for(int j=0; j<=6; j++){
FumiyaKoike 0:22a84e434ec8 147 RxData[j] = getc();
FumiyaKoike 0:22a84e434ec8 148 CheckSum = CheckSum ^ RxData[j];
FumiyaKoike 0:22a84e434ec8 149 }
FumiyaKoike 0:22a84e434ec8 150
FumiyaKoike 0:22a84e434ec8 151 if(CheckSum == getc()){
FumiyaKoike 0:22a84e434ec8 152 get_deg = (int)RxData[6];
FumiyaKoike 0:22a84e434ec8 153 get_deg = get_deg << 8;
FumiyaKoike 0:22a84e434ec8 154 get_deg |= (int)RxData[5];
FumiyaKoike 0:22a84e434ec8 155 }
FumiyaKoike 0:22a84e434ec8 156
FumiyaKoike 0:22a84e434ec8 157 return _s16(get_deg);
FumiyaKoike 0:22a84e434ec8 158 }
FumiyaKoike 0:22a84e434ec8 159
FumiyaKoike 0:22a84e434ec8 160 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 161 Arguments:
FumiyaKoike 0:22a84e434ec8 162 ID ... 電流を読みたいサーボのID
FumiyaKoike 0:22a84e434ec8 163
FumiyaKoike 0:22a84e434ec8 164 Return:
FumiyaKoike 0:22a84e434ec8 165 get_mA(int) ... サーボに流れている電流量
FumiyaKoike 0:22a84e434ec8 166
FumiyaKoike 0:22a84e434ec8 167 Description:
FumiyaKoike 0:22a84e434ec8 168 メーカー曰く，参考程度の値なので注意
FumiyaKoike 0:22a84e434ec8 169
FumiyaKoike 0:22a84e434ec8 170 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 171 int RS405CB::Read_mA(unsigned char ID)
FumiyaKoike 0:22a84e434ec8 172 {
FumiyaKoike 0:22a84e434ec8 173 unsigned char CheckSum=0;
FumiyaKoike 0:22a84e434ec8 174 int get_mA = 0;
FumiyaKoike 0:22a84e434ec8 175 unsigned char RxData[8];
FumiyaKoike 0:22a84e434ec8 176 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 177 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 178 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 179 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 180 TxData[3] = 0x0F;
FumiyaKoike 0:22a84e434ec8 181 TxData[4] = 0x30;
FumiyaKoike 0:22a84e434ec8 182 TxData[5] = 0x02;
FumiyaKoike 0:22a84e434ec8 183 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 184
FumiyaKoike 0:22a84e434ec8 185 for(int j=2; j<=6; j++)
FumiyaKoike 0:22a84e434ec8 186 {
FumiyaKoike 0:22a84e434ec8 187 CheckSum = CheckSum ^ TxData[j];
FumiyaKoike 0:22a84e434ec8 188 }
FumiyaKoike 0:22a84e434ec8 189 TxData[7]=CheckSum;
FumiyaKoike 0:22a84e434ec8 190
FumiyaKoike 0:22a84e434ec8 191 _permit = 1;
FumiyaKoike 0:22a84e434ec8 192 for (int j=0; j<=7; j++)
FumiyaKoike 0:22a84e434ec8 193 putc(TxData[j]);
FumiyaKoike 0:22a84e434ec8 194 wait_us(670);
FumiyaKoike 0:22a84e434ec8 195 _permit = 0;
FumiyaKoike 0:22a84e434ec8 196
FumiyaKoike 0:22a84e434ec8 197 CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 198 while(getc() != 0xFD);
FumiyaKoike 0:22a84e434ec8 199 while(getc() != 0xDF);
FumiyaKoike 0:22a84e434ec8 200
FumiyaKoike 0:22a84e434ec8 201 for(int j=0; j<=6; j++){
FumiyaKoike 0:22a84e434ec8 202 RxData[j] = getc();
FumiyaKoike 0:22a84e434ec8 203 CheckSum = CheckSum ^ RxData[j];
FumiyaKoike 0:22a84e434ec8 204 }
FumiyaKoike 0:22a84e434ec8 205
FumiyaKoike 0:22a84e434ec8 206 if(CheckSum == getc()){
FumiyaKoike 0:22a84e434ec8 207 get_mA = (int)RxData[6];
FumiyaKoike 0:22a84e434ec8 208 get_mA = get_mA << 8;
FumiyaKoike 0:22a84e434ec8 209 get_mA |= (int)RxData[5];
FumiyaKoike 0:22a84e434ec8 210 }
FumiyaKoike 0:22a84e434ec8 211
FumiyaKoike 0:22a84e434ec8 212 return _s16(get_mA);
FumiyaKoike 0:22a84e434ec8 213 }
FumiyaKoike 0:22a84e434ec8 214
FumiyaKoike 0:22a84e434ec8 215
FumiyaKoike 0:22a84e434ec8 216 /* -----------------------------------------------------------------------------
FumiyaKoike 0:22a84e434ec8 217 Arguments:
FumiyaKoike 0:22a84e434ec8 218 ID ... ROMをアップデートしたいサーボのID
FumiyaKoike 0:22a84e434ec8 219
FumiyaKoike 0:22a84e434ec8 220 Description:
FumiyaKoike 0:22a84e434ec8 221 ROM領域の値を変更した時に実行する．
FumiyaKoike 0:22a84e434ec8 222
FumiyaKoike 0:22a84e434ec8 223 ----------------------------------------------------------------------------- */
FumiyaKoike 0:22a84e434ec8 224 void RS405CB::_ROM_Update(unsigned char ID){
FumiyaKoike 0:22a84e434ec8 225 unsigned char TxData[8];
FumiyaKoike 0:22a84e434ec8 226 unsigned char CheckSum = 0;
FumiyaKoike 0:22a84e434ec8 227
FumiyaKoike 0:22a84e434ec8 228 TxData[0] = 0xFA;
FumiyaKoike 0:22a84e434ec8 229 TxData[1] = 0xAF;
FumiyaKoike 0:22a84e434ec8 230 TxData[2] = ID;
FumiyaKoike 0:22a84e434ec8 231 TxData[3] = 0x40;
FumiyaKoike 0:22a84e434ec8 232 TxData[4] = 0xFF;
FumiyaKoike 0:22a84e434ec8 233 TxData[5] = 0x00;
FumiyaKoike 0:22a84e434ec8 234 TxData[6] = 0x00;
FumiyaKoike 0:22a84e434ec8 235
FumiyaKoike 0:22a84e434ec8 236 for(int i=2; i<=6; i++)
FumiyaKoike 0:22a84e434ec8 237 {
FumiyaKoike 0:22a84e434ec8 238 CheckSum = CheckSum ^ TxData[i];
FumiyaKoike 0:22a84e434ec8 239 }
FumiyaKoike 0:22a84e434ec8 240 TxData[7] = CheckSum;
FumiyaKoike 0:22a84e434ec8 241
FumiyaKoike 0:22a84e434ec8 242 _permit = 1;
FumiyaKoike 0:22a84e434ec8 243 for(int i=0; i<=7; i++) {
FumiyaKoike 0:22a84e434ec8 244 putc(TxData[i]);
FumiyaKoike 0:22a84e434ec8 245 }
FumiyaKoike 0:22a84e434ec8 246 wait_us(810);
FumiyaKoike 0:22a84e434ec8 247 _permit = 0;
FumiyaKoike 0:22a84e434ec8 248 wait(1.5);
FumiyaKoike 0:22a84e434ec8 249 }
FumiyaKoike 0:22a84e434ec8 250
FumiyaKoike 0:22a84e434ec8 251 int RS405CB::_s16(int value)
FumiyaKoike 0:22a84e434ec8 252 {
FumiyaKoike 0:22a84e434ec8 253 return -(value & 0b1000000000000000)|(value & 0b0111111111111111);
FumiyaKoike 0:22a84e434ec8 254 }