#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>

#define dirPin1 27 //atribui pinos ligados ao Dir e Step
#define dirPin2 12
#define stepPin1 14 // do A4988 1 e 2
#define stepPin2 13
#define buttonPin1 5
#define buttonPin2 2
#define buttonDir 26
#define potPin 34
#define PIN_TRIG 33
#define PIN_ECHO 32
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

unsigned long contaTempo1 = 0; //usados com millis para
unsigned long contaTempo2 = 0; // controlar velocidade
int flag_stepPin1 = LOW; //dos motores de passo
int flag_stepPin2 = LOW;
int tempo1 = 5;
int tempo2 = 5;
int potValue;

void setup() {

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE,0);
  display.setCursor(10,20);
  display.print("ESP32 BOT");
  display.display();

  Serial.begin(115200);
  pinMode(dirPin1, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  pinMode(stepPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(buttonDir, INPUT_PULLUP);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  digitalWrite(dirPin1, HIGH); //controla diração da rotação
  digitalWrite(dirPin2, LOW); //dos motores 1 e 2

}

void loop() {

  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  // Leia o resultado:
  int duration = pulseIn(PIN_ECHO, HIGH);

  display.clearDisplay();
  display.setCursor(10,5);
  display.print("Distancia");
  display.setCursor(50,40);
  display.print(duration / 58);
  display.display();    
  Serial.print("Distância em CM: ");
  Serial.println(duration / 58);

  potValue = analogRead(potPin);
  potValue = map(potValue, 0, 4095 , 10, 1);

  if (digitalRead(buttonDir) == LOW){
    digitalWrite(dirPin1, LOW);
    digitalWrite(dirPin2, HIGH);
  }
  else {
    digitalWrite(dirPin1, HIGH);
    digitalWrite(dirPin2, LOW);
  }

  if (digitalRead(buttonPin1) == LOW){
    tempo1 = 25;
  }
  else {
    tempo1 = 5;
  }

  if (digitalRead(buttonPin2) == LOW){
    tempo2 = 25;
  }
  else {
    tempo2 = 5;
  }

  if (millis() - contaTempo1 >= tempo1*potValue){
    contaTempo1 = millis();
    if (flag_stepPin1 == LOW) {
      flag_stepPin1 = HIGH;
    }
    else {
      flag_stepPin1 = LOW;
    }
    digitalWrite(stepPin1, flag_stepPin1);
  }

  if (millis() - contaTempo2 >= tempo2*potValue){
    contaTempo2 = millis();
    if (flag_stepPin2 == LOW) {
      flag_stepPin2 = HIGH;
    }
    else {
      flag_stepPin2 = LOW;
    }
    digitalWrite(stepPin2, flag_stepPin2);
  }
}