//#include int DIR_X_PIN = 2; int STEP_X_PIN = 3; int Chapac_PIN = 4; int DIR_Y_PIN = 5; int STEP_Y_PIN = 6; int DIR_PIN = DIR_X_PIN; int STEP_PIN = STEP_X_PIN; int keyStart = A0; // PIN pro připojení tlačítka Start int keyHome = A1; // PIN pro připojení tlačítka Home int keyEmergency = 10; // tlacitko nouzoveho zastaveni int keyInfo = 11; // tlacitko nouzoveho zastaveni int limit_XL_Refer = 8; // PIN pro připojení tlačítka Refer osy X leva strana int limit_YP_Refer = 9; // PIN pro připojení tlačítka Refer osy Y predni snimac float stoupani_sroubu_X = 8; float kroku_na_otacku_X = 200; float stoupani_sroubu_Y = 8; float kroku_na_otacku_Y = 200; float Xcounter = 0; float Ycounter = 0; float Zcounter = 0; int get_out = 0; int view = 0; // pro diagnostiku systemu view = 1 void setup() { pinMode(DIR_X_PIN, OUTPUT); pinMode(STEP_X_PIN, OUTPUT); pinMode(DIR_Y_PIN, OUTPUT); pinMode(STEP_Y_PIN, OUTPUT); pinMode(Chapac_PIN, OUTPUT); pinMode(keyStart, INPUT_PULLUP); // tady připojuješ tlačítko na pullup. pinMode(keyHome, INPUT_PULLUP); //tlačítko Home pinMode(keyEmergency, INPUT_PULLUP); //tlačítko nouzoveho zastaveni pinMode(keyInfo, INPUT_PULLUP); //tlačítko nouzoveho zastaveni pinMode(limit_XL_Refer, INPUT_PULLUP); //referenční snímač 1 na ose x pinMode(limit_YP_Refer, INPUT_PULLUP); //referenční snímač 1 na ose Y Serial.begin(9600); } //********************************************************************************** void loop() { if (digitalRead(keyStart) == LOW) { Serial.println("tlacitko start aktivovano"); moveX(150, 0.7); // přesun na 400mm rychlosti 0.07 chapac(1); // zavreni chapadla moveY(40,0.7); moveX(10, 0.7); moveX(5, 0.07); chapac(0); // otevření chapadla moveY(200,0.7); moveY(100, 0.7); Serial.println("cyklus ukončen"); } if (digitalRead(keyHome) == LOW) { Serial.println("tlacitko home aktivovano"); moveX(-10, 0.3); //Hledání výchozí polohy. moveY(-10, 0.3); } if (digitalRead(keyInfo) == LOW) { Serial.println("tlacitko Info aktivovano"); counters(); delay(1000); } } //********************************************************************************** // functions void moveX(float posun, float rychlost){ Serial.println("pohyb na ose X zahajen"); int Current = 1; float Bx = Xcounter-posun; Bx = Bx*(-1); int Ot = Bx * (kroku_na_otacku_X/stoupani_sroubu_X); int zrychleni = 400; // tady se mění zrychlení motoru při rozjezdu (počet kroků na plnou rychlost) float maxDelay = 700; // tady se mění prudkost náběhu motoru (když dělá velký rázy, zvýší se číslo) float Ost = 1/(kroku_na_otacku_X/stoupani_sroubu_X); rotate(Ot, rychlost, DIR_X_PIN, STEP_X_PIN, Current, zrychleni, maxDelay, Ost); } void moveY(float posun, float rychlost){ Serial.println("pohyb na ose Y zahajen"); int Current = 2; float By = Ycounter-posun; By = By*(-1); int Ot = By * (kroku_na_otacku_Y/stoupani_sroubu_Y); int zrychleni = 400; // tady se mění zrychlení motoru při rozjezdu (počet kroků na plnou rychlost) float maxDelay = 700; // tady se mění prudkost náběhu motoru (když dělá velký rázy, zvýší se číslo) float Ost = 1/(kroku_na_otacku_Y/stoupani_sroubu_Y); rotate(Ot, rychlost, DIR_Y_PIN, STEP_Y_PIN, Current, zrychleni, maxDelay, Ost); } void rotate(int steps, float speed, int DIR_PIN, int STEP_PIN, int Current, int zrychleni, float maxDelay, float Ost) { if (view == 1) Serial.println(Current); if (view == 1) Serial.println(Ost); delay(1000); //rotate a specific number of steps - (negitive for reverse movement) //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger int dir = (steps > 0) ? HIGH : LOW; steps = abs(steps); digitalWrite(DIR_PIN, dir); int stepsStart = zrychleni; int stepsMid = steps-zrychleni*2; int stepsEnd = zrychleni; if (dir == LOW) Ost = Ost*(-1); float oneStep = maxDelay/zrychleni; float usDelay = (1 / speed) * 70; if (steps < 2){ digitalWrite(STEP_PIN, HIGH); delayMicroseconds(usDelay); digitalWrite(STEP_PIN, LOW); if (Current ==1) Xcounter += Ost; if (Current ==2) Ycounter += Ost; counters(); delayMicroseconds(usDelay); return; } if (steps < zrychleni*2){ int stepsMod = steps % 2; int stepsDiv = steps-stepsMod; stepsStart = (stepsDiv/2); stepsEnd = stepsDiv/2; stepsMid = stepsMod; } // +++++++++++++++++++****************************************** for (int i = 0; i < stepsStart; i++){ //rozjezd motoru limits(); if (get_out == 1){ get_out = 0; return;} digitalWrite(STEP_PIN, HIGH); delayMicroseconds(usDelay); digitalWrite(STEP_PIN, LOW); if (Current ==1) Xcounter += Ost; if (Current ==2) Ycounter += Ost; if (view == 1) counters(); if (view == 1) Serial.print("STARTSTEP "); delayMicroseconds(usDelay+(maxDelay-oneStep*i)); } for (int i = 0; i < stepsMid; i++) { // středová vzdálenost limits(); if (get_out == 1){ get_out = 0; return;} digitalWrite(STEP_PIN, HIGH); delayMicroseconds(usDelay); digitalWrite(STEP_PIN, LOW); if (Current ==1) Xcounter += Ost; if (Current ==2) Ycounter += Ost; if (view == 1) counters(); if (view == 1) Serial.print("MIDLESTEP "); delayMicroseconds(usDelay); } for (int i = 0; i <= stepsEnd; i++){ //zastavení motoru limits(); if (get_out == 1){ get_out = 0; return;} digitalWrite(STEP_PIN, HIGH); delayMicroseconds(usDelay); digitalWrite(STEP_PIN, LOW); if (Current ==1) Xcounter += Ost; if (Current ==2) Ycounter += Ost; if (view == 1) counters(); if (view == 1) Serial.print("STOPSTEP "); delayMicroseconds(usDelay+(oneStep*i)); } //++++++++++++++++++++++++++++++++-------------*************************************** } void rotateDeg(float deg, float speed) { //rotate a specific number of degrees (negitive for reverse movement) //speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger int dir = (deg > 0) ? HIGH : LOW; digitalWrite(DIR_PIN, dir); int steps = abs(deg) * (1 / 0.225); float usDelay = (1 / speed) * 70; for (int i = 0; i < steps; i++) { digitalWrite(STEP_PIN, HIGH); delayMicroseconds(usDelay); digitalWrite(STEP_PIN, LOW); delayMicroseconds(usDelay); } } void limits(){ // if (koncák 1 tak... // if (koncák 2 tak... if (digitalRead(limit_YP_Refer) == LOW) { Serial.println("Referencni snimac osy Y aktivni"); while (digitalRead(limit_YP_Refer) == LOW) { digitalWrite(DIR_Y_PIN, HIGH); digitalWrite(STEP_Y_PIN, HIGH); delayMicroseconds(5); digitalWrite(STEP_Y_PIN, LOW); if (view == 1) Serial.println("sjizdeni z refer. snimace Y"); delay(10); } Serial.println("Referencni snimac deaktivovan"); digitalWrite(DIR_Y_PIN, LOW); Ycounter = 0; get_out = 1; if (view = 1) Serial.print("get_out "); if (view = 1) Serial.println(get_out); } if (digitalRead(limit_XL_Refer) == LOW) { Serial.println("Referencni snimac osy X aktivni"); while (digitalRead(limit_XL_Refer) == LOW) { digitalWrite(DIR_X_PIN, HIGH); digitalWrite(STEP_X_PIN, HIGH); delayMicroseconds(5); digitalWrite(STEP_X_PIN, LOW); if (view == 1) Serial.println("sjizdeni z refer. snimace X"); delay(10); } Serial.println("Referencni snimac deaktivovan"); digitalWrite(DIR_X_PIN, LOW); Xcounter = 0; get_out = 1; if (view == 1) Serial.print("get_out "); if (view == 1) Serial.println(get_out); } } void chapac(int command){ Serial.println(); Serial.println("ovladani chapadla aktivni"); if (command == 1) digitalWrite(Chapac_PIN, HIGH); // sevreni chapadla if (command == 0) digitalWrite(Chapac_PIN, LOW); // uvolneni chapadla } void counters(){ // odesilani udaju o poloze Serial.print("Y:"); Serial.print(Ycounter); Serial.print(" X:"); Serial.print(Xcounter); Serial.print(" Z:"); Serial.println(Zcounter); }