/*
 * Original Servo Library sourse: https://github.com/RoboticsBrno/ESP32-Arduino-Servo-Library
 * 
 * This is Arduino code to control Multiple servo motors with ESP32 boards over bluetooth 
 * When press a button on screen, Servo will go 0° to 180° and stops and next press will send the servo from 180° to 0° 
 * this is ESP-SERV-BT-4
 * The Mininum Angle(0°) and Maximum Angle (180°) can be set to any angle you prefer.
 * 
 * Watch video instruction for this code: https://youtu.be/FOz9u_R5eG4
-code ESP-SERV-BT-1 Using ESP32 Bluetooth to Control Servo motor to move it to any angle 
-code ESP-SERV-BT-2 Using ESP32 Bluetooth to Control Servo motor to move them 0° to 180° or in reverse or any angle    
-code ESP-SERV-BT-3 PUSH-RETURN Using ESP32 Bluetooth to Control 1 or more Servo motors to move them 0° to 180° or in reverse or any angle    
-code ESP-SERV-BT-4 Using ESP32 Bluetooth to Control 1 or more Servo motors to move them 0° to 180° or in reverse or any angle one way    

 * Written by Ahmad Shamshiri for Robojax Video channel www.Robojax.com
 * Date: Jan 10, 2019, at 20:25 in Ajax, Ontario, Canada
 * Permission granted to share this code given that this
 * note is kept with the code.
 * Disclaimer: this code is "AS IS" and for educational purpose only.
 * this code has been downloaded from https://robojax.com
 
 * Get this code and other Arduino codes from Robojax.com


or make donation using PayPal http://robojax.com/L/?id=64

 *  * This code is "AS IS" without warranty or liability. Free to be used as long as you keep this note intact.* 
 * This code has been download from Robojax.com
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
#include <Servo_ESP32.h>

const int servoCount = 4;
static const int servosPins[servoCount] = {12, 14, 27, 26};

Servo_ESP32 servos[servoCount];

const char turnON[] ={'a', 'b', 'c', 'd'};


int servoAngle[] ={0, 0, 0, 0};
int servoAngleStep[] ={10, 10, 10, 10};

int servoAngleMin[] ={0, 0, 0, 0};
int servoAngleMax[] ={180, 180, 180, 180};

int buttonPushed[] ={0, 0, 0, 0};
char receivedChar;// received value will be stored as CHAR in this variable
boolean fullDebug = true;//set to false or true. watch video for details

#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

BluetoothSerial SerialBT;

void handleServo(int servoNum);
void setup() {
  // robojax.com code for servo https://youtu.be/FOz9u_R5eG4
  Serial.begin(115200);
  SerialBT.begin("ESP32_Robojax"); //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth\n with ESP32_Robojax");
  
    for(int i = 0; i < servoCount; ++i) {
        if(!servos[i].attach(servosPins[i])) {
            Serial.print("Servo ");
            Serial.print(i);
            Serial.println("attach error");
        }
    }
    //Servo control using ESP32 from Robojax.com

}

void loop() {
  //Robojax.com ESP32 Servo BT code
    receivedChar =(char)SerialBT.read();

  for(int i=0; i< servoCount; i++)
  {
       if(receivedChar == turnON[i]){
        buttonPushed[i] = 1;
       }//if 
  }//for


  for(int i=0; i< servoCount; i++)
  {
       if( buttonPushed[i] ){
        handleServo(i);//
       }//if 
  }//for

  
  for(int i=0; i< servoCount; i++)
  {
       servos[i].write(servoAngle[i]);
      
  }//for
   
  //servos[0].write(30);
   delay(20);

}//loop

/*
 * handleServo()
 * updates the servo angle
 * returns nothing
 * Written by Ahmad Shamshiri on Jan 10, 2019
 * www.Robojax.com
 * watch video for details https://youtu.be/FOz9u_R5eG4
 */
void handleServo(int servoNum)
{
  if(fullDebug)
  {
    SerialBT.print("Servo ");// write on BT app
    SerialBT.print(servoNum+1);// write on BT app 
    SerialBT.print(" at:");// write on BT app       
    SerialBT.println(servoAngle[servoNum]);// write on BT app 
    Serial.print("Servo ");//print on Serial Monitor
    Serial.print(servoNum+1);//print on Serial Monitor
    Serial.print(" at ");//print on Serial Monitor        
    Serial.println(servoAngle[servoNum]); //print on Serial Monitor     
  }
      servoAngle[servoNum] += servoAngleStep[servoNum];
       
        if (servoAngle[servoNum] >= servoAngleMax[servoNum]) {
          buttonPushed[servoNum] =0; 
          servoAngleStep[servoNum] = -servoAngleStep[servoNum];
            if(!fullDebug)
            {
            SerialBT.print("Servo ");// write on BT app
            SerialBT.print(servoNum+1);// write on BT app 
            SerialBT.print(" at:");// write on BT app       
            SerialBT.println(servoAngle[servoNum]);// write on BT app              
            }
        }
        
        if (servoAngle[servoNum] <= servoAngleMin[servoNum]) {
          buttonPushed[servoNum] =0;       
          servoAngleStep[servoNum] = -servoAngleStep[servoNum];
            if(!fullDebug)
            {
            SerialBT.print("Servo ");// write on BT app
            SerialBT.print(servoNum+1);// write on BT app 
            SerialBT.print(" at:");// write on BT app       
            SerialBT.println(servoAngle[servoNum]);// write on BT app              
            }          
        }
    
}//handleServo