Ryota Yamamoto / L6470SDC

Library for STMicroelectronics dSPIN L6470 stepper driver. "daisy-chain" supported.

Dependents:   L6470_daisy_chain l6470

L6470SDC.cpp@0:e1964b6e160c, 2015-05-30 (annotated)

Committer:
Yajirushi
Date:
Sat May 30 14:32:48 2015 +0000
Revision:
0:e1964b6e160c
Child:
1:db64ad30b4b3
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yajirushi 0:e1964b6e160c 1 #include "L6470SDC.h"
Yajirushi 0:e1964b6e160c 2
Yajirushi 0:e1964b6e160c 3 #define CS_ACTIVE cs->write(0)
Yajirushi 0:e1964b6e160c 4 #define CS_INACTIVE cs->write(1)
Yajirushi 0:e1964b6e160c 5
Yajirushi 0:e1964b6e160c 6 //充実のコンストラクタ
Yajirushi 0:e1964b6e160c 7 L6470SDC::L6470SDC(PinName tx, PinName rx, PinName mosi, PinName miso, PinName sclk, PinName csel){
Yajirushi 0:e1964b6e160c 8 pc = new Serial(tx, rx);
Yajirushi 0:e1964b6e160c 9 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 10 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 11 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 12 }
Yajirushi 0:e1964b6e160c 13 L6470SDC::L6470SDC(PinName tx, PinName rx, PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 14 pc = new Serial(tx, rx);
Yajirushi 0:e1964b6e160c 15 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 16 cs = csel;
Yajirushi 0:e1964b6e160c 17 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 18 }
Yajirushi 0:e1964b6e160c 19 L6470SDC::L6470SDC(Serial *serial, PinName mosi, PinName miso, PinName sclk, PinName csel){
Yajirushi 0:e1964b6e160c 20 pc = serial;
Yajirushi 0:e1964b6e160c 21 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 22 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 23 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 24 }
Yajirushi 0:e1964b6e160c 25 L6470SDC::L6470SDC(Serial *serial, PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 26 pc = serial;
Yajirushi 0:e1964b6e160c 27 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 28 cs = csel;
Yajirushi 0:e1964b6e160c 29 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 30 }
Yajirushi 0:e1964b6e160c 31 L6470SDC::L6470SDC(Serial *serial, SPI *spi, PinName csel){
Yajirushi 0:e1964b6e160c 32 pc = serial;
Yajirushi 0:e1964b6e160c 33 this->spi = spi;
Yajirushi 0:e1964b6e160c 34 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 35 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 36 }
Yajirushi 0:e1964b6e160c 37 L6470SDC::L6470SDC(Serial *serial, SPI *spi, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 38 pc = serial;
Yajirushi 0:e1964b6e160c 39 this->spi = spi;
Yajirushi 0:e1964b6e160c 40 cs = csel;
Yajirushi 0:e1964b6e160c 41 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 42 }
Yajirushi 0:e1964b6e160c 43 L6470SDC::L6470SDC(PinName mosi, PinName miso, PinName sclk, PinName csel){
Yajirushi 0:e1964b6e160c 44 hasSerial = false;
Yajirushi 0:e1964b6e160c 45 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 46 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 47 }
Yajirushi 0:e1964b6e160c 48 L6470SDC::L6470SDC(PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 49 hasSerial = false;
Yajirushi 0:e1964b6e160c 50 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 51 cs = csel;
Yajirushi 0:e1964b6e160c 52 }
Yajirushi 0:e1964b6e160c 53 L6470SDC::L6470SDC(SPI *spi, PinName csel){
Yajirushi 0:e1964b6e160c 54 hasSerial = false;
Yajirushi 0:e1964b6e160c 55 this->spi = spi;
Yajirushi 0:e1964b6e160c 56 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 57 }
Yajirushi 0:e1964b6e160c 58 L6470SDC::L6470SDC(SPI *spi, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 59 hasSerial = false;
Yajirushi 0:e1964b6e160c 60 this->spi = spi;
Yajirushi 0:e1964b6e160c 61 cs = csel;
Yajirushi 0:e1964b6e160c 62 }
Yajirushi 0:e1964b6e160c 63
Yajirushi 0:e1964b6e160c 64 void L6470SDC::init(){
Yajirushi 0:e1964b6e160c 65 //SPI初期化
Yajirushi 0:e1964b6e160c 66 spi->frequency(L6470_SPI_FREQ);
Yajirushi 0:e1964b6e160c 67 spi->format(8, 3);
Yajirushi 0:e1964b6e160c 68 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 69
Yajirushi 0:e1964b6e160c 70 wait_ms(5);
Yajirushi 0:e1964b6e160c 71
Yajirushi 0:e1964b6e160c 72 //SPIで値を送り出し、値が出てくるまでの個数を特定する(デイジーチェーンされている個数がわかる)
Yajirushi 0:e1964b6e160c 73 unsigned char rsv = 0x00;
Yajirushi 0:e1964b6e160c 74 motor_count = 0;
Yajirushi 0:e1964b6e160c 75 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 76 spi->write(0xEE);
Yajirushi 0:e1964b6e160c 77 do{
Yajirushi 0:e1964b6e160c 78 rsv = spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 79 motor_count++;
Yajirushi 0:e1964b6e160c 80 }while(rsv != 0xEE);
Yajirushi 0:e1964b6e160c 81 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 82
Yajirushi 0:e1964b6e160c 83 //全てのモーターに空コマンドを送り、リセットをかける
Yajirushi 0:e1964b6e160c 84 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 85 for(int i = 0; i < (motor_count << 2); i++) spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 86 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 87 for(int i = 1; i <= motor_count; i++) motorReset(i);
Yajirushi 0:e1964b6e160c 88
Yajirushi 0:e1964b6e160c 89 //全てのモーターに初期設定
Yajirushi 0:e1964b6e160c 90 //最大速度 1秒間に4回転
Yajirushi 0:e1964b6e160c 91 //加速、減速 0x02E0(ほぼ一瞬で加減速)
Yajirushi 0:e1964b6e160c 92 //フルステップ移行速度 0xff(常にマイクロステップ駆動)
Yajirushi 0:e1964b6e160c 93 for(int i = 1; i <= motor_count; i++){
Yajirushi 0:e1964b6e160c 94 setMaximumSpeed(i, calcMaxSpd(800.0));
Yajirushi 0:e1964b6e160c 95 setAcceleration(i, 0x02E0);
Yajirushi 0:e1964b6e160c 96 setDeceleration(i, 0x02E0);
Yajirushi 0:e1964b6e160c 97 setFSSpeed(i, 0xff);
Yajirushi 0:e1964b6e160c 98 }
Yajirushi 0:e1964b6e160c 99 }
Yajirushi 0:e1964b6e160c 100
Yajirushi 0:e1964b6e160c 101 int L6470SDC::getMotorCount(){
Yajirushi 0:e1964b6e160c 102 return motor_count;
Yajirushi 0:e1964b6e160c 103 }
Yajirushi 0:e1964b6e160c 104
Yajirushi 0:e1964b6e160c 105 void L6470SDC::setCmd(int motorNumber, unsigned char cmdAddress, unsigned long value, int bitLen){
Yajirushi 0:e1964b6e160c 106 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 107 if(hasSerial) pc->printf("[Command] Motor-No.%03d, CMD:0x%04X, VAL:0x%06X\r\n", motorNumber, cmdAddress, value);
Yajirushi 0:e1964b6e160c 108 #endif
Yajirushi 0:e1964b6e160c 109 //コマンド送信：対象のモーター以外にはNOPを送信する
Yajirushi 0:e1964b6e160c 110 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 111 for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? cmdAddress : CMD_NOP);
Yajirushi 0:e1964b6e160c 112 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 113
Yajirushi 0:e1964b6e160c 114 //コマンドの後続の値がなければ終了
Yajirushi 0:e1964b6e160c 115 if(bitLen < 1) return;
Yajirushi 0:e1964b6e160c 116
Yajirushi 0:e1964b6e160c 117 //値を8bitに分けて送信していく：対象のモーター以外には同じ数だけNOPを送信する
Yajirushi 0:e1964b6e160c 118 //bitLenを8で割り、送信回数を求める
Yajirushi 0:e1964b6e160c 119 //送信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
Yajirushi 0:e1964b6e160c 120 //対象のモーター以外には同じ数だけNOPを送信する
Yajirushi 0:e1964b6e160c 121 int send8 = (bitLen - 1) >> 3; //int send8 = (bitLen-1) / 8;
Yajirushi 0:e1964b6e160c 122 int top8bit = bitLen - (send8 << 3); //int top8bit = bitLen - (send8 * 8);
Yajirushi 0:e1964b6e160c 123 ++send8;
Yajirushi 0:e1964b6e160c 124 for(int j = 1; j <= send8; j++){
Yajirushi 0:e1964b6e160c 125 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 126 for(int i = motor_count; i >= 1; i--){
Yajirushi 0:e1964b6e160c 127 if(i == motorNumber){
Yajirushi 0:e1964b6e160c 128 bitLen -= (j == 1) ? top8bit : 8;
Yajirushi 0:e1964b6e160c 129 spi->write((unsigned char)(value >> bitLen));
Yajirushi 0:e1964b6e160c 130 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 131 if(hasSerial) pc->printf(">>>> value transfer[%d] = 0x%02X\r\n", j, (unsigned char)(value >> bitLen));
Yajirushi 0:e1964b6e160c 132 #endif
Yajirushi 0:e1964b6e160c 133 }else{
Yajirushi 0:e1964b6e160c 134 spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 135 }
Yajirushi 0:e1964b6e160c 136 }
Yajirushi 0:e1964b6e160c 137 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 138 }
Yajirushi 0:e1964b6e160c 139 }
Yajirushi 0:e1964b6e160c 140
Yajirushi 0:e1964b6e160c 141 void L6470SDC::setParam(int motorNumber, unsigned char regAddress, unsigned long value, int bitLen){
Yajirushi 0:e1964b6e160c 142 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 143 if(hasSerial) pc->printf("[SetParam] Motor-No.%03d, SetParam: REG:0x%04X, VAL:0x%06X\r\n", motorNumber, regAddress, value);
Yajirushi 0:e1964b6e160c 144 #endif
Yajirushi 0:e1964b6e160c 145 //SETPARAM送信：対象のモーター以外にはNOPを送信する
Yajirushi 0:e1964b6e160c 146 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 147 for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? (CMD_SETPARAM | regAddress) : CMD_NOP);
Yajirushi 0:e1964b6e160c 148 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 149
Yajirushi 0:e1964b6e160c 150 //SETPARAMに続く引数を8bitに分けて送信していく：対象のモーター以外には同じ数だけNOPを送信する
Yajirushi 0:e1964b6e160c 151 //bitLenを8で割り、送信回数を求める
Yajirushi 0:e1964b6e160c 152 //送信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
Yajirushi 0:e1964b6e160c 153 //対象のモーター以外には同じ数だけNOPを送信する
Yajirushi 0:e1964b6e160c 154 int send8 = (bitLen - 1) >> 3; //int send8 = (bitLen-1) / 8;
Yajirushi 0:e1964b6e160c 155 int top8bit = bitLen - (send8 << 3); //int top8bit = bitLen - (send8 * 8);
Yajirushi 0:e1964b6e160c 156 ++send8;
Yajirushi 0:e1964b6e160c 157 for(int j = 1; j <= send8; j++){
Yajirushi 0:e1964b6e160c 158 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 159 for(int i = motor_count; i >= 1; i--){
Yajirushi 0:e1964b6e160c 160 if(i == motorNumber){
Yajirushi 0:e1964b6e160c 161 bitLen -= (j == 1) ? top8bit : 8;
Yajirushi 0:e1964b6e160c 162 spi->write((unsigned char)(value >> bitLen));
Yajirushi 0:e1964b6e160c 163 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 164 if(hasSerial) pc->printf(">>>> value transfer[%d] = 0x%02X\r\n", j, (unsigned char)(value >> bitLen));
Yajirushi 0:e1964b6e160c 165 #endif
Yajirushi 0:e1964b6e160c 166 }else{
Yajirushi 0:e1964b6e160c 167 spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 168 }
Yajirushi 0:e1964b6e160c 169 }
Yajirushi 0:e1964b6e160c 170 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 171 }
Yajirushi 0:e1964b6e160c 172 }
Yajirushi 0:e1964b6e160c 173
Yajirushi 0:e1964b6e160c 174 unsigned long L6470SDC::getParam(int motorNumber, unsigned char regAddress, int bitLen){
Yajirushi 0:e1964b6e160c 175 //戻り値
Yajirushi 0:e1964b6e160c 176 unsigned long ret = 0;
Yajirushi 0:e1964b6e160c 177 unsigned char val = 0;
Yajirushi 0:e1964b6e160c 178
Yajirushi 0:e1964b6e160c 179 //GETPARAM送信：対象のモーター以外にはNOPを送信する
Yajirushi 0:e1964b6e160c 180 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 181 for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? (CMD_GETPARAM | regAddress) : CMD_NOP);
Yajirushi 0:e1964b6e160c 182 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 183
Yajirushi 0:e1964b6e160c 184 //データを8bitに分けて受信してretに格納していく
Yajirushi 0:e1964b6e160c 185 //bitLenを8で割り、受信回数を求める
Yajirushi 0:e1964b6e160c 186 //受信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
Yajirushi 0:e1964b6e160c 187 //対象のモーター以外の受信は無視する
Yajirushi 0:e1964b6e160c 188 int receive8 = (bitLen - 1) >> 3; //int receive8 = (bitLen-1) / 8;
Yajirushi 0:e1964b6e160c 189 ++receive8;
Yajirushi 0:e1964b6e160c 190 for(int j = 1; j <= receive8; j++){
Yajirushi 0:e1964b6e160c 191 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 192 for(int i = motor_count; i >= 1; i--){
Yajirushi 0:e1964b6e160c 193 if(i == motorNumber){
Yajirushi 0:e1964b6e160c 194 ret |= (j == receive8) ? spi->write(CMD_NOP) : spi->write(CMD_NOP) << ((receive8 - j) << 3);
Yajirushi 0:e1964b6e160c 195 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 196 if(hasSerial) pc->printf("<<<< receive data[%d] = 0x%06X\r\n", j, ret);
Yajirushi 0:e1964b6e160c 197 #endif
Yajirushi 0:e1964b6e160c 198 }else{
Yajirushi 0:e1964b6e160c 199 val = spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 200 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 201 if(hasSerial) pc->printf("???? receive other data[%d] = 0x%02X\r\n", j, val);
Yajirushi 0:e1964b6e160c 202 #endif
Yajirushi 0:e1964b6e160c 203 }
Yajirushi 0:e1964b6e160c 204 }
Yajirushi 0:e1964b6e160c 205 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 206 }
Yajirushi 0:e1964b6e160c 207 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 208 if(hasSerial) pc->printf("[GetParam] Motor-No.%03d, GetParam: REG:0x%04X, VAL:0x%06X\r\n", motorNumber, regAddress, ret);
Yajirushi 0:e1964b6e160c 209 #endif
Yajirushi 0:e1964b6e160c 210 return ret;
Yajirushi 0:e1964b6e160c 211 }
Yajirushi 0:e1964b6e160c 212
Yajirushi 0:e1964b6e160c 213 // cmd method ------------------------------------------------------------------------------
Yajirushi 0:e1964b6e160c 214 bool L6470SDC::isBusy(int motorNumber){
Yajirushi 0:e1964b6e160c 215 return !((getStatus(motorNumber) & 0x02) >> 1);
Yajirushi 0:e1964b6e160c 216 }
Yajirushi 0:e1964b6e160c 217
Yajirushi 0:e1964b6e160c 218 void L6470SDC::run(int motorNumber, unsigned long hex_speed, bool isClockwise){
Yajirushi 0:e1964b6e160c 219 setCmd(motorNumber, (isClockwise) ? CMD_RUN_MINUS : CMD_RUN_PLUS, hex_speed, 20);
Yajirushi 0:e1964b6e160c 220 }
Yajirushi 0:e1964b6e160c 221
Yajirushi 0:e1964b6e160c 222 void L6470SDC::step(int motorNumber, unsigned int count, bool isClockwise){
Yajirushi 0:e1964b6e160c 223 for(;count<=0;count--){
Yajirushi 0:e1964b6e160c 224 while(isBusy(motorNumber));
Yajirushi 0:e1964b6e160c 225 setCmd(motorNumber, (isClockwise) ? CMD_STEP_MINUS : CMD_STEP_PLUS, 0, 0);
Yajirushi 0:e1964b6e160c 226 }
Yajirushi 0:e1964b6e160c 227 }
Yajirushi 0:e1964b6e160c 228
Yajirushi 0:e1964b6e160c 229 void L6470SDC::move(int motorNumber, unsigned long stepsCount, bool isClockwise){
Yajirushi 0:e1964b6e160c 230 setCmd(motorNumber, (isClockwise) ? CMD_ADDSTEP_MINUS : CMD_ADDSTEP_PLUS, stepsCount, 22);
Yajirushi 0:e1964b6e160c 231 }
Yajirushi 0:e1964b6e160c 232
Yajirushi 0:e1964b6e160c 233 void L6470SDC::goto1(int motorNumber, unsigned long ABSPos){
Yajirushi 0:e1964b6e160c 234 setCmd(motorNumber, CMD_GOTO, ABSPos, 22);
Yajirushi 0:e1964b6e160c 235 }
Yajirushi 0:e1964b6e160c 236
Yajirushi 0:e1964b6e160c 237 void L6470SDC::goto2(int motorNumber, unsigned long ABSPos, bool isClockwise){
Yajirushi 0:e1964b6e160c 238 setCmd(motorNumber, (isClockwise) ? CMD_GOTO_DIR_MINUS : CMD_GOTO_DIR_PLUS, ABSPos, 22);
Yajirushi 0:e1964b6e160c 239 }
Yajirushi 0:e1964b6e160c 240
Yajirushi 0:e1964b6e160c 241 void L6470SDC::goUntil(int motorNumber, unsigned long hex_speed, bool isClockwise, bool setMark){
Yajirushi 0:e1964b6e160c 242 unsigned short cmdAddr = (isClockwise) ? CMD_GO_UNTIL_MINUS : CMD_GO_UNTIL_PLUS;
Yajirushi 0:e1964b6e160c 243 setCmd(motorNumber, (setMark) ? (cmdAddr | 0x08) : cmdAddr, hex_speed, 22);
Yajirushi 0:e1964b6e160c 244 }
Yajirushi 0:e1964b6e160c 245
Yajirushi 0:e1964b6e160c 246 void L6470SDC::releaseSwitch(int motorNumber, bool isClockwise, bool setMark){
Yajirushi 0:e1964b6e160c 247 unsigned short cmdAddr = (isClockwise) ? CMD_RELEASE_SW_MINUS : CMD_RELEASE_SW_PLUS;
Yajirushi 0:e1964b6e160c 248 setCmd(motorNumber, (setMark) ? (cmdAddr | 0x08) : cmdAddr, 0, 0);
Yajirushi 0:e1964b6e160c 249 }
Yajirushi 0:e1964b6e160c 250
Yajirushi 0:e1964b6e160c 251 void L6470SDC::home(int motorNumber){
Yajirushi 0:e1964b6e160c 252 setCmd(motorNumber, CMD_GO_HOME, 0, 0);
Yajirushi 0:e1964b6e160c 253 }
Yajirushi 0:e1964b6e160c 254
Yajirushi 0:e1964b6e160c 255 //zeroはhomeのエイリアス(別名)
Yajirushi 0:e1964b6e160c 256 void L6470SDC::zero(int motorNumber){
Yajirushi 0:e1964b6e160c 257 setCmd(motorNumber, CMD_GO_HOME, 0, 0);
Yajirushi 0:e1964b6e160c 258 }
Yajirushi 0:e1964b6e160c 259
Yajirushi 0:e1964b6e160c 260 void L6470SDC::gotoMark(int motorNumber){
Yajirushi 0:e1964b6e160c 261 setCmd(motorNumber, CMD_GO_MARK, 0, 0);
Yajirushi 0:e1964b6e160c 262 }
Yajirushi 0:e1964b6e160c 263
Yajirushi 0:e1964b6e160c 264 void L6470SDC::resetHome(int motorNumber){
Yajirushi 0:e1964b6e160c 265 setCmd(motorNumber, CMD_RESET_POS, 0, 0);
Yajirushi 0:e1964b6e160c 266 }
Yajirushi 0:e1964b6e160c 267
Yajirushi 0:e1964b6e160c 268 //resetZeroはresetHomeのエイリアス(別名)
Yajirushi 0:e1964b6e160c 269 void L6470SDC::resetZero(int motorNumber){
Yajirushi 0:e1964b6e160c 270 setCmd(motorNumber, CMD_RESET_POS, 0, 0);
Yajirushi 0:e1964b6e160c 271 }
Yajirushi 0:e1964b6e160c 272
Yajirushi 0:e1964b6e160c 273 void L6470SDC::motorReset(int motorNumber){
Yajirushi 0:e1964b6e160c 274 setCmd(motorNumber, CMD_RESET_DEVICE, 0, 0);
Yajirushi 0:e1964b6e160c 275 }
Yajirushi 0:e1964b6e160c 276
Yajirushi 0:e1964b6e160c 277 void L6470SDC::stop(int motorNumber){
Yajirushi 0:e1964b6e160c 278 setCmd(motorNumber, CMD_SOFT_STOP, 0, 0);
Yajirushi 0:e1964b6e160c 279 }
Yajirushi 0:e1964b6e160c 280
Yajirushi 0:e1964b6e160c 281 void L6470SDC::stopImmidiate(int motorNumber){
Yajirushi 0:e1964b6e160c 282 setCmd(motorNumber, CMD_HARD_STOP, 0, 0);
Yajirushi 0:e1964b6e160c 283 }
Yajirushi 0:e1964b6e160c 284
Yajirushi 0:e1964b6e160c 285 void L6470SDC::stop_HighImpedance(int motorNumber){
Yajirushi 0:e1964b6e160c 286 setCmd(motorNumber, CMD_SOFT_HIZ, 0, 0);
Yajirushi 0:e1964b6e160c 287 }
Yajirushi 0:e1964b6e160c 288
Yajirushi 0:e1964b6e160c 289 void L6470SDC::stopImmidiate_HighImpedance(int motorNumber){
Yajirushi 0:e1964b6e160c 290 setCmd(motorNumber, CMD_HARD_HIZ, 0, 0);
Yajirushi 0:e1964b6e160c 291 }
Yajirushi 0:e1964b6e160c 292
Yajirushi 0:e1964b6e160c 293 // calc method ------------------------------------------------------------------------------
Yajirushi 0:e1964b6e160c 294 unsigned long L6470SDC::calcSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 295 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 296 //SPEED = ステップ毎秒 * 250ナノ秒 / 2^-28
Yajirushi 0:e1964b6e160c 297 //250ナノ秒 / 2^-28 = 2.5e-7 / 2^-28 = 67.108864
Yajirushi 0:e1964b6e160c 298 unsigned long ret = (unsigned long)(stepPerSecond * 67.108864);
Yajirushi 0:e1964b6e160c 299 return (ret > 1048575) ? 1048575 : ret;
Yajirushi 0:e1964b6e160c 300 }
Yajirushi 0:e1964b6e160c 301
Yajirushi 0:e1964b6e160c 302 unsigned short L6470SDC::calcAcc(float stepPerSecond_2){
Yajirushi 0:e1964b6e160c 303 stepPerSecond_2 = (stepPerSecond_2 < 0) ? 0 : stepPerSecond_2;
Yajirushi 0:e1964b6e160c 304 //ACC = ステップ毎秒毎秒 * 250ナノ秒^2 / 2^-40
Yajirushi 0:e1964b6e160c 305 //250ナノ秒^2 / 2^-40 = (2.5e-7)^2 / 2^-40 = 0.0687194
Yajirushi 0:e1964b6e160c 306 unsigned short ret = (unsigned short)(stepPerSecond_2 * 0.0687194);
Yajirushi 0:e1964b6e160c 307 return (ret > 4095) ? 4095 : ret;
Yajirushi 0:e1964b6e160c 308 }
Yajirushi 0:e1964b6e160c 309
Yajirushi 0:e1964b6e160c 310 unsigned short L6470SDC::calcDec(float stepPerSecond_2){
Yajirushi 0:e1964b6e160c 311 stepPerSecond_2 = (stepPerSecond_2 < 0) ? 0 : stepPerSecond_2;
Yajirushi 0:e1964b6e160c 312 //DEC = ステップ毎秒毎秒 * 250ナノ秒^2 / 2^-40
Yajirushi 0:e1964b6e160c 313 //250ナノ秒^2 / 2^-40 = (2.5e-7)^2 / 2^-40 = 0.0687194
Yajirushi 0:e1964b6e160c 314 unsigned short ret = (unsigned short)(stepPerSecond_2 * 0.0687194);
Yajirushi 0:e1964b6e160c 315 return (ret > 4095) ? 4095 : ret;
Yajirushi 0:e1964b6e160c 316 }
Yajirushi 0:e1964b6e160c 317
Yajirushi 0:e1964b6e160c 318 unsigned short L6470SDC::calcMaxSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 319 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 320 //MAXSPEED = ステップ毎秒 * 250ナノ秒 / 2^-18
Yajirushi 0:e1964b6e160c 321 //250ナノ秒 / 2^-18 = 2.5e-7 / 2^-18 = 0.065536
Yajirushi 0:e1964b6e160c 322 unsigned long ret = (unsigned long)(stepPerSecond * 0.065536);
Yajirushi 0:e1964b6e160c 323 return (ret > 1023) ? 1023 : ret;
Yajirushi 0:e1964b6e160c 324 }
Yajirushi 0:e1964b6e160c 325
Yajirushi 0:e1964b6e160c 326 unsigned short L6470SDC::calcMinSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 327 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 328 //MINSPEED = ステップ毎秒 * 250ナノ秒 / 2^-24
Yajirushi 0:e1964b6e160c 329 //250ナノ秒 / 2^-24 = 2.5e-7 / 2^-24 = 4.194304
Yajirushi 0:e1964b6e160c 330 unsigned long ret = (unsigned long)(stepPerSecond * 4.194304);
Yajirushi 0:e1964b6e160c 331 return (ret > 4094) ? 4094 : ret;
Yajirushi 0:e1964b6e160c 332 }
Yajirushi 0:e1964b6e160c 333
Yajirushi 0:e1964b6e160c 334 unsigned short L6470SDC::calcIntSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 335 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 336 //INT_SPEED = ステップ毎秒 * 250ナノ秒 / 2^-24
Yajirushi 0:e1964b6e160c 337 //250ナノ秒 / 2^-24 = 2.5e-7 / 2^-24 = 4.194304
Yajirushi 0:e1964b6e160c 338 unsigned long ret = (unsigned long)(stepPerSecond * 4.194304);
Yajirushi 0:e1964b6e160c 339 return (ret > 16382) ? 16382 : ret;
Yajirushi 0:e1964b6e160c 340 }
Yajirushi 0:e1964b6e160c 341
Yajirushi 0:e1964b6e160c 342 unsigned short L6470SDC::calcFullStepSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 343 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 344 //FS_SPEED = (ステップ毎秒 * 250ナノ秒 / 2^-18) - 0.5
Yajirushi 0:e1964b6e160c 345 //250ナノ秒 / 2^-18 = 2.5e-7 / 2^-18 = 0.065536
Yajirushi 0:e1964b6e160c 346 unsigned long ret = (unsigned long)(stepPerSecond * 0.065536 - 0.5);
Yajirushi 0:e1964b6e160c 347 return (ret > 1023) ? 1023 : ret;
Yajirushi 0:e1964b6e160c 348 }
Yajirushi 0:e1964b6e160c 349
Yajirushi 0:e1964b6e160c 350 // SET method ------------------------------------------------------------------------------
Yajirushi 0:e1964b6e160c 351 void L6470SDC::setAbsPosition(int motorNumber, unsigned long value){
Yajirushi 0:e1964b6e160c 352 setParam(motorNumber, REG_ABS_POS, value, 22);
Yajirushi 0:e1964b6e160c 353 }
Yajirushi 0:e1964b6e160c 354
Yajirushi 0:e1964b6e160c 355 void L6470SDC::setElecPosition(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 356 setParam(motorNumber, REG_EL_POS, value, 9);
Yajirushi 0:e1964b6e160c 357 }
Yajirushi 0:e1964b6e160c 358
Yajirushi 0:e1964b6e160c 359 void L6470SDC::setMarkPosition(int motorNumber, unsigned long value){
Yajirushi 0:e1964b6e160c 360 setParam(motorNumber, REG_MARK, value, 22);
Yajirushi 0:e1964b6e160c 361 }
Yajirushi 0:e1964b6e160c 362
Yajirushi 0:e1964b6e160c 363 void L6470SDC::setAcceleration(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 364 setParam(motorNumber, REG_ACC, value, 12);
Yajirushi 0:e1964b6e160c 365 }
Yajirushi 0:e1964b6e160c 366
Yajirushi 0:e1964b6e160c 367 void L6470SDC::setDeceleration(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 368 setParam(motorNumber, REG_DEC, value, 12);
Yajirushi 0:e1964b6e160c 369 }
Yajirushi 0:e1964b6e160c 370
Yajirushi 0:e1964b6e160c 371 void L6470SDC::setMaximumSpeed(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 372 setParam(motorNumber, REG_MAX_SPEED, value, 10);
Yajirushi 0:e1964b6e160c 373 }
Yajirushi 0:e1964b6e160c 374
Yajirushi 0:e1964b6e160c 375 void L6470SDC::setMinimumSpeed(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 376 setParam(motorNumber, REG_MIN_SPEED, value, 13);
Yajirushi 0:e1964b6e160c 377 }
Yajirushi 0:e1964b6e160c 378
Yajirushi 0:e1964b6e160c 379 void L6470SDC::setHoldingKVAL(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 380 setParam(motorNumber, REG_KVAL_HOLD, value, 8);
Yajirushi 0:e1964b6e160c 381 }
Yajirushi 0:e1964b6e160c 382
Yajirushi 0:e1964b6e160c 383 void L6470SDC::setRunningKVAL(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 384 setParam(motorNumber, REG_KVAL_RUN, value, 8);
Yajirushi 0:e1964b6e160c 385 }
Yajirushi 0:e1964b6e160c 386
Yajirushi 0:e1964b6e160c 387 void L6470SDC::setAccelerationKVAL(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 388 setParam(motorNumber, REG_KVAL_ACC, value, 8);
Yajirushi 0:e1964b6e160c 389 }
Yajirushi 0:e1964b6e160c 390
Yajirushi 0:e1964b6e160c 391 void L6470SDC::setDecelerationKVAL(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 392 setParam(motorNumber, REG_KVAL_DEC, value, 8);
Yajirushi 0:e1964b6e160c 393 }
Yajirushi 0:e1964b6e160c 394
Yajirushi 0:e1964b6e160c 395 void L6470SDC::setKVAL(int motorNumber, unsigned char holdVal, unsigned char runVal, unsigned char accVal, unsigned char decVal){
Yajirushi 0:e1964b6e160c 396 setParam(motorNumber, REG_KVAL_HOLD, holdVal, 8);
Yajirushi 0:e1964b6e160c 397 setParam(motorNumber, REG_KVAL_RUN, runVal, 8);
Yajirushi 0:e1964b6e160c 398 setParam(motorNumber, REG_KVAL_ACC, accVal, 8);
Yajirushi 0:e1964b6e160c 399 setParam(motorNumber, REG_KVAL_DEC, decVal, 8);
Yajirushi 0:e1964b6e160c 400 }
Yajirushi 0:e1964b6e160c 401
Yajirushi 0:e1964b6e160c 402 void L6470SDC::setInterpolateSpeed(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 403 setParam(motorNumber, REG_INT_SPD, value, 14);
Yajirushi 0:e1964b6e160c 404 }
Yajirushi 0:e1964b6e160c 405
Yajirushi 0:e1964b6e160c 406 void L6470SDC::setInterpolateSlope(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 407 setParam(motorNumber, REG_ST_SLP, value, 8);
Yajirushi 0:e1964b6e160c 408 }
Yajirushi 0:e1964b6e160c 409
Yajirushi 0:e1964b6e160c 410 void L6470SDC::setAccSlopeFinal(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 411 setParam(motorNumber, REG_FN_SLP_ACC, value, 8);
Yajirushi 0:e1964b6e160c 412 }
Yajirushi 0:e1964b6e160c 413
Yajirushi 0:e1964b6e160c 414 void L6470SDC::setDecSlopeFinal(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 415 setParam(motorNumber, REG_FN_SLP_DEC, value, 8);
Yajirushi 0:e1964b6e160c 416 }
Yajirushi 0:e1964b6e160c 417
Yajirushi 0:e1964b6e160c 418 void L6470SDC::setThermoCorrect(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 419 setParam(motorNumber, REG_K_THERM, value, 4);
Yajirushi 0:e1964b6e160c 420 }
Yajirushi 0:e1964b6e160c 421
Yajirushi 0:e1964b6e160c 422 void L6470SDC::setOCThreshold(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 423 setParam(motorNumber, REG_OCD_TH, value, 4);
Yajirushi 0:e1964b6e160c 424 }
Yajirushi 0:e1964b6e160c 425
Yajirushi 0:e1964b6e160c 426 void L6470SDC::setStallThreshold(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 427 setParam(motorNumber, REG_STALL_TH, value, 7);
Yajirushi 0:e1964b6e160c 428 }
Yajirushi 0:e1964b6e160c 429
Yajirushi 0:e1964b6e160c 430 void L6470SDC::setFSSpeed(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 431 setParam(motorNumber, REG_FS_SPD, value, 10);
Yajirushi 0:e1964b6e160c 432 }
Yajirushi 0:e1964b6e160c 433
Yajirushi 0:e1964b6e160c 434 void L6470SDC::setStepMode(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 435 setParam(motorNumber, REG_STEP_MODE, value, 8);
Yajirushi 0:e1964b6e160c 436 }
Yajirushi 0:e1964b6e160c 437
Yajirushi 0:e1964b6e160c 438 void L6470SDC::setAlermEnable(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 439 setParam(motorNumber, REG_ALARM_EN, value, 8);
Yajirushi 0:e1964b6e160c 440 }
Yajirushi 0:e1964b6e160c 441
Yajirushi 0:e1964b6e160c 442 void L6470SDC::setSystemConfig(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 443 setParam(motorNumber, REG_CONFIG, value, 16);
Yajirushi 0:e1964b6e160c 444 }
Yajirushi 0:e1964b6e160c 445
Yajirushi 0:e1964b6e160c 446 // GET method ------------------------------------------------------------------------------
Yajirushi 0:e1964b6e160c 447 unsigned long L6470SDC::getSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 448 return getParam(motorNumber, REG_SPEED, 20);
Yajirushi 0:e1964b6e160c 449 }
Yajirushi 0:e1964b6e160c 450
Yajirushi 0:e1964b6e160c 451 unsigned short L6470SDC::getADC(int motorNumber){
Yajirushi 0:e1964b6e160c 452 return (unsigned short) getParam(motorNumber, REG_ADC_OUT, 5);
Yajirushi 0:e1964b6e160c 453 }
Yajirushi 0:e1964b6e160c 454
Yajirushi 0:e1964b6e160c 455 unsigned short L6470SDC::getStatus(int motorNumber){
Yajirushi 0:e1964b6e160c 456 return (unsigned short) getParam(motorNumber, REG_STATUS, 16);
Yajirushi 0:e1964b6e160c 457 }
Yajirushi 0:e1964b6e160c 458
Yajirushi 0:e1964b6e160c 459 unsigned long L6470SDC::getAbsPosition(int motorNumber){
Yajirushi 0:e1964b6e160c 460 return getParam(motorNumber, REG_ABS_POS, 22);
Yajirushi 0:e1964b6e160c 461 }
Yajirushi 0:e1964b6e160c 462
Yajirushi 0:e1964b6e160c 463 unsigned short L6470SDC::getElecPosition(int motorNumber){
Yajirushi 0:e1964b6e160c 464 return (unsigned short) getParam(motorNumber, REG_EL_POS, 9);
Yajirushi 0:e1964b6e160c 465 }
Yajirushi 0:e1964b6e160c 466
Yajirushi 0:e1964b6e160c 467 unsigned long L6470SDC::getMarkPosition(int motorNumber){
Yajirushi 0:e1964b6e160c 468 return getParam(motorNumber, REG_MARK, 22);
Yajirushi 0:e1964b6e160c 469 }
Yajirushi 0:e1964b6e160c 470
Yajirushi 0:e1964b6e160c 471 unsigned short L6470SDC::getAcceleration(int motorNumber){
Yajirushi 0:e1964b6e160c 472 return (unsigned short) getParam(motorNumber, REG_ACC, 12);
Yajirushi 0:e1964b6e160c 473 }
Yajirushi 0:e1964b6e160c 474
Yajirushi 0:e1964b6e160c 475 unsigned short L6470SDC::getDeceleration(int motorNumber){
Yajirushi 0:e1964b6e160c 476 return (unsigned short) getParam(motorNumber, REG_DEC, 12);
Yajirushi 0:e1964b6e160c 477 }
Yajirushi 0:e1964b6e160c 478
Yajirushi 0:e1964b6e160c 479 unsigned short L6470SDC::getMaximumSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 480 return (unsigned short) getParam(motorNumber, REG_MAX_SPEED, 10);
Yajirushi 0:e1964b6e160c 481 }
Yajirushi 0:e1964b6e160c 482
Yajirushi 0:e1964b6e160c 483 unsigned short L6470SDC::getMinimumSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 484 return (unsigned short) getParam(motorNumber, REG_MIN_SPEED, 13);
Yajirushi 0:e1964b6e160c 485 }
Yajirushi 0:e1964b6e160c 486
Yajirushi 0:e1964b6e160c 487 unsigned char L6470SDC::getHoldingKVAL(int motorNumber){
Yajirushi 0:e1964b6e160c 488 return (unsigned char) getParam(motorNumber, REG_KVAL_HOLD, 8);
Yajirushi 0:e1964b6e160c 489 }
Yajirushi 0:e1964b6e160c 490
Yajirushi 0:e1964b6e160c 491 unsigned char L6470SDC::getRunningKVAL(int motorNumber){
Yajirushi 0:e1964b6e160c 492 return (unsigned char) getParam(motorNumber, REG_KVAL_RUN, 8);
Yajirushi 0:e1964b6e160c 493 }
Yajirushi 0:e1964b6e160c 494
Yajirushi 0:e1964b6e160c 495 unsigned char L6470SDC::getAccelerationKVAL(int motorNumber){
Yajirushi 0:e1964b6e160c 496 return (unsigned char) getParam(motorNumber, REG_KVAL_ACC, 8);
Yajirushi 0:e1964b6e160c 497 }
Yajirushi 0:e1964b6e160c 498
Yajirushi 0:e1964b6e160c 499 unsigned char L6470SDC::getDecelerationKVAL(int motorNumber){
Yajirushi 0:e1964b6e160c 500 return (unsigned char) getParam(motorNumber, REG_KVAL_DEC, 8);
Yajirushi 0:e1964b6e160c 501 }
Yajirushi 0:e1964b6e160c 502
Yajirushi 0:e1964b6e160c 503 unsigned short L6470SDC::getInterpolateSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 504 return (unsigned short) getParam(motorNumber, REG_INT_SPD, 14);
Yajirushi 0:e1964b6e160c 505 }
Yajirushi 0:e1964b6e160c 506
Yajirushi 0:e1964b6e160c 507 unsigned char L6470SDC::getInterpolateSlope(int motorNumber){
Yajirushi 0:e1964b6e160c 508 return (unsigned char) getParam(motorNumber, REG_ST_SLP, 8);
Yajirushi 0:e1964b6e160c 509 }
Yajirushi 0:e1964b6e160c 510
Yajirushi 0:e1964b6e160c 511 unsigned char L6470SDC::getAccSlopeFinal(int motorNumber){
Yajirushi 0:e1964b6e160c 512 return (unsigned char) getParam(motorNumber, REG_FN_SLP_ACC, 8);
Yajirushi 0:e1964b6e160c 513 }
Yajirushi 0:e1964b6e160c 514
Yajirushi 0:e1964b6e160c 515 unsigned char L6470SDC::getDecSlopeFinal(int motorNumber){
Yajirushi 0:e1964b6e160c 516 return (unsigned char) getParam(motorNumber, REG_FN_SLP_DEC, 8);
Yajirushi 0:e1964b6e160c 517 }
Yajirushi 0:e1964b6e160c 518
Yajirushi 0:e1964b6e160c 519 unsigned char L6470SDC::getThermoCorrect(int motorNumber){
Yajirushi 0:e1964b6e160c 520 return (unsigned char) getParam(motorNumber, REG_K_THERM, 4);
Yajirushi 0:e1964b6e160c 521 }
Yajirushi 0:e1964b6e160c 522
Yajirushi 0:e1964b6e160c 523 unsigned char L6470SDC::getOCThreshold(int motorNumber){
Yajirushi 0:e1964b6e160c 524 return (unsigned char) getParam(motorNumber, REG_OCD_TH, 4);
Yajirushi 0:e1964b6e160c 525 }
Yajirushi 0:e1964b6e160c 526
Yajirushi 0:e1964b6e160c 527 unsigned char L6470SDC::getStallThreshold(int motorNumber){
Yajirushi 0:e1964b6e160c 528 return (unsigned char) getParam(motorNumber, REG_STALL_TH, 7);
Yajirushi 0:e1964b6e160c 529 }
Yajirushi 0:e1964b6e160c 530
Yajirushi 0:e1964b6e160c 531 unsigned short L6470SDC::getFSSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 532 return (unsigned short) getParam(motorNumber, REG_FS_SPD, 10);
Yajirushi 0:e1964b6e160c 533 }
Yajirushi 0:e1964b6e160c 534
Yajirushi 0:e1964b6e160c 535 unsigned char L6470SDC::getStepMode(int motorNumber){
Yajirushi 0:e1964b6e160c 536 return (unsigned char) getParam(motorNumber, REG_STEP_MODE, 8);
Yajirushi 0:e1964b6e160c 537 }
Yajirushi 0:e1964b6e160c 538
Yajirushi 0:e1964b6e160c 539 unsigned char L6470SDC::getAlermEnable(int motorNumber){
Yajirushi 0:e1964b6e160c 540 return (unsigned char) getParam(motorNumber, REG_ALARM_EN, 8);
Yajirushi 0:e1964b6e160c 541 }
Yajirushi 0:e1964b6e160c 542
Yajirushi 0:e1964b6e160c 543 unsigned short L6470SDC::getSystemConfig(int motorNumber){
Yajirushi 0:e1964b6e160c 544 return (unsigned short) getParam(motorNumber, REG_CONFIG, 16);
Yajirushi 0:e1964b6e160c 545 }