#include <Encoder.h>

// --- ENCODER CONFIGURATION ---
#define ENCODER_PIN_A 2
#define ENCODER_PIN_B 4
Encoder motorEncoder(ENCODER_PIN_A, ENCODER_PIN_B);

// The encoder provides 100 Pulses Per Revolution (PPR).
// With X4 decoding (used by the library), the Counts Per Revolution (CPR) is 400.
const int CPR = 400;

// --- MOTOR CONTROL CONFIGURATION ---
#define PWM_PIN 9      // Speed control via PWM
#define DIR_PIN 7      // Direction (DIR) control
#define ENABLE_PIN 8   // Motor Enable/Start

// --- VELOCITY MEASUREMENT VARIABLES ---
long currentPosition = 0;
long previousPosition = 0;
unsigned long previousTime = 0; // Time of the last reading (in milliseconds)
const int MEASUREMENT_INTERVAL_MS = 100; // Time between velocity calculations (100ms)

// --- OPEN-LOOP VELOCITY SETTING ---
// Set a fixed speed and direction for testing.
const int MOTOR_DUTY_CYCLE = 150; // PWM duty cycle (0-255). Adjust as needed.
const int MOTOR_DIRECTION = HIGH; // HIGH or LOW for direction control.

void setup() {
  Serial.begin(115200);
  Serial.println("Nidec 24H055M020 Velocity Control and Encoder Verification");

  // --- 1. CONFIGURE HIGH-FREQUENCY PWM (Mandatory 16kHz - 26kHz) ---
  // Sets Timer 1 Prescaler to 1, resulting in a PWM frequency of ~31.37 kHz
  // (Closest achievable high-frequency using standard 8-bit resolution).
  TCCR1B = TCCR1B & B11111000 | B00000001; 

  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(ENABLE_PIN, OUTPUT);

  // --- 2. MOTOR INITIALIZATION AND START ---
  digitalWrite(ENABLE_PIN, LOW); // Start disabled
  digitalWrite(DIR_PIN, MOTOR_DIRECTION); // Set the desired direction

  delay(100);
  digitalWrite(ENABLE_PIN, HIGH); // Enable the motor
  
  // Apply the fixed duty cycle to start the motor
  analogWrite(PWM_PIN, MOTOR_DUTY_CYCLE);

  previousTime = millis(); // Initialize the time reference
}

void loop() {
  unsigned long currentTime = millis();
  
  // --- 3. CALCULATE VELOCITY AT FIXED INTERVAL ---
  if (currentTime - previousTime >= MEASUREMENT_INTERVAL_MS) {
    
    currentPosition = motorEncoder.read();
    
    // Calculate difference in time and counts
    long counts_change = currentPosition - previousPosition;
    unsigned long time_change_ms = currentTime - previousTime;
    
    // --- 4. RPM CALCULATION ---
    // (Counts / CPR) = Revolutions
    // (Revolutions / Time in seconds) = Revolutions Per Second (RPS)
    // (RPS * 60) = Revolutions Per Minute (RPM)
    
    // Formula: RPM = (counts_change / CPR) / (time_change_ms / 1000) * 60
    // Simplified: RPM = (counts_change * 60000) / (CPR * time_change_ms)
    
    float rpm = (float)counts_change * 60000.0 / ((float)CPR * (float)time_change_ms);

    // --- 5. OUTPUT DATA ---
    Serial.print("Count: ");
    Serial.print(currentPosition);
    Serial.print(" | Delta Counts: ");
    Serial.print(counts_change);
    Serial.print(" | Measured RPM: ");
    Serial.println(rpm, 2); // Print RPM with 2 decimal places

    // --- 6. UPDATE FOR NEXT CALCULATION ---
    previousPosition = currentPosition;
    previousTime = currentTime;
  }
}