#include <elapsedMillis.h>
elapsedMillis timeElapsed;

int RPWM = 10;
int LPWM = 11;
int downPin = 12;
int upPin = 13;
int autoPin = 8;
int manPin = 9;

int sensorPin = A0;
int potPin = A2;
int potVal;
int sensorVal;
int Buffer = 10;

int maxAnalogReading;
int minAnalogReading;

void setup() {
  pinMode(RPWM, OUTPUT);
  pinMode(LPWM, OUTPUT);
  pinMode(downPin, INPUT_PULLUP);
  pinMode(upPin, INPUT_PULLUP);
  pinMode(autoPin, INPUT_PULLUP);
  pinMode(manPin, INPUT_PULLUP);
   pinMode(sensorPin, INPUT);
  pinMode(potPin, INPUT);
  Serial.begin(9600);
  maxAnalogReading = moveToLimit(1); // otwieracie monitor portu szeregowego i odczytujecie wartoscii poprzez sterowanie potenjometrem, w jakim zakresie ma działać was siłownik, usuwacie movetolimit(1) i zmieniacie np na 937
  minAnalogReading = moveToLimit(-1) ; // otwieracie monitor portu szeregowego i odczytujecie wartoscii poprzez sterowanie potenjometrem, w jakim zakresie ma działać was siłownik, usuwacie movetolimit(-1) i zmieniacie np na 242
}

void loop() {
  if(digitalRead(manPin)==LOW)
  {
  if(digitalRead(upPin)==LOW){ //wysuwaj jesli guzik jest wcisniety
    analogWrite(RPWM, 0);
    analogWrite(LPWM, 255);
  }

  else if(digitalRead(downPin)==LOW){ //wsuwaj jesli guzik jest wcisniety
    analogWrite(RPWM, 255);
    analogWrite(LPWM, 0);
  }

  else{ //jesli guzik nie jest wcisniety stoj
    analogWrite(RPWM, 0);
    analogWrite(LPWM, 0);
  }
  }

  else if(digitalRead(autoPin)==LOW)
  {
    potVal = map(analogRead(potPin), 0, 1023, minAnalogReading, maxAnalogReading);
  sensorVal = analogRead(sensorPin);
  if(potVal > (sensorVal+Buffer)){               //Dodatkowy bufor, zapobiegajacy wibracja silnika z powodu zaklocen danych wejscia
    driveActuator(1, 100);
  }
  else if(potVal < (sensorVal-Buffer)){
    driveActuator(-1, 100);
  }
  else{
    driveActuator(0, 100);
  }
  Serial.print("Potentiometer Reading: "); //po ustaleniu zakresy pracy na potencjometrze można usunąć ten kod od tej linijki do delay(10)
  Serial.print(potVal);
  Serial.print("\tActuator reading: ");
  Serial.println(sensorVal);
  delay(10);
  }

  else{ //jesli zaden przycisk trybu nie jest przelaczony, stoj
    analogWrite(RPWM, 0);
    analogWrite(LPWM, 0);
  }
}

int moveToLimit(int Direction){  //po ustaleniu zakresy pracy na potencjometrze można usunąć ten kod od tej linijki do return currreading i znak } który znajduje się w linijce pod spodem
  int prevReading=0;
  int currReading=0;
  do{
    prevReading = currReading;
    driveActuator(Direction, 255);
    timeElapsed = 0;
    while(timeElapsed < 200){ delay(1);}
   currReading = analogRead(sensorPin);
  }while(prevReading != currReading);
  return currReading;
}

void driveActuator(int Direction){
  switch(Direction){
    case 1:       //wysuwanie
      analogWrite(RPWM, 100);
      analogWrite(LPWM, 0);
      break;

    case 0:       //stop
      analogWrite(RPWM, 0);
      analogWrite(LPWM, 0);
      break;

    case -1:      //wsuwanie
      analogWrite(RPWM, 0);
      analogWrite(LPWM, 100);
      break;
  }
}