Diff: L6470SDC.cpp

Revision:

3:486fb90dc7d5

Parent:

1:db64ad30b4b3

Child:

4:4127dd195311

--- a/L6470SDC.cpp	Tue Jun 09 06:10:11 2015 +0000
+++ b/L6470SDC.cpp	Tue Jan 12 11:27:01 2016 +0000
@@ -1,9 +1,11 @@
 #include "L6470SDC.h"
 
-#define CS_ACTIVE       cs->write(0)
-#define CS_INACTIVE     cs->write(1)
+#define CS_ACTIVE cs->write(0)
+#define CS_INACTIVE cs->write(1)
 
-//充実のコンストラクタ
+/* ---------------------------------------------------------------------------
+ * Constructor (override +9)
+ -------------------------------------------------------------------------- */
 L6470SDC::L6470SDC(PinName tx, PinName rx, PinName mosi, PinName miso, PinName sclk, PinName csel){
     pc = new Serial(tx, rx);
     spi = new SPI(mosi, miso, sclk);
@@ -61,6 +63,19 @@
     cs = csel;
 }
 
+/* ---------------------------------------------------------------------------
+ * Destructor
+ -------------------------------------------------------------------------- */
+L6470SDC::~L6470SDC(){
+    delete[] Queue;
+    delete pc;
+    delete spi;
+    delete cs;
+}
+
+/* ---------------------------------------------------------------------------
+ * initializer
+ -------------------------------------------------------------------------- */
 void L6470SDC::init(bool initSPI){
     //SPI初期化
     if(initSPI){
@@ -82,6 +97,12 @@
         motor_count++;
     }while(rsv != 0xEE);
     CS_INACTIVE;
+    
+    //モーター個数分のキューを作成(要素番号とモーター番号を合わせるため+1している)
+    Queue = new L6470CMDQ[motor_count+1];
+
+    //キューをクリア
+    Qclear();
 
     //全てのモーターに空コマンドを送り、リセットをかける
     CS_ACTIVE;
@@ -101,10 +122,16 @@
     }
 }
 
+/* ---------------------------------------------------------------------------
+ * GET motor count
+ -------------------------------------------------------------------------- */
 int L6470SDC::getMotorCount(){
     return motor_count;
 }
 
+/* ---------------------------------------------------------------------------
+ * PRIVATE: command send to L6470
+ -------------------------------------------------------------------------- */
 void L6470SDC::setCmd(int motorNumber, unsigned char cmdAddress, unsigned long value, int bitLen){
 #ifdef DEBUG_L6470SDC
     if(hasSerial) pc->printf("[Command] Motor-No.%03d, CMD:0x%04X, VAL:0x%06X\r\n", motorNumber, cmdAddress, value);
@@ -141,6 +168,9 @@
     }
 }
 
+/* ---------------------------------------------------------------------------
+ * PRIVATE: parameter send to L6470
+ -------------------------------------------------------------------------- */
 void L6470SDC::setParam(int motorNumber, unsigned char regAddress, unsigned long value, int bitLen){
 #ifdef DEBUG_L6470SDC
     if(hasSerial) pc->printf("[SetParam] Motor-No.%03d, SetParam: REG:0x%04X, VAL:0x%06X\r\n", motorNumber, regAddress, value);
@@ -174,6 +204,9 @@
     }
 }
 
+/* ---------------------------------------------------------------------------
+ * PRIVATE: parameter get from L6470
+ -------------------------------------------------------------------------- */
 unsigned long L6470SDC::getParam(int motorNumber, unsigned char regAddress, int bitLen){
     //戻り値
     unsigned long ret = 0;
@@ -213,87 +246,327 @@
     return ret;
 }
 
-// cmd method ------------------------------------------------------------------------------
+/* ---------------------------------------------------------------------------
+ * COMMAND
+ -------------------------------------------------------------------------- */
+//@ GET busyflag
 bool L6470SDC::isBusy(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return false;
     return !((getStatus(motorNumber) & 0x02) >> 1);
 }
-
+//@ motor run
 void L6470SDC::run(int motorNumber, unsigned long hex_speed, bool isClockwise){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, (isClockwise) ? CMD_RUN_MINUS : CMD_RUN_PLUS, hex_speed, 20);
 }
-
+//@ motor step
 void L6470SDC::step(int motorNumber, unsigned int count, bool isClockwise){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     for(;count<=0;count--){
         while(isBusy(motorNumber));
         setCmd(motorNumber, (isClockwise) ? CMD_STEP_MINUS : CMD_STEP_PLUS, 0, 0);
     }
 }
-
+//@ motor move
 void L6470SDC::move(int motorNumber, unsigned long stepsCount, bool isClockwise){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, (isClockwise) ? CMD_ADDSTEP_MINUS : CMD_ADDSTEP_PLUS, stepsCount, 22);
 }
-
+//@ motor goto ABSpos(shortest path)
 void L6470SDC::goto1(int motorNumber, unsigned long ABSPos){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_GOTO, ABSPos, 22);
 }
