test2017-1-13
Dependencies: AT24C1024 DMX-STM32 mbed
main.cpp
- Committer:
- tomo370
- Date:
- 2017-04-03
- Revision:
- 5:fd097a259856
- Parent:
- 4:c3f3fdde7eee
- Child:
- 6:2a9e0228f81d
File content as of revision 5:fd097a259856:
#include "mbed.h" #include "AT24C1024.h" #include "AccelStepper.h" #include "DMX.h" //モータ制御基板の通し番号を設定してください 初期設定は1です #define BoardNumber 1 //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 AccelStepper stepper1(1, D9, D6); // step, direction AccelStepper stepper2(1, D10, D7); // step, direction //モーター1 CN0とCN1を使用 DigitalOut stepper1Enable(D2); DigitalOut stepper1MS1(D11); DigitalOut stepper1MS2(D12); DigitalOut stepper1MS3(A4); //モーター2 CN2とCN3を使用 DigitalOut stepper2Enable(D3); DigitalOut stepper2MS1(A5); DigitalOut stepper2MS2(A6); DigitalOut stepper2MS3(A7); DigitalIn EndSW1(A0); //CN4 //3端子エンドスイッチ1 予備 DigitalIn EndSW2(A1); //CN5 //3端子エンドスイッチ2 予備 DigitalIn EndSW3(A2); //CN6 //2端子エンドスイッチ1 モーター1用にする DigitalIn EndSW4(A3); //CN7 //2端子エンドスイッチ2 モーター2用にする DigitalOut myLED(LED3); //動作確認用LED unsigned long position1, position2; //モーター1の設定位置 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:自動 //モーター1のパラメーター DMXチャンネルリスト チャンネル数:16個 #define CH_Origin BoardNumber * 32 - 32 #define DMX_Position1_H CH_Origin //モーター1制御用の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 //モーター2のパラメーター DMXチャンネルリスト チャンネル数:16個 #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; //ステップ数設定 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 3 #define StepSize2 4 //未実装 void calc() { 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() { //モーター1用エンドスイッチのチェック if(EndSW1.read() == 0) { stepper1.stop(); stepper1.setCurrentPosition(0); } //モーター2用エンドスイッチのチェック if(EndSW2.read() == 0) { stepper2.stop(); stepper2.setCurrentPosition(0); } } //モーター1のみの往復テスト void testMove1() { //initなしの場合 stepper1.setCurrentPosition(0); stepper1.setMaxSpeed(4000 / StepAdj1); stepper1.setAcceleration(20000 / StepAdj1); stepper1.setMaxSpeed(180000); while(1) { stepper1.moveTo(3200); //stepper1.moveTo(800 / StepAdj1); while(stepper1.distanceToGo() != 0) { stepper1.run(); } wait(0.5); //stepper1.moveTo(0 / StepAdj1); stepper1.moveTo(0); while(stepper1.distanceToGo() != 0) { stepper1.run(); } wait(0.5); } } //モーター2個同時の往復テスト 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.setCurrentPosition(-1); stepper1.setMaxSpeed(4000 / StepAdj1); stepper1.setAcceleration(20000 / StepAdj1); //原点復帰動作 stepper1.moveTo(-30000 / StepAdj1); while(stepper1.currentPosition() != 0) { stepper1.run(); checkSW(); } //初期位置へ移動 //something } void interuptInit2() { stepper2.setCurrentPosition(-1); stepper2.setMaxSpeed(4000 / StepAdj2); stepper2.setAcceleration(20000 / StepAdj2); //原点復帰動作 stepper2.moveTo(-30000 / StepAdj2); while(stepper2.currentPosition() != 0) { stepper2.run(); checkSW(); } //初期位置へ移動 //something } void init1() { stepper1.setCurrentPosition(-1); stepper1.setMaxSpeed(4000 / StepAdj1); stepper1.setAcceleration(20000 / StepAdj1); stepper1.moveTo(-30000 / StepAdj1); while(stepper1.currentPosition() != 0) { stepper1.run(); checkSW(); } // stepper1.moveTo(500 / StepAdj1); // while(stepper1.distanceToGo() != 0) stepper1.run(); } void init2() { stepper2.setCurrentPosition(-1); stepper2.setMaxSpeed(4000 / StepAdj2); stepper2.setAcceleration(20000 / StepAdj2); stepper2.moveTo(-30000 / StepAdj2); while(stepper2.currentPosition() != 0) { stepper2.run(); checkSW(); } // stepper2.moveTo(500 / StepAdj2); // while(stepper2.distanceToGo() != 0) stepper2.run(); } 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(); } } void checkSafeBreak() { //モーター1用エンドスイッチのチェック if(EndSW1.read() == 1) { stepper1.stop(); while(1) { blink(); } } //モーター2用エンドスイッチのチェック if(EndSW2.read() == 1) { stepper2.stop(); while(1) { blink(); } } } //モーター1のパラメータ保存 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); } //モーター2のパラメータ保存 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); } //モーター1の 速度・加速度・オフセットの値を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(D11); DigitalIn stepper1MS2(D12); 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); dmxEnable = 0; //0:receiver 1:sender conf1(); //motor1 config ドライバの設定 conf2(); //motor2 config t.start(); // to.attach(&calc, 0.001); stepper1.setCurrentPosition(-1); stepper2.setCurrentPosition(-1); //モーターの初期化シーケンス init1(); // init2(); // initBoth(); //往復運動を繰り返すテストシーケンス(無限ループ) // testMove1(); myLED =1; readROMValue1(); // readROMValue2(); stepper1.setMaxSpeed(10000 / StepAdj1); // stepper2.setMaxSpeed(10000 / StepAdj2); while(1) { //操作モード切り替えボタンのチェック checkManipulateButton(); //書き込みボタンチェック checkWriteButton(); //モーター1の目標位置をスライダーから取得 dmxInHighBit = dmx.get(DMX_Position1_H); dmxInLowBit = dmx.get(DMX_Position1_L); dmxInHighBit = dmxInHighBit << 8; position1 = (long)(dmxInHighBit + dmxInLowBit); //モーター2の目標位置をスライダーから取得(停止中) // 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; } //モーター1のパラメーター設定 position1 = map(position1, 0, 0xFFFF, 0, max1); stepper1.setMaxSpeed(speed1); stepper1.setAcceleration(Acceleration1); //モーター2のパラメーター設定(停止中) //position2 = map(position2, 0, 0xFFFF, 0, max2); //stepper2.setMaxSpeed(speed2); //stepper2.setAcceleration(Acceleration2); //モーター1の目標位置設定 stepper1.moveTo(position1); //モーター2の目標位置設定(停止中) //stepper2.moveTo(position2); //安全エンドスイッチの監視(停止中) //checkSafeBreak(); //モーター1への原点復帰命令のチェック checkInitCommand1(); //モーター2への原点復帰命令のチェック(停止中) //checkInitCommand2(); //モーター1の走行 if(stepper1.distanceToGo() != 0) stepper1.run(); //モーター2の走行(停止中) //if(stepper2.distanceToGo() != 0) stepper2.run(); } }