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

  2025/08/25
  ec2021

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

*/

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

#define ENCODEROUTPUT_MOTOR 48

#define EN_MOTOR 3
#define IN1_MOTOR 2
#define IN2_MOTOR 4
#define ENCA_MOTOR 10
#define ENCB_MOTOR 11

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

void setup() {
  Serial.begin(9600);
  // delay(5000);
  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();
}

void loop() {
  /****************************/
  createPulsesPerSecond(48);
  /****************************/

  long pos;
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    pos = pos_i;
  }

  currT = micros();

  // update after 100ms
  if (currT - prevT >= 100000) {  // 100000 us = 100 ms
    float deltaT = (currT - prevT) / 1.0e6;
    long deltaPos = pos - posPrev;

    if (deltaT > 0.0001) {
      float velocity = deltaPos / deltaT; //pulse/s
      float v = velocity / ENCODEROUTPUT_MOTOR * 60.0; // RPM
      Serial.print("RPM:");
      Serial.println(v);
    }

    prevT = currT;
    posPrev = pos;
  }

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

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() {
  if (digitalRead(ENCB_MOTOR) == HIGH) {
    pos_i++;
  } else {
    pos_i--;
  }
}

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

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