-
+//@ motor goto ABSpos
 void L6470SDC::goto2(int motorNumber, unsigned long ABSPos, bool isClockwise){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, (isClockwise) ? CMD_GOTO_DIR_MINUS : CMD_GOTO_DIR_PLUS, ABSPos, 22);
 }
-
+//@ motor goto mark
 void L6470SDC::goUntil(int motorNumber, unsigned long hex_speed, bool isClockwise, bool setMark){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     unsigned short cmdAddr = (isClockwise) ? CMD_GO_UNTIL_MINUS : CMD_GO_UNTIL_PLUS;
     setCmd(motorNumber, (setMark) ? (cmdAddr | 0x08) : cmdAddr, hex_speed, 22);
 }
-
+//@ release sw
 void L6470SDC::releaseSwitch(int motorNumber, bool isClockwise, bool setMark){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     unsigned short cmdAddr = (isClockwise) ? CMD_RELEASE_SW_MINUS : CMD_RELEASE_SW_PLUS;
     setCmd(motorNumber, (setMark) ? (cmdAddr | 0x08) : cmdAddr, 0, 0);
 }
-
+//@ motor goto zero-position
 void L6470SDC::home(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_GO_HOME, 0, 0);
 }
-
-//zeroはhomeのエイリアス(別名)
+//@ alias:home
 void L6470SDC::zero(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_GO_HOME, 0, 0);
 }
-
+//@ motor goto MARK-position
 void L6470SDC::gotoMark(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_GO_MARK, 0, 0);
 }
-
+//@ reset zero-position
 void L6470SDC::resetHome(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_RESET_POS, 0, 0);
 }
-
-//resetZeroはresetHomeのエイリアス(別名)
+//@ alias:resetHome
 void L6470SDC::resetZero(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_RESET_POS, 0, 0);
 }
-
+//@ motor reset
 void L6470SDC::motorReset(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_RESET_DEVICE, 0, 0);
 }
-
+//@ motor soft-stop
 void L6470SDC::stop(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_SOFT_STOP, 0, 0);
 }
-
+//@ motor hard-stop
 void L6470SDC::stopImmidiate(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_HARD_STOP, 0, 0);
 }
-
+//@ motor HighImpedance
 void L6470SDC::stop_HighImpedance(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_SOFT_HIZ, 0, 0);
 }
-
+//@ motor HighImpedance hard-stop
 void L6470SDC::stopImmidiate_HighImpedance(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return;
     setCmd(motorNumber, CMD_HARD_HIZ, 0, 0);
 }
 
