test2017-1-13
Dependencies: AT24C1024 DMX-STM32 mbed
main.cpp
- Committer:
- tomo370
- Date:
- 2017-07-28
- Revision:
- 8:2e68b9de685d
- Parent:
- 7:813cdaad1200
File content as of revision 8:2e68b9de685d:
#include "mbed.h" #include "AT24C1024.h" #include "AccelStepper.h" #include "DMX.h" //モータ制御基板の通し番号を設定してください 初期設定は1です 0は使わないでね。 #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 //モーター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; //未実装 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() { //モーター1用エンドスイッチのチェック if(EndSW3.read() == 1) { stepper1.stop(); stepper1.setCurrentPosition(0); } //モーター2用エンドスイッチのチェック if(EndSW4.read() == 1) { 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(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); } } //モーター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.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); //30度程度時計回りに回転動作 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() { //モーター1用エンドスイッチのチェック if(EndSW3.read() == 1) { stepper1.stop(); while(1) { blink(); } } //モーター2用エンドスイッチのチェック if(EndSW4.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(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(); //モーター1の目標位置をDMXから取得 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) { //checkInitCommand1(); stepper1.run(); } //モーター2の走行 if(stepper2.distanceToGo() != 0) stepper2.run(); } }