File content as of revision 8:2e68b9de685d:

#include "mbed.h"
#include "AT24C1024.h"
#include "AccelStepper.h"
#include "DMX.h"

//モータ制御基板の通し番号を設定してください　　初期設定は１です　０は使わないでね。
#define BoardNumber 1 

//ステップ数設定　1:DRV full, 2:DRV 1/2, 3:DRV 1/4, 4:DRV 1/8, 5:DRV 1/16, 6:DRV 1/32, 7:TMC StealthChop µ16
#define StepSize1 7
#define StepSize2 7


Ticker to;

Timer t;
DigitalOut dmxEnable(D8);   //基本L（レシーブ）に固定します
DMX dmx(PA_9, PA_10);

I2C         i2c(D4, D5);      // SDA, SCL
AT24C1024   at24c1024(i2c);     // Atmel 1Mbit EE-PROM
const int addr = 0x70;

AccelStepper stepper1(1, D9, D6);   // step, direction
AccelStepper stepper2(1, D10, D7);  // step, direction

//モーター１ CN0とCN1を使用
DigitalOut stepper1Enable(D2);
DigitalOut stepper1MS1(D11);
DigitalOut stepper1MS2(D12);
DigitalOut stepper1MS3(A4);

//モーター２ CN2とCN3を使用
DigitalOut stepper2Enable(D3);
DigitalOut stepper2MS1(A5);
DigitalOut stepper2MS2(A6);
DigitalOut stepper2MS3(A7);

DigitalIn EndSW1(A0); //CN4 //３端子エンドスイッチ１  予備
DigitalIn EndSW2(A1); //CN5 //３端子エンドスイッチ２  予備
DigitalIn EndSW3(A2); //CN6 //２端子エンドスイッチ１　モーター１用にする
DigitalIn EndSW4(A3); //CN7 //２端子エンドスイッチ２　モーター２用にする

DigitalOut myLED(LED3);     //動作確認用LED

unsigned long position1, position2; //モーター１の設定位置
unsigned long max1, max2;
unsigned long speed1, speed2;
unsigned long Acceleration1, Acceleration2;
unsigned long param1, param2;
unsigned long dmxInHighBit, dmxInLowBit;
unsigned long HighBit, LowBit;

bool manipulateMode1 = false; //0:手動 1:自動
bool manipulateMode2 = false; //0:手動 1:自動


//モーター１のパラメーター　DMXチャンネルリスト　チャンネル数：１６個
#define CH_Origin BoardNumber * 32 - 32

#define DMX_Position1_H CH_Origin  //モーター１制御用のDMX信号の始まりの値
#define DMX_Position1_L CH_Origin + 1
#define DMX_Speed1_H CH_Origin + 2
#define DMX_Speed1_L CH_Origin + 3
#define DMX_Acceleration1_H CH_Origin + 4
#define DMX_Acceleration1_L CH_Origin + 5
#define DMX_Max1_H CH_Origin + 6
#define DMX_Max1_L CH_Origin + 7
#define DMX_Offset1_H CH_Origin + 8
#define DMX_Offset1_L CH_Origin + 9
#define DMX_SaveValue1_H CH_Origin + 10
#define DMX_SaveValue1_L CH_Origin + 11
#define DMX_ParmSetMode1_H CH_Origin + 12
#define DMX_ParmSetMode1_L CH_Origin + 13
#define DMX_Initialize1_H CH_Origin + 14
#define DMX_Initialize1_L CH_Origin + 15

//モーター２のパラメーター　DMXチャンネルリスト　チャンネル数：１６個
#define DMX_Position2_H CH_Origin + 16
#define DMX_Position2_L CH_Origin + 17
#define DMX_Speed2_H CH_Origin + 18
#define DMX_Speed2_L CH_Origin + 19
#define DMX_Acceleration2_H CH_Origin + 20
#define DMX_Acceleration2_L CH_Origin + 21
#define DMX_Max2_H CH_Origin + 22
#define DMX_Max2_L CH_Origin + 23
#define DMX_Offset2_H CH_Origin + 24
#define DMX_Offset2_L CH_Origin + 25
#define DMX_SaveValue2_H CH_Origin + 26
#define DMX_SaveValue2_L CH_Origin + 27
#define DMX_ParmSetMode2_H CH_Origin + 28
#define DMX_ParmSetMode2_L CH_Origin + 29
#define DMX_Initialize2_H CH_Origin + 30
#define DMX_Initialize2_L CH_Origin + 31

