Ryota Yamamoto / L6470SDC

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

Dependents:   L6470_daisy_chain l6470

L6470SDC.cpp@4:4127dd195311, 2017-04-30 (annotated)

Committer:
Yajirushi
Date:
Sun Apr 30 03:52:13 2017 +0000
Revision:
4:4127dd195311
Parent:
3:486fb90dc7d5 
Qexec() : i and j were wrong : line 552 - 555

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yajirushi 0:e1964b6e160c 1 #include "L6470SDC.h"
Yajirushi 0:e1964b6e160c 2
Yajirushi 3:486fb90dc7d5 3 #define CS_ACTIVE cs->write(0)
Yajirushi 3:486fb90dc7d5 4 #define CS_INACTIVE cs->write(1)
Yajirushi 0:e1964b6e160c 5
Yajirushi 3:486fb90dc7d5 6 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 7 * Constructor (override +9)
Yajirushi 3:486fb90dc7d5 8 -------------------------------------------------------------------------- */
Yajirushi 0:e1964b6e160c 9 L6470SDC::L6470SDC(PinName tx, PinName rx, PinName mosi, PinName miso, PinName sclk, PinName csel){
Yajirushi 0:e1964b6e160c 10 pc = new Serial(tx, rx);
Yajirushi 0:e1964b6e160c 11 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 12 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 13 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 14 }
Yajirushi 0:e1964b6e160c 15 L6470SDC::L6470SDC(PinName tx, PinName rx, PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 16 pc = new Serial(tx, rx);
Yajirushi 0:e1964b6e160c 17 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 18 cs = csel;
Yajirushi 0:e1964b6e160c 19 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 20 }
Yajirushi 0:e1964b6e160c 21 L6470SDC::L6470SDC(Serial *serial, PinName mosi, PinName miso, PinName sclk, PinName csel){
Yajirushi 0:e1964b6e160c 22 pc = serial;
Yajirushi 0:e1964b6e160c 23 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 24 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 25 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 26 }
Yajirushi 0:e1964b6e160c 27 L6470SDC::L6470SDC(Serial *serial, PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 28 pc = serial;
Yajirushi 0:e1964b6e160c 29 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 30 cs = csel;
Yajirushi 0:e1964b6e160c 31 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 32 }
Yajirushi 0:e1964b6e160c 33 L6470SDC::L6470SDC(Serial *serial, SPI *spi, PinName csel){
Yajirushi 0:e1964b6e160c 34 pc = serial;
Yajirushi 0:e1964b6e160c 35 this->spi = spi;
Yajirushi 0:e1964b6e160c 36 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 37 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 38 }
Yajirushi 0:e1964b6e160c 39 L6470SDC::L6470SDC(Serial *serial, SPI *spi, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 40 pc = serial;
Yajirushi 0:e1964b6e160c 41 this->spi = spi;
Yajirushi 0:e1964b6e160c 42 cs = csel;
Yajirushi 0:e1964b6e160c 43 hasSerial = !(pc == NULL);
Yajirushi 0:e1964b6e160c 44 }
Yajirushi 0:e1964b6e160c 45 L6470SDC::L6470SDC(PinName mosi, PinName miso, PinName sclk, PinName csel){
Yajirushi 0:e1964b6e160c 46 hasSerial = false;
Yajirushi 0:e1964b6e160c 47 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 48 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 49 }
Yajirushi 0:e1964b6e160c 50 L6470SDC::L6470SDC(PinName mosi, PinName miso, PinName sclk, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 51 hasSerial = false;
Yajirushi 0:e1964b6e160c 52 spi = new SPI(mosi, miso, sclk);
Yajirushi 0:e1964b6e160c 53 cs = csel;
Yajirushi 0:e1964b6e160c 54 }
Yajirushi 0:e1964b6e160c 55 L6470SDC::L6470SDC(SPI *spi, PinName csel){
Yajirushi 0:e1964b6e160c 56 hasSerial = false;
Yajirushi 0:e1964b6e160c 57 this->spi = spi;
Yajirushi 0:e1964b6e160c 58 cs = new DigitalOut(csel);
Yajirushi 0:e1964b6e160c 59 }
Yajirushi 0:e1964b6e160c 60 L6470SDC::L6470SDC(SPI *spi, DigitalOut *csel){
Yajirushi 0:e1964b6e160c 61 hasSerial = false;
Yajirushi 0:e1964b6e160c 62 this->spi = spi;
Yajirushi 0:e1964b6e160c 63 cs = csel;
Yajirushi 0:e1964b6e160c 64 }
Yajirushi 0:e1964b6e160c 65
Yajirushi 3:486fb90dc7d5 66 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 67 * Destructor
Yajirushi 3:486fb90dc7d5 68 -------------------------------------------------------------------------- */
Yajirushi 3:486fb90dc7d5 69 L6470SDC::~L6470SDC(){
Yajirushi 3:486fb90dc7d5 70 delete[] Queue;
Yajirushi 3:486fb90dc7d5 71 delete pc;
Yajirushi 3:486fb90dc7d5 72 delete spi;
Yajirushi 3:486fb90dc7d5 73 delete cs;
Yajirushi 3:486fb90dc7d5 74 }
Yajirushi 3:486fb90dc7d5 75
Yajirushi 3:486fb90dc7d5 76 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 77 * initializer
Yajirushi 3:486fb90dc7d5 78 -------------------------------------------------------------------------- */
Yajirushi 1:db64ad30b4b3 79 void L6470SDC::init(bool initSPI){
Yajirushi 0:e1964b6e160c 80 //SPI初期化
Yajirushi 1:db64ad30b4b3 81 if(initSPI){
Yajirushi 1:db64ad30b4b3 82 spi->frequency(L6470_SPI_FREQ);
Yajirushi 1:db64ad30b4b3 83 spi->format(8, 3);
Yajirushi 1:db64ad30b4b3 84 }
Yajirushi 1:db64ad30b4b3 85
Yajirushi 0:e1964b6e160c 86 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 87
Yajirushi 0:e1964b6e160c 88 wait_ms(5);
Yajirushi 0:e1964b6e160c 89
Yajirushi 0:e1964b6e160c 90 //SPIで値を送り出し、値が出てくるまでの個数を特定する(デイジーチェーンされている個数がわかる)
Yajirushi 0:e1964b6e160c 91 unsigned char rsv = 0x00;
Yajirushi 0:e1964b6e160c 92 motor_count = 0;
Yajirushi 0:e1964b6e160c 93 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 94 spi->write(0xEE);
Yajirushi 0:e1964b6e160c 95 do{
Yajirushi 0:e1964b6e160c 96 rsv = spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 97 motor_count++;
Yajirushi 0:e1964b6e160c 98 }while(rsv != 0xEE);
Yajirushi 0:e1964b6e160c 99 CS_INACTIVE;
Yajirushi 3:486fb90dc7d5 100
Yajirushi 3:486fb90dc7d5 101 //モーター個数分のキューを作成(要素番号とモーター番号を合わせるため+1している)
Yajirushi 3:486fb90dc7d5 102 Queue = new L6470CMDQ[motor_count+1];
Yajirushi 3:486fb90dc7d5 103
Yajirushi 3:486fb90dc7d5 104 //キューをクリア
Yajirushi 3:486fb90dc7d5 105 Qclear();
Yajirushi 0:e1964b6e160c 106
Yajirushi 0:e1964b6e160c 107 //全てのモーターに空コマンドを送り、リセットをかける
Yajirushi 0:e1964b6e160c 108 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 109 for(int i = 0; i < (motor_count << 2); i++) spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 110 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 111 for(int i = 1; i <= motor_count; i++) motorReset(i);
Yajirushi 0:e1964b6e160c 112
Yajirushi 0:e1964b6e160c 113 //全てのモーターに初期設定
Yajirushi 0:e1964b6e160c 114 //最大速度 1秒間に4回転
Yajirushi 0:e1964b6e160c 115 //加速、減速 0x02E0(ほぼ一瞬で加減速)
Yajirushi 0:e1964b6e160c 116 //フルステップ移行速度 0xff(常にマイクロステップ駆動)
Yajirushi 0:e1964b6e160c 117 for(int i = 1; i <= motor_count; i++){
Yajirushi 0:e1964b6e160c 118 setMaximumSpeed(i, calcMaxSpd(800.0));
Yajirushi 0:e1964b6e160c 119 setAcceleration(i, 0x02E0);
Yajirushi 0:e1964b6e160c 120 setDeceleration(i, 0x02E0);
Yajirushi 0:e1964b6e160c 121 setFSSpeed(i, 0xff);
Yajirushi 0:e1964b6e160c 122 }
Yajirushi 0:e1964b6e160c 123 }
Yajirushi 0:e1964b6e160c 124
Yajirushi 3:486fb90dc7d5 125 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 126 * GET motor count
Yajirushi 3:486fb90dc7d5 127 -------------------------------------------------------------------------- */
Yajirushi 0:e1964b6e160c 128 int L6470SDC::getMotorCount(){
Yajirushi 0:e1964b6e160c 129 return motor_count;
Yajirushi 0:e1964b6e160c 130 }
Yajirushi 0:e1964b6e160c 131
Yajirushi 3:486fb90dc7d5 132 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 133 * PRIVATE: command send to L6470
Yajirushi 3:486fb90dc7d5 134 -------------------------------------------------------------------------- */
Yajirushi 0:e1964b6e160c 135 void L6470SDC::setCmd(int motorNumber, unsigned char cmdAddress, unsigned long value, int bitLen){
Yajirushi 0:e1964b6e160c 136 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 137 if(hasSerial) pc->printf("[Command] Motor-No.%03d, CMD:0x%04X, VAL:0x%06X\r\n", motorNumber, cmdAddress, value);
Yajirushi 0:e1964b6e160c 138 #endif
Yajirushi 0:e1964b6e160c 139 //コマンド送信：対象のモーター以外にはNOPを送信する
Yajirushi 0:e1964b6e160c 140 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 141 for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? cmdAddress : CMD_NOP);
Yajirushi 0:e1964b6e160c 142 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 143
Yajirushi 0:e1964b6e160c 144 //コマンドの後続の値がなければ終了
Yajirushi 0:e1964b6e160c 145 if(bitLen < 1) return;
Yajirushi 0:e1964b6e160c 146
Yajirushi 0:e1964b6e160c 147 //値を8bitに分けて送信していく：対象のモーター以外には同じ数だけNOPを送信する
Yajirushi 0:e1964b6e160c 148 //bitLenを8で割り、送信回数を求める
Yajirushi 0:e1964b6e160c 149 //送信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
Yajirushi 0:e1964b6e160c 150 //対象のモーター以外には同じ数だけNOPを送信する
Yajirushi 0:e1964b6e160c 151 int send8 = (bitLen - 1) >> 3; //int send8 = (bitLen-1) / 8;
Yajirushi 0:e1964b6e160c 152 int top8bit = bitLen - (send8 << 3); //int top8bit = bitLen - (send8 * 8);
Yajirushi 0:e1964b6e160c 153 ++send8;
Yajirushi 0:e1964b6e160c 154 for(int j = 1; j <= send8; j++){
Yajirushi 0:e1964b6e160c 155 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 156 for(int i = motor_count; i >= 1; i--){
Yajirushi 0:e1964b6e160c 157 if(i == motorNumber){
Yajirushi 0:e1964b6e160c 158 bitLen -= (j == 1) ? top8bit : 8;
Yajirushi 0:e1964b6e160c 159 spi->write((unsigned char)(value >> bitLen));
Yajirushi 0:e1964b6e160c 160 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 161 if(hasSerial) pc->printf(">>>> value transfer[%d] = 0x%02X\r\n", j, (unsigned char)(value >> bitLen));
Yajirushi 0:e1964b6e160c 162 #endif
Yajirushi 0:e1964b6e160c 163 }else{
Yajirushi 0:e1964b6e160c 164 spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 165 }
Yajirushi 0:e1964b6e160c 166 }
Yajirushi 0:e1964b6e160c 167 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 168 }
Yajirushi 0:e1964b6e160c 169 }
Yajirushi 0:e1964b6e160c 170
Yajirushi 3:486fb90dc7d5 171 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 172 * PRIVATE: parameter send to L6470
Yajirushi 3:486fb90dc7d5 173 -------------------------------------------------------------------------- */
Yajirushi 0:e1964b6e160c 174 void L6470SDC::setParam(int motorNumber, unsigned char regAddress, unsigned long value, int bitLen){
Yajirushi 0:e1964b6e160c 175 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 176 if(hasSerial) pc->printf("[SetParam] Motor-No.%03d, SetParam: REG:0x%04X, VAL:0x%06X\r\n", motorNumber, regAddress, value);
Yajirushi 0:e1964b6e160c 177 #endif
Yajirushi 0:e1964b6e160c 178 //SETPARAM送信：対象のモーター以外にはNOPを送信する
Yajirushi 0:e1964b6e160c 179 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 180 for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? (CMD_SETPARAM | regAddress) : CMD_NOP);
Yajirushi 0:e1964b6e160c 181 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 182
Yajirushi 0:e1964b6e160c 183 //SETPARAMに続く引数を8bitに分けて送信していく：対象のモーター以外には同じ数だけNOPを送信する
Yajirushi 0:e1964b6e160c 184 //bitLenを8で割り、送信回数を求める
Yajirushi 0:e1964b6e160c 185 //送信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
Yajirushi 0:e1964b6e160c 186 //対象のモーター以外には同じ数だけNOPを送信する
Yajirushi 0:e1964b6e160c 187 int send8 = (bitLen - 1) >> 3; //int send8 = (bitLen-1) / 8;
Yajirushi 0:e1964b6e160c 188 int top8bit = bitLen - (send8 << 3); //int top8bit = bitLen - (send8 * 8);
Yajirushi 0:e1964b6e160c 189 ++send8;
Yajirushi 0:e1964b6e160c 190 for(int j = 1; j <= send8; 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 bitLen -= (j == 1) ? top8bit : 8;
Yajirushi 0:e1964b6e160c 195 spi->write((unsigned char)(value >> bitLen));
Yajirushi 0:e1964b6e160c 196 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 197 if(hasSerial) pc->printf(">>>> value transfer[%d] = 0x%02X\r\n", j, (unsigned char)(value >> bitLen));
Yajirushi 0:e1964b6e160c 198 #endif
Yajirushi 0:e1964b6e160c 199 }else{
Yajirushi 0:e1964b6e160c 200 spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 201 }
Yajirushi 0:e1964b6e160c 202 }
Yajirushi 0:e1964b6e160c 203 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 204 }
Yajirushi 0:e1964b6e160c 205 }
Yajirushi 0:e1964b6e160c 206
Yajirushi 3:486fb90dc7d5 207 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 208 * PRIVATE: parameter get from L6470
Yajirushi 3:486fb90dc7d5 209 -------------------------------------------------------------------------- */
Yajirushi 0:e1964b6e160c 210 unsigned long L6470SDC::getParam(int motorNumber, unsigned char regAddress, int bitLen){
Yajirushi 0:e1964b6e160c 211 //戻り値
Yajirushi 0:e1964b6e160c 212 unsigned long ret = 0;
Yajirushi 0:e1964b6e160c 213 unsigned char val = 0;
Yajirushi 0:e1964b6e160c 214
Yajirushi 0:e1964b6e160c 215 //GETPARAM送信：対象のモーター以外にはNOPを送信する
Yajirushi 0:e1964b6e160c 216 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 217 for(int i = motor_count; i >= 1; i--) spi->write((i == motorNumber)? (CMD_GETPARAM | regAddress) : CMD_NOP);
Yajirushi 0:e1964b6e160c 218 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 219
Yajirushi 0:e1964b6e160c 220 //データを8bitに分けて受信してretに格納していく
Yajirushi 0:e1964b6e160c 221 //bitLenを8で割り、受信回数を求める
Yajirushi 0:e1964b6e160c 222 //受信回数の分だけモーター個数分ループさせ(jとiをひっくり返している)
Yajirushi 0:e1964b6e160c 223 //対象のモーター以外の受信は無視する
Yajirushi 0:e1964b6e160c 224 int receive8 = (bitLen - 1) >> 3; //int receive8 = (bitLen-1) / 8;
Yajirushi 0:e1964b6e160c 225 ++receive8;
Yajirushi 0:e1964b6e160c 226 for(int j = 1; j <= receive8; j++){
Yajirushi 0:e1964b6e160c 227 CS_ACTIVE;
Yajirushi 0:e1964b6e160c 228 for(int i = motor_count; i >= 1; i--){
Yajirushi 0:e1964b6e160c 229 if(i == motorNumber){
Yajirushi 0:e1964b6e160c 230 ret |= (j == receive8) ? spi->write(CMD_NOP) : spi->write(CMD_NOP) << ((receive8 - j) << 3);
Yajirushi 0:e1964b6e160c 231 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 232 if(hasSerial) pc->printf("<<<< receive data[%d] = 0x%06X\r\n", j, ret);
Yajirushi 0:e1964b6e160c 233 #endif
Yajirushi 0:e1964b6e160c 234 }else{
Yajirushi 0:e1964b6e160c 235 val = spi->write(CMD_NOP);
Yajirushi 0:e1964b6e160c 236 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 237 if(hasSerial) pc->printf("???? receive other data[%d] = 0x%02X\r\n", j, val);
Yajirushi 0:e1964b6e160c 238 #endif
Yajirushi 0:e1964b6e160c 239 }
Yajirushi 0:e1964b6e160c 240 }
Yajirushi 0:e1964b6e160c 241 CS_INACTIVE;
Yajirushi 0:e1964b6e160c 242 }
Yajirushi 0:e1964b6e160c 243 #ifdef DEBUG_L6470SDC
Yajirushi 0:e1964b6e160c 244 if(hasSerial) pc->printf("[GetParam] Motor-No.%03d, GetParam: REG:0x%04X, VAL:0x%06X\r\n", motorNumber, regAddress, ret);
Yajirushi 0:e1964b6e160c 245 #endif
Yajirushi 0:e1964b6e160c 246 return ret;
Yajirushi 0:e1964b6e160c 247 }
Yajirushi 0:e1964b6e160c 248
Yajirushi 3:486fb90dc7d5 249 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 250 * COMMAND
Yajirushi 3:486fb90dc7d5 251 -------------------------------------------------------------------------- */
Yajirushi 3:486fb90dc7d5 252 //@ GET busyflag
Yajirushi 0:e1964b6e160c 253 bool L6470SDC::isBusy(int motorNumber){
Yajirushi 3:486fb90dc7d5 254 if(motorNumber > motor_count || motorNumber < 1) return false;
Yajirushi 0:e1964b6e160c 255 return !((getStatus(motorNumber) & 0x02) >> 1);
Yajirushi 0:e1964b6e160c 256 }
Yajirushi 3:486fb90dc7d5 257 //@ motor run
Yajirushi 0:e1964b6e160c 258 void L6470SDC::run(int motorNumber, unsigned long hex_speed, bool isClockwise){
Yajirushi 3:486fb90dc7d5 259 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 260 setCmd(motorNumber, (isClockwise) ? CMD_RUN_MINUS : CMD_RUN_PLUS, hex_speed, 20);
Yajirushi 0:e1964b6e160c 261 }
Yajirushi 3:486fb90dc7d5 262 //@ motor step
Yajirushi 0:e1964b6e160c 263 void L6470SDC::step(int motorNumber, unsigned int count, bool isClockwise){
Yajirushi 3:486fb90dc7d5 264 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 265 for(;count<=0;count--){
Yajirushi 0:e1964b6e160c 266 while(isBusy(motorNumber));
Yajirushi 0:e1964b6e160c 267 setCmd(motorNumber, (isClockwise) ? CMD_STEP_MINUS : CMD_STEP_PLUS, 0, 0);
Yajirushi 0:e1964b6e160c 268 }
Yajirushi 0:e1964b6e160c 269 }
Yajirushi 3:486fb90dc7d5 270 //@ motor move
Yajirushi 0:e1964b6e160c 271 void L6470SDC::move(int motorNumber, unsigned long stepsCount, bool isClockwise){
Yajirushi 3:486fb90dc7d5 272 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 273 setCmd(motorNumber, (isClockwise) ? CMD_ADDSTEP_MINUS : CMD_ADDSTEP_PLUS, stepsCount, 22);
Yajirushi 0:e1964b6e160c 274 }
Yajirushi 3:486fb90dc7d5 275 //@ motor goto ABSpos(shortest path)
Yajirushi 0:e1964b6e160c 276 void L6470SDC::goto1(int motorNumber, unsigned long ABSPos){
Yajirushi 3:486fb90dc7d5 277 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 278 setCmd(motorNumber, CMD_GOTO, ABSPos, 22);
Yajirushi 0:e1964b6e160c 279 }
Yajirushi 3:486fb90dc7d5 280 //@ motor goto ABSpos
Yajirushi 0:e1964b6e160c 281 void L6470SDC::goto2(int motorNumber, unsigned long ABSPos, bool isClockwise){
Yajirushi 3:486fb90dc7d5 282 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 283 setCmd(motorNumber, (isClockwise) ? CMD_GOTO_DIR_MINUS : CMD_GOTO_DIR_PLUS, ABSPos, 22);
Yajirushi 0:e1964b6e160c 284 }
Yajirushi 3:486fb90dc7d5 285 //@ motor goto mark
Yajirushi 0:e1964b6e160c 286 void L6470SDC::goUntil(int motorNumber, unsigned long hex_speed, bool isClockwise, bool setMark){
Yajirushi 3:486fb90dc7d5 287 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 288 unsigned short cmdAddr = (isClockwise) ? CMD_GO_UNTIL_MINUS : CMD_GO_UNTIL_PLUS;
Yajirushi 0:e1964b6e160c 289 setCmd(motorNumber, (setMark) ? (cmdAddr | 0x08) : cmdAddr, hex_speed, 22);
Yajirushi 0:e1964b6e160c 290 }
Yajirushi 3:486fb90dc7d5 291 //@ release sw
Yajirushi 0:e1964b6e160c 292 void L6470SDC::releaseSwitch(int motorNumber, bool isClockwise, bool setMark){
Yajirushi 3:486fb90dc7d5 293 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 294 unsigned short cmdAddr = (isClockwise) ? CMD_RELEASE_SW_MINUS : CMD_RELEASE_SW_PLUS;
Yajirushi 0:e1964b6e160c 295 setCmd(motorNumber, (setMark) ? (cmdAddr | 0x08) : cmdAddr, 0, 0);
Yajirushi 0:e1964b6e160c 296 }
Yajirushi 3:486fb90dc7d5 297 //@ motor goto zero-position
Yajirushi 0:e1964b6e160c 298 void L6470SDC::home(int motorNumber){
Yajirushi 3:486fb90dc7d5 299 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 300 setCmd(motorNumber, CMD_GO_HOME, 0, 0);
Yajirushi 0:e1964b6e160c 301 }
Yajirushi 3:486fb90dc7d5 302 //@ alias:home
Yajirushi 0:e1964b6e160c 303 void L6470SDC::zero(int motorNumber){
Yajirushi 3:486fb90dc7d5 304 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 305 setCmd(motorNumber, CMD_GO_HOME, 0, 0);
Yajirushi 0:e1964b6e160c 306 }
Yajirushi 3:486fb90dc7d5 307 //@ motor goto MARK-position
Yajirushi 0:e1964b6e160c 308 void L6470SDC::gotoMark(int motorNumber){
Yajirushi 3:486fb90dc7d5 309 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 310 setCmd(motorNumber, CMD_GO_MARK, 0, 0);
Yajirushi 0:e1964b6e160c 311 }
Yajirushi 3:486fb90dc7d5 312 //@ reset zero-position
Yajirushi 0:e1964b6e160c 313 void L6470SDC::resetHome(int motorNumber){
Yajirushi 3:486fb90dc7d5 314 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 315 setCmd(motorNumber, CMD_RESET_POS, 0, 0);
Yajirushi 0:e1964b6e160c 316 }
Yajirushi 3:486fb90dc7d5 317 //@ alias:resetHome
Yajirushi 0:e1964b6e160c 318 void L6470SDC::resetZero(int motorNumber){
Yajirushi 3:486fb90dc7d5 319 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 320 setCmd(motorNumber, CMD_RESET_POS, 0, 0);
Yajirushi 0:e1964b6e160c 321 }
Yajirushi 3:486fb90dc7d5 322 //@ motor reset
Yajirushi 0:e1964b6e160c 323 void L6470SDC::motorReset(int motorNumber){
Yajirushi 3:486fb90dc7d5 324 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 325 setCmd(motorNumber, CMD_RESET_DEVICE, 0, 0);
Yajirushi 0:e1964b6e160c 326 }
Yajirushi 3:486fb90dc7d5 327 //@ motor soft-stop
Yajirushi 0:e1964b6e160c 328 void L6470SDC::stop(int motorNumber){
Yajirushi 3:486fb90dc7d5 329 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 330 setCmd(motorNumber, CMD_SOFT_STOP, 0, 0);
Yajirushi 0:e1964b6e160c 331 }
Yajirushi 3:486fb90dc7d5 332 //@ motor hard-stop
Yajirushi 0:e1964b6e160c 333 void L6470SDC::stopImmidiate(int motorNumber){
Yajirushi 3:486fb90dc7d5 334 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 335 setCmd(motorNumber, CMD_HARD_STOP, 0, 0);
Yajirushi 0:e1964b6e160c 336 }
Yajirushi 3:486fb90dc7d5 337 //@ motor HighImpedance
Yajirushi 0:e1964b6e160c 338 void L6470SDC::stop_HighImpedance(int motorNumber){
Yajirushi 3:486fb90dc7d5 339 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 340 setCmd(motorNumber, CMD_SOFT_HIZ, 0, 0);
Yajirushi 0:e1964b6e160c 341 }
Yajirushi 3:486fb90dc7d5 342 //@ motor HighImpedance hard-stop
Yajirushi 0:e1964b6e160c 343 void L6470SDC::stopImmidiate_HighImpedance(int motorNumber){
Yajirushi 3:486fb90dc7d5 344 if(motorNumber > motor_count || motorNumber < 1) return;
Yajirushi 0:e1964b6e160c 345 setCmd(motorNumber, CMD_HARD_HIZ, 0, 0);
Yajirushi 0:e1964b6e160c 346 }
Yajirushi 0:e1964b6e160c 347
Yajirushi 3:486fb90dc7d5 348 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 349 * COMMAND (Daisy-chain Enqueue)
Yajirushi 3:486fb90dc7d5 350 -------------------------------------------------------------------------- */
Yajirushi 3:486fb90dc7d5 351 //@ enqueue NOP
Yajirushi 3:486fb90dc7d5 352 int L6470SDC::ENQ_nop(int motorNumber){
Yajirushi 3:486fb90dc7d5 353 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 354 Queue[motorNumber].addr = CMD_NOP;
Yajirushi 3:486fb90dc7d5 355 Queue[motorNumber].val = CMD_NOP;
Yajirushi 3:486fb90dc7d5 356 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 357 return motorNumber;
Yajirushi 3:486fb90dc7d5 358 }
Yajirushi 3:486fb90dc7d5 359 //@ enqueue run command
Yajirushi 3:486fb90dc7d5 360 int L6470SDC::ENQ_run(int motorNumber, unsigned long hex_speed, bool isClockwise){
Yajirushi 3:486fb90dc7d5 361 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 362 Queue[motorNumber].addr = (isClockwise) ? CMD_RUN_MINUS : CMD_RUN_PLUS;
Yajirushi 3:486fb90dc7d5 363 Queue[motorNumber].val = hex_speed;
Yajirushi 3:486fb90dc7d5 364 Queue[motorNumber].bitLen = 20;
Yajirushi 3:486fb90dc7d5 365 return motorNumber;
Yajirushi 3:486fb90dc7d5 366 }
Yajirushi 3:486fb90dc7d5 367 //@ enqueue move command
Yajirushi 3:486fb90dc7d5 368 int L6470SDC::ENQ_move(int motorNumber, unsigned long stepsCount, bool isClockwise){
Yajirushi 3:486fb90dc7d5 369 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 370 Queue[motorNumber].addr = (isClockwise) ? CMD_ADDSTEP_MINUS : CMD_ADDSTEP_PLUS;
Yajirushi 3:486fb90dc7d5 371 Queue[motorNumber].val = stepsCount;
Yajirushi 3:486fb90dc7d5 372 Queue[motorNumber].bitLen = 22;
Yajirushi 3:486fb90dc7d5 373 return motorNumber;
Yajirushi 3:486fb90dc7d5 374 }
Yajirushi 3:486fb90dc7d5 375 //@ enqueue goto(shortest) command
Yajirushi 3:486fb90dc7d5 376 int L6470SDC::ENQ_goto1(int motorNumber, unsigned long ABSPos){
Yajirushi 3:486fb90dc7d5 377 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 378 Queue[motorNumber].addr = CMD_GOTO;
Yajirushi 3:486fb90dc7d5 379 Queue[motorNumber].val = ABSPos;
Yajirushi 3:486fb90dc7d5 380 Queue[motorNumber].bitLen = 22;
Yajirushi 3:486fb90dc7d5 381 return motorNumber;
Yajirushi 3:486fb90dc7d5 382 }
Yajirushi 3:486fb90dc7d5 383 //@ enqueue goto command
Yajirushi 3:486fb90dc7d5 384 int L6470SDC::ENQ_goto2(int motorNumber, unsigned long ABSPos, bool isClockwise){
Yajirushi 3:486fb90dc7d5 385 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 386 Queue[motorNumber].addr = (isClockwise) ? CMD_GOTO_DIR_MINUS : CMD_GOTO_DIR_PLUS;
Yajirushi 3:486fb90dc7d5 387 Queue[motorNumber].val = ABSPos;
Yajirushi 3:486fb90dc7d5 388 Queue[motorNumber].bitLen = 22;
Yajirushi 3:486fb90dc7d5 389 return motorNumber;
Yajirushi 3:486fb90dc7d5 390 }
Yajirushi 3:486fb90dc7d5 391 //@ enqueue goUntil command
Yajirushi 3:486fb90dc7d5 392 int L6470SDC::ENQ_goUntil(int motorNumber, unsigned long hex_speed, bool isClockwise, bool setMark){
Yajirushi 3:486fb90dc7d5 393 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 394 unsigned short cmdAddr = (isClockwise) ? CMD_GO_UNTIL_MINUS : CMD_GO_UNTIL_PLUS;
Yajirushi 3:486fb90dc7d5 395 Queue[motorNumber].addr = (setMark)? (cmdAddr | 0x08) : cmdAddr;
Yajirushi 3:486fb90dc7d5 396 Queue[motorNumber].val = hex_speed;
Yajirushi 3:486fb90dc7d5 397 Queue[motorNumber].bitLen = 22;
Yajirushi 3:486fb90dc7d5 398 return motorNumber;
Yajirushi 3:486fb90dc7d5 399 }
Yajirushi 3:486fb90dc7d5 400 //@ enqueue release switch command
Yajirushi 3:486fb90dc7d5 401 int L6470SDC::ENQ_releaseSwitch(int motorNumber, bool isClockwise, bool setMark){
Yajirushi 3:486fb90dc7d5 402 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 403 unsigned short cmdAddr = (isClockwise) ? CMD_RELEASE_SW_MINUS : CMD_RELEASE_SW_PLUS;
Yajirushi 3:486fb90dc7d5 404 Queue[motorNumber].addr = (setMark)? (cmdAddr | 0x08) : cmdAddr;
Yajirushi 3:486fb90dc7d5 405 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 406 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 407 return motorNumber;
Yajirushi 3:486fb90dc7d5 408 }
Yajirushi 3:486fb90dc7d5 409 //@ enqueue goHome command
Yajirushi 3:486fb90dc7d5 410 int L6470SDC::ENQ_home(int motorNumber){
Yajirushi 3:486fb90dc7d5 411 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 412 Queue[motorNumber].addr = CMD_GO_HOME;
Yajirushi 3:486fb90dc7d5 413 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 414 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 415 return motorNumber;
Yajirushi 3:486fb90dc7d5 416 }
Yajirushi 3:486fb90dc7d5 417 //@ alias:ENQ_home
Yajirushi 3:486fb90dc7d5 418 int L6470SDC::ENQ_zero(int motorNumber){
Yajirushi 3:486fb90dc7d5 419 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 420 Queue[motorNumber].addr = CMD_GO_HOME;
Yajirushi 3:486fb90dc7d5 421 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 422 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 423 return motorNumber;
Yajirushi 3:486fb90dc7d5 424 }
Yajirushi 3:486fb90dc7d5 425 //@ enqueue gotoMark command
Yajirushi 3:486fb90dc7d5 426 int L6470SDC::ENQ_gotoMark(int motorNumber){
Yajirushi 3:486fb90dc7d5 427 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 428 Queue[motorNumber].addr = CMD_GO_MARK;
Yajirushi 3:486fb90dc7d5 429 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 430 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 431 return motorNumber;
Yajirushi 3:486fb90dc7d5 432 }
Yajirushi 3:486fb90dc7d5 433 //@ enqueue reset_pos command
Yajirushi 3:486fb90dc7d5 434 int L6470SDC::ENQ_resetHome(int motorNumber){
Yajirushi 3:486fb90dc7d5 435 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 436 Queue[motorNumber].addr = CMD_RESET_POS;
Yajirushi 3:486fb90dc7d5 437 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 438 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 439 return motorNumber;
Yajirushi 3:486fb90dc7d5 440 }
Yajirushi 3:486fb90dc7d5 441 //@ alias:ENQ_resetHome
Yajirushi 3:486fb90dc7d5 442 int L6470SDC::ENQ_resetZero(int motorNumber){
Yajirushi 3:486fb90dc7d5 443 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 444 Queue[motorNumber].addr = CMD_RESET_POS;
Yajirushi 3:486fb90dc7d5 445 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 446 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 447 return motorNumber;
Yajirushi 3:486fb90dc7d5 448 }
Yajirushi 3:486fb90dc7d5 449 //@ enqueue reset motor command
Yajirushi 3:486fb90dc7d5 450 int L6470SDC::ENQ_motorReset(int motorNumber){
Yajirushi 3:486fb90dc7d5 451 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 452 Queue[motorNumber].addr = CMD_RESET_DEVICE;
Yajirushi 3:486fb90dc7d5 453 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 454 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 455 return motorNumber;
Yajirushi 3:486fb90dc7d5 456 }
Yajirushi 3:486fb90dc7d5 457 //@ enqueue soft-stop command
Yajirushi 3:486fb90dc7d5 458 int L6470SDC::ENQ_stop(int motorNumber){
Yajirushi 3:486fb90dc7d5 459 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 460 Queue[motorNumber].addr = CMD_SOFT_STOP;
Yajirushi 3:486fb90dc7d5 461 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 462 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 463 return motorNumber;
Yajirushi 3:486fb90dc7d5 464 }
Yajirushi 3:486fb90dc7d5 465 //@ enqueue hard-stop command
Yajirushi 3:486fb90dc7d5 466 int L6470SDC::ENQ_stopImmidiate(int motorNumber){
Yajirushi 3:486fb90dc7d5 467 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 468 Queue[motorNumber].addr = CMD_HARD_STOP;
Yajirushi 3:486fb90dc7d5 469 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 470 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 471 return motorNumber;
Yajirushi 3:486fb90dc7d5 472 }
Yajirushi 3:486fb90dc7d5 473 //@ enqueue soft-stop(HIZ) command
Yajirushi 3:486fb90dc7d5 474 int L6470SDC::ENQ_stop_HighImpedance(int motorNumber){
Yajirushi 3:486fb90dc7d5 475 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 476 Queue[motorNumber].addr = CMD_SOFT_HIZ;
Yajirushi 3:486fb90dc7d5 477 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 478 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 479 return motorNumber;
Yajirushi 3:486fb90dc7d5 480 }
Yajirushi 3:486fb90dc7d5 481 //@ enqueue hard-stop(HIZ) command
Yajirushi 3:486fb90dc7d5 482 int L6470SDC::ENQ_stopImmidiate_HighImpedance(int motorNumber){
Yajirushi 3:486fb90dc7d5 483 if(motorNumber > motor_count || motorNumber < 1) return -1;
Yajirushi 3:486fb90dc7d5 484 Queue[motorNumber].addr = CMD_HARD_HIZ;
Yajirushi 3:486fb90dc7d5 485 Queue[motorNumber].val = 0;
Yajirushi 3:486fb90dc7d5 486 Queue[motorNumber].bitLen = 0;
Yajirushi 3:486fb90dc7d5 487 return motorNumber;
Yajirushi 3:486fb90dc7d5 488 }
Yajirushi 3:486fb90dc7d5 489 //@ command queue clear
Yajirushi 3:486fb90dc7d5 490 void L6470SDC::Qclear(){
Yajirushi 3:486fb90dc7d5 491 for(int i=0; i<=motor_count; i++){
Yajirushi 3:486fb90dc7d5 492 Queue[i].addr = 0;
Yajirushi 3:486fb90dc7d5 493 Queue[i].val = 0;
Yajirushi 3:486fb90dc7d5 494 Queue[i].bitLen = -1;
Yajirushi 3:486fb90dc7d5 495 }
Yajirushi 3:486fb90dc7d5 496 }
Yajirushi 3:486fb90dc7d5 497 //@ queue execute
Yajirushi 3:486fb90dc7d5 498 int L6470SDC::Qexec(bool finallyClearQueue){
Yajirushi 3:486fb90dc7d5 499 int bitLens[motor_count];
Yajirushi 3:486fb90dc7d5 500 memset(bitLens, 0, sizeof(int) * motor_count);
Yajirushi 3:486fb90dc7d5 501 int maxBitLen = 0;
Yajirushi 3:486fb90dc7d5 502
Yajirushi 3:486fb90dc7d5 503 //構造体の中のbitLenの最大値を抽出, 計算に使用するので別枠に格納
Yajirushi 3:486fb90dc7d5 504 for(int i=0; i<motor_count; i++){
Yajirushi 3:486fb90dc7d5 505 bitLens[i] = Queue[i+1].bitLen;
Yajirushi 3:486fb90dc7d5 506 if(maxBitLen < bitLens[i]) maxBitLen = bitLens[i];
Yajirushi 3:486fb90dc7d5 507 }
Yajirushi 3:486fb90dc7d5 508
Yajirushi 3:486fb90dc7d5 509 //bitLenの最大値が0未満、つまり構造体に値が入っていないのでここで終わる
Yajirushi 3:486fb90dc7d5 510 if(maxBitLen < 0) return -1;
Yajirushi 3:486fb90dc7d5 511
Yajirushi 3:486fb90dc7d5 512 //コマンド送信
Yajirushi 3:486fb90dc7d5 513 CS_ACTIVE;
Yajirushi 3:486fb90dc7d5 514 for(int i=0; i<motor_count; i++){
Yajirushi 3:486fb90dc7d5 515 spi->write(Queue[i+1].addr);
Yajirushi 3:486fb90dc7d5 516 #ifdef DEBUG_L6470SDC
Yajirushi 3:486fb90dc7d5 517 if(hasSerial) pc->printf("[Queue:Command] Motor-No.%03d, CMD:0x%04X\r\n", i+1, Queue[i+1].addr);
Yajirushi 3:486fb90dc7d5 518 #endif
Yajirushi 3:486fb90dc7d5 519 }
Yajirushi 3:486fb90dc7d5 520 CS_INACTIVE;
Yajirushi 3:486fb90dc7d5 521
Yajirushi 3:486fb90dc7d5 522 //bitLenの最大値が0、つまり送る値がないのでここで終わる
Yajirushi 3:486fb90dc7d5 523 if(maxBitLen < 1) return 0;
Yajirushi 3:486fb90dc7d5 524
Yajirushi 3:486fb90dc7d5 525 //モーター個数分×送信回数分の箱を作り、その中にあらかじめ値を入れ、それを順次送り出す
Yajirushi 3:486fb90dc7d5 526 //bitLenが最も長いものから送信回数を求める
Yajirushi 3:486fb90dc7d5 527 int maxSend8 = (maxBitLen - 1) >> 3;
Yajirushi 3:486fb90dc7d5 528
Yajirushi 3:486fb90dc7d5 529 //8bitに区切った場合の区切りの場所を算出
Yajirushi 3:486fb90dc7d5 530 int top8bit[motor_count];
Yajirushi 3:486fb90dc7d5 531 memset(top8bit, 0, sizeof(int) * motor_count);
Yajirushi 3:486fb90dc7d5 532 for(int i=0, send8c=0; i<motor_count; i++){
Yajirushi 3:486fb90dc7d5 533 send8c = (bitLens[i] - 1) >> 3;
Yajirushi 3:486fb90dc7d5 534 top8bit[i] = bitLens[i] - (send8c << 3);
Yajirushi 3:486fb90dc7d5 535 }
Yajirushi 3:486fb90dc7d5 536
Yajirushi 3:486fb90dc7d5 537 //箱の中に値を入れていく
Yajirushi 3:486fb90dc7d5 538 ++maxSend8;
Yajirushi 3:486fb90dc7d5 539 unsigned char valBox[motor_count * maxSend8];
Yajirushi 3:486fb90dc7d5 540 memset(valBox, 0, sizeof(char) * motor_count * maxSend8);
Yajirushi 3:486fb90dc7d5 541 for(int j=0; j<maxSend8; j++){
Yajirushi 3:486fb90dc7d5 542 for(int i=0, slide=j*maxSend8; i<motor_count; i++){
Yajirushi 3:486fb90dc7d5 543 bitLens[i] -= (j==0)? top8bit[i] : 8;
Yajirushi 3:486fb90dc7d5 544 valBox[i+slide] = (bitLens[i]<0)? CMD_NOP : (unsigned char)(Queue[i+1].val >> bitLens[i]);
Yajirushi 3:486fb90dc7d5 545 }
Yajirushi 3:486fb90dc7d5 546 }
Yajirushi 3:486fb90dc7d5 547
Yajirushi 3:486fb90dc7d5 548 //値の送出
Yajirushi 3:486fb90dc7d5 549 for(int j=0; j<maxSend8; j++){
Yajirushi 3:486fb90dc7d5 550 CS_ACTIVE;
Yajirushi 3:486fb90dc7d5 551 for(int i=0, slide=0; i<motor_count; i++){
Yajirushi 4:4127dd195311 552 slide = j*maxSend8;
Yajirushi 4:4127dd195311 553 spi->write(valBox[i+slide]);
Yajirushi 3:486fb90dc7d5 554 #ifdef DEBUG_L6470SDC
Yajirushi 4:4127dd195311 555 if(hasSerial) pc->printf("[Queue:Values]>>>> Motor-No.%03d, VAL[%d]:0x%02X\r\n", i+1, j, valBox[i+slide]);
Yajirushi 3:486fb90dc7d5 556 #endif
Yajirushi 3:486fb90dc7d5 557 }
Yajirushi 3:486fb90dc7d5 558 CS_INACTIVE;
Yajirushi 3:486fb90dc7d5 559 }
Yajirushi 3:486fb90dc7d5 560
Yajirushi 3:486fb90dc7d5 561 //キューの値を空に
Yajirushi 3:486fb90dc7d5 562 if(finallyClearQueue) Qclear();
Yajirushi 3:486fb90dc7d5 563
Yajirushi 3:486fb90dc7d5 564 return 1;
Yajirushi 3:486fb90dc7d5 565 }
Yajirushi 3:486fb90dc7d5 566
Yajirushi 3:486fb90dc7d5 567 /* ---------------------------------------------------------------------------
Yajirushi 3:486fb90dc7d5 568 * CALCULATE
Yajirushi 3:486fb90dc7d5 569 -------------------------------------------------------------------------- */
Yajirushi 0:e1964b6e160c 570 unsigned long L6470SDC::calcSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 571 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 572 //SPEED = ステップ毎秒 * 250ナノ秒 / 2^-28
Yajirushi 0:e1964b6e160c 573 //250ナノ秒 / 2^-28 = 2.5e-7 / 2^-28 = 67.108864
Yajirushi 1:db64ad30b4b3 574 unsigned long ret = stepPerSecond * 67.108864;
Yajirushi 0:e1964b6e160c 575 return (ret > 1048575) ? 1048575 : ret;
Yajirushi 0:e1964b6e160c 576 }
Yajirushi 0:e1964b6e160c 577
Yajirushi 0:e1964b6e160c 578 unsigned short L6470SDC::calcAcc(float stepPerSecond_2){
Yajirushi 0:e1964b6e160c 579 stepPerSecond_2 = (stepPerSecond_2 < 0) ? 0 : stepPerSecond_2;
Yajirushi 0:e1964b6e160c 580 //ACC = ステップ毎秒毎秒 * 250ナノ秒^2 / 2^-40
Yajirushi 0:e1964b6e160c 581 //250ナノ秒^2 / 2^-40 = (2.5e-7)^2 / 2^-40 = 0.0687194
Yajirushi 1:db64ad30b4b3 582 unsigned short ret = stepPerSecond_2 * 0.0687194;
Yajirushi 0:e1964b6e160c 583 return (ret > 4095) ? 4095 : ret;
Yajirushi 0:e1964b6e160c 584 }
Yajirushi 0:e1964b6e160c 585
Yajirushi 0:e1964b6e160c 586 unsigned short L6470SDC::calcDec(float stepPerSecond_2){
Yajirushi 0:e1964b6e160c 587 stepPerSecond_2 = (stepPerSecond_2 < 0) ? 0 : stepPerSecond_2;
Yajirushi 0:e1964b6e160c 588 //DEC = ステップ毎秒毎秒 * 250ナノ秒^2 / 2^-40
Yajirushi 0:e1964b6e160c 589 //250ナノ秒^2 / 2^-40 = (2.5e-7)^2 / 2^-40 = 0.0687194
Yajirushi 1:db64ad30b4b3 590 unsigned short ret = stepPerSecond_2 * 0.0687194;
Yajirushi 0:e1964b6e160c 591 return (ret > 4095) ? 4095 : ret;
Yajirushi 0:e1964b6e160c 592 }
Yajirushi 0:e1964b6e160c 593
Yajirushi 0:e1964b6e160c 594 unsigned short L6470SDC::calcMaxSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 595 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 596 //MAXSPEED = ステップ毎秒 * 250ナノ秒 / 2^-18
Yajirushi 0:e1964b6e160c 597 //250ナノ秒 / 2^-18 = 2.5e-7 / 2^-18 = 0.065536
Yajirushi 1:db64ad30b4b3 598 unsigned long ret = stepPerSecond * 0.065536;
Yajirushi 0:e1964b6e160c 599 return (ret > 1023) ? 1023 : ret;
Yajirushi 0:e1964b6e160c 600 }
Yajirushi 0:e1964b6e160c 601
Yajirushi 0:e1964b6e160c 602 unsigned short L6470SDC::calcMinSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 603 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 604 //MINSPEED = ステップ毎秒 * 250ナノ秒 / 2^-24
Yajirushi 0:e1964b6e160c 605 //250ナノ秒 / 2^-24 = 2.5e-7 / 2^-24 = 4.194304
Yajirushi 1:db64ad30b4b3 606 unsigned long ret = stepPerSecond * 4.194304;
Yajirushi 0:e1964b6e160c 607 return (ret > 4094) ? 4094 : ret;
Yajirushi 0:e1964b6e160c 608 }
Yajirushi 0:e1964b6e160c 609
Yajirushi 0:e1964b6e160c 610 unsigned short L6470SDC::calcIntSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 611 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 612 //INT_SPEED = ステップ毎秒 * 250ナノ秒 / 2^-24
Yajirushi 0:e1964b6e160c 613 //250ナノ秒 / 2^-24 = 2.5e-7 / 2^-24 = 4.194304
Yajirushi 1:db64ad30b4b3 614 unsigned long ret = stepPerSecond * 4.194304;
Yajirushi 0:e1964b6e160c 615 return (ret > 16382) ? 16382 : ret;
Yajirushi 0:e1964b6e160c 616 }
Yajirushi 0:e1964b6e160c 617
Yajirushi 0:e1964b6e160c 618 unsigned short L6470SDC::calcFullStepSpd(float stepPerSecond){
Yajirushi 0:e1964b6e160c 619 stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
Yajirushi 0:e1964b6e160c 620 //FS_SPEED = (ステップ毎秒 * 250ナノ秒 / 2^-18) - 0.5
Yajirushi 0:e1964b6e160c 621 //250ナノ秒 / 2^-18 = 2.5e-7 / 2^-18 = 0.065536
Yajirushi 1:db64ad30b4b3 622 unsigned long ret = stepPerSecond * 0.065536 - 0.5;
Yajirushi 0:e1964b6e160c 623 return (ret > 1023) ? 1023 : ret;
Yajirushi 0:e1964b6e160c 624 }
Yajirushi 0:e1964b6e160c 625
Yajirushi 0:e1964b6e160c 626 // SET method ------------------------------------------------------------------------------
Yajirushi 0:e1964b6e160c 627 void L6470SDC::setAbsPosition(int motorNumber, unsigned long value){
Yajirushi 0:e1964b6e160c 628 setParam(motorNumber, REG_ABS_POS, value, 22);
Yajirushi 0:e1964b6e160c 629 }
Yajirushi 0:e1964b6e160c 630
Yajirushi 0:e1964b6e160c 631 void L6470SDC::setElecPosition(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 632 setParam(motorNumber, REG_EL_POS, value, 9);
Yajirushi 0:e1964b6e160c 633 }
Yajirushi 0:e1964b6e160c 634
Yajirushi 0:e1964b6e160c 635 void L6470SDC::setMarkPosition(int motorNumber, unsigned long value){
Yajirushi 0:e1964b6e160c 636 setParam(motorNumber, REG_MARK, value, 22);
Yajirushi 0:e1964b6e160c 637 }
Yajirushi 0:e1964b6e160c 638
Yajirushi 0:e1964b6e160c 639 void L6470SDC::setAcceleration(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 640 setParam(motorNumber, REG_ACC, value, 12);
Yajirushi 0:e1964b6e160c 641 }
Yajirushi 0:e1964b6e160c 642
Yajirushi 0:e1964b6e160c 643 void L6470SDC::setDeceleration(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 644 setParam(motorNumber, REG_DEC, value, 12);
Yajirushi 0:e1964b6e160c 645 }
Yajirushi 0:e1964b6e160c 646
Yajirushi 0:e1964b6e160c 647 void L6470SDC::setMaximumSpeed(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 648 setParam(motorNumber, REG_MAX_SPEED, value, 10);
Yajirushi 0:e1964b6e160c 649 }
Yajirushi 0:e1964b6e160c 650
Yajirushi 0:e1964b6e160c 651 void L6470SDC::setMinimumSpeed(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 652 setParam(motorNumber, REG_MIN_SPEED, value, 13);
Yajirushi 0:e1964b6e160c 653 }
Yajirushi 0:e1964b6e160c 654
Yajirushi 0:e1964b6e160c 655 void L6470SDC::setHoldingKVAL(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 656 setParam(motorNumber, REG_KVAL_HOLD, value, 8);
Yajirushi 0:e1964b6e160c 657 }
Yajirushi 0:e1964b6e160c 658
Yajirushi 0:e1964b6e160c 659 void L6470SDC::setRunningKVAL(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 660 setParam(motorNumber, REG_KVAL_RUN, value, 8);
Yajirushi 0:e1964b6e160c 661 }
Yajirushi 0:e1964b6e160c 662
Yajirushi 0:e1964b6e160c 663 void L6470SDC::setAccelerationKVAL(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 664 setParam(motorNumber, REG_KVAL_ACC, value, 8);
Yajirushi 0:e1964b6e160c 665 }
Yajirushi 0:e1964b6e160c 666
Yajirushi 0:e1964b6e160c 667 void L6470SDC::setDecelerationKVAL(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 668 setParam(motorNumber, REG_KVAL_DEC, value, 8);
Yajirushi 0:e1964b6e160c 669 }
Yajirushi 0:e1964b6e160c 670
Yajirushi 0:e1964b6e160c 671 void L6470SDC::setKVAL(int motorNumber, unsigned char holdVal, unsigned char runVal, unsigned char accVal, unsigned char decVal){
Yajirushi 0:e1964b6e160c 672 setParam(motorNumber, REG_KVAL_HOLD, holdVal, 8);
Yajirushi 0:e1964b6e160c 673 setParam(motorNumber, REG_KVAL_RUN, runVal, 8);
Yajirushi 0:e1964b6e160c 674 setParam(motorNumber, REG_KVAL_ACC, accVal, 8);
Yajirushi 0:e1964b6e160c 675 setParam(motorNumber, REG_KVAL_DEC, decVal, 8);
Yajirushi 0:e1964b6e160c 676 }
Yajirushi 0:e1964b6e160c 677
Yajirushi 0:e1964b6e160c 678 void L6470SDC::setInterpolateSpeed(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 679 setParam(motorNumber, REG_INT_SPD, value, 14);
Yajirushi 0:e1964b6e160c 680 }
Yajirushi 0:e1964b6e160c 681
Yajirushi 0:e1964b6e160c 682 void L6470SDC::setInterpolateSlope(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 683 setParam(motorNumber, REG_ST_SLP, value, 8);
Yajirushi 0:e1964b6e160c 684 }
Yajirushi 0:e1964b6e160c 685
Yajirushi 0:e1964b6e160c 686 void L6470SDC::setAccSlopeFinal(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 687 setParam(motorNumber, REG_FN_SLP_ACC, value, 8);
Yajirushi 0:e1964b6e160c 688 }
Yajirushi 0:e1964b6e160c 689
Yajirushi 0:e1964b6e160c 690 void L6470SDC::setDecSlopeFinal(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 691 setParam(motorNumber, REG_FN_SLP_DEC, value, 8);
Yajirushi 0:e1964b6e160c 692 }
Yajirushi 0:e1964b6e160c 693
Yajirushi 0:e1964b6e160c 694 void L6470SDC::setThermoCorrect(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 695 setParam(motorNumber, REG_K_THERM, value, 4);
Yajirushi 0:e1964b6e160c 696 }
Yajirushi 0:e1964b6e160c 697
Yajirushi 0:e1964b6e160c 698 void L6470SDC::setOCThreshold(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 699 setParam(motorNumber, REG_OCD_TH, value, 4);
Yajirushi 0:e1964b6e160c 700 }
Yajirushi 0:e1964b6e160c 701
Yajirushi 0:e1964b6e160c 702 void L6470SDC::setStallThreshold(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 703 setParam(motorNumber, REG_STALL_TH, value, 7);
Yajirushi 0:e1964b6e160c 704 }
Yajirushi 0:e1964b6e160c 705
Yajirushi 0:e1964b6e160c 706 void L6470SDC::setFSSpeed(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 707 setParam(motorNumber, REG_FS_SPD, value, 10);
Yajirushi 0:e1964b6e160c 708 }
Yajirushi 0:e1964b6e160c 709
Yajirushi 0:e1964b6e160c 710 void L6470SDC::setStepMode(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 711 setParam(motorNumber, REG_STEP_MODE, value, 8);
Yajirushi 0:e1964b6e160c 712 }
Yajirushi 0:e1964b6e160c 713
Yajirushi 0:e1964b6e160c 714 void L6470SDC::setAlermEnable(int motorNumber, unsigned char value){
Yajirushi 0:e1964b6e160c 715 setParam(motorNumber, REG_ALARM_EN, value, 8);
Yajirushi 0:e1964b6e160c 716 }
Yajirushi 0:e1964b6e160c 717
Yajirushi 0:e1964b6e160c 718 void L6470SDC::setSystemConfig(int motorNumber, unsigned short value){
Yajirushi 0:e1964b6e160c 719 setParam(motorNumber, REG_CONFIG, value, 16);
Yajirushi 0:e1964b6e160c 720 }
Yajirushi 0:e1964b6e160c 721
Yajirushi 0:e1964b6e160c 722 // GET method ------------------------------------------------------------------------------
Yajirushi 0:e1964b6e160c 723 unsigned long L6470SDC::getSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 724 return getParam(motorNumber, REG_SPEED, 20);
Yajirushi 0:e1964b6e160c 725 }
Yajirushi 0:e1964b6e160c 726
Yajirushi 0:e1964b6e160c 727 unsigned short L6470SDC::getADC(int motorNumber){
Yajirushi 0:e1964b6e160c 728 return (unsigned short) getParam(motorNumber, REG_ADC_OUT, 5);
Yajirushi 0:e1964b6e160c 729 }
Yajirushi 0:e1964b6e160c 730
Yajirushi 0:e1964b6e160c 731 unsigned short L6470SDC::getStatus(int motorNumber){
Yajirushi 0:e1964b6e160c 732 return (unsigned short) getParam(motorNumber, REG_STATUS, 16);
Yajirushi 0:e1964b6e160c 733 }
Yajirushi 0:e1964b6e160c 734
Yajirushi 0:e1964b6e160c 735 unsigned long L6470SDC::getAbsPosition(int motorNumber){
Yajirushi 0:e1964b6e160c 736 return getParam(motorNumber, REG_ABS_POS, 22);
Yajirushi 0:e1964b6e160c 737 }
Yajirushi 0:e1964b6e160c 738
Yajirushi 0:e1964b6e160c 739 unsigned short L6470SDC::getElecPosition(int motorNumber){
Yajirushi 0:e1964b6e160c 740 return (unsigned short) getParam(motorNumber, REG_EL_POS, 9);
Yajirushi 0:e1964b6e160c 741 }
Yajirushi 0:e1964b6e160c 742
Yajirushi 0:e1964b6e160c 743 unsigned long L6470SDC::getMarkPosition(int motorNumber){
Yajirushi 0:e1964b6e160c 744 return getParam(motorNumber, REG_MARK, 22);
Yajirushi 0:e1964b6e160c 745 }
Yajirushi 0:e1964b6e160c 746
Yajirushi 0:e1964b6e160c 747 unsigned short L6470SDC::getAcceleration(int motorNumber){
Yajirushi 0:e1964b6e160c 748 return (unsigned short) getParam(motorNumber, REG_ACC, 12);
Yajirushi 0:e1964b6e160c 749 }
Yajirushi 0:e1964b6e160c 750
Yajirushi 0:e1964b6e160c 751 unsigned short L6470SDC::getDeceleration(int motorNumber){
Yajirushi 0:e1964b6e160c 752 return (unsigned short) getParam(motorNumber, REG_DEC, 12);
Yajirushi 0:e1964b6e160c 753 }
Yajirushi 0:e1964b6e160c 754
Yajirushi 0:e1964b6e160c 755 unsigned short L6470SDC::getMaximumSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 756 return (unsigned short) getParam(motorNumber, REG_MAX_SPEED, 10);
Yajirushi 0:e1964b6e160c 757 }
Yajirushi 0:e1964b6e160c 758
Yajirushi 0:e1964b6e160c 759 unsigned short L6470SDC::getMinimumSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 760 return (unsigned short) getParam(motorNumber, REG_MIN_SPEED, 13);
Yajirushi 0:e1964b6e160c 761 }
Yajirushi 0:e1964b6e160c 762
Yajirushi 0:e1964b6e160c 763 unsigned char L6470SDC::getHoldingKVAL(int motorNumber){
Yajirushi 0:e1964b6e160c 764 return (unsigned char) getParam(motorNumber, REG_KVAL_HOLD, 8);
Yajirushi 0:e1964b6e160c 765 }
Yajirushi 0:e1964b6e160c 766
Yajirushi 0:e1964b6e160c 767 unsigned char L6470SDC::getRunningKVAL(int motorNumber){
Yajirushi 0:e1964b6e160c 768 return (unsigned char) getParam(motorNumber, REG_KVAL_RUN, 8);
Yajirushi 0:e1964b6e160c 769 }
Yajirushi 0:e1964b6e160c 770
Yajirushi 0:e1964b6e160c 771 unsigned char L6470SDC::getAccelerationKVAL(int motorNumber){
Yajirushi 0:e1964b6e160c 772 return (unsigned char) getParam(motorNumber, REG_KVAL_ACC, 8);
Yajirushi 0:e1964b6e160c 773 }
Yajirushi 0:e1964b6e160c 774
Yajirushi 0:e1964b6e160c 775 unsigned char L6470SDC::getDecelerationKVAL(int motorNumber){
Yajirushi 0:e1964b6e160c 776 return (unsigned char) getParam(motorNumber, REG_KVAL_DEC, 8);
Yajirushi 0:e1964b6e160c 777 }
Yajirushi 0:e1964b6e160c 778
Yajirushi 0:e1964b6e160c 779 unsigned short L6470SDC::getInterpolateSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 780 return (unsigned short) getParam(motorNumber, REG_INT_SPD, 14);
Yajirushi 0:e1964b6e160c 781 }
Yajirushi 0:e1964b6e160c 782
Yajirushi 0:e1964b6e160c 783 unsigned char L6470SDC::getInterpolateSlope(int motorNumber){
Yajirushi 0:e1964b6e160c 784 return (unsigned char) getParam(motorNumber, REG_ST_SLP, 8);
Yajirushi 0:e1964b6e160c 785 }
Yajirushi 0:e1964b6e160c 786
Yajirushi 0:e1964b6e160c 787 unsigned char L6470SDC::getAccSlopeFinal(int motorNumber){
Yajirushi 0:e1964b6e160c 788 return (unsigned char) getParam(motorNumber, REG_FN_SLP_ACC, 8);
Yajirushi 0:e1964b6e160c 789 }
Yajirushi 0:e1964b6e160c 790
Yajirushi 0:e1964b6e160c 791 unsigned char L6470SDC::getDecSlopeFinal(int motorNumber){
Yajirushi 0:e1964b6e160c 792 return (unsigned char) getParam(motorNumber, REG_FN_SLP_DEC, 8);
Yajirushi 0:e1964b6e160c 793 }
Yajirushi 0:e1964b6e160c 794
Yajirushi 0:e1964b6e160c 795 unsigned char L6470SDC::getThermoCorrect(int motorNumber){
Yajirushi 0:e1964b6e160c 796 return (unsigned char) getParam(motorNumber, REG_K_THERM, 4);
Yajirushi 0:e1964b6e160c 797 }
Yajirushi 0:e1964b6e160c 798
Yajirushi 0:e1964b6e160c 799 unsigned char L6470SDC::getOCThreshold(int motorNumber){
Yajirushi 0:e1964b6e160c 800 return (unsigned char) getParam(motorNumber, REG_OCD_TH, 4);
Yajirushi 0:e1964b6e160c 801 }
Yajirushi 0:e1964b6e160c 802
Yajirushi 0:e1964b6e160c 803 unsigned char L6470SDC::getStallThreshold(int motorNumber){
Yajirushi 0:e1964b6e160c 804 return (unsigned char) getParam(motorNumber, REG_STALL_TH, 7);
Yajirushi 0:e1964b6e160c 805 }
Yajirushi 0:e1964b6e160c 806
Yajirushi 0:e1964b6e160c 807 unsigned short L6470SDC::getFSSpeed(int motorNumber){
Yajirushi 0:e1964b6e160c 808 return (unsigned short) getParam(motorNumber, REG_FS_SPD, 10);
Yajirushi 0:e1964b6e160c 809 }
Yajirushi 0:e1964b6e160c 810
Yajirushi 0:e1964b6e160c 811 unsigned char L6470SDC::getStepMode(int motorNumber){
Yajirushi 0:e1964b6e160c 812 return (unsigned char) getParam(motorNumber, REG_STEP_MODE, 8);
Yajirushi 0:e1964b6e160c 813 }
Yajirushi 0:e1964b6e160c 814
Yajirushi 0:e1964b6e160c 815 unsigned char L6470SDC::getAlermEnable(int motorNumber){
Yajirushi 0:e1964b6e160c 816 return (unsigned char) getParam(motorNumber, REG_ALARM_EN, 8);
Yajirushi 0:e1964b6e160c 817 }
Yajirushi 0:e1964b6e160c 818
Yajirushi 0:e1964b6e160c 819 unsigned short L6470SDC::getSystemConfig(int motorNumber){
Yajirushi 0:e1964b6e160c 820 return (unsigned short) getParam(motorNumber, REG_CONFIG, 16);
Yajirushi 0:e1964b6e160c 821 }