/*
  Arduino | hardware-help
  Dc Motor Circuit Problems
  Nick — 6/25 at 5:29 PM
  Hi i am trying to build a circuit which uses a arduino and
  the following components a 1000kv dc brushless motor with
  a 30A esc speed controller, a 3s lipo battery, arduino,
  16x2 screen, hall sensor and bread board.
  I tried making the circuit by hand but the software from
  chatgpt encounterd many problems. I am therefore not sure
  if the following are correct my circuit as i want to be able
  to supply up to 8v to the motor but idk if the arduino is
  limiting it due to the 5v line. If the pulse modulation to
  the motor works as my code could only do max or min and not
  control the power. 3rd the software as my program didnt work.
  Any input on my circuit or program would be greatly appreciated.
  I am sorry for it being so messy
*/

#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <TimerOne.h>

Servo esc; // Create a Servo object to control the ESC

const int ESC_PIN = 4; // ESC signal pin
const int HALL_PIN = 2; // Hall sensor digital output pin

volatile unsigned long totalLoops = 0;
volatile float accumulatedL = 0;
volatile float currentRPM = 0; // Current RPM of the motor
float targetRPM = 2000; // Example target RPM
float pwmValue = 1050; // Initial PWM signal

// PID constants
float Kp = 0.5;
float Ki = 0.1;
float Kd = 0.05;

float previousError = 0;
float integral = 0;

LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C address 0x27, adjust if necessary

void initEsc()  {
  // ESC Initialization sequence
  Serial.println("Initializing ESC...");
  esc.write(45);
  esc.writeMicroseconds(1000); // Minimum throttle signal (ESC arm signal)
  delay(1000); // Wait for a second
  esc.writeMicroseconds(2000); // Maximum throttle signal for calibration
  delay(1000); // Wait for a second
  esc.writeMicroseconds(1000); // Minimum throttle signal
  delay(2000); // Wait for 20 seconds
  Serial.println("ESC Initialized");
}

void setup() {
  Serial.begin(115200);
  lcd.init();
  lcd.backlight();
  Timer1.initialize(10000); // trigger 100 times per second
  esc.attach(ESC_PIN);
  pinMode(HALL_PIN, INPUT_PULLUP);
  Timer1.attachInterrupt(updateAndAdjustPWM);
  attachInterrupt(digitalPinToInterrupt(HALL_PIN), rpmCounter, FALLING);
  // calibrate ESC
  initEsc();
  // init LCD
  lcd.setCursor(0, 0);
  lcd.print("RPM: ");
}

void loop() {

  Serial.println(totalLoops);

  /*
    // Adjust PWM based on target RPM for 40 seconds
    unsigned long currentTime = millis();
    while (millis() - currentTime < 40000) {
    // Display current RPM on LCD
    lcd.setCursor(5, 0);
    lcd.print(currentRPM);
    lcd.print("       "); // Clear any remaining characters

    // Print current RPM to serial monitor
    Serial.print("Current RPM: ");
    Serial.println(currentRPM);
    }

    // Stop the motor
    esc.writeMicroseconds(1000); // Minimum throttle to stop the motor
    Serial.println("Motor stopped");

    // Clear the display
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Motor stopped");

    while (true) {
    // Infinite loop to prevent restarting
    }
  */
}


void rpmCounter() {
  totalLoops++;
  accumulatedL += 0.5; // Each pulse is 0.5 L
}

void updateAndAdjustPWM() {
  // Calculate current RPM
  currentRPM = (accumulatedL / 10.0) * 6.0; // RPM calculation every second over the last 10 seconds
  accumulatedL = 0; // Reset accumulated L value

  // PID Controller
  float error = targetRPM - currentRPM;
  integral += error * (0.005); // Integral term, 0.005 is the interval in seconds
  float derivative = (error - previousError) / 0.005; // Derivative term
  float output = Kp * error + Ki * integral + Kd * derivative;

  // Adjust PWM value
  pwmValue += constrain(output, -0.5, 0.5); // Ensure PWM value is adjusted smoothly
  pwmValue = constrain(pwmValue, 1000, 2000); // Ensure PWM value is within range
  esc.writeMicroseconds(pwmValue);

  previousError = error;
}