//モーターの回転方向　TMCは1  DRVは-1に設定すること
//未実装（実装予定）

//ステップ数の調整用の係数
float StepAdj1 = 1;
float StepAdj2 = 1;


//未実装
void sendDataToLPC()
{
    char cmd[2];
    cmd[0] = 0x51;  
    cmd[1] = 0x51;  
    i2c.write(addr, cmd, 2);
    
    //stepper1.newSpeed();
}

//map
long map(long x, long in_min, long in_max, long out_min, long out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void blink()
{
    for(int i=0; i<5; i++) {
        myLED=0;
        wait_ms(60);
        myLED=1;
        wait_ms(60);
    }
}

void checkSW()
{
    //モーター１用エンドスイッチのチェック
    if(EndSW3.read() == 1) {
        stepper1.stop();
        stepper1.setCurrentPosition(0);
    }

    //モーター２用エンドスイッチのチェック
    if(EndSW4.read() == 1) {
        stepper2.stop();
        stepper2.setCurrentPosition(0);
    }
}


//モーター１のみの往復テスト
void testMove1()
{
    //initなしの場合
    stepper1.setCurrentPosition(0);
    stepper1.setMaxSpeed(4000 / StepAdj1);
    stepper1.setAcceleration(20000 / StepAdj1);

    stepper1.setMaxSpeed(180000);
    while(1) {
        stepper1.moveTo(6200);
        //stepper1.moveTo(800 / StepAdj1);
        while(stepper1.distanceToGo() != 0) {
            stepper1.run();
        }
        wait(0.5);
        //stepper1.moveTo(0 / StepAdj1);
        stepper1.moveTo(200);
        while(stepper1.distanceToGo() != 0) {
            stepper1.run();
        }
        wait(0.5);
    }
}


//モーター２個同時の往復テスト
void testMove2()
{
    stepper1.setMaxSpeed(10000 / StepAdj1);
    stepper2.setMaxSpeed(10000 / StepAdj2);

    //stepper1.setMaxSpeed(180000);

    while(1) {
        stepper1.moveTo(20000 / StepAdj1);
        stepper2.moveTo(20000 / StepAdj2);
        while(stepper1.distanceToGo() != 0) {
            stepper1.run();
            stepper2.run();
        }
        while(stepper2.distanceToGo() != 0) {
            stepper1.run();
            stepper2.run();
        }

        stepper1.moveTo(500 / StepAdj1);
        stepper2.moveTo(500 / StepAdj2);
        while(stepper1.distanceToGo() != 0) {
            stepper1.run();
            stepper2.run();
        }
        while(stepper2.distanceToGo() != 0) {
            stepper2.run();
            stepper1.run();
        }
    }
}


void interuptInit1()
{
    //stepper1.setMaxSpeed(1000);
    //stepper1.setAcceleration(5000);
    

    //まずとにかく原点を目指す
    stepper1.moveTo(1);
    while(stepper1.distanceToGo() != 0) {
        stepper1.run();
    }

    //15度程度の時計回り動作
    stepper1.moveTo(200);
    while(stepper1.distanceToGo() != 0) {
        stepper1.run();
    }

    //原点復帰動作、反時計回り
    stepper1.setCurrentPosition(-1);
    stepper1.moveTo(-30000 / StepAdj1);
    while(stepper1.currentPosition() != 0) {
        stepper1.run();
        checkSW();
    }
    //初期位置へ移動
    //something
}

void interuptInit2()
{
    stepper2.setMaxSpeed(4000 / StepAdj2);
    stepper2.setAcceleration(20000 / StepAdj2);

    //まず原点を目指す


    //15度程度の後退動作
    stepper2.setCurrentPosition(1);
    stepper2.moveTo(100);
    while(stepper2.distanceToGo() != 0) {
        stepper2.run();
    }

    //原点復帰動作
    stepper2.setCurrentPosition(-1);
    stepper2.moveTo(-30000 / StepAdj2);
    while(stepper2.currentPosition() != 0) {
        stepper2.run();
        checkSW();
    }
    //初期位置へ移動
    //something
}


void init1()
{
    stepper1.setMaxSpeed(4000 / StepAdj1);
    stepper1.setAcceleration(20000 / StepAdj1);

    //30度程度の後退動作
    stepper1.moveTo(200);
    while(stepper1.distanceToGo() != 0) {
        stepper1.run();
    }
    stepper1.setCurrentPosition(-1);

    //反時計回りに回転、エンドスイッチで停止
    stepper1.moveTo(-30000 / StepAdj1);
    while(stepper1.currentPosition() != 0) {
        stepper1.run();
        checkSW();
    }
//    stepper1.moveTo(500 / StepAdj1);
//    while(stepper1.distanceToGo() != 0) stepper1.run();
}

void init2()
{

    stepper2.setMaxSpeed(4000 / StepAdj2);
    stepper2.setAcceleration(20000 / StepAdj2);

    //３０度程度時計回りに回転動作
    stepper2.moveTo(200);
    while(stepper2.distanceToGo() != 0) {
        stepper2.run();
    }

    stepper2.setCurrentPosition(-1);

    //反時計回りに回転、エンドスイッチで停止
    stepper2.moveTo(-30000 / StepAdj2);
    while(stepper2.currentPosition() != 0) {
        stepper2.run();
        checkSW();
    }
}

void initBoth()
{
    stepper1.setCurrentPosition(-1);
    stepper1.setMaxSpeed(4000 / StepAdj1);
    stepper1.setAcceleration(20000 / StepAdj1);
    stepper1.moveTo(-30000 / StepAdj1);

    stepper2.setCurrentPosition(-1);
    stepper2.setMaxSpeed(4000 / StepAdj2);
    stepper2.setAcceleration(20000 / StepAdj2);
    stepper2.moveTo(-30000 / StepAdj2);

    while((stepper1.distanceToGo() != 0)||(stepper2.distanceToGo() != 0)) {
        if (stepper1.distanceToGo() != 0) stepper1.run();
        if (stepper2.distanceToGo() != 0) stepper2.run();
        checkSW();
    }
    stepper1.moveTo(500 / StepAdj1);
    stepper2.moveTo(500 / StepAdj2);
    while((stepper1.distanceToGo() != 0)||(stepper2.distanceToGo() != 0)) {
        if (stepper1.distanceToGo() != 0) stepper1.run();
        if (stepper2.distanceToGo() != 0) stepper2.run();
    }
    stepper1.setCurrentPosition(-1);
    stepper2.setCurrentPosition(-1);
    
}


void checkSafeBreak()
{
    //モーター１用エンドスイッチのチェック
    if(EndSW3.read() == 1) {
        stepper1.stop();
        while(1) {
            blink();
        }
    }

    //モーター２用エンドスイッチのチェック
    if(EndSW4.read() == 1) {
        stepper2.stop();
        while(1) {
            blink();
        }
    }
}


//モーター１のパラメータ保存
void paramStore1()
{
    //速度
    uint8_t dt = dmx.get(DMX_Speed1_H);
    at24c1024.write(DMX_Speed1_H, dt);
    wait_ms(2);
    dt = dmx.get(DMX_Speed1_L);
    at24c1024.write(DMX_Speed1_L, dt);
    wait_ms(2);

    //加速度
    dt = dmx.get(DMX_Acceleration1_H);
    at24c1024.write(DMX_Acceleration1_H, dt);
    wait_ms(2);
    dt = dmx.get(DMX_Acceleration1_L);
    at24c1024.write(DMX_Acceleration1_L, dt);
    wait_ms(2);

    //最大値
    dt = dmx.get(DMX_Max1_H);
    at24c1024.write(DMX_Max1_H, dt);
    wait_ms(2);
    dt = dmx.get(DMX_Max1_L);
    at24c1024.write(DMX_Max1_L, dt);

    wait_ms(5);
}

//モーター２のパラメータ保存
void paramStore2()
{
    //速度
    uint8_t dt = dmx.get(DMX_Speed2_H);
    at24c1024.write(DMX_Speed2_H, dt);
    wait_ms(2);
    dt = dmx.get(DMX_Speed2_L);
    at24c1024.write(DMX_Speed2_L, dt);
    wait_ms(2);

    //加速度
    dt = dmx.get(DMX_Acceleration2_H);
    at24c1024.write(DMX_Acceleration2_H, dt);
    wait_ms(2);
    dt = dmx.get(DMX_Acceleration2_L);
    at24c1024.write(DMX_Acceleration2_L, dt);
    wait_ms(2);

    //最大値
    dt = dmx.get(DMX_Max2_H);
    at24c1024.write(DMX_Max2_H, dt);
    wait_ms(2);
    dt = dmx.get(DMX_Max2_L);
    at24c1024.write(DMX_Max2_L, dt);

    wait_ms(2);
}


//速度・加速度・最大値・オフセットの値をスライダーから取得
void captureSliderValue()
{
    dmxInHighBit = dmx.get(DMX_Speed1_H);
    dmxInLowBit = dmx.get(DMX_Speed1_L);
    dmxInHighBit = dmxInHighBit << 8;
    speed1 = (long)(dmxInHighBit + dmxInLowBit);

//（動作）
    dmxInHighBit = dmx.get(DMX_Speed2_H);
    dmxInLowBit = dmx.get(DMX_Speed2_L);
    dmxInHighBit = dmxInHighBit << 8;
    speed2 = (long)(dmxInHighBit + dmxInLowBit);

    dmxInHighBit = dmx.get(DMX_Acceleration1_H);
    dmxInLowBit = dmx.get(DMX_Acceleration1_L);
    dmxInHighBit = dmxInHighBit << 8;
    Acceleration1 = (long)(dmxInHighBit + dmxInLowBit);

//（動作）
    dmxInHighBit = dmx.get(DMX_Acceleration2_H);
    dmxInLowBit = dmx.get(DMX_Acceleration2_L);
    dmxInHighBit = dmxInHighBit << 8;
    Acceleration2 = (long)(dmxInHighBit + dmxInLowBit);

    dmxInHighBit = dmx.get(DMX_Max1_H);
    dmxInLowBit = dmx.get(DMX_Max1_L);
    dmxInHighBit = dmxInHighBit << 8;
    max1 = (long)(dmxInHighBit + dmxInLowBit);

//（動作）
    dmxInHighBit = dmx.get(DMX_Max2_H);
    dmxInLowBit = dmx.get(DMX_Max2_L);
    dmxInHighBit = dmxInHighBit << 8;
    max2 = (long)(dmxInHighBit + dmxInLowBit);
}


//モーター１の　速度・加速度・オフセットの値をEEPROMから取得
void readROMValue1()
{
    //速度
    HighBit = at24c1024.read(DMX_Speed1_H);
    LowBit = at24c1024.read(DMX_Speed1_L);
    HighBit = HighBit << 8;
    speed1 = (long)(HighBit + LowBit);

    //加速度
    HighBit = at24c1024.read(DMX_Acceleration1_H);
    LowBit = at24c1024.read(DMX_Acceleration1_L);
    HighBit = HighBit << 8;
    Acceleration1 = (long)(HighBit + LowBit);

    //最大値
    HighBit = at24c1024.read(DMX_Max1_H);
    LowBit = at24c1024.read(DMX_Max1_L);
    HighBit = HighBit << 8;
    max1 = (long)(HighBit + LowBit);
}


//モーター2の　速度・加速度・オフセットの値をEEPROMから取得
void readROMValue2()
{
    //速度
    HighBit = at24c1024.read(DMX_Speed2_H);
    LowBit = at24c1024.read(DMX_Speed2_L);
    HighBit = HighBit << 8;
    speed2 = (long)(HighBit + LowBit);

    //加速度
    HighBit = at24c1024.read(DMX_Acceleration2_H);
    LowBit = at24c1024.read(DMX_Acceleration2_L);
    HighBit = HighBit << 8;
    Acceleration2 = (long)(HighBit + LowBit);

    //最大値
    HighBit = at24c1024.read(DMX_Max2_H);
    LowBit = at24c1024.read(DMX_Max2_L);
    HighBit = HighBit << 8;
    max2 = (long)(HighBit + LowBit);
}


//操作モード切り替えボタンのチェック
void checkManipulateButton()
{
    dmxInHighBit = dmx.get(DMX_ParmSetMode1_H);
    dmxInLowBit = dmx.get(DMX_ParmSetMode1_L);
    if(dmxInHighBit == 102 && dmxInLowBit == 255) {
        manipulateMode1 = !manipulateMode1;
        blink();
    }

    dmxInHighBit = dmx.get(DMX_ParmSetMode2_H);
    dmxInLowBit = dmx.get(DMX_ParmSetMode2_L);
    if(dmxInHighBit == 102 && dmxInLowBit == 255) {
        manipulateMode2 = !manipulateMode2;
        blink();
    }
}

//書き込みボタンのチェック
void checkWriteButton()
{
    dmxInHighBit = dmx.get(DMX_SaveValue1_H);
    dmxInLowBit = dmx.get(DMX_SaveValue1_L);
    dmxInHighBit = dmxInHighBit << 8;
    param1 = (long)(dmxInHighBit + dmxInLowBit);

    dmxInHighBit = dmx.get(DMX_SaveValue2_H);
    dmxInLowBit = dmx.get(DMX_SaveValue2_L);
    dmxInHighBit = dmxInHighBit << 8;
    param2 = (long)(dmxInHighBit + dmxInLowBit);

    if(param1 == 24932) {
        paramStore1();
        blink();
    } else;

    if(param2 == 24932) {
        paramStore2();
        blink();
    } else;
}

//原点復帰命令のチェック
void checkInitCommand1()
{
    dmxInHighBit = dmx.get(DMX_Initialize1_H);
    dmxInLowBit = dmx.get(DMX_Initialize1_L);

    if(dmxInHighBit == 75 && dmxInLowBit == 255) {
        interuptInit1();
    }
}

void checkInitCommand2()
{
    dmxInHighBit = dmx.get(DMX_Initialize2_H);
    dmxInLowBit = dmx.get(DMX_Initialize2_L);

    if(dmxInHighBit == 75 && dmxInLowBit == 255) {
        interuptInit2();
    }
}


//モータードライバの設定
void conf1()
{
    switch (StepSize1) {
        case 1: //DRV8825 full steps
            stepper1MS1 = 0;
            stepper1MS2 = 0;
            stepper1MS3 = 0;
            StepAdj1 = 16;
            break;

        case 2: //DRV8825 1/2 steps
            stepper1MS1 = 1;
            stepper1MS2 = 0;
            stepper1MS3 = 0;
            StepAdj1 = 8;
            break;

        case 3: //DRV8825 1/4 steps
            stepper1MS1 = 0;
            stepper1MS2 = 1;
            stepper1MS3 = 0;
            StepAdj1 = 4;
            break;

        case 4: //DRV8825 1/8 steps
            stepper1MS1 = 1;
            stepper1MS2 = 1;
            stepper1MS3 = 0;
            StepAdj1 = 2;
            break;

        case 5: //DRV8825 1/16 steps
            stepper1MS1 = 0;
            stepper1MS2 = 0;
            stepper1MS3 = 1;
            StepAdj1 = 1;
            break;

        case 6: //DRV8825 1/32 steps
            stepper1MS1 = 1;
            stepper1MS2 = 1;
            stepper1MS3 = 1;
            StepAdj1 = 0.5;
            break;

        case 7: //TMC StealthChop µ16 steps
            DigitalIn stepper1MS1(A5);
            DigitalIn stepper1MS2(A6);
            StepAdj1 = 1;
            break;
    }
    stepper1Enable = 0;
}

void conf2()
{
    switch (StepSize2) {
        case 1: //DRV8825 full steps
            stepper2MS1 = 0;
            stepper2MS2 = 0;
            stepper2MS3 = 0;
            StepAdj2 = 16;
            break;

        case 2: //DRV8825 1/2 steps
            stepper2MS1 = 1;
            stepper2MS2 = 0;
            stepper2MS3 = 0;
            StepAdj2 = 8;
            break;

        case 3: //DRV8825 1/4 steps
            stepper2MS1 = 0;
            stepper2MS2 = 1;
            stepper2MS3 = 0;
            StepAdj2 = 4;
            break;

        case 4: //DRV8825 1/8 steps
            stepper2MS1 = 1;
            stepper2MS2 = 1;
            stepper2MS3 = 0;
            StepAdj2 = 2;
            break;

        case 5: //DRV8825 1/16 steps
            stepper2MS1 = 0;
            stepper2MS2 = 0;
            stepper2MS3 = 1;
            StepAdj2 = 1;
            break;

        case 6: //DRV8825 1/32 steps
            stepper2MS1 = 1;
            stepper2MS2 = 1;
            stepper2MS3 = 1;
            StepAdj2 = 0.5;
            break;

        case 7: //TMC StealthChop µ16 steps
            DigitalIn stepper2MS1(D11);
            DigitalIn stepper2MS2(D12);
            StepAdj2 = 1;
            break;
    }
    stepper2Enable = 0;
}


int main()
{
    wait(1);

//DMXの機能をレシーブに固定　（ハードウェア的に固定するため停止中）
//    dmxEnable = 0;  //0:receiver 1:sender

    conf1();    //motor1 config     ドライバの設定
    conf2();    //motor2 config

    t.start();
    //to.attach(&sendDataToLPC, 1);

    stepper1.setCurrentPosition(-1);
    stepper2.setCurrentPosition(-1);

//モーターの初期化シーケンス
//    init1();
//    init2();
    initBoth();

//往復運動を繰り返すテストシーケンス（無限ループ）
//    testMove2();

    myLED =1;

    readROMValue1();
    readROMValue2();

    stepper1.setMaxSpeed(10000 / StepAdj1);
    stepper2.setMaxSpeed(10000 / StepAdj2);

    while(1) {
        //操作モード切り替えボタンのチェック
        checkManipulateButton();

        //書き込みボタンチェック
        checkWriteButton();

        //モーター１の目標位置をDMXから取得
        dmxInHighBit = dmx.get(DMX_Position1_H);
        dmxInLowBit = dmx.get(DMX_Position1_L);
        dmxInHighBit = dmxInHighBit << 8;
        position1 = (long)(dmxInHighBit + dmxInLowBit);

        //モーター２の目標位置をスライダーから取得（停止中）
        dmxInHighBit = dmx.get(DMX_Position2_H);
        dmxInLowBit = dmx.get(DMX_Position2_L);
        dmxInHighBit = dmxInHighBit << 8;
        position2 = (long)(dmxInHighBit + dmxInLowBit);

        //手動モードの場合、現在のスライダーの値を取得、パラメーターを設定
        if(manipulateMode1 == false) {
            captureSliderValue();
            myLED = 0;
        }

        //自動モードの場合、EEPROMから値を取得、パラメーターを設定
        else {
            myLED = 1;
        }

        //手動モードの場合、現在のスライダーの値を取得、パラメーターを設定（停止中）
//        if(manipulateMode2 == false) {
//            captureSliderValue();
//            myLED = 0;
//        }

        //自動モードの場合、EEPROMから値を取得、パラメータを設定（停止中）
//        else {
//            myLED = 1;
//        }


        //モーター１のパラメーター設定
        position1 = map(position1, 0, 0xFFFF, 0, max1);
        stepper1.setMaxSpeed(speed1);
        stepper1.setAcceleration(Acceleration1);


        //モーター２のパラメーター設定（停止中）
        position2 = map(position2, 0, 0xFFFF, 0, max2);
        stepper2.setMaxSpeed(speed2);
        stepper2.setAcceleration(Acceleration2);


        //モーター１の目標位置設定
        stepper1.moveTo(position1);

        //モーター２の目標位置設定（）
        stepper2.moveTo(position2);


        //安全エンドスイッチの監視（停止中）
        //checkSafeBreak();

        //モーター１への原点復帰命令のチェック
        checkInitCommand1();

        //モーター２への原点復帰命令のチェック（）
        checkInitCommand2();

        //モーター１の走行
        if(stepper1.distanceToGo() != 0) {
            //checkInitCommand1();
            stepper1.run();
        }
        //モーター２の走行
        if(stepper2.distanceToGo() != 0) stepper2.run();
    }
}