/*
 * Arduinobestand G:\Arduino\MotAndTCRT5000_01b\MotAndTCRT5000_01d.ino
 * App gemaakt met MITappinventor http://ai2.appinventor.mit.edu/#5682380796198912
 * App: Mot_TCRT5000_01d
 * Werkt op de 38 pins ESP32, genummerd 11 t/m 15
 * Aliexpress: ESP-32S ESP-WROOM-32 Bluetooth En Wifi Dual Core Cpu Met Laag Stroomverbruik Mcu
 * Print ontworpen met Eagle: 
 * C:\Users\Rien\Documents\eagle\projects\RW_ESP32_4xmotoren_1xULN2803\ESP32_4xmotoren_1xULN2803_01
 */

#include "BluetoothSerial.h"
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
#include "esp32-hal-ledc.h"
ESP-32S ESP-WROOM-32 ESP32 ESP-32 Bluetooth En Wifi Dual Core Cpu Met Laag Stroomverbruik Mcu ESP-32
int inlees = 0;
const int Analog_channel_pin_1 = 32;
const int Analog_channel_pin_2 = 35;
int ADC_VALUE_1 = 0;
int ADC_VALUE_2 = 0;
int motorVooruit1 = 0;
int motorAchteruit1 = 0;
int motorVooruit2 = 0;
int motorAchteruit2 = 0;
// MOTOR 1
int Pin_M1a = 12;
int Pin_M1b = 25;
int Pin_M1e = 13;
// MOTOR 2
int Pin_M2a = 18;
int Pin_M2b = 2;
int Pin_M2e = 19;
// MOTOR 3
int Pin_M3a = 23;
int Pin_M3b = 22;
int Pin_M3e = 21;
// MOTOR 2
int Pin_M4a = 4;
int Pin_M4b = 5;
int Pin_M4e = 33; 
   
int freq = 5000;
int resolution = 8;
int snelheid = 255;
int ledChannel_0 = 0;
int ledChannel_1 = 0;
int ledChannel_2 = 0;
int ledChannel_3 = 0;
int afstand_1 = 255;
int afstand_2 = 255;

BluetoothSerial SerialBT;

void setup() {
Serial.begin(115200);
  SerialBT.begin("ESP32_IR_2Mot"); //Bluetooth device name   
  Serial.println("");
  Serial.println("ESP32_IR_2Mot"); //Bluetooth device name
  Serial.println("Device started; pair it with bluetooth!");
  Serial.println("");
  pinMode(Pin_M1a, OUTPUT);
  pinMode(Pin_M1b, OUTPUT);
  pinMode(Pin_M1e, OUTPUT);
  pinMode(Pin_M2a, OUTPUT);
  pinMode(Pin_M2b, OUTPUT);
  pinMode(Pin_M2e, OUTPUT);
  pinMode(Pin_M3a, OUTPUT);
  pinMode(Pin_M3b, OUTPUT);
  pinMode(Pin_M3e, OUTPUT);
  pinMode(Pin_M4a, OUTPUT);
  pinMode(Pin_M4b, OUTPUT);
  pinMode(Pin_M4e, OUTPUT);    
  ledcSetup(ledChannel_0, freq, resolution);
  ledcAttachPin(Pin_M1e, ledChannel_0);
  ledcSetup(ledChannel_1, freq, resolution);
  ledcAttachPin(Pin_M2e, ledChannel_1); 
  ledcSetup(ledChannel_2, freq, resolution);
  ledcAttachPin(Pin_M3e, ledChannel_2);
  ledcSetup(ledChannel_3, freq, resolution);
  ledcAttachPin(Pin_M4e, ledChannel_3);   
}

