Diff: L6470SDC.cpp

Revision:

0:e1964b6e160c

Child:

1:db64ad30b4b3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/L6470SDC.cpp	Sat May 30 14:32:48 2015 +0000
@@ -0,0 +1,545 @@
+#include "L6470SDC.h"
+
+#define CS_ACTIVE       cs->write(0)
+#define CS_INACTIVE     cs->write(1)
+
+//充実のコンストラクタ
+L6470SDC::L6470SDC(PinName tx, PinName rx, PinName mosi, PinName miso, PinName sclk, PinName csel){
+    pc = new Serial(tx, rx);
+    spi = new SPI(mosi, miso, sclk);
+    cs = new DigitalOut(csel);
+    hasSerial = !(pc == NULL);
+}
+L6470SDC::L6470SDC(PinName tx, PinName rx, PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
+    pc = new Serial(tx, rx);
+    spi = new SPI(mosi, miso, sclk);
+    cs = csel;
+    hasSerial = !(pc == NULL);
+}
+L6470SDC::L6470SDC(Serial *serial, PinName mosi, PinName miso, PinName sclk, PinName csel){
+    pc = serial;
+    spi = new SPI(mosi, miso, sclk);
+    cs = new DigitalOut(csel);
+    hasSerial = !(pc == NULL);
+}
+L6470SDC::L6470SDC(Serial *serial, PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
+    pc = serial;
+    spi = new SPI(mosi, miso, sclk);
+    cs = csel;
+    hasSerial = !(pc == NULL);
+}
+L6470SDC::L6470SDC(Serial *serial, SPI *spi, PinName csel){
+    pc = serial;
+    this->spi = spi;
+    cs = new DigitalOut(csel);
+    hasSerial = !(pc == NULL);
+}
+L6470SDC::L6470SDC(Serial *serial, SPI *spi, DigitalOut *csel){
+    pc = serial;
+    this->spi = spi;
+    cs = csel;
+    hasSerial = !(pc == NULL);
+}
+L6470SDC::L6470SDC(PinName mosi, PinName miso, PinName sclk, PinName csel){
+    hasSerial = false;
+    spi = new SPI(mosi, miso, sclk);
+    cs = new DigitalOut(csel);
+}
+L6470SDC::L6470SDC(PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
+    hasSerial = false;
+    spi = new SPI(mosi, miso, sclk);
+    cs = csel;
+}
+L6470SDC::L6470SDC(SPI *spi, PinName csel){
+    hasSerial = false;
+    this->spi = spi;
+    cs = new DigitalOut(csel);
+}
+L6470SDC::L6470SDC(SPI *spi, DigitalOut *csel){
+    hasSerial = false;
+    this->spi = spi;
+    cs = csel;
+}
+
+void L6470SDC::init(){
+    //SPI初期化
+    spi->frequency(L6470_SPI_FREQ);
+    spi->format(8, 3);
+    CS_INACTIVE;
+
+    wait_ms(5);
+
+    //SPIで値を送り出し、値が出てくるまでの個数を特定する(デイジーチェーンされている個数がわかる)
+    unsigned char rsv = 0x00;
+    motor_count = 0;
+    CS_ACTIVE;
+    spi->write(0xEE);
+    do{
+        rsv = spi->write(CMD_NOP);
+        motor_count++;
+    }while(rsv != 0xEE);
+    CS_INACTIVE;
+
+    //全てのモーターに空コマンドを送り、リセットをかける
+    CS_ACTIVE;
+    for(int i = 0; i < (motor_count << 2); i++) spi->write(CMD_NOP);
+    CS_INACTIVE;
+    for(int i = 1; i <= motor_count; i++) motorReset(i);
+
+    //全てのモーターに初期設定
+    //最大速度 1秒間に4回転
+    //加速、減速 0x02E0(ほぼ一瞬で加減速)
+    //フルステップ移行速度 0xff(常にマイクロステップ駆動)
+    for(int i = 1; i <= motor_count; i++){
+        setMaximumSpeed(i, calcMaxSpd(800.0));
+        setAcceleration(i, 0x02E0);
+        setDeceleration(i, 0x02E0);
+        setFSSpeed(i, 0xff);
+    }
+}
+
+int L6470SDC::getMotorCount(){
+    return motor_count;
+}
+
+void L6470SDC::setCmd(int motorNumber, unsigned char cmdAddress, unsigned long value, int bitLen){
+#ifdef DEBUG_L6470SDC
+    if(hasSerial) pc->printf("[Command] Motor-No.%03d, CMD:0x%04X, VAL:0x%06X\r\n", motorNumber, cmdAddress, value);
+#endif
+    //コマンド送信：対象のモーター以外にはNOPを送信する
+    CS_ACTIVE;
+    for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? cmdAddress : CMD_NOP);
+    CS_INACTIVE;
+
+    //コマンドの後続の値がなければ終了
+    if(bitLen < 1) return;
+
+    //値を8bitに分けて送信していく：対象のモーター以外には同じ数だけNOPを送信する
+    //bitLenを8で割り、送信回数を求める
+    //送信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
+    //対象のモーター以外には同じ数だけNOPを送信する
+    int send8 = (bitLen - 1) >> 3; //int send8 = (bitLen-1) / 8;
+    int top8bit = bitLen - (send8 << 3); //int top8bit = bitLen - (send8 * 8);
+    ++send8;
+    for(int j = 1; j <= send8; j++){
+        CS_ACTIVE;
+        for(int i = motor_count; i >= 1; i--){
+            if(i == motorNumber){
+                bitLen -= (j == 1) ? top8bit : 8;
+                spi->write((unsigned char)(value >> bitLen));
+#ifdef DEBUG_L6470SDC
+                if(hasSerial) pc->printf(">>>> value transfer[%d] = 0x%02X\r\n", j, (unsigned char)(value >> bitLen));
+#endif
+            }else{
+                spi->write(CMD_NOP);
+            }
+        }
+        CS_INACTIVE;
+    }
+}
+
+void L6470SDC::setParam(int motorNumber, unsigned char regAddress, unsigned long value, int bitLen){
+#ifdef DEBUG_L6470SDC
+    if(hasSerial) pc->printf("[SetParam] Motor-No.%03d, SetParam: REG:0x%04X, VAL:0x%06X\r\n", motorNumber, regAddress, value);
+#endif
+    //SETPARAM送信：対象のモーター以外にはNOPを送信する
+    CS_ACTIVE;
+    for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? (CMD_SETPARAM | regAddress) : CMD_NOP);
+    CS_INACTIVE;
+
+    //SETPARAMに続く引数を8bitに分けて送信していく：対象のモーター以外には同じ数だけNOPを送信する
+    //bitLenを8で割り、送信回数を求める
+    //送信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
+    //対象のモーター以外には同じ数だけNOPを送信する
+    int send8 = (bitLen - 1) >> 3; //int send8 = (bitLen-1) / 8;
+    int top8bit = bitLen - (send8 << 3); //int top8bit = bitLen - (send8 * 8);
+    ++send8;
+    for(int j = 1; j <= send8; j++){
+        CS_ACTIVE;
+        for(int i = motor_count; i >= 1; i--){
+            if(i == motorNumber){
+                bitLen -= (j == 1) ? top8bit : 8;
+                spi->write((unsigned char)(value >> bitLen));
+#ifdef DEBUG_L6470SDC
+                if(hasSerial) pc->printf(">>>> value transfer[%d] = 0x%02X\r\n", j, (unsigned char)(value >> bitLen));
+#endif
+            }else{
+                spi->write(CMD_NOP);
+            }
+        }
+        CS_INACTIVE;
+    }
+}
+
+unsigned long L6470SDC::getParam(int motorNumber, unsigned char regAddress, int bitLen){
+    //戻り値
+    unsigned long ret = 0;
+    unsigned char val = 0;
+
+    //GETPARAM送信：対象のモーター以外にはNOPを送信する
+    CS_ACTIVE;
+    for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? (CMD_GETPARAM | regAddress) : CMD_NOP);
+    CS_INACTIVE;
+
+    //データを8bitに分けて受信してretに格納していく
+    //bitLenを8で割り、受信回数を求める
+    //受信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
+    //対象のモーター以外の受信は無視する
+    int receive8 = (bitLen - 1) >> 3; //int receive8 = (bitLen-1) / 8;
+    ++receive8;
+    for(int j = 1; j <= receive8; j++){
+        CS_ACTIVE;
+        for(int i = motor_count; i >= 1; i--){
+            if(i == motorNumber){
+                ret |= (j == receive8) ? spi->write(CMD_NOP) : spi->write(CMD_NOP) << ((receive8 - j) << 3);
+#ifdef DEBUG_L6470SDC
+                if(hasSerial) pc->printf("<<<< receive data[%d] = 0x%06X\r\n", j, ret);
+#endif
+            }else{
+                val = spi->write(CMD_NOP);
+#ifdef DEBUG_L6470SDC
+                if(hasSerial) pc->printf("???? receive other data[%d] = 0x%02X\r\n", j, val);
+#endif
+            }
+        }
+        CS_INACTIVE;
+    }
+#ifdef DEBUG_L6470SDC
+    if(hasSerial) pc->printf("[GetParam] Motor-No.%03d, GetParam: REG:0x%04X, VAL:0x%06X\r\n", motorNumber, regAddress, ret);
+#endif
+    return ret;
+}
+
+// cmd method ------------------------------------------------------------------------------
+bool L6470SDC::isBusy(int motorNumber){
+    return !((getStatus(motorNumber) & 0x02) >> 1);
+}
+
+void L6470SDC::run(int motorNumber, unsigned long hex_speed, bool isClockwise){
+    setCmd(motorNumber, (isClockwise) ? CMD_RUN_MINUS : CMD_RUN_PLUS, hex_speed, 20);
+}
+
+void L6470SDC::step(int motorNumber, unsigned int count, bool isClockwise){
+    for(;count<=0;count--){
+        while(isBusy(motorNumber));
+        setCmd(motorNumber, (isClockwise) ? CMD_STEP_MINUS : CMD_STEP_PLUS, 0, 0);
+    }
+}
+
+void L6470SDC::move(int motorNumber, unsigned long stepsCount, bool isClockwise){
+    setCmd(motorNumber, (isClockwise) ? CMD_ADDSTEP_MINUS : CMD_ADDSTEP_PLUS, stepsCount, 22);
+}
+
+void L6470SDC::goto1(int motorNumber, unsigned long ABSPos){
+    setCmd(motorNumber, CMD_GOTO, ABSPos, 22);
+}
+
+void L6470SDC::goto2(int motorNumber, unsigned long ABSPos, bool isClockwise){
+    setCmd(motorNumber, (isClockwise) ? CMD_GOTO_DIR_MINUS : CMD_GOTO_DIR_PLUS, ABSPos, 22);
+}
+
+void L6470SDC::goUntil(int motorNumber, unsigned long hex_speed, bool isClockwise, bool setMark){
+    unsigned short cmdAddr = (isClockwise) ? CMD_GO_UNTIL_MINUS : CMD_GO_UNTIL_PLUS;
+    setCmd(motorNumber, (setMark) ? (cmdAddr | 0x08) : cmdAddr, hex_speed, 22);
+}
+
+void L6470SDC::releaseSwitch(int motorNumber, bool isClockwise, bool setMark){
+    unsigned short cmdAddr = (isClockwise) ? CMD_RELEASE_SW_MINUS : CMD_RELEASE_SW_PLUS;
+    setCmd(motorNumber, (setMark) ? (cmdAddr | 0x08) : cmdAddr, 0, 0);
+}
+
+void L6470SDC::home(int motorNumber){
+    setCmd(motorNumber, CMD_GO_HOME, 0, 0);
+}
+
+//zeroはhomeのエイリアス(別名)
+void L6470SDC::zero(int motorNumber){
+    setCmd(motorNumber, CMD_GO_HOME, 0, 0);
+}
+
+void L6470SDC::gotoMark(int motorNumber){
+    setCmd(motorNumber, CMD_GO_MARK, 0, 0);
+}
+
+void L6470SDC::resetHome(int motorNumber){
+    setCmd(motorNumber, CMD_RESET_POS, 0, 0);
+}
+
+//resetZeroはresetHomeのエイリアス(別名)
+void L6470SDC::resetZero(int motorNumber){
+    setCmd(motorNumber, CMD_RESET_POS, 0, 0);
+}
+
+void L6470SDC::motorReset(int motorNumber){
+    setCmd(motorNumber, CMD_RESET_DEVICE, 0, 0);
+}
+
+void L6470SDC::stop(int motorNumber){
+    setCmd(motorNumber, CMD_SOFT_STOP, 0, 0);
+}
+
+void L6470SDC::stopImmidiate(int motorNumber){
+    setCmd(motorNumber, CMD_HARD_STOP, 0, 0);
+}
+
+void L6470SDC::stop_HighImpedance(int motorNumber){
+    setCmd(motorNumber, CMD_SOFT_HIZ, 0, 0);
+}
+
+void L6470SDC::stopImmidiate_HighImpedance(int motorNumber){
+    setCmd(motorNumber, CMD_HARD_HIZ, 0, 0);
+}
+
+// calc method ------------------------------------------------------------------------------
+unsigned long L6470SDC::calcSpd(float stepPerSecond){
+    stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
+    //SPEED = ステップ毎秒 * 250ナノ秒 / 2^-28
+    //250ナノ秒 / 2^-28 = 2.5e-7 / 2^-28 = 67.108864
+    unsigned long ret = (unsigned long)(stepPerSecond * 67.108864);
+    return (ret > 1048575) ? 1048575 : ret;
+}
+
+unsigned short L6470SDC::calcAcc(float stepPerSecond_2){
+    stepPerSecond_2 = (stepPerSecond_2 < 0) ? 0 : stepPerSecond_2;
+    //ACC = ステップ毎秒毎秒 * 250ナノ秒^2 / 2^-40
+    //250ナノ秒^2 / 2^-40 = (2.5e-7)^2 / 2^-40 = 0.0687194
+    unsigned short ret = (unsigned short)(stepPerSecond_2 * 0.0687194);
+    return (ret > 4095) ? 4095 : ret;
+}
+
+unsigned short L6470SDC::calcDec(float stepPerSecond_2){
+    stepPerSecond_2 = (stepPerSecond_2 < 0) ? 0 : stepPerSecond_2;
+    //DEC = ステップ毎秒毎秒 * 250ナノ秒^2 / 2^-40
+    //250ナノ秒^2 / 2^-40 = (2.5e-7)^2 / 2^-40 = 0.0687194
+    unsigned short ret = (unsigned short)(stepPerSecond_2 * 0.0687194);
+    return (ret > 4095) ? 4095 : ret;
+}
+
+unsigned short L6470SDC::calcMaxSpd(float stepPerSecond){
+    stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
+    //MAXSPEED = ステップ毎秒 * 250ナノ秒 / 2^-18
+    //250ナノ秒 / 2^-18 = 2.5e-7 / 2^-18 = 0.065536
+    unsigned long ret = (unsigned long)(stepPerSecond * 0.065536);
+    return (ret > 1023) ? 1023 : ret;
+}
+
+unsigned short L6470SDC::calcMinSpd(float stepPerSecond){
+    stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
+    //MINSPEED = ステップ毎秒 * 250ナノ秒 / 2^-24
+    //250ナノ秒 / 2^-24 = 2.5e-7 / 2^-24 = 4.194304
+    unsigned long ret = (unsigned long)(stepPerSecond * 4.194304);
+    return (ret > 4094) ? 4094 : ret;
+}
+
+unsigned short L6470SDC::calcIntSpd(float stepPerSecond){
+    stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
+    //INT_SPEED = ステップ毎秒 * 250ナノ秒 / 2^-24
+    //250ナノ秒 / 2^-24 = 2.5e-7 / 2^-24 = 4.194304
+    unsigned long ret = (unsigned long)(stepPerSecond * 4.194304);
+    return (ret > 16382) ? 16382 : ret;
+}
+
+unsigned short L6470SDC::calcFullStepSpd(float stepPerSecond){
+    stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
+    //FS_SPEED = (ステップ毎秒 * 250ナノ秒 / 2^-18) - 0.5
+    //250ナノ秒 / 2^-18 = 2.5e-7 / 2^-18 = 0.065536
+    unsigned long ret = (unsigned long)(stepPerSecond * 0.065536 - 0.5);
+    return (ret > 1023) ? 1023 : ret;
+}
+
+// SET method ------------------------------------------------------------------------------
+void L6470SDC::setAbsPosition(int motorNumber, unsigned long value){
+    setParam(motorNumber, REG_ABS_POS, value, 22);
+}
+
+void L6470SDC::setElecPosition(int motorNumber, unsigned short value){
+    setParam(motorNumber, REG_EL_POS, value, 9);
+}
+
+void L6470SDC::setMarkPosition(int motorNumber, unsigned long value){
+    setParam(motorNumber, REG_MARK, value, 22);
+}
+
+void L6470SDC::setAcceleration(int motorNumber, unsigned short value){
+    setParam(motorNumber, REG_ACC, value, 12);
+}
+
+void L6470SDC::setDeceleration(int motorNumber, unsigned short value){
+    setParam(motorNumber, REG_DEC, value, 12);
+}
+
+void L6470SDC::setMaximumSpeed(int motorNumber, unsigned short value){
+    setParam(motorNumber, REG_MAX_SPEED, value, 10);
+}
+
+void L6470SDC::setMinimumSpeed(int motorNumber, unsigned short value){
+    setParam(motorNumber, REG_MIN_SPEED, value, 13);
+}
+
+void L6470SDC::setHoldingKVAL(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_KVAL_HOLD, value, 8);
+}
+
+void L6470SDC::setRunningKVAL(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_KVAL_RUN, value, 8);
+}
+
+void L6470SDC::setAccelerationKVAL(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_KVAL_ACC, value, 8);
+}
+
+void L6470SDC::setDecelerationKVAL(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_KVAL_DEC, value, 8);
+}
+
+void L6470SDC::setKVAL(int motorNumber, unsigned char holdVal, unsigned char runVal, unsigned char accVal, unsigned char decVal){
+    setParam(motorNumber, REG_KVAL_HOLD, holdVal, 8);
+    setParam(motorNumber, REG_KVAL_RUN, runVal, 8);
+    setParam(motorNumber, REG_KVAL_ACC, accVal, 8);
+    setParam(motorNumber, REG_KVAL_DEC, decVal, 8);
+}
+
+void L6470SDC::setInterpolateSpeed(int motorNumber, unsigned short value){
+    setParam(motorNumber, REG_INT_SPD, value, 14);
+}
+
+void L6470SDC::setInterpolateSlope(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_ST_SLP, value, 8);
+}
+
+void L6470SDC::setAccSlopeFinal(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_FN_SLP_ACC, value, 8);
+}
+
+void L6470SDC::setDecSlopeFinal(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_FN_SLP_DEC, value, 8);
+}
+
+void L6470SDC::setThermoCorrect(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_K_THERM, value, 4);
+}
+
+void L6470SDC::setOCThreshold(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_OCD_TH, value, 4);
+}
+
+void L6470SDC::setStallThreshold(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_STALL_TH, value, 7);
+}
+
+void L6470SDC::setFSSpeed(int motorNumber, unsigned short value){
+    setParam(motorNumber, REG_FS_SPD, value, 10);
+}
+
+void L6470SDC::setStepMode(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_STEP_MODE, value, 8);
+}
+
+void L6470SDC::setAlermEnable(int motorNumber, unsigned char value){
+    setParam(motorNumber, REG_ALARM_EN, value, 8);
+}
+
+void L6470SDC::setSystemConfig(int motorNumber, unsigned short value){
+    setParam(motorNumber, REG_CONFIG, value, 16);
+}
+
+// GET method ------------------------------------------------------------------------------
+unsigned long L6470SDC::getSpeed(int motorNumber){
+    return getParam(motorNumber, REG_SPEED, 20);
+}
+
+unsigned short L6470SDC::getADC(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_ADC_OUT, 5);
+}
+
+unsigned short L6470SDC::getStatus(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_STATUS, 16);
+}
+
+unsigned long L6470SDC::getAbsPosition(int motorNumber){
+    return getParam(motorNumber, REG_ABS_POS, 22);
+}
+
+unsigned short L6470SDC::getElecPosition(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_EL_POS, 9);
+}
+
+unsigned long L6470SDC::getMarkPosition(int motorNumber){
+    return getParam(motorNumber, REG_MARK, 22);
+}
+
+unsigned short L6470SDC::getAcceleration(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_ACC, 12);
+}
+
+unsigned short L6470SDC::getDeceleration(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_DEC, 12);
+}
+
+unsigned short L6470SDC::getMaximumSpeed(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_MAX_SPEED, 10);
+}
+
+unsigned short L6470SDC::getMinimumSpeed(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_MIN_SPEED, 13);
+}
+
+unsigned char L6470SDC::getHoldingKVAL(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_KVAL_HOLD, 8);
+}
+
+unsigned char L6470SDC::getRunningKVAL(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_KVAL_RUN, 8);
+}
+
+unsigned char L6470SDC::getAccelerationKVAL(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_KVAL_ACC, 8);
+}
+
+unsigned char L6470SDC::getDecelerationKVAL(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_KVAL_DEC, 8);
+}
+
+unsigned short L6470SDC::getInterpolateSpeed(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_INT_SPD, 14);
+}
+
+unsigned char L6470SDC::getInterpolateSlope(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_ST_SLP, 8);
+}
+
+unsigned char L6470SDC::getAccSlopeFinal(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_FN_SLP_ACC, 8);
+}
+
+unsigned char L6470SDC::getDecSlopeFinal(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_FN_SLP_DEC, 8);
+}
+
+unsigned char L6470SDC::getThermoCorrect(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_K_THERM, 4);
+}
+
+unsigned char L6470SDC::getOCThreshold(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_OCD_TH, 4);
+}
+
+unsigned char L6470SDC::getStallThreshold(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_STALL_TH, 7);
+}
+
+unsigned short L6470SDC::getFSSpeed(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_FS_SPD, 10);
+}
+
+unsigned char L6470SDC::getStepMode(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_STEP_MODE, 8);
+}
+
+unsigned char L6470SDC::getAlermEnable(int motorNumber){
+    return (unsigned char) getParam(motorNumber, REG_ALARM_EN, 8);
+}
+
+unsigned short L6470SDC::getSystemConfig(int motorNumber){
+    return (unsigned short) getParam(motorNumber, REG_CONFIG, 16);
+}