//This is the program that contain varied speed control, direction control and calculation speed with encoder
//Declaration for direction control
#define cwleft 33
#define acwleft 32
#define cwright 21
#define acwright 18
#define pwm_left 25
#define pwm_right 5
const int ch0 = 0;
const int ch1 = 1;
const int freq = 1000;
const int res = 8;
char i;

// Declaration for Encoder
#define left 2
#define right 4
int counter1 = 0;
int counter2 = 0;
int speed_left, speed_right;
const long interval = 1000;
unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
const long interval1 = 100;
unsigned long previousMillis1 = 0;
unsigned long previousMillis2 = 0;

// Declaration for Speed Control
#define control_speed 12
int counter3 = 0;
int a,b;
unsigned long previousMillis3 = 0;
const long interval2 = 500;
String message;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  // Pin Mode for Direction Control 
  pinMode(cwleft, OUTPUT);
  pinMode(acwleft, OUTPUT);
  pinMode(cwright, OUTPUT);
  pinMode(acwright, OUTPUT);
  ledcAttachPin(pwm_left, ch0);
  ledcAttachPin(pwm_right, ch1);
  ledcSetup(ch0,freq,res);
  ledcSetup(ch1,freq,res);
  // Pin Mode for Encoder
  pinMode(left, INPUT);
  pinMode(right, INPUT);
  attachInterrupt(left, detect1, RISING);
  attachInterrupt(right, detect2, RISING);
  // Pin Mode for speed control
  pinMode(control_speed, INPUT);
  attachInterrupt(control_speed, detect3, RISING);
  
}

void loop() {
  // put your main code here, to run repeatedly:
  currentMillis=millis();
  if(currentMillis-previousMillis>=interval){
    // To calculate speed
    speed_left = counter1 * 60; //RPM = ((Total pules/Pulse per rotation)/sec) * 60
    speed_right = counter2 * 60; //RPM = ((Total pules/Pulse per rotation)/sec) * 60
    // Show the speed mode in serial monitor
    Serial.print("Speed Mode: ");
    Serial.println(message);
    // Show the left motor speed in serial monitor
    Serial.print("Left: ");
    Serial.print(speed_left);
    Serial.println(" RPM");
    // Show the right motor speed in serial monitor
    Serial.print("Right: ");
    Serial.print(speed_right);
    Serial.println(" RPM");
    previousMillis = currentMillis;
    counter1 = 0; // To reset the counter and prevent wrong calculation
    counter2 = 0; // To reset the counter and prevent wrong calculation
  }

  // Speed Mode
  if(counter3 == 0){
    // Slow
    a=100;
    b=50;
    message="Slow";
  }
  else if(counter3 == 1){
    // Normal
    a=175;
    b=100;
    message="Medium";
  }
  else if(counter3 == 2){
    // Fast
    a=255;
    b=150;
    message="Fast";
  }

  if (Serial.available()>0){
    i=Serial.read(); // Read the operating command
    // Choose operating case
    if((i=='F')||(i=='f')){
      //Move Forward
      digitalWrite(cwleft,LOW);
      digitalWrite(acwleft, HIGH);
      digitalWrite(cwright, HIGH);
      digitalWrite(acwright, LOW);
      ledcWrite(ch0,a);
      ledcWrite(ch1,a);
    }
    else if((i=='B')||(i=='b')){
      //Move Backward
      digitalWrite(cwleft,HIGH);
      digitalWrite(acwleft, LOW);
      digitalWrite(cwright, LOW);
      digitalWrite(acwright, HIGH);
      ledcWrite(ch0,a);
      ledcWrite(ch1,a);
    }
    else if((i=='L')||(i=='l')){
      //Turn Left
      digitalWrite(cwleft,HIGH);
      digitalWrite(acwleft, LOW);
      digitalWrite(cwright, HIGH);
      digitalWrite(acwright, LOW);
      ledcWrite(ch0,a);
      ledcWrite(ch1,a);
    }
    else if((i=='R')||(i=='r')){
      //Turn Right
      digitalWrite(cwleft,LOW);
      digitalWrite(acwleft, HIGH);
      digitalWrite(cwright, LOW);
      digitalWrite(acwright, HIGH);
      ledcWrite(ch0,a);
      ledcWrite(ch1,a);
    }
    else if((i=='Q')||(i=='q')){
      //Move Left Forward 
      digitalWrite(cwleft,LOW);
      digitalWrite(acwleft, HIGH);
      digitalWrite(cwright, HIGH);
      digitalWrite(acwright, LOW);
      ledcWrite(ch0,b);
      ledcWrite(ch1,a);
    }
    else if((i=='W')||(i=='w')){
      //Move Right Forward 
      digitalWrite(cwleft,LOW);
      digitalWrite(acwleft, HIGH);
      digitalWrite(cwright, HIGH);
      digitalWrite(acwright, LOW);
      ledcWrite(ch0,a);
      ledcWrite(ch1,b);
    }
    else if((i=='T')||(i=='t')){
      //Move Left Backward 
      digitalWrite(cwleft,HIGH);
      digitalWrite(acwleft, LOW);
      digitalWrite(cwright, LOW);
      digitalWrite(acwright, HIGH);
      ledcWrite(ch0,b);
      ledcWrite(ch1,a);
    }
    else if((i=='E')||(i=='e')){
      //Move Right Backward 
      digitalWrite(cwleft,HIGH);
      digitalWrite(acwleft, LOW);
      digitalWrite(cwright, LOW);
      digitalWrite(acwright, HIGH);
      ledcWrite(ch0,a);
      ledcWrite(ch1,b);
    }
  }
}

void detect1(){
  // Counter the pulse for left motor
  if (currentMillis-previousMillis1>=interval1){
    counter1++;
    previousMillis1 = currentMillis;
  }
  
}
void detect2(){
  // Counter the pulse for right motor
  if (currentMillis-previousMillis2>=interval1){
    counter2++;
    previousMillis2 = currentMillis;
  }
  
}

void detect3(){
  // Counter the pulse for the speed mode
  if (currentMillis-previousMillis3>=interval2){
    counter3++;
    previousMillis3 = currentMillis;
  }
  if (counter3>=3){
    counter3=0;
  }
  
}