-// calc method ------------------------------------------------------------------------------
+/* ---------------------------------------------------------------------------
+ * COMMAND (Daisy-chain Enqueue)
+ -------------------------------------------------------------------------- */
+//@ enqueue NOP
+int L6470SDC::ENQ_nop(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_NOP;
+    Queue[motorNumber].val = CMD_NOP;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue run command
+int L6470SDC::ENQ_run(int motorNumber, unsigned long hex_speed, bool isClockwise){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = (isClockwise) ? CMD_RUN_MINUS : CMD_RUN_PLUS;
+    Queue[motorNumber].val = hex_speed;
+    Queue[motorNumber].bitLen = 20;
+    return motorNumber;
+}
+//@ enqueue move command
+int L6470SDC::ENQ_move(int motorNumber, unsigned long stepsCount, bool isClockwise){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = (isClockwise) ? CMD_ADDSTEP_MINUS : CMD_ADDSTEP_PLUS;
+    Queue[motorNumber].val = stepsCount;
+    Queue[motorNumber].bitLen = 22;
+    return motorNumber;
+}
+//@ enqueue goto(shortest) command
+int L6470SDC::ENQ_goto1(int motorNumber, unsigned long ABSPos){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_GOTO;
+    Queue[motorNumber].val = ABSPos;
+    Queue[motorNumber].bitLen = 22;
+    return motorNumber;
+}
+//@ enqueue goto command
+int L6470SDC::ENQ_goto2(int motorNumber, unsigned long ABSPos, bool isClockwise){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = (isClockwise) ? CMD_GOTO_DIR_MINUS : CMD_GOTO_DIR_PLUS;
+    Queue[motorNumber].val = ABSPos;
+    Queue[motorNumber].bitLen = 22;
+    return motorNumber;
+}
+//@ enqueue goUntil command
+int L6470SDC::ENQ_goUntil(int motorNumber, unsigned long hex_speed, bool isClockwise, bool setMark){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    unsigned short cmdAddr = (isClockwise) ? CMD_GO_UNTIL_MINUS : CMD_GO_UNTIL_PLUS;
+    Queue[motorNumber].addr = (setMark)? (cmdAddr | 0x08) : cmdAddr;
+    Queue[motorNumber].val = hex_speed;
+    Queue[motorNumber].bitLen = 22;
+    return motorNumber;
+}
+//@ enqueue release switch command
+int L6470SDC::ENQ_releaseSwitch(int motorNumber, bool isClockwise, bool setMark){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    unsigned short cmdAddr = (isClockwise) ? CMD_RELEASE_SW_MINUS : CMD_RELEASE_SW_PLUS;
+    Queue[motorNumber].addr = (setMark)? (cmdAddr | 0x08) : cmdAddr;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue goHome command
+int L6470SDC::ENQ_home(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_GO_HOME;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ alias:ENQ_home
+int L6470SDC::ENQ_zero(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_GO_HOME;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue gotoMark command
+int L6470SDC::ENQ_gotoMark(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_GO_MARK;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue reset_pos command
+int L6470SDC::ENQ_resetHome(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_RESET_POS;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ alias:ENQ_resetHome
+int L6470SDC::ENQ_resetZero(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_RESET_POS;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue reset motor command
+int L6470SDC::ENQ_motorReset(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_RESET_DEVICE;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue soft-stop command
+int L6470SDC::ENQ_stop(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_SOFT_STOP;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue hard-stop command
+int L6470SDC::ENQ_stopImmidiate(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_HARD_STOP;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue soft-stop(HIZ) command
+int L6470SDC::ENQ_stop_HighImpedance(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_SOFT_HIZ;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ enqueue hard-stop(HIZ) command
+int L6470SDC::ENQ_stopImmidiate_HighImpedance(int motorNumber){
+    if(motorNumber > motor_count || motorNumber < 1) return -1;
+    Queue[motorNumber].addr = CMD_HARD_HIZ;
+    Queue[motorNumber].val = 0;
+    Queue[motorNumber].bitLen = 0;
+    return motorNumber;
+}
+//@ command queue clear
+void L6470SDC::Qclear(){
+    for(int i=0; i<=motor_count; i++){
+        Queue[i].addr = 0;
+        Queue[i].val = 0;
+        Queue[i].bitLen = -1;
+    }
+}
+//@ queue execute
+int L6470SDC::Qexec(bool finallyClearQueue){
+    int bitLens[motor_count];
+    memset(bitLens, 0, sizeof(int) * motor_count);
+    int maxBitLen = 0;
+
+    //構造体の中のbitLenの最大値を抽出, 計算に使用するので別枠に格納
+    for(int i=0; i<motor_count; i++){
+        bitLens[i] = Queue[i+1].bitLen;
+        if(maxBitLen < bitLens[i]) maxBitLen = bitLens[i];
+    }
+
+    //bitLenの最大値が0未満、つまり構造体に値が入っていないのでここで終わる
+    if(maxBitLen < 0) return -1;
+
+    //コマンド送信
+    CS_ACTIVE;
+    for(int i=0; i<motor_count; i++){
+        spi->write(Queue[i+1].addr);
+#ifdef DEBUG_L6470SDC
+        if(hasSerial) pc->printf("[Queue:Command] Motor-No.%03d, CMD:0x%04X\r\n", i+1, Queue[i+1].addr);
+#endif
+    }
+    CS_INACTIVE;
+
+    //bitLenの最大値が0、つまり送る値がないのでここで終わる
+    if(maxBitLen < 1) return 0;
+
+    //モーター個数分×送信回数分の箱を作り、その中にあらかじめ値を入れ、それを順次送り出す
+    //bitLenが最も長いものから送信回数を求める
+    int maxSend8 = (maxBitLen - 1) >> 3;
+
+    //8bitに区切った場合の区切りの場所を算出
+    int top8bit[motor_count];
+    memset(top8bit, 0, sizeof(int) * motor_count);
+    for(int i=0, send8c=0; i<motor_count; i++){
+        send8c = (bitLens[i] - 1) >> 3;
+        top8bit[i] = bitLens[i] - (send8c << 3);
+    }
+
+    //箱の中に値を入れていく
+    ++maxSend8;
+    unsigned char valBox[motor_count * maxSend8];
+    memset(valBox, 0, sizeof(char) * motor_count * maxSend8);
+    for(int j=0; j<maxSend8; j++){
+        for(int i=0, slide=j*maxSend8; i<motor_count; i++){
+            bitLens[i] -= (j==0)? top8bit[i] : 8;
+            valBox[i+slide] = (bitLens[i]<0)? CMD_NOP : (unsigned char)(Queue[i+1].val >> bitLens[i]);
+        }
+    }
+
+    //値の送出
+    for(int j=0; j<maxSend8; j++){
+        CS_ACTIVE;
+        for(int i=0, slide=0; i<motor_count; i++){
+            slide = i*maxSend8;
+            spi->write(valBox[j+slide]);
+#ifdef DEBUG_L6470SDC
+            if(hasSerial) pc->printf("[Queue:Values]>>>> Motor-No.%03d, VAL[%d]:0x%02X\r\n", i+1, j, valBox[j+slide]);
+#endif
+        }
+        CS_INACTIVE;
+    }
+
+    //キューの値を空に
+    if(finallyClearQueue) Qclear();
+
+    return 1;
+}
+
+/* ---------------------------------------------------------------------------
+ * CALCULATE
+ -------------------------------------------------------------------------- */
 unsigned long L6470SDC::calcSpd(float stepPerSecond){
     stepPerSecond = (stepPerSecond < 0) ? 0 : stepPerSecond;
     //SPEED = ステップ毎秒 * 250ナノ秒 / 2^-28