/*  Code for electric longboard PWM code
 *  Tutorial webpage: https://www.electronoobs.com/eng_robotica_tut12.php
 *  Schematic: https://www.electronoobs.com/eng_robotica_tut12_sch1.php
 *  Part list: https://www.electronoobs.com/eng_robotica_tut12_parts1.php */

#include <Servo.h>;                         //We need this library for the PWm signal
Servo motor;                                //We create the servo pin, later attach to D3

//We create variables for the time width values of each PWM input signal
unsigned long counter_1, current_count;

//We create 4 variables to stopre the previous value of the input signal (if LOW or HIGH)
byte last_CH1_state;

int THROTTLE = 1500;
int LAST_THROTTLE = 1500;
int PWM_VALUE = 1500;
int THROTTLE_MAPED = 0;

void setup() {
  motor.attach(3);
  motor.writeMicroseconds(1505);            //The range of this ESC is 1000us to 1500 for CW and 1500 to 2000us for CCW
  PCICR |= (1 << PCIE0);                    //enable PCMSK0 scan                                                 
  PCMSK0 |= (1 << PCINT0);                  //Set pin D8 trigger an interrupt on state change. 
  
  Serial.begin(250000);  
  delay(500);
}

void loop() {
  /*My ESC has ranges from 1500 to 2100 for forward adn from 1500 to 1000 for backwards*/
  
  THROTTLE_MAPED = map(THROTTLE,1085,2100,1000,2000);     //Change the range here.
  THROTTLE_MAPED = THROTTLE_MAPED - 50;
  
  if(THROTTLE_MAPED > 1480)
  {
    if(THROTTLE_MAPED > PWM_VALUE)
    {
      PWM_VALUE = PWM_VALUE + 1;
      delay(7);
      LAST_THROTTLE = THROTTLE_MAPED;
    }
    
  
    if(THROTTLE_MAPED < PWM_VALUE)
    {
      PWM_VALUE = PWM_VALUE - 1;
      delay(7);
      LAST_THROTTLE = THROTTLE_MAPED;
    }
    PWM_VALUE = constrain(PWM_VALUE,1505,2100);
  }

  if(THROTTLE_MAPED < 1450)
  {
  
    if(THROTTLE_MAPED < PWM_VALUE)
    {
      PWM_VALUE = PWM_VALUE - 1;
      delayMicroseconds(2000);
      LAST_THROTTLE = THROTTLE_MAPED;
    }
    PWM_VALUE = constrain(PWM_VALUE,1350,2100);
  }

  
  
  motor.writeMicroseconds(PWM_VALUE);             //Finally write the PWM signal on pin D3
}




ISR(PCINT0_vect){
//First we take the current count value in micro seconds using the micros() function
  
  current_count = micros();
  ///////////////////////////////////////PWM input from receiver on D8
  if(PINB & B00000001){                                //We make an AND with the pin state register, We verify if pin 8 is HIGH???
    if(last_CH1_state == 0){                           //If the last state was 0, then we have a state change...
      last_CH1_state = 1;                              //Store the current state into the last state for the next loop
      counter_1 = current_count;                       //Set counter_1 to current value.
    }
  }
  else if(last_CH1_state == 1){                        //If pin 8 is LOW and the last state was HIGH then we have a state change      
    last_CH1_state = 0;                               //Store the current state into the last state for the next loop
    THROTTLE = current_count - counter_1;             //We make the time difference. Channel 1 is current_time - timer_1.
  }
}