#include <LiquidCrystal.h>
#define ANALOG_PIN A15

int Contrast = 60;
LiquidCrystal lcd(32,  31, 24, 25, 26, 27);

const int LED_CW = 2;
const int LED_CCW = 23;
const int LED_STOP = 22;

//connections to ultrasonic sensor
const int TRIG_PIN_M1 = 6;
const int ECHO_PIN_M1 = 5;
const int TRIG_PIN_M2 = 4;
const int ECHO_PIN_M2 = 3;

// Connections to motordriver
const int STEP_M1 = 13; // Step
const int DIR_M1 = 12;  // Direction
const int STEP_M2 = 11; // Step
const int DIR_M2 = 10;

//buttons
const int BUTTON_CW = 9;
const int BUTTON_CCW = 8;
const int BUTTON_STOP = 7;

// Motor steps per rotation
const int STEPS_PER_REV = 200;
const int MAX_SPEED_M1 = 1000;
 
//motor state
bool motorRunning;
bool motorCW;
bool motorCCW;

//motor delay
double speed;
double speedValue;


void setup() {

  Serial.begin(9600);

  analogWrite(36, Contrast);
  lcd.begin(16,  2);
  lcd.setCursor(0, 0);
  lcd.print("Speed : ");
  

  // Setup the pinsnand buttons
  pinMode(STEP_M1,OUTPUT); 
  pinMode(DIR_M1,OUTPUT);
  pinMode(STEP_M2,OUTPUT);
  pinMode(DIR_M2,OUTPUT);
  

  pinMode(BUTTON_STOP,INPUT); 
  pinMode(BUTTON_CCW,INPUT); 
  pinMode(BUTTON_CW,INPUT);

  pinMode(TRIG_PIN_M1, OUTPUT);
  pinMode(ECHO_PIN_M1, INPUT);
  pinMode(TRIG_PIN_M2, OUTPUT);
  pinMode(ECHO_PIN_M2, INPUT);

  pinMode(LED_CW, OUTPUT);
  pinMode(LED_CCW, OUTPUT);
  pinMode(LED_STOP, OUTPUT);

  //set motor state to false
  motorRunning = false;
  motorCW = false;
  motorCCW = false;

  digitalWrite(LED_STOP, HIGH);
  digitalWrite(LED_CW, LOW);
  digitalWrite(LED_CCW, LOW);

}
void loop() {

  lcd.setCursor(8, 0);
  lcd.print(speed);
  lcd.setCursor(12, 0);
  lcd.print(" m/s ");


  //SPEED DETERMINATION

  //read potmeter value
  double potValue = analogRead(A15);
  speedValue = map(potValue, 0, 1023, 1, 10);

  speed = speedValue/100;

  if (motorRunning == false) {
    

  }

  if (digitalRead(BUTTON_CW) == HIGH && motorRunning == false) {

    //motor is running
    motorRunning = true;
    
    // Set motor direction clockwise
    digitalWrite(DIR_M1, HIGH);
    digitalWrite(DIR_M2, HIGH);

    digitalWrite(LED_CW, HIGH);
    digitalWrite(LED_STOP, LOW);
    digitalWrite(LED_CCW, LOW);

  }

  else if (digitalRead(BUTTON_CCW) == HIGH && motorRunning == false) {
   
    //motor is running
    motorRunning = true;

    // Set motor direction clockwise
    digitalWrite(DIR_M1, LOW);
    digitalWrite(DIR_M2, LOW);

    digitalWrite(LED_CCW, HIGH);
    digitalWrite(LED_STOP, LOW);
    digitalWrite(LED_CW, LOW);

  }

  else if (digitalRead(BUTTON_STOP) == HIGH) { //min diameter 9.6

    digitalWrite(LED_STOP, HIGH);
    digitalWrite(LED_CW, LOW);
    digitalWrite(LED_CCW, LOW);
    motorRunning = false;
 
  }
  
  if (motorRunning == true) {

    unsigned long currentMotor1Time = millis();
    unsigned long currentMotor2Time = millis();

    digitalWrite(STEP_M1,LOW);
    digitalWrite(STEP_M2,LOW);
    delay(speedValue);
    digitalWrite(STEP_M1, HIGH);
    digitalWrite(STEP_M2, HIGH);
    delay(speedValue);

    }
    

  }