# ESP32 Motor Encoder Test

// Pin definitions
const int encVcc_Pin = 16;
const int dirPin     = 17; // Motor direction
const int pwmPin     = 18; // Motor PWM
const int brakePin   = 19; // Motor brake
const int ledPin     = 2;  // Onboard LED for many ESP32 dev boards

// Encoder pins (MUST support interrupts!)
const int encA_Pin = 27;   // Change if needed
const int encB_Pin = 14;   // Change if needed

volatile long encoderTicks = 0;
volatile int direction = 1;

struct RPSMap {
  int pwm;
  float targetRPS;
};

RPSMap rpsTargets[] = {
  {240, 2.2}, {225, 4.7}, {210, 7.3}, {195, 9.7}, {180, 12.2},
  {165, 14.6}, {150, 17.1}, {135, 19.6}, {120, 22.1}, {105, 24.6},
  {90, 27.1}, {75, 29.6}, {60, 32.0}, {45, 34.5}, {30, 37.0}, {15, 39.5}
};

// ESP32 PWM setup
const int pwmChannel = 0;  // Use channel 0
const int pwmFreq = 20000; // 20 kHz PWM
const int pwmResolution = 8; // 8-bit (0-255)

bool hasReachedTargetRPS(float currentRPS, float targetRPS, float tolerance = 0.01) {
  return currentRPS >= (targetRPS - tolerance);
}

float getRPS() {
  long startTicks = encoderTicks;
  delay(100);
  long endTicks = encoderTicks;
  // 100 ticks per revolution assumed; adjust if different!
  return abs(endTicks - startTicks) / 100.0 / 0.1;
}

void brakeAndWait(float stopThresholdRPS = 0.2) {
  ledcWrite(pwmChannel, 255);    // Stop (min PWM)
  digitalWrite(brakePin, LOW);   // Engage brake
  delay(100);                    // Let brake engage

  // Wait for the motor to stop spinning
  while (true) {
    float rps = getRPS();
    if (rps <= stopThresholdRPS) break;
    delay(50);
  }
  digitalWrite(brakePin, HIGH);  // Release brake
  delay(100);
}

void setup() {
  pinMode(pwmPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(brakePin, OUTPUT);
  pinMode(encA_Pin, INPUT_PULLUP);
  pinMode(encB_Pin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);

  // PWM setup for ESP32
  ledcSetup(pwmChannel, pwmFreq, pwmResolution);
  ledcAttachPin(pwmPin, pwmChannel);

  attachInterrupt(digitalPinToInterrupt(encA_Pin), updateEncoder, RISING);

  Serial.begin(115200);

  ledcWrite(pwmChannel, 255);
  digitalWrite(brakePin, LOW);
  delay(1500);
  digitalWrite(brakePin, HIGH);

  digitalWrite(ledPin, HIGH);
  Serial.println("Hello, starting RPS sweep.");
}

void loop() {
  for (size_t i = 0; i < sizeof(rpsTargets) / sizeof(RPSMap); i++) {
    int pwm = rpsTargets[i].pwm;
    float targetRPS = rpsTargets[i].targetRPS;

    // CW test
    brakeAndWait();
    encoderTicks = 0;
    digitalWrite(dirPin, HIGH);
    ledcWrite(pwmChannel, pwm);
    unsigned long timeCW = 0;

    while (true) {
      float rps = getRPS();
      if (hasReachedTargetRPS(rps, targetRPS)) {
        timeCW = millis();
        break;
      }
    }

    // Brake before switching direction
    brakeAndWait();

    // CCW test
    encoderTicks = 0;
    digitalWrite(dirPin, LOW);
    ledcWrite(pwmChannel, pwm);
    unsigned long timeCCW = 0;

    while (true) {
      float rps = getRPS();
      if (hasReachedTargetRPS(rps, targetRPS)) {
        timeCCW = millis();
        break;
      }
    }

    // Brake at end of step
    brakeAndWait();

    // Print result
    Serial.print("PWM: ");
    Serial.print(pwm);
    Serial.print(" | Target RPS: ");
    Serial.print(targetRPS);
    Serial.print(" | CW time: ");
    Serial.print(timeCW);
    Serial.print(" ms | CCW time: ");
    Serial.print(timeCCW);
    Serial.print(" ms | Δt: ");
    Serial.print(abs((long)timeCCW - (long)timeCW) / 1000.0, 3);
    Serial.println(" s");
  }

  ledcWrite(pwmChannel, 255);
  Serial.println("Sweep complete.");

  // Blink LED 3 times to indicate end of sweep
  for (int i = 0; i < 3; i++) {
    digitalWrite(ledPin, LOW);  delay(200);
    digitalWrite(ledPin, HIGH); delay(200);
  }
  digitalWrite(ledPin, HIGH); // Leave LED on

  while (true); // Halt after sweep
}

void IRAM_ATTR updateEncoder() {
  int B = digitalRead(encB_Pin);
  if (B == LOW) {
    encoderTicks++;
    direction = 1;
  } else {
    encoderTicks--;
    direction = -1;
  }
}