// -------------------- Parameter zur Liniarisierung -----------------
int   a = 1;         //Hier maximale Geschwindigkeit je kleiner umso schneller 
float b = 30000.0f;    //Hier minimale Geschwindigkeit je größer umso langsamer
int   c = 1021;
// -------------------------------------------------------------------



#include <math.h>
//TIMER
#include <TimerOne.h>        
void _timerOne();
volatile long int timerOne;
//BUZZER
#define BUZZER             2 
//TASTER
#define TASTER_START_PAUSE 3 
#define TASTER_NULLPUNKT   4
#define TASTER_LINKS       5
#define TASTER_RECHTS      6
//MOTOR
#define DIR                7 
#define STEP               8 
//ENABLE
#define ENABLE             9
//POTI       
#define POTI_JOY          A6 
#define POTI_NULL         A7


//Globale Variablen
byte toggel                =  0;
byte motorRechtsDrehen     =  0;
byte motorLinksDrehen      =  0;
unsigned long myTimer50    =  0;
unsigned long myTimeout50  = 50;
unsigned long myTimer150   =  0;
unsigned long myTimeout150 = 125;
byte tasterLinks           =  0;
byte tasterRechts          =  0;
byte tasterStartPause      =  0;
byte tasterNullpunkt       =  0;
byte nullpunktAnfahren     =  0;
byte startPause            =  0; 
int  potiJoy               =  0;
int  potiNull              =  0;
int  potiWertGlaetten      =  0;
int  zwischenWert          =  0;
unsigned long zaehler      =  2147483647;
unsigned long zaehlerErgebnis = 0;
unsigned long nullpunkt    =  0;
byte blockierung = 1;




//Programmvoreinstellungen
void setup() {
    Serial.begin(9600);
   
    //TIMER ONE
    Timer1.initialize(timerOne);       //Alle x Sekunden wird die Funktion aufgerufen  
    Timer1.attachInterrupt(_timerOne); //Ruft eine Funktion auf
    //MOTOR
    pinMode(STEP,              OUTPUT); 
    pinMode(DIR,               OUTPUT);
    //TASTER  
    pinMode(TASTER_LINKS,       INPUT);
    pinMode(TASTER_RECHTS,      INPUT);
    pinMode(TASTER_START_PAUSE, INPUT);
    pinMode(TASTER_NULLPUNKT,   INPUT);   
    //POTIS
    pinMode(POTI_JOY,           INPUT);
    pinMode(POTI_NULL,          INPUT);   
    //BUZZER
    pinMode(BUZZER,            OUTPUT);   
    //ENABLE
    pinMode(ENABLE,            OUTPUT);
    digitalWrite(ENABLE,          LOW);

    zaehlerErgebnis = zaehler;

    //Berechnung
    b = log(b - a) / c;
    Serial.println(b, 8);
    
    Serial.println(" ++++++++++ Betriebsbereit ++++++++++");
}


//Hauptprogramm
void loop() {  
    if(nullpunktAnfahren == 0) {
        
        _potiJoy(); //Poti auslesen


        //Taster links
        if(digitalRead(TASTER_LINKS) == LOW) {
            if(millis() > myTimeout50 + myTimer50) { 
                myTimer50 = millis();
                //digitalWrite(ENABLE, LOW);    
                motorLinksDrehen = 1;
                blockierung = 0;
            }
            //Serial.println("Motor links");
        } 
        //Taster Rechts
        else if(digitalRead(TASTER_RECHTS) == LOW) {
            if(millis() > myTimeout50 + myTimer50) { 
                myTimer50 = millis(); 
                //digitalWrite(ENABLE, LOW);
                motorRechtsDrehen = 1; 
                blockierung = 0;
            }      
            //Serial.println("Motor rechts");
        }
        else {
            motorRechtsDrehen = 0;
            motorLinksDrehen  = 0;
            //digitalWrite(ENABLE, HIGH);
             
            if(blockierung == 0) {
                if(zaehlerErgebnis > zaehler) {zaehlerErgebnis = zaehlerErgebnis - zaehler;}
                else {zaehlerErgebnis = (zaehler - zaehlerErgebnis) / 2;}
                Serial.print("zaehlerErgebnis: "); Serial.println(zaehlerErgebnis);                  
                blockierung = 1;
                zaehlerErgebnis = 2147483647; //Mittelwert
            }
        }      
    }
    

         
    //Taster Start/Pause
    if(digitalRead(TASTER_START_PAUSE) == LOW) {
        if(millis() > myTimeout50 + myTimer50) { 
            myTimer50 = millis(); 

            //Nullpunkt anfahren starten
            if(nullpunktAnfahren == 0)      {
                nullpunktAnfahren = 1;
                //digitalWrite(ENABLE, LOW);
                Serial.println("    START - Nullpunkt wird angefahren");  

                

                //
                if(     zaehler > nullpunkt)  {nullpunktAnfahren = 1;}                                 
                else if(zaehler < nullpunkt)  {nullpunktAnfahren = 2;}
                else if(zaehler == nullpunkt) {
                    digitalWrite(BUZZER, HIGH);
                    delay(1000);
                    digitalWrite(BUZZER, LOW);
                }                                                
            }
            //Nullpubnkt anfahren pausieren
            else if(nullpunktAnfahren == 1) {
                nullpunktAnfahren = 0;
                Serial.println("    PAUSE - Nullpunkt anfahren wird unterbrochen");
                motorRechtsDrehen = 0;
                motorLinksDrehen  = 0;
                //digitalWrite(ENABLE, HIGH);
            }           
        }      
        
        while(digitalRead(TASTER_START_PAUSE) == LOW);
    }
    //Taster Nullpunkt
    else if(digitalRead(TASTER_NULLPUNKT) == LOW) {
        if(millis() > myTimeout50 + myTimer50) { 
            myTimer50 = millis();        
            nullpunkt = zaehler;           
        }      
        Serial.println("NULLPUNKT - Nullpunkt wurde gespeichert");
        Serial.print("nullpunkt: "); Serial.println(nullpunkt);
        Serial.print("zaehler: ");   Serial.println(zaehler);
        
        digitalWrite(BUZZER, HIGH);
        while(digitalRead(TASTER_NULLPUNKT) == LOW);        
        digitalWrite(BUZZER, LOW);
    }   
    


    //Nullpunkt anfahren
    if(     nullpunktAnfahren == 1) {_nullpunktAnfahren();}
    else if(nullpunktAnfahren == 2) {_nullpunktAnfahren2();}         
}


