// ESP32 ESC controler with PWM Signal
// Author: Tales-sv


#include <ESP8266WiFi.h>
#define REVERSIBLE_ESC //coment if your ESC is configurated with only direction (non-reversible, only foward or foward/break)

#define OUTPUT_ESC D4 //Signal output pin
#define CHANNEL_ESC D5 //lec channel used for generating a pwm signal

u_int16_t PWM_width_us = 0; //PWM high width in Microseconds (us) (amount of time the signal is high)
u_int16_t Us_2000_duty = 410;
u_int16_t Us_1000_duty = 205;
u_int16_t DutyCycle = 0;    //Duty Cycle of the pwm signal generated

bool speed_up = 1; //switch the generated signal, speed up or slow down


#define RED D5
const int analog_ip = A0;
int error=0;
int new_val=0;
float Kp= 0.08;
int feedback_val=0;
int pwmValue=0;
int break_loop =0;

void setup() {

  Serial.begin(115200);
   
  //setup ledc channel:
  pinMode(CHANNEL_ESC, OUTPUT);            // Setting the signal output pin
  analogWriteFreq(20000);
  analogWriteRange(1024);
  // ledcSetup(CHANNEL_ESC, 50, 16);         // CHANNEL_ESC config: freq: 50Hz (20mS) Resolution: 16 bits (0 a 65535)
  // ledcAttachPin(OUTPUT_ESC, CHANNEL_ESC); // Attach the signal output pin to a ledc channel, necessary for generating PWM with ledc
  digitalWrite(RED, LOW);
  delay(20);
  pwmValue = 62; // Scaling factor of 310

  // Set the PWM output on pin D5
  new_val=pwmValue;
  analogWrite(RED, pwmValue);
//   //some ESCs need a initial stop signal to connect

//  #ifdef REVERSIBLE_ESC // if your esc is foward/reverse configurated:
//   PWM_width_us = 1500; // Reversible esc: stop

//  #else                 // if your esc is single direction configurated:
//   PWM_width_us = 1000; // Non-reversible esc: stop

//  #endif
//   Send_PWM_Signal(1); //generate the PWM signal

}//end setup()

void loop() {
    //ramp test:
    // break_loop=0;
    if(break_loop==0){
    do{     
            feedback_val = analogRead (analog_ip);
            // feedback_val = (310.303*(0.0005*pwmValue + 0.0373)) + feedback_val;
            // feedback_val = feedback_val - 10;
            Serial.println("feedback_val= ");
            Serial.println(feedback_val);
            Serial.println("pwmValue= ");
            Serial.println(pwmValue);
            Serial.println("new_val= ");
            Serial.println(new_val);
            error= pwmValue - feedback_val;
            delay(10);
            new_val= new_val + int(Kp * error);
            if (new_val < 1024 && new_val >=0){
              analogWrite(RED,new_val);

              
            }
            else if(new_val<0){
              analogWrite(RED,0);
              new_val=0;
            }
            else{
              analogWrite(RED,1024);
              new_val=1024;
            }
            delay(50);
            }while(abs(error)>=5);
    Serial.println("Reached stopping condition");
    break_loop=1;
    feedback_val = analogRead (analog_ip);}
    delay(5); //delay fo An accel ramp, witch is very usefull for safe controling of your motor
}//end loop()