//#include int DIR_X_PIN = 2; int STEP_X_PIN = 3; int Chapac_PIN = 7; int DIR_Y_PIN = 4; int STEP_Y_PIN = 5; 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 = 80; float kroku_na_otacku_X = 10000; float stoupani_sroubu_Y = 80; float kroku_na_otacku_Y = 10000; float Xcounter = 0; float Ycounter = 0; float Zcounter = 0; int get_out = 0; int view = 0; // pro diagnostiku systemu view = 1 int incoming = 0; 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); // tady připojuješ tlačítko na pullup. pinMode(keyHome, INPUT); //tlačítko Home pinMode(keyEmergency, INPUT); //tlačítko nouzoveho zastaveni pinMode(keyInfo, INPUT); //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 (Serial.available()>0){ incoming = Serial.read(); if (incoming == 109)view = 1; // m = monitor zapnout if (incoming == 48)view = 0; // 0 = monitor vypnout if (incoming == 105)counters(); // i = info o pocitadlech } if (digitalRead(keyStart) == HIGH) { moveX(40, 3.7); moveX(20, 3.1); moveX(40, 3.7); moveX(0, 3.7); } if (digitalRead(keyHome) == HIGH) { Serial.println(); Serial.println("tlacitko home aktivovano"); moveX(-30, 0.1); //Hledání výchozí polohy. moveY(-30, 0.1); Serial.println("referencni cyklus ukoncen"); } if (digitalRead(keyInfo) == HIGH) { Serial.println(); Serial.println("tlacitko Info aktivovano"); counters(); delay(1000); } } //********************************************************************************** // functions void moveX(float posun, float rychlost){ int Current = 1; float Bx = Xcounter-posun; Bx = Bx*(-1); int Ot = Bx * (kroku_na_otacku_X/stoupani_sroubu_X); int zrychleni = 400; // zrychlení motoru při rozjezdu (počet kroků na plnou rychlost) float maxDelay = 700; // 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){ int Current = 2; float By = Ycounter-posun; By = By*(-1); int Ot = By * (kroku_na_otacku_Y/stoupani_sroubu_Y); int zrychleni = 400; // zrychlení motoru při rozjezdu (počet kroků na plnou rychlost) float maxDelay = 700; // 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(200); //pauza mezi jednotlivými prikazy int dir = (steps > 0) ? LOW : HIGH; 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) { 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"); for (int i = 0; i<=200; i++) { digitalWrite(DIR_Y_PIN, LOW); 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"); for (int i = 0; i<=200; i++) { digitalWrite(DIR_X_PIN, LOW); 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); }