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?
User | Revision | Line number | New 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 | } |