//
void _nullpunktAnfahren() {
    _potiNull(); //Poti auslesen
       
    if(zaehler > nullpunkt) {
        //Serial.println("  motorLinksDrehen");
        //Serial.print  ("  zaehler:   "); Serial.println(zaehler);
        //Serial.print  ("  nullpunkt: "); Serial.println(nullpunkt);
        motorLinksDrehen = 1;
    }
    else {
        //Serial.println("NULLPUNKT ERREICHT"); 
        nullpunktAnfahren = 0; 
        motorLinksDrehen  = 0;
        motorRechtsDrehen = 0;                
    }    
}


//
void _nullpunktAnfahren2() {
    _potiNull(); //Poti auslesen

    if(millis() > myTimeout150 + myTimer150) { 
        myTimer150 = millis();    
        
        if(zaehler < nullpunkt) {
            //Serial.println("  motorLinksDrehen");
            //Serial.print  ("  zaehler:   "); Serial.println(zaehler);
            //Serial.print  ("  nullpunkt: "); Serial.println(nullpunkt);
            motorRechtsDrehen = 1;
        }
        else {
            //Serial.println("NULLPUNKT ERREICHT"); 
            nullpunktAnfahren = 0; 
            motorLinksDrehen  = 0;
            motorRechtsDrehen = 0;                
        }
    }  
}


//Diese Funktion wird alle timerOne = 400 µs aufgerufen
void _timerOne() {
    //(1) GESCHWINDIGKEIT - Ansteuerung Motoren 
    if(motorRechtsDrehen == 1) {
        _nema17UhrzeigersinDrehen();
        zaehler ++;
    }                         
    else if(motorLinksDrehen == 1) {
        _nema17GegenUhrzeigersinDrehen();
        zaehler --;
    }
}


//Nema17 Motor im Uhrzeigersin drehen 
void _nema17UhrzeigersinDrehen() {        
    digitalWrite(DIR, LOW); //Im Uhrzeigersin einstellen

    if(toggel == 0) {toggel = 1;}            
    else {toggel = 0;} 
              
    digitalWrite(STEP, toggel);     
}


//Nema17 Motor gegen den Uhrzeigersin drehen 
void _nema17GegenUhrzeigersinDrehen() {       
    digitalWrite(DIR, HIGH); //gegen Uhrzeigersin einstellen
        
    if(toggel == 0) {toggel = 1;}            
    else {toggel = 0;}         
     
    digitalWrite(STEP, toggel);                
} 


//Poti beliebige Position
void _potiJoy() {
    potiWertGlaetten = 0.6 * potiWertGlaetten + 0.4 * analogRead(POTI_JOY);
    
    if(zwischenWert != potiWertGlaetten) {
        //Serial.print("poti: "); Serial.println(potiWertGlaetten);

        timerOne = _berechnung(potiWertGlaetten);
        //Serial.print("    timerOne: "); Serial.println(timerOne);       
        Timer1.initialize(timerOne);   
    }    
    zwischenWert = potiWertGlaetten;
    delay(125);
}



//Poti Nullpunkt anfahren
void _potiNull() {
    potiWertGlaetten = 0.6 * potiWertGlaetten + 0.4 * analogRead(POTI_NULL);
    
    if(zwischenWert != potiWertGlaetten) {
        //Serial.print("    poti: "); Serial.println(potiWertGlaetten);

        timerOne = _berechnung(potiWertGlaetten);
        //Serial.print("    timerOne: "); Serial.println(timerOne);       
        Timer1.initialize(timerOne);   
    }    
    zwischenWert = potiWertGlaetten;   
    delay(125);
}


//Liniarisierung Motorwert
int _berechnung(int var) {
    int ergebnis = 0;
    
    ergebnis = a + exp(b*(c-var));
    
    timerOne = ergebnis;
    Timer1.initialize(timerOne);
    //Serial.print("  berechnung: "); Serial.println(ergebnis);
    
    return ergebnis;
}