void loop() {
ADC_VALUE_1 = analogRead(Analog_channel_pin_1);
afstand_1 = map(ADC_VALUE_1, 1000, 4950, 0, 255);
Serial.print("afstand_1 = ");
Serial.println(afstand_1); 
if (afstand_1 <= 150){
     Serial.println(" --> Motor 1 STOP");          
     digitalWrite(Pin_M1a, HIGH);
     digitalWrite(Pin_M1b, HIGH);}
     else if (motorVooruit1 == 1){
     Serial.println(" --> Motor 1 draait vooruit");  
     digitalWrite(Pin_M1a, HIGH);
     digitalWrite(Pin_M1b, LOW);}
     else if (motorAchteruit1 == 1){ 
     Serial.println(" --> Motor 1 draait achteruit"); 
     digitalWrite(Pin_M1a, LOW);
     digitalWrite(Pin_M1b, HIGH);}
ADC_VALUE_2 = analogRead(Analog_channel_pin_2);
afstand_2 = map(ADC_VALUE_2, 2000, 4950, 0, 255);
Serial.print("afstand_2 = ");
Serial.println(afstand_2);
if (afstand_2 <= 150){
     Serial.println(" --> Motor 2 STOP");          
     digitalWrite(Pin_M2a, HIGH);
     digitalWrite(Pin_M2b, HIGH);}
     else if (motorVooruit2 == 1){
     Serial.println(" --> Motor 2 draait vooruit");      
     digitalWrite(Pin_M2a, HIGH);
     digitalWrite(Pin_M2b, LOW);}
     else if (motorAchteruit2 == 1){
     Serial.println(" --> Motor 2 draait achteruit"); 
     digitalWrite(Pin_M2a, LOW);
     digitalWrite(Pin_M2b, HIGH);} 
     
if (Serial.available()) {
    SerialBT.write(Serial.read());
    SerialBT.write(111);}
    if (SerialBT.available()) {
       inlees = (SerialBT.read());         
//Motor 1
       if (inlees == 97){//a
          Serial.print(inlees);
          Serial.print(" --> Motor 1 linksom - 50% - ");
          motorVooruit1 = 1;
          Serial.println(afstand_1);
          ledcWrite(ledChannel_0, 50);
          digitalWrite(Pin_M1a, HIGH);
          digitalWrite(Pin_M1b, LOW);}
       if (inlees == 98){//b
          Serial.print(inlees);
          Serial.print(" --> Motor 1 linksom - 75% - ");
          motorVooruit1 = 1;
          Serial.println(afstand_1);
          ledcWrite(ledChannel_0, 150);
          digitalWrite(Pin_M1a, HIGH);
          digitalWrite(Pin_M1b, LOW);}
       if (inlees == 99){//c
          Serial.print(inlees);
          Serial.print(" --> Motor 1 linksom - 100% - ");
          motorVooruit1 = 1;
          Serial.println(afstand_1);
          ledcWrite(ledChannel_0, 255);
          digitalWrite(Pin_M1a, HIGH);
          digitalWrite(Pin_M1b, LOW);}                    
       if (inlees == 100){//d
          Serial.print(inlees);
          Serial.println(" --> Motor 1 STOP");
          motorVooruit1 = 0;
          motorAchteruit1 = 0;          
          digitalWrite(Pin_M1a, HIGH);
          digitalWrite(Pin_M1b, HIGH);}          
       if (inlees == 101){//e
          Serial.print(inlees);
          Serial.print(" --> Motor 1 rechtsom - 50% - ");
          motorAchteruit1 = 1;
          Serial.println(afstand_1);
          ledcWrite(ledChannel_0, 50);
          digitalWrite(Pin_M1a, LOW);
          digitalWrite(Pin_M1b, HIGH);}
       if (inlees == 102){//f
          Serial.print(inlees);
          Serial.print(" --> Motor 1 rechtsom - 75% - ");
          motorAchteruit1 = 1;
          Serial.println(afstand_1);
          ledcWrite(ledChannel_0, 150);
          digitalWrite(Pin_M1a, LOW);
          digitalWrite(Pin_M1b, HIGH);}
       if (inlees == 103){//g
          Serial.print(inlees);
          Serial.print(" --> Motor 1 rechtsom - 100% - ");
          motorAchteruit1 = 1;
          Serial.println(afstand_1);
          ledcWrite(ledChannel_0, 255);
          digitalWrite(Pin_M1a, LOW);
          digitalWrite(Pin_M1b, HIGH);}                            
//Motor 2
       if (inlees == 104){//h
          Serial.print(inlees);
          Serial.print(" --> Motor 2 linksom - 50% - ");
          motorVooruit2 = 1;
          Serial.println(afstand_2);
          ledcWrite(ledChannel_1, 50);
          digitalWrite(Pin_M2a, HIGH);
          digitalWrite(Pin_M2b, LOW);}
       if (inlees == 105){//i
          Serial.print(inlees);
          Serial.print(" --> Motor 2 linksom - 75% - ");
          motorVooruit2 = 1;
          Serial.println(afstand_2);
          ledcWrite(ledChannel_1, 150);
          digitalWrite(Pin_M2a, HIGH);
          digitalWrite(Pin_M2b, LOW);}
       if (inlees == 106){//j
          Serial.print(inlees);
          Serial.print(" --> Motor 2 linksom - 100% - ");
          motorVooruit2 = 1;
          Serial.println(afstand_2);
          ledcWrite(ledChannel_1, 255);
          digitalWrite(Pin_M2a, HIGH);
          digitalWrite(Pin_M2b, LOW);}                    
       if (inlees == 107){//k
          Serial.print(inlees);
          Serial.println(" --> Motor 2 STOP");
          motorVooruit2 = 0;
          motorAchteruit2 = 0;          
          digitalWrite(Pin_M2a, HIGH);
          digitalWrite(Pin_M2b, HIGH);}          
       if (inlees == 108){//l
          Serial.print(inlees);
          Serial.print(" --> Motor 2 rechtsom - 50% - ");
          motorAchteruit2 = 1;
          Serial.println(afstand_2);
          ledcWrite(ledChannel_1, 50);
          digitalWrite(Pin_M2a, LOW);
          digitalWrite(Pin_M2b, HIGH);}
       if (inlees == 109){//m
          Serial.print(inlees);
          Serial.print(" --> Motor 2 rechtsom - 75% - ");
          motorAchteruit2 = 1;
          Serial.println(afstand_2);
          ledcWrite(ledChannel_1, 150);
          digitalWrite(Pin_M2a, LOW);
          digitalWrite(Pin_M2b, HIGH);}
       if (inlees == 110){//n
          Serial.print(inlees);
          Serial.print(" --> Motor 2 rechtsom - 100% - ");
          motorAchteruit2 = 1;
          Serial.println(afstand_2);
          ledcWrite(ledChannel_1, 255);
          digitalWrite(Pin_M2a, LOW);
          digitalWrite(Pin_M2b, HIGH);}    
//Motor 3
       if (inlees == 111){//o
          Serial.print(inlees);
          Serial.print(" --> Motor 3 linksom - 50% - ");
          ledcWrite(ledChannel_2, 50);
          digitalWrite(Pin_M3a, HIGH);
          digitalWrite(Pin_M3b, LOW);}
       if (inlees == 112){//p
          Serial.print(inlees);
          Serial.print(" --> Motor 3 linksom - 75% - ");
          ledcWrite(ledChannel_2, 150);
          digitalWrite(Pin_M3a, HIGH);
          digitalWrite(Pin_M3b, LOW);}
       if (inlees == 113){//q
          Serial.print(inlees);
          Serial.print(" --> Motor 3 linksom - 100% - ");
          ledcWrite(ledChannel_2, 255);
          digitalWrite(Pin_M3a, HIGH);
          digitalWrite(Pin_M3b, LOW);}                    
       if (inlees == 114){//r
          Serial.print(inlees);
          Serial.println(" --> Motor 3 STOP");
          digitalWrite(Pin_M3a, HIGH);
          digitalWrite(Pin_M3b, HIGH);}          
       if (inlees == 115){//s
          Serial.print(inlees);
          Serial.print(" --> Motor 3 rechtsom - 50% - ");
          ledcWrite(ledChannel_2, 50);
          digitalWrite(Pin_M3a, LOW);
          digitalWrite(Pin_M3b, HIGH);}
       if (inlees == 116){//t
          Serial.print(inlees);
          Serial.print(" --> Motor 3 rechtsom - 75% - ");
          ledcWrite(ledChannel_2, 150);
          digitalWrite(Pin_M3a, LOW);
          digitalWrite(Pin_M3b, HIGH);}
      if (inlees == 117){//u
          Serial.print(inlees);
          Serial.print(" --> Motor 3 rechtsom - 100% - ");
          ledcWrite(ledChannel_2, 255);
          digitalWrite(Pin_M3a, LOW);
          digitalWrite(Pin_M3b, HIGH);}
//Motor 4
       if (inlees == 118){//v
          Serial.print(inlees);
          Serial.print(" --> Motor 4 linksom - 50% - ");
          ledcWrite(ledChannel_3, 50);
          digitalWrite(Pin_M4a, HIGH);
          digitalWrite(Pin_M4b, LOW);}
       if (inlees == 119){//w
          Serial.print(inlees);
          Serial.print(" --> Motor 4 linksom - 75% - ");
          ledcWrite(ledChannel_3, 150);
          digitalWrite(Pin_M4a, HIGH);
          digitalWrite(Pin_M4b, LOW);}
       if (inlees == 120){//x
          Serial.print(inlees);
          Serial.print(" --> Motor 4 linksom - 100% - ");
          ledcWrite(ledChannel_3, 255);
          digitalWrite(Pin_M4a, HIGH);
          digitalWrite(Pin_M4b, LOW);}                    
       if (inlees == 121){//y
          Serial.print(inlees);
          Serial.println(" --> Motor 4 STOP");
          digitalWrite(Pin_M4a, HIGH);
          digitalWrite(Pin_M4b, HIGH);}          
       if (inlees == 122){//z
          Serial.print(inlees);
          Serial.print(" --> Motor 4 rechtsom - 50% - ");
          ledcWrite(ledChannel_3, 50);
          digitalWrite(Pin_M4a, LOW);
          digitalWrite(Pin_M4b, HIGH);}
       if (inlees == 65){//A
          Serial.print(inlees);
          Serial.print(" --> Motor 4 rechtsom - 75% - ");
          ledcWrite(ledChannel_3, 150);
          digitalWrite(Pin_M4a, LOW);
          digitalWrite(Pin_M4b, HIGH);}
       if (inlees == 66){//B
          Serial.print(inlees);
          Serial.print(" --> Motor 4 rechtsom - 100% - ");
          ledcWrite(ledChannel_3, 255);
          digitalWrite(Pin_M4a, LOW);
          digitalWrite(Pin_M4b, HIGH);}
  } 
delay(10);
}