/*
   Forum: https://forum.arduino.cc/t/arduino-stop-working-or-automatic-reset/1404103
   Wokwi: https://wokwi.com/projects/440281601942207489

  2025/08/25
  ec2021

  The function simulateRPM() was added to simulate the encoder pulses that usually come from
  a motor encoder.

  This is a version where the commented parts of

  https://wokwi.com/projects/440279150495983617

  have been changed.

*/

#include <util/atomic.h>
#include <PinChangeInterrupt.h>

constexpr uint16_t ENCODEROUTPUT_MOTOR {48};
constexpr uint8_t EN_MOTOR {3};
constexpr uint8_t IN1_MOTOR {2};
constexpr uint8_t IN2_MOTOR {4};
constexpr uint8_t ENCA_MOTOR {10};
constexpr uint8_t ENCB_MOTOR {11};

unsigned long prevT = 0;
unsigned long currT = 0;
long posPrev = 0;
long pos_i = 0;

void setup() {
  Serial.begin(115200);
  pinMode(EN_MOTOR, OUTPUT);
  pinMode(IN1_MOTOR, OUTPUT);
  pinMode(IN2_MOTOR, OUTPUT);
  pinMode(ENCA_MOTOR, INPUT_PULLUP);
  pinMode(ENCB_MOTOR, INPUT_PULLUP);

  attachPCINT(digitalPinToPCINT(ENCA_MOTOR), readEncoderMotor, RISING);
  prevT = micros();

  setMotor(1, 15, EN_MOTOR, IN1_MOTOR, IN2_MOTOR);

}

void loop() {
  /****************************/
  simulateRPM(600);
  /****************************/
  currT = micros();
  // update after 100ms
  if (currT - prevT >= 100000) {  // 100000 us = 100 ms
    long pos;
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
      pos = pos_i;
    }
    float deltaT = (currT - prevT) / 1.0e6;
    long deltaPos = pos - posPrev;
    float velocity = deltaPos / deltaT; //pulse/s
    float v = velocity / ENCODEROUTPUT_MOTOR * 60.0; // RPM
    Serial.print("RPM:");
    Serial.println(v);
    prevT = currT;
    posPrev = pos;
  }
}

void setMotor(int dir, int pwmVal, int EN, int IN1, int IN2) {
  analogWrite(EN, pwmVal);
  if (dir == 1) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  } else if (dir == -1) {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }
}

void readEncoderMotor() {
  pos_i++;
}

/* The following function creates pulses in pulseOutPin that */
/* simulate the encoder pulses of a given motor              */

void simulateRPM(uint16_t RPM) {
  constexpr byte pulseOutPin {5};
  static unsigned long lastPulse = 0;
  if (lastPulse == 0) {
    pinMode(pulseOutPin, OUTPUT);
    digitalWrite(pulseOutPin, HIGH);
  }
  unsigned long noOfPulses = ENCODEROUTPUT_MOTOR * RPM / 60;
  unsigned long microsInterval = 1000000UL / noOfPulses;
  if (micros() - lastPulse >= microsInterval) {
    lastPulse = micros();
    digitalWrite(pulseOutPin, LOW);
    delayMicroseconds(5);
    digitalWrite(pulseOutPin, HIGH);
  }
}