%Program pro pojezd Z-ové osy s krokovým motorem pro vávojové prostředí Atom mikročipu Arduino nano a přípravek POLOLU%

#include <Arduino.h>
#include "A4988.h"
#include <Encoder.h>

//------------------------------------------------------------------------------
//PINOUT
//ovládání krokáče
#define ENBL 7
#define DIR 8
#define STEP 9
//krokáč - microstepping
#define MS1 10
#define MS2 11
#define MS3 12
//"uživatelské rozhraní"
#define LED 13
#define TL_ENC 4  //tlačítko na enkodéru
#define TL_ZVEDNI 5 //tlačítko pro zvedání a ukládání polohy
#define H_DORAZ 6
#define D_DORAZ A0
#define ENC_A 2
#define ENC_B 3
//rychlosti normálního posuvu a mikroposuvu
#define POSUV 128
#define MIKROPOSUV 8
#define MIKROKROKY 8  //aby nedocházelo ke ztrátě polohy vlivem vypnutí proudu do motoru, musí být nastaveno na stejnou hodnotu jako MIKROPOSUV
#define RYCH_POSUV 120  //ot/min rychloposuvu
#define RYCH_MIKRO 60   //ot/min jemného posuvu

//objekt krokáče
#define MOTOR_STEPS 200
A4988 stepper(MOTOR_STEPS, DIR, STEP, MS1, MS2, MS3);

//objekt enkodéru
Encoder myEnc(ENC_A, ENC_B);

void setup() {
  //inicializace I/O pinů
  pinMode(LED, OUTPUT);
  pinMode(ENBL, OUTPUT);
  digitalWrite(LED, LOW);
  digitalWrite(ENBL, HIGH);
  pinMode(TL_ENC, INPUT_PULLUP);
  pinMode(TL_ZVEDNI, INPUT_PULLUP);
  pinMode(H_DORAZ, INPUT_PULLUP);
  pinMode(D_DORAZ, INPUT_PULLUP);
  //nastavení motorku
  stepper.setRPM(RYCH_POSUV);
  stepper.setMicrostep(MIKROKROKY);

  Serial.begin(9600);
  Serial.println("Pojezd lupa: Ahoj!");
  //Homing - najetí na horní doraz
  digitalWrite(ENBL, LOW);  //zapnutí proudu do motoru
  while(digitalRead(H_DORAZ)) {
    stepper.move(1);
  }
  stepper.move(MIKROKROKY*200*(-1));
  digitalWrite(ENBL, HIGH); //vypnutí proudu do motoru
  Serial.println("baf");
}

//"globální" proměnné - jsou potřeba jen v loop, je potřeba je nastavit na uvedené hodnpty
//ale při novém začátku loop už je přenastavit nechci - asi by se dalo použít static nebo volatile ...
long poloha = 0;
long zaost_poloha = 0;
long posl_enc = 0;
long rozdil = 0;
int krok = POSUV;
byte db = 1;  //flag pro kontrolu uvolnění tlačítka enkodéru "debounce"
byte db1 = 1; //flag pro kontrolu délky stisku tlačítka pro popojíždění sem tam
long posl_millis = millis();  //pro měření doby stisku tlačítka pro popojíždění

void loop() {
  rozdil = myEnc.read()/4 - posl_enc; //enc je na IRQ, o kolik se změnil jeho stav od posledního průbehu smyčky
  posl_enc = myEnc.read()/4;          //dělení 4 je protože jedno mech. cvaknutí odpovídá změně čísla o 4

  //pokud došlo k otočení enkodéru, zapne se pololu driver a dá příkaz k otáčení motoru
  if((rozdil > 0 && digitalRead(H_DORAZ)) || (rozdil < 0 && digitalRead(D_DORAZ))) {
    digitalWrite(ENBL, LOW);  //zapnutí proudu do motoru
    poloha = poloha + rozdil*krok;
    stepper.move(rozdil*krok);
    //Serial.println(rozdil*krok);
    Serial.println(poloha);
    //Serial.println(myEnc.read());
    digitalWrite(ENBL, HIGH); //vypnutí proudu do motoru
  }

  //přepínání mezi normálním posuvem a mikroposuvem
  if(!digitalRead(TL_ENC) && db) {
    if(krok == POSUV) {
      stepper.setRPM(RYCH_MIKRO);
      krok = MIKROPOSUV;
      digitalWrite(LED, HIGH);
    }
    else  {
      stepper.setRPM(RYCH_POSUV);
      krok = POSUV;
      digitalWrite(LED, LOW);
    }
    db = 0;
  }
  db = digitalRead(TL_ENC);

  //najíždění nahoru a na zazoomovanou polohu
  if(!digitalRead(TL_ZVEDNI) && db1 ) {
    posl_millis = millis();
    db1 = 0;
  }
  //dlouhý stisk - uložit aktuální pozici
  if (digitalRead(TL_ZVEDNI) && !db1 && millis() - posl_millis > 2000) {
    zaost_poloha = poloha;
    Serial.print("ulozena zaostrena poloha: ");
    Serial.println(zaost_poloha);
    db1 = 1;
    return;
  }
  //krátký stisk - popojet sem nebo tam
  else if (digitalRead(TL_ZVEDNI) && !db1 && millis() - posl_millis > 100) {
    digitalWrite(ENBL, LOW);  //zapnutí proudu do motoru
    stepper.setRPM(RYCH_POSUV);
    //najeto nahoře, motor má sjet dolů na zaostřenou polohu
    if(poloha == 0) {
      Serial.print("nahore, najizdim na: ");
      Serial.println(zaost_poloha);
      long pomocna = zaost_poloha;

      //if - else řeší kam točit motorem v závislosti na tom, jestli je poloha kladná nebo záporná
      if(zaost_poloha > 0) {
        while (pomocna--) {
          stepper.move(1);
          if (!digitalRead(H_DORAZ))
            break;
          }
      }
      else {
        while (pomocna++) {
          stepper.move(-1);
          if (!digitalRead(D_DORAZ))
            break;
          }
      }
      poloha = zaost_poloha;  //ať dojel nebo narazil na doraz, předpokládám, že je tam, kde měl být
    }
    //najeto někde jiinde, motor má najet nahoru
    else {
      Serial.print("aktualni pozice: ");
      Serial.print(poloha);
      Serial.println(", najizdim na 0");
      long pomocna = poloha;

      //if - else řeší kam točit motorem v závislosti na tom, jestli je poloha kladná nebo záporná
      if(poloha > 0)  {
        while (pomocna--) {
          stepper.move(-1);
          if (!digitalRead(D_DORAZ))
            break;
          }
        }
      else  {
        while (pomocna++) {
          stepper.move(1);
          if (!digitalRead(H_DORAZ))
            break;
          }
      }
      poloha = 0; //ať dojel nebo narazil na doraz, předpokládám, že je tam, kde měl být
    }
    if(krok == POSUV) {
      stepper.setRPM(RYCH_POSUV);
      digitalWrite(LED, LOW);
    }
    else  {
      stepper.setRPM(RYCH_MIKRO);
      digitalWrite(LED, HIGH);
    }

    digitalWrite(ENBL, HIGH);  //vypnutí proudu do motoru
    db1 = 1;
  }
  //hodně krátký stisk - nic se neděje, šlo o falešný poplach - soft. debounce
  if (digitalRead(TL_ZVEDNI) && !db1)  {
    Serial.println("falesny poplach");
    db1 = 1;
  }

}
